diff options
Diffstat (limited to 'source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp')
| -rw-r--r-- | source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp | 312 |
1 files changed, 64 insertions, 248 deletions
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp index 26687663006..ed5171a87a2 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp @@ -49,95 +49,7 @@ #include "MT_MinMax.h" -MT_Scalar SM_Object::ImpulseThreshold = -1.0; - -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; +MT_Scalar SM_Object::ImpulseThreshold = -10.; SM_Object::SM_Object( DT_ShapeHandle shape, @@ -157,6 +69,9 @@ 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), @@ -166,11 +81,7 @@ 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_kinematic(false), - m_prev_kinematic(false), - m_is_rigid_body(false), - m_static(0) + m_inv_inertia(0., 0., 0.) { m_object = DT_CreateObject(this, shape); m_xform.setIdentity(); @@ -197,7 +108,7 @@ integrateForces( MT_Scalar timeStep ){ if (!m_suspended) { - m_prev_state = getNextFrame(); + m_prev_state = *this; m_prev_state.setLinearVelocity(actualLinVelocity()); m_prev_state.setAngularVelocity(actualAngVelocity()); if (isDynamic()) { @@ -208,8 +119,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 - getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass); - getNextFrame().setAngularVelocity(m_inv_inertia_tensor * m_ang_mom); + m_lin_vel = m_lin_mom * m_inv_mass; + m_ang_vel = m_inv_inertia_tensor * m_ang_mom; } } @@ -235,13 +146,13 @@ integrateMomentum( //#define BACKWARD #ifdef MIDPOINT // Midpoint rule - getNextFrame().integrateMidpoint(timeStep, m_prev_state, actualLinVelocity(), actualAngVelocity()); + integrateMidpoint(timeStep, m_prev_state, actualLinVelocity(), actualAngVelocity()); #elif defined BACKWARD // Backward Euler - getNextFrame().integrateBackward(timeStep, actualLinVelocity(), actualAngVelocity()); + integrateBackward(timeStep, actualLinVelocity(), actualAngVelocity()); #else // Forward Euler - getNextFrame().integrateForward(timeStep, m_prev_state); + integrateForward(timeStep, m_prev_state); #endif calcXform(); @@ -278,11 +189,11 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2, /** * if rel_vel_normal > 0, the objects are moving apart! */ - if (rel_vel_normal < -MT_EPSILON) { + if (rel_vel_normal < 0.) { /** * 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); MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal; @@ -296,7 +207,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2, * Apply impulse at the collision point. * Take rotational inertia into account. */ - applyImpulse(local2 + getNextFrame().getPosition(), impulse * normal); + applyImpulse(local2 + getPosition(), impulse * normal); } else { /** * Apply impulse through object centre. (no rotation.) @@ -333,7 +244,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2, * local coordinates. */ - MT_Matrix3x3 lcs(getNextFrame().getOrientation()); + MT_Matrix3x3 lcs(m_orn); /** * We cannot use m_xform.getBasis() for the matrix, since @@ -407,7 +318,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 + getNextFrame().getPosition(), -lateral * friction); + applyImpulse(local2 + getPosition(), -lateral * friction); } else { MT_Scalar impulse_lateral = rel_vel_lateral / invMass; @@ -488,8 +399,8 @@ DT_Bool SM_Object::boing( (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback())) scene->notifyCollision(obj1, obj2); - local1 -= obj1->getNextFrame().getPosition(); - local2 -= obj2->getNextFrame().getPosition(); + local1 -= obj1->getPosition(); + local2 -= obj2->getPosition(); // Calculate collision parameters MT_Vector3 rel_vel = obj1->getVelocity(local1) - obj2->getVelocity(local2); @@ -511,11 +422,7 @@ 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; } @@ -559,38 +466,31 @@ DT_Bool SM_Object::fix( if (dist < MT_EPSILON || dist > obj2->m_shapeProps->m_radius*obj2->m_shapeProps->m_radius) return DT_CONTINUE; - - 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; + // 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; } - - 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; - getNextFrame().setPosition(getNextFrame().getPosition() + m_error); + setPosition(getPosition() + m_error); m_error.setValue(0., 0., 0.); - //calcXform(); - //notifyClient(); + calcXform(); + notifyClient(); } SM_Object::SM_Object() : @@ -607,6 +507,9 @@ 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), @@ -614,10 +517,7 @@ SM_Object::SM_Object() : m_error(0.0, 0.0, 0.0), m_combined_lin_vel (0.0, 0.0, 0.0), m_combined_ang_vel (0.0, 0.0, 0.0), - m_fh_object(0), - m_kinematic(false), - m_prev_kinematic(false), - m_is_rigid_body(false) + m_fh_object(0) { // warning no initialization of variables done by moto. } @@ -736,15 +636,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(getNextFrame().getPosition()); - m_xform.setBasis(MT_Matrix3x3(getNextFrame().getOrientation(), m_scaling)); + m_xform.setOrigin(getPosition()); + m_xform.setBasis(MT_Matrix3x3(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(getNextFrame().getPosition()); + m_fh_object->setPosition(getPosition()); m_fh_object->calcXform(); } updateInvInertiaTensor(); @@ -860,8 +760,7 @@ setPosition( const MT_Point3& pos ){ m_kinematic = true; - getNextFrame().setPosition(pos); - endFrame(); + SM_MotionState::setPosition(pos); } void @@ -871,8 +770,7 @@ setOrientation( ){ assert(!orn.fuzzyZero()); m_kinematic = true; - getNextFrame().setOrientation(orn); - endFrame(); + SM_MotionState::setOrientation(orn); } void @@ -909,7 +807,10 @@ SM_Object:: addLinearVelocity( const MT_Vector3& lin_vel ){ - setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel); + m_lin_vel += lin_vel; + if (m_shapeProps) { + m_lin_mom = m_lin_vel * m_shapeProps->m_mass; + } } void @@ -917,9 +818,9 @@ SM_Object:: setLinearVelocity( const MT_Vector3& lin_vel ){ - getNextFrame().setLinearVelocity(lin_vel); + m_lin_vel = lin_vel; if (m_shapeProps) { - m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass; + m_lin_mom = m_lin_vel * m_shapeProps->m_mass; } } @@ -948,9 +849,9 @@ SM_Object:: setAngularVelocity( const MT_Vector3& ang_vel ) { - getNextFrame().setAngularVelocity(ang_vel); + m_ang_vel = ang_vel; if (m_shapeProps) { - m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia; + m_ang_mom = m_ang_vel * m_shapeProps->m_inertia; } } @@ -959,7 +860,10 @@ SM_Object:: addAngularVelocity( const MT_Vector3& ang_vel ) { - setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel); + m_ang_vel += ang_vel; + if (m_shapeProps) { + m_ang_mom = m_ang_vel * m_shapeProps->m_inertia; + } } @@ -992,18 +896,18 @@ resolveCombinedVelocities( if (isDynamic()) { #if 1 - getNextFrame().setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel); - getNextFrame().setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel); + m_lin_vel += lin_vel; + m_ang_vel += 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(); - setLinearVelocity(getNextFrame().getLinearVelocity() - (getNextFrame().getLinearVelocity().dot(lin_vel_norm) * lin_vel_norm)); + m_lin_vel -= (m_lin_vel.dot(lin_vel_norm) * lin_vel_norm); #endif - m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass; - m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia; + m_lin_mom = m_lin_vel * m_shapeProps->m_mass; + m_ang_mom = m_ang_vel * m_shapeProps->m_inertia; clearCombinedVelocities(); } @@ -1066,7 +970,7 @@ applyImpulse( const MT_Point3& attach, const MT_Vector3& impulse ) { applyCenterImpulse(impulse); // Change in linear momentum - applyAngularImpulse((attach - getNextFrame().getPosition()).cross(impulse)); // Change in angular momentump + applyAngularImpulse((attach - m_pos).cross(impulse)); // Change in angular momentump } void @@ -1077,7 +981,7 @@ applyCenterImpulse( if (m_shapeProps) { m_lin_mom += impulse; m_reaction_impulse += impulse; - getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass); + m_lin_vel = m_lin_mom * m_inv_mass; // The linear velocity is immedialtely updated since otherwise // simultaneous collisions will get a double impulse. @@ -1091,7 +995,7 @@ applyAngularImpulse( ) { if (m_shapeProps) { m_ang_mom += impulse; - getNextFrame().setAngularVelocity( m_inv_inertia_tensor * m_ang_mom); + m_ang_vel = m_inv_inertia_tensor * m_ang_mom; } } @@ -1195,7 +1099,7 @@ const SM_Object:: actualLinVelocity( ) const { - return m_combined_lin_vel + getNextFrame().getLinearVelocity(); + return m_combined_lin_vel + m_lin_vel; }; const @@ -1203,98 +1107,10 @@ const SM_Object:: actualAngVelocity( ) const { - 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(); -} + return m_combined_ang_vel + m_ang_vel; +}; -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; -} |