Update the physics engine: interpolate between frames so track to/camera actuators...
authorKester Maddock <Christopher.Maddock.1@uni.massey.ac.nz>
Sat, 4 Dec 2004 09:51:04 +0000 (09:51 +0000)
committerKester Maddock <Christopher.Maddock.1@uni.massey.ac.nz>
Sat, 4 Dec 2004 09:51:04 +0000 (09:51 +0000)
source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h
source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h
source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h
source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp
source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp

index b0e0717cf8cf8b1e16513d593b50812ef9178062..08e29265d0c0ee956820f6a31f5aab2bc801abde 100644 (file)
@@ -61,6 +61,7 @@ public:
        void integrateBackward(MT_Scalar timeStep, const MT_Vector3 &velocity, const MT_Quaternion& ang_vel);
        void integrateForward(MT_Scalar timeStep, const SM_MotionState &prev_state);
        
+       void lerp(const SM_MotionState &prev, const SM_MotionState &next);
        void lerp(MT_Scalar t, const SM_MotionState &other);
        
        virtual MT_Transform getTransform() const {
index b37e1a5466b0d4848ed7572c933d614176272331..07eb5a7637fa8bc3d6e461324400681a4f803e50 100644 (file)
@@ -81,7 +81,8 @@ public:
  * It encapsulates an object in the physics scene, and is responsible
  * for calculating the collision response of objects.
  */
-class SM_Object : public SM_MotionState {
+class SM_Object 
+{
 public:
        SM_Object() ;
        SM_Object(
@@ -266,7 +267,29 @@ public:
        
        void relax();
        
+       SM_MotionState &getCurrentFrame();
+       SM_MotionState &getPreviousFrame();
+       SM_MotionState &getNextFrame();
+       
+       const SM_MotionState &getCurrentFrame()   const;
+       const SM_MotionState &getPreviousFrame()  const;
+       const SM_MotionState &getNextFrame()      const;
+
+       // Motion state functions       
+       const MT_Point3&     getPosition()        const;
+       const MT_Quaternion& getOrientation()     const;
+       const MT_Vector3&    getLinearVelocity()  const;
+       const MT_Vector3&    getAngularVelocity() const;
+       
+       MT_Scalar            getTime()            const;
+       
+       void setTime(MT_Scalar time);
+       
+       void interpolate(MT_Scalar timeStep);
+       void endFrame();
+       
 private:
+       friend class Contact;
        // Tweak parameters
        static MT_Scalar ImpulseThreshold;
 
@@ -324,14 +347,12 @@ private:
        MT_Vector3              m_reaction_impulse;      // The accumulated impulse resulting from collisions
        MT_Vector3              m_reaction_force;        // The reaction force derived from the reaction impulse   
 
-       unsigned int            m_kinematic      : 1;    // Have I been displaced (translated, rotated, scaled) in this frame? 
-       unsigned int            m_prev_kinematic : 1;    // Have I been displaced (translated, rotated, scaled) in the previous frame? 
-       unsigned int            m_is_rigid_body  : 1;    // Should friction give me a change in angular momentum?
-
        MT_Vector3              m_lin_mom;               // Linear momentum (linear velocity times mass)
        MT_Vector3              m_ang_mom;               // Angular momentum (angualr velocity times inertia)
        MT_Vector3              m_force;                 // Force on center of mass (afffects linear momentum)
-       MT_Vector3              m_torque;                // Torque around center of mass (affects angualr momentum)
+       MT_Vector3              m_torque;                // Torque around center of mass (affects angular momentum)
+       
+       SM_MotionState          m_frames[3];             
        
        MT_Vector3              m_error;                 // Error in position:- amount object must be moved to prevent intersection with scene
 
@@ -355,6 +376,12 @@ private:
        MT_Scalar               m_inv_mass;              // 1/mass
        MT_Vector3              m_inv_inertia;           // [1/inertia_x, 1/inertia_y, 1/inertia_z]
        MT_Matrix3x3            m_inv_inertia_tensor;    // Inverse Inertia Tensor
+       
+       bool                    m_kinematic;             // Have I been displaced (translated, rotated, scaled) in this frame? 
+       bool                    m_prev_kinematic;        // Have I been displaced (translated, rotated, scaled) in the previous frame? 
+       bool                    m_is_rigid_body;         // Should friction give me a change in angular momentum?
+       int                     m_static;                // temporarily static.
+
 };
 
 #endif
index 80e8c635cbb0fdd11e5f99f36f2b927d4b40bac4..a0d5c983031dbcb964603aeee6605e6a962d992f 100644 (file)
@@ -104,6 +104,7 @@ public:
        // 'subSampling' can be used to control aliasing effects
        // (fast moving objects traversing through walls and such). 
        bool proceed(MT_Scalar curtime, MT_Scalar ticrate);
+       void proceed(MT_Scalar subStep);
 
        /**
         * Test whether any objects lie on the line defined by from and
@@ -167,7 +168,7 @@ private:
         * collision tests. */
        T_ObjectList        m_objectList;
        
-       MT_Scalar           m_lastTime;
+       unsigned int        m_frames;
 };
 
 #endif
index a3285b191165f6a95fa28720c8be9dccc001b989..1fadbdadfa02f2ca46942c119fd40eb999253e29 100644 (file)
@@ -29,7 +29,7 @@
  *
  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  */
-
+#include <MT_Scalar.h>
 #include <MT_Vector3.h>
 #include <MT_Quaternion.h>
  
@@ -56,15 +56,47 @@ void SM_MotionState::integrateForward(MT_Scalar timeStep, const SM_MotionState &
        m_orn.normalize();
 }
 
+/*
+// Newtonian lerp: interpolate based on Newtonian motion
+void SM_MotionState::nlerp(const SM_MotionState &prev, const SM_MotionState &next)
+{
+       MT_Scalar dt = next.getTime() - prev.getTime();
+       MT_Scalar t = getTime() - prev.getTime();
+       MT_Vector3 dx = next.getPosition() - prev.getPosition();
+       MT_Vector3 a = dx/(dt*dt) - prev.getLinearVelocity()/dt;
+       
+       m_pos = prev.getPosition() + prev.getLinearVelocity()*t + a*t*t;
+}
+*/
+
+void SM_MotionState::lerp(const SM_MotionState &prev, const SM_MotionState &next)
+{
+       MT_Scalar dt = next.getTime() - prev.getTime();
+       if (MT_fuzzyZero(dt))
+       {
+               *this = next;
+               return;
+       }
+       
+       MT_Scalar x = (getTime() - prev.getTime())/dt;
+       
+       m_pos = x*next.getPosition() + (1-x)*prev.getPosition();
+       
+       m_orn = prev.getOrientation().slerp(next.getOrientation(), 1-x);
+       
+       m_lin_vel = x*next.getLinearVelocity() + (1-x)*prev.getLinearVelocity();
+       m_ang_vel = x*next.getAngularVelocity() + (1-x)*prev.getAngularVelocity();
+}
+
 void SM_MotionState::lerp(MT_Scalar t, const SM_MotionState &other)
 {
        MT_Scalar x = (t - getTime())/(other.getTime() - getTime());
-       m_pos = x*m_pos + (1-x)*other.getPosition();
+       m_pos = (1-x)*m_pos + x*other.getPosition();
        
-       m_orn = m_orn.slerp(other.getOrientation(), x);
+       m_orn =  other.getOrientation().slerp(m_orn, x);
        
-       m_lin_vel = x*m_lin_vel + (1-x)*other.getLinearVelocity();
-       m_ang_vel = x*m_ang_vel + (1-x)*other.getAngularVelocity();
+       m_lin_vel = (1-x)*m_lin_vel + x*other.getLinearVelocity();
+       m_ang_vel = (1-x)*m_ang_vel + x*other.getAngularVelocity();
        
        m_time = t;
 }
index ed5171a87a27589b66e20eabbd8500b76801674a..cab191707bebaeae253a802c9f27b8f5ee82871a 100644 (file)
 
 #include "MT_MinMax.h"
 
-MT_Scalar SM_Object::ImpulseThreshold = -10.;
+MT_Scalar SM_Object::ImpulseThreshold = -0.01;
+
+struct Contact
+{
+       SM_Object *obj1;
+       SM_Object *obj2;
+       MT_Vector3 normal;
+       MT_Point3 pos;
+       
+       // Sort objects by height
+       bool operator()(const Contact *a, const Contact *b)
+       {
+               return a->pos[2] < b->pos[2];
+       }
+       
+       Contact(SM_Object *o1, SM_Object *o2, const MT_Vector3 nor, const MT_Point3 p)
+               : obj1(o1),
+                 obj2(o2),
+                 normal(nor),
+                 pos(p)
+       {
+       }
+       
+       Contact()
+       {
+       }
+       
+       void resolve()
+       {
+               if (obj1->m_static || obj2->m_static)
+               {
+                       if (obj1->isDynamic())
+                       {
+                               if (obj1->m_static && obj2->m_static)
+                               {
+                                       if (obj1->m_static < obj2->m_static)
+                                       {
+                                               obj2->m_error -= normal;
+                                               obj2->m_static = obj1->m_static + 1;
+                                       }
+                                       else
+                                       {
+                                               obj1->m_error += normal;
+                                               obj1->m_static = obj2->m_static + 1;
+                                       }
+                               }
+                               else
+                               {
+                                       if (obj1->m_static)
+                                       {
+                                               obj2->m_error -= normal;
+                                               obj2->m_static = obj1->m_static + 1;
+                                       }
+                                       else
+                                       {
+                                               obj1->m_error += normal;
+                                               obj1->m_static = obj2->m_static + 1;
+                                       }
+                               }
+                       }
+                       else
+                       {
+                               obj2->m_error -= normal;
+                               obj2->m_static = 1;
+                       }
+               }
+               else
+               {
+                       // This distinction between dynamic and non-dynamic objects should not be 
+                       // necessary. Non-dynamic objects are assumed to have infinite mass.
+                       if (obj1->isDynamic()) {
+                               MT_Vector3 error = normal * 0.5f;
+                               obj1->m_error += error;
+                               obj2->m_error -= error;
+                       }
+                       else {
+                               // Same again but now obj1 is non-dynamic
+                               obj2->m_error -= normal;
+                               obj2->m_static = obj1->m_static + 1;
+                       }
+               }
+       
+       }
+               
+       
+       typedef std::set<Contact*, Contact> Set;
+};
+
+static Contact::Set contacts;
 
 SM_Object::SM_Object(
        DT_ShapeHandle shape, 
@@ -69,9 +157,6 @@ SM_Object::SM_Object(
        m_scaling(1.0, 1.0, 1.0),
        m_reaction_impulse(0.0, 0.0, 0.0),
        m_reaction_force(0.0, 0.0, 0.0),
-       m_kinematic(false),
-       m_prev_kinematic(false),
-       m_is_rigid_body(false),
        m_lin_mom(0.0, 0.0, 0.0),
        m_ang_mom(0.0, 0.0, 0.0),
        m_force(0.0, 0.0, 0.0),
@@ -81,7 +166,11 @@ SM_Object::SM_Object(
        m_combined_ang_vel (0.0, 0.0, 0.0),
        m_fh_object(0),
        m_inv_mass(0.0),
-       m_inv_inertia(0., 0., 0.)
+       m_inv_inertia(0., 0., 0.),
+       m_kinematic(false),
+       m_prev_kinematic(false),
+       m_is_rigid_body(false),
+       m_static(0)
 {
        m_object = DT_CreateObject(this, shape);
        m_xform.setIdentity();
@@ -108,7 +197,7 @@ integrateForces(
        MT_Scalar timeStep
 ){
        if (!m_suspended) {
-               m_prev_state = *this;
+               m_prev_state = getNextFrame();
                m_prev_state.setLinearVelocity(actualLinVelocity());
                m_prev_state.setAngularVelocity(actualAngVelocity());
                if (isDynamic()) {
@@ -119,8 +208,8 @@ integrateForces(
                        m_lin_mom *= pow(m_shapeProps->m_lin_drag, timeStep);
                        m_ang_mom *= pow(m_shapeProps->m_ang_drag, timeStep);
                        // Set velocities according momentum
-                       m_lin_vel = m_lin_mom * m_inv_mass;
-                       m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
+                       getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass);
+                       getNextFrame().setAngularVelocity(m_inv_inertia_tensor * m_ang_mom);
                }
        }       
 
@@ -146,13 +235,13 @@ integrateMomentum(
 //#define BACKWARD
 #ifdef  MIDPOINT
 // Midpoint rule
-               integrateMidpoint(timeStep, m_prev_state, actualLinVelocity(), actualAngVelocity());
+               getNextFrame().integrateMidpoint(timeStep, m_prev_state, actualLinVelocity(), actualAngVelocity());
 #elif defined BACKWARD
 // Backward Euler
-               integrateBackward(timeStep, actualLinVelocity(), actualAngVelocity());
+               getNextFrame().integrateBackward(timeStep, actualLinVelocity(), actualAngVelocity());
 #else 
 // Forward Euler
-               integrateForward(timeStep, m_prev_state);
+               getNextFrame().integrateForward(timeStep, m_prev_state);
 #endif
 
                calcXform();
@@ -194,7 +283,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
                 * if rel_vel_normal < ImpulseThreshold, scale the restitution down.
                 * This should improve the simulation where the object is stacked.
                 */
-               restitution *= MT_min(MT_Scalar(1.0), rel_vel_normal/ImpulseThreshold);
+               restitution *= MT_min(MT_Scalar(1.0), m_shapeProps->m_mass*rel_vel_normal/ImpulseThreshold);
                                
                MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal;
                
@@ -207,7 +296,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
                         * Apply impulse at the collision point.
                         * Take rotational inertia into account.
                         */
-                       applyImpulse(local2 + getPosition(), impulse * normal);
+                       applyImpulse(local2 + getNextFrame().getPosition(), impulse * normal);
                } else {
                        /**
                         * Apply impulse through object centre. (no rotation.)
@@ -244,7 +333,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
                         * local coordinates.
                         */
 
-                       MT_Matrix3x3 lcs(m_orn);
+                       MT_Matrix3x3 lcs(getNextFrame().getOrientation());
                        
                        /**
                         * We cannot use m_xform.getBasis() for the matrix, since 
@@ -318,7 +407,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
                                        (invMass + lateral.dot(temp.cross(local2)));
 
                                MT_Scalar friction = MT_min(impulse_lateral, max_friction);
-                               applyImpulse(local2 + getPosition(), -lateral * friction);
+                               applyImpulse(local2 + getNextFrame().getPosition(), -lateral * friction);
                        }
                        else {
                                MT_Scalar impulse_lateral = rel_vel_lateral / invMass;
@@ -399,8 +488,8 @@ DT_Bool SM_Object::boing(
            (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()))
                scene->notifyCollision(obj1, obj2);
        
-       local1 -= obj1->getPosition();
-       local2 -= obj2->getPosition();
+       local1 -= obj1->getNextFrame().getPosition();
+       local2 -= obj2->getNextFrame().getPosition();
        
        // Calculate collision parameters
        MT_Vector3 rel_vel        = obj1->getVelocity(local1) - obj2->getVelocity(local2);
@@ -422,7 +511,11 @@ DT_Bool SM_Object::boing(
                obj1->dynamicCollision(local1, normal, dist, rel_vel, restitution, friction_factor, invMass);
                
        if (obj2->isDynamic())
+       {
                obj2->dynamicCollision(local2, -normal, dist, -rel_vel, restitution, friction_factor, invMass);
+               if (!obj1->isDynamic() || obj1->m_static)
+                       obj2->m_static = obj1->m_static + 1;
+       }
        
        return DT_CONTINUE;
 }
@@ -466,31 +559,38 @@ DT_Bool SM_Object::fix(
        if (dist < MT_EPSILON || dist > obj2->m_shapeProps->m_radius*obj2->m_shapeProps->m_radius)
                return DT_CONTINUE;
                
-       // This distinction between dynamic and non-dynamic objects should not be 
-       // necessary. Non-dynamic objects are assumed to have infinite mass.
-       if (obj1->isDynamic()) {
-               MT_Vector3 error = normal * 0.5f;
-               obj1->m_error += error;
-               obj2->m_error -= error;
-       }
-       else {
-               // Same again but now obj1 is non-dynamic
-               obj2->m_error -= normal;
+               
+       if ((obj1->m_static || !obj1->isDynamic()) && obj1->m_static < obj2->m_static)
+       {
+               obj2->m_static = obj1->m_static + 1;
+       } else if (obj2->m_static && obj2->m_static < obj1->m_static)
+       {
+               obj1->m_static = obj2->m_static + 1;
        }
        
+       contacts.insert(new Contact(obj1, obj2, normal, MT_Point3(local1 + 0.5*(local2 - local1))));
+       
+       
        return DT_CONTINUE;
 }
 
 void SM_Object::relax(void)
-{ 
+{
+       for (Contact::Set::iterator csit = contacts.begin() ; csit != contacts.end(); ++csit)
+       {
+               (*csit)->resolve();
+               delete (*csit);
+       }
+               
+       contacts.clear();
        if (m_error.fuzzyZero())
                return;
        //std::cout << "SM_Object::relax: { " << m_error << " }" << std::endl;
        
-       setPosition(getPosition() + m_error); 
+       getNextFrame().setPosition(getNextFrame().getPosition() + m_error); 
        m_error.setValue(0., 0., 0.); 
-       calcXform();
-       notifyClient();
+       //calcXform();
+       //notifyClient();
 }
        
 SM_Object::SM_Object() :
@@ -636,15 +736,15 @@ calcXform() {
        printf("                 m_scaling = { %-0.5f, %-0.5f, %-0.5f }\n",
                m_scaling[0], m_scaling[1], m_scaling[2]);
 #endif
-       m_xform.setOrigin(getPosition());
-       m_xform.setBasis(MT_Matrix3x3(getOrientation(), m_scaling));
+       m_xform.setOrigin(getNextFrame().getPosition());
+       m_xform.setBasis(MT_Matrix3x3(getNextFrame().getOrientation(), m_scaling));
        m_xform.getValue(m_ogl_matrix);
        
        /* Blender has been known to crash here.
           This usually means SM_Object *this has been deleted more than once. */
        DT_SetMatrixd(m_object, m_ogl_matrix);
        if (m_fh_object) {
-               m_fh_object->setPosition(getPosition());
+               m_fh_object->setPosition(getNextFrame().getPosition());
                m_fh_object->calcXform();
        }
        updateInvInertiaTensor();
@@ -760,7 +860,8 @@ setPosition(
        const MT_Point3& pos
 ){
        m_kinematic = true;
-       SM_MotionState::setPosition(pos);
+       getNextFrame().setPosition(pos);
+       endFrame();
 }
        
        void 
@@ -770,7 +871,8 @@ setOrientation(
 ){
        assert(!orn.fuzzyZero());
        m_kinematic = true;
-       SM_MotionState::setOrientation(orn);
+       getNextFrame().setOrientation(orn);
+       endFrame();
 }
 
        void 
@@ -807,10 +909,7 @@ SM_Object::
 addLinearVelocity(
        const MT_Vector3& lin_vel
 ){
-       m_lin_vel += lin_vel;
-       if (m_shapeProps) {
-               m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
-       }
+       setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
 }
 
        void 
@@ -818,9 +917,9 @@ SM_Object::
 setLinearVelocity(
        const MT_Vector3& lin_vel
 ){
-       m_lin_vel = lin_vel;
+       getNextFrame().setLinearVelocity(lin_vel);
        if (m_shapeProps) {
-               m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
+               m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
        }
 }
 
@@ -849,9 +948,9 @@ SM_Object::
 setAngularVelocity(
        const MT_Vector3& ang_vel
 ) {
-       m_ang_vel = ang_vel;
+       getNextFrame().setAngularVelocity(ang_vel);
        if (m_shapeProps) {
-               m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
+               m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
        }
 }
 
@@ -860,10 +959,7 @@ SM_Object::
 addAngularVelocity(
        const MT_Vector3& ang_vel
 ) {
-       m_ang_vel += ang_vel;
-       if (m_shapeProps) {
-               m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
-       }
+       setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
 }
 
 
@@ -896,18 +992,18 @@ resolveCombinedVelocities(
        if (isDynamic()) {              
 
 #if 1
-               m_lin_vel += lin_vel;
-               m_ang_vel += ang_vel;
+               getNextFrame().setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
+               getNextFrame().setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
 #else
 
                //compute the component of the physics velocity in the 
                // direction of the set velocity and set it to zero.
                MT_Vector3 lin_vel_norm = lin_vel.normalized();
 
-               m_lin_vel -= (m_lin_vel.dot(lin_vel_norm) * lin_vel_norm);
+               setLinearVelocity(getNextFrame().getLinearVelocity() - (getNextFrame().getLinearVelocity().dot(lin_vel_norm) * lin_vel_norm));
 #endif
-               m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
-               m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
+               m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
+               m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
                clearCombinedVelocities();
 
        }
@@ -970,7 +1066,7 @@ applyImpulse(
        const MT_Point3& attach, const MT_Vector3& impulse
 ) {
        applyCenterImpulse(impulse);                          // Change in linear momentum
-       applyAngularImpulse((attach - m_pos).cross(impulse)); // Change in angular momentump
+       applyAngularImpulse((attach - getNextFrame().getPosition()).cross(impulse)); // Change in angular momentump
 }
 
        void 
@@ -981,7 +1077,7 @@ applyCenterImpulse(
        if (m_shapeProps) {
                m_lin_mom          += impulse;
                m_reaction_impulse += impulse;
-               m_lin_vel           = m_lin_mom * m_inv_mass;
+               getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass);
 
                // The linear velocity is immedialtely updated since otherwise
                // simultaneous collisions will get a double impulse. 
@@ -995,7 +1091,7 @@ applyAngularImpulse(
 ) {
        if (m_shapeProps) {
                m_ang_mom += impulse;
-               m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
+               getNextFrame().setAngularVelocity( m_inv_inertia_tensor * m_ang_mom);
        }
 }
 
@@ -1099,7 +1195,7 @@ const
 SM_Object::
 actualLinVelocity(
 ) const {
-       return m_combined_lin_vel + m_lin_vel;
+       return m_combined_lin_vel + getNextFrame().getLinearVelocity();
 };
 
 const 
@@ -1107,10 +1203,98 @@ const
 SM_Object::
 actualAngVelocity(
 ) const {
-       return m_combined_ang_vel + m_ang_vel;
-};
+       return m_combined_ang_vel + getNextFrame().getAngularVelocity();
+}
+
+
+SM_MotionState&
+SM_Object::
+getCurrentFrame()
+{
+       return m_frames[1];
+}
+
+SM_MotionState&
+SM_Object::
+getPreviousFrame()
+{
+       return m_frames[0];
+}
+
+SM_MotionState &
+SM_Object::
+getNextFrame()
+{
+       return m_frames[2];
+}
+
+const SM_MotionState &
+SM_Object::
+getCurrentFrame() const
+{
+       return m_frames[1];
+}
+
+const SM_MotionState &
+SM_Object::
+getPreviousFrame() const
+{
+       return m_frames[0];
+}
+
+const SM_MotionState &
+SM_Object::
+getNextFrame() const
+{
+       return m_frames[2];
+}
 
 
+const MT_Point3&     
+SM_Object::
+getPosition()        const
+{
+       return m_frames[1].getPosition();
+}
 
+const MT_Quaternion& 
+SM_Object::
+getOrientation()     const
+{
+       return m_frames[1].getOrientation();
+}
 
+const MT_Vector3&    
+SM_Object::
+getLinearVelocity()  const
+{
+       return m_frames[1].getLinearVelocity();
+}
 
+const MT_Vector3&    
+SM_Object::
+getAngularVelocity() const
+{
+       return m_frames[1].getAngularVelocity();
+}
+
+void
+SM_Object::
+interpolate(MT_Scalar timeStep)
+{
+       if (!actualLinVelocity().fuzzyZero() || !actualAngVelocity().fuzzyZero()) 
+       {
+               getCurrentFrame().setTime(timeStep);
+               getCurrentFrame().lerp(getPreviousFrame(), getNextFrame());
+               notifyClient();
+       }
+}
+
+void
+SM_Object::
+endFrame()
+{
+       getPreviousFrame() = getNextFrame();
+       getCurrentFrame() = getNextFrame();
+       m_static = 0;
+}
index c29be228d64bb19521090b1d4090e0af4d0fe003..e9f25e25e6c84d3102f73c184a82a1adfbbd871e 100644 (file)
@@ -54,7 +54,7 @@ SM_Scene::SM_Scene() :
        m_secondaryRespTable(DT_CreateRespTable()),
        m_fixRespTable(DT_CreateRespTable()),
        m_forceField(0.0, 0.0, 0.0),
-       m_lastTime(-1.0)
+       m_frames(0)
 {
        for (int i = 0 ; i < NUM_RESPONSE; i++)
        {
@@ -83,8 +83,8 @@ SM_Scene::SM_Scene() :
        
        /* Fh Object */
        DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this);
-       DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
-       DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_FhObject::ray_hit, DT_BROAD_RESPONSE, this);
+       DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
+       DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this);
        DT_AddPairResponse(m_respTable, m_ResponseClass[FH_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this);
        
        /* Object (Fix Pass) */
@@ -177,27 +177,31 @@ void SM_Scene::endFrame()
                (*i)->clearForce();
 }
 
-bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
-       if (m_lastTime < 0.0)
+bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) 
+{
+       if (!m_frames)
        {
-               m_lastTime = curtime;
-               return false;
+               if (ticrate > 0.)
+                       m_frames = (unsigned int)(curtime*ticrate + 1.0);
+               else
+                       m_frames = (unsigned int)(curtime*65536.0);
        }
-               
+       
        // Divide the timeStep into a number of subsamples of size roughly 
        // equal to subS (might be a little smaller).
-       MT_Scalar timeStep = curtime - m_lastTime;
        MT_Scalar subStep;
        int num_samples;
        
+       // Compute the number of steps to do this update.
        if (ticrate > 0.0)
        {
+               // Fixed time step
                subStep = 1.0/ticrate;
-               num_samples = int(timeStep * ticrate);
+               num_samples = (unsigned int)(curtime*ticrate + 1.0) - m_frames;
        
                if (num_samples > 4)
                {
-                       std::cout << "Dropping physics frames! step: " << timeStep << " frames:" << num_samples << std::endl;
+                       std::cout << "Dropping physics frames! frames:" << num_samples << std::endl;
                        num_samples /= 4;
                        subStep *= 4.0;
                }
@@ -206,76 +210,70 @@ bool SM_Scene::proceed(MT_Scalar curtime, MT_Scalar ticrate) {
        {
                // Variable time step. (old update)
                // Integrate at least 100 Hz
+               MT_Scalar timeStep = curtime - m_frames/65536.0;
                subStep = timeStep > 0.01 ? 0.01 : timeStep;
                num_samples = int(timeStep * 0.01);
                if (num_samples < 1)
                        num_samples = 1;
        }
        
+       // Do a physics timestep.
        T_ObjectList::iterator i;
-       
-       // No timestep! (should do a mini update)
-       if (num_samples <= 0)
+       if (num_samples > 0)
        {
-               // Apply a forcefield (such as gravity)
-#if 0
-               for (i = m_objectList.begin(); i != m_objectList.end(); ++i) 
+               // Do the integration steps per object.
+               for (int step = 0; step != num_samples; ++step) 
                {
-                       //(*i)->applyForceField(m_forceField);
-                       //(*i)->integrateForces(timeStep);
-                       // And second we update the object positions by performing
-                       // an integration step for each object
-                       (*i)->integrateMomentum(timeStep);
-                       //(*i)->clearForce();
-               }
-#endif
-               return false;   
-       }
-       
-       m_lastTime += MT_Scalar(num_samples)*subStep;
+                       MT_Scalar time;
+                       if (ticrate > 0.)
+                               time = MT_Scalar(m_frames + step + 1) * subStep;
+                       else
+                               time = MT_Scalar(m_frames)/65536.0 + MT_Scalar(step + 1)*subStep;
+                       
+                       for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
+                               (*i)->endFrame();
+                               // Apply a forcefield (such as gravity)
+                               (*i)->integrateForces(subStep);
+                               // And second we update the object positions by performing
+                               // an integration step for each object
+                               (*i)->integrateMomentum(subStep);
+                       }
        
-       // Do the integration steps per object.
-       int step;
-       for (step = 0; step != num_samples; ++step) {
-
-               for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
-                       // Apply a forcefield (such as gravity)
-                       //(*i)->applyForceField(m_forceField);
-                       //(*i)->setTimeStep(timeStep);
-                       (*i)->integrateForces(subStep);
-                       // And second we update the object positions by performing
-                       // an integration step for each object
-                       (*i)->integrateMomentum(subStep);
-               }
+                       // So now first we let the physics scene respond to 
+                       // new forces, velocities set externally. 
+                       // The collsion and friction impulses are computed here. 
+                       // Collision phase
+                       DT_Test(m_scene, m_respTable);
                
-               // I changed the order of the next 2 statements.
-               // Originally objects were first integrated with a call
-               // to proceed(). However if external objects were 
-               // directly manipulating the velocities etc of physics 
-               // objects then the physics environment would not be able 
-               // to react before object positions were updated. --- Laurence.
-
-               // So now first we let the physics scene respond to 
-               // new forces, velocities set externally. 
-               // The collsion and friction impulses are computed here. 
-               DT_Test(m_scene, m_respTable);
-
-       // clear the user set velocities.
-#if 0
-       clearObjectCombinedVelocities();
-#endif
-               DT_Test(m_scene, m_fixRespTable);
-               
-               // Finish this timestep by saving al state information for the next
-               // timestep and clearing the accumulated forces. 
-               for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
-                       (*i)->relax();
-                       (*i)->proceedKinematic(subStep);
-                       (*i)->saveReactionForce(subStep);
-                       //(*i)->clearForce();
+                       // Contact phase
+                       DT_Test(m_scene, m_fixRespTable);
+                       
+                       // Finish this timestep by saving al state information for the next
+                       // timestep and clearing the accumulated forces. 
+                       for (i = m_objectList.begin(); i != m_objectList.end(); ++i) {
+                               (*i)->relax();
+                               (*i)->proceedKinematic(subStep);
+                               (*i)->saveReactionForce(subStep);
+                               (*i)->getNextFrame().setTime(time);
+                               //(*i)->clearForce();
+                       }
                }
        }
-       return true;
+
+       if (ticrate > 0)
+       {
+               // Interpolate between time steps.
+               for (i = m_objectList.begin(); i != m_objectList.end(); ++i) 
+                       (*i)->interpolate(curtime);
+       
+               m_frames = (unsigned int)(curtime*ticrate + 1.0);
+       }
+       else
+       {
+               m_frames = (unsigned int)(curtime*65536.0);
+       }
+               
+       return num_samples != 0;
 }
 
 void SM_Scene::notifyCollision(SM_Object *obj1, SM_Object *obj2)