From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- .../ode-0.9/contrib/BreakableJoints/README.txt | 110 + libraries/ode-0.9/contrib/BreakableJoints/common.h | 337 +++ .../contrib/BreakableJoints/diff/common.h.diff | 21 + .../contrib/BreakableJoints/diff/joint.cpp.diff | 148 ++ .../contrib/BreakableJoints/diff/joint.h.diff | 18 + .../contrib/BreakableJoints/diff/objects.h.diff | 13 + .../contrib/BreakableJoints/diff/ode.cpp.diff | 28 + .../contrib/BreakableJoints/diff/step.cpp.diff | 130 + .../contrib/BreakableJoints/diff/stepfast.cpp.diff | 143 + .../BreakableJoints/diff/test_buggy.cpp.diff | 16 + .../ode-0.9/contrib/BreakableJoints/joint.cpp | 2803 ++++++++++++++++++++ libraries/ode-0.9/contrib/BreakableJoints/joint.h | 282 ++ .../ode-0.9/contrib/BreakableJoints/objects.h | 252 ++ libraries/ode-0.9/contrib/BreakableJoints/ode.cpp | 1404 ++++++++++ libraries/ode-0.9/contrib/BreakableJoints/step.cpp | 1170 ++++++++ .../ode-0.9/contrib/BreakableJoints/stepfast.cpp | 1212 +++++++++ .../contrib/BreakableJoints/test_breakable.cpp | 416 +++ .../ode-0.9/contrib/BreakableJoints/test_buggy.cpp | 327 +++ 18 files changed, 8830 insertions(+) create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/README.txt create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/common.h create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/joint.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/joint.h create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/objects.h create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/ode.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/step.cpp create mode 100755 libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp (limited to 'libraries/ode-0.9/contrib/BreakableJoints') diff --git a/libraries/ode-0.9/contrib/BreakableJoints/README.txt b/libraries/ode-0.9/contrib/BreakableJoints/README.txt new file mode 100644 index 0000000..e996816 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/README.txt @@ -0,0 +1,110 @@ +Breakable Joints + +================================================================================ + +Description: +This is a small addition to ODE that makes joints breakable. Breakable means +that if a force on a joint is to high it wil break. I have included a modified +version of test_buggy.cpp (test_breakable.cpp) so you can see it for your self. +Just drive your buggy into an obstacle and enjoy! + +================================================================================ + +Installation instructions: +- copy joint.h, joint.cpp, ode.cpp and step.cpp to the ode/src/ directory +- copy common.h and object.h to the include/ directory +- copy test_breakable.cpp to the ode/test/ directory +- add test_breakable.cpp to the ODE_TEST_SRC_CPP object in the makefile. +- make ode-lib +- make ode-test +You can also use the diffs. The above files will quickly go out of sync with the +rest of ODE but the diffs wil remain valid longer. + +================================================================================ + +Functions: +dJointSetBreakable (dJointID joint, int b) + If b is 1 the joint is made breakable. If b is 0 the joint is made + unbreakable. + +void dJointSetBreakCallback (dJointID joint, dJointBreakCallback *callbackFunc) + Sets the callback function for this joint. If a funtion is set it will be + called if the joint is broken but before it is actually detached or deleted. + +void dJointSetBreakMode (dJointID joint, int mode) + Use this functions to set some flags. These flags can be ORred ( | ) + together; ie. dJointSetBreakMode (someJoint, +dJOINT_BREAK_AT_B1_FORCE|dJOINT_DELETE_ON_BREAK) + dJOINT_DELETE_ON_BREAK - If the joint breaks it wil be deleted. + dJOINT_BREAK_AT_B1_FORCE - If the force on body 1 is to high the joint will + break + dJOINT_BREAK_AT_B1_TORQUE - If the torque on body 1 is to high the joint will + break + dJOINT_BREAK_AT_B2_FORCE - If the force on body 2 is to high the joint will + break + dJOINT_BREAK_AT_B2_TORQUE - If the torque on body 2 is to high the joint will + break + +void dJointSetBreakForce (dJointID joint, int body, dReal x, dReal y, dReal z) + With this function you can set the maximum force for a body connected to this + joint. A value of 0 for body means body 1, 1 means body 2. The force is + relative to the bodies rotation. + +void dJointSetBreakTorque (dJointID joint, int body, dReal x, dReal y, dReal z) + With this function you can set the maximum torque for a body connected to this + joint. A value of 0 for body means body 1, 1 means body 2. The torque is + relative to the bodies rotation. + +int dJointIsBreakable (dJointID joint) + Returns 1 if this joint is breakable, 0 otherwise. + +int dJointGetBreakMode (dJointID joint) + Returns the breakmode flag. + +void dJointGetBreakForce (dJointID joint, int body, dReal *force) + Returns the force at what this joint will break. A value of 0 for body means + body 1, 1 means body 2. force must have enough space for 3 dReal values. + +void dJointGetBreakTorque (dJointID joint, int body, dReal *torque) + Returns the torque at what this joint will break. A value of 0 for body + means body 1, 1 means body 2. force must have enough space for 3 dReal + values. + +================================================================================ + +The callback function is defined like this (in common.h): +void dJointBreakCallback (dJointID); + +================================================================================ + +Problems, known bugs & other issues: +- If the timestep is very small then joints get a lot weaker. They can even fall + apart! +- I have tested all this with the latest checkout from CVS (at the time of + writing ofcourse). I haven't tested it with earlier versions of ODE. +- I have modified the code that fills the jointfeedback struct. I haven't tested + if it still works. +- I'm not sure if the forces are really relative to the connected bodies. +- There are some memory leaks in the test_breakable.cpp example. + +================================================================================ + +Bugfixes and changes: +09/08/2003 +- I fixed a bug when there where 0 joints in the simulation + +06/12/2003 +- dJointGetBreakMode() added, by vadim_mcagon@hotmail.com + +11/03/2004 +- Updated files to work with latest CVS checkout. +- Added support for dWorldStepFast1() +- Added separate test_breakable.cpp example. +- Updated the code that breaks and destroys a joint. + +================================================================================ + +Send me an e-mail if you have any suggestions, ideas, bugs, bug-fixes, anything! +e-mail: roelvandijk@home.nl + +Roel van Dijk - 11/03/2004 diff --git a/libraries/ode-0.9/contrib/BreakableJoints/common.h b/libraries/ode-0.9/contrib/BreakableJoints/common.h new file mode 100644 index 0000000..bc4272d --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/common.h @@ -0,0 +1,337 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library 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 files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_COMMON_H_ +#define _ODE_COMMON_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +/* configuration stuff */ + +/* the efficient alignment. most platforms align data structures to some + * number of bytes, but this is not always the most efficient alignment. + * for example, many x86 compilers align to 4 bytes, but on a pentium it + * is important to align doubles to 8 byte boundaries (for speed), and + * the 4 floats in a SIMD register to 16 byte boundaries. many other + * platforms have similar behavior. setting a larger alignment can waste + * a (very) small amount of memory. NOTE: this number must be a power of + * two. this is set to 16 by default. + */ +#define EFFICIENT_ALIGNMENT 16 + + +/* constants */ + +/* pi and 1/sqrt(2) are defined here if necessary because they don't get + * defined in on some platforms (like MS-Windows) + */ + +#ifndef M_PI +#define M_PI REAL(3.1415926535897932384626433832795029) +#endif +#ifndef M_SQRT1_2 +#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490) +#endif + + +/* debugging: + * IASSERT is an internal assertion, i.e. a consistency check. if it fails + * we want to know where. + * UASSERT is a user assertion, i.e. if it fails a nice error message + * should be printed for the user. + * AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)" + * is printed. + * DEBUGMSG just prints out a message + */ + +#ifndef dNODEBUG +#ifdef __GNUC__ +#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__); +#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ + msg " in %s()", __FUNCTION__); +#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ + msg " in %s()", __FUNCTION__); +#else +#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__); +#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ + msg " (%s:%d)", __FILE__,__LINE__); +#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ + msg " (%s:%d)", __FILE__,__LINE__); +#endif +#else +#define dIASSERT(a) ; +#define dUASSERT(a,msg) ; +#define dDEBUGMSG(msg) ; +#endif +#define dAASSERT(a) dUASSERT(a,"Bad argument(s)") + +/* floating point data type, vector, matrix and quaternion types */ + +#if defined(dSINGLE) +typedef float dReal; +#elif defined(dDOUBLE) +typedef double dReal; +#else +#error You must #define dSINGLE or dDOUBLE +#endif + + +/* round an integer up to a multiple of 4, except that 0 and 1 are unmodified + * (used to compute matrix leading dimensions) + */ +#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a)) + +/* these types are mainly just used in headers */ +typedef dReal dVector3[4]; +typedef dReal dVector4[4]; +typedef dReal dMatrix3[4*3]; +typedef dReal dMatrix4[4*4]; +typedef dReal dMatrix6[8*6]; +typedef dReal dQuaternion[4]; + + +/* precision dependent scalar math functions */ + +#if defined(dSINGLE) + +#define REAL(x) (x ## f) /* form a constant */ +#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ +#define dSqrt(x) ((float)sqrt(x)) /* square root */ +#define dRecipSqrt(x) ((float)(1.0f/sqrt(x))) /* reciprocal square root */ +#define dSin(x) ((float)sin(x)) /* sine */ +#define dCos(x) ((float)cos(x)) /* cosine */ +#define dFabs(x) ((float)fabs(x)) /* absolute value */ +#define dAtan2(y,x) ((float)atan2((y),(x))) /* arc tangent with 2 args */ + +#elif defined(dDOUBLE) + +#define REAL(x) (x) +#define dRecip(x) (1.0/(x)) +#define dSqrt(x) sqrt(x) +#define dRecipSqrt(x) (1.0/sqrt(x)) +#define dSin(x) sin(x) +#define dCos(x) cos(x) +#define dFabs(x) fabs(x) +#define dAtan2(y,x) atan2((y),(x)) + +#else +#error You must #define dSINGLE or dDOUBLE +#endif + + +/* utility */ + + +/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */ + +#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) + + +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ + +#define dALLOCA16(n) \ + ((char*)dEFFICIENT_SIZE(((int)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) + + +/* internal object types (all prefixed with `dx') */ + +struct dxWorld; /* dynamics world */ +struct dxSpace; /* collision space */ +struct dxBody; /* rigid body (dynamics object) */ +struct dxGeom; /* geometry (collision object) */ +struct dxJoint; +struct dxJointNode; +struct dxJointGroup; + +typedef struct dxWorld *dWorldID; +typedef struct dxSpace *dSpaceID; +typedef struct dxBody *dBodyID; +typedef struct dxGeom *dGeomID; +typedef struct dxJoint *dJointID; +typedef struct dxJointGroup *dJointGroupID; + + +/* error numbers */ + +enum { + d_ERR_UNKNOWN = 0, /* unknown error */ + d_ERR_IASSERT, /* internal assertion failed */ + d_ERR_UASSERT, /* user assertion failed */ + d_ERR_LCP /* user assertion failed */ +}; + + +/* joint type numbers */ + +enum { + dJointTypeNone = 0, /* or "unknown" */ + dJointTypeBall, + dJointTypeHinge, + dJointTypeSlider, + dJointTypeContact, + dJointTypeUniversal, + dJointTypeHinge2, + dJointTypeFixed, + dJointTypeNull, + dJointTypeAMotor +}; + +/******************** breakable joint contribution ***********************/ +/* joint break callback function */ +typedef void dJointBreakCallback (dJointID joint); + +/* joint break modes */ +enum { + // if this flag is set, the joint wil break + dJOINT_BROKEN = 0x0001, + // if this flag is set, the joint wil be deleted when it breaks + dJOINT_DELETE_ON_BREAK = 0x0002, + // if this flag is set, the joint can break at a certain force on body 1 + dJOINT_BREAK_AT_B1_FORCE = 0x0004, + // if this flag is set, the joint can break at a certain torque on body 1 + dJOINT_BREAK_AT_B1_TORQUE = 0x0008, + // if this flag is set, the joint can break at a certain force on body 2 + dJOINT_BREAK_AT_B2_FORCE = 0x0010, + // if this flag is set, the joint can break at a certain torque on body 2 + dJOINT_BREAK_AT_B2_TORQUE = 0x0020 +}; +/*************************************************************************/ + +/* an alternative way of setting joint parameters, using joint parameter + * structures and member constants. we don't actually do this yet. + */ + +/* +typedef struct dLimot { + int mode; + dReal lostop, histop; + dReal vel, fmax; + dReal fudge_factor; + dReal bounce, soft; + dReal suspension_erp, suspension_cfm; +} dLimot; + +enum { + dLimotLoStop = 0x0001, + dLimotHiStop = 0x0002, + dLimotVel = 0x0004, + dLimotFMax = 0x0008, + dLimotFudgeFactor = 0x0010, + dLimotBounce = 0x0020, + dLimotSoft = 0x0040 +}; +*/ + + +/* standard joint parameter names. why are these here? - because we don't want + * to include all the joint function definitions in joint.cpp. hmmmm. + * MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second argument, + * which is why we have the D_ALL_PARAM_NAMES macro as well. please copy and + * paste between these two. + */ + +#define D_ALL_PARAM_NAMES(start) \ + /* parameters for limits and motors */ \ + dParamLoStop = start, \ + dParamHiStop, \ + dParamVel, \ + dParamFMax, \ + dParamFudgeFactor, \ + dParamBounce, \ + dParamCFM, \ + dParamStopERP, \ + dParamStopCFM, \ + /* parameters for suspension */ \ + dParamSuspensionERP, \ + dParamSuspensionCFM, + +#define D_ALL_PARAM_NAMES_X(start,x) \ + /* parameters for limits and motors */ \ + dParamLoStop ## x = start, \ + dParamHiStop ## x, \ + dParamVel ## x, \ + dParamFMax ## x, \ + dParamFudgeFactor ## x, \ + dParamBounce ## x, \ + dParamCFM ## x, \ + dParamStopERP ## x, \ + dParamStopCFM ## x, \ + /* parameters for suspension */ \ + dParamSuspensionERP ## x, \ + dParamSuspensionCFM ## x, + +enum { + D_ALL_PARAM_NAMES(0) + D_ALL_PARAM_NAMES_X(0x100,2) + D_ALL_PARAM_NAMES_X(0x200,3) + + /* add a multiple of this constant to the basic parameter numbers to get + * the parameters for the second, third etc axes. + */ + dParamGroup=0x100 +}; + + +/* angular motor mode numbers */ + +enum{ + dAMotorUser = 0, + dAMotorEuler = 1 +}; + + +/* joint force feedback information */ + +typedef struct dJointFeedback { + dVector3 f1; /* force applied to body 1 */ + dVector3 t1; /* torque applied to body 1 */ + dVector3 f2; /* force applied to body 2 */ + dVector3 t2; /* torque applied to body 2 */ +} dJointFeedback; + + +/* private functions that must be implemented by the collision library: + * (1) indicate that a geom has moved, (2) get the next geom in a body list. + * these functions are called whenever the position of geoms connected to a + * body have changed, e.g. with dBodySetPosition(), dBodySetRotation(), or + * when the ODE step function updates the body state. + */ + +void dGeomMoved (dGeomID); +dGeomID dGeomGetBodyNext (dGeomID); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff new file mode 100644 index 0000000..24415a1 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff @@ -0,0 +1,21 @@ +208,227d207 +< /******************** breakable joint contribution ***********************/ +< /* joint break callback function */ +< typedef void dJointBreakCallback (dJointID joint); +< +< /* joint break modes */ +< enum { +< // if this flag is set, the joint wil break +< dJOINT_BROKEN = 0x0001, +< // if this flag is set, the joint wil be deleted when it breaks +< dJOINT_DELETE_ON_BREAK = 0x0002, +< // if this flag is set, the joint can break at a certain force on body 1 +< dJOINT_BREAK_AT_B1_FORCE = 0x0004, +< // if this flag is set, the joint can break at a certain torque on body 1 +< dJOINT_BREAK_AT_B1_TORQUE = 0x0008, +< // if this flag is set, the joint can break at a certain force on body 2 +< dJOINT_BREAK_AT_B2_FORCE = 0x0010, +< // if this flag is set, the joint can break at a certain torque on body 2 +< dJOINT_BREAK_AT_B2_TORQUE = 0x0020 +< }; +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff new file mode 100644 index 0000000..80397f0 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff @@ -0,0 +1,148 @@ +2659,2804d2658 +< +< /******************** breakable joint contribution ***********************/ +< extern "C" void dJointSetBreakable (dxJoint *joint, int b) { +< dAASSERT(joint); +< if (b) { +< // we want this joint to be breakable but we must first check if it +< // was already breakable +< if (!joint->breakInfo) { +< // allocate a dxJointBreakInfo struct +< joint->breakInfo = new dxJointBreakInfo; +< joint->breakInfo->flags = 0; +< for (int i = 0; i < 3; i++) { +< joint->breakInfo->b1MaxF[0] = 0; +< joint->breakInfo->b1MaxT[0] = 0; +< joint->breakInfo->b2MaxF[0] = 0; +< joint->breakInfo->b2MaxT[0] = 0; +< } +< joint->breakInfo->callback = 0; +< } +< else { +< // the joint was already breakable +< return; +< } +< } +< else { +< // we want this joint to be unbreakable mut we must first check if +< // it is alreay unbreakable +< if (joint->breakInfo) { +< // deallocate the dxJointBreakInfo struct +< delete joint->breakInfo; +< joint->breakInfo = 0; +< } +< else { +< // the joint was already unbreakable +< return; +< } +< } +< } +< +< extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakCallback called on unbreakable joint"); +< } +< # endif +< joint->breakInfo->callback = callbackFunc; +< } +< +< extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakMode called on unbreakable joint"); +< } +< # endif +< joint->breakInfo->flags = mode; +< } +< +< extern "C" int dJointGetBreakMode (dxJoint *joint) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointGetBreakMode called on unbreakable joint"); +< } +< # endif +< return joint->breakInfo->flags; +< } +< +< extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakForce called on unbreakable joint"); +< } +< # endif +< if (body) { +< joint->breakInfo->b2MaxF[0] = x; +< joint->breakInfo->b2MaxF[1] = y; +< joint->breakInfo->b2MaxF[2] = z; +< } +< else { +< joint->breakInfo->b1MaxF[0] = x; +< joint->breakInfo->b1MaxF[1] = y; +< joint->breakInfo->b1MaxF[2] = z; +< } +< } +< +< extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakTorque called on unbreakable joint"); +< } +< # endif +< if (body) { +< joint->breakInfo->b2MaxT[0] = x; +< joint->breakInfo->b2MaxT[1] = y; +< joint->breakInfo->b2MaxT[2] = z; +< } +< else { +< joint->breakInfo->b1MaxT[0] = x; +< joint->breakInfo->b1MaxT[1] = y; +< joint->breakInfo->b1MaxT[2] = z; +< } +< } +< +< extern "C" int dJointIsBreakable (dxJoint *joint) { +< dAASSERT(joint); +< return joint->breakInfo != 0; +< } +< +< extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointGetBreakForce called on unbreakable joint"); +< } +< # endif +< if (body) +< for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i]; +< else +< for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i]; +< } +< +< extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointGetBreakTorque called on unbreakable joint"); +< } +< # endif +< if (body) +< for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i]; +< else +< for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i]; +< } +< /*************************************************************************/ +< +\ No newline at end of file diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff new file mode 100644 index 0000000..eed3c24 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff @@ -0,0 +1,18 @@ +61,70d60 +< /******************** breakable joint contribution ***********************/ +< struct dxJointBreakInfo : public dBase { +< int flags; +< dReal b1MaxF[3]; // maximum force on body 1 +< dReal b1MaxT[3]; // maximum torque on body 1 +< dReal b2MaxF[3]; // maximum force on body 2 +< dReal b2MaxT[3]; // maximum torque on body 2 +< dJointBreakCallback *callback; // function that is called when this joint breaks +< }; +< /*************************************************************************/ +135,140d124 +< +< /******************** breakable joint contribution ***********************/ +< // optional break info structure. if this is not NULL the the joint is +< // breakable. +< dxJointBreakInfo *breakInfo; +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff new file mode 100644 index 0000000..fd2129e --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff @@ -0,0 +1,13 @@ +168,179d167 +< /******************** breakable joint contribution ***********************/ +< void dJointSetBreakable (dJointID, int b); +< void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc); +< void dJointSetBreakMode (dJointID, int mode); +< int dJointGetBreakMode (dJointID); +< void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z); +< void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z); +< int dJointIsBreakable (dJointID); +< void dJointGetBreakForce (dJointID, int body, dReal *force); +< void dJointGetBreakTorque (dJointID, int body, dReal *torque); +< /*************************************************************************/ +< diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff new file mode 100644 index 0000000..761b7be --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff @@ -0,0 +1,28 @@ +212,230d211 +< /******************** breakable joint contribution ***********************/ +< dxJoint* nextJ; +< if (!world->firstjoint) +< nextJ = 0; +< else +< nextJ = (dxJoint*)world->firstjoint->next; +< for (j=world->firstjoint; j; j=nextJ) { +< nextJ = (dxJoint*)j->next; +< // check if joint is breakable and broken +< if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { +< // detach (break) the joint +< dJointAttach (j, 0, 0); +< // call the callback function if it is set +< if (j->breakInfo->callback) j->breakInfo->callback (j); +< // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set +< if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); +< } +< } +< /*************************************************************************/ +931,933d911 +< /******************** breakable joint contribution ***********************/ +< j->breakInfo = 0; +< /*************************************************************************/ +1011,1013d988 +< /******************** breakable joint contribution ***********************/ +< if (j->breakInfo) delete j->breakInfo; +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff new file mode 100644 index 0000000..dfc8c2f --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff @@ -0,0 +1,130 @@ +966,1066c966,989 +< /******************** breakable joint contribution ***********************/ +< // this saves us a few dereferences +< dxJointBreakInfo *jBI = joint[i]->breakInfo; +< // we need joint feedback if the joint is breakable or if the user +< // requested feedback. +< if (jBI||fb) { +< // we need feedback on the amount of force that this joint is +< // applying to the bodies. we use a slightly slower computation +< // that splits out the force components and puts them in the +< // feedback structure. +< dJointFeedback temp_fb; // temporary storage for joint feedback +< dReal data1[8],data2[8]; +< Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); +< dReal *cf1 = cforce + 8*b1->tag; +< cf1[0] += (temp_fb.f1[0] = data1[0]); +< cf1[1] += (temp_fb.f1[1] = data1[1]); +< cf1[2] += (temp_fb.f1[2] = data1[2]); +< cf1[4] += (temp_fb.t1[0] = data1[4]); +< cf1[5] += (temp_fb.t1[1] = data1[5]); +< cf1[6] += (temp_fb.t1[2] = data1[6]); +< if (b2) { +< Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); +< dReal *cf2 = cforce + 8*b2->tag; +< cf2[0] += (temp_fb.f2[0] = data2[0]); +< cf2[1] += (temp_fb.f2[1] = data2[1]); +< cf2[2] += (temp_fb.f2[2] = data2[2]); +< cf2[4] += (temp_fb.t2[0] = data2[4]); +< cf2[5] += (temp_fb.t2[1] = data2[5]); +< cf2[6] += (temp_fb.t2[2] = data2[6]); +< } +< // if the user requested so we must copy the feedback information to +< // the feedback struct that the user suplied. +< if (fb) { +< // copy temp_fb to fb +< fb->f1[0] = temp_fb.f1[0]; +< fb->f1[1] = temp_fb.f1[1]; +< fb->f1[2] = temp_fb.f1[2]; +< fb->t1[0] = temp_fb.t1[0]; +< fb->t1[1] = temp_fb.t1[1]; +< fb->t1[2] = temp_fb.t1[2]; +< if (b2) { +< fb->f2[0] = temp_fb.f2[0]; +< fb->f2[1] = temp_fb.f2[1]; +< fb->f2[2] = temp_fb.f2[2]; +< fb->t2[0] = temp_fb.t2[0]; +< fb->t2[1] = temp_fb.t2[1]; +< fb->t2[2] = temp_fb.t2[2]; +< } +< } +< // if the joint is breakable we need to check the breaking conditions +< if (jBI) { +< dReal relCF1[3]; +< dReal relCT1[3]; +< // multiply the force and torque vectors by the rotation matrix of body 1 +< dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]); +< dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF1[i] > jBI->b1MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT1[i] > jBI->b1MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (b2) { +< dReal relCF2[3]; +< dReal relCT2[3]; +< // multiply the force and torque vectors by the rotation matrix of body 2 +< dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]); +< dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF2[i] > jBI->b2MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT2[i] > jBI->b2MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< } +< doneCheckingBreaks: +< ; +--- +> if (fb) { +> // the user has requested feedback on the amount of force that this +> // joint is applying to the bodies. we use a slightly slower +> // computation that splits out the force components and puts them +> // in the feedback structure. +> dReal data1[8],data2[8]; +> Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); +> dReal *cf1 = cforce + 8*b1->tag; +> cf1[0] += (fb->f1[0] = data1[0]); +> cf1[1] += (fb->f1[1] = data1[1]); +> cf1[2] += (fb->f1[2] = data1[2]); +> cf1[4] += (fb->t1[0] = data1[4]); +> cf1[5] += (fb->t1[1] = data1[5]); +> cf1[6] += (fb->t1[2] = data1[6]); +> if (b2){ +> Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); +> dReal *cf2 = cforce + 8*b2->tag; +> cf2[0] += (fb->f2[0] = data2[0]); +> cf2[1] += (fb->f2[1] = data2[1]); +> cf2[2] += (fb->f2[2] = data2[2]); +> cf2[4] += (fb->t2[0] = data2[4]); +> cf2[5] += (fb->t2[1] = data2[5]); +> cf2[6] += (fb->t2[2] = data2[6]); +> } +1068,1069d990 +< } +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff new file mode 100644 index 0000000..ed64cba --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff @@ -0,0 +1,143 @@ +587,598c587,593 +< /******************** breakable joint contribution ***********************/ +< // this saves us a few dereferences +< dxJointBreakInfo *jBI = joint->breakInfo; +< // we need joint feedback if the joint is breakable or if the user +< // requested feedback. +< if (jBI||fb) { +< // we need feedback on the amount of force that this joint is +< // applying to the bodies. we use a slightly slower computation +< // that splits out the force components and puts them in the +< // feedback structure. +< dJointFeedback temp_fb; // temporary storage for joint feedback +< dReal data1[8],data2[8]; +--- +> if (fb) +> { +> // the user has requested feedback on the amount of force that this +> // joint is applying to the bodies. we use a slightly slower +> // computation that splits out the force components and puts them +> // in the feedback structure. +> dReal data1[8], data2[8]; +603,608c598,603 +< cf1[0] = (temp_fb.f1[0] = data1[0]); +< cf1[1] = (temp_fb.f1[1] = data1[1]); +< cf1[2] = (temp_fb.f1[2] = data1[2]); +< cf1[4] = (temp_fb.t1[0] = data1[4]); +< cf1[5] = (temp_fb.t1[1] = data1[5]); +< cf1[6] = (temp_fb.t1[2] = data1[6]); +--- +> cf1[0] = (fb->f1[0] = data1[0]); +> cf1[1] = (fb->f1[1] = data1[1]); +> cf1[2] = (fb->f1[2] = data1[2]); +> cf1[4] = (fb->t1[0] = data1[4]); +> cf1[5] = (fb->t1[1] = data1[5]); +> cf1[6] = (fb->t1[2] = data1[6]); +614,691c609,614 +< cf2[0] = (temp_fb.f2[0] = data2[0]); +< cf2[1] = (temp_fb.f2[1] = data2[1]); +< cf2[2] = (temp_fb.f2[2] = data2[2]); +< cf2[4] = (temp_fb.t2[0] = data2[4]); +< cf2[5] = (temp_fb.t2[1] = data2[5]); +< cf2[6] = (temp_fb.t2[2] = data2[6]); +< } +< // if the user requested so we must copy the feedback information to +< // the feedback struct that the user suplied. +< if (fb) { +< // copy temp_fb to fb +< fb->f1[0] = temp_fb.f1[0]; +< fb->f1[1] = temp_fb.f1[1]; +< fb->f1[2] = temp_fb.f1[2]; +< fb->t1[0] = temp_fb.t1[0]; +< fb->t1[1] = temp_fb.t1[1]; +< fb->t1[2] = temp_fb.t1[2]; +< if (body[1]) { +< fb->f2[0] = temp_fb.f2[0]; +< fb->f2[1] = temp_fb.f2[1]; +< fb->f2[2] = temp_fb.f2[2]; +< fb->t2[0] = temp_fb.t2[0]; +< fb->t2[1] = temp_fb.t2[1]; +< fb->t2[2] = temp_fb.t2[2]; +< } +< } +< // if the joint is breakable we need to check the breaking conditions +< if (jBI) { +< dReal relCF1[3]; +< dReal relCT1[3]; +< // multiply the force and torque vectors by the rotation matrix of body 1 +< dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]); +< dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF1[i] > jBI->b1MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT1[i] > jBI->b1MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (body[1]) { +< dReal relCF2[3]; +< dReal relCT2[3]; +< // multiply the force and torque vectors by the rotation matrix of body 2 +< dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]); +< dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF2[i] > jBI->b2MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT2[i] > jBI->b2MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< } +< doneCheckingBreaks: +< ; +--- +> cf2[0] = (fb->f2[0] = data2[0]); +> cf2[1] = (fb->f2[1] = data2[1]); +> cf2[2] = (fb->f2[2] = data2[2]); +> cf2[4] = (fb->t2[0] = data2[4]); +> cf2[5] = (fb->t2[1] = data2[5]); +> cf2[6] = (fb->t2[2] = data2[6]); +694d616 +< /*************************************************************************/ +1178,1196d1099 +< /******************** breakable joint contribution ***********************/ +< dxJoint* nextJ; +< if (!world->firstjoint) +< nextJ = 0; +< else +< nextJ = (dxJoint*)world->firstjoint->next; +< for (j=world->firstjoint; j; j=nextJ) { +< nextJ = (dxJoint*)j->next; +< // check if joint is breakable and broken +< if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { +< // detach (break) the joint +< dJointAttach (j, 0, 0); +< // call the callback function if it is set +< if (j->breakInfo->callback) j->breakInfo->callback (j); +< // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set +< if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); +< } +< } +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff new file mode 100644 index 0000000..65770da --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff @@ -0,0 +1,16 @@ +266,270d265 +< +< // breakable joints contribution +< dJointSetBreakable (joint[i], 1); +< dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_FORCE); +< dJointSetBreakForce (joint[i], 0.5); +298c293 +< ground_box = dCreateBox (space,2,1.5,5); +--- +> ground_box = dCreateBox (space,2,1.5,1); +300,301c295,296 +< dRFromAxisAndAngle (R,0,1,0,-0.85); +< dGeomSetPosition (ground_box,5,0,-1); +--- +> dRFromAxisAndAngle (R,0,1,0,-0.15); +> dGeomSetPosition (ground_box,2,0,-0.34); diff --git a/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp b/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp new file mode 100644 index 0000000..2c724f8 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp @@ -0,0 +1,2803 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library 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 files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +design note: the general principle for giving a joint the option of connecting +to the static environment (i.e. the absolute frame) is to check the second +body (joint->node[1].body), and if it is zero then behave as if its body +transform is the identity. + +*/ + +#include +#include +#include +#include "joint.h" + +//**************************************************************************** +// externs + +extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); +extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); + +//**************************************************************************** +// utility + +// set three "ball-and-socket" rows in the constraint equation, and the +// corresponding right hand side. + +static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int s = info->rowskip; + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dCROSSMAT (info->J1a,a1,s,-,+); + if (joint->node[1].body) { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dCROSSMAT (info->J2a,a2,s,+,-); + } + + // set right hand side + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) { + info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] - + a1[j] - joint->node[0].body->pos[j]); + } + } + else { + for (int j=0; j<3; j++) { + info->c[j] = k * (anchor2[j] - a1[j] - + joint->node[0].body->pos[j]); + } + } +} + + +// this is like setBall(), except that `axis' is a unit length vector +// (in global coordinates) that should be used for the first jacobian +// position row (the other two row vectors will be derived from this). +// `erp1' is the erp value to use along the axis. + +static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2, + dVector3 axis, dReal erp1) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int i,s = info->rowskip; + + // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], + // [0 1 0] and [0 0 1], which makes everything much easier. + dVector3 q1,q2; + dPlaneSpace (axis,q1,q2); + + // set jacobian + for (i=0; i<3; i++) info->J1l[i] = axis[i]; + for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; + for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; + dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dCROSS (info->J1a,=,a1,axis); + dCROSS (info->J1a+s,=,a1,q1); + dCROSS (info->J1a+2*s,=,a1,q2); + if (joint->node[1].body) { + for (i=0; i<3; i++) info->J2l[i] = -axis[i]; + for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; + for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; + dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dCROSS (info->J2a,= -,a2,axis); + dCROSS (info->J2a+s,= -,a2,q1); + dCROSS (info->J2a+2*s,= -,a2,q2); + } + + // set right hand side - measure error along (axis,q1,q2) + dReal k1 = info->fps * erp1; + dReal k = info->fps * info->erp; + + for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i]; + if (joint->node[1].body) { + for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i]; + info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); + } + else { + info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); + } +} + + +// set three orientation rows in the constraint equation, and the +// corresponding right hand side. + +static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) +{ + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->J1a[start_index] = 1; + info->J1a[start_index + s + 1] = 1; + info->J1a[start_index + s*2+2] = 1; + if (joint->node[1].body) { + info->J2a[start_index] = -1; + info->J2a[start_index + s+1] = -1; + info->J2a[start_index + s*2+2] = -1; + } + + // compute the right hand side. the first three elements will result in + // relative angular velocity of the two bodies - this is set to bring them + // back into alignment. the correcting angular velocity is + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * u + // = (erp*fps) * theta * u + // where rotation along unit length axis u by theta brings body 2's frame + // to qrel with respect to body 1's frame. using a small angle approximation + // for sin(), this gives + // angular_velocity = (erp*fps) * 2 * v + // where the quaternion of the relative rotation between the two bodies is + // q = [cos(theta/2) sin(theta/2)*u] = [s v] + + // get qerr = relative rotation (rotation error) between two bodies + dQuaternion qerr,e; + if (joint->node[1].body) { + dQuaternion qq; + dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); + dQMultiply2 (qerr,qq,qrel); + } + else { + dQMultiply3 (qerr,joint->node[0].body->q,qrel); + } + if (qerr[0] < 0) { + qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small + qerr[2] = -qerr[2]; + qerr[3] = -qerr[3]; + } + dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! + dReal k = info->fps * info->erp; + info->c[start_row] = 2*k * e[0]; + info->c[start_row+1] = 2*k * e[1]; + info->c[start_row+2] = 2*k * e[2]; +} + + +// compute anchor points relative to bodies + +static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 anchor1, dVector3 anchor2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x - j->node[0].body->pos[0]; + q[1] = y - j->node[0].body->pos[1]; + q[2] = z - j->node[0].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); + if (j->node[1].body) { + q[0] = x - j->node[1].body->pos[0]; + q[1] = y - j->node[1].body->pos[1]; + q[2] = z - j->node[1].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); + } + else { + anchor2[0] = x; + anchor2[1] = y; + anchor2[2] = z; + } + } + anchor1[3] = 0; + anchor2[3] = 0; +} + + +// compute axes relative to bodies. either axis1 or axis2 can be 0. + +static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 axis1, dVector3 axis2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + if (axis1) { + dMULTIPLY1_331 (axis1,j->node[0].body->R,q); + axis1[3] = 0; + } + if (axis2) { + if (j->node[1].body) { + dMULTIPLY1_331 (axis2,j->node[1].body->R,q); + } + else { + axis2[0] = x; + axis2[1] = y; + axis2[2] = z; + } + axis2[3] = 0; + } + } +} + + +static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->R,anchor1); + result[0] += j->node[0].body->pos[0]; + result[1] += j->node[0].body->pos[1]; + result[2] += j->node[0].body->pos[2]; + } +} + + +static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->R,anchor2); + result[0] += j->node[1].body->pos[0]; + result[1] += j->node[1].body->pos[1]; + result[2] += j->node[1].body->pos[2]; + } + else { + result[0] = anchor2[0]; + result[1] = anchor2[1]; + result[2] = anchor2[2]; + } +} + + +static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->R,axis1); + } +} + + +static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->R,axis2); + } + else { + result[0] = axis2[0]; + result[1] = axis2[1]; + result[2] = axis2[2]; + } +} + + +static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) +{ + // the angle between the two bodies is extracted from the quaternion that + // represents the relative rotation between them. recall that a quaternion + // q is: + // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] + // where s is a scalar and v is a 3-vector. u is a unit length axis and + // theta is a rotation along that axis. we can get theta/2 by: + // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) + // but we can't get sin(theta/2) directly, only its absolute value, i.e.: + // |v| = |sin(theta/2)| * |u| + // = |sin(theta/2)| + // using this value will have a strange effect. recall that there are two + // quaternion representations of a given rotation, q and -q. typically as + // a body rotates along the axis it will go through a complete cycle using + // one representation and then the next cycle will use the other + // representation. this corresponds to u pointing in the direction of the + // hinge axis and then in the opposite direction. the result is that theta + // will appear to go "backwards" every other cycle. here is a fix: if u + // points "away" from the direction of the hinge (motor) axis (i.e. more + // than 90 degrees) then use -q instead of q. this represents the same + // rotation, but results in the cos(theta/2) value being sign inverted. + + // extract the angle from the quaternion. cost2 = cos(theta/2), + // sint2 = |sin(theta/2)| + dReal cost2 = qrel[0]; + dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); + dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions + (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis + (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction + + // the angle we get will be between 0..2*pi, but we want to return angles + // between -pi..pi + if (theta > M_PI) theta -= 2*M_PI; + + // the angle we've just extracted has the wrong sign + theta = -theta; + + return theta; +} + + +// given two bodies (body1,body2), the hinge axis that they are connected by +// w.r.t. body1 (axis), and the initial relative orientation between them +// (q_initial), return the relative rotation angle. the initial relative +// orientation corresponds to an angle of zero. if body2 is 0 then measure the +// angle between body1 and the static frame. +// +// this will not return the correct angle if the bodies rotate along any axis +// other than the given hinge axis. + +static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, + dQuaternion q_initial) +{ + // get qrel = relative rotation between the two bodies + dQuaternion qrel; + if (body2) { + dQuaternion qq; + dQMultiply1 (qq,body1->q,body2->q); + dQMultiply2 (qrel,qq,q_initial); + } + else { + // pretend body2->q is the identity + dQMultiply3 (qrel,body1->q,q_initial); + } + + return getHingeAngleFromRelativeQuat (qrel,axis); +} + +//**************************************************************************** +// dxJointLimitMotor + +void dxJointLimitMotor::init (dxWorld *world) +{ + vel = 0; + fmax = 0; + lostop = -dInfinity; + histop = dInfinity; + fudge_factor = 1; + normal_cfm = world->global_cfm; + stop_erp = world->global_erp; + stop_cfm = world->global_cfm; + bounce = 0; + limit = 0; + limit_err = 0; +} + + +void dxJointLimitMotor::set (int num, dReal value) +{ + switch (num) { + case dParamLoStop: + if (value <= histop) lostop = value; + break; + case dParamHiStop: + if (value >= lostop) histop = value; + break; + case dParamVel: + vel = value; + break; + case dParamFMax: + if (value >= 0) fmax = value; + break; + case dParamFudgeFactor: + if (value >= 0 && value <= 1) fudge_factor = value; + break; + case dParamBounce: + bounce = value; + break; + case dParamCFM: + normal_cfm = value; + break; + case dParamStopERP: + stop_erp = value; + break; + case dParamStopCFM: + stop_cfm = value; + break; + } +} + + +dReal dxJointLimitMotor::get (int num) +{ + switch (num) { + case dParamLoStop: return lostop; + case dParamHiStop: return histop; + case dParamVel: return vel; + case dParamFMax: return fmax; + case dParamFudgeFactor: return fudge_factor; + case dParamBounce: return bounce; + case dParamCFM: return normal_cfm; + case dParamStopERP: return stop_erp; + case dParamStopCFM: return stop_cfm; + default: return 0; + } +} + + +int dxJointLimitMotor::testRotationalLimit (dReal angle) +{ + if (angle <= lostop) { + limit = 1; + limit_err = angle - lostop; + return 1; + } + else if (angle >= histop) { + limit = 2; + limit_err = angle - histop; + return 1; + } + else { + limit = 0; + return 0; + } +} + + +int dxJointLimitMotor::addLimot (dxJoint *joint, + dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational) +{ + int srow = row * info->rowskip; + + // if the joint is powered, or has joint limits, add in the extra row + int powered = fmax > 0; + if (powered || limit) { + dReal *J1 = rotational ? info->J1a : info->J1l; + dReal *J2 = rotational ? info->J2a : info->J2l; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if (joint->node[1].body) { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), we have to be careful + // that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (!rotational && joint->node[1].body) { + dVector3 c; + c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]); + c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]); + c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]); + dCROSS (ltd,=,c,ax1); + info->J1a[srow+0] = ltd[0]; + info->J1a[srow+1] = ltd[1]; + info->J1a[srow+2] = ltd[2]; + info->J2a[srow+0] = ltd[0]; + info->J2a[srow+1] = ltd[1]; + info->J2a[srow+2] = ltd[2]; + } + + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (limit && (lostop == histop)) powered = 0; + + if (powered) { + info->cfm[row] = normal_cfm; + if (! limit) { + info->c[row] = vel; + info->lo[row] = -fmax; + info->hi[row] = fmax; + } + else { + // the joint is at a limit, AND is being powered. if the joint is + // being powered into the limit then we apply the maximum motor force + // in that direction, because the motor is working against the + // immovable limit. if the joint is being powered away from the limit + // then we have problems because actually we need *two* lcp + // constraints to handle this case. so we fake it and apply some + // fraction of the maximum force. the fraction to use can be set as + // a fudge factor. + + dReal fm = fmax; + if (vel > 0) fm = -fm; + + // if we're powering away from the limit, apply the fudge factor + if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; + + if (rotational) { + dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1], + -fm*ax1[2]); + if (joint->node[1].body) + dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + } + else { + dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]); + if (joint->node[1].body) { + dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + + // linear limot torque decoupling step: refer to above discussion + dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + } + } + } + } + + if (limit) { + dReal k = info->fps * stop_erp; + info->c[row] = -k * limit_err; + info->cfm[row] = stop_cfm; + + if (lostop == histop) { + // limited low and high simultaneously + info->lo[row] = -dInfinity; + info->hi[row] = dInfinity; + } + else { + if (limit == 1) { + // low limit + info->lo[row] = 0; + info->hi[row] = dInfinity; + } + else { + // high limit + info->lo[row] = -dInfinity; + info->hi[row] = 0; + } + + // deal with bounce + if (bounce > 0) { + // calculate joint velocity + dReal vel; + if (rotational) { + vel = dDOT(joint->node[0].body->avel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->avel,ax1); + } + else { + vel = dDOT(joint->node[0].body->lvel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->lvel,ax1); + } + + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) { + // low limit + if (vel < 0) { + dReal newc = -bounce * vel; + if (newc > info->c[row]) info->c[row] = newc; + } + } + else { + // high limit - all those computations are reversed + if (vel > 0) { + dReal newc = -bounce * vel; + if (newc < info->c[row]) info->c[row] = newc; + } + } + } + } + } + return 1; + } + else return 0; +} + +//**************************************************************************** +// ball and socket + +static void ballInit (dxJointBall *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); +} + + +static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info) +{ + info->m = 3; + info->nub = 3; +} + + +static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) +{ + setBall (joint,info,joint->anchor1,joint->anchor2); +} + + +extern "C" void dJointSetBallAnchor (dxJointBall *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); +} + + +extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetBallAnchor2 (dxJointBall *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +dxJoint::Vtable __dball_vtable = { + sizeof(dxJointBall), + (dxJoint::init_fn*) ballInit, + (dxJoint::getInfo1_fn*) ballGetInfo1, + (dxJoint::getInfo2_fn*) ballGetInfo2, + dJointTypeBall}; + +//**************************************************************************** +// hinge + +static void hingeInit (dxJointHinge *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[0] = 1; + dSetZero (j->qrel,4); + j->limot.init (j->world); +} + + +static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered hinge needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && + j->limot.lostop <= j->limot.histop) { + dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, + j->qrel); + if (j->limot.testRotationalLimit (angle)) info->m = 6; + } +} + + +static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) +{ + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + // set the two hinge rows. the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body + dVector3 p,q; // plane space vectors for ax1 + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dPlaneSpace (ax1,p,q); + + int s3=3*info->rowskip; + int s4=4*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + info->J1a[s4+0] = q[0]; + info->J1a[s4+1] = q[1]; + info->J1a[s4+2] = q[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + info->J2a[s4+0] = -q[0]; + info->J2a[s4+1] = -q[1]; + info->J2a[s4+2] = -q[2]; + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + + dVector3 ax2,b; + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } + dCROSS (b,=,ax1,ax2); + dReal k = info->fps * info->erp; + info->c[3] = k * dDOT(b,p); + info->c[4] = k * dDOT(b,q); + + // if the hinge is powered, or has joint limits, add in the stuff + joint->limot.addLimot (joint,info,5,ax1,1); +} + + +// compute initial relative rotation body1 -> body2, or env -> body1 + +static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) +{ + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + } + else { + // set joint->qrel to the transpose of the first body q + joint->qrel[0] = joint->node[0].body->q[0]; + for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + } + } +} + + +extern "C" void dJointSetHingeAnchor (dxJointHinge *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + hingeComputeInitialRelativeRotation (joint); +} + + +extern "C" void dJointSetHingeAxis (dxJointHinge *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAxes (joint,x,y,z,joint->axis1,joint->axis2); + hingeComputeInitialRelativeRotation (joint); +} + + +extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetHingeAnchor2 (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointSetHingeParam (dxJointHinge *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + joint->limot.set (parameter,value); +} + + +extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + return joint->limot.get (parameter); +} + + +extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->node[0].body) { + dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, + joint->qrel); + if (joint->flags & dJOINT_REVERSE) + return -ang; + else + return ang; + } + else return 0; +} + + +extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + if (joint->flags & dJOINT_REVERSE) rate = - rate; + return rate; + } + else return 0; +} + + +extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque) +{ + dVector3 axis; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + + if (joint->flags & dJOINT_REVERSE) + torque = -torque; + + getAxis (joint,axis,joint->axis1); + axis[0] *= torque; + axis[1] *= torque; + axis[2] *= torque; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dhinge_vtable = { + sizeof(dxJointHinge), + (dxJoint::init_fn*) hingeInit, + (dxJoint::getInfo1_fn*) hingeGetInfo1, + (dxJoint::getInfo2_fn*) hingeGetInfo2, + dJointTypeHinge}; + +//**************************************************************************** +// slider + +static void sliderInit (dxJointSlider *j) +{ + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->qrel,4); + dSetZero (j->offset,4); + j->limot.init (j->world); +} + + +extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1,q; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + // get body2 + offset point in global coordinates + dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset); + for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] - + joint->node[1].body->pos[i]; + } + else { + for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - + joint->offset[i]; + + } + return dDOT(ax1,q); +} + + +extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + return dDOT(ax1,joint->node[0].body->lvel) - + dDOT(ax1,joint->node[1].body->lvel); + } + else { + return dDOT(ax1,joint->node[0].body->lvel); + } +} + + +static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered slider needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + j->limot.limit = 0; + if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && + j->limot.lostop <= j->limot.histop) { + // measure joint position + dReal pos = dJointGetSliderPosition (j); + if (pos <= j->limot.lostop) { + j->limot.limit = 1; + j->limot.limit_err = pos - j->limot.lostop; + info->m = 6; + } + else if (pos >= j->limot.histop) { + j->limot.limit = 2; + j->limot.limit_err = pos - j->limot.histop; + info->m = 6; + } + } +} + + +static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) +{ + int i,s = info->rowskip; + int s2=2*s,s3=3*s,s4=4*s; + + // pull out pos and R for both bodies. also get the `connection' + // vector pos2-pos1. + + dReal *pos1,*pos2,*R1,*R2; + dVector3 c; + pos1 = joint->node[0].body->pos; + R1 = joint->node[0].body->R; + if (joint->node[1].body) { + pos2 = joint->node[1].body->pos; + R2 = joint->node[1].body->R; + for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; + } + else { + pos2 = 0; + R2 = 0; + } + + // 3 rows to make body rotations equal + setFixedOrientation(joint, info, joint->qrel, 0); + + // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. + + dVector3 ax1; // joint axis in global coordinates (unit length) + dVector3 p,q; // plane space of ax1 + dMULTIPLY0_331 (ax1,R1,joint->axis1); + dPlaneSpace (ax1,p,q); + if (joint->node[1].body) { + dVector3 tmp; + dCROSS (tmp, = REAL(0.5) * ,c,p); + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + dCROSS (tmp, = REAL(0.5) * ,c,q); + for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; + for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; + } + for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; + for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; + + // compute last two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + dVector3 ofs; // offset point in global coordinates + dMULTIPLY0_331 (ofs,R2,joint->offset); + for (i=0; i<3; i++) c[i] += ofs[i]; + info->c[3] = k * dDOT(p,c); + info->c[4] = k * dDOT(q,c); + } + else { + dVector3 ofs; // offset point in global coordinates + for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; + info->c[3] = k * dDOT(p,ofs); + info->c[4] = k * dDOT(q,ofs); + } + + // if the slider is powered, or has joint limits, add in the extra row + joint->limot.addLimot (joint,info,5,ax1,0); +} + + +extern "C" void dJointSetSliderAxis (dxJointSlider *joint, + dReal x, dReal y, dReal z) +{ + int i; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + setAxes (joint,x,y,z,joint->axis1,0); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dVector3 c; + for (i=0; i<3; i++) + c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i]; + } +} + + +extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointSetSliderParam (dxJointSlider *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + joint->limot.set (parameter,value); +} + + +extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + return joint->limot.get (parameter); +} + + +extern "C" void dJointAddSliderForce (dxJointSlider *joint, dReal force) +{ + dVector3 axis; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + if (joint->flags & dJOINT_REVERSE) + force -= force; + + getAxis (joint,axis,joint->axis1); + axis[0] *= force; + axis[1] *= force; + axis[2] *= force; + + if (joint->node[0].body != 0) + dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); + if (joint->node[1].body != 0) + dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dslider_vtable = { + sizeof(dxJointSlider), + (dxJoint::init_fn*) sliderInit, + (dxJoint::getInfo1_fn*) sliderGetInfo1, + (dxJoint::getInfo2_fn*) sliderGetInfo2, + dJointTypeSlider}; + +//**************************************************************************** +// contact + +static void contactInit (dxJointContact *j) +{ + // default frictionless contact. hmmm, this info gets overwritten straight + // away anyway, so why bother? +#if 0 /* so don't bother ;) */ + j->contact.surface.mode = 0; + j->contact.surface.mu = 0; + dSetZero (j->contact.geom.pos,4); + dSetZero (j->contact.geom.normal,4); + j->contact.geom.depth = 0; +#endif +} + + +static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) +{ + // make sure mu's >= 0, then calculate number of constraint rows and number + // of unbounded rows. + int m = 1, nub=0; + if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; + if (j->contact.surface.mode & dContactMu2) { + if (j->contact.surface.mu > 0) m++; + if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; + if (j->contact.surface.mu2 > 0) m++; + if (j->contact.surface.mu == dInfinity) nub ++; + if (j->contact.surface.mu2 == dInfinity) nub ++; + } + else { + if (j->contact.surface.mu > 0) m += 2; + if (j->contact.surface.mu == dInfinity) nub += 2; + } + + j->the_m = m; + info->m = m; + info->nub = nub; +} + + +static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) +{ + int i,s = info->rowskip; + int s2 = 2*s; + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + if (j->flags & dJOINT_REVERSE) { + normal[0] = - j->contact.geom.normal[0]; + normal[1] = - j->contact.geom.normal[1]; + normal[2] = - j->contact.geom.normal[2]; + } + else { + normal[0] = j->contact.geom.normal[0]; + normal[1] = j->contact.geom.normal[1]; + normal[2] = j->contact.geom.normal[2]; + } + normal[3] = 0; // @@@ hmmm + + // c1,c2 = contact points with respect to body PORs + dVector3 c1,c2; + for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i]; + + // set jacobian for normal + info->J1l[0] = normal[0]; + info->J1l[1] = normal[1]; + info->J1l[2] = normal[2]; + dCROSS (info->J1a,=,c1,normal); + if (j->node[1].body) { + for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - + j->node[1].body->pos[i]; + info->J2l[0] = -normal[0]; + info->J2l[1] = -normal[1]; + info->J2l[2] = -normal[2]; + dCROSS (info->J2a,= -,c2,normal); + } + + // set right hand side and cfm value for normal + dReal erp = info->erp; + if (j->contact.surface.mode & dContactSoftERP) + erp = j->contact.surface.soft_erp; + dReal k = info->fps * erp; + info->c[0] = k*j->contact.geom.depth; + if (j->contact.surface.mode & dContactSoftCFM) + info->cfm[0] = j->contact.surface.soft_cfm; + + // deal with bounce + if (j->contact.surface.mode & dContactBounce) { + // calculate outgoing velocity (-ve for incoming contact) + dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + + dDOT(info->J1a,j->node[0].body->avel); + if (j->node[1].body) { + outgoing += dDOT(info->J2l,j->node[1].body->lvel) + + dDOT(info->J2a,j->node[1].body->avel); + } + // only apply bounce if the outgoing velocity is greater than the + // threshold, and if the resulting c[0] exceeds what we already have. + if (j->contact.surface.bounce_vel >= 0 && + (-outgoing) > j->contact.surface.bounce_vel) { + dReal newc = - j->contact.surface.bounce * outgoing; + if (newc > info->c[0]) info->c[0] = newc; + } + } + + // set LCP limits for normal + info->lo[0] = 0; + info->hi[0] = dInfinity; + + // now do jacobian for tangential forces + dVector3 t1,t2; // two vectors tangential to normal + + // first friction direction + if (j->the_m >= 2) { + if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? + t1[0] = j->contact.fdir1[0]; + t1[1] = j->contact.fdir1[1]; + t1[2] = j->contact.fdir1[2]; + dCROSS (t2,=,normal,t1); + } + else { + dPlaneSpace (normal,t1,t2); + } + info->J1l[s+0] = t1[0]; + info->J1l[s+1] = t1[1]; + info->J1l[s+2] = t1[2]; + dCROSS (info->J1a+s,=,c1,t1); + if (j->node[1].body) { + info->J2l[s+0] = -t1[0]; + info->J2l[s+1] = -t1[1]; + info->J2l[s+2] = -t1[2]; + dCROSS (info->J2a+s,= -,c2,t1); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion1) { + info->c[1] = j->contact.surface.motion1; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + info->lo[1] = -j->contact.surface.mu; + info->hi[1] = j->contact.surface.mu; + if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip1) + info->cfm[1] = j->contact.surface.slip1; + } + + // second friction direction + if (j->the_m >= 3) { + info->J1l[s2+0] = t2[0]; + info->J1l[s2+1] = t2[1]; + info->J1l[s2+2] = t2[2]; + dCROSS (info->J1a+s2,=,c1,t2); + if (j->node[1].body) { + info->J2l[s2+0] = -t2[0]; + info->J2l[s2+1] = -t2[1]; + info->J2l[s2+2] = -t2[2]; + dCROSS (info->J2a+s2,= -,c2,t2); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion2) { + info->c[2] = j->contact.surface.motion2; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + if (j->contact.surface.mode & dContactMu2) { + info->lo[2] = -j->contact.surface.mu2; + info->hi[2] = j->contact.surface.mu2; + } + else { + info->lo[2] = -j->contact.surface.mu; + info->hi[2] = j->contact.surface.mu; + } + if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip2) + info->cfm[2] = j->contact.surface.slip2; + } +} + + +dxJoint::Vtable __dcontact_vtable = { + sizeof(dxJointContact), + (dxJoint::init_fn*) contactInit, + (dxJoint::getInfo1_fn*) contactGetInfo1, + (dxJoint::getInfo2_fn*) contactGetInfo2, + dJointTypeContact}; + +//**************************************************************************** +// hinge 2. note that this joint must be attached to two bodies for it to work + +static dReal measureHinge2Angle (dxJointHinge2 *joint) +{ + dVector3 a1,a2; + dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2); + dMULTIPLY1_331 (a2,joint->node[0].body->R,a1); + dReal x = dDOT(joint->v1,a2); + dReal y = dDOT(joint->v2,a2); + return -dAtan2 (y,x); +} + + +static void hinge2Init (dxJointHinge2 *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + j->c0 = 0; + j->s0 = 0; + + dSetZero (j->v1,4); + j->v1[0] = 1; + dSetZero (j->v2,4); + j->v2[1] = 1; + + j->limot1.init (j->world); + j->limot2.init (j->world); + + j->susp_erp = j->world->global_erp; + j->susp_cfm = j->world->global_cfm; + + j->flags |= dJOINT_TWOBODIES; +} + + +static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) +{ + info->m = 4; + info->nub = 4; + + // see if we're powered or at a joint limit for axis 1 + int atlimit=0; + if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop) { + dReal angle = measureHinge2Angle (j); + if (j->limot1.testRotationalLimit (angle)) atlimit = 1; + } + if (atlimit || j->limot1.fmax > 0) info->m++; + + // see if we're powering axis 2 (we currently never limit this axis) + j->limot2.limit = 0; + if (j->limot2.fmax > 0) info->m++; +} + + +// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are +// relative to body 1 and 2 initially) and then computes the constrained +// rotational axis as the cross product of ax1 and ax2. +// the sin and cos of the angle between axis 1 and 2 is computed, this comes +// from dot and cross product rules. + +#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ + dVector3 ax1,ax2; \ + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \ + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \ + dCROSS (axis,=,ax1,ax2); \ + sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ + cos_angle = dDOT (ax1,ax2); + + +static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) +{ + // get information we need to set the hinge row + dReal s,c; + dVector3 q; + HINGE2_GET_AXIS_INFO (q,s,c); + dNormalize3 (q); // @@@ quicker: divide q by s ? + + // set the three ball-and-socket rows (aligned to the suspension axis ax1) + setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); + + // set the hinge row + int s3=3*info->rowskip; + info->J1a[s3+0] = q[0]; + info->J1a[s3+1] = q[1]; + info->J1a[s3+2] = q[2]; + if (joint->node[1].body) { + info->J2a[s3+0] = -q[0]; + info->J2a[s3+1] = -q[1]; + info->J2a[s3+2] = -q[2]; + } + + // compute the right hand side for the constrained rotational DOF. + // axis 1 and axis 2 are separated by an angle `theta'. the desired + // separation angle is theta0. sin(theta0) and cos(theta0) are recorded + // in the joint structure. the correcting angular velocity is: + // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize + // = (erp*fps) * (theta0-theta) + // (theta0-theta) can be computed using the following small-angle-difference + // approximation: + // theta0-theta ~= tan(theta0-theta) + // = sin(theta0-theta)/cos(theta0-theta) + // = (c*s0 - s*c0) / (c*c0 + s*s0) + // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 + // where c = cos(theta), s = sin(theta) + // c0 = cos(theta0), s0 = sin(theta0) + + dReal k = info->fps * info->erp; + info->c[3] = k * (joint->c0 * s - joint->s0 * c); + + // if the axis1 hinge is powered, or has joint limits, add in more stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the axis2 hinge is powered, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); + + // set parameter for the suspension + info->cfm[0] = joint->susp_cfm; +} + + +// compute vectors v1 and v2 (embedded in body1), used to measure angle +// between body 1 and body 2 + +static void makeHinge2V1andV2 (dxJointHinge2 *joint) +{ + if (joint->node[0].body) { + // get axis 1 and 2 in global coords + dVector3 ax1,ax2,v; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + + // don't do anything if the axis1 or axis2 vectors are zero or the same + if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || + (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || + (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; + + // modify axis 2 so it's perpendicular to axis 1 + dReal k = dDOT(ax1,ax2); + for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; + dNormalize3 (ax2); + + // make v1 = modified axis2, v2 = axis1 x (modified axis2) + dCROSS (v,=,ax1,ax2); + dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2); + dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v); + } +} + + +extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + if (parameter == dParamSuspensionERP) joint->susp_erp = value; + else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; + else joint->limot1.set (parameter,value); + } +} + + +extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetHinge2Anchor2 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1); + } +} + + +extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2); + } +} + + +extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + if (parameter == dParamSuspensionERP) return joint->susp_erp; + else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; + else return joint->limot1.get (parameter); + } +} + + +extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) return measureHinge2Angle (joint); + else return 0; +} + + +extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body && joint->node[1].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dReal torque2) +{ + dVector3 axis1, axis2; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + + if (joint->node[0].body && joint->node[1].body) { + dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); + } +} + + +dxJoint::Vtable __dhinge2_vtable = { + sizeof(dxJointHinge2), + (dxJoint::init_fn*) hinge2Init, + (dxJoint::getInfo1_fn*) hinge2GetInfo1, + (dxJoint::getInfo2_fn*) hinge2GetInfo2, + dJointTypeHinge2}; + +//**************************************************************************** +// universal + +// I just realized that the universal joint is equivalent to a hinge 2 joint with +// perfectly stiff suspension. By comparing the hinge 2 implementation to +// the universal implementation, you may be able to improve this +// implementation (or, less likely, the hinge2 implementation). + +static void universalInit (dxJointUniversal *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + dSetZero(j->qrel1,4); + dSetZero(j->qrel2,4); + j->limot1.init (j->world); + j->limot2.init (j->world); +} + + +static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) +{ + // This says "ax1 = joint->node[0].body->R * joint->axis1" + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } +} + + +static dReal getUniversalAngle1(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ (R,qcross); + + // This code is essential the same as getHingeAngle(), see the comments + // there for details. + + // get qrel = relative rotation between node[0] and the cross + dQMultiply1 (qq,joint->node[0].body->q,qcross); + dQMultiply2 (qrel,qq,joint->qrel1); + + return getHingeAngleFromRelativeQuat(qrel, joint->axis1); + } + return 0; +} + + +static dReal getUniversalAngle2(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + + if (joint->node[1].body) { + dQMultiply1 (qq, joint->node[1].body->q, qcross); + dQMultiply2 (qrel,qq,joint->qrel2); + } + else { + // pretend joint->node[1].body->q is the identity + dQMultiply2 (qrel,qcross, joint->qrel2); + } + + return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); + } + return 0; +} + + +static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) +{ + info->nub = 4; + info->m = 4; + + // see if we're powered or at a joint limit. + bool constraint1 = j->limot1.fmax > 0; + bool constraint2 = j->limot2.fmax > 0; + + bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop; + bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) && + j->limot2.lostop <= j->limot2.histop; + + // We need to call testRotationLimit() even if we're motored, since it + // records the result. + if (limiting1 || limiting2) { + dReal angle1, angle2; + angle1 = getUniversalAngle1(j); + angle2 = getUniversalAngle2(j); + if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; + if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; + } + if (constraint1) + info->m++; + if (constraint2) + info->m++; +} + + +static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) +{ + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + // set the universal joint row. the angular velocity about an axis + // perpendicular to both joint axes should be equal. thus the constraint + // equation is + // p*w1 - p*w2 = 0 + // where p is a vector normal to both joint axes, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dVector3 ax2_temp; + // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate + // about this. + dVector3 p; + dReal k; + + getUniversalAxes(joint, ax1, ax2); + k = dDOT(ax1, ax2); + ax2_temp[0] = ax2[0] - k*ax1[0]; + ax2_temp[1] = ax2[1] - k*ax1[1]; + ax2_temp[2] = ax2[2] - k*ax1[2]; + dCROSS(p, =, ax1, ax2_temp); + dNormalize3(p); + + int s3=3*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p to bring the axes back to perpendicular. + // If ax1, ax2 are unit length joint axes as computed from body1 and + // body2, we need to rotate both bodies along the axis p. If theta + // is the angle between ax1 and ax2, we need an angular velocity + // along p to cover the angle erp * (theta - Pi/2) in one step: + // + // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize + // = (erp*fps) * (theta - Pi/2) + // + // if theta is close to Pi/2, + // theta - Pi/2 ~= cos(theta), so + // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) + + info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); + + // if the first angle is powered, or has joint limits, add in the stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the second angle is powered, or has joint limits, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); +} + + +static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) +{ + if (joint->node[0].body) { + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross; + + getUniversalAxes(joint, ax1, ax2); + + // Axis 1. + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ(R, qcross); + dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); + + // Axis 2. + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + if (joint->node[1].body) { + dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); + } + else { + // set joint->qrel to qcross + for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; + } + } +} + + +extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,NULL,joint->axis2); + else + setAxes (joint,x,y,z,joint->axis1,NULL); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,joint->axis1,NULL); + else + setAxes (joint,x,y,z,NULL,joint->axis2); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetUniversalAnchor2 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,result,joint->axis2); + else + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,result,joint->axis1); + else + getAxis2 (joint,result,joint->axis2); +} + + +extern "C" void dJointSetUniversalParam (dxJointUniversal *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + joint->limot1.set (parameter,value); + } +} + + +extern "C" dReal dJointGetUniversalParam (dxJointUniversal *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + return joint->limot1.get (parameter); + } +} + + +extern "C" dReal dJointGetUniversalAngle1 (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle2 (joint); + else + return getUniversalAngle1 (joint); +} + + +extern "C" dReal dJointGetUniversalAngle2 (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle1 (joint); + else + return getUniversalAngle2 (joint); +} + + +extern "C" dReal dJointGetUniversalAngle1Rate (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,axis,joint->axis2); + else + getAxis (joint,axis,joint->axis1); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +extern "C" dReal dJointGetUniversalAngle2Rate (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,axis,joint->axis1); + else + getAxis2 (joint,axis,joint->axis2); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +extern "C" void dJointAddUniversalTorques (dxJointUniversal *joint, dReal torque1, dReal torque2) +{ + dVector3 axis1, axis2; + dAASSERT(joint); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->flags & dJOINT_REVERSE) { + dReal temp = torque1; + torque1 = - torque2; + torque2 = - temp; + } + + getAxis (joint,axis1,joint->axis1); + getAxis2 (joint,axis2,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); +} + + + + + +dxJoint::Vtable __duniversal_vtable = { + sizeof(dxJointUniversal), + (dxJoint::init_fn*) universalInit, + (dxJoint::getInfo1_fn*) universalGetInfo1, + (dxJoint::getInfo2_fn*) universalGetInfo2, + dJointTypeUniversal}; + +//**************************************************************************** +// angular motor + +static void amotorInit (dxJointAMotor *j) +{ + int i; + j->num = 0; + j->mode = dAMotorUser; + for (i=0; i<3; i++) { + j->rel[i] = 0; + dSetZero (j->axis[i],4); + j->limot[i].init (j->world); + j->angle[i] = 0; + } + dSetZero (j->reference1,4); + dSetZero (j->reference2,4); + + j->flags |= dJOINT_TWOBODIES; +} + + +// compute the 3 axes in global coordinates + +static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) +{ + if (joint->mode == dAMotorEuler) { + // special handling for euler mode + dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]); + dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]); + dCROSS (ax[1],=,ax[2],ax[0]); + dNormalize3 (ax[1]); + } + else { + for (int i=0; i < joint->num; i++) { + if (joint->rel[i] == 1) { + // relative to b1 + dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]); + } + if (joint->rel[i] == 2) { + // relative to b2 + dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]); + } + else { + // global - just copy it + ax[i][0] = joint->axis[i][0]; + ax[i][1] = joint->axis[i][1]; + ax[i][2] = joint->axis[i][2]; + } + } + } +} + + +static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) +{ + // assumptions: + // global axes already calculated --> ax + // axis[0] is relative to body 1 --> global ax[0] + // axis[2] is relative to body 2 --> global ax[2] + // ax[1] = ax[2] x ax[0] + // original ax[0] and ax[2] are perpendicular + // reference1 is perpendicular to ax[0] (in body 1 frame) + // reference2 is perpendicular to ax[2] (in body 2 frame) + // all ax[] and reference vectors are unit length + + // calculate references in global frame + dVector3 ref1,ref2; + dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1); + dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2); + + // get q perpendicular to both ax[0] and ref1, get first euler angle + dVector3 q; + dCROSS (q,=,ax[0],ref1); + joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); + + // get q perpendicular to both ax[0] and ax[1], get second euler angle + dCROSS (q,=,ax[0],ax[1]); + joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); + + // get q perpendicular to both ax[1] and ax[2], get third euler angle + dCROSS (q,=,ax[1],ax[2]); + joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); +} + + +// set the reference vectors as follows: +// * reference1 = current axis[2] relative to body 1 +// * reference2 = current axis[0] relative to body 2 +// this assumes that: +// * axis[0] is relative to body 1 +// * axis[2] is relative to body 2 + +static void amotorSetEulerReferenceVectors (dxJointAMotor *j) +{ + if (j->node[0].body && j->node[1].body) { + dVector3 r; // axis[2] and axis[0] in global coordinates + dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]); + dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r); + dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]); + dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r); + } +} + + +static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; + + // compute the axes and angles, if in euler mode + if (j->mode == dAMotorEuler) { + dVector3 ax[3]; + amotorComputeGlobalAxes (j,ax); + amotorComputeEulerAngles (j,ax); + } + + // see if we're powered or at a joint limit for each axis + for (int i=0; i < j->num; i++) { + if (j->limot[i].testRotationalLimit (j->angle[i]) || + j->limot[i].fmax > 0) { + info->m++; + } + } +} + + +static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) +{ + int i; + + // compute the axes (if not global) + dVector3 ax[3]; + amotorComputeGlobalAxes (joint,ax); + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + dVector3 *axptr[3]; + axptr[0] = &ax[0]; + axptr[1] = &ax[1]; + axptr[2] = &ax[2]; + + dVector3 ax0_cross_ax1; + dVector3 ax1_cross_ax2; + if (joint->mode == dAMotorEuler) { + dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); + axptr[2] = &ax0_cross_ax1; + dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); + axptr[0] = &ax1_cross_ax2; + } + + int row=0; + for (i=0; i < joint->num; i++) { + row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); + } +} + + +extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num) +{ + dAASSERT(joint && num >= 0 && num <= 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorEuler) { + joint->num = 3; + } + else { + if (num < 0) num = 0; + if (num > 3) num = 3; + joint->num = num; + } +} + + +extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel, + dReal x, dReal y, dReal z) +{ + dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + joint->rel[anum] = rel; + + // x,y,z is always in global coordinates regardless of rel, so we may have + // to convert it to be relative to a body + dVector3 r; + r[0] = x; + r[1] = y; + r[2] = z; + r[3] = 0; + if (rel > 0) { + if (rel==1) { + dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r); + } + else { + dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r); + } + } + else { + joint->axis[anum][0] = r[0]; + joint->axis[anum][1] = r[1]; + joint->axis[anum][2] = r[2]; + } + dNormalize3 (joint->axis[anum]); + if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); +} + + +extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum, + dReal angle) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorUser) { + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + joint->angle[anum] = angle; + } +} + + +extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter, + dReal value) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + joint->limot[anum].set (parameter, value); +} + + +extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + joint->mode = mode; + if (joint->mode == dAMotorEuler) { + joint->num = 3; + amotorSetEulerReferenceVectors (joint); + } +} + + +extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->num; +} + + +extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum, + dVector3 result) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + if (joint->rel[anum] > 0) { + if (joint->rel[anum]==1) { + dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]); + } + else { + dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]); + } + } + else { + result[0] = joint->axis[anum][0]; + result[1] = joint->axis[anum][1]; + result[2] = joint->axis[anum][2]; + } +} + + +extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + return joint->rel[anum]; +} + + +extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + return joint->angle[anum]; +} + + +extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum) +{ + // @@@ + dDebug (0,"not yet implemented"); + return 0; +} + + +extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + return joint->limot[anum].get (parameter); +} + + +extern "C" int dJointGetAMotorMode (dxJointAMotor *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->mode; +} + + +extern "C" void dJointAddAMotorTorques (dxJointAMotor *joint, dReal torque1, dReal torque2, dReal torque3) +{ + dVector3 axes[3]; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + + if (joint->num == 0) + return; + dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints"); + + amotorComputeGlobalAxes (joint,axes); + axes[0][0] *= torque1; + axes[0][1] *= torque1; + axes[0][2] *= torque1; + if (joint->num >= 2) { + axes[0][0] += axes[1][0] * torque2; + axes[0][1] += axes[1][0] * torque2; + axes[0][2] += axes[1][0] * torque2; + if (joint->num >= 3) { + axes[0][0] += axes[2][0] * torque3; + axes[0][1] += axes[2][0] * torque3; + axes[0][2] += axes[2][0] * torque3; + } + } + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); +} + + +dxJoint::Vtable __damotor_vtable = { + sizeof(dxJointAMotor), + (dxJoint::init_fn*) amotorInit, + (dxJoint::getInfo1_fn*) amotorGetInfo1, + (dxJoint::getInfo2_fn*) amotorGetInfo2, + dJointTypeAMotor}; + +//**************************************************************************** +// fixed joint + +static void fixedInit (dxJointFixed *j) +{ + dSetZero (j->offset,4); + dSetZero (j->qrel,4); +} + + +static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) +{ + info->m = 6; + info->nub = 6; +} + + +static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) +{ + int s = info->rowskip; + + // Three rows for orientation + setFixedOrientation(joint, info, joint->qrel, 3); + + // Three rows for position. + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + dVector3 ofs; + dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset); + if (joint->node[1].body) { + dCROSSMAT (info->J1a,ofs,s,+,-); + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + } + + // set right hand side for the first three rows (linear) + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->node[1].body->pos[j] - + joint->node[0].body->pos[j] + ofs[j]); + } + else { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]); + } +} + + +extern "C" void dJointSetFixed (dxJointFixed *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); + int i; + + // This code is taken from sJointSetSliderAxis(), we should really put the + // common code in its own function. + // compute the offset between the bodies + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dReal ofs[4]; + for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; + for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; + } + } +} + + +dxJoint::Vtable __dfixed_vtable = { + sizeof(dxJointFixed), + (dxJoint::init_fn*) fixedInit, + (dxJoint::getInfo1_fn*) fixedGetInfo1, + (dxJoint::getInfo2_fn*) fixedGetInfo2, + dJointTypeFixed}; + +//**************************************************************************** +// null joint + +static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; +} + + +static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) +{ + dDebug (0,"this should never get called"); +} + + +dxJoint::Vtable __dnull_vtable = { + sizeof(dxJointNull), + (dxJoint::init_fn*) 0, + (dxJoint::getInfo1_fn*) nullGetInfo1, + (dxJoint::getInfo2_fn*) nullGetInfo2, + dJointTypeNull}; + +/******************** breakable joint contribution ***********************/ +extern "C" void dJointSetBreakable (dxJoint *joint, int b) { + dAASSERT(joint); + if (b) { + // we want this joint to be breakable but we must first check if it + // was already breakable + if (!joint->breakInfo) { + // allocate a dxJointBreakInfo struct + joint->breakInfo = new dxJointBreakInfo; + joint->breakInfo->flags = 0; + for (int i = 0; i < 3; i++) { + joint->breakInfo->b1MaxF[0] = 0; + joint->breakInfo->b1MaxT[0] = 0; + joint->breakInfo->b2MaxF[0] = 0; + joint->breakInfo->b2MaxT[0] = 0; + } + joint->breakInfo->callback = 0; + } + else { + // the joint was already breakable + return; + } + } + else { + // we want this joint to be unbreakable mut we must first check if + // it is alreay unbreakable + if (joint->breakInfo) { + // deallocate the dxJointBreakInfo struct + delete joint->breakInfo; + joint->breakInfo = 0; + } + else { + // the joint was already unbreakable + return; + } + } +} + +extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakCallback called on unbreakable joint"); + } +# endif + joint->breakInfo->callback = callbackFunc; +} + +extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakMode called on unbreakable joint"); + } +# endif + joint->breakInfo->flags = mode; +} + +extern "C" int dJointGetBreakMode (dxJoint *joint) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointGetBreakMode called on unbreakable joint"); + } +# endif + return joint->breakInfo->flags; +} + +extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakForce called on unbreakable joint"); + } +# endif + if (body) { + joint->breakInfo->b2MaxF[0] = x; + joint->breakInfo->b2MaxF[1] = y; + joint->breakInfo->b2MaxF[2] = z; + } + else { + joint->breakInfo->b1MaxF[0] = x; + joint->breakInfo->b1MaxF[1] = y; + joint->breakInfo->b1MaxF[2] = z; + } +} + +extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakTorque called on unbreakable joint"); + } +# endif + if (body) { + joint->breakInfo->b2MaxT[0] = x; + joint->breakInfo->b2MaxT[1] = y; + joint->breakInfo->b2MaxT[2] = z; + } + else { + joint->breakInfo->b1MaxT[0] = x; + joint->breakInfo->b1MaxT[1] = y; + joint->breakInfo->b1MaxT[2] = z; + } +} + +extern "C" int dJointIsBreakable (dxJoint *joint) { + dAASSERT(joint); + return joint->breakInfo != 0; +} + +extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointGetBreakForce called on unbreakable joint"); + } +# endif + if (body) + for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i]; + else + for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i]; +} + +extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointGetBreakTorque called on unbreakable joint"); + } +# endif + if (body) + for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i]; + else + for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i]; +} +/*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/joint.h b/libraries/ode-0.9/contrib/BreakableJoints/joint.h new file mode 100644 index 0000000..0573119 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/joint.h @@ -0,0 +1,282 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library 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 files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_H_ +#define _ODE_JOINT_H_ + + +#include "objects.h" +#include +#include "obstack.h" + + +// joint flags +enum { + // if this flag is set, the joint was allocated in a joint group + dJOINT_INGROUP = 1, + + // if this flag is set, the joint was attached with arguments (0,body). + // our convention is to treat all attaches as (body,0), i.e. so node[0].body + // is always nonzero, so this flag records the fact that the arguments were + // swapped. + dJOINT_REVERSE = 2, + + // if this flag is set, the joint can not have just one body attached to it, + // it must have either zero or two bodies attached. + dJOINT_TWOBODIES = 4 +}; + + +// there are two of these nodes in the joint, one for each connection to a +// body. these are node of a linked list kept by each body of it's connecting +// joints. but note that the body pointer in each node points to the body that +// makes use of the *other* node, not this node. this trick makes it a bit +// easier to traverse the body/joint graph. + +struct dxJointNode { + dxJoint *joint; // pointer to enclosing dxJoint object + dxBody *body; // *other* body this joint is connected to + dxJointNode *next; // next node in body's list of connected joints +}; + +/******************** breakable joint contribution ***********************/ +struct dxJointBreakInfo : public dBase { + int flags; + dReal b1MaxF[3]; // maximum force on body 1 + dReal b1MaxT[3]; // maximum torque on body 1 + dReal b2MaxF[3]; // maximum force on body 2 + dReal b2MaxT[3]; // maximum torque on body 2 + dJointBreakCallback *callback; // function that is called when this joint breaks +}; +/*************************************************************************/ + +struct dxJoint : public dObject { + // naming convention: the "first" body this is connected to is node[0].body, + // and the "second" body is node[1].body. if this joint is only connected + // to one body then the second body is 0. + + // info returned by getInfo1 function. the constraint dimension is m (<=6). + // i.e. that is the total number of rows in the jacobian. `nub' is the + // number of unbounded variables (which have lo,hi = -/+ infinity). + + struct Info1 { + int m,nub; + }; + + // info returned by getInfo2 function + + struct Info2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + dReal fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + dReal *J1l,*J1a,*J2l,*J2a; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + dReal *c,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + dReal *lo,*hi; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + + // virtual function table: size of the joint structure, function pointers. + // we do it this way instead of using C++ virtual functions because + // sometimes we need to allocate joints ourself within a memory pool. + + typedef void init_fn (dxJoint *joint); + typedef void getInfo1_fn (dxJoint *joint, Info1 *info); + typedef void getInfo2_fn (dxJoint *joint, Info2 *info); + struct Vtable { + int size; + init_fn *init; + getInfo1_fn *getInfo1; + getInfo2_fn *getInfo2; + int typenum; // a dJointTypeXXX type number + }; + + Vtable *vtable; // virtual function table + int flags; // dJOINT_xxx flags + dxJointNode node[2]; // connections to bodies. node[1].body can be 0 + dJointFeedback *feedback; // optional feedback structure + + /******************** breakable joint contribution ***********************/ + // optional break info structure. if this is not NULL the the joint is + // breakable. + dxJointBreakInfo *breakInfo; + /*************************************************************************/ +}; + + +// joint group. NOTE: any joints in the group that have their world destroyed +// will have their world pointer set to 0. + +struct dxJointGroup : public dBase { + int num; // number of joints on the stack + dObStack stack; // a stack of (possibly differently sized) dxJoint +}; // objects. + + +// common limit and motor information for a single joint axis of movement +struct dxJointLimitMotor { + dReal vel,fmax; // powered joint: velocity, max force + dReal lostop,histop; // joint limits, relative to initial position + dReal fudge_factor; // when powering away from joint limits + dReal normal_cfm; // cfm to use when not at a stop + dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit + dReal bounce; // restitution factor + // variables used between getInfo1() and getInfo2() + int limit; // 0=free, 1=at lo limit, 2=at hi limit + dReal limit_err; // if at limit, amount over limit + + void init (dxWorld *); + void set (int num, dReal value); + dReal get (int num); + int testRotationalLimit (dReal angle); + int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational); +}; + + +// ball and socket + +struct dxJointBall : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body +}; +extern struct dxJoint::Vtable __dball_vtable; + + +// hinge + +struct dxJointHinge : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dhinge_vtable; + + +// universal + +struct dxJointUniversal : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece + dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2 + dxJointLimitMotor limot1; // limit and motor information for axis1 + dxJointLimitMotor limot2; // limit and motor information for axis2 +}; +extern struct dxJoint::Vtable __duniversal_vtable; + + +// slider. if body2 is 0 then qrel is the absolute rotation of body1 and +// offset is the position of body1 center along axis1. + +struct dxJointSlider : public dxJoint { + dVector3 axis1; // axis w.r.t first body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // point relative to body2 that should be + // aligned with body1 center along axis1 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dslider_vtable; + + +// contact + +struct dxJointContact : public dxJoint { + int the_m; // number of rows computed by getInfo1 + dContact contact; +}; +extern struct dxJoint::Vtable __dcontact_vtable; + + +// hinge 2 + +struct dxJointHinge2 : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis 1 w.r.t first body + dVector3 axis2; // axis 2 w.r.t second body + dReal c0,s0; // cos,sin of desired angle between axis 1,2 + dVector3 v1,v2; // angle ref vectors embedded in first body + dxJointLimitMotor limot1; // limit+motor info for axis 1 + dxJointLimitMotor limot2; // limit+motor info for axis 2 + dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm) +}; +extern struct dxJoint::Vtable __dhinge2_vtable; + + +// angular motor + +struct dxJointAMotor : public dxJoint { + int num; // number of axes (0..3) + int mode; // a dAMotorXXX constant + int rel[3]; // what the axes are relative to (global,b1,b2) + dVector3 axis[3]; // three axes + dxJointLimitMotor limot[3]; // limit+motor info for axes + dReal angle[3]; // user-supplied angles for axes + // these vectors are used for calculating euler angles + dVector3 reference1; // original axis[2], relative to body 1 + dVector3 reference2; // original axis[0], relative to body 2 +}; +extern struct dxJoint::Vtable __damotor_vtable; + + +// fixed + +struct dxJointFixed : public dxJoint { + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // relative offset between the bodies +}; +extern struct dxJoint::Vtable __dfixed_vtable; + + +// null joint, for testing only + +struct dxJointNull : public dxJoint { +}; +extern struct dxJoint::Vtable __dnull_vtable; + + + +#endif diff --git a/libraries/ode-0.9/contrib/BreakableJoints/objects.h b/libraries/ode-0.9/contrib/BreakableJoints/objects.h new file mode 100644 index 0000000..de08391 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/objects.h @@ -0,0 +1,252 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library 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 files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_OBJECTS_H_ +#define _ODE_OBJECTS_H_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* world */ + +dWorldID dWorldCreate(); +void dWorldDestroy (dWorldID); + +void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z); +void dWorldGetGravity (dWorldID, dVector3 gravity); +void dWorldSetERP (dWorldID, dReal erp); +dReal dWorldGetERP (dWorldID); +void dWorldSetCFM (dWorldID, dReal cfm); +dReal dWorldGetCFM (dWorldID); +void dWorldStep (dWorldID, dReal stepsize); +void dWorldImpulseToForce (dWorldID, dReal stepsize, + dReal ix, dReal iy, dReal iz, dVector3 force); + +/* StepFast1 functions */ + +void dWorldStepFast1(dWorldID, dReal stepsize, int maxiterations); +void dWorldSetAutoEnableDepthSF1(dWorldID, int autoEnableDepth); + +int dWorldGetAutoEnableDepthSF1(dWorldID); + +void dBodySetAutoDisableThresholdSF1(dBodyID, dReal autoDisableThreshold); + +/* These functions are not yet implemented by ODE. */ +/* +dReal dBodyGetAutoDisableThresholdSF1(dBodyID); + +void dBodySetAutoDisableStepsSF1(dBodyID, int AutoDisableSteps); + +int dBodyGetAutoDisableStepsSF1(dBodyID); + +void dBodySetAutoDisableSF1(dBodyID, int doAutoDisable); + +int dBodyGetAutoDisableSF1(dBodyID); +*/ + +/* bodies */ + +dBodyID dBodyCreate (dWorldID); +void dBodyDestroy (dBodyID); + +void dBodySetData (dBodyID, void *data); +void *dBodyGetData (dBodyID); + +void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z); +void dBodySetRotation (dBodyID, const dMatrix3 R); +void dBodySetQuaternion (dBodyID, const dQuaternion q); +void dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z); +void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z); +const dReal * dBodyGetPosition (dBodyID); +const dReal * dBodyGetRotation (dBodyID); /* ptr to 4x3 rot matrix */ +const dReal * dBodyGetQuaternion (dBodyID); +const dReal * dBodyGetLinearVel (dBodyID); +const dReal * dBodyGetAngularVel (dBodyID); + +void dBodySetMass (dBodyID, const dMass *mass); +void dBodyGetMass (dBodyID, dMass *mass); + +void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); + +const dReal * dBodyGetForce (dBodyID); +const dReal * dBodyGetTorque (dBodyID); +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z); +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z); + +void dBodyGetRelPointPos (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetRelPointVel (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetPointVel (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetPosRelPoint (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyVectorToWorld (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyVectorFromWorld (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); + +void dBodySetFiniteRotationMode (dBodyID, int mode); +void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z); + +int dBodyGetFiniteRotationMode (dBodyID); +void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result); + +int dBodyGetNumJoints (dBodyID b); +dJointID dBodyGetJoint (dBodyID, int index); + +void dBodyEnable (dBodyID); +void dBodyDisable (dBodyID); +int dBodyIsEnabled (dBodyID); + +void dBodySetGravityMode (dBodyID b, int mode); +int dBodyGetGravityMode (dBodyID b); + + +/* joints */ + +dJointID dJointCreateBall (dWorldID, dJointGroupID); +dJointID dJointCreateHinge (dWorldID, dJointGroupID); +dJointID dJointCreateSlider (dWorldID, dJointGroupID); +dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *); +dJointID dJointCreateHinge2 (dWorldID, dJointGroupID); +dJointID dJointCreateUniversal (dWorldID, dJointGroupID); +dJointID dJointCreateFixed (dWorldID, dJointGroupID); +dJointID dJointCreateNull (dWorldID, dJointGroupID); +dJointID dJointCreateAMotor (dWorldID, dJointGroupID); + +void dJointDestroy (dJointID); + +dJointGroupID dJointGroupCreate (int max_size); +void dJointGroupDestroy (dJointGroupID); +void dJointGroupEmpty (dJointGroupID); + +void dJointAttach (dJointID, dBodyID body1, dBodyID body2); +void dJointSetData (dJointID, void *data); +void *dJointGetData (dJointID); +int dJointGetType (dJointID); +dBodyID dJointGetBody (dJointID, int index); + +void dJointSetFeedback (dJointID, dJointFeedback *); +dJointFeedback *dJointGetFeedback (dJointID); + +/******************** breakable joint contribution ***********************/ +void dJointSetBreakable (dJointID, int b); +void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc); +void dJointSetBreakMode (dJointID, int mode); +int dJointGetBreakMode (dJointID); +void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z); +void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z); +int dJointIsBreakable (dJointID); +void dJointGetBreakForce (dJointID, int body, dReal *force); +void dJointGetBreakTorque (dJointID, int body, dReal *torque); +/*************************************************************************/ + +void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeParam (dJointID, int parameter, dReal value); +void dJointAddHingeTorque(dJointID joint, dReal torque); +void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z); +void dJointSetSliderParam (dJointID, int parameter, dReal value); +void dJointAddSliderForce(dJointID joint, dReal force); +void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Param (dJointID, int parameter, dReal value); +void dJointAddHinge2Torques(dJointID joint, dReal torque1, dReal torque2); +void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalParam (dJointID, int parameter, dReal value); +void dJointAddUniversalTorques(dJointID joint, dReal torque1, dReal torque2); +void dJointSetFixed (dJointID); +void dJointSetAMotorNumAxes (dJointID, int num); +void dJointSetAMotorAxis (dJointID, int anum, int rel, + dReal x, dReal y, dReal z); +void dJointSetAMotorAngle (dJointID, int anum, dReal angle); +void dJointSetAMotorParam (dJointID, int parameter, dReal value); +void dJointSetAMotorMode (dJointID, int mode); +void dJointAddAMotorTorques (dJointID, dReal torque1, dReal torque2, dReal torque3); + +void dJointGetBallAnchor (dJointID, dVector3 result); +void dJointGetBallAnchor2 (dJointID, dVector3 result); +void dJointGetHingeAnchor (dJointID, dVector3 result); +void dJointGetHingeAnchor2 (dJointID, dVector3 result); +void dJointGetHingeAxis (dJointID, dVector3 result); +dReal dJointGetHingeParam (dJointID, int parameter); +dReal dJointGetHingeAngle (dJointID); +dReal dJointGetHingeAngleRate (dJointID); +dReal dJointGetSliderPosition (dJointID); +dReal dJointGetSliderPositionRate (dJointID); +void dJointGetSliderAxis (dJointID, dVector3 result); +dReal dJointGetSliderParam (dJointID, int parameter); +void dJointGetHinge2Anchor (dJointID, dVector3 result); +void dJointGetHinge2Anchor2 (dJointID, dVector3 result); +void dJointGetHinge2Axis1 (dJointID, dVector3 result); +void dJointGetHinge2Axis2 (dJointID, dVector3 result); +dReal dJointGetHinge2Param (dJointID, int parameter); +dReal dJointGetHinge2Angle1 (dJointID); +dReal dJointGetHinge2Angle1Rate (dJointID); +dReal dJointGetHinge2Angle2Rate (dJointID); +void dJointGetUniversalAnchor (dJointID, dVector3 result); +void dJointGetUniversalAnchor2 (dJointID, dVector3 result); +void dJointGetUniversalAxis1 (dJointID, dVector3 result); +void dJointGetUniversalAxis2 (dJointID, dVector3 result); +dReal dJointGetUniversalParam (dJointID, int parameter); +dReal dJointGetUniversalAngle1 (dJointID); +dReal dJointGetUniversalAngle2 (dJointID); +dReal dJointGetUniversalAngle1Rate (dJointID); +dReal dJointGetUniversalAngle2Rate (dJointID); +int dJointGetAMotorNumAxes (dJointID); +void dJointGetAMotorAxis (dJointID, int anum, dVector3 result); +int dJointGetAMotorAxisRel (dJointID, int anum); +dReal dJointGetAMotorAngle (dJointID, int anum); +dReal dJointGetAMotorAngleRate (dJointID, int anum); +dReal dJointGetAMotorParam (dJointID, int parameter); +int dJointGetAMotorMode (dJointID); + +int dAreConnected (dBodyID, dBodyID); +int dAreConnectedExcluding (dBodyID, dBodyID, int joint_type); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp b/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp new file mode 100644 index 0000000..7137960 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp @@ -0,0 +1,1404 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library 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 files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +// this source file is mostly concerned with the data structures, not the +// numerics. + +#include "objects.h" +#include +#include "joint.h" +#include +#include +#include "step.h" +#include +#include + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// utility + +static inline void initObject (dObject *obj, dxWorld *w) +{ + obj->world = w; + obj->next = 0; + obj->tome = 0; + obj->userdata = 0; + obj->tag = 0; +} + + +// add an object `obj' to the list who's head pointer is pointed to by `first'. + +static inline void addObjectToList (dObject *obj, dObject **first) +{ + obj->next = *first; + obj->tome = first; + if (*first) (*first)->tome = &obj->next; + (*first) = obj; +} + + +// remove the object from the linked list + +static inline void removeObjectFromList (dObject *obj) +{ + if (obj->next) obj->next->tome = obj->tome; + *(obj->tome) = obj->next; + // safeguard + obj->next = 0; + obj->tome = 0; +} + + +// remove the joint from neighbour lists of all connected bodies + +static void removeJointReferencesFromAttachedBodies (dxJoint *j) +{ + for (int i=0; i<2; i++) { + dxBody *body = j->node[i].body; + if (body) { + dxJointNode *n = body->firstjoint; + dxJointNode *last = 0; + while (n) { + if (n->joint == j) { + if (last) last->next = n->next; + else body->firstjoint = n->next; + break; + } + last = n; + n = n->next; + } + } + } + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; +} + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +static void processIslands (dxWorld *world, dReal stepsize) +{ + dxBody *b,*bb,**body; + dxJoint *j,**joint; + + // nothing to do if no bodies + if (world->nb <= 0) return; + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*)); + joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + + // set all body/joint tags to 0 + for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; + for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*)); + + for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) { + b = stack[--stacksize]; // pop body off stack + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (!n->joint->tag) { + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) { + n->body->tag = 1; + stack[stacksize++] = n->body; + } + } + } + dIASSERT(stacksize <= world->nb); + dIASSERT(stacksize <= world->nj); + } + + // now do something with body and joint lists + dInternalStepIsland (world,body,bcount,joint,jcount,stepsize); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i=0; itag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i=0; itag = 1; + } + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b=world->firstbody; b; b=(dxBody*)b->next) { + if (b->flags & dxBodyDisabled) { + if (b->tag) dDebug (0,"disabled body tagged"); + } + else { + if (!b->tag) dDebug (0,"enabled body not tagged"); + } + } + for (j=world->firstjoint; j; j=(dxJoint*)j->next) { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) || + (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) { + if (!j->tag) dDebug (0,"attached enabled joint not tagged"); + } + else { + if (j->tag) dDebug (0,"unattached or disabled joint tagged"); + } + } +# endif + /******************** breakable joint contribution ***********************/ + dxJoint* nextJ; + if (!world->firstjoint) + nextJ = 0; + else + nextJ = (dxJoint*)world->firstjoint->next; + for (j=world->firstjoint; j; j=nextJ) { + nextJ = (dxJoint*)j->next; + // check if joint is breakable and broken + if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { + // detach (break) the joint + dJointAttach (j, 0, 0); + // call the callback function if it is set + if (j->breakInfo->callback) j->breakInfo->callback (j); + // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set + if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); + } + } + /*************************************************************************/ +} + +//**************************************************************************** +// debugging + +// see if an object list loops on itself (if so, it's bad). + +static int listHasLoops (dObject *first) +{ + if (first==0 || first->next==0) return 0; + dObject *a=first,*b=first->next; + int skip=0; + while (b) { + if (a==b) return 1; + b = b->next; + if (skip) a = a->next; + skip ^= 1; + } + return 0; +} + + +// check the validity of the world data structures + +static void checkWorld (dxWorld *w) +{ + dxBody *b; + dxJoint *j; + + // check there are no loops + if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops"); + if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops"); + + // check lists are well formed (check `tome' pointers) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + if (b->next && b->next->tome != &b->next) + dDebug (0,"bad tome pointer in body list"); + } + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->next && j->next->tome != &j->next) + dDebug (0,"bad tome pointer in joint list"); + } + + // check counts + int n = 0; + for (b=w->firstbody; b; b=(dxBody*)b->next) n++; + if (w->nb != n) dDebug (0,"body count incorrect"); + n = 0; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++; + if (w->nj != n) dDebug (0,"joint count incorrect"); + + // set all tag values to a known value + static int count = 0; + count++; + for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count; + + // check all body/joint world pointers are ok + for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w) + dDebug (0,"bad world pointer in body list"); + for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w) + dDebug (0,"bad world pointer in joint list"); + + /* + // check for half-connected joints - actually now these are valid + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body || j->node[1].body) { + if (!(j->node[0].body && j->node[1].body)) + dDebug (0,"half connected joint found"); + } + } + */ + + // check that every joint node appears in the joint lists of both bodies it + // attaches + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + for (int i=0; i<2; i++) { + if (j->node[i].body) { + int ok = 0; + for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) { + if (n->joint == j) ok = 1; + } + if (ok==0) dDebug (0,"joint not in joint list of attached body"); + } + } + } + + // check all body joint lists (correct body ptrs) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (&n->joint->node[0] == n) { + if (n->joint->node[1].body != b) + dDebug (0,"bad body pointer in joint node of body list (1)"); + } + else { + if (n->joint->node[0].body != b) + dDebug (0,"bad body pointer in joint node of body list (2)"); + } + if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body"); + } + } + + // check all body pointers in joints, check they are distinct + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body && (j->node[0].body == j->node[1].body)) + dDebug (0,"non-distinct body pointers in joint"); + if ((j->node[0].body && j->node[0].body->tag != count) || + (j->node[1].body && j->node[1].body->tag != count)) + dDebug (0,"bad body pointer in joint"); + } +} + + +void dWorldCheck (dxWorld *w) +{ + checkWorld (w); +} + +//**************************************************************************** +// body + +dxBody *dBodyCreate (dxWorld *w) +{ + dAASSERT (w); + dxBody *b = new dxBody; + initObject (b,w); + b->firstjoint = 0; + b->flags = 0; + b->geom = 0; + dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0); + dSetZero (b->invI,4*3); + b->invI[0] = 1; + b->invI[5] = 1; + b->invI[10] = 1; + b->invMass = 1; + dSetZero (b->pos,4); + dSetZero (b->q,4); + b->q[0] = 1; + dRSetIdentity (b->R); + dSetZero (b->lvel,4); + dSetZero (b->avel,4); + dSetZero (b->facc,4); + dSetZero (b->tacc,4); + dSetZero (b->finite_rot_axis,4); + addObjectToList (b,(dObject **) &w->firstbody); + w->nb++; + return b; +} + + +void dBodyDestroy (dxBody *b) +{ + dAASSERT (b); + + // all geoms that link to this body must be notified that the body is about + // to disappear. note that the call to dGeomSetBody(geom,0) will result in + // dGeomGetBodyNext() returning 0 for the body, so we must get the next body + // before setting the body to 0. + dxGeom *next_geom = 0; + for (dxGeom *geom = b->geom; geom; geom = next_geom) { + next_geom = dGeomGetBodyNext (geom); + dGeomSetBody (geom,0); + } + + // detach all neighbouring joints, then delete this body. + dxJointNode *n = b->firstjoint; + while (n) { + // sneaky trick to speed up removal of joint references (black magic) + n->joint->node[(n == n->joint->node)].body = 0; + + dxJointNode *next = n->next; + n->next = 0; + removeJointReferencesFromAttachedBodies (n->joint); + n = next; + } + removeObjectFromList (b); + b->world->nb--; + delete b; +} + + +void dBodySetData (dBodyID b, void *data) +{ + dAASSERT (b); + b->userdata = data; +} + + +void *dBodyGetData (dBodyID b) +{ + dAASSERT (b); + return b->userdata; +} + + +void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->pos[0] = x; + b->pos[1] = y; + b->pos[2] = z; + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetRotation (dBodyID b, const dMatrix3 R) +{ + dAASSERT (b && R); + dQuaternion q; + dRtoQ (R,q); + dNormalize4 (q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetQuaternion (dBodyID b, const dQuaternion q) +{ + dAASSERT (b && q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dNormalize4 (b->q); + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->lvel[0] = x; + b->lvel[1] = y; + b->lvel[2] = z; +} + + +void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->avel[0] = x; + b->avel[1] = y; + b->avel[2] = z; +} + + +const dReal * dBodyGetPosition (dBodyID b) +{ + dAASSERT (b); + return b->pos; +} + + +const dReal * dBodyGetRotation (dBodyID b) +{ + dAASSERT (b); + return b->R; +} + + +const dReal * dBodyGetQuaternion (dBodyID b) +{ + dAASSERT (b); + return b->q; +} + + +const dReal * dBodyGetLinearVel (dBodyID b) +{ + dAASSERT (b); + return b->lvel; +} + + +const dReal * dBodyGetAngularVel (dBodyID b) +{ + dAASSERT (b); + return b->avel; +} + + +void dBodySetMass (dBodyID b, const dMass *mass) +{ + dAASSERT (b && mass); + memcpy (&b->mass,mass,sizeof(dMass)); + if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) { + dDEBUGMSG ("inertia must be positive definite"); + dRSetIdentity (b->invI); + } + b->invMass = dRecip(b->mass.mass); +} + + +void dBodyGetMass (dBodyID b, dMass *mass) +{ + dAASSERT (b && mass); + memcpy (mass,&b->mass,sizeof(dMass)); +} + + +void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; +} + + +void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->tacc[0] += fx; + b->tacc[1] += fy; + b->tacc[2] += fz; +} + + +void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->R,t1); + b->facc[0] += t2[0]; + b->facc[1] += t2[1]; + b->facc[2] += t2[2]; +} + + +void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->R,t1); + b->tacc[0] += t2[0]; + b->tacc[1] += t2[1]; + b->tacc[2] += t2[2]; +} + + +void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; + dVector3 f,q; + f[0] = fx; + f[1] = fy; + f[2] = fz; + q[0] = px - b->pos[0]; + q[1] = py - b->pos[1]; + q[2] = pz - b->pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 prel,f,p; + f[0] = fx; + f[1] = fy; + f[2] = fz; + f[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,f; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + dMULTIPLY0_331 (f,b->R,frel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dVector3 q; + q[0] = px - b->pos[0]; + q[1] = py - b->pos[1]; + q[2] = pz - b->pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,prel,f,p; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (f,b->R,frel); + dMULTIPLY0_331 (p,b->R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +const dReal * dBodyGetForce (dBodyID b) +{ + dAASSERT (b); + return b->facc; +} + + +const dReal * dBodyGetTorque (dBodyID b) +{ + dAASSERT (b); + return b->tacc; +} + + +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->facc[0] = x; + b->facc[1] = y; + b->facc[2] = z; +} + + +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->tacc[0] = x; + b->tacc[1] = y; + b->tacc[2] = z; +} + + +void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + result[0] = p[0] + b->pos[0]; + result[1] = p[1] + b->pos[1]; + result[2] = p[2] + b->pos[2]; +} + + +void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px - b->pos[0]; + p[1] = py - b->pos[1]; + p[2] = pz - b->pos[2]; + p[3] = 0; + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel; + prel[0] = px - b->pos[0]; + prel[1] = py - b->pos[1]; + prel[2] = pz - b->pos[2]; + prel[3] = 0; + dMULTIPLY1_331 (result,b->R,prel); +} + + +void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY0_331 (result,b->R,p); +} + + +void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY1_331 (result,b->R,p); +} + + +void dBodySetFiniteRotationMode (dBodyID b, int mode) +{ + dAASSERT (b); + b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis); + if (mode) { + b->flags |= dxBodyFlagFiniteRotation; + if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 || + b->finite_rot_axis[2] != 0) { + b->flags |= dxBodyFlagFiniteRotationAxis; + } + } +} + + +void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->finite_rot_axis[0] = x; + b->finite_rot_axis[1] = y; + b->finite_rot_axis[2] = z; + if (x != 0 || y != 0 || z != 0) { + dNormalize3 (b->finite_rot_axis); + b->flags |= dxBodyFlagFiniteRotationAxis; + } + else { + b->flags &= ~dxBodyFlagFiniteRotationAxis; + } +} + + +int dBodyGetFiniteRotationMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyFlagFiniteRotation) != 0); +} + + +void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result) +{ + dAASSERT (b); + result[0] = b->finite_rot_axis[0]; + result[1] = b->finite_rot_axis[1]; + result[2] = b->finite_rot_axis[2]; +} + + +int dBodyGetNumJoints (dBodyID b) +{ + dAASSERT (b); + int count=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, count++); + return count; +} + + +dJointID dBodyGetJoint (dBodyID b, int index) +{ + dAASSERT (b); + int i=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) { + if (i == index) return n->joint; + } + return 0; +} + + +void dBodyEnable (dBodyID b) +{ + dAASSERT (b); + b->flags &= ~dxBodyDisabled; +} + + +void dBodyDisable (dBodyID b) +{ + dAASSERT (b); + b->flags |= dxBodyDisabled; +} + + +int dBodyIsEnabled (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyDisabled) == 0); +} + + +void dBodySetGravityMode (dBodyID b, int mode) +{ + dAASSERT (b); + if (mode) b->flags &= ~dxBodyNoGravity; + else b->flags |= dxBodyNoGravity; +} + + +int dBodyGetGravityMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyNoGravity) == 0); +} + +//**************************************************************************** +// joints + +static void dJointInit (dxWorld *w, dxJoint *j) +{ + dIASSERT (w && j); + initObject (j,w); + j->vtable = 0; + j->flags = 0; + j->node[0].joint = j; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].joint = j; + j->node[1].body = 0; + j->node[1].next = 0; + addObjectToList (j,(dObject **) &w->firstjoint); + w->nj++; +} + + +static dxJoint *createJoint (dWorldID w, dJointGroupID group, + dxJoint::Vtable *vtable) +{ + dIASSERT (w && vtable); + dxJoint *j; + if (group) { + j = (dxJoint*) group->stack.alloc (vtable->size); + group->num++; + } + else j = (dxJoint*) dAlloc (vtable->size); + dJointInit (w,j); + j->vtable = vtable; + if (group) j->flags |= dJOINT_INGROUP; + if (vtable->init) vtable->init (j); + j->feedback = 0; + /******************** breakable joint contribution ***********************/ + j->breakInfo = 0; + /*************************************************************************/ + return j; +} + + +dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dball_vtable); +} + + +dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge_vtable); +} + + +dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dslider_vtable); +} + + +dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group, + const dContact *c) +{ + dAASSERT (w && c); + dxJointContact *j = (dxJointContact *) + createJoint (w,group,&__dcontact_vtable); + j->contact = *c; + return j; +} + + +dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge2_vtable); +} + + +dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__duniversal_vtable); +} + + +dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dfixed_vtable); +} + + +dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dnull_vtable); +} + + +dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__damotor_vtable); +} + + +void dJointDestroy (dxJoint *j) +{ + dAASSERT (j); + if (j->flags & dJOINT_INGROUP) return; + removeJointReferencesFromAttachedBodies (j); + removeObjectFromList (j); + /******************** breakable joint contribution ***********************/ + if (j->breakInfo) delete j->breakInfo; + /*************************************************************************/ + j->world->nj--; + dFree (j,j->vtable->size); +} + + +dJointGroupID dJointGroupCreate (int max_size) +{ + // not any more ... dUASSERT (max_size > 0,"max size must be > 0"); + dxJointGroup *group = new dxJointGroup; + group->num = 0; + return group; +} + + +void dJointGroupDestroy (dJointGroupID group) +{ + dAASSERT (group); + dJointGroupEmpty (group); + delete group; +} + + +void dJointGroupEmpty (dJointGroupID group) +{ + // the joints in this group are detached starting from the most recently + // added (at the top of the stack). this helps ensure that the various + // linked lists are not traversed too much, as the joints will hopefully + // be at the start of those lists. + // if any group joints have their world pointer set to 0, their world was + // previously destroyed. no special handling is required for these joints. + + dAASSERT (group); + int i; + dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*)); + dxJoint *j = (dxJoint*) group->stack.rewind(); + for (i=0; i < group->num; i++) { + jlist[i] = j; + j = (dxJoint*) (group->stack.next (j->vtable->size)); + } + for (i=group->num-1; i >= 0; i--) { + if (jlist[i]->world) { + removeJointReferencesFromAttachedBodies (jlist[i]); + removeObjectFromList (jlist[i]); + jlist[i]->world->nj--; + } + } + group->num = 0; + group->stack.freeAll(); +} + + +void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2) +{ + // check arguments + dUASSERT (joint,"bad joint argument"); + dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2"); + dxWorld *world = joint->world; + dUASSERT ( (!body1 || body1->world == world) && + (!body2 || body2->world == world), + "joint and bodies must be in same world"); + + // check if the joint can not be attached to just one body + dUASSERT (!((joint->flags & dJOINT_TWOBODIES) && + ((body1 != 0) ^ (body2 != 0))), + "joint can not be attached to just one body"); + + // remove any existing body attachments + if (joint->node[0].body || joint->node[1].body) { + removeJointReferencesFromAttachedBodies (joint); + } + + // if a body is zero, make sure that it is body2, so 0 --> node[1].body + if (body1==0) { + body1 = body2; + body2 = 0; + joint->flags |= dJOINT_REVERSE; + } + else { + joint->flags &= (~dJOINT_REVERSE); + } + + // attach to new bodies + joint->node[0].body = body1; + joint->node[1].body = body2; + if (body1) { + joint->node[1].next = body1->firstjoint; + body1->firstjoint = &joint->node[1]; + } + else joint->node[1].next = 0; + if (body2) { + joint->node[0].next = body2->firstjoint; + body2->firstjoint = &joint->node[0]; + } + else { + joint->node[0].next = 0; + } +} + + +void dJointSetData (dxJoint *joint, void *data) +{ + dAASSERT (joint); + joint->userdata = data; +} + + +void *dJointGetData (dxJoint *joint) +{ + dAASSERT (joint); + return joint->userdata; +} + + +int dJointGetType (dxJoint *joint) +{ + dAASSERT (joint); + return joint->vtable->typenum; +} + + +dBodyID dJointGetBody (dxJoint *joint, int index) +{ + dAASSERT (joint); + if (index >= 0 && index < 2) return joint->node[index].body; + else return 0; +} + + +void dJointSetFeedback (dxJoint *joint, dJointFeedback *f) +{ + dAASSERT (joint); + joint->feedback = f; +} + + +dJointFeedback *dJointGetFeedback (dxJoint *joint) +{ + dAASSERT (joint); + return joint->feedback; +} + + +int dAreConnected (dBodyID b1, dBodyID b2) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return 1; + } + return 0; +} + + +int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1; + } + return 0; +} + +//**************************************************************************** +// world + +dxWorld * dWorldCreate() +{ + dxWorld *w = new dxWorld; + w->firstbody = 0; + w->firstjoint = 0; + w->nb = 0; + w->nj = 0; + dSetZero (w->gravity,4); + w->global_erp = REAL(0.2); +#if defined(dSINGLE) + w->global_cfm = 1e-5f; +#elif defined(dDOUBLE) + w->global_cfm = 1e-10; +#else + #error dSINGLE or dDOUBLE must be defined +#endif + return w; +} + + +void dWorldDestroy (dxWorld *w) +{ + // delete all bodies and joints + dAASSERT (w); + dxBody *nextb, *b = w->firstbody; + while (b) { + nextb = (dxBody*) b->next; + delete b; + b = nextb; + } + dxJoint *nextj, *j = w->firstjoint; + while (j) { + nextj = (dxJoint*)j->next; + if (j->flags & dJOINT_INGROUP) { + // the joint is part of a group, so "deactivate" it instead + j->world = 0; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; + dMessage (0,"warning: destroying world containing grouped joints"); + } + else { + dFree (j,j->vtable->size); + } + j = nextj; + } + delete w; +} + + +void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z) +{ + dAASSERT (w); + w->gravity[0] = x; + w->gravity[1] = y; + w->gravity[2] = z; +} + + +void dWorldGetGravity (dWorldID w, dVector3 g) +{ + dAASSERT (w); + g[0] = w->gravity[0]; + g[1] = w->gravity[1]; + g[2] = w->gravity[2]; +} + + +void dWorldSetERP (dWorldID w, dReal erp) +{ + dAASSERT (w); + w->global_erp = erp; +} + + +dReal dWorldGetERP (dWorldID w) +{ + dAASSERT (w); + return w->global_erp; +} + + +void dWorldSetCFM (dWorldID w, dReal cfm) +{ + dAASSERT (w); + w->global_cfm = cfm; +} + + +dReal dWorldGetCFM (dWorldID w) +{ + dAASSERT (w); + return w->global_cfm; +} + + +void dWorldStep (dWorldID w, dReal stepsize) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + processIslands (w,stepsize); +} + + +void dWorldImpulseToForce (dWorldID w, dReal stepsize, + dReal ix, dReal iy, dReal iz, + dVector3 force) +{ + dAASSERT (w); + stepsize = dRecip(stepsize); + force[0] = stepsize * ix; + force[1] = stepsize * iy; + force[2] = stepsize * iz; + // @@@ force[3] = 0; +} + +//**************************************************************************** +// testing + +#define NUM 100 + +#define DO(x) + + +extern "C" void dTestDataStructures() +{ + int i; + DO(printf ("testDynamicsStuff()\n")); + + dBodyID body [NUM]; + int nb = 0; + dJointID joint [NUM]; + int nj = 0; + + for (i=0; i 0.5) { + DO(printf ("creating body\n")); + body[nb] = dBodyCreate (w); + DO(printf ("\t--> %p\n",body[nb])); + nb++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj < NUM && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + DO(printf ("creating joint, attaching to %p,%p\n",b1,b2)); + joint[nj] = dJointCreateBall (w,0); + DO(printf ("\t-->%p\n",joint[nj])); + checkWorld (w); + dJointAttach (joint[nj],b1,b2); + nj++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nj > 0 && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + int k = dRand() % nj; + DO(printf ("reattaching joint %p\n",joint[k])); + dJointAttach (joint[k],b1,b2); + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nb > 0 && dRandReal() > 0.5) { + int k = dRand() % nb; + DO(printf ("destroying body %p\n",body[k])); + dBodyDestroy (body[k]); + checkWorld (w); + for (; k < (NUM-1); k++) body[k] = body[k+1]; + nb--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj > 0 && dRandReal() > 0.5) { + int k = dRand() % nj; + DO(printf ("destroying joint %p\n",joint[k])); + dJointDestroy (joint[k]); + checkWorld (w); + for (; k < (NUM-1); k++) joint[k] = joint[k+1]; + nj--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + + /* + printf ("creating world\n"); + dWorldID w = dWorldCreate(); + checkWorld (w); + printf ("creating body\n"); + dBodyID b1 = dBodyCreate (w); + checkWorld (w); + printf ("creating body\n"); + dBodyID b2 = dBodyCreate (w); + checkWorld (w); + printf ("creating joint\n"); + dJointID j = dJointCreateBall (w); + checkWorld (w); + printf ("attaching joint\n"); + dJointAttach (j,b1,b2); + checkWorld (w); + printf ("destroying joint\n"); + dJointDestroy (j); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b1); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b2); + checkWorld (w); + printf ("destroying world\n"); + dWorldDestroy (w); + */ +} diff --git a/libraries/ode-0.9/contrib/BreakableJoints/step.cpp b/libraries/ode-0.9/contrib/BreakableJoints/step.cpp new file mode 100644 index 0000000..38aed6c --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/step.cpp @@ -0,0 +1,1170 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library 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 files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include "objects.h" +#include "joint.h" +#include +#include +#include +#include +#include +#include +#include "lcp.h" + +//**************************************************************************** +// misc defines + +#define FAST_FACTOR +//#define TIMING + +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// debugging - comparison of various vectors and matrices produced by the +// slow and fast versions of the stepper. + +//#define COMPARE_METHODS + +#ifdef COMPARE_METHODS +#include "testing.h" +dMatrixComparison comparator; +#endif + +//**************************************************************************** +// special matrix multipliers + +// this assumes the 4th and 8th rows of B and C are zero. + +static void Multiply2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) = sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B and C are zero. + +static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) += sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q) +{ + int k; + dReal sum; + dIASSERT (q>0 && A && B && C); + sum = 0; + for (k=0; k0 && A && B && C); + sum = 0; + for (k=0; kpos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv,irv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis,b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL(0.5); + dReal theta = k * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + + b->avel[2]*b->avel[2]); + h *= REAL(0.5); + dReal theta = wlen * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2,q,b->q); + for (j=0; j<4; j++) b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) { + dReal dq[4]; + dWtoDQ (irv,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + } + } + else { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +// the slow, but sure way +// note that this does not do any joint feedback! + +// given lists of bodies and joints that form an island, perform a first +// order timestep. +// +// `body' is the body array, `nb' is the size of the array. +// `_joint' is the body array, `nj' is the size of the array. + +void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j,k; + int n6 = 6*nb; + +# ifdef TIMING + dTimerStart("preprocessing"); +# endif + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*)); + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. + // @@@ check computation of rotational force. + dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; imass.I,body[i]->R); + dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); + dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + int m = 0; + dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1)); + int *ofs = (int*) ALLOCA (nj*sizeof(int)); + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; iinvMass; + MM[nskip+1] = body[i]->invMass; + MM[2*nskip+2] = body[i]->invMass; + MM += 3*nskip+3; + for (j=0; j<3; j++) for (k=0; k<3; k++) { + MM[j*nskip+k] = invI[i*12+j*4+k]; + } + } + + // assemble some body vectors: fe = external forces, v = velocities + dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal)); + dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal)); + //dSetZero (fe,n6); + //dSetZero (v,n6); + for (i=0; ifacc[j]; + for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j]; + for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j]; + for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j]; + } + + // this will be set to the velocity update + dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal)); + dSetZero (vnew,n6); + + // if there are constraints, compute cforce + if (m > 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dReal *c = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal)); + int *findex = (int*) alloca (m*sizeof(int)); + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; inode[0].body->tag; + Jinfo.J1a = Jinfo.J1l + 3; + if (joint[i]->node[1].body) { + Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag; + Jinfo.J2a = Jinfo.J2l + 3; + } + else { + Jinfo.J2l = 0; + Jinfo.J2a = 0; + } + Jinfo.c = c + ofs[i]; + Jinfo.cfm = cfm + ofs[i]; + Jinfo.lo = lo + ofs[i]; + Jinfo.hi = hi + ofs[i]; + Jinfo.findex = findex + ofs[i]; + joint[i]->vtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J' +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal)); + //dSetZero (JinvM,m*nskip); + dMultiply0 (JinvM,J,invM,m,n6,n6); + int mskip = dPAD(m); + dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); + //dSetZero (A,m*mskip); + dMultiply2 (A,JinvM,J,m,n6,m); + + // add cfm to the diagonal of A + for (i=0; ilvel[j] = vnew[i*6+j]; + for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j]; + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +# ifdef TIMING + dTimerNow ("update position"); +# endif + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +# ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +# endif +} + +//**************************************************************************** +// an optimized version of dInternalStepIsland1() + +void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j,k; +# ifdef TIMING + dTimerStart("preprocessing"); +# endif + + dReal stepsize1 = dRecip(stepsize); + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*)); + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. + dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; imass.I,body[i]->R); + dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); + dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1)); + int *ofs = (int*) ALLOCA (nj*sizeof(int)); + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + joint[i]->tag = i; + i++; + } + else { + joint[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dReal *c = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal)); + int *findex = (int*) alloca (m*sizeof(int)); + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; ivtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal)); + dSetZero (JinvM,2*m*8); + for (i=0; inode[0].body->tag; + dReal body_invMass = body[b]->invMass; + dReal *body_invI = invI + b*12; + dReal *Jsrc = J + 2*8*ofs[i]; + dReal *Jdst = JinvM + 2*8*ofs[i]; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + if (joint[i]->node[1].body) { + b = joint[i]->node[1].body->tag; + body_invMass = body[b]->invMass; + body_invI = invI + b*12; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + } + } + + // now compute A = JinvM * J'. A's rows and columns are grouped by joint, + // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero + // if joints i and j have at least one body in common. this fact suggests + // the algorithm used to fill A: + // + // for b = all bodies + // n = number of joints attached to body b + // for i = 1..n + // for j = i+1..n + // ii = actual joint number for i + // jj = actual joint number for j + // // (ii,jj) will be set to all pairs of joints around body b + // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)' + // + // this algorithm catches all pairs of joints that have at least one body + // in common. it does not compute the diagonal blocks of A however - + // another similar algorithm does that. + + int mskip = dPAD(m); + dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); + dSetZero (A,m*mskip); + for (i=0; ifirstjoint; n1; n1=n1->next) { + for (dxJointNode *n2=n1->next; n2; n2=n2->next) { + // get joint numbers and ensure ofs[j1] >= ofs[j2] + int j1 = n1->joint->tag; + int j2 = n2->joint->tag; + if (ofs[j1] < ofs[j2]) { + int tmp = j1; + j1 = j2; + j2 = tmp; + } + + // if either joint was tagged as -1 then it is an inactive (m=0) + // joint that should not be considered + if (j1==-1 || j2==-1) continue; + + // determine if body i is the 1st or 2nd body of joints j1 and j2 + int jb1 = (joint[j1]->node[1].body == body[i]); + int jb2 = (joint[j2]->node[1].body == body[i]); + // jb1/jb2 must be 0 for joints with only one body + dIASSERT(joint[j1]->node[1].body || jb1==0); + dIASSERT(joint[j2]->node[1].body || jb2==0); + + // set block of A + MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2], + JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m, + J + 2*8*ofs[j2] + jb2*8*info[j2].m, + info[j1].m,info[j2].m, mskip); + } + } + } + // compute diagonal blocks of A + for (i=0; inode[1].body) { + MultiplyAdd2_p8r (A + ofs[i]*(mskip+1), + JinvM + 2*8*ofs[i] + 8*info[i].m, + J + 2*8*ofs[i] + 8*info[i].m, + info[i].m,info[i].m, mskip); + } + } + + // add cfm to the diagonal of A + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass + + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc); + for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal)); + //dSetZero (rhs,m); + for (i=0; inode[0].body->tag, info[i].m); + if (joint[i]->node[1].body) { + MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m, + tmp1 + 8*joint[i]->node[1].body->tag, info[i].m); + } + } + // complete rhs + for (i=0; inode[0].body; + dxBody* b2 = joint[i]->node[1].body; + dJointFeedback *fb = joint[i]->feedback; + +/******************** breakable joint contribution ***********************/ + // this saves us a few dereferences + dxJointBreakInfo *jBI = joint[i]->breakInfo; + // we need joint feedback if the joint is breakable or if the user + // requested feedback. + if (jBI||fb) { + // we need feedback on the amount of force that this joint is + // applying to the bodies. we use a slightly slower computation + // that splits out the force components and puts them in the + // feedback structure. + dJointFeedback temp_fb; // temporary storage for joint feedback + dReal data1[8],data2[8]; + Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); + dReal *cf1 = cforce + 8*b1->tag; + cf1[0] += (temp_fb.f1[0] = data1[0]); + cf1[1] += (temp_fb.f1[1] = data1[1]); + cf1[2] += (temp_fb.f1[2] = data1[2]); + cf1[4] += (temp_fb.t1[0] = data1[4]); + cf1[5] += (temp_fb.t1[1] = data1[5]); + cf1[6] += (temp_fb.t1[2] = data1[6]); + if (b2) { + Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + dReal *cf2 = cforce + 8*b2->tag; + cf2[0] += (temp_fb.f2[0] = data2[0]); + cf2[1] += (temp_fb.f2[1] = data2[1]); + cf2[2] += (temp_fb.f2[2] = data2[2]); + cf2[4] += (temp_fb.t2[0] = data2[4]); + cf2[5] += (temp_fb.t2[1] = data2[5]); + cf2[6] += (temp_fb.t2[2] = data2[6]); + } + // if the user requested so we must copy the feedback information to + // the feedback struct that the user suplied. + if (fb) { + // copy temp_fb to fb + fb->f1[0] = temp_fb.f1[0]; + fb->f1[1] = temp_fb.f1[1]; + fb->f1[2] = temp_fb.f1[2]; + fb->t1[0] = temp_fb.t1[0]; + fb->t1[1] = temp_fb.t1[1]; + fb->t1[2] = temp_fb.t1[2]; + if (b2) { + fb->f2[0] = temp_fb.f2[0]; + fb->f2[1] = temp_fb.f2[1]; + fb->f2[2] = temp_fb.f2[2]; + fb->t2[0] = temp_fb.t2[0]; + fb->t2[1] = temp_fb.t2[1]; + fb->t2[2] = temp_fb.t2[2]; + } + } + // if the joint is breakable we need to check the breaking conditions + if (jBI) { + dReal relCF1[3]; + dReal relCT1[3]; + // multiply the force and torque vectors by the rotation matrix of body 1 + dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]); + dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]); + if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF1[i] > jBI->b1MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT1[i] > jBI->b1MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (b2) { + dReal relCF2[3]; + dReal relCT2[3]; + // multiply the force and torque vectors by the rotation matrix of body 2 + dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]); + dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]); + if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF2[i] > jBI->b2MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT2[i] > jBI->b2MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + } + doneCheckingBreaks: + ; + } + } +/*************************************************************************/ + else { + // no feedback is required, let's compute cforce the faster way + MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m); + if (b2) { + MultiplyAdd1_8q1 (cforce + 8*b2->tag, + JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + } + } + } + } + + // compute the velocity update +# ifdef TIMING + dTimerNow ("compute velocity update"); +# endif + + // add fe to cforce + for (i=0; ifacc[j]; + for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j]; + } + // multiply cforce by stepsize + for (i=0; i < nb*8; i++) cforce[i] *= stepsize; + // add invM * cforce to the body velocity + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j]; + dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4); + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +# ifdef TIMING + dTimerNow ("update position"); +# endif + for (i=0; ilvel[j]; + for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j]; + } + comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew"); +# endif + +# ifdef TIMING + dTimerNow ("tidy up"); +# endif + + // zero all force accumulators + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +# ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +# endif +} + +//**************************************************************************** + +void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *joint, int nj, dReal stepsize) +{ +# ifndef COMPARE_METHODS + dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize); +# endif + +# ifdef COMPARE_METHODS + int i; + + // save body state + dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody)); + for (i=0; i +#include +#include +#include +#include +#include +#include +#include "lcp.h" +#include "step.h" + + +// misc defines + +#define ALLOCA dALLOCA16 + +#define RANDOM_JOINT_ORDER +//#define FAST_FACTOR //use a factorization approximation to the LCP solver (fast, theoretically less accurate) +#define SLOW_LCP //use the old LCP solver +//#define NO_ISLANDS //does not perform island creation code (3~4% of simulation time), body disabling doesn't work +//#define TIMING + + +static int autoEnableDepth = 2; + +void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth) +{ + if (autodepth > 0) + autoEnableDepth = autodepth; + else + autoEnableDepth = 0; +} + +int dWorldGetAutoEnableDepthSF1 (dxWorld *world) +{ + return autoEnableDepth; +} + +//little bit of math.... the _sym_ functions assume the return matrix will be symmetric +static void +Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) = *ad = sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + +static void +MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) += sum; + *ad += sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q) +{ + int k; + dReal sum; + dIASSERT (q > 0 && A && B && C); + sum = 0; + for (k = 0; k < q; k++) + sum += B[k * 8] * C[k]; + A[0] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[1 + k * 8] * C[k]; + A[1] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[2 + k * 8] * C[k]; + A[2] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[4 + k * 8] * C[k]; + A[4] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[5 + k * 8] * C[k]; + A[5] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[6 + k * 8] * C[k]; + A[6] = sum; +} + +//**************************************************************************** +// body rotation + +// return sin(x)/x. this has a singularity at 0 so special handling is needed +// for small arguments. + +static inline dReal +sinc (dReal x) +{ + // if |x| < 1e-4 then use a taylor series expansion. this two term expansion + // is actually accurate to one LS bit within this range if double precision + // is being used - so don't worry! + if (dFabs (x) < 1.0e-4) + return REAL (1.0) - x * x * REAL (0.166666666666666666667); + else + return dSin (x) / x; +} + + +// given a body b, apply its linear and angular rotation over the time +// interval h, thereby adjusting its position and orientation. + +static inline void +moveAndRotateBody (dxBody * b, dReal h) +{ + int j; + + // handle linear velocity + for (j = 0; j < 3; j++) + b->pos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) + { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv, irv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis, b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL (0.5); + dReal theta = k * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else + { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]); + h *= REAL (0.5); + dReal theta = wlen * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2, q, b->q); + for (j = 0; j < 4; j++) + b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + dReal dq[4]; + dWtoDQ (irv, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + } + else + { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q, b->R); + + // notify all attached geoms that this body has moved + for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +//This is an implementation of the iterated/relaxation algorithm. +//Here is a quick overview of the algorithm per Sergi Valverde's posts to the +//mailing list: +// +// for i=0..N-1 do +// for c = 0..C-1 do +// Solve constraint c-th +// Apply forces to constraint bodies +// next +// next +// Integrate bodies + +void +dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) +{ + int i, j, k; +# ifdef TIMING + dTimerNow ("constraint preprocessing"); +# endif + + dReal stepsize1 = dRecip (stepsize); + + int m = info.m; + // nothing to do if no constraints. + if (m <= 0) + return; + + int nub = 0; + if (info.nub == info.m) + nub = m; + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal JinvM[2 * 6 * 8]; + //dSetZero (JinvM, 2 * m * 8); + + dReal *Jsrc = Jinfo.J1l; + dReal *Jdst = JinvM; + if (body[0]) + { + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[0]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); + Jsrc += 8; + Jdst += 8; + } + } + if (body[1]) + { + Jsrc = Jinfo.J2l; + Jdst = JinvM + 8 * m; + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[1]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); + Jsrc += 8; + Jdst += 8; + } + } + + + // now compute A = JinvM * J'. + int mskip = dPAD (m); + dReal A[6 * 8]; + //dSetZero (A, 6 * 8); + + if (body[0]) + Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip); + if (body[1]) + MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, m, mskip); + + // add cfm to the diagonal of A + for (i = 0; i < m; i++) + A[i * mskip + i] += Jinfo.cfm[i] * stepsize1; + + // compute the right hand side `rhs' +# ifdef TIMING + dTimerNow ("compute rhs"); +# endif + dReal tmp1[16]; + //dSetZero (tmp1, 16); + // put v/h + invM*fe into tmp1 + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc); + for (j = 0; j < 3; j++) + tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + dReal rhs[6]; + //dSetZero (rhs, 6); + + if (body[0]) + Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m); + if (body[1]) + MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); + + // complete rhs + for (i = 0; i < m; i++) + rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i]; + +#ifdef SLOW_LCP + // solve the LCP problem and get lambda. + // this will destroy A but that's okay +# ifdef TIMING + dTimerNow ("solving LCP problem"); +# endif + dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal lo[6], hi[6]; + memcpy (lo, Jinfo.lo, m * sizeof (dReal)); + memcpy (hi, Jinfo.hi, m * sizeof (dReal)); + dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); +#endif + + // LCP Solver replacement: + // This algorithm goes like this: + // Do a straightforward LDLT factorization of the matrix A, solving for + // A*x = rhs + // For each x[i] that is outside of the bounds of lo[i] and hi[i], + // clamp x[i] into that range. + // Substitute into A the now known x's + // subtract the residual away from the rhs. + // Remove row and column i from L, updating the factorization + // place the known x's at the end of the array, keeping up with location in p + // Repeat until all constraints have been clamped or all are within bounds + // + // This is probably only faster in the single joint case where only one repeat is + // the norm. + +#ifdef FAST_FACTOR + // factorize A (L*D*L'=A) +# ifdef TIMING + dTimerNow ("factorize A"); +# endif + dReal d[6]; + dReal L[6 * 8]; + memcpy (L, A, m * mskip * sizeof (dReal)); + dFactorLDLT (L, d, m, mskip); + + // compute lambda +# ifdef TIMING + dTimerNow ("compute lambda"); +# endif + + int left = m; //constraints left to solve. + int remove[6]; + dReal lambda[6]; + dReal x[6]; + int p[6]; + for (i = 0; i < 6; i++) + p[i] = i; + while (true) + { + memcpy (x, rhs, left * sizeof (dReal)); + dSolveLDLT (L, d, x, left, mskip); + + int fixed = 0; + for (i = 0; i < left; i++) + { + j = p[i]; + remove[i] = false; + // This isn't the exact same use of findex as dSolveLCP.... since x[findex] + // may change after I've already clamped x[i], but it should be close + if (Jinfo.findex[j] > -1) + { + dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]); + if (x[i] > f) + x[i] = f; + else if (x[i] < -f) + x[i] = -f; + else + continue; + } + else + { + if (x[i] > Jinfo.hi[j]) + x[i] = Jinfo.hi[j]; + else if (x[i] < Jinfo.lo[j]) + x[i] = Jinfo.lo[j]; + else + continue; + } + remove[i] = true; + fixed++; + } + if (fixed == 0 || fixed == left) //no change or all constraints solved + break; + + for (i = 0; i < left; i++) //sub in to right hand side. + if (remove[i]) + for (j = 0; j < left; j++) + if (!remove[j]) + rhs[j] -= A[j * mskip + i] * x[i]; + + for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables + { + if (remove[r]) + { + //dRemoveLDLT adapted for use without row pointers. + if (r == left - 1) + { + left--; + continue; // deleting last row/col is easy + } + else if (r == 0) + { + dReal a[6]; + for (i = 0; i < left; i++) + a[i] = -A[i * mskip]; + a[0] += REAL (1.0); + dLDLTAddTL (L, d, a, left, mskip); + } + else + { + dReal t[6]; + dReal a[6]; + for (i = 0; i < r; i++) + t[i] = L[r * mskip + i] / d[i]; + for (i = 0; i < left - r; i++) + a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r]; + a[0] += REAL (1.0); + dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip); + } + + dRemoveRowCol (L, left, mskip, r); + //end dRemoveLDLT + + left--; + if (r < (left - 1)) + { + dReal tx = x[r]; + memmove (d + r, d + r + 1, (left - r) * sizeof (dReal)); + memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal)); + //x will get written over by rhs anyway, no need to move it around + //just store the fixed value we just discovered in it. + x[left] = tx; + for (i = 0; i < m; i++) + if (p[i] > r && p[i] <= left) + p[i]--; + p[r] = left; + } + } + } + } + + for (i = 0; i < m; i++) + lambda[i] = x[p[i]]; +# endif + // compute the constraint force `cforce' +# ifdef TIMING + dTimerNow ("compute constraint force"); +#endif + + // compute cforce = J'*lambda + dJointFeedback *fb = joint->feedback; + dReal cforce[16]; + //dSetZero (cforce, 16); + +/******************** breakable joint contribution ***********************/ + // this saves us a few dereferences + dxJointBreakInfo *jBI = joint->breakInfo; + // we need joint feedback if the joint is breakable or if the user + // requested feedback. + if (jBI||fb) { + // we need feedback on the amount of force that this joint is + // applying to the bodies. we use a slightly slower computation + // that splits out the force components and puts them in the + // feedback structure. + dJointFeedback temp_fb; // temporary storage for joint feedback + dReal data1[8],data2[8]; + if (body[0]) + { + Multiply1_8q1 (data1, Jinfo.J1l, lambda, m); + dReal *cf1 = cforce; + cf1[0] = (temp_fb.f1[0] = data1[0]); + cf1[1] = (temp_fb.f1[1] = data1[1]); + cf1[2] = (temp_fb.f1[2] = data1[2]); + cf1[4] = (temp_fb.t1[0] = data1[4]); + cf1[5] = (temp_fb.t1[1] = data1[5]); + cf1[6] = (temp_fb.t1[2] = data1[6]); + } + if (body[1]) + { + Multiply1_8q1 (data2, Jinfo.J2l, lambda, m); + dReal *cf2 = cforce + 8; + cf2[0] = (temp_fb.f2[0] = data2[0]); + cf2[1] = (temp_fb.f2[1] = data2[1]); + cf2[2] = (temp_fb.f2[2] = data2[2]); + cf2[4] = (temp_fb.t2[0] = data2[4]); + cf2[5] = (temp_fb.t2[1] = data2[5]); + cf2[6] = (temp_fb.t2[2] = data2[6]); + } + // if the user requested so we must copy the feedback information to + // the feedback struct that the user suplied. + if (fb) { + // copy temp_fb to fb + fb->f1[0] = temp_fb.f1[0]; + fb->f1[1] = temp_fb.f1[1]; + fb->f1[2] = temp_fb.f1[2]; + fb->t1[0] = temp_fb.t1[0]; + fb->t1[1] = temp_fb.t1[1]; + fb->t1[2] = temp_fb.t1[2]; + if (body[1]) { + fb->f2[0] = temp_fb.f2[0]; + fb->f2[1] = temp_fb.f2[1]; + fb->f2[2] = temp_fb.f2[2]; + fb->t2[0] = temp_fb.t2[0]; + fb->t2[1] = temp_fb.t2[1]; + fb->t2[2] = temp_fb.t2[2]; + } + } + // if the joint is breakable we need to check the breaking conditions + if (jBI) { + dReal relCF1[3]; + dReal relCT1[3]; + // multiply the force and torque vectors by the rotation matrix of body 1 + dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]); + dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]); + if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF1[i] > jBI->b1MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT1[i] > jBI->b1MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (body[1]) { + dReal relCF2[3]; + dReal relCT2[3]; + // multiply the force and torque vectors by the rotation matrix of body 2 + dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]); + dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]); + if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF2[i] > jBI->b2MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT2[i] > jBI->b2MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + } + doneCheckingBreaks: + ; + } + } +/*************************************************************************/ + else + { + // no feedback is required, let's compute cforce the faster way + if (body[0]) + Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m); + if (body[1]) + Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m); + } + + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + { + body[i]->facc[j] += cforce[i * 8 + j]; + body[i]->tacc[j] += cforce[i * 8 + 4 + j]; + } + } +} + +void +dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations) +{ +# ifdef TIMING + dTimerNow ("preprocessing"); +# endif + dxBody *bodyPair[2], *body; + dReal *GIPair[2], *GinvIPair[2]; + dxJoint *joint; + int iter, b, j, i; + dReal ministep = stepsize / maxiterations; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); + memcpy (joints, _joints, nj * sizeof (dxJoint *)); + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); + int *ofs = (int *) ALLOCA (nj * sizeof (int)); + for (i = 0, j = 0; j < nj; j++) + { // i=dest, j=src + joints[j]->vtable->getInfo1 (joints[j], info + i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) + { + joints[i] = joints[j]; + joints[i]->tag = i; + i++; + } + else + { + joints[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i = 0; i < nj; i++) + { + ofs[i] = m; + m += info[i].m; + } + dReal *c = NULL; + dReal *cfm = NULL; + dReal *lo = NULL; + dReal *hi = NULL; + int *findex = NULL; + + dReal *J = NULL; + dxJoint::Info2 * Jinfo = NULL; + + if (m) + { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + c = (dReal *) ALLOCA (m * sizeof (dReal)); + cfm = (dReal *) ALLOCA (m * sizeof (dReal)); + lo = (dReal *) ALLOCA (m * sizeof (dReal)); + hi = (dReal *) ALLOCA (m * sizeof (dReal)); + findex = (int *) ALLOCA (m * sizeof (int)); + dSetZero (c, m); + dSetValue (cfm, m, world->global_cfm); + dSetValue (lo, m, -dInfinity); + dSetValue (hi, m, dInfinity); + for (i = 0; i < m; i++) + findex[i] = -1; + + // get jacobian data from constraints. a (2*m)x8 matrix will be created + // to store the two jacobian blocks from each constraint. it has this + // format: + // + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) + // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) + // etc... + // + // (lll) = linear jacobian data + // (aaa) = angular jacobian data + // +# ifdef TIMING + dTimerNow ("create J"); +# endif + J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); + dSetZero (J, 2 * m * 8); + Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); + for (i = 0; i < nj; i++) + { + Jinfo[i].rowskip = 8; + Jinfo[i].fps = dRecip (stepsize); + Jinfo[i].erp = world->global_erp; + Jinfo[i].J1l = J + 2 * 8 * ofs[i]; + Jinfo[i].J1a = Jinfo[i].J1l + 4; + Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; + Jinfo[i].J2a = Jinfo[i].J2l + 4; + Jinfo[i].c = c + ofs[i]; + Jinfo[i].cfm = cfm + ofs[i]; + Jinfo[i].lo = lo + ofs[i]; + Jinfo[i].hi = hi + ofs[i]; + Jinfo[i].findex = findex + ofs[i]; + //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); + } + + } + + dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + for (b = 0; b < nb; b++) + { + for (i = 0; i < 4; i++) + { + saveFacc[b * 4 + i] = bodies[b]->facc[i]; + saveTacc[b * 4 + i] = bodies[b]->tacc[i]; + bodies[b]->tag = b; + } + } + + for (iter = 0; iter < maxiterations; iter++) + { +# ifdef TIMING + dTimerNow ("applying inertia and gravity"); +# endif + dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. + + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->mass.I, body->R); + dMULTIPLY0_333 (globalI + b * 12, body->R, tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->invI, body->R); + dMULTIPLY0_333 (globalInvI + b * 12, body->R, tmp); + + for (i = 0; i < 4; i++) + body->tacc[i] = saveTacc[b * 4 + i]; + // compute rotational force + dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel); + dCROSS (body->tacc, -=, body->avel, tmp); + + // add the gravity force to all bodies + if ((body->flags & dxBodyNoGravity) == 0) + { + body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0]; + body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1]; + body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2]; + body->facc[3] = 0; + } + + } + +#ifdef RANDOM_JOINT_ORDER +#ifdef TIMING + dTimerNow ("randomizing joint order"); +#endif + //randomize the order of the joints by looping through the array + //and swapping the current joint pointer with a random one before it. + for (j = 0; j < nj; j++) + { + joint = joints[j]; + dxJoint::Info1 i1 = info[j]; + dxJoint::Info2 i2 = Jinfo[j]; + int r = rand () % (j + 1); + joints[j] = joints[r]; + info[j] = info[r]; + Jinfo[j] = Jinfo[r]; + joints[r] = joint; + info[r] = i1; + Jinfo[r] = i2; + } +#endif + + //now iterate through the random ordered joint array we created. + for (j = 0; j < nj; j++) + { +#ifdef TIMING + dTimerNow ("setting up joint"); +#endif + joint = joints[j]; + bodyPair[0] = joint->node[0].body; + bodyPair[1] = joint->node[1].body; + + if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled)) + bodyPair[0] = 0; + if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled)) + bodyPair[1] = 0; + + //if this joint is not connected to any enabled bodies, skip it. + if (!bodyPair[0] && !bodyPair[1]) + continue; + + if (bodyPair[0]) + { + GIPair[0] = globalI + bodyPair[0]->tag * 12; + GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12; + } + if (bodyPair[1]) + { + GIPair[1] = globalI + bodyPair[1]->tag * 12; + GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12; + } + + joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); + + //dInternalStepIslandFast is an exact copy of the old routine with one + //modification: the calculated forces are added back to the facc and tacc + //vectors instead of applying them to the bodies and moving them. + if (info[j].m > 0) + { + dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep); + } + } + // } +# ifdef TIMING + dTimerNow ("moving bodies"); +# endif + //Now we can simulate all the free floating bodies, and move them. + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + for (i = 0; i < 4; i++) + { + body->facc[i] *= ministep; + body->tacc[i] *= ministep; + } + + //apply torque + dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc); + + //apply force + for (i = 0; i < 3; i++) + body->lvel[i] += body->invMass * body->facc[i]; + + //move It! + moveAndRotateBody (body, ministep); + } + } + for (b = 0; b < nb; b++) + for (j = 0; j < 4; j++) + bodies[b]->facc[j] = bodies[b]->tacc[j] = 0; +} + + +#ifdef NO_ISLANDS + +// Since the iterative algorithm doesn't care about islands of bodies, this is a +// faster algorithm that just sends it all the joints and bodies in one array. +// It's downfall is it's inability to handle disabled bodies as well as the old one. +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ + // nothing to do if no bodies + if (world->nb <= 0) + return; + +# ifdef TIMING + dTimerStart ("creating joint and body arrays"); +# endif + dxBody **bodies, *body; + dxJoint **joints, *joint; + joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + + int nj = 0; + for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next) + joints[nj++] = joint; + + int nb = 0; + for (body = world->firstbody; body; body = (dxBody *) body->next) + bodies[nb++] = body; + + dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations); +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#else + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ +#ifdef TIMING + dTimerStart ("Island Setup"); +#endif + dxBody *b, *bb, **body; + dxJoint *j, **joint; + + // nothing to do if no bodies + if (world->nb <= 0) + return; + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + int tbcount = 0; + int tjcount = 0; + + // set all body/joint tags to 0 + for (b = world->firstbody; b; b = (dxBody *) b->next) + b->tag = 0; + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *)); + int *autostack = (int *) ALLOCA (stackalloc * sizeof (int)); + + for (bb = world->firstbody; bb; bb = (dxBody *) bb->next) + { +#ifdef TIMING + dTimerNow ("Island Processing"); +#endif + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) + continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + int autoDepth = autoEnableDepth; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) + { + b = stack[--stacksize]; // pop body off stack + autoDepth = autostack[stacksize]; + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode * n = b->firstjoint; n; n = n->next) + { + if (!n->joint->tag) + { + int thisDepth = autoEnableDepth; + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) + { + if (n->body->flags & dxBodyDisabled) + thisDepth = autoDepth - 1; + if (thisDepth < 0) + continue; + n->body->flags &= ~dxBodyDisabled; + n->body->tag = 1; + autostack[stacksize] = thisDepth; + stack[stacksize++] = n->body; + } + } + } + dIASSERT (stacksize <= world->nb); + dIASSERT (stacksize <= world->nj); + } + + // now do something with body and joint lists + dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i = 0; i < bcount; i++) + { + body[i]->tag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i = 0; i < jcount; i++) + joint[i]->tag = 1; + + tbcount += bcount; + tjcount += jcount; + } + +#ifdef TIMING + dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount); +#endif + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b = world->firstbody; b; b = (dxBody *) b->next) + { + if (b->flags & dxBodyDisabled) + { + if (b->tag) + dDebug (0, "disabled body tagged"); + } + else + { + if (!b->tag) + dDebug (0, "enabled body not tagged"); + } + } + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0)) + { + if (!j->tag) + dDebug (0, "attached enabled joint not tagged"); + } + else + { + if (j->tag) + dDebug (0, "unattached or disabled joint tagged"); + } + } +# endif + /******************** breakable joint contribution ***********************/ + dxJoint* nextJ; + if (!world->firstjoint) + nextJ = 0; + else + nextJ = (dxJoint*)world->firstjoint->next; + for (j=world->firstjoint; j; j=nextJ) { + nextJ = (dxJoint*)j->next; + // check if joint is breakable and broken + if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { + // detach (break) the joint + dJointAttach (j, 0, 0); + // call the callback function if it is set + if (j->breakInfo->callback) j->breakInfo->callback (j); + // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set + if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); + } + } + /*************************************************************************/ + +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#endif + + +void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations) +{ + dUASSERT (w, "bad world argument"); + dUASSERT (stepsize > 0, "stepsize must be > 0"); + processIslandsFast (w, stepsize, maxiterations); +} diff --git a/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp b/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp new file mode 100644 index 0000000..bfed3a3 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp @@ -0,0 +1,416 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library 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 files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +buggy with suspension. +this also shows you how to use geom groups. + +*/ + + +#include + +#include +#include + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCappedCylinder dsDrawCappedCylinderD +#endif + + +// some constants + +#define LENGTH 0.7 // chassis length +#define WIDTH 0.4 // chassis width +#define HEIGHT 0.2 // chassis height +#define RADIUS 0.22 // wheel radius +#define STARTZ 0.4 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 0.2 // wheel mass + +// dynamics and collision objects (chassis, 4 wheels, environment, obstacles, chain) +static dWorldID world; +static dSpaceID space; + +// chain stuff +static const float chain_radius = 0.1; +static const float chain_mass = 0.1; +static const int chain_num = 10; +static dBodyID chain_body[chain_num]; +static dGeomID chain_geom[chain_num]; +static dJointID chain_joint[chain_num-1]; + +// 1 chasses, 4 wheels +static dBodyID body[5]; +// joint[0] is left front wheel, joint[1] is right front wheel +static dJointID joint[4]; +static int joint_exists[4]; +static dJointGroupID contactgroup; +static dGeomID ground; +static dSpaceID car_space; +static dGeomID box[1]; +static dGeomID sphere[4]; +static dGeomID ground_box; +static const int obstacle_num = 25; +static dGeomID obstacle[obstacle_num]; + +// things that the user controls + +static dReal speed=0,steer=0; // user commands + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + int i,n; + +// // do not collide objects that are connected +// dBodyID b1 = dGeomGetBody (o1), +// b2 = dGeomGetBody (o2); +// if (b1 && b2 && dAreConnected(b1, b2)) return; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i 0.1) v = 0.1; + if (v < -0.1) v = -0.1; + v *= 10.0; + dJointSetHinge2Param (joint[i],dParamVel,v); + dJointSetHinge2Param (joint[i],dParamFMax,0.2); + dJointSetHinge2Param (joint[i],dParamLoStop,-0.75); + dJointSetHinge2Param (joint[i],dParamHiStop,0.75); + dJointSetHinge2Param (joint[i],dParamFudgeFactor,0.1); + } + } + + dSpaceCollide (space,0,&nearCallback); + //dWorldStep (world,0.05); + dWorldStepFast1 (world,0.05,5); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + } + + dsSetColor (0,1,1); + dsSetTexture (DS_WOOD); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); + dsSetColor (1,1,1); + for (i=1; i<=4; i++) + dsDrawCylinder (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]), + 0.2, + RADIUS); + + dVector3 ss; + dGeomBoxGetLengths (ground_box,ss); + dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); + + dsSetColor (1,0,0); + for (i=0; i +#include + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCappedCylinder dsDrawCappedCylinderD +#endif + + +// some constants + +#define LENGTH 0.7 // chassis length +#define WIDTH 0.5 // chassis width +#define HEIGHT 0.2 // chassis height +#define RADIUS 0.18 // wheel radius +#define STARTZ 0.5 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 0.2 // wheel mass + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +static dBodyID body[4]; +static dJointID joint[3]; // joint[0] is the front wheel +static dJointGroupID contactgroup; +static dGeomID ground; +static dSpaceID car_space; +static dGeomID box[1]; +static dGeomID sphere[3]; +static dGeomID ground_box; + + +// things that the user controls + +static dReal speed=0,steer=0; // user commands + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + int i,n; + + // only collide things with the ground + int g1 = (o1 == ground || o1 == ground_box); + int g2 = (o2 == ground || o2 == ground_box); + if (!(g1 ^ g2)) return; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i 0.1) v = 0.1; + if (v < -0.1) v = -0.1; + v *= 10.0; + dJointSetHinge2Param (joint[0],dParamVel,v); + dJointSetHinge2Param (joint[0],dParamFMax,0.2); + dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); + dJointSetHinge2Param (joint[0],dParamHiStop,0.75); + dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); + + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + } + + dsSetColor (0,1,1); + dsSetTexture (DS_WOOD); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); + dsSetColor (1,1,1); + for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]),0.02f,RADIUS); + + dVector3 ss; + dGeomBoxGetLengths (ground_box,ss); + dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); + + /* + printf ("%.10f %.10f %.10f %.10f\n", + dJointGetHingeAngle (joint[1]), + dJointGetHingeAngle (joint[2]), + dJointGetHingeAngleRate (joint[1]), + dJointGetHingeAngleRate (joint[2])); + */ +} + + +int main (int argc, char **argv) +{ + int i; + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = "../../drawstuff/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + ground = dCreatePlane (space,0,0,1,0); + + // chassis body + body[0] = dBodyCreate (world); + dBodySetPosition (body[0],0,0,STARTZ); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m,CMASS); + dBodySetMass (body[0],&m); + box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[0],body[0]); + + // wheel bodies + for (i=1; i<=3; i++) { + body[i] = dBodyCreate (world); + dQuaternion q; + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); + dBodySetQuaternion (body[i],q); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m,WMASS); + dBodySetMass (body[i],&m); + sphere[i-1] = dCreateSphere (0,RADIUS); + dGeomSetBody (sphere[i-1],body[i]); + } + dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5); + + // front wheel hinge + /* + joint[0] = dJointCreateHinge2 (world,0); + dJointAttach (joint[0],body[0],body[1]); + const dReal *a = dBodyGetPosition (body[1]); + dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]); + dJointSetHinge2Axis1 (joint[0],0,0,1); + dJointSetHinge2Axis2 (joint[0],0,1,0); + */ + + // front and back wheel hinges + for (i=0; i<3; i++) { + joint[i] = dJointCreateHinge2 (world,0); + dJointAttach (joint[i],body[0],body[i+1]); + const dReal *a = dBodyGetPosition (body[i+1]); + dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]); + dJointSetHinge2Axis1 (joint[i],0,0,1); + dJointSetHinge2Axis2 (joint[i],0,1,0); + + // breakable joints contribution + // the wheels can break + dJointSetBreakable (joint[i], 1); + // the wheels wil break at a specific force + dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_B1_FORCE|dJOINT_BREAK_AT_B2_FORCE); + // specify the force for the first body connected to the joint ... + dJointSetBreakForce (joint[i], 0, 1.5, 1.5, 1.5); + // and for the second body + dJointSetBreakForce (joint[i], 1, 1.5, 1.5, 1.5); + } + + // set joint suspension + for (i=0; i<3; i++) { + dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4); + dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8); + } + + // lock back wheels along the steering axis + for (i=1; i<3; i++) { + // set stops to make sure wheels always stay in alignment + dJointSetHinge2Param (joint[i],dParamLoStop,0); + dJointSetHinge2Param (joint[i],dParamHiStop,0); + // the following alternative method is no good as the wheels may get out + // of alignment: + // dJointSetHinge2Param (joint[i],dParamVel,0); + // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity); + } + + // create car space and add it to the top level space + car_space = dSimpleSpaceCreate (space); + dSpaceSetCleanup (car_space,0); + dSpaceAdd (car_space,box[0]); + dSpaceAdd (car_space,sphere[0]); + dSpaceAdd (car_space,sphere[1]); + dSpaceAdd (car_space,sphere[2]); + + // environment + ground_box = dCreateBox (space,2,1.5,1); + dMatrix3 R; + dRFromAxisAndAngle (R,0,1,0,-0.15); + dGeomSetPosition (ground_box,2,0,-0.34); + dGeomSetRotation (ground_box,R); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dGeomDestroy (box[0]); + dGeomDestroy (sphere[0]); + dGeomDestroy (sphere[1]); + dGeomDestroy (sphere[2]); + + return 0; +} -- cgit v1.1