diff options
Diffstat (limited to 'source/gameengine/Physics/Sumo/Fuzzics')
9 files changed, 796 insertions, 339 deletions
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Debug.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Debug.h new file mode 100644 index 00000000000..48d5906e53d --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Debug.h @@ -0,0 +1,26 @@ + + +#ifndef __SM_DEBUG_H__ +#define __SM_DEBUG_H__ + +/* Comment this to disable all SUMO debugging printfs */ + +#define SM_DEBUG + +#ifdef SM_DEBUG + +#include <stdio.h> + +/* Uncomment this to printf all ray casts */ +//#define SM_DEBUG_RAYCAST + +/* Uncomment this to printf collision callbacks */ +//#define SM_DEBUG_BOING + +/* Uncomment this to printf Xform matrix calculations */ +//#define SM_DEBUG_XFORM + +#endif /* SM_DEBUG */ + +#endif /* __SM_DEBUG_H__ */ + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_FhObject.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_FhObject.h index 36ba075eeff..116eed12cea 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_FhObject.h +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_FhObject.h @@ -1,3 +1,34 @@ +/** + * $Id$ + * + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ #ifndef SM_FHOBJECT_H #define SM_FHOBJECT_H @@ -5,19 +36,15 @@ class SM_FhObject : public SM_Object { public: - SM_FhObject() {} - SM_FhObject(const MT_Vector3& ray, SM_Object *client_object) : - SM_Object(DT_Ray(ray[0], ray[1], ray[2]), 0, 0, 0), - m_ray(ray), - m_ray_direction(ray.normalized()), - m_client_object(client_object) {} + virtual ~SM_FhObject(); + SM_FhObject(DT_ShapeHandle rayshape, MT_Vector3 ray, SM_Object *parent_object); const MT_Vector3& getRay() const { return m_ray; } MT_Point3 getSpot() const { return m_pos + m_ray; } const MT_Vector3& getRayDirection() const { return m_ray_direction; } - SM_Object *getClientObject() const { return m_client_object; } + SM_Object *getParentObject() const { return m_parent_object; } - static void ray_hit(void *client_data, + static DT_Bool ray_hit(void *client_data, void *object1, void *object2, const DT_CollData *coll_data); @@ -25,7 +52,7 @@ public: private: MT_Vector3 m_ray; MT_Vector3 m_ray_direction; - SM_Object *m_client_object; + SM_Object *m_parent_object; }; #endif diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h index 5342ba5d3a5..c84a3c2ff56 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h @@ -1,9 +1,40 @@ +/** + * $Id$ + * + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ #ifndef SM_OBJECT_H #define SM_OBJECT_H #include <vector> -#include "solid.h" +#include <SOLID/SOLID.h> #include "SM_Callback.h" #include "SM_MotionState.h" @@ -15,6 +46,7 @@ class SM_FhObject; // Properties of dynamic objects struct SM_ShapeProps { MT_Scalar m_mass; // Total mass + MT_Scalar m_radius; // Bound sphere size MT_Scalar m_inertia; // Inertia, should be a tensor some time MT_Scalar m_lin_drag; // Linear drag (air, water) 0 = concrete, 1 = vacuum MT_Scalar m_ang_drag; // Angular drag @@ -36,6 +68,7 @@ struct SM_MaterialProps { }; + class SM_Object : public SM_MotionState { public: SM_Object() ; @@ -45,7 +78,6 @@ public: const SM_ShapeProps *shapeProps, SM_Object *dynamicParent ); - virtual ~SM_Object(); bool isDynamic() const; @@ -89,7 +121,7 @@ public: void setMargin(MT_Scalar margin) ; MT_Scalar getMargin() const ; - + const SM_MaterialProps *getMaterialProps() const ; const SM_ShapeProps *getShapeProps() const ; @@ -164,7 +196,8 @@ public: void applyAngularImpulse(const MT_Vector3& impulse); MT_Point3 getWorldCoord(const MT_Point3& local) const; - + MT_Point3 getLocalCoord(const MT_Point3& world) const; + MT_Vector3 getVelocity(const MT_Point3& local) const; @@ -180,9 +213,6 @@ public: DT_ObjectHandle getObjectHandle() const ; DT_ShapeHandle getShapeHandle() const ; - void setClientObject(void *clientobj) ; - void *getClientObject() ; - SM_Object *getDynamicParent() ; void integrateForces(MT_Scalar timeStep); @@ -192,10 +222,9 @@ public: bool isRigidBody() const ; - // This is the callback for handling collisions of dynamic objects static - void + DT_Bool boing( void *client_data, void *object1, @@ -203,6 +232,26 @@ public: const DT_CollData *coll_data ); + static + DT_Bool + fix( + void *client_data, + void *object1, + void *object2, + const DT_CollData *coll_data + ); + + + void *getClientObject() { return m_client_object; } + void setClientObject(void *client_object) { m_client_object = client_object; } + + void relax(); + + void backup() { + m_pos = m_prev_state.getPosition(); + m_orn = m_prev_state.getOrientation(); + m_xform = m_prev_xform; + } private: // return the actual linear_velocity of this object this @@ -217,11 +266,20 @@ private: MT_Vector3 actualAngVelocity( ) const ; + + void dynamicCollision(MT_Point3 local2, + MT_Vector3 normal, + MT_Scalar dist, + MT_Vector3 rel_vel, + MT_Scalar restitution, + MT_Scalar friction_factor, + MT_Scalar invMass + ); typedef std::vector<SM_Callback *> T_CallbackList; - T_CallbackList m_callbackList; // Each object can have multiple callbacks from the client (=game engine) + T_CallbackList m_callbackList; // Each object can have multiple callbacks from the client (=game engine) SM_Object *m_dynamicParent; // Collisions between parent and children are ignored // as the collision callback now has only information @@ -229,7 +287,7 @@ private: // can identify it's clientdata after a collision void *m_client_object; - DT_ShapeHandle m_shape; // Shape for collision detection + DT_ShapeHandle m_shape; // Shape for collision detection // Material and shape properties are not owned by this class. @@ -237,7 +295,7 @@ private: const SM_MaterialProps *m_materialPropsBackup; // Backup in case the object temporarily becomes a ghost. const SM_ShapeProps *m_shapeProps; const SM_ShapeProps *m_shapePropsBackup; // Backup in case the object's dynamics is temporarily suspended - DT_ObjectHandle m_object; // A handle to the corresponding object in SOLID. + DT_ObjectHandle m_object; // A handle to the corresponding object in SOLID. MT_Scalar m_margin; // Offset for the object's shape (also for collision detection) MT_Vector3 m_scaling; // Non-uniform scaling of the object's shape @@ -258,6 +316,8 @@ private: 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_error; // Error in position:- amount object must be moved to prevent intersection with scene // Here are the values of externally set linear and angular // velocity. These are updated from the outside diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h index 1d4a4afc721..7afa2a59084 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h @@ -2,38 +2,70 @@ * $Id$ * Copyright (C) 2001 NaN Technologies B.V. * The physics scene. + * + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** */ - #ifndef SM_SCENE_H #define SM_SCENE_H +#ifdef WIN32 #pragma warning (disable : 4786) +#endif #include <vector> #include <set> #include <utility> //needed for pair -#include "solid.h" +#include <SOLID/SOLID.h> #include "MT_Vector3.h" #include "MT_Point3.h" -class SM_Object; +#include "SM_Object.h" + +typedef enum +{ + FH_RESPONSE, + SENSOR_RESPONSE, /* Touch Sensors */ + CAMERA_RESPONSE, /* Visibility Culling */ + OBJECT_RESPONSE, /* Object Dynamic Geometry Response */ + STATIC_RESPONSE, /* Static Geometry Response */ + + NUM_RESPONSE +}; class SM_Scene { public: - SM_Scene() : - m_scene(DT_CreateScene()), - m_respTable(DT_CreateRespTable()), - m_secondaryRespTable(0), - m_forceField(0.0, 0.0, 0.0) - {} + SM_Scene(); - ~SM_Scene() { - DT_DeleteRespTable(m_respTable); - DT_DeleteScene(m_scene); - } - + ~SM_Scene(); + DT_RespTableHandle getRespTableHandle() const { return m_respTable; } @@ -50,6 +82,9 @@ public: m_forceField = forceField; } + void addTouchCallback(int response_class, DT_ResponseCallback callback, void *user); + + void addSensor(SM_Object& object); void add(SM_Object& object); void remove(SM_Object& object); @@ -61,9 +96,10 @@ public: m_pairList.clear(); } - void setSecondaryRespTable(DT_RespTableHandle secondaryRespTable) { - m_secondaryRespTable = secondaryRespTable; - } + void setSecondaryRespTable(DT_RespTableHandle secondaryRespTable); + DT_RespTableHandle getSecondaryRespTable() { return m_secondaryRespTable; } + + void requestCollisionCallback(SM_Object &object); // Perform an integration step of duration 'timeStep'. @@ -96,7 +132,16 @@ private: // Clear the user set velocities. void clearObjectCombinedVelocities(); - + // This is the callback for handling collisions of dynamic objects + static + DT_Bool + boing( + void *client_data, + void *object1, + void *object2, + const DT_CollData *coll_data + ); + /** internal type */ typedef std::vector<SM_Object *> T_ObjectList; /** internal type */ @@ -106,10 +151,19 @@ private: DT_SceneHandle m_scene; /** Following response table contains the callbacks for the dynmics */ DT_RespTableHandle m_respTable; + DT_ResponseClass m_ResponseClass[NUM_RESPONSE]; /** * Following response table contains callbacks for the client (= * game engine) */ DT_RespTableHandle m_secondaryRespTable; // Handle + DT_ResponseClass m_secondaryResponseClass[NUM_RESPONSE]; + + /** + * Following resposne table contains callbacks for fixing the simulation + * ie making sure colliding objects do not intersect. + */ + DT_RespTableHandle m_fixRespTable; + DT_ResponseClass m_fixResponseClass[NUM_RESPONSE]; /** The acceleration from the force field */ MT_Vector3 m_forceField; diff --git a/source/gameengine/Physics/Sumo/Fuzzics/sample/particle.cpp b/source/gameengine/Physics/Sumo/Fuzzics/sample/particle.cpp index 53b9ae684f3..d7aca326b42 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/sample/particle.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/sample/particle.cpp @@ -17,7 +17,7 @@ #include "SM_FhObject.h" #include "SM_Scene.h" -#include "solid.h" +#include <SOLID/SOLID.h> #ifdef HAVE_CONFIG_H #include <config.h> diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/Makefile b/source/gameengine/Physics/Sumo/Fuzzics/src/Makefile index 5e3b35f6cf4..b2744c5496a 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/Makefile +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/Makefile @@ -3,11 +3,12 @@ # Copyright (C) 2001 NaN Technologies B.V. LIBNAME = fuzzics -DIR = $(OCGDIR)/sumo +DIR = $(OCGDIR)/gameengine/blphys/$(LIBNAME) include nan_compile.mk CCFLAGS += $(LEVEL_1_CPP_WARNINGS) CPPFLAGS += -I../include -I$(NAN_MOTO)/include -I../../include +CPPFLAGS += -I$(NAN_SOLID)/include diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp index 0ba5fd673ca..c6134bde8f8 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp @@ -1,3 +1,34 @@ +/** + * $Id$ + * + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ #include "SM_FhObject.h" #include "MT_MinMax.h" @@ -5,52 +36,82 @@ #include <config.h> #endif -void SM_FhObject::ray_hit(void *client_data, +SM_FhObject::SM_FhObject(DT_ShapeHandle rayshape, MT_Vector3 ray, SM_Object *parent_object) : + SM_Object(rayshape, NULL, NULL, NULL), + m_ray(ray), + m_ray_direction(ray.normalized()), + m_parent_object(parent_object) +{ +} + +SM_FhObject::~SM_FhObject() +{ +} + +DT_Bool SM_FhObject::ray_hit(void *client_data, void *client_object1, void *client_object2, - const DT_CollData *coll_data) { + const DT_CollData *coll_data) +{ + SM_FhObject *fh_object = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2); + if (!fh_object) + { + std::swap(client_object1, client_object2); + fh_object = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2); + } + SM_Object *hit_object = (SM_Object *)client_object1; const SM_MaterialProps *matProps = hit_object->getMaterialProps(); - + if ((matProps == 0) || (matProps->m_fh_distance < MT_EPSILON)) { - return; + return DT_CONTINUE; } - SM_FhObject *fh_object = (SM_FhObject *)client_object2; - SM_Object *cl_object = fh_object->getClientObject(); + SM_Object *cl_object = fh_object->getParentObject(); + + assert(fh_object); if (hit_object == cl_object) { // Shot myself in the foot... - return; + return DT_CONTINUE; } const SM_ShapeProps *shapeProps = cl_object->getShapeProps(); // Exit if the client object is not dynamic. if (shapeProps == 0) { - return; + return DT_CONTINUE; } MT_Point3 lspot; MT_Vector3 normal; - if (DT_ObjectRayTest(hit_object->getObjectHandle(), - fh_object->getPosition().getValue(), - fh_object->getSpot().getValue(), - lspot.getValue(), normal.getValue())) { + DT_Vector3 from, to, dnormal; + DT_Scalar dlspot; + fh_object->getPosition().getValue(from); + fh_object->getSpot().getValue(to); + + + if (DT_ObjectRayCast(hit_object->getObjectHandle(), + from, + to, + 1., + &dlspot, + dnormal)) { + lspot = fh_object->getPosition() + (fh_object->getSpot() - fh_object->getPosition()) * dlspot; const MT_Vector3& ray_dir = fh_object->getRayDirection(); - MT_Scalar dist = MT_distance(fh_object->getPosition(), - hit_object->getWorldCoord(lspot)) - - cl_object->getMargin(); + MT_Scalar dist = MT_distance(fh_object->getPosition(), lspot) - + cl_object->getMargin() - shapeProps->m_radius; - normal.normalize(); - + normal = MT_Vector3(dnormal).safe_normalized(); + if (dist < matProps->m_fh_distance) { if (shapeProps->m_do_fh) { - MT_Vector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocity(lspot); + lspot -= hit_object->getPosition(); + MT_Vector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocity(lspot); MT_Scalar rel_vel_ray = ray_dir.dot(rel_vel); MT_Scalar spring_extent = 1.0 - dist / matProps->m_fh_distance; @@ -84,7 +145,7 @@ void SM_FhObject::ray_hit(void *client_data, if (rel_vel_lateral > MT_EPSILON) { MT_Scalar friction_factor = matProps->m_friction; - MT_Scalar max_friction = friction_factor * MT_max(0.0, i_spring); + MT_Scalar max_friction = friction_factor * MT_max(MT_Scalar(0.0), i_spring); MT_Scalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass(); @@ -113,6 +174,8 @@ void SM_FhObject::ray_hit(void *client_data, } } } + + return DT_CONTINUE; } diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp index 51673b92ffb..84aef195a9e 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp @@ -2,6 +2,34 @@ * $Id$ * Copyright (C) 2001 NaN Technologies B.V. * The basic physics object. + * + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** */ #ifdef HAVE_CONFIG_H @@ -17,25 +45,29 @@ #include "SM_Object.h" #include "SM_Scene.h" #include "SM_FhObject.h" +#include "SM_Debug.h" #include "MT_MinMax.h" // Tweak parameters -static MT_Scalar ImpulseThreshold = 0.5; - +static const MT_Scalar ImpulseThreshold = 0.5; +static const MT_Scalar FixThreshold = 0.01; +static const MT_Scalar FixVelocity = 0.01; SM_Object::SM_Object( DT_ShapeHandle shape, const SM_MaterialProps *materialProps, const SM_ShapeProps *shapeProps, - SM_Object *dynamicParent -) : + SM_Object *dynamicParent) : + + m_dynamicParent(dynamicParent), + m_client_object(0), + m_shape(shape), m_materialProps(materialProps), m_materialPropsBackup(0), m_shapeProps(shapeProps), m_shapePropsBackup(0), - m_dynamicParent(dynamicParent), m_object(DT_CreateObject(this, shape)), m_margin(0.0), m_scaling(1.0, 1.0, 1.0), @@ -48,23 +80,22 @@ SM_Object::SM_Object( m_ang_mom(0.0, 0.0, 0.0), m_force(0.0, 0.0, 0.0), m_torque(0.0, 0.0, 0.0), - m_client_object(0), - m_fh_object(0), - + 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_combined_ang_vel (0.0, 0.0, 0.0), + m_fh_object(0) { m_xform.setIdentity(); m_xform.getValue(m_ogl_matrix); if (shapeProps && (shapeProps->m_do_fh || shapeProps->m_do_rot_fh)) { - MT_Vector3 ray(0.0, 0.0, -10.0); - m_fh_object = new SM_FhObject(ray, this); + DT_Vector3 zero = {0., 0., 0.}, ray = {0.0, 0.0, -10.0}; + m_fh_object = new SM_FhObject(DT_NewLineSegment(zero, ray), MT_Vector3(ray), this); + //printf("SM_Object:: WARNING! fh disabled.\n"); } m_suspended = false; } - void SM_Object:: integrateForces( @@ -129,258 +160,310 @@ integrateMomentum( } } +void SM_Object::dynamicCollision(MT_Point3 local2, + MT_Vector3 normal, + MT_Scalar dist, + MT_Vector3 rel_vel, + MT_Scalar restitution, + MT_Scalar friction_factor, + MT_Scalar invMass +) +{ + // Same again but now obj1 is non-dynamic + // Compute the point on obj1 closest to obj2 (= sphere with radius = 0) + // local1 is th point closest to obj2 + // local2 is the local origin of obj2 + if (MT_EPSILON < dist) { + //printf("SM_Object::Boing: local2 = { %0.5f, %0.5f, %0.5f } (%0.5f)\n", + // local2[0], local2[1], local2[2], local2.length()); + + // the normal to the contact plane + normal /= dist; + + // wr2 points from obj2's origin to the global contact point + // wr2 is only needed for rigid bodies (objects for which the + // friction can change the angular momentum). + // vel2 is adapted to denote the velocity of the contact point + // This should look familiar.... + MT_Scalar rel_vel_normal = normal.dot(rel_vel); + + //printf(" rel_vel = { %0.5f, %0.5f, %0.5f } (%0.5f)\n", + // rel_vel[0], rel_vel[1], rel_vel[2], rel_vel.length()); + + if (rel_vel_normal <= 0.0) { + if (-rel_vel_normal < ImpulseThreshold) { + restitution = 0.0; + } + + MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal / invMass; + applyCenterImpulse( impulse * normal); + +// The friction part starts here!!!!!!!! + + // Compute the lateral component of the relative velocity + // lateral actually points in the opposite direction, i.e., + // into the direction of the friction force. -void SM_Object::boing( +#if 0 + // test - only do friction on the physics part of the + // velocity. + vel1 -= obj1->m_combined_lin_vel; + vel2 -= obj2->m_combined_lin_vel; + + // This should look familiar.... + rel_vel = vel2 - vel1; + rel_vel_normal = normal.dot(rel_vel); +#endif + + MT_Vector3 lateral = rel_vel - normal * rel_vel_normal; + //printf(" lateral = { %0.5f, %0.5f, %0.5f } (%0.5f)\n", + // lateral[0], lateral[1], lateral[2], lateral.length()); + + //const SM_ShapeProps *shapeProps = obj2->getShapeProps(); + + if (m_shapeProps->m_do_anisotropic) { + + // For anisotropic friction we scale the lateral component, + // rather than compute a direction-dependent fricition + // factor. For this the lateral component is transformed to + // local coordinates. + + MT_Matrix3x3 lcs(m_orn); + // We cannot use m_xform.getBasis() for the matrix, since + // it might contain a non-uniform scaling. + // OPT: it's a bit daft to compute the matrix since the + // quaternion itself can be used to do the transformation. + + MT_Vector3 loc_lateral = lateral * lcs; + // lcs is orthogonal so lcs.inversed() == lcs.transposed(), + // and lcs.transposed() * lateral == lateral * lcs. + + const MT_Vector3& friction_scaling = + m_shapeProps->m_friction_scaling; + + // Scale the local lateral... + loc_lateral.scale(friction_scaling[0], + friction_scaling[1], + friction_scaling[2]); + // ... and transform it back to global coordinates + lateral = lcs * loc_lateral; + } + + // A tiny Coulomb friction primer: + // The Coulomb friction law states that the magnitude of the + // maximum possible friction force depends linearly on the + // magnitude of the normal force. + // + // F_max_friction = friction_factor * F_normal + // + // (NB: independent of the contact area!!) + // + // The friction factor depends on the material. + // We use impulses rather than forces but let us not be + // bothered by this. + + + MT_Scalar rel_vel_lateral = lateral.length(); + //printf("rel_vel = { %0.05f, %0.05f, %0.05f}\n", rel_vel[0], rel_vel[1], rel_vel[2]); + //printf("n.l = %0.15f\n", normal.dot(lateral)); /* Should be 0.0 */ + + if (rel_vel_lateral > MT_EPSILON) { + lateral /= rel_vel_lateral; + + // Compute the maximum friction impulse + MT_Scalar max_friction = + friction_factor * MT_max(MT_Scalar(0.0), impulse); + + // I guess the GEN_max is not necessary, so let's check it + + assert(impulse >= 0.0); + + // Here's the trick. We compute the impulse to make the + // lateral velocity zero. (Make the objects stick together + // at the contact point. If this impulse is larger than + // the maximum possible friction impulse, then shrink its + // magnitude to the maximum friction. + + if (isRigidBody()) { + + // For rigid bodies we take the inertia into account, + // since the friction impulse is going to change the + // angular momentum as well. + MT_Vector3 temp = getInvInertia() * local2.cross(lateral); + MT_Scalar impulse_lateral = rel_vel_lateral / + (invMass + lateral.dot(temp.cross(local2))); + + MT_Scalar friction = MT_min(impulse_lateral, max_friction); + applyImpulse(local2 + m_pos, -lateral * friction); + } + else { + MT_Scalar impulse_lateral = rel_vel_lateral / invMass; + + MT_Scalar friction = MT_min(impulse_lateral, max_friction); + applyCenterImpulse( -friction * lateral); + } + + + } + + calcXform(); + notifyClient(); + + } + } +} + +DT_Bool SM_Object::boing( void *client_data, void *object1, void *object2, const DT_CollData *coll_data ){ + //if (!coll_data) + // return DT_CONTINUE; + SM_Scene *scene = (SM_Scene *)client_data; SM_Object *obj1 = (SM_Object *)object1; SM_Object *obj2 = (SM_Object *)object2; scene->addPair(obj1, obj2); // Record this collision for client callbacks + // If one of the objects is a ghost then ignore it for the dynamics + if (obj1->isGhost() || obj2->isGhost()) { + return DT_CONTINUE; + } + + // Objects do not collide with parent objects + if (obj1->getDynamicParent() == obj2 || obj2->getDynamicParent() == obj1) { + return DT_CONTINUE; + } + + if (!obj2->isDynamic()) { + std::swap(obj1, obj2); + } + if (!obj2->isDynamic()) { + return DT_CONTINUE; + } + + // Get collision data from SOLID + DT_Vector3 p1, p2; + if (!DT_GetPenDepth(obj1->getObjectHandle(), obj2->getObjectHandle(), p1, p2)) + return DT_CONTINUE; + MT_Point3 local1(p1), local2(p2); + MT_Vector3 normal(local2 - local1); + MT_Scalar dist = normal.length(); + + local1 -= obj1->m_pos, local2 -= obj2->m_pos; + + // Calculate collision parameters + MT_Vector3 rel_vel = obj1->getVelocity(local1) + obj1->m_combined_lin_vel - + obj2->getVelocity(local2) - obj2->m_combined_lin_vel; + + MT_Scalar restitution = + MT_min(obj1->getMaterialProps()->m_restitution, + obj2->getMaterialProps()->m_restitution); + + MT_Scalar friction_factor = + MT_min(obj1->getMaterialProps()->m_friction, + obj2->getMaterialProps()->m_friction); + + MT_Scalar invMass = obj1->getInvMass() + obj2->getInvMass(); + + // Calculate reactions + if (obj1->isDynamic()) + 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); + + return DT_CONTINUE; +} + +DT_Bool SM_Object::fix( + void *client_data, + void *object1, + void *object2, + const DT_CollData *coll_data +){ + SM_Scene *scene = (SM_Scene *)client_data; + SM_Object *obj1 = (SM_Object *)object1; + SM_Object *obj2 = (SM_Object *)object2; + // If one of the objects is a ghost then ignore it for the dynamics if (obj1->isGhost() || obj2->isGhost()) { - return; + return DT_CONTINUE; } + if (obj1->getDynamicParent() == obj2 || obj2->getDynamicParent() == obj1) { + return DT_CONTINUE; + } if (!obj2->isDynamic()) { std::swap(obj1, obj2); } if (!obj2->isDynamic()) { - return; + return DT_CONTINUE; } // obj1 points to a dynamic object - + DT_Vector3 p1, p2; + if (!DT_GetPenDepth(obj1->getObjectHandle(), obj2->getObjectHandle(), p1, p2)) + return DT_CONTINUE; + MT_Point3 local1(p1), local2(p2); + // Get collision data from SOLID + MT_Vector3 normal(local2 - local1); // This distinction between dynamic and non-dynamic objects should not be // necessary. Non-dynamic objects are assumed to have infinite mass. - if (obj1->isDynamic()) { - // normal to the contact plane - MT_Vector3 normal = obj2->m_pos - obj1->m_pos; - MT_Scalar dist = normal.length(); - - if (MT_EPSILON < dist && dist <= obj1->getMargin() + obj2->getMargin()) - { - normal /= dist; - // relative velocity of the contact points - MT_Vector3 rel_vel = obj2->actualLinVelocity() - obj1->actualLinVelocity(); - // relative velocity projected onto the normal - MT_Scalar rel_vel_normal = normal.dot(rel_vel); - - // if the objects are approaching eachother... - if (rel_vel_normal < 0.0) { - MT_Scalar restitution; - // ...and the approaching velocity is large enough... - if (-rel_vel_normal > ImpulseThreshold) { - // ...this must be a true collision. - restitution = - MT_min(obj1->getMaterialProps()->m_restitution, - obj2->getMaterialProps()->m_restitution); - } - else { - // otherwise, it is a resting contact, and thus the - // relative velocity must be made zero - restitution = 0.0; - // We also need to interfere with the positions of the - // objects, otherwise they might drift into eachother. - MT_Vector3 penalty = normal * - (0.5 * (obj1->getMargin() + obj2->getMargin() - dist)); - obj1->m_pos -= penalty; - obj2->m_pos += penalty; - } - - // Compute the impulse necessary to yield the desired relative velocity... - MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal / - (obj1->getInvMass() + obj2->getInvMass()); - - // ... and apply it. - obj1->applyCenterImpulse(-impulse * normal); - obj2->applyCenterImpulse( impulse * normal); - } + MT_Vector3 error = normal * 0.5f; + obj1->m_error += error; + obj2->m_error -= error; + // Remove the velocity component in the normal direction + // Calculate collision parameters + MT_Vector3 rel_vel = obj1->getLinearVelocity() - obj2->getLinearVelocity(); + if (normal.length() > FixThreshold && rel_vel.length() < FixVelocity) { + normal.normalize(); + MT_Scalar rel_vel_normal = 0.49*(normal.dot(rel_vel)); + + obj1->addLinearVelocity(-rel_vel_normal*normal); + obj2->addLinearVelocity(rel_vel_normal*normal); } } else { // Same again but now obj1 is non-dynamic - - // Compute the point on obj1 closest to obj2 (= sphere with radius = 0) - MT_Point3 local1, local2; - MT_Scalar dist = DT_GetClosestPair(obj1->m_object, obj2->m_object, - local1.getValue(), local2.getValue()); - - // local1 is th point closest to obj2 - // local2 is the local origin of obj2 - - if (MT_EPSILON < dist && dist <= obj2->getMargin()) { - MT_Point3 world1 = obj1->getWorldCoord(local1); - MT_Point3 world2 = obj2->getWorldCoord(local2); - MT_Vector3 vel1 = obj1->getVelocity(local1) + obj1->m_combined_lin_vel; - MT_Vector3 vel2 = obj2->getVelocity(local2) + obj2->m_combined_lin_vel; - - // the normal to the contact plane - MT_Vector3 normal = world2 - world1; - normal /= dist; - - // wr2 points from obj2's origin to the global contact point - // wr2 is only needed for rigid bodies (objects for which the - // friction can change the angular momentum). - // vel2 is adapted to denote the velocity of the contact point - MT_Vector3 wr2; - if (obj2->isRigidBody()) { - wr2 = -obj2->getMargin() * normal; - vel2 += obj2->actualAngVelocity().cross(wr2); - } - - - // This should look familiar.... - MT_Vector3 rel_vel = vel2 - vel1; - MT_Scalar rel_vel_normal = normal.dot(rel_vel); - - if (rel_vel_normal < 0.0) { - MT_Scalar restitution; - if (-rel_vel_normal > ImpulseThreshold) { - restitution = - MT_min(obj1->getMaterialProps()->m_restitution, - obj2->getMaterialProps()->m_restitution); - } - else { - restitution = 0.0; - // We fix drift by moving obj2 only, since obj1 was - // non-dynamic. - obj2->m_pos += normal * (obj2->getMargin() - dist); - - } - - MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal / - (obj1->getInvMass() + obj2->getInvMass()); - - - obj2->applyCenterImpulse( impulse * normal); - -// The friction part starts here!!!!!!!! - - // Compute the lateral component of the relative velocity - // lateral actually points in the opposite direction, i.e., - // into the direction of the friction force. - -#if 1 - // test - only do friction on the physics part of the - // velocity. - vel1 -= obj1->m_combined_lin_vel; - vel2 -= obj2->m_combined_lin_vel; - - // This should look familiar.... - rel_vel = vel2 - vel1; - rel_vel_normal = normal.dot(rel_vel); -#endif - - MT_Vector3 lateral = normal * rel_vel_normal - rel_vel; - - const SM_ShapeProps *shapeProps = obj2->getShapeProps(); - - if (shapeProps->m_do_anisotropic) { - - // For anisotropic friction we scale the lateral component, - // rather than compute a direction-dependent fricition - // factor. For this the lateral component is transformed to - // local coordinates. - - MT_Matrix3x3 lcs(obj2->m_orn); - // We cannot use m_xform.getBasis() for the matrix, since - // it might contain a non-uniform scaling. - // OPT: it's a bit daft to compute the matrix since the - // quaternion itself can be used to do the transformation. - - MT_Vector3 loc_lateral = lateral * lcs; - // lcs is orthogonal so lcs.inversed() == lcs.transposed(), - // and lcs.transposed() * lateral == lateral * lcs. - - const MT_Vector3& friction_scaling = - shapeProps->m_friction_scaling; - - // Scale the local lateral... - loc_lateral.scale(friction_scaling[0], - friction_scaling[1], - friction_scaling[2]); - // ... and transform it back to global coordinates - lateral = lcs * loc_lateral; - } - - // A tiny Coulomb friction primer: - // The Coulomb friction law states that the magnitude of the - // maximum possible friction force depends linearly on the - // magnitude of the normal force. - // - // F_max_friction = friction_factor * F_normal - // - // (NB: independent of the contact area!!) - // - // The friction factor depends on the material. - // We use impulses rather than forces but let us not be - // bothered by this. - - - MT_Scalar rel_vel_lateral = lateral.length(); - - if (rel_vel_lateral > MT_EPSILON) { - lateral /= rel_vel_lateral; - - MT_Scalar friction_factor = - MT_min(obj1->getMaterialProps()->m_friction, - obj2->getMaterialProps()->m_friction); - - // Compute the maximum friction impulse - - MT_Scalar max_friction = - friction_factor * MT_max(0.0, impulse); - - // I guess the GEN_max is not necessary, so let's check it - - assert(impulse >= 0.0); - - // Here's the trick. We compute the impulse to make the - // lateral velocity zero. (Make the objects stick together - // at the contact point. If this impulse is larger than - // the maximum possible friction impulse, then shrink its - // magnitude to the maximum friction. - - if (obj2->isRigidBody()) { - - // For rigid bodies we take the inertia into account, - // since the friciton impulse is going to change the - // angular momentum as well. - MT_Scalar impulse_lateral = rel_vel_lateral / - (obj1->getInvMass() + obj2->getInvMass() + - ((obj2->getInvInertia() * wr2.cross(lateral)).cross(wr2)).dot(lateral)); - MT_Scalar friction = MT_min(impulse_lateral, max_friction); - obj2->applyImpulse(world2 + wr2, friction * lateral); - } - else { - MT_Scalar impulse_lateral = rel_vel_lateral / - (obj1->getInvMass() + obj2->getInvMass()); - - MT_Scalar friction = MT_min(impulse_lateral, max_friction); - obj2->applyCenterImpulse( friction * lateral); - } - } - - obj2->calcXform(); - obj2->notifyClient(); - } + obj2->m_error -= normal; + MT_Vector3 rel_vel = obj2->getLinearVelocity(); + if (normal.length() > FixThreshold && rel_vel.length() < FixVelocity) { + // Calculate collision parameters + normal.normalize(); + MT_Scalar rel_vel_normal = -0.99*(normal.dot(rel_vel)); + + obj2->addLinearVelocity(rel_vel_normal*normal); } } + + return DT_CONTINUE; } - - +void SM_Object::relax(void) +{ + if (m_error.fuzzyZero()) + return; + //std::cout << "SM_Object::relax: { " << m_error << " }" << std::endl; + + m_pos += m_error; + m_error.setValue(0., 0., 0.); +/* m_pos.getValue(pos); + DT_SetPosition(m_object, pos); + m_xform.setOrigin(m_pos); + m_xform.getValue(m_ogl_matrix); */ + calcXform(); + notifyClient(); +} + SM_Object::SM_Object( ) { // warning no initialization of variables done by moto. @@ -388,8 +471,11 @@ SM_Object::SM_Object( SM_Object:: ~SM_Object() { - delete m_fh_object; - DT_DeleteObject(m_object); + if (m_fh_object) + delete m_fh_object; + + DT_DestroyObject(m_object); + m_object = NULL; } bool @@ -489,14 +575,35 @@ registerCallback( void SM_Object:: calcXform() { +#ifdef SM_DEBUG_XFORM + printf("SM_Object::calcXform m_pos = { %-0.5f, %-0.5f, %-0.5f }\n", + m_pos[0], m_pos[1], m_pos[2]); + printf(" m_orn = { %-0.5f, %-0.5f, %-0.5f, %-0.5f }\n", + m_orn[0], m_orn[1], m_orn[2], m_orn[3]); + printf(" m_scaling = { %-0.5f, %-0.5f, %-0.5f }\n", + m_scaling[0], m_scaling[1], m_scaling[2]); +#endif m_xform.setOrigin(m_pos); m_xform.setBasis(MT_Matrix3x3(m_orn, 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(m_pos); m_fh_object->calcXform(); } +#ifdef SM_DEBUG_XFORM + printf("\n | %-0.5f %-0.5f %-0.5f %-0.5f |\n", + m_ogl_matrix[0], m_ogl_matrix[4], m_ogl_matrix[ 8], m_ogl_matrix[12]); + printf( " | %-0.5f %-0.5f %-0.5f %-0.5f |\n", + m_ogl_matrix[1], m_ogl_matrix[5], m_ogl_matrix[ 9], m_ogl_matrix[13]); + printf( "m_ogl_matrix = | %-0.5f %-0.5f %-0.5f %-0.5f |\n", + m_ogl_matrix[2], m_ogl_matrix[6], m_ogl_matrix[10], m_ogl_matrix[14]); + printf( " | %-0.5f %-0.5f %-0.5f %-0.5f |\n\n", + m_ogl_matrix[3], m_ogl_matrix[7], m_ogl_matrix[11], m_ogl_matrix[15]); +#endif } // Call callbacks to notify the client of a change of placement @@ -561,7 +668,7 @@ setMargin( MT_Scalar margin ) { m_margin = margin; - DT_SetMargin(m_object, margin); + DT_SetMargin(m_object, margin); } MT_Scalar @@ -841,11 +948,18 @@ getVelocity( // For displaced objects the velocity is faked using the previous state. // Dynamic objects get their own velocity, not the faked velocity. // (Dynamic objects shouldn't be displaced in the first place!!) - +/* FIXME: -KM- Valgrind report: +==17624== Use of uninitialised value of size 8 +==17624== at 0x831F925: MT_Vector3::dot(MT_Vector3 const&) const (MT_Tuple3.h:60) +==17624== by 0x82E4574: SM_Object::getVelocity(MT_Point3 const&) const (MT_Matrix3x3.h:81) +==17624== by 0x82E324D: SM_Object::boing(void*, void*, void*, DT_CollData const*) (SM_Object.cpp:319) +==17624== by 0x83E7308: DT_Encounter::exactTest(DT_RespTable const*, int&) const (in /home/kester/blender-src/DEBUG/blender) +*/ return m_prev_kinematic && !isDynamic() ? (m_xform(local) - m_prev_xform(local)) / m_timeStep : - m_lin_vel + m_ang_vel.cross(m_xform.getBasis() * local); + m_lin_vel + m_ang_vel.cross(local); + // m_lin_vel + m_ang_vel.cross(m_xform.getBasis() * local); // NB: m_xform.getBasis() * local == m_xform(local) - m_xform.getOrigin() } @@ -898,21 +1012,6 @@ getShapeHandle( return m_shape; } - void -SM_Object:: -setClientObject( - void *clientobj -) { - m_client_object = clientobj; -} - - void * -SM_Object:: -getClientObject( -){ - return m_client_object; -} - SM_Object * SM_Object:: getDynamicParent( diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp index 424796ea71e..5ac65cbd816 100644 --- a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp @@ -2,6 +2,34 @@ * $Id$ * Copyright (C) 2001 NaN Technologies B.V. * The physics scene. + * + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** */ #ifdef HAVE_CONFIG_H @@ -16,31 +44,101 @@ #include "SM_Object.h" #include "SM_FhObject.h" +#include "SM_Debug.h" + #include <algorithm> +SM_Scene::SM_Scene() : + m_scene(DT_CreateScene()), + m_respTable(DT_CreateRespTable()), + m_secondaryRespTable(DT_CreateRespTable()), + m_fixRespTable(DT_CreateRespTable()), + m_forceField(0.0, 0.0, 0.0) +{ + for (int i = 0 ; i < NUM_RESPONSE; i++) + { + m_ResponseClass[i] = DT_GenResponseClass(m_respTable); + m_secondaryResponseClass[i] = DT_GenResponseClass(m_secondaryRespTable); + m_fixResponseClass[i] = DT_GenResponseClass(m_fixRespTable); + } + + /* Sensor */ + DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[SENSOR_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this); + + /* Static */ + DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[STATIC_RESPONSE], 0, DT_NO_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[STATIC_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this); + + /* Object */ + DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], SM_Scene::boing, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::boing, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_respTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this); + + /* 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_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) */ + DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[SENSOR_RESPONSE], 0, DT_NO_RESPONSE, this); + DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[STATIC_RESPONSE], SM_Object::fix, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[OBJECT_RESPONSE], SM_Object::fix, DT_SIMPLE_RESPONSE, this); + DT_AddPairResponse(m_fixRespTable, m_ResponseClass[OBJECT_RESPONSE], m_ResponseClass[FH_RESPONSE], 0, DT_NO_RESPONSE, this); +} + +void SM_Scene::addTouchCallback(int response_class, DT_ResponseCallback callback, void *user) +{ + DT_AddClassResponse(m_secondaryRespTable, m_secondaryResponseClass[response_class], callback, DT_SIMPLE_RESPONSE, user); +} + +void SM_Scene::addSensor(SM_Object& object) +{ + object.calcXform(); + m_objectList.push_back(&object); + DT_AddObject(m_scene, object.getObjectHandle()); + DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[SENSOR_RESPONSE]); + DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[SENSOR_RESPONSE]); + DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[SENSOR_RESPONSE]); +} + void SM_Scene::add(SM_Object& object) { object.calcXform(); m_objectList.push_back(&object); DT_AddObject(m_scene, object.getObjectHandle()); if (object.isDynamic()) { - DT_SetObjectResponse(m_respTable, object.getObjectHandle(), - SM_Object::boing, DT_SIMPLE_RESPONSE, this); - } + DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]); + DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]); + DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]); + } else { + DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[STATIC_RESPONSE]); + DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[STATIC_RESPONSE]); + DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[STATIC_RESPONSE]); + } - if (object.getDynamicParent()) { - DT_SetPairResponse(m_respTable, object.getObjectHandle(), - object.getDynamicParent()->getObjectHandle(), - 0, DT_NO_RESPONSE, 0); - } SM_FhObject *fh_object = object.getFhObject(); if (fh_object) { DT_AddObject(m_scene, fh_object->getObjectHandle()); - DT_SetObjectResponse(m_respTable, fh_object->getObjectHandle(), - SM_FhObject::ray_hit, DT_SIMPLE_RESPONSE, this); + DT_SetResponseClass(m_respTable, fh_object->getObjectHandle(), m_ResponseClass[FH_RESPONSE]); + DT_SetResponseClass(m_secondaryRespTable, fh_object->getObjectHandle(), m_secondaryResponseClass[FH_RESPONSE]); + DT_SetResponseClass(m_fixRespTable, fh_object->getObjectHandle(), m_fixResponseClass[FH_RESPONSE]); } } +void SM_Scene::requestCollisionCallback(SM_Object &object) +{ + DT_SetResponseClass(m_respTable, object.getObjectHandle(), m_ResponseClass[OBJECT_RESPONSE]); + DT_SetResponseClass(m_secondaryRespTable, object.getObjectHandle(), m_secondaryResponseClass[OBJECT_RESPONSE]); +// DT_SetResponseClass(m_fixRespTable, object.getObjectHandle(), m_fixResponseClass[OBJECT_RESPONSE]); +} + void SM_Scene::remove(SM_Object& object) { T_ObjectList::iterator i = std::find(m_objectList.begin(), m_objectList.end(), &object); @@ -49,21 +147,11 @@ void SM_Scene::remove(SM_Object& object) { std::swap(*i, m_objectList.back()); m_objectList.pop_back(); DT_RemoveObject(m_scene, object.getObjectHandle()); - if (object.isDynamic()) { - DT_ClearObjectResponse(m_respTable, object.getObjectHandle()); - } - - if (object.getDynamicParent()) { - DT_ClearPairResponse(m_respTable, object.getObjectHandle(), - object.getDynamicParent()->getObjectHandle()); - } SM_FhObject *fh_object = object.getFhObject(); if (fh_object) { DT_RemoveObject(m_scene, fh_object->getObjectHandle()); - DT_ClearObjectResponse(m_respTable, - fh_object->getObjectHandle()); } } else { @@ -81,29 +169,28 @@ void SM_Scene::proceed(MT_Scalar timeStep, MT_Scalar subSampling) { // equal to subSampling (might be a little smaller). int num_samples = (int)ceil(timeStep / subSampling); - MT_Scalar subStep = timeStep / num_samples; T_ObjectList::iterator i; // Apply a forcefield (such as gravity) for (i = m_objectList.begin(); i != m_objectList.end(); ++i) { (*i)->applyForceField(m_forceField); + //(*i)->integrateForces(subStep); + //(*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) { (*i)->integrateForces(subStep); - } - + //(*i)->backup(); // And second we update the object positions by performing // an integration step for each object - for (i = m_objectList.begin(); i != m_objectList.end(); ++i) { (*i)->integrateMomentum(subStep); } -#if 0 // I changed the order of the next 2 statements. // Originally objects were first integrated with a call // to proceed(). However if external objects were @@ -113,17 +200,18 @@ void SM_Scene::proceed(MT_Scalar timeStep, MT_Scalar subSampling) { // So now first we let the physics scene respond to // new forces, velocities set externally. -#endif // The collsion and friction impulses are computed here. DT_Test(m_scene, m_respTable); - } - // clear the user set velocities. #if 0 clearObjectCombinedVelocities(); #endif + for (step = 0; step < 5 && DT_Test(m_scene, m_fixRespTable); step++) + for (i = m_objectList.begin(); i != m_objectList.end(); ++i) + (*i)->relax(); + // Finish this timestep by saving al state information for the next // timestep and clearing the accumulated forces. @@ -132,7 +220,7 @@ void SM_Scene::proceed(MT_Scalar timeStep, MT_Scalar subSampling) { (*i)->saveReactionForce(timeStep); (*i)->clearForce(); } - + // For each pair of object that collided, call the corresponding callback. // Additional collisions of a pair within the same time step are ignored. @@ -152,14 +240,25 @@ void SM_Scene::proceed(MT_Scalar timeStep, MT_Scalar subSampling) { SM_Object *SM_Scene::rayTest(void *ignore_client, const MT_Point3& from, const MT_Point3& to, MT_Point3& result, MT_Vector3& normal) const { - MT_Point3 local; +#ifdef SM_DEBUG_RAYCAST + std::cout << "ray: { " << from << " } - { " << to << " }" << std::endl; +#endif + DT_Vector3 n, dfrom, dto; + DT_Scalar param; + from.getValue(dfrom); + to.getValue(dto); SM_Object *hit_object = (SM_Object *) - DT_RayTest(m_scene, ignore_client, from.getValue(), to.getValue(), - local.getValue(), normal.getValue()); + DT_RayCast(m_scene, ignore_client, dfrom, dto, 1., ¶m, n); if (hit_object) { - result = hit_object->getWorldCoord(local); + //result = hit_object->getWorldCoord(from + (to - from)*param); + result = from + (to - from) * param; + normal.setValue(n); +#ifdef SM_DEBUG_RAYCAST + std::cout << "ray: { " << from << " } -> { " << to << " }: { " << result + << " } (" << param << "), normal = { " << normal << " }" << std::endl; +#endif } return hit_object; @@ -178,6 +277,34 @@ void SM_Scene::clearObjectCombinedVelocities() { } +void SM_Scene::setSecondaryRespTable(DT_RespTableHandle secondaryRespTable) { + m_secondaryRespTable = secondaryRespTable; +} +DT_Bool SM_Scene::boing( + void *client_data, + void *object1, + void *object2, + const DT_CollData *coll_data +){ + SM_Scene *scene = (SM_Scene *)client_data; + SM_Object *obj1 = (SM_Object *)object1; + SM_Object *obj2 = (SM_Object *)object2; + + scene->addPair(obj1, obj2); // Record this collision for client callbacks +#ifdef SM_DEBUG_BOING + printf("SM_Scene::boing\n"); +#endif + + return DT_CONTINUE; +} + +SM_Scene::~SM_Scene() +{ +/* if (m_objectList.begin() != m_objectList.end()) + std::cout << "SM_Scene::~SM_Scene: There are still objects in the Sumo scene!" << std::endl; */ + DT_DestroyRespTable(m_respTable); + DT_DestroyScene(m_scene); +} |