diff options
author | Kester Maddock <Christopher.Maddock.1@uni.massey.ac.nz> | 2004-12-04 12:51:04 +0300 |
---|---|---|
committer | Kester Maddock <Christopher.Maddock.1@uni.massey.ac.nz> | 2004-12-04 12:51:04 +0300 |
commit | ae8102c8d657096302e1b29d7e1ef55153b96da5 (patch) | |
tree | 3b7515bc83b346020317871103fd902f27a746ab /source/gameengine | |
parent | ab7134839af06d74cb7cea78b45b986fcb10045e (diff) |
Update the physics engine: interpolate between frames so track to/camera actuators work properly. (bug #1816)
Diffstat (limited to 'source/gameengine')
6 files changed, 382 insertions, 139 deletions
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h index b0e0717cf8c..08e29265d0c 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h @@ -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 { diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h index b37e1a5466b..07eb5a7637f 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h @@ -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 diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h index 80e8c635cbb..a0d5c983031 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h @@ -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 diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp index a3285b19116..1fadbdadfa0 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_MotionState.cpp @@ -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; } diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp index ed5171a87a2..cab191707be 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp @@ -49,7 +49,95 @@ #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; +} diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp index c29be228d64..e9f25e25e6c 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp @@ -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) |