From 2f8d7092bc2c9609fa98d6888106b96f38b22828 Mon Sep 17 00:00:00 2001 From: dan miller Date: Sun, 21 Oct 2007 08:36:32 +0000 Subject: libraries moved to opensim-libs, a new repository --- .../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 deletions(-) delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/README.txt delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/common.h delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/joint.cpp delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/joint.h delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/objects.h delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/ode.cpp delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/step.cpp delete mode 100755 libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp delete 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 deleted file mode 100644 index e996816..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/README.txt +++ /dev/null @@ -1,110 +0,0 @@ -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 deleted file mode 100644 index bc4272d..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/common.h +++ /dev/null @@ -1,337 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 24415a1..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff +++ /dev/null @@ -1,21 +0,0 @@ -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 deleted file mode 100644 index 80397f0..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff +++ /dev/null @@ -1,148 +0,0 @@ -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 deleted file mode 100644 index eed3c24..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff +++ /dev/null @@ -1,18 +0,0 @@ -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 deleted file mode 100644 index fd2129e..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff +++ /dev/null @@ -1,13 +0,0 @@ -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 deleted file mode 100644 index 761b7be..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff +++ /dev/null @@ -1,28 +0,0 @@ -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 deleted file mode 100644 index dfc8c2f..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff +++ /dev/null @@ -1,130 +0,0 @@ -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 deleted file mode 100644 index ed64cba..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff +++ /dev/null @@ -1,143 +0,0 @@ -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 deleted file mode 100644 index 65770da..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff +++ /dev/null @@ -1,16 +0,0 @@ -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 deleted file mode 100644 index 2c724f8..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp +++ /dev/null @@ -1,2803 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 0573119..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/joint.h +++ /dev/null @@ -1,282 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index de08391..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/objects.h +++ /dev/null @@ -1,252 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 7137960..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp +++ /dev/null @@ -1,1404 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index 38aed6c..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/step.cpp +++ /dev/null @@ -1,1170 +0,0 @@ -/************************************************************************* - * * - * 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 deleted file mode 100644 index bfed3a3..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp +++ /dev/null @@ -1,416 +0,0 @@ -/************************************************************************* - * * - * 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