aesop-physics.cpp

Go to the documentation of this file.

/*
 * aesop-physics.cpp
 *
 * Copyright (C) 2008,2009  Thomas A. Vaughan
 * All rights reserved.
 *
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <organization> nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THOMAS A. VAUGHAN ''AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THOMAS A. VAUGHAN BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Implementation of aesop physics engine.
 * For now, this is just a thin wrapper on top of the Bullet engine.
 */

// includes --------------------------------------------------------------------
#include "aesop-physics.h"              // always include our own header first!

#include "bullet/bullet.h"      // bullet physics library

#include "common/wave_ex.h"
#include "datahash/datahash_util.h"
#include "hfield/heightfield.h"
#include "perf/perf.h"
#include "threadsafe/threadsafe_queue.h"
#include "trimesh/trimesh.h"


namespace aesop {


// TODO: make this settable by clients!
static const float s_stepHeight                 = 0.35;         // 35cm
static const int s_ghostCollisionFlags          =
                        btCollisionObject::CF_KINEMATIC_OBJECT;

// interface destructor implementation
PhysicsObject::~PhysicsObject(void) throw() { }
PhysicsShape::~PhysicsShape(void) throw() { }
PhysicsWorld::~PhysicsWorld(void) throw() { }


typedef btTransform Transform3D;


//
//      static physics object ID map
//

typedef std::map<dword_t, smart_ptr<PhysicsObject> > id_object_map_t;
static id_object_map_t s_idMap;
static smart_mutex s_idMapMutex;


template <class T>
long
getNewIdForObject
(
IN smart_ptr<T>& in_obj
)
{
        // attempt to cast
        smart_ptr<PhysicsObject> obj = in_obj;
        ASSERT(obj, "failed to downcast?");

        // lock!
        mlock l(s_idMapMutex);

        // generate a random ID
        for (;;) {
                long id = rand() + 1;
                ASSERT(id > 0, "bad id: %ld", id);
                if (s_idMap.end() == s_idMap.find(id)) {
                        // found an unused ID!
                        s_idMap[id] = obj;
                        return id;
                }
        }
}



smart_ptr<PhysicsObject>
getPhysicsObjectById
(
IN dword_t id
)
{
        ASSERT(id > 0, "Bad id: %ld", (long) id);

        // lock!
        mlock l(s_idMapMutex);

        id_object_map_t::iterator i = s_idMap.find(id);
        if (s_idMap.end() == i) {
                DPRINTF("Object with id %ld not found!", (long) id);
                return NULL;
        }

        return i->second;
}



static void
removeObjectFromIdMap
(
IN long id
)
{
        ASSERT(id > 0, "Bad id: %ld", id);

        // lock!
        mlock l(s_idMapMutex);

        id_object_map_t::iterator i = s_idMap.find(id);
        if (s_idMap.end() != i) {
                s_idMap.erase(i);
        }
}



//
//      static helper methods
//

static inline btVector3
getBulletVectorFromPoint3d
(
IN const point3d_t& pos
)
throw()
{
        return btVector3(pos.x, pos.y, pos.z);
}



template <class T>
static inline point3d_t
getPoint3dFromBulletType
(
IN const T& v
)
throw()
{
        return point3d_t(v.x(), v.y(), v.z());
}



static inline point3d_t
getNullPoint
(
void
)
throw()
{
        point3d_t p;
        p.clear();
        return p;
}



static point3d_t
getCollisionObjectPosition
(
IN const btCollisionObject * obj
)
{
        ASSERT(obj, "null");

        const btTransform& T = obj->getWorldTransform();
        btVector3 localOrigin(0, 0, 0);
        btVector3 position = T(localOrigin);

        return point3d_t(position.x(), position.y(), position.z());
}



static void
dumpVector
(
IN const char * title,
IN const btVector3& v
)
throw()
{
        ASSERT(title, "null");

        DPRINTF("%s: [ %5.1f, %5.1f, %5.1f ]", title,
            v[0], v[1], v[2]);
}



static void
dumpAabb
(
IN const char * title,
IN const btVector3& aabbMin,
IN const btVector3& aabbMax
)
throw()
{
        ASSERT(title, "null");

        DPRINTF("%s: ( %5.1f, %5.1f, %5.1f ) - ( %5.1f, %5.1f, %5.1f )",
            title, aabbMin[0], aabbMin[1], aabbMin[2],
            aabbMax[0], aabbMax[1], aabbMax[2]);
}



static void
dumpCollisionObject
(
IN const char * title,
IN const btCollisionObject * obj
)
throw()
{
        ASSERT(obj, "null");
        ASSERT(sizeof(btScalar) == sizeof(float), "mismatch!");

        // what is it?
        const btCollisionShape * shape = obj->getCollisionShape();
        ASSERT(shape, "null shape for rigid body");

        const char * shape_name = shape->getName();
        if (!title) {
                title = shape_name;
        }

        DPRINTF("  %s", title);
        DPRINTF("    shape name = '%s'", shape->getName());
        DPRINTF("    shape type = %d", shape->getShapeType());
        DPRINTF("    Object flags: %s %s %s %s",
            shape->isPolyhedral() ? "polyhedral" : "",
            shape->isConvex() ? "convex" : "",
            shape->isConcave() ? "concave" : "",
            shape->isCompound() ? "compound" : "");

        // where is it?
        point3d_t pos = getCollisionObjectPosition(obj);
        DPRINTF("    position: (%f, %f, %f)", pos.x, pos.y, pos.z);

        // bounding box
        btTransform T = obj->getWorldTransform();
        btVector3 aabbMin, aabbMax;
        shape->getAabb(T, aabbMin, aabbMax);
        dumpAabb("    aabb from shape", aabbMin, aabbMax);

        T.setIdentity();
        shape->getAabb(T, aabbMin, aabbMax);
        dumpAabb("    aabb (relative)", aabbMin, aabbMax);

        // broadphase proxy (world's additional state)
        const btBroadphaseProxy * proxy = obj->getBroadphaseHandle();
        if (proxy) {
                DPRINTF("    unique ID = %d", proxy->getUid());
        } else {
                DPRINTF("    NULL proxy -- not in world?");
        }

        // rigid body particulars
        const btRigidBody * prb = dynamic_cast<const btRigidBody *>(obj);
        if (prb) {
                DPRINTF("    inverse mass = %f", prb->getInvMass());

                btVector3 linVel = prb->getLinearVelocity();
                dumpVector("    linear velocity", linVel);

                prb->getAabb(aabbMin, aabbMax);
                dumpAabb("    aabb from rigid body ", aabbMin, aabbMax);
        }
}



//
//      ObjectImpl -- class that implements the PhysicsObject interface
//

class ObjectImpl : public PhysicsObject {
public:
        ObjectImpl(void) throw();
        ~ObjectImpl(void) throw() { }

        // aesop::PhysicsObject class interface methods ------------------------
        dword_t getId(void) const throw() { return m_id; }
        int getFlags(void) const throw() { return m_flags; }
        void dump(IN const char * txt) const throw() {
                        DPRINTF("Physics object: %s", txt);
                        DPRINTF("  id: %ld", (long) m_id);
                        dumpCollisionObject(txt, m_obj);
                }
        point3d_t getPosition(void) const throw()
                                { return getCollisionObjectPosition(m_obj); }
        rect3d_t getBoundingBox(void) const throw();
        quaternion_t getOrientation(void) const throw();
        point3d_t getLinearVelocity(void) const throw();
        point3d_t getAngularVelocity(void) const throw();
        void setOrientation(IN const quaternion_t& orient);
        void setLinearVelocity(IN const point3d_t& linear);
        void setAngularVelocity(IN const point3d_t& angular);
        
        void requestMove(IN const point3d_t& delta, IN float dt);
        void setPosition(IN const point3d_t& pos);
        float getMass(void) const throw() { return m_mass; }
        void iterateOverlappingObjects(IN object_iteration_fn fn,
                                IN void * context);
        void setUserPointer(IN void * up) throw() { m_up = up; }
        void * getUserPointer(void) const throw() { return m_up; }

        // public class methods ------------------------------------------------
        void setId(IN dword_t id) throw() { m_id = id; }
        bool isMoveable(void) const throw() { return !!m_ghost; }
        btCollisionObject * getCollisionObject(void) throw() {
                                        ASSERT(m_obj, "null");
                                        return m_obj;
                                }
        btKinematicCharacterController * getCharacter(void)
                                { return m_character; }
        bool wantCollisionEvents(void) const throw()
                                { return eFlag_Events & m_flags; }

        // static factory methods ----------------------------------------------
        static smart_ptr<ObjectImpl> create(IN smart_ptr<btCollisionObject>& o,
                                IN const physics_meta_t& meta);
        static smart_ptr<ObjectImpl> create(
                                IN smart_ptr<btKinematicCharacterController>& c,
                                IN smart_ptr<btPairCachingGhostObject>& ghost,
                                IN const physics_meta_t& meta);

private:
        // private helper methods ----------------------------------------------
        void addContext(IN const physics_meta_t& meta);

        // private member data -------------------------------------------------
        int                                     m_flags;
        dword_t                                 m_id;
        float                                   m_mass;
        void *                                  m_up;
        point3d_t                               m_linVel; // only for moveable
        btRigidBody *                           m_rigidBody; // weak ref!
        smart_ptr<btCollisionObject>            m_obj;
        smart_ptr<btKinematicCharacterController> m_character;
        smart_ptr<btPairCachingGhostObject>     m_ghost;
};

typedef std::vector<ObjectImpl *> vec_objimpl_t;



ObjectImpl::ObjectImpl(void)
throw()
{
        m_id = 0;
        m_flags = 0;
        m_mass = 0.0;
        m_up = 0;
        m_linVel.clear();
}



static ObjectImpl *
getObject
(
IN PhysicsObject * obj
)
{
        ASSERT(obj, "null");
        ObjectImpl * o = dynamic_cast<ObjectImpl *>(obj);
        ASSERT(o, "PhysicsObject not implemented by ObjectImpl?");
        return o;
}



// Funky!  Bullet sometimes provides btCollisionObjects as void *
// I think this is some sort of polymorphism.
// Anyway, for these use cases we can map (using the userPointer)
//  back to ObjectImpl *
static ObjectImpl *
getPhysicsObject
(
IN void * pv
)
{
        ASSERT(pv, "null");
        btCollisionObject * obj = (btCollisionObject *) pv;
        ObjectImpl * poi = (ObjectImpl *) obj->getUserPointer();
        ASSERT(poi, "user pointer was never set?");
        return poi;
}



rect3d_t
ObjectImpl::getBoundingBox
(
void
)
const
throw()
{
        ASSERT(m_obj, "null collision object");
        const btCollisionShape * shape = m_obj->getCollisionShape();

        btTransform T = m_obj->getWorldTransform();
        btVector3 aabbMin, aabbMax;
        shape->getAabb(T, aabbMin, aabbMax);

        rect3d_t r;
        r.x0 = aabbMin.x();
        r.y0 = aabbMin.y();
        r.z0 = aabbMin.z();
        r.x1 = aabbMax.x();
        r.y1 = aabbMax.y();
        r.z1 = aabbMax.z();
        return r;
}



quaternion_t
ObjectImpl::getOrientation
(
void
)
const
throw()
{
        ASSERT(m_obj, "null collision object?");

        const btTransform& T = m_obj->getWorldTransform();
        btQuaternion q = T.getRotation();
        quaternion_t retval;
        retval.x = q.x();
        retval.y = q.y();
        retval.z = q.z();
        retval.w = q.w();
        return retval;
}



point3d_t
ObjectImpl::getLinearVelocity
(
void
)
const
throw()
{
        ASSERT(m_obj, "null collision object?");

        // are we moveable?
        if (this->isMoveable()) {
                //DPRINTF("Moveable object has linear velocity!");
                //m_linVel.dump("  here it is");
                return m_linVel;
        }

        // not moveable: see what bullet says
        if (m_rigidBody) {
                return getPoint3dFromBulletType(
                    m_rigidBody->getLinearVelocity());
        }

        // no idea!  Say zero
        //DPRINTF("Unknown velocity!");
        return getNullPoint();
}



point3d_t
ObjectImpl::getAngularVelocity
(
void
)
const
throw()
{
        ASSERT(m_obj, "null collision object?");

        if (m_rigidBody) {
                return getPoint3dFromBulletType(
                    m_rigidBody->getAngularVelocity());
        }
        return getNullPoint();
}



void
ObjectImpl::setOrientation
(
IN const quaternion_t& orient
)
{
        ASSERT(m_obj, "requires object");

//      orient.dump("Setting orientation");

        // TODO: is there a cleaner way to set orientation?
        // this is what I do:
        //  - get the position
        //  - create a new transform for position and orientation

        point3d_t pos = this->getPosition();
        btTransform T;
        T.setOrigin(getBulletVectorFromPoint3d(pos));
        btQuaternion q(orient.x, orient.y, orient.z, orient.w);
        T.setRotation(q);

        m_obj->setWorldTransform(T);
}



void
ObjectImpl::setLinearVelocity
(
IN const point3d_t& linear
)
{
        ASSERT(m_obj, "requires object");

        // moveable object?
        if (this->isMoveable()) {
                this->requestMove(linear, 1.0);
                return;
        }

        if (!m_rigidBody) {
                return;
        }

        m_rigidBody->setLinearVelocity(getBulletVectorFromPoint3d(linear));
}



void
ObjectImpl::setAngularVelocity
(
IN const point3d_t& angular
)
{
        ASSERT(m_obj, "requires object");
        if (!m_rigidBody) {
                return;
        }

        m_rigidBody->setAngularVelocity(getBulletVectorFromPoint3d(angular));
}



void
ObjectImpl::requestMove
(
IN const point3d_t& delta,
IN float dt
)
{
        ASSERT(dt > 0.0, "bad time delta: %f", dt);
        if (!this->isMoveable()) {
                DPRINTF("Asking a non-moveable object to move?");
                return;
        }
        ASSERT(m_character, "null");

        // TODO: we are hoping that another requestMove() update comes in
        //      before this one expires!  We (outside of the character)
        //      have no idea when the time dt expires.
        // We probably need a bullet fix for this: moving character objects
        //      should advertise their linear velocity.
        m_linVel = (1.0 / dt) * delta;
//      m_linVel.dump("Velocity");
        m_character->setVelocityForTimeInterval(
                getBulletVectorFromPoint3d(m_linVel), dt);
}



void
ObjectImpl::setPosition
(
IN const point3d_t& pos
)
{
        // create a new world transform
        btTransform T;
        T.setIdentity();
        T.setOrigin(getBulletVectorFromPoint3d(pos));

        // move
        m_obj->setWorldTransform(T);
}



void
ObjectImpl::iterateOverlappingObjects
(
IN object_iteration_fn fn,
IN void * context
)
{
        ASSERT(fn, "null");
        // ASSERT(context) -- we don't care!

        btGhostObject * ghost =
            dynamic_cast<btGhostObject *>((btCollisionObject *) m_obj);
        if (!ghost) {
                DPRINTF("WARNING: can only iterate overlapping pairs for "
                    "ghost objects!");
                return;         // do nothing
        }

        int numObjects = ghost->getNumOverlappingObjects();
        for (int i = 0; i < numObjects; ++i) {
                btCollisionObject * cobj = ghost->getOverlappingObject(i);
                ASSERT(cobj, "null");

                ObjectImpl * other = getPhysicsObject(cobj);
                ASSERT(other, "null");

                // get ref-counted object
                // TODO: check performance impact of this additional lookup!
                long id = other->getId();
                smart_ptr<PhysicsObject> obj = getPhysicsObjectById(id);
                ASSERT(obj, "null");

                // call the provided callback function
                fn(obj, context);
        }
}



void
ObjectImpl::addContext
(
IN const physics_meta_t& meta
)
{
        // directly copy some meta information to instance
        m_flags = meta.objectFlags;

        // copy mass
        m_mass = meta.mass;

        // attempt an upcast
        m_rigidBody = dynamic_cast<btRigidBody *>((btCollisionObject *) m_obj);

        // store backpointer in bullet collision object
        m_obj->setUserPointer((void *) this);
}



smart_ptr<ObjectImpl>
ObjectImpl::create
(
IN smart_ptr<btCollisionObject>& o,
IN const physics_meta_t& meta
)
{
        ASSERT(o, "null");

        smart_ptr<ObjectImpl> local = new ObjectImpl;
        ASSERT(local, "out of memory");
        local->m_obj = o;

        local->addContext(meta);

        return local;
}



smart_ptr<ObjectImpl>
ObjectImpl::create
(
IN smart_ptr<btKinematicCharacterController>& character,
IN smart_ptr<btPairCachingGhostObject>& ghost,
IN const physics_meta_t& meta
)
{
        ASSERT(character, "null");
        ASSERT(ghost, "null");

        smart_ptr<ObjectImpl> local = new ObjectImpl;
        ASSERT(local, "out of memory");

        local->m_character = character;
        local->m_ghost = ghost;
        local->m_obj = ghost;

        local->addContext(meta);

        return local;
}



//
//      ShapeImpl -- class that implements the PhysicsShape interface
//

class ShapeImpl : public PhysicsShape {
public:
        ~ShapeImpl(void) throw() { }

        // sigh... need yet another place to store bullet state! ---------------
        struct private_context_t {
                smart_ptr<btTriangleIndexVertexArray>           tiva;
        };

        // aesop::PhysicsShape class interface methods ------------------------
        void setContext(IN void * ctx) throw() {
                        ASSERT(m_shape, "null");
                        m_shape->setUserPointer((void *) ctx);
                }

        void * getContext(void) throw() {
                        ASSERT(m_shape, "null");
                        return m_shape->getUserPointer();
                }

        // public class methods ------------------------------------------------
        btCollisionShape * getShape(void) throw() {
                        ASSERT(m_shape, "null");
                        return m_shape;
                }

        // static factory methods ----------------------------------------------
        static smart_ptr<ShapeImpl> create(IN smart_ptr<btCollisionShape>& shape,
                                IN private_context_t * ctx = NULL)
                {
                        ASSERT(shape, "null");
                        smart_ptr<ShapeImpl> local = new ShapeImpl;
                        ASSERT(local, "out of memory");
                        local->m_shape = shape;
                        if (ctx) {
                                local->m_ctx = *ctx;
                        }
                        return local;
                }

private:
        // private data members ------------------------------------------------
        smart_ptr<btCollisionShape>     m_shape;
        private_context_t               m_ctx;
};



static ShapeImpl *
getShape
(
IN const PhysicsShape * s
)
{
        ASSERT(s, "null");

        const ShapeImpl * csi = dynamic_cast<const ShapeImpl *>(s);
        ASSERT(csi, "PhysicsShape not implemented with ShapeImpl?");

        // have to cast away const-ness
        return (ShapeImpl *) csi;
}



//
//      World -- class that implements the aesop::PhysicsWorld interface
//

class World : public PhysicsWorld {
public:
        World(void) throw();
        ~World(void) throw() { this->removeAll(); }

        // public class methods ------------------------------------------------
        void initialize(IN const Datahash * params);

        // aesop::PhysicsWorld class interface methods --------------------------
        void addObject(IN smart_ptr<PhysicsObject>&obj);
        void removeObject(IN PhysicsObject * obj);
        long getObjectCount(void) throw();
        void tick(IN float seconds);

        void getObjectIterator(OUT object_iterator_t& i);
        bool getNextObject(IO object_iterator_t& i,
                                OUT smart_ptr<PhysicsObject>& obj);

        void getCollisionIterator(OUT collision_iterator_t& i);
        bool getNextCollision(IO collision_iterator_t& i,
                                OUT collision_record_t& cr);

        smart_ptr<PhysicsObject> rayTest(IN const point3d_t& from,
                                IN const point3d_t& to);

private:
        // private typedefs ----------------------------------------------------
        struct real_obj_iter_t {
                dword_t                         magic;
                dword_t                         rvn;
                id_object_map_t::iterator       iter;
        };

        struct real_collision_iter_t {
                dword_t                         magic;
                dword_t                         rvn;
                int                             iManifold;
        };

        // private helper methods ----------------------------------------------
        static real_obj_iter_t * getRealObjectIterator(
                                IN object_iterator_t& i) throw()
                { return (real_obj_iter_t *) &i; }

        static real_collision_iter_t * getRealCollisionIterator(
                                IN collision_iterator_t& i) throw()
                { return (real_collision_iter_t *) &i; }

        void removeObjectInternal(IN ObjectImpl * o);
        void removeAll(void);

        // private member data -------------------------------------------------
        id_object_map_t                                 m_objects;
        int                                             m_maxTimeSteps;
        float                                           m_engineTimeStep;
        smart_ptr<btDefaultCollisionConfiguration>      m_colcfg;
        smart_ptr<btCollisionDispatcher>                m_dispatch;
        smart_ptr<btAxisSweep3>                         m_as3;
        smart_ptr<btSequentialImpulseConstraintSolver>  m_solver;
        smart_ptr<btDiscreteDynamicsWorld>              m_world;
        smart_mutex                                     m_mutex;
        dword_t                                         m_rvn; // record version
};

static const dword_t s_dwMagicObject            = 0x34892199;
static const dword_t s_dwMagicCollision         = 0x55784923;



World::World
(
void
)
throw()
{
        m_rvn = 777;            // random
}



void
World::initialize
(
IN const Datahash * params
)
{
        ASSERT(params, "null");

        // first: sanity checks on iterator sizes
        ASSERT2(sizeof(object_iterator_t) >= sizeof(real_obj_iter_t),
            "Public iterator is too small!  " << sizeof(object_iterator_t)
            << " bytes vs. " << sizeof(real_obj_iter_t) << " bytes.");
        ASSERT2(sizeof(collision_iterator_t) >= sizeof(real_collision_iter_t),
            "Public iterator is too small!  " << sizeof(collision_iterator_t)
            << " bytes vs. " << sizeof(real_collision_iter_t) << " bytes.");

        smart_ptr<Datahash> phys = getSubhash(params, "physics");
        ASSERT(phys, "null physics subhash?");

        // look up any parameters from physics section?
        float minCoord = getFloat(phys, "minCoord");
        float maxCoord = getFloat(phys, "maxCoord");
        ASSERT(maxCoord > minCoord, "Bad min/max = %f / %f",
            minCoord, maxCoord);

        m_maxTimeSteps = getInt(phys, "maxTimeSteps");
        ASSERT(m_maxTimeSteps > 0, "Bad maxTimeSteps: %d",
            m_maxTimeSteps);
        m_engineTimeStep = getFloat(phys, "engineTimeStep");
        ASSERT(m_engineTimeStep > 0.0, "Bad engineTimeStep: %f",
            m_engineTimeStep);

        int gravAxis = getInt(phys, "gravityAxis");
        ASSERT(gravAxis > -2 && gravAxis < 3, "Bad gravity axis: %d",
            gravAxis);
        float gravity = getFloat(phys, "gravity");

        // initialize bullet objects
        // TODO: understand these better!  See test_bullet code...
        DPRINTF("Initializing Bullet physics engine...");
        DPRINTF("  physics engine time step = %f seconds",
            m_engineTimeStep);

        // collision configuration
        m_colcfg = new btDefaultCollisionConfiguration();
        ASSERT(m_colcfg, "failed to create collision config");

        // collision dispatcher
        m_dispatch = new btCollisionDispatcher(m_colcfg);
        ASSERT(m_dispatch, "failed to create collision dispatcher");

        // create axis-aligned bounding box (AABB)
        btVector3 min(minCoord, minCoord, minCoord);
        btVector3 max(maxCoord, maxCoord, maxCoord);

        m_as3 = new btAxisSweep3(min, max);
        ASSERT(m_as3, "Failed to create axis sweep object");

        // constraint solver
        m_solver = new btSequentialImpulseConstraintSolver;
        ASSERT(m_solver, "failed to create constraint solver");

        // okay, finally create the physics simulation world
        m_world = new btDiscreteDynamicsWorld(m_dispatch, m_as3, m_solver,
            m_colcfg);
        ASSERT(m_world, "failed to create bullet physics world");

        // set up for ghost collisions
        m_as3->getOverlappingPairCache()->setInternalGhostPairCallback(
            new btGhostPairCallback);

        // set gravity
        if (gravAxis >= 0) {
                btVector3 grav(0, 0, 0);
                grav[gravAxis] = gravity;
                dumpVector("  world gravity", grav);
                m_world->setGravity(grav);
        }

        // all done!
        DPRINTF("Finished initializing physics engine...");
}



//
//      World -- PhysicsWorld class interface methods
//

void
World::addObject
(
IN smart_ptr<PhysicsObject>& obj
)
{
        ASSERT(obj, "null");

//      obj->dump("Adding object!");

        ObjectImpl * o = getObject(obj);
        ASSERT(o, "null");

        // lock!
        mlock l(m_mutex);
        ++m_rvn;        // modifying data structures...

        // get an id
        long id = getNewIdForObject(obj);
        o->setId(id);

        btCollisionObject * co = o->getCollisionObject();
        btRigidBody * rb = dynamic_cast<btRigidBody *>(co);
        if (rb) {
//              DPRINTF("  Adding as rigid body...");
                m_world->addRigidBody(rb);
        } else {
//              DPRINTF("  Adding as collision object...");

                int filterGroup = btBroadphaseProxy::DefaultFilter;
                int filterMask = btBroadphaseProxy::AllFilter;
                m_world->addCollisionObject(co, filterGroup, filterMask);
        }

        if (o->isMoveable()) {
                DPRINTF("  Adding character...");
                m_world->addCharacter(o->getCharacter());
        }

        // add to our map
        m_objects[id] = obj;
}



void
World::removeObject
(
IN PhysicsObject * obj
)
{
        ASSERT(obj, "null");
        ObjectImpl * o = getObject(obj);
        ASSERT(o, "null");

        // lock!
        mlock l(m_mutex);
        ++m_rvn;        // updating map
        this->removeObjectInternal(o);
}



long
World::getObjectCount
(
void
)
throw()
{
        // lock!
        mlock l(m_mutex);

        int n0 = m_world->getNumCollisionObjects();
        int n1 = m_objects.size();
        ASSERT(n0 == n1, "World has %d objects, we have %d", n0, n1);
        return n1;
}



void
World::tick
(
IN float seconds
)
{
        ASSERT(seconds > 0.0, "Bad tick duration: %f", seconds);

        // lock!
        mlock l(m_mutex);
        ++m_rvn;        // simulation changes state

        // wrap simulation calls with a timer
        perf::Timer timer("PhysicsWorld::tick");
        long nSteps = (long) (seconds / m_engineTimeStep);
        if (nSteps > m_maxTimeSteps) {
                DPRINTF("WARNING: losing time");
        }
        m_world->stepSimulation(seconds, m_maxTimeSteps,
            m_engineTimeStep);
}



void
World::getObjectIterator
(
OUT object_iterator_t& i
)
{
        real_obj_iter_t * ri = getRealObjectIterator(i);
        ri->magic = s_dwMagicObject;

        // lock!
        mlock l(m_mutex);
        ri->rvn = m_rvn;
        ri->iter = m_objects.begin();
}



bool
World::getNextObject
(
IO object_iterator_t& i,
OUT smart_ptr<PhysicsObject>& obj
)
{
        obj = NULL;
        real_obj_iter_t * ri = getRealObjectIterator(i);
        if (s_dwMagicObject != ri->magic) {
                WAVE_EX(wex);
                wex << "Invalid iterator passed to getNextObject()";
        }

        // lock!
        mlock l(m_mutex);
        if (ri->rvn != m_rvn)
                return false;           // something changed!

        if (m_objects.end() == ri->iter)
                return false;           // end of iteration

        // have a valid iterator
        obj = ri->iter->second;
        ASSERT(obj, "null object in our map");
        ++ri->iter;
        return true;
}



void
World::getCollisionIterator
(
OUT collision_iterator_t& i
)
{
        real_collision_iter_t * ri = getRealCollisionIterator(i);
        ri->magic = s_dwMagicCollision;

        // lock!
        mlock l(m_mutex);
        ri->rvn = m_rvn;
        ri->iManifold = 0;
}



bool
World::getNextCollision
(
IO collision_iterator_t& i,
OUT collision_record_t& cr
)
{
        cr.clear();
        real_collision_iter_t * ri = getRealCollisionIterator(i);
        if (s_dwMagicCollision != ri->magic) {
                WAVE_EX(wex);
                wex << "Invalid collision iterator in getNextCollision()";
        }

        // lock!
        mlock l(m_mutex);

        // does the record version number (rvn) match?
        if (m_rvn != ri->rvn)
                return false;   // something has changed

        int N = m_dispatch->getNumManifolds();
        if (ri->iManifold >= N)
                return false;   // end of iteration

        // loop through remaining manifolds
        for (; ri->iManifold < N; ++ri->iManifold) {
                btPersistentManifold * m =
                    m_dispatch->getManifoldByIndexInternal(ri->iManifold);
                if (!m || !m->getNumContacts())
                        continue;       // skip boring manifolds

                ObjectImpl * o0 = getPhysicsObject(m->getBody0());
                ObjectImpl * o1 = getPhysicsObject(m->getBody1());

                if (o0->wantCollisionEvents() &&
                    o1->wantCollisionEvents()) {
                        // got a collision!
                        cr.obj1 = getPhysicsObjectById(o0->getId());
                        cr.obj2 = getPhysicsObjectById(o1->getId());
                        ASSERT(cr.obj1 && cr.obj2,
                            "objects in collision not in global id map");
                        return true;
                }
        }

        // got here?  end of iteration
        return false;
}



smart_ptr<PhysicsObject>
World::rayTest
(
IN const point3d_t& from,
IN const point3d_t& to
)
{
        perf::Timer timer("PhysicsWorld::rayTest");
        ASSERT(m_world, "null");

        // xform to btVector3 objects
        btVector3 btFrom = getBulletVectorFromPoint3d(from);
        btVector3 btTo = getBulletVectorFromPoint3d(to);

        // construct raycast callback object
        btCollisionWorld::ClosestRayResultCallback rayCallback(btFrom, btTo);

        // lock
        mlock m(m_mutex);

        // raycast in world
        m_world->rayTest(btFrom, btTo, rayCallback);

        // hit anything?
        if (!rayCallback.hasHit())
                return NULL;            // nope

        // hit something--figure out what it was!
        ASSERT(rayCallback.m_collisionObject, "null");
        ObjectImpl * poi = getPhysicsObject(rayCallback.m_collisionObject);
        ASSERT(poi, "null");

        // a bit weird--we need to do another lookup to verify we hand back
        //   a safely ref-counted pointer.  If this becomes a performance
        //   issue, we may need another level of indirection in the
        //   bullet user pointer.
        long id = poi->getId();
        ASSERT(id, "bad object ID");
        return getPhysicsObjectById(id);
}



//
//      World -- private helper methods
//

void
World::removeObjectInternal
(
IN ObjectImpl * o
)
{
        // NOTE: assumes the caller has taken the mutex!
        // also, caller should update rvn if appropriate
        ASSERT(o, "null");

        // take out of global ID map
        long id = o->getId();
        removeObjectFromIdMap(id);
        o->setId(0);            // no longer ID'd

        // remove from world
        btCollisionObject * bto = o->getCollisionObject();
        btRigidBody * btr = dynamic_cast<btRigidBody *>(bto);
        if (btr) {
                m_world->removeRigidBody(btr);
        } else {
                m_world->removeCollisionObject(bto);
        }

        btKinematicCharacterController * btc = o->getCharacter();
        if (btc) {
                m_world->removeCharacter(btc);
        }

        // remove from our collection
        id_object_map_t::iterator i = m_objects.find(id);
        if (m_objects.end() != i) {
                m_objects.erase(i);
        }
}



void
World::removeAll
(
void
)
{
        // NOTE: assumes caller has taken mutex!
        // caller should also increment rvn if appropriate
        while (m_objects.size() > 0) {
                id_object_map_t::iterator i = m_objects.begin();
                smart_ptr<PhysicsObject> obj = i->second;
                ASSERT(obj, "null object in our id map");

                ObjectImpl * o = getObject(obj);
                ASSERT(o, "null");

                this->removeObjectInternal(o);
        }
}



//
//      public API
//

void
getVectorFromEulerAngles
(
IN const point3d_t& euler,
OUT point3d_t& vector
)
throw()
{
        btQuaternion q;
        q.setEulerZYX(euler.z, euler.y, euler.x);

        btTransform T(q);

        btVector3 out = T * btVector3(0, 0, 1);
        vector.x = out.x();
        vector.y = out.y();
        vector.z = out.z();
}



void
getVectorFromQuaternion
(
IN const quaternion_t& q,
OUT point3d_t& vector
)
throw()
{
        btQuaternion Q(q.x, q.y, q.z, q.w);
        btTransform T(Q);

        btVector3 out = T * btVector3(0, 0, 1);
        vector.x = out.x();
        vector.y = out.y();
        vector.z = out.z();
}



void
dumpWorld
(
IN PhysicsWorld * world
)
throw()
{
        ASSERT(world, "null");

        DPRINTF("Debug information for physics world:");

        PhysicsWorld::object_iterator_t i;
        world->getObjectIterator(i);
        smart_ptr<PhysicsObject> obj;
        while (world->getNextObject(i, obj)) {
                ASSERT(obj, "null");
                obj->dump("");
        }
}




void
getTransformFromPlacement
(
IN const placement_t& placement,
OUT Transform3D& T
)
{
        T.setIdentity();
        T.setOrigin(getBulletVectorFromPoint3d(placement.position));
        btQuaternion q(placement.rotation.x, placement.rotation.y,
                        placement.rotation.z, placement.rotation.w);
        T.setRotation(q);
}



smart_ptr<PhysicsShape>
createBoxShape
(
IN const point3d_t& dimensions
)
{
        ASSERT(dimensions.x >= 0.0, "Bad box x-dimension: %f", dimensions.x);
        ASSERT(dimensions.y >= 0.0, "Bad box y-dimension: %f", dimensions.y);
        ASSERT(dimensions.z >= 0.0, "Bad box z-dimension: %f", dimensions.z);

        btVector3 halfExtent(0.5 * dimensions.x, 0.5 * dimensions.y,
            0.5 * dimensions.z);
        smart_ptr<btCollisionShape> shape = new btBoxShape(halfExtent);
        ASSERT(shape, "out of memory");

        return ShapeImpl::create(shape);
}



smart_ptr<PhysicsShape>
createCubeShape
(
IN float edge_length
)
{
        ASSERT(edge_length > 0.0, "Bad cube edge length: %f", edge_length);

        return createBoxShape(point3d_t(edge_length, edge_length, edge_length));
}



smart_ptr<PhysicsShape>
createTrimeshShape
(
IN const trimesh::Trimesh * trimesh
)
{
        ASSERT(trimesh, "null");

        const int triangleStride = 3 * sizeof(int);
        const int vertexStride = sizeof(point3d_t);

        // create object that lets bullet access vertex/triangle arrays
        // we put this in shape private context so the shape can own it
        ShapeImpl::private_context_t pctx;
        pctx.tiva = new btTriangleIndexVertexArray(
            trimesh->getTriangleCount(), (int *) trimesh->getTriangleArray(), triangleStride,
            trimesh->getVertexCount(), (btScalar *) trimesh->getVertexArray(), vertexStride);
        ASSERT(pctx.tiva, "out of memory");

        // create actual bullet triangle mesh
        btBvhTriangleMeshShape * btmShape =
            new btBvhTriangleMeshShape(pctx.tiva, true);
        ASSERT(btmShape, "null");

        // wrap and return
        smart_ptr<btCollisionShape> shape = btmShape;
        return ShapeImpl::create(shape, &pctx);
}



smart_ptr<PhysicsShape>
createHeightfieldShape
(
IN const hfield::Heightfield * field
)
{
        ASSERT(field, "null");

        DPRINTF("Constructing heightfield shape");

        // construct btHeightfieldTerrainShape
        long width = field->getWidth();
        long length = field->getLength();
        DPRINTF("  width = %ld", width);
        DPRINTF("  length= %ld", length);
        ASSERT(width > 1 && length > 1, "Bad width/length?");

        long nSize = width * length;
        DPRINTF("  size  = %ld", nSize);

        float yScale = field->getYScale();
        DPRINTF("  yScale = %f", yScale);
        ASSERT(yScale > 0.0, "Bad y-scale: %f", yScale);

        const short * raw = field->getHeightArray();
        ASSERT(raw, "null height array?");

        // min/max height
        float minHeight = field->getMinHeight() * yScale;
        float maxHeight = field->getMaxHeight() * yScale;
        DPRINTF("  min = %f", minHeight);
        DPRINTF("  max = %f", maxHeight);

        int upAxis = 1;         // y-axis is "up"

        btHeightfieldTerrainShape * terrain =
            new btHeightfieldTerrainShape(width, length, (void *) raw, yScale,
                                          minHeight, maxHeight, upAxis,
                                          PHY_SHORT, false);
        ASSERT(terrain, "out of memory");
        DPRINTF("Successfully created a btHeightfieldTerrainShape!");

        // apply local scaling (grid spacing)
        float xzScale = field->getXZScale();
        ASSERT(xzScale > 0.0, "Bad xz scale: %f", xzScale);
        btVector3 localScaling(xzScale, 1.0, xzScale);
        terrain->setLocalScaling(localScaling);

        // wrap and return
        smart_ptr<btCollisionShape> shape = terrain;
        return ShapeImpl::create(shape);
}



smart_ptr<PhysicsShape>
createCapsuleShape
(
IN float height,
IN float radius
)
{
        ASSERT(height > 0, "Bad capsule height: %f", height);
        ASSERT(radius > 0, "Bad capsule radius: %f", radius);

        smart_ptr<btCapsuleShape> capsule = new btCapsuleShape(radius, height);
        ASSERT(capsule, "out of memory");

        smart_ptr<btCollisionShape> shape = capsule;
        return ShapeImpl::create(shape);
}



smart_ptr<PhysicsObject>
createMoveableObject
(
IN const physics_meta_t& meta,
IN const placement_t& placement
)
{
        ASSERT(meta.shape, "null");
        ASSERT(meta.mass >= 0.0, "Bad mass: %f", meta.mass);

        // create transform from instance
        Transform3D T;
        getTransformFromPlacement(placement, T);

        // get actual bullet shape
        ShapeImpl * si = getShape(meta.shape);
        ASSERT(si, "null");
        btCollisionShape * shape1 = si->getShape();
        ASSERT(shape1, "null");
        btConvexShape * shape = dynamic_cast<btConvexShape *>(shape1);
        ASSERT(shape, "moveable objects must use convex shapes!");

        // okay, create ghost
        smart_ptr<btPairCachingGhostObject> ghost =
            new btPairCachingGhostObject;
        ASSERT(ghost, "out of memory");
        ghost->setWorldTransform(T);
        ghost->setCollisionShape(shape);
        ghost->setCollisionFlags(s_ghostCollisionFlags);

        // create character
        smart_ptr<btKinematicCharacterController> character =
            new btKinematicCharacterController(ghost, shape, s_stepHeight);
        ASSERT(character, "out of memory");

        // create this object
        return ObjectImpl::create(character, ghost, meta);
}



smart_ptr<PhysicsObject>
createObject
(
IN const physics_meta_t& meta,
IN const placement_t& placement
)
{
        ASSERT(meta.shape, "null");
        ASSERT(meta.mass >= 0.0, "Bad mass: %f", meta.mass);

        // quick shortcut--moveable is handled differently for now...
        if (PhysicsObject::eFlag_Moveable & meta.objectFlags) {
                return createMoveableObject(meta, placement);
        }

        // create transform from instance
        Transform3D T;
        getTransformFromPlacement(placement, T);

        bool isDynamic = (meta.mass > 0.0);

        // get actual bullet shape
        ShapeImpl * si = getShape(meta.shape);
        ASSERT(si, "null");
        btCollisionShape * shape = si->getShape();
        ASSERT(shape, "null");

        smart_ptr<btCollisionObject> obj;
        if (PhysicsObject::eFlag_Ghost & meta.objectFlags) {
                if (meta.mass > 0.0) {
                        WAVE_EX(wex);
                        wex << "Only immobile ghost objects allowed for now.";
                }

                // okay, create ghost
                smart_ptr<btPairCachingGhostObject> ghost =
                    new btPairCachingGhostObject;
                ASSERT(ghost, "out of memory");
                ghost->setWorldTransform(T);
                ghost->setCollisionShape(shape);

                int flags = s_ghostCollisionFlags;
                if (PhysicsObject::eFlag_NoCollisions & meta.objectFlags) {
                        // override default ghost behavior
                        DPRINTF("Updating flags!");
                        flags = btCollisionObject::CF_NO_CONTACT_RESPONSE;
                }
                ghost->setCollisionFlags(flags);
                obj = ghost;
        } else {
                // not a ghost!  add real object

                // determine inertia
                btVector3 localInertia(0, 0, 0);
                if (isDynamic) {
                        shape->calculateLocalInertia(meta.mass, localInertia);
                }

                // leak!
                btDefaultMotionState * motionState = new btDefaultMotionState(T);
                ASSERT(motionState, "null");
                motionState->setWorldTransform(T);

                // create rigid body
                btRigidBody::btRigidBodyConstructionInfo
                    info(meta.mass, motionState, shape, localInertia);
                smart_ptr<btRigidBody> body = new btRigidBody(info);
                ASSERT(body, "out of memory");
                obj = body;
        }

        // wrap
        ASSERT(obj, "should have collision object now");
        return ObjectImpl::create(obj, meta);
}



smart_ptr<PhysicsWorld>
PhysicsWorld::create
(
IN const Datahash * params
)
{
        ASSERT(params, "null");

        smart_ptr<World> local = new World;
        ASSERT(local, "out of memory");

        local->initialize(params);

        return local;
}



smart_ptr<PhysicsObject>
getObjectHitFromPlacement
(
IN PhysicsWorld * world,
IN const placement_t& placement,
IN float half_extent
)
{
        ASSERT(world, "null");
        ASSERT(half_extent > 0, "Bad half extent: %f", half_extent);

        const float max_range = 256.0;

        // get the orientation (what direction the object is pointed)
        point3d_t dir;
        getVectorFromQuaternion(placement.rotation, dir);

        point3d_t from = placement.position + half_extent * dir;
        point3d_t to = from + max_range * dir;

        return world->rayTest(from, to);
}



};              // aesop namespace