diff options
Diffstat (limited to 'source/gameengine/Physics/Sumo/Fuzzics')
15 files changed, 3256 insertions, 0 deletions
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/Makefile b/source/gameengine/Physics/Sumo/Fuzzics/Makefile new file mode 100644 index 00000000000..d721a416862 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/Makefile @@ -0,0 +1,9 @@ +# +# $Id$ +# Copyright (C) 2001 NaN Technologies B.V. +# Bounce make to subdirectories. + +SOURCEDIR = source/sumo/Fuzzics +DIRS = src + +include nan_subdirs.mk diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Callback.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Callback.h new file mode 100644 index 00000000000..a43ddbec483 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Callback.h @@ -0,0 +1,10 @@ +#ifndef SM_CALLBACK_H +#define SM_CALLBACK_H + +class SM_Callback { +public: + virtual void do_me() = 0; +}; + +#endif + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_ClientObjectInfo.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_ClientObjectInfo.h new file mode 100644 index 00000000000..2063892e671 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_ClientObjectInfo.h @@ -0,0 +1,15 @@ +#ifndef __SM_CLIENTOBJECT_INFO_H +#define __SM_CLIENTOBJECT_INFO_H + +/** + * Client Type and Additional Info. This structure can be use instead of a bare void* pointer, for safeness, and additional info for callbacks + */ + +struct SM_ClientObjectInfo +{ + int m_type; + void* m_clientobject; + void* m_auxilary_info; +}; + +#endif //__SM_CLIENTOBJECT_INFO_H diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_FhObject.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_FhObject.h new file mode 100644 index 00000000000..4aac43f6712 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_FhObject.h @@ -0,0 +1,36 @@ +#ifndef SM_FHOBJECT_H +#define SM_FHOBJECT_H + +#include "SM_Object.h" + +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) {} + + 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; } + + static void ray_hit(void *client_data, + void *object1, + void *object2, + const DT_CollData *coll_data); + +private: + MT_Vector3 m_ray; + MT_Vector3 m_ray_direction; + SM_Object *m_client_object; +}; + +#endif + + + + + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h new file mode 100644 index 00000000000..9c6a9ddaec2 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_MotionState.h @@ -0,0 +1,36 @@ +#ifndef SM_MOTIONSTATE_H +#define SM_MOTIONSTATE_H + +#include "MT_Transform.h" + +class SM_MotionState { +public: + SM_MotionState() : + m_pos(0.0, 0.0, 0.0), + m_orn(0.0, 0.0, 0.0, 1.0), + m_lin_vel(0.0, 0.0, 0.0), + m_ang_vel(0.0, 0.0, 0.0) + {} + + void setPosition(const MT_Point3& pos) { m_pos = pos; } + void setOrientation(const MT_Quaternion& orn) { m_orn = orn; } + void setLinearVelocity(const MT_Vector3& lin_vel) { m_lin_vel = lin_vel; } + void setAngularVelocity(const MT_Vector3& ang_vel) { m_ang_vel = ang_vel; } + + const MT_Point3& getPosition() const { return m_pos; } + const MT_Quaternion& getOrientation() const { return m_orn; } + const MT_Vector3& getLinearVelocity() const { return m_lin_vel; } + const MT_Vector3& getAngularVelocity() const { return m_ang_vel; } + + virtual MT_Transform getTransform() const { + return MT_Transform(m_pos, m_orn); + } + +protected: + MT_Point3 m_pos; + MT_Quaternion m_orn; + MT_Vector3 m_lin_vel; + MT_Vector3 m_ang_vel; +}; + +#endif diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h new file mode 100644 index 00000000000..670da6e71db --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h @@ -0,0 +1,280 @@ +#ifndef SM_OBJECT_H +#define SM_OBJECT_H + +#include <vector> + +#include "solid.h" + +#include "SM_Callback.h" +#include "SM_MotionState.h" +#include <stdio.h> + + +class SM_FhObject; + + +// Properties of dynamic objects +struct SM_ShapeProps { + MT_Scalar m_mass; // Total mass + 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 + MT_Scalar m_friction_scaling[3]; // Scaling for anisotropic friction. Component in range [0, 1] + bool m_do_anisotropic; // Should I do anisotropic friction? + bool m_do_fh; // Should the object have a linear Fh spring? + bool m_do_rot_fh; // Should the object have an angular Fh spring? +}; + + +// Properties of collidable objects (non-ghost objects) +struct SM_MaterialProps { + MT_Scalar m_restitution; // restitution of energie after a collision 0 = inelastic, 1 = elastic + MT_Scalar m_friction; // Coulomb friction (= ratio between the normal en maximum friction force) + MT_Scalar m_fh_spring; // Spring constant (both linear and angular) + MT_Scalar m_fh_damping; // Damping factor (linear and angular) in range [0, 1] + MT_Scalar m_fh_distance; // The range above the surface where Fh is active. + bool m_fh_normal; // Should the object slide off slopes? +}; + + +class SM_Object : public SM_MotionState { +public: + SM_Object() ; + SM_Object( + DT_ShapeHandle shape, + const SM_MaterialProps *materialProps, + const SM_ShapeProps *shapeProps, + SM_Object *dynamicParent + ); + + virtual ~SM_Object(); + + bool isDynamic() const; + + /* nzc experimental. There seem to be two places where kinematics + * are evaluated: proceedKinematic (called from SM_Scene) and + * proceed() in this object. I'll just try and bunge these out for + * now. */ + + void suspend(void); + void resume(void); + + void suspendDynamics(); + + void restoreDynamics(); + + bool isGhost() const; + + void suspendMaterial(); + + void restoreMaterial(); + + SM_FhObject *getFhObject() const; + + void registerCallback(SM_Callback& callback); + + void calcXform(); + void notifyClient(); + + // Save the current state information for use in the + // velocity computation in the next frame. + + void proceedKinematic(MT_Scalar timeStep); + + void saveReactionForce(MT_Scalar timeStep) ; + + void clearForce() ; + + void clearMomentum() ; + + void setMargin(MT_Scalar margin) ; + + MT_Scalar getMargin() const ; + + const SM_MaterialProps *getMaterialProps() const ; + + const SM_ShapeProps *getShapeProps() const ; + + void setPosition(const MT_Point3& pos); + void setOrientation(const MT_Quaternion& orn); + void setScaling(const MT_Vector3& scaling); + + + /** + * set an external velocity. This velocity complements + * the physics velocity. So setting it does not override the + * physics velocity. It is your responsibility to clear + * this external velocity. This velocity is not subject to + * friction or damping. + */ + + + void setExternalLinearVelocity(const MT_Vector3& lin_vel) ; + void addExternalLinearVelocity(const MT_Vector3& lin_vel) ; + + /** Override the physics velocity */ + + void addLinearVelocity(const MT_Vector3& lin_vel); + void setLinearVelocity(const MT_Vector3& lin_vel); + + /** + * Set an external angular velocity. This velocity complemetns + * the physics angular velocity so does not override it. It is + * your responsibility to clear this velocity. This velocity + * is not subject to friction or damping. + */ + + void setExternalAngularVelocity(const MT_Vector3& ang_vel) ; + void addExternalAngularVelocity(const MT_Vector3& ang_vel); + + /** Override the physics angular velocity */ + + void addAngularVelocity(const MT_Vector3& ang_vel); + void setAngularVelocity(const MT_Vector3& ang_vel); + + /** Clear the external velocities */ + + void clearCombinedVelocities(); + + /** + * Tell the physics system to combine the external velocity + * with the physics velocity. + */ + + void resolveCombinedVelocities( + const MT_Vector3 & lin_vel, + const MT_Vector3 & ang_vel + ) ; + + + + MT_Scalar getInvMass() const; + + MT_Scalar getInvInertia() const ; + + void applyForceField(const MT_Vector3& accel) ; + + void applyCenterForce(const MT_Vector3& force) ; + + void applyTorque(const MT_Vector3& torque) ; + + void applyImpulse(const MT_Point3& attach, const MT_Vector3& impulse) ; + + void applyCenterImpulse(const MT_Vector3& impulse); + + void applyAngularImpulse(const MT_Vector3& impulse); + + MT_Point3 getWorldCoord(const MT_Point3& local) const; + + MT_Vector3 getVelocity(const MT_Point3& local) const; + + + const MT_Vector3& getReactionForce() const ; + + void getMatrix(double *m) const ; + + const double *getMatrix() const ; + + // Still need this??? + const MT_Transform& getScaledTransform() const; + + DT_ObjectHandle getObjectHandle() const ; + DT_ShapeHandle getShapeHandle() const ; + + void setClientObject(void *clientobj) ; + void *getClientObject() ; + + SM_Object *getDynamicParent() ; + + void integrateForces(MT_Scalar timeStep); + void integrateMomentum(MT_Scalar timeSteo); + + void setRigidBody(bool is_rigid_body) ; + + bool isRigidBody() const ; + + + // This is the callback for handling collisions of dynamic objects + static + void + boing( + void *client_data, + void *object1, + void *object2, + const DT_CollData *coll_data + ); + +private: + + // return the actual linear_velocity of this object this + // is the addition of m_combined_lin_vel and m_lin_vel. + + const + MT_Vector3 + actualLinVelocity( + ) const ; + + const + MT_Vector3 + actualAngVelocity( + ) const ; + + typedef std::vector<SM_Callback *> T_CallbackList; + + + 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 + // on an SM_Object, there must be a way that the SM_Object client + // can identify it's clientdata after a collision + void *m_client_object; + + DT_ShapeHandle m_shape; // Shape for collision detection + + // Material and shape properties are not owned by this class. + + const SM_MaterialProps *m_materialProps; + 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. + 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 + + double m_ogl_matrix[16]; // An OpenGL-type 4x4 matrix + MT_Transform m_xform; // The object's local coordinate system + MT_Transform m_prev_xform; // The object's local coordinate system in the previous frame + SM_MotionState m_prev_state; // The object's motion state in the previous frame + MT_Scalar m_timeStep; // The duration of the last frame + + 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) + + // Here are the values of externally set linear and angular + // velocity. These are updated from the outside + // (actuators and python) each frame and combined with the + // physics values. At the end of each frame (at the end of a + // call to proceed) they are set to zero. This allows the + // outside world to contribute to the velocity of an object + // but still have it react to physics. + + MT_Vector3 m_combined_lin_vel; + MT_Vector3 m_combined_ang_vel; + + // The force and torque are the accumulated forces and torques applied by the client (game logic, python). + + SM_FhObject *m_fh_object; // The ray object used for Fh + bool m_suspended; // Is this object frozen? +}; + +#endif diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Props.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Props.h new file mode 100644 index 00000000000..0b2efacac2a --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Props.h @@ -0,0 +1,60 @@ +/** + * $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_PROPSH +#define SM_PROPSH + +#include <MT_Scalar.h> + +// Properties of dynamic objects +struct SM_ShapeProps { + MT_Scalar m_mass; // Total mass + 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 + MT_Scalar m_friction_scaling[3]; // Scaling for anisotropic friction. Component in range [0, 1] + bool m_do_anisotropic; // Should I do anisotropic friction? + bool m_do_fh; // Should the object have a linear Fh spring? + bool m_do_rot_fh; // Should the object have an angular Fh spring? +}; + + +// Properties of collidable objects (non-ghost objects) +struct SM_MaterialProps { + MT_Scalar m_restitution; // restitution of energie after a collision 0 = inelastic, 1 = elastic + MT_Scalar m_friction; // Coulomb friction (= ratio between the normal en maximum friction force) + MT_Scalar m_fh_spring; // Spring constant (both linear and angular) + MT_Scalar m_fh_damping; // Damping factor (linear and angular) in range [0, 1] + MT_Scalar m_fh_distance; // The range above the surface where Fh is active. + bool m_fh_normal; // Should the object slide off slopes? +}; + +#endif //SM_PROPSH diff --git a/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h new file mode 100644 index 00000000000..761e6c7c449 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/include/SM_Scene.h @@ -0,0 +1,129 @@ +/** + * $Id$ + * Copyright (C) 2001 NaN Technologies B.V. + * The physics scene. + */ + +#ifndef SM_SCENE_H +#define SM_SCENE_H + +#pragma warning (disable : 4786) + +#include <vector> +#include <set> +#include <utility> //needed for pair + +#include "solid.h" + +#include "MT_Vector3.h" +#include "MT_Point3.h" + +class SM_Object; + +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() { + DT_DeleteRespTable(m_respTable); + DT_DeleteScene(m_scene); + } + + DT_RespTableHandle getRespTableHandle() const { + return m_respTable; + } + + const MT_Vector3& getForceField() const { + return m_forceField; + } + + MT_Vector3& getForceField() { + return m_forceField; + } + + void setForceField(const MT_Vector3& forceField) { + m_forceField = forceField; + } + + void add(SM_Object& object); + void remove(SM_Object& object); + + void addPair(SM_Object *obj1, SM_Object *obj2) { + m_pairList.insert(std::make_pair(obj1, obj2)); + } + + void clearPairs() { + m_pairList.clear(); + } + + void setSecondaryRespTable(DT_RespTableHandle secondaryRespTable) { + m_secondaryRespTable = secondaryRespTable; + } + + + // Perform an integration step of duration 'timeStep'. + // 'subSampling' is the maximum duration of a substep, i.e., + // The maximum time interval between two collision checks. + // 'subSampling' can be used to control aliasing effects + // (fast moving objects traversing through walls and such). + void proceed(MT_Scalar timeStep, MT_Scalar subSampling); + + /** + * Test whether any objects lie on the line defined by from and + * to. The search returns the first such bject starting at from, + * or NULL if there was none. + * @returns A reference to the object, or NULL if there was none. + * @param ignore_client Do not look for collisions with this + * object. This can be useful to avoid self-hits if + * starting from the location of an object. + * @param from The start point, in world coordinates, of the search. + * @param to The end point, in world coordinates, of the search. + * @param result A store to return the point where intersection + * took place (if there was an intersection). + * @param normal A store to return the normal of the hit object on + * the location of the intersection, if it took place. + */ + SM_Object *rayTest(void *ignore_client, + const MT_Point3& from, const MT_Point3& to, + MT_Point3& result, MT_Vector3& normal) const; + +private: + + // Clear the user set velocities. + void clearObjectCombinedVelocities(); + + /** internal type */ + typedef std::vector<SM_Object *> T_ObjectList; + /** internal type */ + typedef std::set<std::pair<SM_Object *, SM_Object *> > T_PairList; + + /** Handle to the scene in SOLID */ + DT_SceneHandle m_scene; + /** Following response table contains the callbacks for the dynmics */ + DT_RespTableHandle m_respTable; + /** + * Following response table contains callbacks for the client (= + * game engine) */ + DT_RespTableHandle m_secondaryRespTable; // Handle + + /** The acceleration from the force field */ + MT_Vector3 m_forceField; + + /** + * The list of objects that receive motion updates and do + * collision tests. */ + T_ObjectList m_objectList; + + /** + * A list with pairs of objects that collided the previous + * timestep. The list is built during the proceed(). During that + * time, it is not valid. */ + T_PairList m_pairList; +}; + +#endif diff --git a/source/gameengine/Physics/Sumo/Fuzzics/sample/Makefile b/source/gameengine/Physics/Sumo/Fuzzics/sample/Makefile new file mode 100644 index 00000000000..672dff39028 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/sample/Makefile @@ -0,0 +1,25 @@ +# +# $Id$ +# Copyright (C) 2001 NaN Technologies B.V. + +DIR = $(OCGDIR)/sumo +ALLTARGETS = $(OBJS) $(DIR)/$(DEBUG_DIR)particle $(DIR)/$(DEBUG_DIR)particle0 + +include nan_compile.mk + +CPPFLAGS = -I../../include -I../include -I$(NAN_MOTO)/include +CPPFLAGS += -I$(OPENGL_HEADERS) + +clean:: + @$(RM) $(DIR)/particle $(DIR)/particle0 + @$(RM) $(DIR)/debug/particle $(DIR)/debug/particle0 + +LDFLAGS = -L$(DIR) -L/usr/X11R6/lib +OGL_LDLIBS = -lglut -lGLU -lGL -pthread +LDLIBS = -lfuzzics -lsolid $(NAN_MOTO)/lib/libmoto.a $(OGL_LDLIBS) + +$(DIR)/$(DEBUG_DIR)particle: particle.o $(DIR)/$(DEBUG_DIR)libfuzzics.a $(DIR)/$(DEBUG_DIR)libsolid.a + $(CCC) $(CCFLAGS) $(CPPFLAGS) $(LDFLAGS) $< -o $@ $(LDLIBS) + +$(DIR)/$(DEBUG_DIR)particle0: particle0.o $(DIR)/$(DEBUG_DIR)libfuzzics.a $(DIR)/$(DEBUG_DIR)libsolid.a + $(CCC) $(CCFLAGS) $(CPPFLAGS) $(LDFLAGS) $< -o $@ $(LDLIBS) diff --git a/source/gameengine/Physics/Sumo/Fuzzics/sample/particle.cpp b/source/gameengine/Physics/Sumo/Fuzzics/sample/particle.cpp new file mode 100644 index 00000000000..e8e78d89a4e --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/sample/particle.cpp @@ -0,0 +1,705 @@ +//#define FAKE_IT +#define USE_COMPLEX +#define QUADS + +#include <algorithm> +#include <new> +#include <GL/glut.h> + +#include "MT_MinMax.h" +#include "MT_Point3.h" +#include "MT_Vector3.h" +#include "MT_Quaternion.h" +#include "MT_Matrix3x3.h" +#include "MT_Transform.h" + +#include "SM_Object.h" +#include "SM_FhObject.h" +#include "SM_Scene.h" + +#include "solid.h" + +const MT_Scalar bowl_curv = 0.10; +const MT_Scalar timeStep = 0.02; +const MT_Scalar ground_margin = 0.0; +const MT_Scalar sphere_radius = 0.5; + +const MT_Vector3 gravity(0, -9.8, 0); + +static MT_Scalar DISTANCE = 5; + +static MT_Scalar ele = 0, azi = 0; +static MT_Point3 eye(0, 0, DISTANCE); +static MT_Point3 center(0, 0, 0); + +inline double irnd() { return 2 * MT_random() - 1; } + +static const double SCALE_BOTTOM = 0.5; +static const double SCALE_FACTOR = 2.0; + +SM_ShapeProps g_shapeProps = { + 1.0, // mass + 1.0, // inertia + 0.1, // linear drag + 0.1, // angular drag + { 1.0, 0.0, 0.0 }, // anisotropic friction + false, // do anisotropic friction? + true, // do fh? + true // do rot fh? +}; + +SM_MaterialProps g_materialProps = { + 0.7, // restitution + 0.0, // friction + 10.0, // Fh spring constant + 1.0, // Fh damping + 0.5, // Fh distance + true // Fh leveling +}; + + +void toggleIdle(); + + +void newRandom(); + +void coordSystem() { + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glColor3f(1, 0, 0); + glVertex3d(0, 0, 0); + glVertex3d(10, 0, 0); + glColor3f(0, 1, 0); + glVertex3d(0, 0, 0); + glVertex3d(0, 10, 0); + glColor3f(0, 0, 1); + glVertex3d(0, 0, 0); + glVertex3d(0, 0, 10); + glEnd(); + glEnable(GL_LIGHTING); +} + + +void display_bbox(const MT_Point3& min, const MT_Point3& max) { + glDisable(GL_DEPTH_TEST); + glDisable(GL_LIGHTING); + glColor3f(0, 1, 1); + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + glBegin(GL_QUAD_STRIP); + glVertex3d(min[0], min[1], min[2]); + glVertex3d(min[0], min[1], max[2]); + glVertex3d(max[0], min[1], min[2]); + glVertex3d(max[0], min[1], max[2]); + glVertex3d(max[0], max[1], min[2]); + glVertex3d(max[0], max[1], max[2]); + glVertex3d(min[0], max[1], min[2]); + glVertex3d(min[0], max[1], max[2]); + glVertex3d(min[0], min[1], min[2]); + glVertex3d(min[0], min[1], max[2]); + glEnd(); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + glEnable(GL_LIGHTING); + glEnable(GL_DEPTH_TEST); +} + + + + +class GLShape { +public: + virtual void paint(GLdouble *m) const = 0; +}; + + +class GLSphere : public GLShape { + MT_Scalar radius; +public: + GLSphere(MT_Scalar r) : radius(r) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + glutSolidSphere(radius, 20, 20); + glPopMatrix(); + } +}; + + +class GLBox : public GLShape { + MT_Vector3 extent; +public: + GLBox(MT_Scalar x, MT_Scalar y, MT_Scalar z) : + extent(x, y, z) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + glPushMatrix(); + glScaled(extent[0], extent[1], extent[2]); + glutSolidCube(1.0); + glPopMatrix(); + glPopMatrix(); + } +}; + + +class GLCone : public GLShape { + MT_Scalar bottomRadius; + MT_Scalar height; + mutable GLuint displayList; + +public: + GLCone(MT_Scalar r, MT_Scalar h) : + bottomRadius(r), + height(h), + displayList(0) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + if (displayList) glCallList(displayList); + else { + GLUquadricObj *quadObj = gluNewQuadric(); + displayList = glGenLists(1); + glNewList(displayList, GL_COMPILE_AND_EXECUTE); + glPushMatrix(); + glRotatef(-90.0, 1.0, 0.0, 0.0); + glTranslatef(0.0, 0.0, -1.0); + gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL); + gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH); + gluCylinder(quadObj, bottomRadius, 0, height, 15, 10); + glPopMatrix(); + glEndList(); + } + glPopMatrix(); + } +}; + +class GLCylinder : public GLShape { + MT_Scalar radius; + MT_Scalar height; + mutable GLuint displayList; + +public: + GLCylinder(MT_Scalar r, MT_Scalar h) : + radius(r), + height(h), + displayList(0) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + if (displayList) glCallList(displayList); + else { + GLUquadricObj *quadObj = gluNewQuadric(); + displayList = glGenLists(1); + glNewList(displayList, GL_COMPILE_AND_EXECUTE); + glPushMatrix(); + glRotatef(-90.0, 1.0, 0.0, 0.0); + glTranslatef(0.0, 0.0, -1.0); + gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL); + gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH); + gluCylinder(quadObj, radius, radius, height, 15, 10); + glPopMatrix (); + glEndList(); + } + glPopMatrix(); + } +}; + +class Object; + +class Callback : public SM_Callback { +public: + Callback(Object& object) : m_object(object) {} + + virtual void do_me(); + +private: + Object& m_object; +}; + + +class Object { +public: + Object(GLShape *gl_shape, SM_Object& object) : + m_gl_shape(gl_shape), + m_object(object), + m_callback(*this) + { + m_object.registerCallback(m_callback); + } + + ~Object() {} + + void paint() { + if (m_gl_shape) { + m_gl_shape->paint(m); + // display_bbox(m_bbox.lower(), m_bbox.upper()); + } + } + + void print_reaction_force() const { + std::cout << m_object.getReactionForce() << std::endl; + } + + MT_Vector3 getAhead() { + return MT_Vector3(&m[4]); + } + + MT_Vector3 getUp() { + return MT_Vector3(&m[8]); + } + + void clearMomentum() { + m_object.clearMomentum(); + } + + void setMargin(MT_Scalar margin) { + m_object.setMargin(margin); + } + + void setScaling(const MT_Vector3& scaling) { + m_object.setScaling(scaling); + } + + const MT_Point3& getPosition() { + return m_object.getPosition(); + } + + void setPosition(const MT_Point3& pos) { + m_object.setPosition(pos); + } + + void setOrientation(const MT_Quaternion& orn) { + m_object.setOrientation(orn); + } + + void applyCenterForce(const MT_Vector3& force) { + m_object.applyCenterForce(force); + } + + void applyTorque(const MT_Vector3& torque) { + m_object.applyTorque(torque); + } + + MT_Point3 getWorldCoord(const MT_Point3& local) const { + return m_object.getWorldCoord(local); + } + + MT_Vector3 getLinearVelocity() const { + return m_object.getLinearVelocity(); + } + + MT_Vector3 getAngularVelocity() const { + return m_object.getAngularVelocity(); + } + + void setMatrix() { + m_object.calcXform(); + m_object.getMatrix(m); + } + + const double *getMatrix() { + m_object.calcXform(); + return m_object.getMatrix(); + } + +private: + GLShape *m_gl_shape; + SM_Object& m_object; + DT_Scalar m[16]; + Callback m_callback; +}; + + + +const MT_Scalar SPACE_SIZE = 2; + +static GLSphere gl_sphere(sphere_radius); +static GLBox gl_ground(50.0, 0.0, 50.0); + + + +#ifdef USE_COMPLEX + +const int GRID_SCALE = 10; +const MT_Scalar GRID_UNIT = 25.0 / GRID_SCALE; + +DT_ShapeHandle createComplex() { + DT_ShapeHandle shape = DT_NewComplexShape(); + for (int i0 = -GRID_SCALE; i0 != GRID_SCALE; ++i0) { + for (int j0 = -GRID_SCALE; j0 != GRID_SCALE; ++j0) { + int i1 = i0 + 1; + int j1 = j0 + 1; +#ifdef QUADS + DT_Begin(); + DT_Vertex(GRID_UNIT * i0, bowl_curv * i0*i0, GRID_UNIT * j0); + DT_Vertex(GRID_UNIT * i0, bowl_curv * i0*i0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, bowl_curv * i1*i1, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, bowl_curv * i1*i1, GRID_UNIT * j0); + DT_End(); +#else + DT_Begin(); + DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j0); + DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j1); + DT_End(); + + DT_Begin(); + DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j0); + DT_End(); +#endif + + } + } + DT_EndComplexShape(); + return shape; +} + + +static DT_ShapeHandle ground_shape = createComplex(); + +#else + +static DT_ShapeHandle ground_shape = DT_Box(50, 0, 50); + +#endif + +static SM_Object sm_ground(ground_shape, &g_materialProps, 0, 0); +static Object ground(&gl_ground, sm_ground); + +static SM_Object sm_sphere(DT_Sphere(0.0), &g_materialProps, &g_shapeProps, 0); +static Object object(&gl_sphere, sm_sphere); + + +static SM_Scene g_scene; + + +bool g_hit = false; +MT_Point3 g_spot; +MT_Vector3 g_normal; + + +void Callback::do_me() +{ + m_object.setMatrix(); + m_object.print_reaction_force(); +} + +void myinit(void) { + + GLfloat light_ambient[] = { 0.0, 0.0, 0.0, 1.0 }; + GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 }; + GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 }; + + /* light_position is NOT default value */ + GLfloat light_position0[] = { 1.0, 1.0, 1.0, 0.0 }; + GLfloat light_position1[] = { -1.0, -1.0, -1.0, 0.0 }; + + glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); + glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse); + glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular); + glLightfv(GL_LIGHT0, GL_POSITION, light_position0); + + glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient); + glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse); + glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular); + glLightfv(GL_LIGHT1, GL_POSITION, light_position1); + + + glEnable(GL_LIGHTING); + glEnable(GL_LIGHT0); + glEnable(GL_LIGHT1); + + glShadeModel(GL_SMOOTH); + + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LESS); + + // glEnable(GL_CULL_FACE); + // glCullFace(GL_BACK); + + ground.setPosition(MT_Point3(0, -10, 0)); + ground.setOrientation(MT_Quaternion(0, 0, 0, 1)); + ground.setMatrix(); + center.setValue(0.0, 0.0, 0.0); + sm_ground.setMargin(ground_margin); + + g_scene.setForceField(gravity); + g_scene.add(sm_ground); + + object.setMargin(sphere_radius); + + g_scene.add(sm_sphere); + + + newRandom(); +} + + +//MT_Point3 cp1, cp2; +//bool intersection; + +void display(void) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + ground.paint(); + object.paint(); + + if (g_hit) { + glDisable(GL_LIGHTING); + glColor3f(1, 0, 0); + + glPointSize(5); + glBegin(GL_POINTS); + glVertex3d(g_spot[0], g_spot[1], g_spot[2]); + glEnd(); + glPointSize(1); + + glColor3f(1, 1, 0); + glBegin(GL_LINES); + glVertex3d(g_spot[0], g_spot[1], g_spot[2]); + glVertex3d(g_spot[0] + g_normal[0], + g_spot[1] + g_normal[1], + g_spot[2] + g_normal[2]); + glEnd(); + glEnable(GL_LIGHTING); + } + + + +#ifdef COLLISION + glDisable(GL_DEPTH_TEST); + glDisable(GL_LIGHTING); + glColor3f(1, 1, 0); + if (intersection) { + glPointSize(5); + glBegin(GL_POINTS); + glVertex3d(cp1[0], cp1[1], cp1[2]); + glEnd(); + glPointSize(1); + } + else { + glBegin(GL_LINES); + glVertex3d(cp1[0], cp1[1], cp1[2]); + glVertex3d(cp2[0], cp2[1], cp2[2]); + glEnd(); + } + glEnable(GL_LIGHTING); + glEnable(GL_DEPTH_TEST); +#endif + + glFlush(); + glutSwapBuffers(); +} + + + + + +void newRandom() { + object.setPosition(MT_Point3(0, 0, 0)); + object.setOrientation(MT_Quaternion::random()); + object.clearMomentum(); + object.setMatrix(); + + display(); +} + +void moveAndDisplay() { + g_scene.proceed(timeStep, 0.01); + + display(); + g_hit = false; +} + + +void turn_left() { + object.applyTorque(5.0 * object.getUp()); +} + +void turn_right() { + object.applyTorque(-5.0 * object.getUp()); +} + +void forward() { + object.applyCenterForce(10.0 * object.getAhead()); +} + +void backward() { + object.applyCenterForce(-10.0 * object.getAhead()); +} + +void jump() { + object.applyCenterForce(MT_Vector3(0.0, 200.0, 0.0)); +} + + +void toggleIdle() { + static bool idle = true; + if (idle) { + glutIdleFunc(moveAndDisplay); + idle = false; + } + else { + glutIdleFunc(NULL); + idle = true; + } +} + + +void setCamera() { + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glFrustum(-1.0, 1.0, -1.0, 1.0, 1.0, 200.0); + MT_Scalar rele = MT_radians(ele); + MT_Scalar razi = MT_radians(azi); + eye.setValue(DISTANCE * sin(razi) * cos(rele), + DISTANCE * sin(rele), + DISTANCE * cos(razi) * cos(rele)); + gluLookAt(eye[0], eye[1], eye[2], + center[0], center[1], center[2], + 0, 1, 0); + glMatrixMode(GL_MODELVIEW); + display(); +} + +const MT_Scalar STEPSIZE = 5; + +void stepLeft() { azi -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); } +void stepRight() { azi += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); } +void stepFront() { ele += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); } +void stepBack() { ele -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); } +void zoomIn() { DISTANCE -= 1; setCamera(); } +void zoomOut() { DISTANCE += 1; setCamera(); } + + +void myReshape(int w, int h) { + glViewport(0, 0, w, h); + setCamera(); +} + +void myKeyboard(unsigned char key, int x, int y) +{ + switch (key) + { + case 'w': forward(); break; + case 's': backward(); break; + case 'a': turn_left(); break; + case 'd': turn_right(); break; + case 'e': jump(); break; + case 'l' : stepLeft(); break; + case 'r' : stepRight(); break; + case 'f' : stepFront(); break; + case 'b' : stepBack(); break; + case 'z' : zoomIn(); break; + case 'x' : zoomOut(); break; + case 'i' : toggleIdle(); break; + case ' ' : newRandom(); break; + default: +// std::cout << "unused key : " << key << std::endl; + break; + } +} + +void mySpecial(int key, int x, int y) +{ + switch (key) + { + case GLUT_KEY_LEFT : stepLeft(); break; + case GLUT_KEY_RIGHT : stepRight(); break; + case GLUT_KEY_UP : stepFront(); break; + case GLUT_KEY_DOWN : stepBack(); break; + case GLUT_KEY_PAGE_UP : zoomIn(); break; + case GLUT_KEY_PAGE_DOWN : zoomOut(); break; + case GLUT_KEY_HOME : toggleIdle(); break; + default: +// std::cout << "unused (special) key : " << key << std::endl; + break; + } +} + +void goodbye( void) +{ + g_scene.remove(sm_ground); + g_scene.remove(sm_sphere); + + std::cout << "goodbye ..." << std::endl; + exit(0); +} + +void menu(int choice) +{ + + static int fullScreen = 0; + static int px, py, sx, sy; + + switch(choice) { + case 1: + if (fullScreen == 1) { + glutPositionWindow(px,py); + glutReshapeWindow(sx,sy); + glutChangeToMenuEntry(1,"Full Screen",1); + fullScreen = 0; + } else { + px=glutGet((GLenum)GLUT_WINDOW_X); + py=glutGet((GLenum)GLUT_WINDOW_Y); + sx=glutGet((GLenum)GLUT_WINDOW_WIDTH); + sy=glutGet((GLenum)GLUT_WINDOW_HEIGHT); + glutFullScreen(); + glutChangeToMenuEntry(1,"Close Full Screen",1); + fullScreen = 1; + } + break; + case 2: + toggleIdle(); + break; + case 3: + goodbye(); + break; + default: + break; + } +} + +void createMenu() +{ + glutCreateMenu(menu); + glutAddMenuEntry("Full Screen", 1); + glutAddMenuEntry("Toggle Idle (Start/Stop)", 2); + glutAddMenuEntry("Quit", 3); + glutAttachMenu(GLUT_RIGHT_BUTTON); +} + +int main(int argc, char **argv) { + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); + glutInitWindowPosition(0, 0); + glutInitWindowSize(500, 500); + glutCreateWindow("Physics demo"); + + myinit(); + glutKeyboardFunc(myKeyboard); + glutSpecialFunc(mySpecial); + glutReshapeFunc(myReshape); + createMenu(); + glutIdleFunc(NULL); + + glutDisplayFunc(display); + glutMainLoop(); + return 0; +} + + + + + + + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/sample/particle0.cpp b/source/gameengine/Physics/Sumo/Fuzzics/sample/particle0.cpp new file mode 100644 index 00000000000..1c03621f530 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/sample/particle0.cpp @@ -0,0 +1,691 @@ +//#define FAKE_IT +#define USE_COMPLEX +#define QUADS + +#include <algorithm> +#include <new> +#include <GL/glut.h> + +#include "MT_MinMax.h" +#include "MT_Point3.h" +#include "MT_Vector3.h" +#include "MT_Quaternion.h" +#include "MT_Matrix3x3.h" +#include "MT_Transform.h" + +#include "SM_Object.h" +#include "SM_Scene.h" + +#include "solid.h" + +const MT_Scalar bowl_curv = 0.10; +const MT_Scalar timeStep = 0.04; +const MT_Scalar ground_margin = 0.0; +const MT_Scalar sphere_radius = 0.5; + +const MT_Vector3 gravity(0, -9.8, 0); + +static MT_Scalar DISTANCE = 5; + +static MT_Scalar ele = 0, azi = 0; +static MT_Point3 eye(0, 0, DISTANCE); +static MT_Point3 center(0, 0, 0); + +inline double irnd() { return 2 * MT_random() - 1; } + +static const double SCALE_BOTTOM = 0.5; +static const double SCALE_FACTOR = 2.0; + +SM_ShapeProps g_shapeProps = { + 1.0, // mass + 1.0, // inertia + 0.9, // linear drag + 0.9 // angular drag +}; + +SM_MaterialProps g_materialProps = { + 0.7, // restitution + 0.0, // friction + 0.0, // spring constant + 0.0 // damping +}; + + +void toggleIdle(); + + +void newRandom(); + +void coordSystem() { + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glColor3f(1, 0, 0); + glVertex3d(0, 0, 0); + glVertex3d(10, 0, 0); + glColor3f(0, 1, 0); + glVertex3d(0, 0, 0); + glVertex3d(0, 10, 0); + glColor3f(0, 0, 1); + glVertex3d(0, 0, 0); + glVertex3d(0, 0, 10); + glEnd(); + glEnable(GL_LIGHTING); +} + + +void display_bbox(const MT_Point3& min, const MT_Point3& max) { + glDisable(GL_DEPTH_TEST); + glDisable(GL_LIGHTING); + glColor3f(0, 1, 1); + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + glBegin(GL_QUAD_STRIP); + glVertex3d(min[0], min[1], min[2]); + glVertex3d(min[0], min[1], max[2]); + glVertex3d(max[0], min[1], min[2]); + glVertex3d(max[0], min[1], max[2]); + glVertex3d(max[0], max[1], min[2]); + glVertex3d(max[0], max[1], max[2]); + glVertex3d(min[0], max[1], min[2]); + glVertex3d(min[0], max[1], max[2]); + glVertex3d(min[0], min[1], min[2]); + glVertex3d(min[0], min[1], max[2]); + glEnd(); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + glEnable(GL_LIGHTING); + glEnable(GL_DEPTH_TEST); +} + + + + +class GLShape { +public: + virtual void paint(GLdouble *m) const = 0; +}; + + +class GLSphere : public GLShape { + MT_Scalar radius; +public: + GLSphere(MT_Scalar r) : radius(r) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + glutSolidSphere(radius, 20, 20); + glPopMatrix(); + } +}; + + +class GLBox : public GLShape { + MT_Vector3 extent; +public: + GLBox(MT_Scalar x, MT_Scalar y, MT_Scalar z) : + extent(x, y, z) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + glPushMatrix(); + glScaled(extent[0], extent[1], extent[2]); + glutSolidCube(1.0); + glPopMatrix(); + glPopMatrix(); + } +}; + + +class GLCone : public GLShape { + MT_Scalar bottomRadius; + MT_Scalar height; + mutable GLuint displayList; + +public: + GLCone(MT_Scalar r, MT_Scalar h) : + bottomRadius(r), + height(h), + displayList(0) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + if (displayList) glCallList(displayList); + else { + GLUquadricObj *quadObj = gluNewQuadric(); + displayList = glGenLists(1); + glNewList(displayList, GL_COMPILE_AND_EXECUTE); + glPushMatrix(); + glRotatef(-90.0, 1.0, 0.0, 0.0); + glTranslatef(0.0, 0.0, -1.0); + gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL); + gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH); + gluCylinder(quadObj, bottomRadius, 0, height, 15, 10); + glPopMatrix(); + glEndList(); + } + glPopMatrix(); + } +}; + +class GLCylinder : public GLShape { + MT_Scalar radius; + MT_Scalar height; + mutable GLuint displayList; + +public: + GLCylinder(MT_Scalar r, MT_Scalar h) : + radius(r), + height(h), + displayList(0) {} + + void paint(GLdouble *m) const { + glPushMatrix(); + glLoadMatrixd(m); + coordSystem(); + if (displayList) glCallList(displayList); + else { + GLUquadricObj *quadObj = gluNewQuadric(); + displayList = glGenLists(1); + glNewList(displayList, GL_COMPILE_AND_EXECUTE); + glPushMatrix(); + glRotatef(-90.0, 1.0, 0.0, 0.0); + glTranslatef(0.0, 0.0, -1.0); + gluQuadricDrawStyle(quadObj, (GLenum)GLU_FILL); + gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH); + gluCylinder(quadObj, radius, radius, height, 15, 10); + glPopMatrix (); + glEndList(); + } + glPopMatrix(); + } +}; + +class Object; + +class Callback : public SM_Callback { +public: + Callback(Object& object) : m_object(object) {} + + virtual void do_me(); + +private: + Object& m_object; +}; + + +class Object { +public: + Object(GLShape *gl_shape, SM_Object& object) : + m_gl_shape(gl_shape), + m_object(object), + m_callback(*this) + { + m_object.registerCallback(m_callback); + } + + ~Object() {} + + void paint() { + m_gl_shape->paint(m); + // display_bbox(m_bbox.lower(), m_bbox.upper()); + } + + MT_Vector3 getAhead() { + return MT_Vector3(-m[8], -m[9], -m[10]); + } + + void clearMomentum() { + m_object.clearMomentum(); + } + + void setMargin(MT_Scalar margin) { + m_object.setMargin(margin); + } + + void setScaling(const MT_Vector3& scaling) { + m_object.setScaling(scaling); + } + + void setPosition(const MT_Point3& pos) { + m_object.setPosition(pos); + } + + void setOrientation(const MT_Quaternion& orn) { + m_object.setOrientation(orn); + } + + void applyCenterForce(const MT_Vector3& force) { + m_object.applyCenterForce(force); + } + + void applyTorque(const MT_Vector3& torque) { + m_object.applyTorque(torque); + } + + MT_Point3 getWorldCoord(const MT_Point3& local) const { + return m_object.getWorldCoord(local); + } + + MT_Vector3 getLinearVelocity() const { + return m_object.getLinearVelocity(); + } + + void setMatrix() { + m_object.getMatrix(m); + } + +private: + GLShape *m_gl_shape; + SM_Object& m_object; + DT_Scalar m[16]; + Callback m_callback; +}; + + +void Callback::do_me() +{ + m_object.setMatrix(); +} + + +const MT_Scalar SPACE_SIZE = 2; + +static GLSphere gl_sphere(sphere_radius); +static GLBox gl_ground(50.0, 0.0, 50.0); + + + +#ifdef USE_COMPLEX + +const int GRID_SCALE = 10; +const MT_Scalar GRID_UNIT = 25.0 / GRID_SCALE; + +DT_ShapeHandle createComplex() { + DT_ShapeHandle shape = DT_NewComplexShape(); + for (int i0 = -GRID_SCALE; i0 != GRID_SCALE; ++i0) { + for (int j0 = -GRID_SCALE; j0 != GRID_SCALE; ++j0) { + int i1 = i0 + 1; + int j1 = j0 + 1; +#ifdef QUADS + DT_Begin(); + DT_Vertex(GRID_UNIT * i0, bowl_curv * i0*i0, GRID_UNIT * j0); + DT_Vertex(GRID_UNIT * i0, bowl_curv * i0*i0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, bowl_curv * i1*i1, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, bowl_curv * i1*i1, GRID_UNIT * j0); + DT_End(); +#else + DT_Begin(); + DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j0); + DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j1); + DT_End(); + + DT_Begin(); + DT_Vertex(GRID_UNIT * i0, 0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j1); + DT_Vertex(GRID_UNIT * i1, 0, GRID_UNIT * j0); + DT_End(); +#endif + + } + } + DT_EndComplexShape(); + return shape; +} + + +static DT_ShapeHandle ground_shape = createComplex(); + +#else + +static DT_ShapeHandle ground_shape = DT_Box(50, 0, 50); + +#endif + +static SM_Object sm_ground(ground_shape, &g_materialProps, 0, 0); +static Object ground(&gl_ground, sm_ground); + +static SM_Object sm_sphere(DT_Sphere(0.0), &g_materialProps, &g_shapeProps, 0); +static Object object(&gl_sphere, sm_sphere); + + +static SM_Object sm_ray(DT_Ray(0.0, -1.0, 0.0), 0, 0, 0); + +static SM_Scene g_scene; + + +void myinit(void) { + + GLfloat light_ambient[] = { 0.0, 0.0, 0.0, 1.0 }; + GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 }; + GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 }; + + /* light_position is NOT default value */ + GLfloat light_position0[] = { 1.0, 1.0, 1.0, 0.0 }; + GLfloat light_position1[] = { -1.0, -1.0, -1.0, 0.0 }; + + glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); + glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse); + glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular); + glLightfv(GL_LIGHT0, GL_POSITION, light_position0); + + glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient); + glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse); + glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular); + glLightfv(GL_LIGHT1, GL_POSITION, light_position1); + + + glEnable(GL_LIGHTING); + glEnable(GL_LIGHT0); + glEnable(GL_LIGHT1); + + glShadeModel(GL_SMOOTH); + + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LESS); + + // glEnable(GL_CULL_FACE); + // glCullFace(GL_BACK); + + g_scene.setForceField(gravity); + g_scene.add(sm_ground); + sm_ground.setMargin(ground_margin); + + new(&object) Object(&gl_sphere, sm_sphere); + + + object.setMargin(sphere_radius); + + g_scene.add(sm_sphere); + + ground.setPosition(MT_Point3(0, -10, 0)); + ground.setOrientation(MT_Quaternion(0, 0, 0, 1)); + ground.setMatrix(); + center.setValue(0.0, 0.0, 0.0); + + newRandom(); +} + + +//MT_Point3 cp1, cp2; +//bool intersection; + +bool g_hit = false; +MT_Point3 g_spot; +MT_Vector3 g_normal; + + +void display(void) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + ground.paint(); + object.paint(); + + if (g_hit) { + glPointSize(5); + glBegin(GL_POINTS); + glVertex3d(g_spot[0], g_spot[1], g_spot[2]); + glEnd(); + glPointSize(1); + } + + + +#ifdef COLLISION + glDisable(GL_DEPTH_TEST); + glDisable(GL_LIGHTING); + glColor3f(1, 1, 0); + if (intersection) { + glPointSize(5); + glBegin(GL_POINTS); + glVertex3d(cp1[0], cp1[1], cp1[2]); + glEnd(); + glPointSize(1); + } + else { + glBegin(GL_LINES); + glVertex3d(cp1[0], cp1[1], cp1[2]); + glVertex3d(cp2[0], cp2[1], cp2[2]); + glEnd(); + } + glEnable(GL_LIGHTING); + glEnable(GL_DEPTH_TEST); +#endif + + glFlush(); + glutSwapBuffers(); +} + + + + + +void newRandom() { + object.setPosition(MT_Point3(0, 0, 0)); + object.clearMomentum(); + object.setMatrix(); + + display(); +} + +void moveAndDisplay() { + g_scene.proceed(timeStep, 0.01); + + MT_Vector3 normal(0, 1, 0); + + MT_Point3 from = object.getWorldCoord(MT_Point3(0, 0, 0)); + MT_Point3 to = from - normal * 10.0; + + g_hit = DT_ObjectRayTest(sm_ground.getObjectHandle(), + from.getValue(), + to.getValue(), g_spot.getValue(), + g_normal.getValue()); + + // Scrap +#define DO_FH +#ifdef DO_FH + MT_Scalar dist = MT_distance(from, g_spot); + if (dist < 5.0) { + MT_Vector3 lin_vel = object.getLinearVelocity(); + MT_Scalar lin_vel_normal = lin_vel.dot(normal); + + MT_Scalar spring_extent = dist + lin_vel_normal * (timeStep * 0.5); + + MT_Scalar f_spring = (5.0 - spring_extent) * 3.0; + object.applyCenterForce(normal * f_spring); + object.applyCenterForce(-lin_vel_normal * normal); + } + +#endif + + + display(); +} + + +void turn_left() { + object.applyTorque(MT_Vector3(0.0, 10.0, 0.0)); +} + +void turn_right() { + object.applyTorque(MT_Vector3(0.0, -10.0, 0.0)); +} + +void forward() { + object.applyCenterForce(20.0 * object.getAhead()); +} + +void backward() { + object.applyCenterForce(-20.0 * object.getAhead()); +} + +void jump() { + object.applyCenterForce(MT_Vector3(0.0, 200.0, 0.0)); +} + + +void toggleIdle() { + static bool idle = true; + if (idle) { + glutIdleFunc(moveAndDisplay); + idle = false; + } + else { + glutIdleFunc(NULL); + idle = true; + } +} + + +void setCamera() { + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glFrustum(-1.0, 1.0, -1.0, 1.0, 1.0, 200.0); + MT_Scalar rele = MT_radians(ele); + MT_Scalar razi = MT_radians(azi); + eye.setValue(DISTANCE * sin(razi) * cos(rele), + DISTANCE * sin(rele), + DISTANCE * cos(razi) * cos(rele)); + gluLookAt(eye[0], eye[1], eye[2], + center[0], center[1], center[2], + 0, 1, 0); + glMatrixMode(GL_MODELVIEW); + display(); +} + +const MT_Scalar STEPSIZE = 5; + +void stepLeft() { azi -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); } +void stepRight() { azi += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); } +void stepFront() { ele += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); } +void stepBack() { ele -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); } +void zoomIn() { DISTANCE -= 1; setCamera(); } +void zoomOut() { DISTANCE += 1; setCamera(); } + + +void myReshape(int w, int h) { + glViewport(0, 0, w, h); + setCamera(); +} + +void myKeyboard(unsigned char key, int x, int y) +{ + switch (key) + { + case 'w': forward(); break; + case 's': backward(); break; + case 'a': turn_left(); break; + case 'd': turn_right(); break; + case 'e': jump(); break; + case 'l' : stepLeft(); break; + case 'r' : stepRight(); break; + case 'f' : stepFront(); break; + case 'b' : stepBack(); break; + case 'z' : zoomIn(); break; + case 'x' : zoomOut(); break; + case 'i' : toggleIdle(); break; + case ' ' : newRandom(); break; + default: +// std::cout << "unused key : " << key << std::endl; + break; + } +} + +void mySpecial(int key, int x, int y) +{ + switch (key) + { + case GLUT_KEY_LEFT : stepLeft(); break; + case GLUT_KEY_RIGHT : stepRight(); break; + case GLUT_KEY_UP : stepFront(); break; + case GLUT_KEY_DOWN : stepBack(); break; + case GLUT_KEY_PAGE_UP : zoomIn(); break; + case GLUT_KEY_PAGE_DOWN : zoomOut(); break; + case GLUT_KEY_HOME : toggleIdle(); break; + default: +// std::cout << "unused (special) key : " << key << std::endl; + break; + } +} + +void goodbye( void) +{ + g_scene.remove(sm_ground); + g_scene.remove(sm_sphere); + + std::cout << "goodbye ..." << std::endl; + exit(0); +} + +void menu(int choice) +{ + + static int fullScreen = 0; + static int px, py, sx, sy; + + switch(choice) { + case 1: + if (fullScreen == 1) { + glutPositionWindow(px,py); + glutReshapeWindow(sx,sy); + glutChangeToMenuEntry(1,"Full Screen",1); + fullScreen = 0; + } else { + px=glutGet((GLenum)GLUT_WINDOW_X); + py=glutGet((GLenum)GLUT_WINDOW_Y); + sx=glutGet((GLenum)GLUT_WINDOW_WIDTH); + sy=glutGet((GLenum)GLUT_WINDOW_HEIGHT); + glutFullScreen(); + glutChangeToMenuEntry(1,"Close Full Screen",1); + fullScreen = 1; + } + break; + case 2: + toggleIdle(); + break; + case 3: + goodbye(); + break; + default: + break; + } +} + +void createMenu() +{ + glutCreateMenu(menu); + glutAddMenuEntry("Full Screen", 1); + glutAddMenuEntry("Toggle Idle (Start/Stop)", 2); + glutAddMenuEntry("Quit", 3); + glutAttachMenu(GLUT_RIGHT_BUTTON); +} + +int main(int argc, char **argv) { + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); + glutInitWindowPosition(0, 0); + glutInitWindowSize(500, 500); + glutCreateWindow("Physics demo"); + + myinit(); + glutKeyboardFunc(myKeyboard); + glutSpecialFunc(mySpecial); + glutReshapeFunc(myReshape); + createMenu(); + glutIdleFunc(NULL); + + glutDisplayFunc(display); + glutMainLoop(); + return 0; +} + + + + + + + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/Makefile b/source/gameengine/Physics/Sumo/Fuzzics/src/Makefile new file mode 100644 index 00000000000..5e3b35f6cf4 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/Makefile @@ -0,0 +1,13 @@ +# +# $Id$ +# Copyright (C) 2001 NaN Technologies B.V. + +LIBNAME = fuzzics +DIR = $(OCGDIR)/sumo + +include nan_compile.mk + +CCFLAGS += $(LEVEL_1_CPP_WARNINGS) + +CPPFLAGS += -I../include -I$(NAN_MOTO)/include -I../../include + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp new file mode 100644 index 00000000000..f8be49aa126 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp @@ -0,0 +1,115 @@ +#include "SM_FhObject.h" +#include "MT_MinMax.h" + +void SM_FhObject::ray_hit(void *client_data, + void *client_object1, + void *client_object2, + const DT_CollData *coll_data) { + + 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; + } + + SM_FhObject *fh_object = (SM_FhObject *)client_object2; + SM_Object *cl_object = fh_object->getClientObject(); + + if (hit_object == cl_object) { + // Shot myself in the foot... + return; + } + + const SM_ShapeProps *shapeProps = cl_object->getShapeProps(); + + // Exit if the client object is not dynamic. + if (shapeProps == 0) { + return; + } + + MT_Point3 lspot; + MT_Vector3 normal; + + if (DT_ObjectRayTest(hit_object->getObjectHandle(), + fh_object->getPosition().getValue(), + fh_object->getSpot().getValue(), + lspot.getValue(), normal.getValue())) { + + const MT_Vector3& ray_dir = fh_object->getRayDirection(); + MT_Scalar dist = MT_distance(fh_object->getPosition(), + hit_object->getWorldCoord(lspot)) - + cl_object->getMargin(); + + normal.normalize(); + + if (dist < matProps->m_fh_distance) { + + if (shapeProps->m_do_fh) { + 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; + + MT_Scalar i_spring = spring_extent * matProps->m_fh_spring; + MT_Scalar i_damp = rel_vel_ray * matProps->m_fh_damping; + + cl_object->addLinearVelocity(-(i_spring + i_damp) * ray_dir); + if (matProps->m_fh_normal) { + cl_object->addLinearVelocity( + (i_spring + i_damp) * + (normal - normal.dot(ray_dir) * ray_dir)); + } + + MT_Vector3 lateral = rel_vel - rel_vel_ray * ray_dir; + const SM_ShapeProps *shapeProps = cl_object->getShapeProps(); + + if (shapeProps->m_do_anisotropic) { + MT_Matrix3x3 lcs(cl_object->getOrientation()); + MT_Vector3 loc_lateral = lateral * lcs; + const MT_Vector3& friction_scaling = + shapeProps->m_friction_scaling; + + loc_lateral.scale(friction_scaling[0], + friction_scaling[1], + friction_scaling[2]); + lateral = lcs * loc_lateral; + } + + + MT_Scalar rel_vel_lateral = lateral.length(); + + 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 rel_mom_lateral = rel_vel_lateral / + cl_object->getInvMass(); + + MT_Vector3 friction = + (rel_mom_lateral > max_friction) ? + -lateral * (max_friction / rel_vel_lateral) : + -lateral; + + cl_object->applyCenterImpulse(friction); + } + } + + if (shapeProps->m_do_rot_fh) { + const double *ogl_mat = cl_object->getMatrix(); + MT_Vector3 up(&ogl_mat[8]); + MT_Vector3 t_spring = up.cross(normal) * matProps->m_fh_spring; + MT_Vector3 ang_vel = cl_object->getAngularVelocity(); + + // only rotations that tilt relative to the normal are damped + ang_vel -= ang_vel.dot(normal) * normal; + + MT_Vector3 t_damp = ang_vel * matProps->m_fh_damping; + + cl_object->addAngularVelocity(t_spring - t_damp); + } + } + } +} + + + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp new file mode 100644 index 00000000000..655590c2086 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp @@ -0,0 +1,953 @@ +/** + * $Id$ + * Copyright (C) 2001 NaN Technologies B.V. + * The basic physics object. + */ + +#ifdef WIN32 +// This warning tells us about truncation of __long__ stl-generated names. +// It can occasionally cause DevStudio to have internal compiler warnings. +#pragma warning( disable : 4786 ) +#endif + +#include "SM_Object.h" +#include "SM_Scene.h" +#include "SM_FhObject.h" + +#include "MT_MinMax.h" + + +// Tweak parameters +static MT_Scalar ImpulseThreshold = 0.5; + +SM_Object::SM_Object( + DT_ShapeHandle shape, + const SM_MaterialProps *materialProps, + const SM_ShapeProps *shapeProps, + SM_Object *dynamicParent +) : + 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), + 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), + m_torque(0.0, 0.0, 0.0), + m_client_object(0), + m_fh_object(0), + + m_combined_lin_vel (0.0, 0.0, 0.0), + m_combined_ang_vel (0.0, 0.0, 0.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); + } + m_suspended = false; +} + + + void +SM_Object:: +integrateForces( + MT_Scalar timeStep +){ + if (!m_suspended) { + m_prev_state = *this; + m_prev_state.setLinearVelocity(actualLinVelocity()); + m_prev_state.setAngularVelocity(actualAngVelocity()); + if (isDynamic()) { + // Integrate momentum (forward Euler) + m_lin_mom += m_force * timeStep; + m_ang_mom += m_torque * timeStep; + // Drain momentum because of air/water resistance + 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_shapeProps->m_mass; + m_ang_vel = m_ang_mom / m_shapeProps->m_inertia; + } + } + +}; + + void +SM_Object:: +integrateMomentum( + MT_Scalar timeStep +){ + // Integrate position and orientation + + // only do it for objects with linear and/or angular velocity + // else clients with hierarchies may get into trouble + if (!actualLinVelocity().fuzzyZero() || !actualAngVelocity().fuzzyZero()) + { + + // those MIDPOINT and BACKWARD integration methods are + // in this form not ok with some testfiles ! + // For a release build please use forward euler unless completely tested +[ +//#define MIDPOINT +//#define BACKWARD +#ifdef MIDPOINT +// Midpoint rule + m_pos += (m_prev_state.getLinearVelocity() + actualLinVelocity()) * (timeStep * 0.5); + m_orn += (m_prev_state.getAngularVelocity() * m_prev_state.getOrientation() + actualAngVelocity() * m_orn) * (timeStep * 0.25); +#elif defined BACKWARD +// Backward Euler + m_pos += actualLinVelocity() * timeStep; + m_orn += actualAngVelocity() * m_orn * (timeStep * 0.5); +#else +// Forward Euler + + m_pos += m_prev_state.getLinearVelocity() * timeStep; + m_orn += m_prev_state.getAngularVelocity() * m_orn * (timeStep * 0.5); +#endif + m_orn.normalize(); // I might not be necessary to do this every call + + calcXform(); + notifyClient(); + + } +} + + +void SM_Object::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 + + + // If one of the objects is a ghost then ignore it for the dynamics + if (obj1->isGhost() || obj2->isGhost()) { + return; + } + + + if (!obj2->isDynamic()) { + std::swap(obj1, obj2); + } + + if (!obj2->isDynamic()) { + return; + } + + // obj1 points to a dynamic object + + + // 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); + } + } + } + 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(); + } + } + } +} + + + +SM_Object::SM_Object( +) { + // warning no initialization of variables done by moto. +} + +SM_Object:: +~SM_Object() { + delete m_fh_object; + DT_DeleteObject(m_object); +} + + bool +SM_Object:: +isDynamic( +) const { + return m_shapeProps != 0; +} + +/* nzc experimental. There seem to be two places where kinematics + * are evaluated: proceedKinematic (called from SM_Scene) and + * proceed() in this object. I'll just try and bunge these out for + * now. */ + void +SM_Object:: +suspend( +){ + if (!m_suspended) { + m_suspended = true; + suspendDynamics(); + } +} + + void +SM_Object:: +resume( +) { + if (m_suspended) { + m_suspended = false; + restoreDynamics(); + } +} + + void +SM_Object:: +suspendDynamics( +) { + if (m_shapeProps) { + m_shapePropsBackup = m_shapeProps; + m_shapeProps = 0; + } +} + + void +SM_Object:: +restoreDynamics( +) { + if (m_shapePropsBackup) { + m_shapeProps = m_shapePropsBackup; + m_shapePropsBackup = 0; + } +} + + bool +SM_Object:: +isGhost( +) const { + return m_materialProps == 0; +} + + void +SM_Object:: +suspendMaterial( +) { + if (m_materialProps) { + m_materialPropsBackup = m_materialProps; + m_materialProps = 0; + } +} + + void +SM_Object:: +restoreMaterial( +) { + if (m_materialPropsBackup) { + m_materialProps = m_materialPropsBackup; + m_materialPropsBackup = 0; + } +} + + SM_FhObject * +SM_Object:: +getFhObject( +) const { + return m_fh_object; +} + + void +SM_Object:: +registerCallback( + SM_Callback& callback +) { + m_callbackList.push_back(&callback); +} + +// Set the local coordinate system according to the current state + void +SM_Object:: +calcXform() { + m_xform.setOrigin(m_pos); + m_xform.setBasis(MT_Matrix3x3(m_orn, m_scaling)); + m_xform.getValue(m_ogl_matrix); + DT_SetMatrixd(m_object, m_ogl_matrix); + if (m_fh_object) { + m_fh_object->setPosition(m_pos); + m_fh_object->calcXform(); + } +} + +// Call callbacks to notify the client of a change of placement + void +SM_Object:: +notifyClient() { + T_CallbackList::iterator i; + for (i = m_callbackList.begin(); i != m_callbackList.end(); ++i) { + (*i)->do_me(); + } +} + + +// Save the current state information for use in the velocity computation in the next frame. + void +SM_Object:: +proceedKinematic( + MT_Scalar timeStep +) { + /* nzc: need to bunge this for the logic bubbling as well? */ + if (!m_suspended) { + m_prev_kinematic = m_kinematic; + if (m_kinematic) { + m_prev_xform = m_xform; + m_timeStep = timeStep; + calcXform(); + m_kinematic = false; + } + } +} + + void +SM_Object:: +saveReactionForce( + MT_Scalar timeStep +) { + if (isDynamic()) { + m_reaction_force = m_reaction_impulse / timeStep; + m_reaction_impulse.setValue(0.0, 0.0, 0.0); + } +} + + void +SM_Object:: +clearForce( +) { + m_force.setValue(0.0, 0.0, 0.0); + m_torque.setValue(0.0, 0.0, 0.0); +} + + void +SM_Object:: +clearMomentum( +) { + m_lin_mom.setValue(0.0, 0.0, 0.0); + m_ang_mom.setValue(0.0, 0.0, 0.0); +} + + void +SM_Object:: +setMargin( + MT_Scalar margin +) { + m_margin = margin; + DT_SetMargin(m_object, margin); +} + + MT_Scalar +SM_Object:: +getMargin( +) const { + return m_margin; +} + +const + SM_MaterialProps * +SM_Object:: +getMaterialProps( +) const { + return m_materialProps; +} + +const + SM_ShapeProps * +SM_Object:: +getShapeProps( +) const { + return m_shapeProps; +} + + void +SM_Object:: +setPosition( + const MT_Point3& pos +){ + m_kinematic = true; + m_pos = pos; +} + + void +SM_Object:: +setOrientation( + const MT_Quaternion& orn +){ + assert(!orn.fuzzyZero()); + m_kinematic = true; + m_orn = orn; +} + + void +SM_Object:: +setScaling( + const MT_Vector3& scaling +){ + m_kinematic = true; + m_scaling = scaling; +} + +/** + * Functions to handle linear velocity + */ + + void +SM_Object:: +setExternalLinearVelocity( + const MT_Vector3& lin_vel +) { + m_combined_lin_vel=lin_vel; +} + + void +SM_Object:: +addExternalLinearVelocity( + const MT_Vector3& lin_vel +) { + m_combined_lin_vel+=lin_vel; +} + + void +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; + } +} + + void +SM_Object:: +setLinearVelocity( + const MT_Vector3& lin_vel +){ + m_lin_vel = lin_vel; + if (m_shapeProps) { + m_lin_mom = m_lin_vel * m_shapeProps->m_mass; + } +} + +/** + * Functions to handle angular velocity + */ + + void +SM_Object:: +setExternalAngularVelocity( + const MT_Vector3& ang_vel +) { + m_combined_ang_vel = ang_vel; +} + + void +SM_Object:: +addExternalAngularVelocity( + const MT_Vector3& ang_vel +) { + m_combined_ang_vel += ang_vel; +} + + void +SM_Object:: +setAngularVelocity( + const MT_Vector3& ang_vel +) { + m_ang_vel = ang_vel; + if (m_shapeProps) { + m_ang_mom = m_ang_vel * m_shapeProps->m_inertia; + } +} + + void +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; + } +} + + + void +SM_Object:: +clearCombinedVelocities( +) { + m_combined_lin_vel = MT_Vector3(0,0,0); + m_combined_ang_vel = MT_Vector3(0,0,0); +} + + void +SM_Object:: +resolveCombinedVelocities( + const MT_Vector3 & lin_vel, + const MT_Vector3 & ang_vel +) { + + // Different behaviours for dynamic and non-dynamic + // objects. For non-dynamic we just set the velocity to + // zero. For dynmic the physics velocity has to be + // taken into account. We must make an arbitrary decision + // on how to resolve the 2 velocities. Choices are + // Add the physics velocity to the linear velocity. Objects + // will just keep on moving in the direction they were + // last set in - untill external forces affect them. + // Set the combinbed linear and physics velocity to zero. + // Set the physics velocity in the direction of the set velocity + // zero. + if (isDynamic()) { + +#if 1 + 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(); + + m_lin_vel -= (m_lin_vel.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; + clearCombinedVelocities(); + + } + +} + + + MT_Scalar +SM_Object:: +getInvMass( +) const { + return m_shapeProps ? 1.0 / m_shapeProps->m_mass : 0.0; + // OPT: cache the result of this division rather than compute it each call +} + + MT_Scalar +SM_Object:: +getInvInertia( +) const { + return m_shapeProps ? 1.0 / m_shapeProps->m_inertia : 0.0; + // OPT: cache the result of this division rather than compute it each call +} + + void +SM_Object:: +applyForceField( + const MT_Vector3& accel +) { + if (m_shapeProps) { + m_force += m_shapeProps->m_mass * accel; // F = m * a + } +} + + void +SM_Object:: +applyCenterForce( + const MT_Vector3& force +) { + m_force += force; +} + + void +SM_Object:: +applyTorque( + const MT_Vector3& torque +) { + m_torque += torque; +} + + void +SM_Object:: +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 +} + + void +SM_Object:: +applyCenterImpulse( + const MT_Vector3& impulse +) { + if (m_shapeProps) { + m_lin_mom += impulse; + m_reaction_impulse += impulse; + m_lin_vel = m_lin_mom / m_shapeProps->m_mass; + + // The linear velocity is immedialtely updated since otherwise + // simultaneous collisions will get a double impulse. + } +} + + void +SM_Object:: +applyAngularImpulse( + const MT_Vector3& impulse +) { + if (m_shapeProps) { + m_ang_mom += impulse; + m_ang_vel = m_ang_mom / m_shapeProps->m_inertia; + } +} + + MT_Point3 +SM_Object:: +getWorldCoord( + const MT_Point3& local +) const { + return m_xform(local); +} + + MT_Vector3 +SM_Object:: +getVelocity( + const MT_Point3& local +) const { + // 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!!) + + 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); + + // NB: m_xform.getBasis() * local == m_xform(local) - m_xform.getOrigin() + +} + + +const + MT_Vector3& +SM_Object:: +getReactionForce( +) const { + return m_reaction_force; +} + + void +SM_Object:: +getMatrix( + double *m +) const { + std::copy(&m_ogl_matrix[0], &m_ogl_matrix[16], &m[0]); +} + +const + double * +SM_Object:: +getMatrix( +) const { + return m_ogl_matrix; +} + +// Still need this??? +const + MT_Transform& +SM_Object:: +getScaledTransform( +) const { + return m_xform; +} + + DT_ObjectHandle +SM_Object:: +getObjectHandle( +) const { + return m_object; +} + + DT_ShapeHandle +SM_Object:: +getShapeHandle( +) const { + 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( +) { + return m_dynamicParent; +} + + void +SM_Object:: +setRigidBody( + bool is_rigid_body +) { + m_is_rigid_body = is_rigid_body; +} + + bool +SM_Object:: +isRigidBody( +) const { + return m_is_rigid_body; +} + +const + MT_Vector3 +SM_Object:: +actualLinVelocity( +) const { + return m_combined_lin_vel + m_lin_vel; +}; + +const + MT_Vector3 +SM_Object:: +actualAngVelocity( +) const { + return m_combined_ang_vel + m_ang_vel; +}; + + + + + diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp new file mode 100644 index 00000000000..e3eb7e5c5ea --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_Scene.cpp @@ -0,0 +1,179 @@ +/** + * $Id$ + * Copyright (C) 2001 NaN Technologies B.V. + * The physics scene. + */ + +#ifdef WIN32 +#pragma warning(disable : 4786) // shut off 255 char limit debug template warning +#endif + +#include "SM_Scene.h" +#include "SM_Object.h" +#include "SM_FhObject.h" + +#include <algorithm> + +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); + } + + 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); + } +} + +void SM_Scene::remove(SM_Object& object) { + T_ObjectList::iterator i = + std::find(m_objectList.begin(), m_objectList.end(), &object); + if (!(i == m_objectList.end())) + { + 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 { + // tried to remove an object that is not in the scene + //assert(false); + } +} + +void SM_Scene::proceed(MT_Scalar timeStep, MT_Scalar subSampling) { + // Don't waste time...but it's OK to spill a little. + if (timeStep < 0.001) + return; + + // Divide the timeStep into a number of subsamples of size roughly + // 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); + } + + // 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); + } + + // 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 + // 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. +#endif + // The collsion and friction impulses are computed here. + DT_Test(m_scene, m_respTable); + + } + + + // clear the user set velocities. +#if 0 + clearObjectCombinedVelocities(); +#endif + // 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)->proceedKinematic(timeStep); + (*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. + + if (m_secondaryRespTable) { + T_PairList::iterator p; + for (p = m_pairList.begin(); p != m_pairList.end(); ++p) { + DT_CallResponse(m_secondaryRespTable, + (*p).first->getObjectHandle(), + (*p).second->getObjectHandle(), + 0); + } + } + + clearPairs(); +} + +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; + + SM_Object *hit_object = (SM_Object *) + DT_RayTest(m_scene, ignore_client, from.getValue(), to.getValue(), + local.getValue(), normal.getValue()); + + if (hit_object) { + result = hit_object->getWorldCoord(local); + } + + return hit_object; +} + +void SM_Scene::clearObjectCombinedVelocities() { + + T_ObjectList::iterator i; + + for (i = m_objectList.begin(); i != m_objectList.end(); ++i) { + + (*i)->clearCombinedVelocities(); + + } + +} + + + + + |