Simulator.cpp

Go to the documentation of this file.

/*************************************************************************
*                                                                       *
* Open Physics Abstraction Layer                                        *
* Copyright (C) 2004-2005                                               *
* Alan Fischer  alan.fischer@gmail.com                                  *
* Andres Reinot  andres@reinot.com                                      *
* Tyler Streeter  tylerstreeter@gmail.com                               *
* All rights reserved.                                                  *
* Web: opal.sourceforge.net                                             *
*                                                                       *
* This library is free software; you can redistribute it and/or         *
* modify it under the terms of EITHER:                                  *
*   (1) The GNU Lesser General Public License as published by the Free  *
*       Software Foundation; either version 2.1 of the License, or (at  *
*       your option) any later version. The text of the GNU Lesser      *
*       General Public License is included with this library in the     *
*       file license-LGPL.txt.                                          *
*   (2) The BSD-style license that is included with this library in     *
*       the file license-BSD.txt.                                       *
*                                                                       *
* This library is distributed in the hope that it will be useful,       *
* but WITHOUT ANY WARRANTY; without even the implied warranty of        *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
* license-LGPL.txt and license-BSD.txt for more details.                *
*                                                                       *
*************************************************************************/

// class headers
#include "Simulator.h"

// project headers
#include "BlueprintInstance.h"
#include "BoxShapeData.h"
#include "SphereShapeData.h"
#include "CapsuleShapeData.h"
#include "PlaneShapeData.h"
#include "ThrusterMotor.h"
#include "GearedMotor.h"
#include "ServoMotor.h"
#include "AttractorMotor.h"
#include "SpringMotor.h"
#include "AccelerationSensor.h"
#include "InclineSensor.h"
#include "RaycastSensor.h"
#include "VolumeSensor.h"
#include "PostStepEventHandler.h"
#include "VelocityMotor.h"

namespace opal
{
        Simulator::Simulator()
        {
                mTimeBuffer = 0;
                setStepSize( defaults::stepSize );
                //setMaxLinearVel(defaults::maxLinearVel);
                //setMaxAngularVel(defaults::maxAngularVel);
                setMaxCorrectingVel( defaults::maxCorrectingVel );
                setMaxContacts( defaults::maxContacts );
                setUserData( NULL );
                mIsSolidDestructionSafe = true;
                mIsJointDestructionSafe = true;
                mStaticSleepingContactsEnabled =
                    defaults::staticSleepingContactsEnabled;

                int i = 0;
                for ( i = 0; i < 32; ++i )
                {
                        mContactGroupFlags[ i ] = defaults::contactGroupFlags;
                }

                mRootSpace = NULL;
        }

        void Simulator::initData( SimulatorData data )
        {
                mData = data;
        }

        Simulator::~Simulator()
        {
                destroyAllSolids();
                destroyAllJoints();
                destroyAllMotors();
                destroyAllSensors();

                while ( !mSpaceList.empty() )
                {
                        mSpaceList.back() ->internal_destroy();
                        mSpaceList.pop_back();
                }
                mRootSpace->internal_destroy();

                mSolidGarbageList.clear();
                mJointGarbageList.clear();
        }

        bool Simulator::simulate( real dt )
        {
                mTimeBuffer += dt;

                // Use this to return true if at least one step occurred.
                bool stepOccurred = false;
                if ( mTimeBuffer >= mStepSize )
                {
                        stepOccurred = true;
                }

                //if (mAllowPartialFrames && mTimeBuffer >= (real)defaults::maxStepsPerFrame * mStepSize)
                //{
                //  mTimeBuffer = defaults::maxStepsPerFrame * mStepSize;
                //}

                while ( mTimeBuffer >= mStepSize )
                {
                        // Update Sensors.
                        std::vector<Sensor*>::iterator sensorIter;
                        for ( sensorIter = mSensorList.begin();
                                sensorIter != mSensorList.end(); ++sensorIter )
                        {
                                ( *sensorIter ) ->internal_update();
                        }

                        // Update Motors.
                        std::vector<Motor*>::iterator motorIter;
                        for ( motorIter = mMotorList.begin();
                                motorIter != mMotorList.end(); ++motorIter )
                        {
                                ( *motorIter ) ->internal_update();
                        }

                        // Apply forces/torques to Solids.
                        std::vector<Solid*>::iterator solidIter;
                        for ( solidIter = mSolidList.begin();
                                solidIter != mSolidList.end(); ++solidIter )
                        {
                                ( *solidIter ) ->internal_applyForces( mStepSize );
                        }

                        // Update Joints.
                        mIsJointDestructionSafe = false;
                        std::vector<Joint*>::iterator jointIter;
                        for ( jointIter = mJointList.begin();
                                jointIter != mJointList.end(); ++jointIter )
                        {
                                // If a Joint gets broken here, it will automatically
                                // send a Joint break event to the Joint's
                                // JointBreakEventHandler (assuming one exists).
                                ( *jointIter ) ->internal_update();
                        }
                        mIsJointDestructionSafe = true;

                        // Now do physics engine-specific stuff; collision events will
                        // be sent from here.
                        stepPhysics();

                        // Loop over Solids again to handle a few more things...
                        mIsSolidDestructionSafe = false;
                        for ( solidIter = mSolidList.begin();
                                solidIter != mSolidList.end(); ++solidIter )
                        {
                                Solid* solid = *solidIter;

                                // Get each dynamic, awake Solid's new transform from the
                                // physics engine.
                                if ( !solid->isStatic() && !solid->isSleeping() )
                                {
                                        solid->internal_updateOPALTransform();
                                        solid->setMovingState( true );
                                }

                                // Update the sleeping value from the physics engine.  This
                                // needs to be updated to keep the Solid's SolidData valid.
                                //solid->internal_updateSleeping();
                                // Update... Now this gets updated in the Solid::getData
                                // function.

                                // Update the Solid's CollisionEventHandler if applicable.
                                if ( solid->getCollisionEventHandler() )
                                {
                                        solid->getCollisionEventHandler() ->
                                        internal_handlePendingCollisionEvents();
                                }

                                if ( solid->getMovementEventHandler() )
                                {
                                        if ( solid->isMoving() )
                                        {
                                                MovementEvent move( solid );
                                                solid->getMovementEventHandler() ->handleMovementEvent( move );
                                        }
                                }
                        }
                        mIsSolidDestructionSafe = true;

                        // Fire an event to the PostStepEventHandler, if one exists.
                        if ( getNumPostStepEventHandlers() > 0 )
                        {
                                for ( size_t i = 0; i < mPostStepEventHandlers.size(); ++i )
                                        mPostStepEventHandlers[ i ] ->handlePostStepEvent();
                        }

                        if ( getNumGlobalCollisionEventHandlers() > 0 )
                        {
                                for ( size_t i = 0; i < mCollisionEventHandlers.size(); ++i )
                                        mCollisionEventHandlers[ i ] ->internal_handlePendingCollisionEvents();
                        }

                        // Destroy garbage now that it's safe.
                        destroyGarbage();

                        //Decrement the time buffer
                        mTimeBuffer -= mStepSize;
                }

                return stepOccurred;
        }

        void Simulator::internal_recordCollision( const CollisionEvent & event )
        {
                if ( mCollisionEventHandlers.size() > 0 )
                {
                        for ( size_t i = 0; i < mCollisionEventHandlers.size(); ++i )
                        {
                                mCollisionEventHandlers[ i ] ->internal_pushCollisionEvent( event );
                        }
                }
        }

        void Simulator::setStepSize( real stepSize )
        {
                assert( stepSize > 0 );
                mStepSize = stepSize;
        }

        real Simulator::getStepSize()
        {
                return mStepSize;
        }

        void Simulator::addPostStepEventHandler(
            PostStepEventHandler* eventHandler )
        {
                mPostStepEventHandlers.push_back( eventHandler );
        }

        PostStepEventHandler* Simulator::getPostStepEventHandler( unsigned int id ) const
        {
                return mPostStepEventHandlers[ id ];
        }

        void Simulator::removePostStepEventHandler(
            PostStepEventHandler * eventHandler )
        {
                for ( size_t i = 0; i < mPostStepEventHandlers.size(); ++i )
                {
                        if ( mPostStepEventHandlers[ i ] == eventHandler )
                        {
                                mPostStepEventHandlers.erase( mPostStepEventHandlers.begin() + i );
                        }
                }
        }

        size_t Simulator::getNumPostStepEventHandlers() const
        {
                return mPostStepEventHandlers.size();
        }

        void Simulator::addGlobalCollisionEventHandler(
            CollisionEventHandler* eventHandler )
        {
                mCollisionEventHandlers.push_back( eventHandler );
        }

        CollisionEventHandler* Simulator::getGlobalCollisionEventHandler( unsigned int id ) const
        {
                return mCollisionEventHandlers[ id ];
        }

        void Simulator::removeGlobalCollisionEventHandler(
            CollisionEventHandler * eventHandler )
        {
                for ( size_t i = 0; i < mCollisionEventHandlers.size(); ++i )
                {
                        if ( mCollisionEventHandlers[ i ] == eventHandler )
                        {
                                mCollisionEventHandlers.erase( mCollisionEventHandlers.begin() + i );
                                break;
                        }
                }
        }

        size_t Simulator::getNumGlobalCollisionEventHandlers() const
        {
                return mCollisionEventHandlers.size();
        }

        void Simulator::instantiateBlueprint(
            BlueprintInstance& instance, const Blueprint& bp,
            const Matrix44r& offset, real scale )
        {
                assert( scale > 0 );

                if ( !bp.isFinalized() )
                {
                        OPAL_LOGGER( "warning" )
                        << "opal::Simulator::instantiateBlueprint: Cannot \
                        instantiate a Blueprint before it is finalized.  Ignoring \
                        the Blueprint." << std::endl;
                        return ;
                }

                bool useOffset = false;
                if ( Matrix44r() != offset )
                {
                        useOffset = true;
                }

                bool useScale = false;
                //Matrix44r scaleMat;
                if ( 1 != scale )
                {
                        useScale = true;
                        //scaleMat.makeScale(scale);
                }

                // For each object created here, only the ones with names get added
                // to the BlueprintInstance.

                // Some of the objects depend on the presence of others, so
                // they must be created in a specific order: Solids depend on
                // nothing, Joints depend on Solids, Motors depend on Joints
                // and Solids, and Sensors depend on Joints and Solids.

                // For the objects with references to their dependencies (i.e.
                // Joints, Motors, and Sensors), use the stored indices to
                // find pointers in the appropriate array.

                // Create these temporary arrays of pointers.  Other objects
                // will use these to find pointers to their dependencies.
                std::vector<Solid*> solidList;
                std::vector<Joint*> jointList;
                unsigned int i;

                // Create all Solids in the Blueprint.
                for ( i = 0; i < bp.getNumSolids(); ++i )
                {
                        Solid* s = createSolid();

                        // Make a local copy of the Solid's data, including all of its
                        // Shapes.
                        SolidData sd = *bp.getSolidData( i );

                        if ( useScale )
                        {
                                // Scale the Solid's transform.
                                sd.transform[ 12 ] *= scale;
                                sd.transform[ 13 ] *= scale;
                                sd.transform[ 14 ] *= scale;

                                // Scale the Solid's Shape's offsets and dimensions.
                                unsigned int i = 0;
                                for ( i = 0; i < sd.getNumShapes(); ++i )
                                {
                                        ShapeData* shapeData = sd.getShapeData( i );
                                        shapeData->offset[ 12 ] *= scale;
                                        shapeData->offset[ 13 ] *= scale;
                                        shapeData->offset[ 14 ] *= scale;

                                        switch ( shapeData->getType() )
                                        {
                                                case BOX_SHAPE:
                                                        {
                                                                BoxShapeData * boxData =
                                                                    ( BoxShapeData* ) shapeData;
                                                                boxData->dimensions =
                                                                    scale * boxData->dimensions;
                                                                break;
                                                        }
                                                case SPHERE_SHAPE:
                                                        {
                                                                SphereShapeData* sphereData =
                                                                    ( SphereShapeData* ) shapeData;
                                                                sphereData->radius =
                                                                    scale * sphereData->radius;
                                                                break;
                                                        }
                                                case CAPSULE_SHAPE:
                                                        {
                                                                CapsuleShapeData* capsuleData =
                                                                    ( CapsuleShapeData* ) shapeData;
                                                                capsuleData->radius =
                                                                    scale * capsuleData->radius;
                                                                capsuleData->length =
                                                                    scale * capsuleData->length;
                                                                break;
                                                        }
                                                case PLANE_SHAPE:
                                                        {
                                                                PlaneShapeData* planeData =
                                                                    ( PlaneShapeData* ) shapeData;
                                                                planeData->abcd[ 3 ] =
                                                                    scale * planeData->abcd[ 3 ];
                                                                break;
                                                        }
                                                        //case RAY_SHAPE:
                                                        //{
                                                        //  RayShapeData* rayData =
                                                        //      (RayShapeData*)shapeData;
                                                        //  Point3r origin = rayData->ray.getOrigin();
                                                        //  origin = scale * origin;
                                                        //  rayData->ray.setOrigin(origin);
                                                        //  break;
                                                        //}
                                                case MESH_SHAPE:
                                                        {
                                                                // Not implemented.  Scaling the Mesh data
                                                                // should be handled elsewhere.
                                                                break;
                                                        }
                                                default:
                                                        assert( false );
                                        }
                                }
                        }

                        if ( useOffset )
                        {
                                // Offset the Solid's transform.
                                sd.transform = offset * sd.transform;
                        }

                        s->init( sd );
                        solidList.push_back( s );

                        // Add the Solid to the BlueprintInstance.
                        instance.internal_addSolid( s );
                }

                // Create all Joints in the Blueprint.
                for ( i = 0; i < bp.getNumJoints(); ++i )
                {
                        Joint* j = createJoint();

                        // Create a local copy of the Joint's data.
                        JointData jd = *bp.getJointData( i );

                        // Setup the Solid reference pointers.  If one of the Solid 
                        // indices is -1, the Joint will attach that side to the 
                        // static environment automatically.
                        if (-1 == jd.internal_solid0Index)
                        {
                                jd.solid0 = NULL;
                        }
                        else
                        {
                                jd.solid0 = solidList.at( jd.internal_solid0Index );
                        }

                        if (-1 == jd.internal_solid1Index)
                        {
                                jd.solid1 = NULL;
                        }
                        else
                        {
                                jd.solid1 = solidList.at( jd.internal_solid1Index );
                        }

                        if ( useScale )
                        {
                                // Scale the Joint's anchor.
                                jd.anchor = scale * jd.anchor;
                        }

                        if ( useOffset )
                        {
                                // Offset the Joint's anchor and axes.
                                jd.anchor = offset * jd.anchor;
                                jd.axis[ 0 ].direction = offset * jd.axis[ 0 ].direction;
                                jd.axis[ 1 ].direction = offset * jd.axis[ 1 ].direction;
                                jd.axis[ 2 ].direction = offset * jd.axis[ 2 ].direction;
                        }

                        j->init( jd );
                        jointList.push_back( j );

                        // Add the Joint to the BlueprintInstance.
                        instance.internal_addJoint( j );
                }

                // Create all Motors in the Blueprint.
                for ( i = 0; i < bp.getNumMotors(); ++i )
                {
                        Motor* m = NULL;
                        MotorData* motorData = bp.getMotorData( i );

                        // Allocate and setup the right type of Motor.
                        switch ( bp.getMotorData( i ) ->getType() )
                        {
                                case ATTRACTOR_MOTOR:
                                        {
                                                // Make a local copy of the Motor's data.
                                                AttractorMotorData data = *( AttractorMotorData* ) motorData;

                                                // Setup the Solid reference pointers.
                                                data.solid0 = solidList.at( data.internal_solid0Index );
                                                data.solid1 = solidList.at( data.internal_solid1Index );

                                                m = createAttractorMotor();
                                                ( ( AttractorMotor* ) m ) ->init( data );
                                                break;
                                        }
                                case GEARED_MOTOR:
                                        {
                                                // Make a local copy of the Motor's data.
                                                GearedMotorData data = *( GearedMotorData* ) motorData;

                                                // Setup the Joint reference pointer.
                                                data.joint = jointList.at( data.internal_jointIndex );

                                                m = createGearedMotor();
                                                ( ( GearedMotor* ) m ) ->init( data );
                                                break;
                                        }
                                case SERVO_MOTOR:
                                        {
                                                // Make a local copy of the Motor's data.
                                                ServoMotorData data = *( ServoMotorData* ) motorData;

                                                // Setup the Joint reference pointer.
                                                data.joint = jointList.at( data.internal_jointIndex );

                                                m = createServoMotor();
                                                ( ( ServoMotor* ) m ) ->init( data );
                                                break;
                                        }
                                case SPRING_MOTOR:
                                        {
                                                // Make a local copy of the Motor's data.
                                                SpringMotorData data = *( SpringMotorData* ) motorData;

                                                // Setup the Solid reference pointer.
                                                data.solid = solidList.at( data.internal_solidIndex );

                                                m = createSpringMotor();
                                                ( ( SpringMotor* ) m ) ->init( data );
                                                break;
                                        }
                                case THRUSTER_MOTOR:
                                        {
                                                // Make a local copy of the Motor's data.
                                                ThrusterMotorData data = *( ThrusterMotorData* ) motorData;

                                                // Setup the Solid reference pointer.
                                                data.solid = solidList.at( data.internal_solidIndex );

                                                m = createThrusterMotor();
                                                ( ( ThrusterMotor* ) m ) ->init( data );
                                                break;
                                        }
                                default:
                                        assert( false );
                        }

                        // Add the Motor to the BlueprintInstance.
                        instance.internal_addMotor( m );
                }

                // Create all Sensors in the Blueprint.
                for ( i = 0; i < bp.getNumSensors(); ++i )
                {
                        Sensor* s = NULL;
                        SensorData* sensorData = bp.getSensorData( i );

                        // These are necessary because we don't want to change teh
                        // Blueprint's Sensor data directly; we need a local copy.
                        Solid* solidPtr = NULL;
                        Matrix44r transform = sensorData->transform;

                        // Setup the Solid reference pointer.
                        if ( -1 == sensorData->internal_solidIndex )
                        {
                                // This Sensor must not be attached to any Solid.
                                solidPtr = NULL;
                        }
                        else
                        {
                                // This Sensor is attached to a Solid.
                                solidPtr = solidList.at( sensorData->internal_solidIndex );
                        }

                        if ( useScale )
                        {
                                // Scale the Solid's transform.
                                transform[ 12 ] *= scale;
                                transform[ 13 ] *= scale;
                                transform[ 14 ] *= scale;
                        }

                        if ( useOffset && !solidPtr )
                        {
                                // If the Sensor is not attached to a Solid, offset the 
                                // Sensor's transform.
                                transform = offset * transform;
                        }

                        // Allocate the right type of Sensor.
                        switch ( sensorData->getType() )
                        {
                                case ACCELERATION_SENSOR:
                                        {
                                                // Make a local copy of the Sensor's data.
                                                AccelerationSensorData data =
                                                    *( AccelerationSensorData* ) sensorData;
                                                data.solid = solidPtr;
                                                data.transform = transform;

                                                s = createAccelerationSensor();
                                                ( ( AccelerationSensor* ) s ) ->init( data );
                                                break;
                                        }
                                case INCLINE_SENSOR:
                                        {
                                                // Make a local copy of the Sensor's data.
                                                InclineSensorData data = *( InclineSensorData* ) sensorData;
                                                data.solid = solidPtr;
                                                data.transform = transform;

                                                s = createInclineSensor();
                                                ( ( InclineSensor* ) s ) ->init( data );
                                                break;
                                        }
                                case RAYCAST_SENSOR:
                                        {
                                                // Make a local copy of the Sensor's data.
                                                RaycastSensorData data = *( RaycastSensorData* ) sensorData;
                                                data.solid = solidPtr;
                                                data.transform = transform;

                                                s = createRaycastSensor();
                                                ( ( RaycastSensor* ) s ) ->init( data );
                                                break;
                                        }
                                case VOLUME_SENSOR:
                                        {
                                                // Make a local copy of the Sensor's data.
                                                VolumeSensorData data = *( VolumeSensorData* ) sensorData;
                                                data.solid = solidPtr;
                                                data.transform = transform;

                                                s = createVolumeSensor();
                                                ( ( VolumeSensor* ) s ) ->init( data );
                                                break;
                                        }
                                default:
                                        assert( false );
                        }

                        // Add the Sensor to the BlueprintInstance.
                        instance.internal_addSensor( s );
                }

                solidList.clear();
                jointList.clear();
        }

        //void Simulator::setMaxLinearVel(real max)
        //{
        //  assert(max >= 0.0);
        //  mMaxLinearVel = max;
        //}

        //real Simulator::getMaxLinearVel()const
        //{
        //  return mMaxLinearVel;
        //}

        //void Simulator::setMaxAngularVel(real max)
        //{
        //  assert(max >= 0.0);
        //  mMaxAngularVel = max;
        //}

        //real Simulator::getMaxAngularVel()const
        //{
        //  return mMaxAngularVel;
        //}

        //void Simulator::setDefaultLinearDamping(real ld)
        //{
        //  assert(ld >= 0.0);
        //  mDefaultLinearDamping = ld;
        //}

        //real Simulator::getDefaultLinearDamping()const
        //{
        //  return mDefaultLinearDamping;
        //}

        //void Simulator::setDefaultAngularDamping(real ad)
        //{
        //  assert(ad >= 0.0);
        //  mDefaultAngularDamping = ad;
        //}

        //real Simulator::getDefaultAngularDamping()const
        //{
        //  return mDefaultAngularDamping;
        //}

        void Simulator::setUserData( void* data )
        {
                mUserData = data;
        }

        void* Simulator::getUserData()
        {
                return mUserData;
        }

        void Simulator::setupContactGroups( unsigned int group0,
                                            unsigned int group1, bool makeContacts )
        {
                if ( group0 > 31 )
                {
                        OPAL_LOGGER( "warning" ) << "opal::Simulator::setupContactGroups: "
                        << "Invalid contact group " << group0
                        << ". Request will be ignored." << std::endl;
                        return ;
                }

                if ( group1 > 31 )
                {
                        OPAL_LOGGER( "warning" ) << "opal::Simulator::setupContactGroups: "
                        << "Invalid contact group " << group1
                        << ". Request will be ignored." << std::endl;
                        return ;
                }

                // The interaction always goes both ways, so we need to set the bit
                // flags both ways.

                unsigned long int group0Bit = 1 << group0;
                unsigned long int group1Bit = 1 << group1;

                if ( makeContacts )
                {
                        // Raise the group1 bit in group0's array.
                        mContactGroupFlags[ group0 ] |= group1Bit;

                        // Raise the group0 bit in group1's array.
                        mContactGroupFlags[ group1 ] |= group0Bit;
                }
                else
                {
                        unsigned long int tempMask = 0xFFFFFFFF;
                        unsigned long int notGroup0Bit = group0Bit ^ tempMask;
                        unsigned long int notGroup1Bit = group1Bit ^ tempMask;

                        // Lower the group1 bit in group0's array.
                        mContactGroupFlags[ group0 ] &= notGroup1Bit;

                        // Lower the group0 bit in group1's array.
                        mContactGroupFlags[ group1 ] &= notGroup0Bit;
                }
        }

        void Simulator::setupContactGroup( unsigned int group,
                                           bool makeContacts )
        {
                int i = 0;
                for ( i = 0; i < 32; ++i )
                {
                        setupContactGroups( group, i, makeContacts );
                }
        }

        bool Simulator::groupsMakeContacts( unsigned int group0,
                                            unsigned int group1 )
        {
                // We only need to check for "one side" of the contact groups
                // here because the groups are always setup both ways (i.e.
                // the interaction between object 0's contact group and
                // object 1's contact group is always symmetric).

                unsigned long int group1Bit = 1 << group1;
                if ( internal_getContactGroupFlags( group0 ) & group1Bit )
                {
                        return true;
                }
                else
                {
                        return false;
                }
        }

        void Simulator::setStaticSleepingContactsEnabled( bool enable )
        {
                mStaticSleepingContactsEnabled = enable;
        }

        bool Simulator::areStaticSleepingContactsEnabled()
        {
                return mStaticSleepingContactsEnabled;
        }

        unsigned long int Simulator::internal_getContactGroupFlags(
            unsigned int groupNum ) const
        {
                return mContactGroupFlags[ groupNum ];
        }

        Solid* Simulator::createPlane( real a, real b, real c, real d,
                                       const Material& m )
        {
                // Create the plane's Solid and make it static.
                Solid * plane = createSolid();
                plane->setStatic( true );

                // Setup the plane's Shape data.
                opal::PlaneShapeData planeData;
                planeData.material = m;
                planeData.abcd[ 0 ] = a;
                planeData.abcd[ 1 ] = b;
                planeData.abcd[ 2 ] = c;
                planeData.abcd[ 3 ] = d;

                // Add the Shape to the Solid.
                plane->addShape( planeData );

                return plane;
        }

        unsigned int Simulator::getNumSolids() const
        {
                return ( unsigned int ) ( mSolidList.size() );
        }

        Solid* Simulator::getSolid( unsigned int i ) const
        {
                return mSolidList.at( i );
        }

        void Simulator::destroySolid( Solid* s )
        {
                if ( mIsSolidDestructionSafe )
                {
                        removeSolid( s );
                }
                else
                {
                        mSolidGarbageList.push_back( s );
                }
        }

        void Simulator::destroyAllSolids()
        {
                for ( size_t i = 0; i < mSolidList.size(); ++i )
                {
                        removeSolid( mSolidList[ i ] );
                }
        }

        void Simulator::destroyJoint( Joint* j )
        {
                if ( mIsJointDestructionSafe )
                {
                        removeJoint( j );
                }
                else
                {
                        mJointGarbageList.push_back( j );
                }
        }

        void Simulator::destroyAllJoints()
        {
                for ( size_t i = 0; i < mJointList.size(); ++i )
                {
                        removeJoint( mJointList[ i ] );
                }
        }

        ThrusterMotor* Simulator::createThrusterMotor()
        {
                ThrusterMotor * newMotor = new ThrusterMotor();
                addMotor( newMotor );
                return newMotor;
        }

        VelocityMotor* Simulator::createVelocityMotor()
        {
                VelocityMotor * newMotor = new VelocityMotor( this );
                addMotor( newMotor );
                return newMotor;
        }

        GearedMotor* Simulator::createGearedMotor()
        {
                GearedMotor * newMotor = new GearedMotor();
                addMotor( newMotor );
                return newMotor;
        }

        ServoMotor* Simulator::createServoMotor()
        {
                ServoMotor * newMotor = new ServoMotor();
                addMotor( newMotor );
                return newMotor;
        }

        AttractorMotor* Simulator::createAttractorMotor()
        {
                AttractorMotor * newMotor = new AttractorMotor();
                addMotor( ( Motor* ) newMotor );
                return newMotor;
        }

        SpringMotor* Simulator::createSpringMotor()
        {
                SpringMotor * newMotor = new SpringMotor();
                addMotor( ( Motor* ) newMotor );
                return newMotor;
        }

        //void Simulator::registerCustomMotor(Motor* m)
        //{
        //  m->internal_setCustom(true);
        //  addMotor(m);
        //}

        void Simulator::destroyMotor( Motor* m )
        {
                removeMotor( m );
        }

        void Simulator::destroyAllMotors()
        {
                for ( size_t i = 0; i < mMotorList.size(); ++i )
                {
                        removeMotor( mMotorList[ i ] );
                }
        }

        AccelerationSensor* Simulator::createAccelerationSensor()
        {
                AccelerationSensor * newSensor = new AccelerationSensor( this );
                addSensor( newSensor );
                return newSensor;
        }

        InclineSensor* Simulator::createInclineSensor()
        {
                InclineSensor * newSensor = new InclineSensor();
                addSensor( newSensor );
                return newSensor;
        }

        RaycastSensor* Simulator::createRaycastSensor()
        {
                RaycastSensor * newSensor = new RaycastSensor( this );
                addSensor( newSensor );
                return newSensor;
        }

        VolumeSensor* Simulator::createVolumeSensor()
        {
                VolumeSensor * newSensor = new VolumeSensor( this );
                addSensor( newSensor );
                return newSensor;
        }

        void Simulator::destroySensor( Sensor* s )
        {
                removeSensor( s );
        }

        void Simulator::destroyAllSensors()
        {
                for ( size_t i = 0; i < mSensorList.size(); ++i )
                {
                        removeSensor( mSensorList[ i ] );
                }
        }

        void Simulator::setSolverAccuracy( SolverAccuracyLevel level )
        {
                mSolverAccuracyLevel = level;
        }

        SolverAccuracyLevel Simulator::getSolverAccuracy() const
        {
                return mSolverAccuracyLevel;
        }

        void Simulator::setMaxCorrectingVel( real vel )
        {
                assert( vel > 0 );
                mMaxCorrectingVel = vel;
        }

        real Simulator::getMaxCorrectingVel() const
        {
                return mMaxCorrectingVel;
        }

        void Simulator::setMaxContacts( unsigned int mc )
        {
                assert( mc <= globals::maxMaxContacts );
                mMaxContacts = mc;
        }

        unsigned int Simulator::getMaxContacts() const
        {
                return mMaxContacts;
        }

        void Simulator::addSolid( Solid* s )
        {
                mSolidList.push_back( s );
        }

        void Simulator::removeSolid( Solid* s )
        {
                // TODO: make this more efficient by not iterating through all Motors,
                // Sensors, and Joints; maybe have the Solid maintain pointers to
                // things that depend on it.

                // Disable Motors that depend on the given Solid.
                std::vector<Motor*>::iterator iter;
                for ( iter = mMotorList.begin(); iter != mMotorList.end(); ++iter )
                {
                        if ( ( *iter ) ->internal_dependsOnSolid( s ) )
                        {
                                ( *iter ) ->setEnabled( false );
                        }
                }

                // Disable Joints that depend on the given Solid.
                std::vector<Joint*>::iterator iter2;
                for ( iter2 = mJointList.begin(); iter2 != mJointList.end(); ++iter2 )
                {
                        if ( ( *iter2 ) ->internal_dependsOnSolid( s ) )
                        {
                                ( *iter2 ) ->setEnabled( false );
                        }
                }

                // Disable Sensors that depend on the given Solid.
                std::vector<Sensor*>::iterator iter3;
                for ( iter3 = mSensorList.begin(); iter3 != mSensorList.end(); ++iter3 )
                {
                        if ( ( *iter3 ) ->internal_dependsOnSolid( s ) )
                        {
                                ( *iter3 ) ->setEnabled( false );
                        }
                }

                // Now remove the Solid.
                for ( size_t i = 0; i < mSolidList.size(); ++i )
                {
                        if ( mSolidList[ i ] == s )
                        {
                                s->internal_destroy();
                                mSolidList[ i ] = mSolidList.back();
                                mSolidList.pop_back();

                                return ;
                        }
                }
        }

        void Simulator::addJoint( Joint* j )
        {
                mJointList.push_back( j );
        }

        void Simulator::removeJoint( Joint* j )
        {
                // TODO: make this more efficient by not iterating through all Motors;
                // maybe have the Solid maintain pointers to things that
                // depend on it.

                // Disable Motors that depend on the given Joint.
                std::vector<Motor*>::iterator iter;
                for ( iter = mMotorList.begin(); iter != mMotorList.end(); ++iter )
                {
                        if ( ( *iter ) ->internal_dependsOnJoint( j ) )
                        {
                                ( *iter ) ->setEnabled( false );
                        }
                }

                // Now remove the Joint.
                for ( size_t i = 0; i < mJointList.size(); ++i )
                {
                        if ( mJointList[ i ] == j )
                        {
                                j->internal_destroy();
                                mJointList[ i ] = mJointList.back();
                                mJointList.pop_back();

                                return ;
                        }
                }
        }

        void Simulator::addMotor( Motor* m )
        {
                mMotorList.push_back( m );
        }

        void Simulator::removeMotor( Motor* m )
        {
                for ( size_t i = 0; i < mMotorList.size(); ++i )
                {
                        if ( mMotorList[ i ] == m )
                        {
                                //if (!m->internal_isCustom())
                                //{
                                //  delete m;
                                //}
                                m->internal_destroy();
                                mMotorList[ i ] = mMotorList.back();
                                mMotorList.pop_back();

                                return ;
                        }
                }
        }

        void Simulator::addSensor( Sensor* s )
        {
                mSensorList.push_back( s );
        }

        void Simulator::removeSensor( Sensor* s )
        {
                for ( size_t i = 0; i < mSensorList.size(); ++i )
                {
                        if ( mSensorList[ i ] == s )
                        {
                                s->internal_destroy();
                                mSensorList[ i ] = mSensorList.back();
                                mSensorList.pop_back();

                                return ;
                        }
                }
        }

        void Simulator::addSpace( Space* s )
        {
                mSpaceList.push_back( s );
        }

        Space* Simulator::getRootSpace()
        {
                return mRootSpace;
        }

        void Simulator::destroyGarbage()
        {
                // Destroy garbage Solids.
                while ( !mSolidGarbageList.empty() )
                {
                        destroySolid( mSolidGarbageList.back() );
                        mSolidGarbageList.pop_back();
                }

                // Destroy garbage Joints.
                while ( !mJointGarbageList.empty() )
                {
                        destroyJoint( mJointGarbageList.back() );
                        mJointGarbageList.pop_back();
                }
        }
}

---