aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/BreakableJoints
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/contrib/BreakableJoints')
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/README.txt110
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/common.h337
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff21
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff148
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff18
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff13
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff28
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff130
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff143
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff16
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/joint.cpp2803
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/joint.h282
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/objects.h252
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/ode.cpp1404
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/step.cpp1170
-rwxr-xr-xlibraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp1212
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp416
-rw-r--r--libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp327
18 files changed, 0 insertions, 8830 deletions
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 @@
1Breakable Joints
2
3================================================================================
4
5Description:
6This is a small addition to ODE that makes joints breakable. Breakable means
7that if a force on a joint is to high it wil break. I have included a modified
8version of test_buggy.cpp (test_breakable.cpp) so you can see it for your self.
9Just drive your buggy into an obstacle and enjoy!
10
11================================================================================
12
13Installation instructions:
14- copy joint.h, joint.cpp, ode.cpp and step.cpp to the ode/src/ directory
15- copy common.h and object.h to the include/ directory
16- copy test_breakable.cpp to the ode/test/ directory
17- add test_breakable.cpp to the ODE_TEST_SRC_CPP object in the makefile.
18- make ode-lib
19- make ode-test
20You can also use the diffs. The above files will quickly go out of sync with the
21rest of ODE but the diffs wil remain valid longer.
22
23================================================================================
24
25Functions:
26dJointSetBreakable (dJointID joint, int b)
27 If b is 1 the joint is made breakable. If b is 0 the joint is made
28 unbreakable.
29
30void dJointSetBreakCallback (dJointID joint, dJointBreakCallback *callbackFunc)
31 Sets the callback function for this joint. If a funtion is set it will be
32 called if the joint is broken but before it is actually detached or deleted.
33
34void dJointSetBreakMode (dJointID joint, int mode)
35 Use this functions to set some flags. These flags can be ORred ( | )
36 together; ie. dJointSetBreakMode (someJoint,
37dJOINT_BREAK_AT_B1_FORCE|dJOINT_DELETE_ON_BREAK)
38 dJOINT_DELETE_ON_BREAK - If the joint breaks it wil be deleted.
39 dJOINT_BREAK_AT_B1_FORCE - If the force on body 1 is to high the joint will
40 break
41 dJOINT_BREAK_AT_B1_TORQUE - If the torque on body 1 is to high the joint will
42 break
43 dJOINT_BREAK_AT_B2_FORCE - If the force on body 2 is to high the joint will
44 break
45 dJOINT_BREAK_AT_B2_TORQUE - If the torque on body 2 is to high the joint will
46 break
47
48void dJointSetBreakForce (dJointID joint, int body, dReal x, dReal y, dReal z)
49 With this function you can set the maximum force for a body connected to this
50 joint. A value of 0 for body means body 1, 1 means body 2. The force is
51 relative to the bodies rotation.
52
53void dJointSetBreakTorque (dJointID joint, int body, dReal x, dReal y, dReal z)
54 With this function you can set the maximum torque for a body connected to this
55 joint. A value of 0 for body means body 1, 1 means body 2. The torque is
56 relative to the bodies rotation.
57
58int dJointIsBreakable (dJointID joint)
59 Returns 1 if this joint is breakable, 0 otherwise.
60
61int dJointGetBreakMode (dJointID joint)
62 Returns the breakmode flag.
63
64void dJointGetBreakForce (dJointID joint, int body, dReal *force)
65 Returns the force at what this joint will break. A value of 0 for body means
66 body 1, 1 means body 2. force must have enough space for 3 dReal values.
67
68void dJointGetBreakTorque (dJointID joint, int body, dReal *torque)
69 Returns the torque at what this joint will break. A value of 0 for body
70 means body 1, 1 means body 2. force must have enough space for 3 dReal
71 values.
72
73================================================================================
74
75The callback function is defined like this (in common.h):
76void dJointBreakCallback (dJointID);
77
78================================================================================
79
80Problems, known bugs & other issues:
81- If the timestep is very small then joints get a lot weaker. They can even fall
82 apart!
83- I have tested all this with the latest checkout from CVS (at the time of
84 writing ofcourse). I haven't tested it with earlier versions of ODE.
85- I have modified the code that fills the jointfeedback struct. I haven't tested
86 if it still works.
87- I'm not sure if the forces are really relative to the connected bodies.
88- There are some memory leaks in the test_breakable.cpp example.
89
90================================================================================
91
92Bugfixes and changes:
9309/08/2003
94- I fixed a bug when there where 0 joints in the simulation
95
9606/12/2003
97- dJointGetBreakMode() added, by vadim_mcagon@hotmail.com
98
9911/03/2004
100- Updated files to work with latest CVS checkout.
101- Added support for dWorldStepFast1()
102- Added separate test_breakable.cpp example.
103- Updated the code that breaks and destroys a joint.
104
105================================================================================
106
107Send me an e-mail if you have any suggestions, ideas, bugs, bug-fixes, anything!
108e-mail: roelvandijk@home.nl
109
110Roel 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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_COMMON_H_
24#define _ODE_COMMON_H_
25
26#include <ode/config.h>
27#include <ode/error.h>
28
29#ifdef __cplusplus
30extern "C" {
31#endif
32
33
34/* configuration stuff */
35
36/* the efficient alignment. most platforms align data structures to some
37 * number of bytes, but this is not always the most efficient alignment.
38 * for example, many x86 compilers align to 4 bytes, but on a pentium it
39 * is important to align doubles to 8 byte boundaries (for speed), and
40 * the 4 floats in a SIMD register to 16 byte boundaries. many other
41 * platforms have similar behavior. setting a larger alignment can waste
42 * a (very) small amount of memory. NOTE: this number must be a power of
43 * two. this is set to 16 by default.
44 */
45#define EFFICIENT_ALIGNMENT 16
46
47
48/* constants */
49
50/* pi and 1/sqrt(2) are defined here if necessary because they don't get
51 * defined in <math.h> on some platforms (like MS-Windows)
52 */
53
54#ifndef M_PI
55#define M_PI REAL(3.1415926535897932384626433832795029)
56#endif
57#ifndef M_SQRT1_2
58#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490)
59#endif
60
61
62/* debugging:
63 * IASSERT is an internal assertion, i.e. a consistency check. if it fails
64 * we want to know where.
65 * UASSERT is a user assertion, i.e. if it fails a nice error message
66 * should be printed for the user.
67 * AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)"
68 * is printed.
69 * DEBUGMSG just prints out a message
70 */
71
72#ifndef dNODEBUG
73#ifdef __GNUC__
74#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
75 "assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__);
76#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
77 msg " in %s()", __FUNCTION__);
78#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
79 msg " in %s()", __FUNCTION__);
80#else
81#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
82 "assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__);
83#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
84 msg " (%s:%d)", __FILE__,__LINE__);
85#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
86 msg " (%s:%d)", __FILE__,__LINE__);
87#endif
88#else
89#define dIASSERT(a) ;
90#define dUASSERT(a,msg) ;
91#define dDEBUGMSG(msg) ;
92#endif
93#define dAASSERT(a) dUASSERT(a,"Bad argument(s)")
94
95/* floating point data type, vector, matrix and quaternion types */
96
97#if defined(dSINGLE)
98typedef float dReal;
99#elif defined(dDOUBLE)
100typedef double dReal;
101#else
102#error You must #define dSINGLE or dDOUBLE
103#endif
104
105
106/* round an integer up to a multiple of 4, except that 0 and 1 are unmodified
107 * (used to compute matrix leading dimensions)
108 */
109#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a))
110
111/* these types are mainly just used in headers */
112typedef dReal dVector3[4];
113typedef dReal dVector4[4];
114typedef dReal dMatrix3[4*3];
115typedef dReal dMatrix4[4*4];
116typedef dReal dMatrix6[8*6];
117typedef dReal dQuaternion[4];
118
119
120/* precision dependent scalar math functions */
121
122#if defined(dSINGLE)
123
124#define REAL(x) (x ## f) /* form a constant */
125#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */
126#define dSqrt(x) ((float)sqrt(x)) /* square root */
127#define dRecipSqrt(x) ((float)(1.0f/sqrt(x))) /* reciprocal square root */
128#define dSin(x) ((float)sin(x)) /* sine */
129#define dCos(x) ((float)cos(x)) /* cosine */
130#define dFabs(x) ((float)fabs(x)) /* absolute value */
131#define dAtan2(y,x) ((float)atan2((y),(x))) /* arc tangent with 2 args */
132
133#elif defined(dDOUBLE)
134
135#define REAL(x) (x)
136#define dRecip(x) (1.0/(x))
137#define dSqrt(x) sqrt(x)
138#define dRecipSqrt(x) (1.0/sqrt(x))
139#define dSin(x) sin(x)
140#define dCos(x) cos(x)
141#define dFabs(x) fabs(x)
142#define dAtan2(y,x) atan2((y),(x))
143
144#else
145#error You must #define dSINGLE or dDOUBLE
146#endif
147
148
149/* utility */
150
151
152/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */
153
154#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1)
155
156
157/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
158 * up to 15 bytes per allocation, depending on what alloca() returns.
159 */
160
161#define dALLOCA16(n) \
162 ((char*)dEFFICIENT_SIZE(((int)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
163
164
165/* internal object types (all prefixed with `dx') */
166
167struct dxWorld; /* dynamics world */
168struct dxSpace; /* collision space */
169struct dxBody; /* rigid body (dynamics object) */
170struct dxGeom; /* geometry (collision object) */
171struct dxJoint;
172struct dxJointNode;
173struct dxJointGroup;
174
175typedef struct dxWorld *dWorldID;
176typedef struct dxSpace *dSpaceID;
177typedef struct dxBody *dBodyID;
178typedef struct dxGeom *dGeomID;
179typedef struct dxJoint *dJointID;
180typedef struct dxJointGroup *dJointGroupID;
181
182
183/* error numbers */
184
185enum {
186 d_ERR_UNKNOWN = 0, /* unknown error */
187 d_ERR_IASSERT, /* internal assertion failed */
188 d_ERR_UASSERT, /* user assertion failed */
189 d_ERR_LCP /* user assertion failed */
190};
191
192
193/* joint type numbers */
194
195enum {
196 dJointTypeNone = 0, /* or "unknown" */
197 dJointTypeBall,
198 dJointTypeHinge,
199 dJointTypeSlider,
200 dJointTypeContact,
201 dJointTypeUniversal,
202 dJointTypeHinge2,
203 dJointTypeFixed,
204 dJointTypeNull,
205 dJointTypeAMotor
206};
207
208/******************** breakable joint contribution ***********************/
209/* joint break callback function */
210typedef void dJointBreakCallback (dJointID joint);
211
212/* joint break modes */
213enum {
214 // if this flag is set, the joint wil break
215 dJOINT_BROKEN = 0x0001,
216 // if this flag is set, the joint wil be deleted when it breaks
217 dJOINT_DELETE_ON_BREAK = 0x0002,
218 // if this flag is set, the joint can break at a certain force on body 1
219 dJOINT_BREAK_AT_B1_FORCE = 0x0004,
220 // if this flag is set, the joint can break at a certain torque on body 1
221 dJOINT_BREAK_AT_B1_TORQUE = 0x0008,
222 // if this flag is set, the joint can break at a certain force on body 2
223 dJOINT_BREAK_AT_B2_FORCE = 0x0010,
224 // if this flag is set, the joint can break at a certain torque on body 2
225 dJOINT_BREAK_AT_B2_TORQUE = 0x0020
226};
227/*************************************************************************/
228
229/* an alternative way of setting joint parameters, using joint parameter
230 * structures and member constants. we don't actually do this yet.
231 */
232
233/*
234typedef struct dLimot {
235 int mode;
236 dReal lostop, histop;
237 dReal vel, fmax;
238 dReal fudge_factor;
239 dReal bounce, soft;
240 dReal suspension_erp, suspension_cfm;
241} dLimot;
242
243enum {
244 dLimotLoStop = 0x0001,
245 dLimotHiStop = 0x0002,
246 dLimotVel = 0x0004,
247 dLimotFMax = 0x0008,
248 dLimotFudgeFactor = 0x0010,
249 dLimotBounce = 0x0020,
250 dLimotSoft = 0x0040
251};
252*/
253
254
255/* standard joint parameter names. why are these here? - because we don't want
256 * to include all the joint function definitions in joint.cpp. hmmmm.
257 * MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second argument,
258 * which is why we have the D_ALL_PARAM_NAMES macro as well. please copy and
259 * paste between these two.
260 */
261
262#define D_ALL_PARAM_NAMES(start) \
263 /* parameters for limits and motors */ \
264 dParamLoStop = start, \
265 dParamHiStop, \
266 dParamVel, \
267 dParamFMax, \
268 dParamFudgeFactor, \
269 dParamBounce, \
270 dParamCFM, \
271 dParamStopERP, \
272 dParamStopCFM, \
273 /* parameters for suspension */ \
274 dParamSuspensionERP, \
275 dParamSuspensionCFM,
276
277#define D_ALL_PARAM_NAMES_X(start,x) \
278 /* parameters for limits and motors */ \
279 dParamLoStop ## x = start, \
280 dParamHiStop ## x, \
281 dParamVel ## x, \
282 dParamFMax ## x, \
283 dParamFudgeFactor ## x, \
284 dParamBounce ## x, \
285 dParamCFM ## x, \
286 dParamStopERP ## x, \
287 dParamStopCFM ## x, \
288 /* parameters for suspension */ \
289 dParamSuspensionERP ## x, \
290 dParamSuspensionCFM ## x,
291
292enum {
293 D_ALL_PARAM_NAMES(0)
294 D_ALL_PARAM_NAMES_X(0x100,2)
295 D_ALL_PARAM_NAMES_X(0x200,3)
296
297 /* add a multiple of this constant to the basic parameter numbers to get
298 * the parameters for the second, third etc axes.
299 */
300 dParamGroup=0x100
301};
302
303
304/* angular motor mode numbers */
305
306enum{
307 dAMotorUser = 0,
308 dAMotorEuler = 1
309};
310
311
312/* joint force feedback information */
313
314typedef struct dJointFeedback {
315 dVector3 f1; /* force applied to body 1 */
316 dVector3 t1; /* torque applied to body 1 */
317 dVector3 f2; /* force applied to body 2 */
318 dVector3 t2; /* torque applied to body 2 */
319} dJointFeedback;
320
321
322/* private functions that must be implemented by the collision library:
323 * (1) indicate that a geom has moved, (2) get the next geom in a body list.
324 * these functions are called whenever the position of geoms connected to a
325 * body have changed, e.g. with dBodySetPosition(), dBodySetRotation(), or
326 * when the ODE step function updates the body state.
327 */
328
329void dGeomMoved (dGeomID);
330dGeomID dGeomGetBodyNext (dGeomID);
331
332
333#ifdef __cplusplus
334}
335#endif
336
337#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 @@
1208,227d207
2< /******************** breakable joint contribution ***********************/
3< /* joint break callback function */
4< typedef void dJointBreakCallback (dJointID joint);
5<
6< /* joint break modes */
7< enum {
8< // if this flag is set, the joint wil break
9< dJOINT_BROKEN = 0x0001,
10< // if this flag is set, the joint wil be deleted when it breaks
11< dJOINT_DELETE_ON_BREAK = 0x0002,
12< // if this flag is set, the joint can break at a certain force on body 1
13< dJOINT_BREAK_AT_B1_FORCE = 0x0004,
14< // if this flag is set, the joint can break at a certain torque on body 1
15< dJOINT_BREAK_AT_B1_TORQUE = 0x0008,
16< // if this flag is set, the joint can break at a certain force on body 2
17< dJOINT_BREAK_AT_B2_FORCE = 0x0010,
18< // if this flag is set, the joint can break at a certain torque on body 2
19< dJOINT_BREAK_AT_B2_TORQUE = 0x0020
20< };
21< /*************************************************************************/
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 @@
12659,2804d2658
2<
3< /******************** breakable joint contribution ***********************/
4< extern "C" void dJointSetBreakable (dxJoint *joint, int b) {
5< dAASSERT(joint);
6< if (b) {
7< // we want this joint to be breakable but we must first check if it
8< // was already breakable
9< if (!joint->breakInfo) {
10< // allocate a dxJointBreakInfo struct
11< joint->breakInfo = new dxJointBreakInfo;
12< joint->breakInfo->flags = 0;
13< for (int i = 0; i < 3; i++) {
14< joint->breakInfo->b1MaxF[0] = 0;
15< joint->breakInfo->b1MaxT[0] = 0;
16< joint->breakInfo->b2MaxF[0] = 0;
17< joint->breakInfo->b2MaxT[0] = 0;
18< }
19< joint->breakInfo->callback = 0;
20< }
21< else {
22< // the joint was already breakable
23< return;
24< }
25< }
26< else {
27< // we want this joint to be unbreakable mut we must first check if
28< // it is alreay unbreakable
29< if (joint->breakInfo) {
30< // deallocate the dxJointBreakInfo struct
31< delete joint->breakInfo;
32< joint->breakInfo = 0;
33< }
34< else {
35< // the joint was already unbreakable
36< return;
37< }
38< }
39< }
40<
41< extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) {
42< dAASSERT(joint);
43< # ifndef dNODEBUG
44< // only works for a breakable joint
45< if (!joint->breakInfo) {
46< dDebug (0, "dJointSetBreakCallback called on unbreakable joint");
47< }
48< # endif
49< joint->breakInfo->callback = callbackFunc;
50< }
51<
52< extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) {
53< dAASSERT(joint);
54< # ifndef dNODEBUG
55< // only works for a breakable joint
56< if (!joint->breakInfo) {
57< dDebug (0, "dJointSetBreakMode called on unbreakable joint");
58< }
59< # endif
60< joint->breakInfo->flags = mode;
61< }
62<
63< extern "C" int dJointGetBreakMode (dxJoint *joint) {
64< dAASSERT(joint);
65< # ifndef dNODEBUG
66< // only works for a breakable joint
67< if (!joint->breakInfo) {
68< dDebug (0, "dJointGetBreakMode called on unbreakable joint");
69< }
70< # endif
71< return joint->breakInfo->flags;
72< }
73<
74< extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
75< dAASSERT(joint);
76< # ifndef dNODEBUG
77< // only works for a breakable joint
78< if (!joint->breakInfo) {
79< dDebug (0, "dJointSetBreakForce called on unbreakable joint");
80< }
81< # endif
82< if (body) {
83< joint->breakInfo->b2MaxF[0] = x;
84< joint->breakInfo->b2MaxF[1] = y;
85< joint->breakInfo->b2MaxF[2] = z;
86< }
87< else {
88< joint->breakInfo->b1MaxF[0] = x;
89< joint->breakInfo->b1MaxF[1] = y;
90< joint->breakInfo->b1MaxF[2] = z;
91< }
92< }
93<
94< extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
95< dAASSERT(joint);
96< # ifndef dNODEBUG
97< // only works for a breakable joint
98< if (!joint->breakInfo) {
99< dDebug (0, "dJointSetBreakTorque called on unbreakable joint");
100< }
101< # endif
102< if (body) {
103< joint->breakInfo->b2MaxT[0] = x;
104< joint->breakInfo->b2MaxT[1] = y;
105< joint->breakInfo->b2MaxT[2] = z;
106< }
107< else {
108< joint->breakInfo->b1MaxT[0] = x;
109< joint->breakInfo->b1MaxT[1] = y;
110< joint->breakInfo->b1MaxT[2] = z;
111< }
112< }
113<
114< extern "C" int dJointIsBreakable (dxJoint *joint) {
115< dAASSERT(joint);
116< return joint->breakInfo != 0;
117< }
118<
119< extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) {
120< dAASSERT(joint);
121< # ifndef dNODEBUG
122< // only works for a breakable joint
123< if (!joint->breakInfo) {
124< dDebug (0, "dJointGetBreakForce called on unbreakable joint");
125< }
126< # endif
127< if (body)
128< for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i];
129< else
130< for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i];
131< }
132<
133< extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) {
134< dAASSERT(joint);
135< # ifndef dNODEBUG
136< // only works for a breakable joint
137< if (!joint->breakInfo) {
138< dDebug (0, "dJointGetBreakTorque called on unbreakable joint");
139< }
140< # endif
141< if (body)
142< for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i];
143< else
144< for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i];
145< }
146< /*************************************************************************/
147<
148\ 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 @@
161,70d60
2< /******************** breakable joint contribution ***********************/
3< struct dxJointBreakInfo : public dBase {
4< int flags;
5< dReal b1MaxF[3]; // maximum force on body 1
6< dReal b1MaxT[3]; // maximum torque on body 1
7< dReal b2MaxF[3]; // maximum force on body 2
8< dReal b2MaxT[3]; // maximum torque on body 2
9< dJointBreakCallback *callback; // function that is called when this joint breaks
10< };
11< /*************************************************************************/
12135,140d124
13<
14< /******************** breakable joint contribution ***********************/
15< // optional break info structure. if this is not NULL the the joint is
16< // breakable.
17< dxJointBreakInfo *breakInfo;
18< /*************************************************************************/
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 @@
1168,179d167
2< /******************** breakable joint contribution ***********************/
3< void dJointSetBreakable (dJointID, int b);
4< void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc);
5< void dJointSetBreakMode (dJointID, int mode);
6< int dJointGetBreakMode (dJointID);
7< void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z);
8< void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z);
9< int dJointIsBreakable (dJointID);
10< void dJointGetBreakForce (dJointID, int body, dReal *force);
11< void dJointGetBreakTorque (dJointID, int body, dReal *torque);
12< /*************************************************************************/
13<
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 @@
1212,230d211
2< /******************** breakable joint contribution ***********************/
3< dxJoint* nextJ;
4< if (!world->firstjoint)
5< nextJ = 0;
6< else
7< nextJ = (dxJoint*)world->firstjoint->next;
8< for (j=world->firstjoint; j; j=nextJ) {
9< nextJ = (dxJoint*)j->next;
10< // check if joint is breakable and broken
11< if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
12< // detach (break) the joint
13< dJointAttach (j, 0, 0);
14< // call the callback function if it is set
15< if (j->breakInfo->callback) j->breakInfo->callback (j);
16< // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
17< if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
18< }
19< }
20< /*************************************************************************/
21931,933d911
22< /******************** breakable joint contribution ***********************/
23< j->breakInfo = 0;
24< /*************************************************************************/
251011,1013d988
26< /******************** breakable joint contribution ***********************/
27< if (j->breakInfo) delete j->breakInfo;
28< /*************************************************************************/
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 @@
1966,1066c966,989
2< /******************** breakable joint contribution ***********************/
3< // this saves us a few dereferences
4< dxJointBreakInfo *jBI = joint[i]->breakInfo;
5< // we need joint feedback if the joint is breakable or if the user
6< // requested feedback.
7< if (jBI||fb) {
8< // we need feedback on the amount of force that this joint is
9< // applying to the bodies. we use a slightly slower computation
10< // that splits out the force components and puts them in the
11< // feedback structure.
12< dJointFeedback temp_fb; // temporary storage for joint feedback
13< dReal data1[8],data2[8];
14< Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
15< dReal *cf1 = cforce + 8*b1->tag;
16< cf1[0] += (temp_fb.f1[0] = data1[0]);
17< cf1[1] += (temp_fb.f1[1] = data1[1]);
18< cf1[2] += (temp_fb.f1[2] = data1[2]);
19< cf1[4] += (temp_fb.t1[0] = data1[4]);
20< cf1[5] += (temp_fb.t1[1] = data1[5]);
21< cf1[6] += (temp_fb.t1[2] = data1[6]);
22< if (b2) {
23< Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
24< dReal *cf2 = cforce + 8*b2->tag;
25< cf2[0] += (temp_fb.f2[0] = data2[0]);
26< cf2[1] += (temp_fb.f2[1] = data2[1]);
27< cf2[2] += (temp_fb.f2[2] = data2[2]);
28< cf2[4] += (temp_fb.t2[0] = data2[4]);
29< cf2[5] += (temp_fb.t2[1] = data2[5]);
30< cf2[6] += (temp_fb.t2[2] = data2[6]);
31< }
32< // if the user requested so we must copy the feedback information to
33< // the feedback struct that the user suplied.
34< if (fb) {
35< // copy temp_fb to fb
36< fb->f1[0] = temp_fb.f1[0];
37< fb->f1[1] = temp_fb.f1[1];
38< fb->f1[2] = temp_fb.f1[2];
39< fb->t1[0] = temp_fb.t1[0];
40< fb->t1[1] = temp_fb.t1[1];
41< fb->t1[2] = temp_fb.t1[2];
42< if (b2) {
43< fb->f2[0] = temp_fb.f2[0];
44< fb->f2[1] = temp_fb.f2[1];
45< fb->f2[2] = temp_fb.f2[2];
46< fb->t2[0] = temp_fb.t2[0];
47< fb->t2[1] = temp_fb.t2[1];
48< fb->t2[2] = temp_fb.t2[2];
49< }
50< }
51< // if the joint is breakable we need to check the breaking conditions
52< if (jBI) {
53< dReal relCF1[3];
54< dReal relCT1[3];
55< // multiply the force and torque vectors by the rotation matrix of body 1
56< dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]);
57< dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]);
58< if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
59< // check if the force is to high
60< for (int i = 0; i < 3; i++) {
61< if (relCF1[i] > jBI->b1MaxF[i]) {
62< jBI->flags |= dJOINT_BROKEN;
63< goto doneCheckingBreaks;
64< }
65< }
66< }
67< if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
68< // check if the torque is to high
69< for (int i = 0; i < 3; i++) {
70< if (relCT1[i] > jBI->b1MaxT[i]) {
71< jBI->flags |= dJOINT_BROKEN;
72< goto doneCheckingBreaks;
73< }
74< }
75< }
76< if (b2) {
77< dReal relCF2[3];
78< dReal relCT2[3];
79< // multiply the force and torque vectors by the rotation matrix of body 2
80< dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]);
81< dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]);
82< if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
83< // check if the force is to high
84< for (int i = 0; i < 3; i++) {
85< if (relCF2[i] > jBI->b2MaxF[i]) {
86< jBI->flags |= dJOINT_BROKEN;
87< goto doneCheckingBreaks;
88< }
89< }
90< }
91< if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
92< // check if the torque is to high
93< for (int i = 0; i < 3; i++) {
94< if (relCT2[i] > jBI->b2MaxT[i]) {
95< jBI->flags |= dJOINT_BROKEN;
96< goto doneCheckingBreaks;
97< }
98< }
99< }
100< }
101< doneCheckingBreaks:
102< ;
103---
104> if (fb) {
105> // the user has requested feedback on the amount of force that this
106> // joint is applying to the bodies. we use a slightly slower
107> // computation that splits out the force components and puts them
108> // in the feedback structure.
109> dReal data1[8],data2[8];
110> Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
111> dReal *cf1 = cforce + 8*b1->tag;
112> cf1[0] += (fb->f1[0] = data1[0]);
113> cf1[1] += (fb->f1[1] = data1[1]);
114> cf1[2] += (fb->f1[2] = data1[2]);
115> cf1[4] += (fb->t1[0] = data1[4]);
116> cf1[5] += (fb->t1[1] = data1[5]);
117> cf1[6] += (fb->t1[2] = data1[6]);
118> if (b2){
119> Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
120> dReal *cf2 = cforce + 8*b2->tag;
121> cf2[0] += (fb->f2[0] = data2[0]);
122> cf2[1] += (fb->f2[1] = data2[1]);
123> cf2[2] += (fb->f2[2] = data2[2]);
124> cf2[4] += (fb->t2[0] = data2[4]);
125> cf2[5] += (fb->t2[1] = data2[5]);
126> cf2[6] += (fb->t2[2] = data2[6]);
127> }
1281068,1069d990
129< }
130< /*************************************************************************/
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 @@
1587,598c587,593
2< /******************** breakable joint contribution ***********************/
3< // this saves us a few dereferences
4< dxJointBreakInfo *jBI = joint->breakInfo;
5< // we need joint feedback if the joint is breakable or if the user
6< // requested feedback.
7< if (jBI||fb) {
8< // we need feedback on the amount of force that this joint is
9< // applying to the bodies. we use a slightly slower computation
10< // that splits out the force components and puts them in the
11< // feedback structure.
12< dJointFeedback temp_fb; // temporary storage for joint feedback
13< dReal data1[8],data2[8];
14---
15> if (fb)
16> {
17> // the user has requested feedback on the amount of force that this
18> // joint is applying to the bodies. we use a slightly slower
19> // computation that splits out the force components and puts them
20> // in the feedback structure.
21> dReal data1[8], data2[8];
22603,608c598,603
23< cf1[0] = (temp_fb.f1[0] = data1[0]);
24< cf1[1] = (temp_fb.f1[1] = data1[1]);
25< cf1[2] = (temp_fb.f1[2] = data1[2]);
26< cf1[4] = (temp_fb.t1[0] = data1[4]);
27< cf1[5] = (temp_fb.t1[1] = data1[5]);
28< cf1[6] = (temp_fb.t1[2] = data1[6]);
29---
30> cf1[0] = (fb->f1[0] = data1[0]);
31> cf1[1] = (fb->f1[1] = data1[1]);
32> cf1[2] = (fb->f1[2] = data1[2]);
33> cf1[4] = (fb->t1[0] = data1[4]);
34> cf1[5] = (fb->t1[1] = data1[5]);
35> cf1[6] = (fb->t1[2] = data1[6]);
36614,691c609,614
37< cf2[0] = (temp_fb.f2[0] = data2[0]);
38< cf2[1] = (temp_fb.f2[1] = data2[1]);
39< cf2[2] = (temp_fb.f2[2] = data2[2]);
40< cf2[4] = (temp_fb.t2[0] = data2[4]);
41< cf2[5] = (temp_fb.t2[1] = data2[5]);
42< cf2[6] = (temp_fb.t2[2] = data2[6]);
43< }
44< // if the user requested so we must copy the feedback information to
45< // the feedback struct that the user suplied.
46< if (fb) {
47< // copy temp_fb to fb
48< fb->f1[0] = temp_fb.f1[0];
49< fb->f1[1] = temp_fb.f1[1];
50< fb->f1[2] = temp_fb.f1[2];
51< fb->t1[0] = temp_fb.t1[0];
52< fb->t1[1] = temp_fb.t1[1];
53< fb->t1[2] = temp_fb.t1[2];
54< if (body[1]) {
55< fb->f2[0] = temp_fb.f2[0];
56< fb->f2[1] = temp_fb.f2[1];
57< fb->f2[2] = temp_fb.f2[2];
58< fb->t2[0] = temp_fb.t2[0];
59< fb->t2[1] = temp_fb.t2[1];
60< fb->t2[2] = temp_fb.t2[2];
61< }
62< }
63< // if the joint is breakable we need to check the breaking conditions
64< if (jBI) {
65< dReal relCF1[3];
66< dReal relCT1[3];
67< // multiply the force and torque vectors by the rotation matrix of body 1
68< dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]);
69< dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]);
70< if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
71< // check if the force is to high
72< for (int i = 0; i < 3; i++) {
73< if (relCF1[i] > jBI->b1MaxF[i]) {
74< jBI->flags |= dJOINT_BROKEN;
75< goto doneCheckingBreaks;
76< }
77< }
78< }
79< if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
80< // check if the torque is to high
81< for (int i = 0; i < 3; i++) {
82< if (relCT1[i] > jBI->b1MaxT[i]) {
83< jBI->flags |= dJOINT_BROKEN;
84< goto doneCheckingBreaks;
85< }
86< }
87< }
88< if (body[1]) {
89< dReal relCF2[3];
90< dReal relCT2[3];
91< // multiply the force and torque vectors by the rotation matrix of body 2
92< dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]);
93< dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]);
94< if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
95< // check if the force is to high
96< for (int i = 0; i < 3; i++) {
97< if (relCF2[i] > jBI->b2MaxF[i]) {
98< jBI->flags |= dJOINT_BROKEN;
99< goto doneCheckingBreaks;
100< }
101< }
102< }
103< if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
104< // check if the torque is to high
105< for (int i = 0; i < 3; i++) {
106< if (relCT2[i] > jBI->b2MaxT[i]) {
107< jBI->flags |= dJOINT_BROKEN;
108< goto doneCheckingBreaks;
109< }
110< }
111< }
112< }
113< doneCheckingBreaks:
114< ;
115---
116> cf2[0] = (fb->f2[0] = data2[0]);
117> cf2[1] = (fb->f2[1] = data2[1]);
118> cf2[2] = (fb->f2[2] = data2[2]);
119> cf2[4] = (fb->t2[0] = data2[4]);
120> cf2[5] = (fb->t2[1] = data2[5]);
121> cf2[6] = (fb->t2[2] = data2[6]);
122694d616
123< /*************************************************************************/
1241178,1196d1099
125< /******************** breakable joint contribution ***********************/
126< dxJoint* nextJ;
127< if (!world->firstjoint)
128< nextJ = 0;
129< else
130< nextJ = (dxJoint*)world->firstjoint->next;
131< for (j=world->firstjoint; j; j=nextJ) {
132< nextJ = (dxJoint*)j->next;
133< // check if joint is breakable and broken
134< if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
135< // detach (break) the joint
136< dJointAttach (j, 0, 0);
137< // call the callback function if it is set
138< if (j->breakInfo->callback) j->breakInfo->callback (j);
139< // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
140< if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
141< }
142< }
143< /*************************************************************************/
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 @@
1266,270d265
2<
3< // breakable joints contribution
4< dJointSetBreakable (joint[i], 1);
5< dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_FORCE);
6< dJointSetBreakForce (joint[i], 0.5);
7298c293
8< ground_box = dCreateBox (space,2,1.5,5);
9---
10> ground_box = dCreateBox (space,2,1.5,1);
11300,301c295,296
12< dRFromAxisAndAngle (R,0,1,0,-0.85);
13< dGeomSetPosition (ground_box,5,0,-1);
14---
15> dRFromAxisAndAngle (R,0,1,0,-0.15);
16> 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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25design note: the general principle for giving a joint the option of connecting
26to the static environment (i.e. the absolute frame) is to check the second
27body (joint->node[1].body), and if it is zero then behave as if its body
28transform is the identity.
29
30*/
31
32#include <ode/odemath.h>
33#include <ode/rotation.h>
34#include <ode/matrix.h>
35#include "joint.h"
36
37//****************************************************************************
38// externs
39
40extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
41extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
42
43//****************************************************************************
44// utility
45
46// set three "ball-and-socket" rows in the constraint equation, and the
47// corresponding right hand side.
48
49static inline void setBall (dxJoint *joint, dxJoint::Info2 *info,
50 dVector3 anchor1, dVector3 anchor2)
51{
52 // anchor points in global coordinates with respect to body PORs.
53 dVector3 a1,a2;
54
55 int s = info->rowskip;
56
57 // set jacobian
58 info->J1l[0] = 1;
59 info->J1l[s+1] = 1;
60 info->J1l[2*s+2] = 1;
61 dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
62 dCROSSMAT (info->J1a,a1,s,-,+);
63 if (joint->node[1].body) {
64 info->J2l[0] = -1;
65 info->J2l[s+1] = -1;
66 info->J2l[2*s+2] = -1;
67 dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
68 dCROSSMAT (info->J2a,a2,s,+,-);
69 }
70
71 // set right hand side
72 dReal k = info->fps * info->erp;
73 if (joint->node[1].body) {
74 for (int j=0; j<3; j++) {
75 info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] -
76 a1[j] - joint->node[0].body->pos[j]);
77 }
78 }
79 else {
80 for (int j=0; j<3; j++) {
81 info->c[j] = k * (anchor2[j] - a1[j] -
82 joint->node[0].body->pos[j]);
83 }
84 }
85}
86
87
88// this is like setBall(), except that `axis' is a unit length vector
89// (in global coordinates) that should be used for the first jacobian
90// position row (the other two row vectors will be derived from this).
91// `erp1' is the erp value to use along the axis.
92
93static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info,
94 dVector3 anchor1, dVector3 anchor2,
95 dVector3 axis, dReal erp1)
96{
97 // anchor points in global coordinates with respect to body PORs.
98 dVector3 a1,a2;
99
100 int i,s = info->rowskip;
101
102 // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
103 // [0 1 0] and [0 0 1], which makes everything much easier.
104 dVector3 q1,q2;
105 dPlaneSpace (axis,q1,q2);
106
107 // set jacobian
108 for (i=0; i<3; i++) info->J1l[i] = axis[i];
109 for (i=0; i<3; i++) info->J1l[s+i] = q1[i];
110 for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i];
111 dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
112 dCROSS (info->J1a,=,a1,axis);
113 dCROSS (info->J1a+s,=,a1,q1);
114 dCROSS (info->J1a+2*s,=,a1,q2);
115 if (joint->node[1].body) {
116 for (i=0; i<3; i++) info->J2l[i] = -axis[i];
117 for (i=0; i<3; i++) info->J2l[s+i] = -q1[i];
118 for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i];
119 dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
120 dCROSS (info->J2a,= -,a2,axis);
121 dCROSS (info->J2a+s,= -,a2,q1);
122 dCROSS (info->J2a+2*s,= -,a2,q2);
123 }
124
125 // set right hand side - measure error along (axis,q1,q2)
126 dReal k1 = info->fps * erp1;
127 dReal k = info->fps * info->erp;
128
129 for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i];
130 if (joint->node[1].body) {
131 for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i];
132 info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1));
133 info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1));
134 info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1));
135 }
136 else {
137 info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1));
138 info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1));
139 info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1));
140 }
141}
142
143
144// set three orientation rows in the constraint equation, and the
145// corresponding right hand side.
146
147static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row)
148{
149 int s = info->rowskip;
150 int start_index = start_row * s;
151
152 // 3 rows to make body rotations equal
153 info->J1a[start_index] = 1;
154 info->J1a[start_index + s + 1] = 1;
155 info->J1a[start_index + s*2+2] = 1;
156 if (joint->node[1].body) {
157 info->J2a[start_index] = -1;
158 info->J2a[start_index + s+1] = -1;
159 info->J2a[start_index + s*2+2] = -1;
160 }
161
162 // compute the right hand side. the first three elements will result in
163 // relative angular velocity of the two bodies - this is set to bring them
164 // back into alignment. the correcting angular velocity is
165 // |angular_velocity| = angle/time = erp*theta / stepsize
166 // = (erp*fps) * theta
167 // angular_velocity = |angular_velocity| * u
168 // = (erp*fps) * theta * u
169 // where rotation along unit length axis u by theta brings body 2's frame
170 // to qrel with respect to body 1's frame. using a small angle approximation
171 // for sin(), this gives
172 // angular_velocity = (erp*fps) * 2 * v
173 // where the quaternion of the relative rotation between the two bodies is
174 // q = [cos(theta/2) sin(theta/2)*u] = [s v]
175
176 // get qerr = relative rotation (rotation error) between two bodies
177 dQuaternion qerr,e;
178 if (joint->node[1].body) {
179 dQuaternion qq;
180 dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q);
181 dQMultiply2 (qerr,qq,qrel);
182 }
183 else {
184 dQMultiply3 (qerr,joint->node[0].body->q,qrel);
185 }
186 if (qerr[0] < 0) {
187 qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
188 qerr[2] = -qerr[2];
189 qerr[3] = -qerr[3];
190 }
191 dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding!
192 dReal k = info->fps * info->erp;
193 info->c[start_row] = 2*k * e[0];
194 info->c[start_row+1] = 2*k * e[1];
195 info->c[start_row+2] = 2*k * e[2];
196}
197
198
199// compute anchor points relative to bodies
200
201static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z,
202 dVector3 anchor1, dVector3 anchor2)
203{
204 if (j->node[0].body) {
205 dReal q[4];
206 q[0] = x - j->node[0].body->pos[0];
207 q[1] = y - j->node[0].body->pos[1];
208 q[2] = z - j->node[0].body->pos[2];
209 q[3] = 0;
210 dMULTIPLY1_331 (anchor1,j->node[0].body->R,q);
211 if (j->node[1].body) {
212 q[0] = x - j->node[1].body->pos[0];
213 q[1] = y - j->node[1].body->pos[1];
214 q[2] = z - j->node[1].body->pos[2];
215 q[3] = 0;
216 dMULTIPLY1_331 (anchor2,j->node[1].body->R,q);
217 }
218 else {
219 anchor2[0] = x;
220 anchor2[1] = y;
221 anchor2[2] = z;
222 }
223 }
224 anchor1[3] = 0;
225 anchor2[3] = 0;
226}
227
228
229// compute axes relative to bodies. either axis1 or axis2 can be 0.
230
231static void setAxes (dxJoint *j, dReal x, dReal y, dReal z,
232 dVector3 axis1, dVector3 axis2)
233{
234 if (j->node[0].body) {
235 dReal q[4];
236 q[0] = x;
237 q[1] = y;
238 q[2] = z;
239 q[3] = 0;
240 dNormalize3 (q);
241 if (axis1) {
242 dMULTIPLY1_331 (axis1,j->node[0].body->R,q);
243 axis1[3] = 0;
244 }
245 if (axis2) {
246 if (j->node[1].body) {
247 dMULTIPLY1_331 (axis2,j->node[1].body->R,q);
248 }
249 else {
250 axis2[0] = x;
251 axis2[1] = y;
252 axis2[2] = z;
253 }
254 axis2[3] = 0;
255 }
256 }
257}
258
259
260static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1)
261{
262 if (j->node[0].body) {
263 dMULTIPLY0_331 (result,j->node[0].body->R,anchor1);
264 result[0] += j->node[0].body->pos[0];
265 result[1] += j->node[0].body->pos[1];
266 result[2] += j->node[0].body->pos[2];
267 }
268}
269
270
271static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2)
272{
273 if (j->node[1].body) {
274 dMULTIPLY0_331 (result,j->node[1].body->R,anchor2);
275 result[0] += j->node[1].body->pos[0];
276 result[1] += j->node[1].body->pos[1];
277 result[2] += j->node[1].body->pos[2];
278 }
279 else {
280 result[0] = anchor2[0];
281 result[1] = anchor2[1];
282 result[2] = anchor2[2];
283 }
284}
285
286
287static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1)
288{
289 if (j->node[0].body) {
290 dMULTIPLY0_331 (result,j->node[0].body->R,axis1);
291 }
292}
293
294
295static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2)
296{
297 if (j->node[1].body) {
298 dMULTIPLY0_331 (result,j->node[1].body->R,axis2);
299 }
300 else {
301 result[0] = axis2[0];
302 result[1] = axis2[1];
303 result[2] = axis2[2];
304 }
305}
306
307
308static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis)
309{
310 // the angle between the two bodies is extracted from the quaternion that
311 // represents the relative rotation between them. recall that a quaternion
312 // q is:
313 // [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
314 // where s is a scalar and v is a 3-vector. u is a unit length axis and
315 // theta is a rotation along that axis. we can get theta/2 by:
316 // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
317 // but we can't get sin(theta/2) directly, only its absolute value, i.e.:
318 // |v| = |sin(theta/2)| * |u|
319 // = |sin(theta/2)|
320 // using this value will have a strange effect. recall that there are two
321 // quaternion representations of a given rotation, q and -q. typically as
322 // a body rotates along the axis it will go through a complete cycle using
323 // one representation and then the next cycle will use the other
324 // representation. this corresponds to u pointing in the direction of the
325 // hinge axis and then in the opposite direction. the result is that theta
326 // will appear to go "backwards" every other cycle. here is a fix: if u
327 // points "away" from the direction of the hinge (motor) axis (i.e. more
328 // than 90 degrees) then use -q instead of q. this represents the same
329 // rotation, but results in the cos(theta/2) value being sign inverted.
330
331 // extract the angle from the quaternion. cost2 = cos(theta/2),
332 // sint2 = |sin(theta/2)|
333 dReal cost2 = qrel[0];
334 dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]);
335 dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions
336 (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis
337 (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction
338
339 // the angle we get will be between 0..2*pi, but we want to return angles
340 // between -pi..pi
341 if (theta > M_PI) theta -= 2*M_PI;
342
343 // the angle we've just extracted has the wrong sign
344 theta = -theta;
345
346 return theta;
347}
348
349
350// given two bodies (body1,body2), the hinge axis that they are connected by
351// w.r.t. body1 (axis), and the initial relative orientation between them
352// (q_initial), return the relative rotation angle. the initial relative
353// orientation corresponds to an angle of zero. if body2 is 0 then measure the
354// angle between body1 and the static frame.
355//
356// this will not return the correct angle if the bodies rotate along any axis
357// other than the given hinge axis.
358
359static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis,
360 dQuaternion q_initial)
361{
362 // get qrel = relative rotation between the two bodies
363 dQuaternion qrel;
364 if (body2) {
365 dQuaternion qq;
366 dQMultiply1 (qq,body1->q,body2->q);
367 dQMultiply2 (qrel,qq,q_initial);
368 }
369 else {
370 // pretend body2->q is the identity
371 dQMultiply3 (qrel,body1->q,q_initial);
372 }
373
374 return getHingeAngleFromRelativeQuat (qrel,axis);
375}
376
377//****************************************************************************
378// dxJointLimitMotor
379
380void dxJointLimitMotor::init (dxWorld *world)
381{
382 vel = 0;
383 fmax = 0;
384 lostop = -dInfinity;
385 histop = dInfinity;
386 fudge_factor = 1;
387 normal_cfm = world->global_cfm;
388 stop_erp = world->global_erp;
389 stop_cfm = world->global_cfm;
390 bounce = 0;
391 limit = 0;
392 limit_err = 0;
393}
394
395
396void dxJointLimitMotor::set (int num, dReal value)
397{
398 switch (num) {
399 case dParamLoStop:
400 if (value <= histop) lostop = value;
401 break;
402 case dParamHiStop:
403 if (value >= lostop) histop = value;
404 break;
405 case dParamVel:
406 vel = value;
407 break;
408 case dParamFMax:
409 if (value >= 0) fmax = value;
410 break;
411 case dParamFudgeFactor:
412 if (value >= 0 && value <= 1) fudge_factor = value;
413 break;
414 case dParamBounce:
415 bounce = value;
416 break;
417 case dParamCFM:
418 normal_cfm = value;
419 break;
420 case dParamStopERP:
421 stop_erp = value;
422 break;
423 case dParamStopCFM:
424 stop_cfm = value;
425 break;
426 }
427}
428
429
430dReal dxJointLimitMotor::get (int num)
431{
432 switch (num) {
433 case dParamLoStop: return lostop;
434 case dParamHiStop: return histop;
435 case dParamVel: return vel;
436 case dParamFMax: return fmax;
437 case dParamFudgeFactor: return fudge_factor;
438 case dParamBounce: return bounce;
439 case dParamCFM: return normal_cfm;
440 case dParamStopERP: return stop_erp;
441 case dParamStopCFM: return stop_cfm;
442 default: return 0;
443 }
444}
445
446
447int dxJointLimitMotor::testRotationalLimit (dReal angle)
448{
449 if (angle <= lostop) {
450 limit = 1;
451 limit_err = angle - lostop;
452 return 1;
453 }
454 else if (angle >= histop) {
455 limit = 2;
456 limit_err = angle - histop;
457 return 1;
458 }
459 else {
460 limit = 0;
461 return 0;
462 }
463}
464
465
466int dxJointLimitMotor::addLimot (dxJoint *joint,
467 dxJoint::Info2 *info, int row,
468 dVector3 ax1, int rotational)
469{
470 int srow = row * info->rowskip;
471
472 // if the joint is powered, or has joint limits, add in the extra row
473 int powered = fmax > 0;
474 if (powered || limit) {
475 dReal *J1 = rotational ? info->J1a : info->J1l;
476 dReal *J2 = rotational ? info->J2a : info->J2l;
477
478 J1[srow+0] = ax1[0];
479 J1[srow+1] = ax1[1];
480 J1[srow+2] = ax1[2];
481 if (joint->node[1].body) {
482 J2[srow+0] = -ax1[0];
483 J2[srow+1] = -ax1[1];
484 J2[srow+2] = -ax1[2];
485 }
486
487 // linear limot torque decoupling step:
488 //
489 // if this is a linear limot (e.g. from a slider), we have to be careful
490 // that the linear constraint forces (+/- ax1) applied to the two bodies
491 // do not create a torque couple. in other words, the points that the
492 // constraint force is applied at must lie along the same ax1 axis.
493 // a torque couple will result in powered or limited slider-jointed free
494 // bodies from gaining angular momentum.
495 // the solution used here is to apply the constraint forces at the point
496 // halfway between the body centers. there is no penalty (other than an
497 // extra tiny bit of computation) in doing this adjustment. note that we
498 // only need to do this if the constraint connects two bodies.
499
500 dVector3 ltd; // Linear Torque Decoupling vector (a torque)
501 if (!rotational && joint->node[1].body) {
502 dVector3 c;
503 c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]);
504 c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]);
505 c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]);
506 dCROSS (ltd,=,c,ax1);
507 info->J1a[srow+0] = ltd[0];
508 info->J1a[srow+1] = ltd[1];
509 info->J1a[srow+2] = ltd[2];
510 info->J2a[srow+0] = ltd[0];
511 info->J2a[srow+1] = ltd[1];
512 info->J2a[srow+2] = ltd[2];
513 }
514
515 // if we're limited low and high simultaneously, the joint motor is
516 // ineffective
517 if (limit && (lostop == histop)) powered = 0;
518
519 if (powered) {
520 info->cfm[row] = normal_cfm;
521 if (! limit) {
522 info->c[row] = vel;
523 info->lo[row] = -fmax;
524 info->hi[row] = fmax;
525 }
526 else {
527 // the joint is at a limit, AND is being powered. if the joint is
528 // being powered into the limit then we apply the maximum motor force
529 // in that direction, because the motor is working against the
530 // immovable limit. if the joint is being powered away from the limit
531 // then we have problems because actually we need *two* lcp
532 // constraints to handle this case. so we fake it and apply some
533 // fraction of the maximum force. the fraction to use can be set as
534 // a fudge factor.
535
536 dReal fm = fmax;
537 if (vel > 0) fm = -fm;
538
539 // if we're powering away from the limit, apply the fudge factor
540 if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor;
541
542 if (rotational) {
543 dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],
544 -fm*ax1[2]);
545 if (joint->node[1].body)
546 dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
547 }
548 else {
549 dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]);
550 if (joint->node[1].body) {
551 dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
552
553 // linear limot torque decoupling step: refer to above discussion
554 dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1],
555 -fm*ltd[2]);
556 dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1],
557 -fm*ltd[2]);
558 }
559 }
560 }
561 }
562
563 if (limit) {
564 dReal k = info->fps * stop_erp;
565 info->c[row] = -k * limit_err;
566 info->cfm[row] = stop_cfm;
567
568 if (lostop == histop) {
569 // limited low and high simultaneously
570 info->lo[row] = -dInfinity;
571 info->hi[row] = dInfinity;
572 }
573 else {
574 if (limit == 1) {
575 // low limit
576 info->lo[row] = 0;
577 info->hi[row] = dInfinity;
578 }
579 else {
580 // high limit
581 info->lo[row] = -dInfinity;
582 info->hi[row] = 0;
583 }
584
585 // deal with bounce
586 if (bounce > 0) {
587 // calculate joint velocity
588 dReal vel;
589 if (rotational) {
590 vel = dDOT(joint->node[0].body->avel,ax1);
591 if (joint->node[1].body)
592 vel -= dDOT(joint->node[1].body->avel,ax1);
593 }
594 else {
595 vel = dDOT(joint->node[0].body->lvel,ax1);
596 if (joint->node[1].body)
597 vel -= dDOT(joint->node[1].body->lvel,ax1);
598 }
599
600 // only apply bounce if the velocity is incoming, and if the
601 // resulting c[] exceeds what we already have.
602 if (limit == 1) {
603 // low limit
604 if (vel < 0) {
605 dReal newc = -bounce * vel;
606 if (newc > info->c[row]) info->c[row] = newc;
607 }
608 }
609 else {
610 // high limit - all those computations are reversed
611 if (vel > 0) {
612 dReal newc = -bounce * vel;
613 if (newc < info->c[row]) info->c[row] = newc;
614 }
615 }
616 }
617 }
618 }
619 return 1;
620 }
621 else return 0;
622}
623
624//****************************************************************************
625// ball and socket
626
627static void ballInit (dxJointBall *j)
628{
629 dSetZero (j->anchor1,4);
630 dSetZero (j->anchor2,4);
631}
632
633
634static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info)
635{
636 info->m = 3;
637 info->nub = 3;
638}
639
640
641static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info)
642{
643 setBall (joint,info,joint->anchor1,joint->anchor2);
644}
645
646
647extern "C" void dJointSetBallAnchor (dxJointBall *joint,
648 dReal x, dReal y, dReal z)
649{
650 dUASSERT(joint,"bad joint argument");
651 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
652 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
653}
654
655
656extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result)
657{
658 dUASSERT(joint,"bad joint argument");
659 dUASSERT(result,"bad result argument");
660 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
661 if (joint->flags & dJOINT_REVERSE)
662 getAnchor2 (joint,result,joint->anchor2);
663 else
664 getAnchor (joint,result,joint->anchor1);
665}
666
667
668extern "C" void dJointGetBallAnchor2 (dxJointBall *joint, dVector3 result)
669{
670 dUASSERT(joint,"bad joint argument");
671 dUASSERT(result,"bad result argument");
672 dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
673 if (joint->flags & dJOINT_REVERSE)
674 getAnchor (joint,result,joint->anchor1);
675 else
676 getAnchor2 (joint,result,joint->anchor2);
677}
678
679
680dxJoint::Vtable __dball_vtable = {
681 sizeof(dxJointBall),
682 (dxJoint::init_fn*) ballInit,
683 (dxJoint::getInfo1_fn*) ballGetInfo1,
684 (dxJoint::getInfo2_fn*) ballGetInfo2,
685 dJointTypeBall};
686
687//****************************************************************************
688// hinge
689
690static void hingeInit (dxJointHinge *j)
691{
692 dSetZero (j->anchor1,4);
693 dSetZero (j->anchor2,4);
694 dSetZero (j->axis1,4);
695 j->axis1[0] = 1;
696 dSetZero (j->axis2,4);
697 j->axis2[0] = 1;
698 dSetZero (j->qrel,4);
699 j->limot.init (j->world);
700}
701
702
703static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info)
704{
705 info->nub = 5;
706
707 // see if joint is powered
708 if (j->limot.fmax > 0)
709 info->m = 6; // powered hinge needs an extra constraint row
710 else info->m = 5;
711
712 // see if we're at a joint limit.
713 if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) &&
714 j->limot.lostop <= j->limot.histop) {
715 dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1,
716 j->qrel);
717 if (j->limot.testRotationalLimit (angle)) info->m = 6;
718 }
719}
720
721
722static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info)
723{
724 // set the three ball-and-socket rows
725 setBall (joint,info,joint->anchor1,joint->anchor2);
726
727 // set the two hinge rows. the hinge axis should be the only unconstrained
728 // rotational axis, the angular velocity of the two bodies perpendicular to
729 // the hinge axis should be equal. thus the constraint equations are
730 // p*w1 - p*w2 = 0
731 // q*w1 - q*w2 = 0
732 // where p and q are unit vectors normal to the hinge axis, and w1 and w2
733 // are the angular velocity vectors of the two bodies.
734
735 dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
736 dVector3 p,q; // plane space vectors for ax1
737 dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
738 dPlaneSpace (ax1,p,q);
739
740 int s3=3*info->rowskip;
741 int s4=4*info->rowskip;
742
743 info->J1a[s3+0] = p[0];
744 info->J1a[s3+1] = p[1];
745 info->J1a[s3+2] = p[2];
746 info->J1a[s4+0] = q[0];
747 info->J1a[s4+1] = q[1];
748 info->J1a[s4+2] = q[2];
749
750 if (joint->node[1].body) {
751 info->J2a[s3+0] = -p[0];
752 info->J2a[s3+1] = -p[1];
753 info->J2a[s3+2] = -p[2];
754 info->J2a[s4+0] = -q[0];
755 info->J2a[s4+1] = -q[1];
756 info->J2a[s4+2] = -q[2];
757 }
758
759 // compute the right hand side of the constraint equation. set relative
760 // body velocities along p and q to bring the hinge back into alignment.
761 // if ax1,ax2 are the unit length hinge axes as computed from body1 and
762 // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
763 // if `theta' is the angle between ax1 and ax2, we need an angular velocity
764 // along u to cover angle erp*theta in one step :
765 // |angular_velocity| = angle/time = erp*theta / stepsize
766 // = (erp*fps) * theta
767 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
768 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
769 // ...as ax1 and ax2 are unit length. if theta is smallish,
770 // theta ~= sin(theta), so
771 // angular_velocity = (erp*fps) * (ax1 x ax2)
772 // ax1 x ax2 is in the plane space of ax1, so we project the angular
773 // velocity to p and q to find the right hand side.
774
775 dVector3 ax2,b;
776 if (joint->node[1].body) {
777 dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
778 }
779 else {
780 ax2[0] = joint->axis2[0];
781 ax2[1] = joint->axis2[1];
782 ax2[2] = joint->axis2[2];
783 }
784 dCROSS (b,=,ax1,ax2);
785 dReal k = info->fps * info->erp;
786 info->c[3] = k * dDOT(b,p);
787 info->c[4] = k * dDOT(b,q);
788
789 // if the hinge is powered, or has joint limits, add in the stuff
790 joint->limot.addLimot (joint,info,5,ax1,1);
791}
792
793
794// compute initial relative rotation body1 -> body2, or env -> body1
795
796static void hingeComputeInitialRelativeRotation (dxJointHinge *joint)
797{
798 if (joint->node[0].body) {
799 if (joint->node[1].body) {
800 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
801 }
802 else {
803 // set joint->qrel to the transpose of the first body q
804 joint->qrel[0] = joint->node[0].body->q[0];
805 for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
806 }
807 }
808}
809
810
811extern "C" void dJointSetHingeAnchor (dxJointHinge *joint,
812 dReal x, dReal y, dReal z)
813{
814 dUASSERT(joint,"bad joint argument");
815 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
816 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
817 hingeComputeInitialRelativeRotation (joint);
818}
819
820
821extern "C" void dJointSetHingeAxis (dxJointHinge *joint,
822 dReal x, dReal y, dReal z)
823{
824 dUASSERT(joint,"bad joint argument");
825 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
826 setAxes (joint,x,y,z,joint->axis1,joint->axis2);
827 hingeComputeInitialRelativeRotation (joint);
828}
829
830
831extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result)
832{
833 dUASSERT(joint,"bad joint argument");
834 dUASSERT(result,"bad result argument");
835 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
836 if (joint->flags & dJOINT_REVERSE)
837 getAnchor2 (joint,result,joint->anchor2);
838 else
839 getAnchor (joint,result,joint->anchor1);
840}
841
842
843extern "C" void dJointGetHingeAnchor2 (dxJointHinge *joint, dVector3 result)
844{
845 dUASSERT(joint,"bad joint argument");
846 dUASSERT(result,"bad result argument");
847 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
848 if (joint->flags & dJOINT_REVERSE)
849 getAnchor (joint,result,joint->anchor1);
850 else
851 getAnchor2 (joint,result,joint->anchor2);
852}
853
854
855extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result)
856{
857 dUASSERT(joint,"bad joint argument");
858 dUASSERT(result,"bad result argument");
859 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
860 getAxis (joint,result,joint->axis1);
861}
862
863
864extern "C" void dJointSetHingeParam (dxJointHinge *joint,
865 int parameter, dReal value)
866{
867 dUASSERT(joint,"bad joint argument");
868 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
869 joint->limot.set (parameter,value);
870}
871
872
873extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter)
874{
875 dUASSERT(joint,"bad joint argument");
876 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
877 return joint->limot.get (parameter);
878}
879
880
881extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint)
882{
883 dAASSERT(joint);
884 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
885 if (joint->node[0].body) {
886 dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1,
887 joint->qrel);
888 if (joint->flags & dJOINT_REVERSE)
889 return -ang;
890 else
891 return ang;
892 }
893 else return 0;
894}
895
896
897extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint)
898{
899 dAASSERT(joint);
900 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge");
901 if (joint->node[0].body) {
902 dVector3 axis;
903 dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
904 dReal rate = dDOT(axis,joint->node[0].body->avel);
905 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
906 if (joint->flags & dJOINT_REVERSE) rate = - rate;
907 return rate;
908 }
909 else return 0;
910}
911
912
913extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque)
914{
915 dVector3 axis;
916 dAASSERT(joint);
917 dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge");
918
919 if (joint->flags & dJOINT_REVERSE)
920 torque = -torque;
921
922 getAxis (joint,axis,joint->axis1);
923 axis[0] *= torque;
924 axis[1] *= torque;
925 axis[2] *= torque;
926
927 if (joint->node[0].body != 0)
928 dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]);
929 if (joint->node[1].body != 0)
930 dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
931}
932
933
934dxJoint::Vtable __dhinge_vtable = {
935 sizeof(dxJointHinge),
936 (dxJoint::init_fn*) hingeInit,
937 (dxJoint::getInfo1_fn*) hingeGetInfo1,
938 (dxJoint::getInfo2_fn*) hingeGetInfo2,
939 dJointTypeHinge};
940
941//****************************************************************************
942// slider
943
944static void sliderInit (dxJointSlider *j)
945{
946 dSetZero (j->axis1,4);
947 j->axis1[0] = 1;
948 dSetZero (j->qrel,4);
949 dSetZero (j->offset,4);
950 j->limot.init (j->world);
951}
952
953
954extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint)
955{
956 dUASSERT(joint,"bad joint argument");
957 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
958
959 // get axis1 in global coordinates
960 dVector3 ax1,q;
961 dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
962
963 if (joint->node[1].body) {
964 // get body2 + offset point in global coordinates
965 dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset);
966 for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] -
967 joint->node[1].body->pos[i];
968 }
969 else {
970 for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] -
971 joint->offset[i];
972
973 }
974 return dDOT(ax1,q);
975}
976
977
978extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint)
979{
980 dUASSERT(joint,"bad joint argument");
981 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
982
983 // get axis1 in global coordinates
984 dVector3 ax1;
985 dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
986
987 if (joint->node[1].body) {
988 return dDOT(ax1,joint->node[0].body->lvel) -
989 dDOT(ax1,joint->node[1].body->lvel);
990 }
991 else {
992 return dDOT(ax1,joint->node[0].body->lvel);
993 }
994}
995
996
997static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info)
998{
999 info->nub = 5;
1000
1001 // see if joint is powered
1002 if (j->limot.fmax > 0)
1003 info->m = 6; // powered slider needs an extra constraint row
1004 else info->m = 5;
1005
1006 // see if we're at a joint limit.
1007 j->limot.limit = 0;
1008 if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) &&
1009 j->limot.lostop <= j->limot.histop) {
1010 // measure joint position
1011 dReal pos = dJointGetSliderPosition (j);
1012 if (pos <= j->limot.lostop) {
1013 j->limot.limit = 1;
1014 j->limot.limit_err = pos - j->limot.lostop;
1015 info->m = 6;
1016 }
1017 else if (pos >= j->limot.histop) {
1018 j->limot.limit = 2;
1019 j->limot.limit_err = pos - j->limot.histop;
1020 info->m = 6;
1021 }
1022 }
1023}
1024
1025
1026static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info)
1027{
1028 int i,s = info->rowskip;
1029 int s2=2*s,s3=3*s,s4=4*s;
1030
1031 // pull out pos and R for both bodies. also get the `connection'
1032 // vector pos2-pos1.
1033
1034 dReal *pos1,*pos2,*R1,*R2;
1035 dVector3 c;
1036 pos1 = joint->node[0].body->pos;
1037 R1 = joint->node[0].body->R;
1038 if (joint->node[1].body) {
1039 pos2 = joint->node[1].body->pos;
1040 R2 = joint->node[1].body->R;
1041 for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i];
1042 }
1043 else {
1044 pos2 = 0;
1045 R2 = 0;
1046 }
1047
1048 // 3 rows to make body rotations equal
1049 setFixedOrientation(joint, info, joint->qrel, 0);
1050
1051 // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
1052 // result in three equations, so we project along the planespace vectors
1053 // so that sliding along the slider axis is disregarded. for symmetry we
1054 // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
1055
1056 dVector3 ax1; // joint axis in global coordinates (unit length)
1057 dVector3 p,q; // plane space of ax1
1058 dMULTIPLY0_331 (ax1,R1,joint->axis1);
1059 dPlaneSpace (ax1,p,q);
1060 if (joint->node[1].body) {
1061 dVector3 tmp;
1062 dCROSS (tmp, = REAL(0.5) * ,c,p);
1063 for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
1064 for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
1065 dCROSS (tmp, = REAL(0.5) * ,c,q);
1066 for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i];
1067 for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i];
1068 for (i=0; i<3; i++) info->J2l[s3+i] = -p[i];
1069 for (i=0; i<3; i++) info->J2l[s4+i] = -q[i];
1070 }
1071 for (i=0; i<3; i++) info->J1l[s3+i] = p[i];
1072 for (i=0; i<3; i++) info->J1l[s4+i] = q[i];
1073
1074 // compute last two elements of right hand side. we want to align the offset
1075 // point (in body 2's frame) with the center of body 1.
1076 dReal k = info->fps * info->erp;
1077 if (joint->node[1].body) {
1078 dVector3 ofs; // offset point in global coordinates
1079 dMULTIPLY0_331 (ofs,R2,joint->offset);
1080 for (i=0; i<3; i++) c[i] += ofs[i];
1081 info->c[3] = k * dDOT(p,c);
1082 info->c[4] = k * dDOT(q,c);
1083 }
1084 else {
1085 dVector3 ofs; // offset point in global coordinates
1086 for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i];
1087 info->c[3] = k * dDOT(p,ofs);
1088 info->c[4] = k * dDOT(q,ofs);
1089 }
1090
1091 // if the slider is powered, or has joint limits, add in the extra row
1092 joint->limot.addLimot (joint,info,5,ax1,0);
1093}
1094
1095
1096extern "C" void dJointSetSliderAxis (dxJointSlider *joint,
1097 dReal x, dReal y, dReal z)
1098{
1099 int i;
1100 dUASSERT(joint,"bad joint argument");
1101 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1102 setAxes (joint,x,y,z,joint->axis1,0);
1103
1104 // compute initial relative rotation body1 -> body2, or env -> body1
1105 // also compute center of body1 w.r.t body 2
1106 if (joint->node[1].body) {
1107 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
1108 dVector3 c;
1109 for (i=0; i<3; i++)
1110 c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i];
1111 dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c);
1112 }
1113 else {
1114 // set joint->qrel to the transpose of the first body's q
1115 joint->qrel[0] = joint->node[0].body->q[0];
1116 for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
1117 for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i];
1118 }
1119}
1120
1121
1122extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result)
1123{
1124 dUASSERT(joint,"bad joint argument");
1125 dUASSERT(result,"bad result argument");
1126 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1127 getAxis (joint,result,joint->axis1);
1128}
1129
1130
1131extern "C" void dJointSetSliderParam (dxJointSlider *joint,
1132 int parameter, dReal value)
1133{
1134 dUASSERT(joint,"bad joint argument");
1135 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1136 joint->limot.set (parameter,value);
1137}
1138
1139
1140extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter)
1141{
1142 dUASSERT(joint,"bad joint argument");
1143 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1144 return joint->limot.get (parameter);
1145}
1146
1147
1148extern "C" void dJointAddSliderForce (dxJointSlider *joint, dReal force)
1149{
1150 dVector3 axis;
1151 dUASSERT(joint,"bad joint argument");
1152 dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
1153
1154 if (joint->flags & dJOINT_REVERSE)
1155 force -= force;
1156
1157 getAxis (joint,axis,joint->axis1);
1158 axis[0] *= force;
1159 axis[1] *= force;
1160 axis[2] *= force;
1161
1162 if (joint->node[0].body != 0)
1163 dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]);
1164 if (joint->node[1].body != 0)
1165 dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
1166}
1167
1168
1169dxJoint::Vtable __dslider_vtable = {
1170 sizeof(dxJointSlider),
1171 (dxJoint::init_fn*) sliderInit,
1172 (dxJoint::getInfo1_fn*) sliderGetInfo1,
1173 (dxJoint::getInfo2_fn*) sliderGetInfo2,
1174 dJointTypeSlider};
1175
1176//****************************************************************************
1177// contact
1178
1179static void contactInit (dxJointContact *j)
1180{
1181 // default frictionless contact. hmmm, this info gets overwritten straight
1182 // away anyway, so why bother?
1183#if 0 /* so don't bother ;) */
1184 j->contact.surface.mode = 0;
1185 j->contact.surface.mu = 0;
1186 dSetZero (j->contact.geom.pos,4);
1187 dSetZero (j->contact.geom.normal,4);
1188 j->contact.geom.depth = 0;
1189#endif
1190}
1191
1192
1193static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info)
1194{
1195 // make sure mu's >= 0, then calculate number of constraint rows and number
1196 // of unbounded rows.
1197 int m = 1, nub=0;
1198 if (j->contact.surface.mu < 0) j->contact.surface.mu = 0;
1199 if (j->contact.surface.mode & dContactMu2) {
1200 if (j->contact.surface.mu > 0) m++;
1201 if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0;
1202 if (j->contact.surface.mu2 > 0) m++;
1203 if (j->contact.surface.mu == dInfinity) nub ++;
1204 if (j->contact.surface.mu2 == dInfinity) nub ++;
1205 }
1206 else {
1207 if (j->contact.surface.mu > 0) m += 2;
1208 if (j->contact.surface.mu == dInfinity) nub += 2;
1209 }
1210
1211 j->the_m = m;
1212 info->m = m;
1213 info->nub = nub;
1214}
1215
1216
1217static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info)
1218{
1219 int i,s = info->rowskip;
1220 int s2 = 2*s;
1221
1222 // get normal, with sign adjusted for body1/body2 polarity
1223 dVector3 normal;
1224 if (j->flags & dJOINT_REVERSE) {
1225 normal[0] = - j->contact.geom.normal[0];
1226 normal[1] = - j->contact.geom.normal[1];
1227 normal[2] = - j->contact.geom.normal[2];
1228 }
1229 else {
1230 normal[0] = j->contact.geom.normal[0];
1231 normal[1] = j->contact.geom.normal[1];
1232 normal[2] = j->contact.geom.normal[2];
1233 }
1234 normal[3] = 0; // @@@ hmmm
1235
1236 // c1,c2 = contact points with respect to body PORs
1237 dVector3 c1,c2;
1238 for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i];
1239
1240 // set jacobian for normal
1241 info->J1l[0] = normal[0];
1242 info->J1l[1] = normal[1];
1243 info->J1l[2] = normal[2];
1244 dCROSS (info->J1a,=,c1,normal);
1245 if (j->node[1].body) {
1246 for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
1247 j->node[1].body->pos[i];
1248 info->J2l[0] = -normal[0];
1249 info->J2l[1] = -normal[1];
1250 info->J2l[2] = -normal[2];
1251 dCROSS (info->J2a,= -,c2,normal);
1252 }
1253
1254 // set right hand side and cfm value for normal
1255 dReal erp = info->erp;
1256 if (j->contact.surface.mode & dContactSoftERP)
1257 erp = j->contact.surface.soft_erp;
1258 dReal k = info->fps * erp;
1259 info->c[0] = k*j->contact.geom.depth;
1260 if (j->contact.surface.mode & dContactSoftCFM)
1261 info->cfm[0] = j->contact.surface.soft_cfm;
1262
1263 // deal with bounce
1264 if (j->contact.surface.mode & dContactBounce) {
1265 // calculate outgoing velocity (-ve for incoming contact)
1266 dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) +
1267 dDOT(info->J1a,j->node[0].body->avel);
1268 if (j->node[1].body) {
1269 outgoing += dDOT(info->J2l,j->node[1].body->lvel) +
1270 dDOT(info->J2a,j->node[1].body->avel);
1271 }
1272 // only apply bounce if the outgoing velocity is greater than the
1273 // threshold, and if the resulting c[0] exceeds what we already have.
1274 if (j->contact.surface.bounce_vel >= 0 &&
1275 (-outgoing) > j->contact.surface.bounce_vel) {
1276 dReal newc = - j->contact.surface.bounce * outgoing;
1277 if (newc > info->c[0]) info->c[0] = newc;
1278 }
1279 }
1280
1281 // set LCP limits for normal
1282 info->lo[0] = 0;
1283 info->hi[0] = dInfinity;
1284
1285 // now do jacobian for tangential forces
1286 dVector3 t1,t2; // two vectors tangential to normal
1287
1288 // first friction direction
1289 if (j->the_m >= 2) {
1290 if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ?
1291 t1[0] = j->contact.fdir1[0];
1292 t1[1] = j->contact.fdir1[1];
1293 t1[2] = j->contact.fdir1[2];
1294 dCROSS (t2,=,normal,t1);
1295 }
1296 else {
1297 dPlaneSpace (normal,t1,t2);
1298 }
1299 info->J1l[s+0] = t1[0];
1300 info->J1l[s+1] = t1[1];
1301 info->J1l[s+2] = t1[2];
1302 dCROSS (info->J1a+s,=,c1,t1);
1303 if (j->node[1].body) {
1304 info->J2l[s+0] = -t1[0];
1305 info->J2l[s+1] = -t1[1];
1306 info->J2l[s+2] = -t1[2];
1307 dCROSS (info->J2a+s,= -,c2,t1);
1308 }
1309 // set right hand side
1310 if (j->contact.surface.mode & dContactMotion1) {
1311 info->c[1] = j->contact.surface.motion1;
1312 }
1313 // set LCP bounds and friction index. this depends on the approximation
1314 // mode
1315 info->lo[1] = -j->contact.surface.mu;
1316 info->hi[1] = j->contact.surface.mu;
1317 if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0;
1318
1319 // set slip (constraint force mixing)
1320 if (j->contact.surface.mode & dContactSlip1)
1321 info->cfm[1] = j->contact.surface.slip1;
1322 }
1323
1324 // second friction direction
1325 if (j->the_m >= 3) {
1326 info->J1l[s2+0] = t2[0];
1327 info->J1l[s2+1] = t2[1];
1328 info->J1l[s2+2] = t2[2];
1329 dCROSS (info->J1a+s2,=,c1,t2);
1330 if (j->node[1].body) {
1331 info->J2l[s2+0] = -t2[0];
1332 info->J2l[s2+1] = -t2[1];
1333 info->J2l[s2+2] = -t2[2];
1334 dCROSS (info->J2a+s2,= -,c2,t2);
1335 }
1336 // set right hand side
1337 if (j->contact.surface.mode & dContactMotion2) {
1338 info->c[2] = j->contact.surface.motion2;
1339 }
1340 // set LCP bounds and friction index. this depends on the approximation
1341 // mode
1342 if (j->contact.surface.mode & dContactMu2) {
1343 info->lo[2] = -j->contact.surface.mu2;
1344 info->hi[2] = j->contact.surface.mu2;
1345 }
1346 else {
1347 info->lo[2] = -j->contact.surface.mu;
1348 info->hi[2] = j->contact.surface.mu;
1349 }
1350 if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0;
1351
1352 // set slip (constraint force mixing)
1353 if (j->contact.surface.mode & dContactSlip2)
1354 info->cfm[2] = j->contact.surface.slip2;
1355 }
1356}
1357
1358
1359dxJoint::Vtable __dcontact_vtable = {
1360 sizeof(dxJointContact),
1361 (dxJoint::init_fn*) contactInit,
1362 (dxJoint::getInfo1_fn*) contactGetInfo1,
1363 (dxJoint::getInfo2_fn*) contactGetInfo2,
1364 dJointTypeContact};
1365
1366//****************************************************************************
1367// hinge 2. note that this joint must be attached to two bodies for it to work
1368
1369static dReal measureHinge2Angle (dxJointHinge2 *joint)
1370{
1371 dVector3 a1,a2;
1372 dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2);
1373 dMULTIPLY1_331 (a2,joint->node[0].body->R,a1);
1374 dReal x = dDOT(joint->v1,a2);
1375 dReal y = dDOT(joint->v2,a2);
1376 return -dAtan2 (y,x);
1377}
1378
1379
1380static void hinge2Init (dxJointHinge2 *j)
1381{
1382 dSetZero (j->anchor1,4);
1383 dSetZero (j->anchor2,4);
1384 dSetZero (j->axis1,4);
1385 j->axis1[0] = 1;
1386 dSetZero (j->axis2,4);
1387 j->axis2[1] = 1;
1388 j->c0 = 0;
1389 j->s0 = 0;
1390
1391 dSetZero (j->v1,4);
1392 j->v1[0] = 1;
1393 dSetZero (j->v2,4);
1394 j->v2[1] = 1;
1395
1396 j->limot1.init (j->world);
1397 j->limot2.init (j->world);
1398
1399 j->susp_erp = j->world->global_erp;
1400 j->susp_cfm = j->world->global_cfm;
1401
1402 j->flags |= dJOINT_TWOBODIES;
1403}
1404
1405
1406static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info)
1407{
1408 info->m = 4;
1409 info->nub = 4;
1410
1411 // see if we're powered or at a joint limit for axis 1
1412 int atlimit=0;
1413 if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) &&
1414 j->limot1.lostop <= j->limot1.histop) {
1415 dReal angle = measureHinge2Angle (j);
1416 if (j->limot1.testRotationalLimit (angle)) atlimit = 1;
1417 }
1418 if (atlimit || j->limot1.fmax > 0) info->m++;
1419
1420 // see if we're powering axis 2 (we currently never limit this axis)
1421 j->limot2.limit = 0;
1422 if (j->limot2.fmax > 0) info->m++;
1423}
1424
1425
1426// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
1427// relative to body 1 and 2 initially) and then computes the constrained
1428// rotational axis as the cross product of ax1 and ax2.
1429// the sin and cos of the angle between axis 1 and 2 is computed, this comes
1430// from dot and cross product rules.
1431
1432#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \
1433 dVector3 ax1,ax2; \
1434 dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \
1435 dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \
1436 dCROSS (axis,=,ax1,ax2); \
1437 sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \
1438 cos_angle = dDOT (ax1,ax2);
1439
1440
1441static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info)
1442{
1443 // get information we need to set the hinge row
1444 dReal s,c;
1445 dVector3 q;
1446 HINGE2_GET_AXIS_INFO (q,s,c);
1447 dNormalize3 (q); // @@@ quicker: divide q by s ?
1448
1449 // set the three ball-and-socket rows (aligned to the suspension axis ax1)
1450 setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp);
1451
1452 // set the hinge row
1453 int s3=3*info->rowskip;
1454 info->J1a[s3+0] = q[0];
1455 info->J1a[s3+1] = q[1];
1456 info->J1a[s3+2] = q[2];
1457 if (joint->node[1].body) {
1458 info->J2a[s3+0] = -q[0];
1459 info->J2a[s3+1] = -q[1];
1460 info->J2a[s3+2] = -q[2];
1461 }
1462
1463 // compute the right hand side for the constrained rotational DOF.
1464 // axis 1 and axis 2 are separated by an angle `theta'. the desired
1465 // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
1466 // in the joint structure. the correcting angular velocity is:
1467 // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
1468 // = (erp*fps) * (theta0-theta)
1469 // (theta0-theta) can be computed using the following small-angle-difference
1470 // approximation:
1471 // theta0-theta ~= tan(theta0-theta)
1472 // = sin(theta0-theta)/cos(theta0-theta)
1473 // = (c*s0 - s*c0) / (c*c0 + s*s0)
1474 // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1
1475 // where c = cos(theta), s = sin(theta)
1476 // c0 = cos(theta0), s0 = sin(theta0)
1477
1478 dReal k = info->fps * info->erp;
1479 info->c[3] = k * (joint->c0 * s - joint->s0 * c);
1480
1481 // if the axis1 hinge is powered, or has joint limits, add in more stuff
1482 int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
1483
1484 // if the axis2 hinge is powered, add in more stuff
1485 joint->limot2.addLimot (joint,info,row,ax2,1);
1486
1487 // set parameter for the suspension
1488 info->cfm[0] = joint->susp_cfm;
1489}
1490
1491
1492// compute vectors v1 and v2 (embedded in body1), used to measure angle
1493// between body 1 and body 2
1494
1495static void makeHinge2V1andV2 (dxJointHinge2 *joint)
1496{
1497 if (joint->node[0].body) {
1498 // get axis 1 and 2 in global coords
1499 dVector3 ax1,ax2,v;
1500 dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
1501 dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
1502
1503 // don't do anything if the axis1 or axis2 vectors are zero or the same
1504 if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) ||
1505 (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) ||
1506 (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return;
1507
1508 // modify axis 2 so it's perpendicular to axis 1
1509 dReal k = dDOT(ax1,ax2);
1510 for (int i=0; i<3; i++) ax2[i] -= k*ax1[i];
1511 dNormalize3 (ax2);
1512
1513 // make v1 = modified axis2, v2 = axis1 x (modified axis2)
1514 dCROSS (v,=,ax1,ax2);
1515 dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2);
1516 dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v);
1517 }
1518}
1519
1520
1521extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint,
1522 dReal x, dReal y, dReal z)
1523{
1524 dUASSERT(joint,"bad joint argument");
1525 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1526 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
1527 makeHinge2V1andV2 (joint);
1528}
1529
1530
1531extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint,
1532 dReal x, dReal y, dReal z)
1533{
1534 dUASSERT(joint,"bad joint argument");
1535 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1536 if (joint->node[0].body) {
1537 dReal q[4];
1538 q[0] = x;
1539 q[1] = y;
1540 q[2] = z;
1541 q[3] = 0;
1542 dNormalize3 (q);
1543 dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
1544 joint->axis1[3] = 0;
1545
1546 // compute the sin and cos of the angle between axis 1 and axis 2
1547 dVector3 ax;
1548 HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
1549 }
1550 makeHinge2V1andV2 (joint);
1551}
1552
1553
1554extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint,
1555 dReal x, dReal y, dReal z)
1556{
1557 dUASSERT(joint,"bad joint argument");
1558 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1559 if (joint->node[1].body) {
1560 dReal q[4];
1561 q[0] = x;
1562 q[1] = y;
1563 q[2] = z;
1564 q[3] = 0;
1565 dNormalize3 (q);
1566 dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
1567 joint->axis1[3] = 0;
1568
1569 // compute the sin and cos of the angle between axis 1 and axis 2
1570 dVector3 ax;
1571 HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
1572 }
1573 makeHinge2V1andV2 (joint);
1574}
1575
1576
1577extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint,
1578 int parameter, dReal value)
1579{
1580 dUASSERT(joint,"bad joint argument");
1581 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1582 if ((parameter & 0xff00) == 0x100) {
1583 joint->limot2.set (parameter & 0xff,value);
1584 }
1585 else {
1586 if (parameter == dParamSuspensionERP) joint->susp_erp = value;
1587 else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value;
1588 else joint->limot1.set (parameter,value);
1589 }
1590}
1591
1592
1593extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result)
1594{
1595 dUASSERT(joint,"bad joint argument");
1596 dUASSERT(result,"bad result argument");
1597 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1598 if (joint->flags & dJOINT_REVERSE)
1599 getAnchor2 (joint,result,joint->anchor2);
1600 else
1601 getAnchor (joint,result,joint->anchor1);
1602}
1603
1604
1605extern "C" void dJointGetHinge2Anchor2 (dxJointHinge2 *joint, dVector3 result)
1606{
1607 dUASSERT(joint,"bad joint argument");
1608 dUASSERT(result,"bad result argument");
1609 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1610 if (joint->flags & dJOINT_REVERSE)
1611 getAnchor (joint,result,joint->anchor1);
1612 else
1613 getAnchor2 (joint,result,joint->anchor2);
1614}
1615
1616
1617extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result)
1618{
1619 dUASSERT(joint,"bad joint argument");
1620 dUASSERT(result,"bad result argument");
1621 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1622 if (joint->node[0].body) {
1623 dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1);
1624 }
1625}
1626
1627
1628extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result)
1629{
1630 dUASSERT(joint,"bad joint argument");
1631 dUASSERT(result,"bad result argument");
1632 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1633 if (joint->node[1].body) {
1634 dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2);
1635 }
1636}
1637
1638
1639extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter)
1640{
1641 dUASSERT(joint,"bad joint argument");
1642 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1643 if ((parameter & 0xff00) == 0x100) {
1644 return joint->limot2.get (parameter & 0xff);
1645 }
1646 else {
1647 if (parameter == dParamSuspensionERP) return joint->susp_erp;
1648 else if (parameter == dParamSuspensionCFM) return joint->susp_cfm;
1649 else return joint->limot1.get (parameter);
1650 }
1651}
1652
1653
1654extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint)
1655{
1656 dUASSERT(joint,"bad joint argument");
1657 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1658 if (joint->node[0].body) return measureHinge2Angle (joint);
1659 else return 0;
1660}
1661
1662
1663extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint)
1664{
1665 dUASSERT(joint,"bad joint argument");
1666 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1667 if (joint->node[0].body) {
1668 dVector3 axis;
1669 dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
1670 dReal rate = dDOT(axis,joint->node[0].body->avel);
1671 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
1672 return rate;
1673 }
1674 else return 0;
1675}
1676
1677
1678extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint)
1679{
1680 dUASSERT(joint,"bad joint argument");
1681 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1682 if (joint->node[0].body && joint->node[1].body) {
1683 dVector3 axis;
1684 dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2);
1685 dReal rate = dDOT(axis,joint->node[0].body->avel);
1686 if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
1687 return rate;
1688 }
1689 else return 0;
1690}
1691
1692
1693extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dReal torque2)
1694{
1695 dVector3 axis1, axis2;
1696 dUASSERT(joint,"bad joint argument");
1697 dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
1698
1699 if (joint->node[0].body && joint->node[1].body) {
1700 dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1);
1701 dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2);
1702 axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
1703 axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
1704 axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
1705 dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]);
1706 dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]);
1707 }
1708}
1709
1710
1711dxJoint::Vtable __dhinge2_vtable = {
1712 sizeof(dxJointHinge2),
1713 (dxJoint::init_fn*) hinge2Init,
1714 (dxJoint::getInfo1_fn*) hinge2GetInfo1,
1715 (dxJoint::getInfo2_fn*) hinge2GetInfo2,
1716 dJointTypeHinge2};
1717
1718//****************************************************************************
1719// universal
1720
1721// I just realized that the universal joint is equivalent to a hinge 2 joint with
1722// perfectly stiff suspension. By comparing the hinge 2 implementation to
1723// the universal implementation, you may be able to improve this
1724// implementation (or, less likely, the hinge2 implementation).
1725
1726static void universalInit (dxJointUniversal *j)
1727{
1728 dSetZero (j->anchor1,4);
1729 dSetZero (j->anchor2,4);
1730 dSetZero (j->axis1,4);
1731 j->axis1[0] = 1;
1732 dSetZero (j->axis2,4);
1733 j->axis2[1] = 1;
1734 dSetZero(j->qrel1,4);
1735 dSetZero(j->qrel2,4);
1736 j->limot1.init (j->world);
1737 j->limot2.init (j->world);
1738}
1739
1740
1741static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2)
1742{
1743 // This says "ax1 = joint->node[0].body->R * joint->axis1"
1744 dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
1745
1746 if (joint->node[1].body) {
1747 dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
1748 }
1749 else {
1750 ax2[0] = joint->axis2[0];
1751 ax2[1] = joint->axis2[1];
1752 ax2[2] = joint->axis2[2];
1753 }
1754}
1755
1756
1757static dReal getUniversalAngle1(dxJointUniversal *joint)
1758{
1759 if (joint->node[0].body) {
1760 // length 1 joint axis in global coordinates, from each body
1761 dVector3 ax1, ax2;
1762 dMatrix3 R;
1763 dQuaternion qcross, qq, qrel;
1764
1765 getUniversalAxes (joint,ax1,ax2);
1766
1767 // It should be possible to get both angles without explicitly
1768 // constructing the rotation matrix of the cross. Basically,
1769 // orientation of the cross about axis1 comes from body 2,
1770 // about axis 2 comes from body 1, and the perpendicular
1771 // axis can come from the two bodies somehow. (We don't really
1772 // want to assume it's 90 degrees, because in general the
1773 // constraints won't be perfectly satisfied, or even very well
1774 // satisfied.)
1775 //
1776 // However, we'd need a version of getHingeAngleFromRElativeQuat()
1777 // that CAN handle when its relative quat is rotated along a direction
1778 // other than the given axis. What I have here works,
1779 // although it's probably much slower than need be.
1780
1781 dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
1782 dRtoQ (R,qcross);
1783
1784 // This code is essential the same as getHingeAngle(), see the comments
1785 // there for details.
1786
1787 // get qrel = relative rotation between node[0] and the cross
1788 dQMultiply1 (qq,joint->node[0].body->q,qcross);
1789 dQMultiply2 (qrel,qq,joint->qrel1);
1790
1791 return getHingeAngleFromRelativeQuat(qrel, joint->axis1);
1792 }
1793 return 0;
1794}
1795
1796
1797static dReal getUniversalAngle2(dxJointUniversal *joint)
1798{
1799 if (joint->node[0].body) {
1800 // length 1 joint axis in global coordinates, from each body
1801 dVector3 ax1, ax2;
1802 dMatrix3 R;
1803 dQuaternion qcross, qq, qrel;
1804
1805 getUniversalAxes (joint,ax1,ax2);
1806
1807 // It should be possible to get both angles without explicitly
1808 // constructing the rotation matrix of the cross. Basically,
1809 // orientation of the cross about axis1 comes from body 2,
1810 // about axis 2 comes from body 1, and the perpendicular
1811 // axis can come from the two bodies somehow. (We don't really
1812 // want to assume it's 90 degrees, because in general the
1813 // constraints won't be perfectly satisfied, or even very well
1814 // satisfied.)
1815 //
1816 // However, we'd need a version of getHingeAngleFromRElativeQuat()
1817 // that CAN handle when its relative quat is rotated along a direction
1818 // other than the given axis. What I have here works,
1819 // although it's probably much slower than need be.
1820
1821 dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
1822 dRtoQ(R, qcross);
1823
1824 if (joint->node[1].body) {
1825 dQMultiply1 (qq, joint->node[1].body->q, qcross);
1826 dQMultiply2 (qrel,qq,joint->qrel2);
1827 }
1828 else {
1829 // pretend joint->node[1].body->q is the identity
1830 dQMultiply2 (qrel,qcross, joint->qrel2);
1831 }
1832
1833 return - getHingeAngleFromRelativeQuat(qrel, joint->axis2);
1834 }
1835 return 0;
1836}
1837
1838
1839static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info)
1840{
1841 info->nub = 4;
1842 info->m = 4;
1843
1844 // see if we're powered or at a joint limit.
1845 bool constraint1 = j->limot1.fmax > 0;
1846 bool constraint2 = j->limot2.fmax > 0;
1847
1848 bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) &&
1849 j->limot1.lostop <= j->limot1.histop;
1850 bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) &&
1851 j->limot2.lostop <= j->limot2.histop;
1852
1853 // We need to call testRotationLimit() even if we're motored, since it
1854 // records the result.
1855 if (limiting1 || limiting2) {
1856 dReal angle1, angle2;
1857 angle1 = getUniversalAngle1(j);
1858 angle2 = getUniversalAngle2(j);
1859 if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true;
1860 if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true;
1861 }
1862 if (constraint1)
1863 info->m++;
1864 if (constraint2)
1865 info->m++;
1866}
1867
1868
1869static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info)
1870{
1871 // set the three ball-and-socket rows
1872 setBall (joint,info,joint->anchor1,joint->anchor2);
1873
1874 // set the universal joint row. the angular velocity about an axis
1875 // perpendicular to both joint axes should be equal. thus the constraint
1876 // equation is
1877 // p*w1 - p*w2 = 0
1878 // where p is a vector normal to both joint axes, and w1 and w2
1879 // are the angular velocity vectors of the two bodies.
1880
1881 // length 1 joint axis in global coordinates, from each body
1882 dVector3 ax1, ax2;
1883 dVector3 ax2_temp;
1884 // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
1885 // about this.
1886 dVector3 p;
1887 dReal k;
1888
1889 getUniversalAxes(joint, ax1, ax2);
1890 k = dDOT(ax1, ax2);
1891 ax2_temp[0] = ax2[0] - k*ax1[0];
1892 ax2_temp[1] = ax2[1] - k*ax1[1];
1893 ax2_temp[2] = ax2[2] - k*ax1[2];
1894 dCROSS(p, =, ax1, ax2_temp);
1895 dNormalize3(p);
1896
1897 int s3=3*info->rowskip;
1898
1899 info->J1a[s3+0] = p[0];
1900 info->J1a[s3+1] = p[1];
1901 info->J1a[s3+2] = p[2];
1902
1903 if (joint->node[1].body) {
1904 info->J2a[s3+0] = -p[0];
1905 info->J2a[s3+1] = -p[1];
1906 info->J2a[s3+2] = -p[2];
1907 }
1908
1909 // compute the right hand side of the constraint equation. set relative
1910 // body velocities along p to bring the axes back to perpendicular.
1911 // If ax1, ax2 are unit length joint axes as computed from body1 and
1912 // body2, we need to rotate both bodies along the axis p. If theta
1913 // is the angle between ax1 and ax2, we need an angular velocity
1914 // along p to cover the angle erp * (theta - Pi/2) in one step:
1915 //
1916 // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
1917 // = (erp*fps) * (theta - Pi/2)
1918 //
1919 // if theta is close to Pi/2,
1920 // theta - Pi/2 ~= cos(theta), so
1921 // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
1922
1923 info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2);
1924
1925 // if the first angle is powered, or has joint limits, add in the stuff
1926 int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
1927
1928 // if the second angle is powered, or has joint limits, add in more stuff
1929 joint->limot2.addLimot (joint,info,row,ax2,1);
1930}
1931
1932
1933static void universalComputeInitialRelativeRotations (dxJointUniversal *joint)
1934{
1935 if (joint->node[0].body) {
1936 dVector3 ax1, ax2;
1937 dMatrix3 R;
1938 dQuaternion qcross;
1939
1940 getUniversalAxes(joint, ax1, ax2);
1941
1942 // Axis 1.
1943 dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
1944 dRtoQ(R, qcross);
1945 dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross);
1946
1947 // Axis 2.
1948 dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
1949 dRtoQ(R, qcross);
1950 if (joint->node[1].body) {
1951 dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross);
1952 }
1953 else {
1954 // set joint->qrel to qcross
1955 for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i];
1956 }
1957 }
1958}
1959
1960
1961extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint,
1962 dReal x, dReal y, dReal z)
1963{
1964 dUASSERT(joint,"bad joint argument");
1965 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
1966 setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
1967 universalComputeInitialRelativeRotations(joint);
1968}
1969
1970
1971extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint,
1972 dReal x, dReal y, dReal z)
1973{
1974 dUASSERT(joint,"bad joint argument");
1975 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
1976 if (joint->flags & dJOINT_REVERSE)
1977 setAxes (joint,x,y,z,NULL,joint->axis2);
1978 else
1979 setAxes (joint,x,y,z,joint->axis1,NULL);
1980 universalComputeInitialRelativeRotations(joint);
1981}
1982
1983
1984extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint,
1985 dReal x, dReal y, dReal z)
1986{
1987 dUASSERT(joint,"bad joint argument");
1988 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
1989 if (joint->flags & dJOINT_REVERSE)
1990 setAxes (joint,x,y,z,joint->axis1,NULL);
1991 else
1992 setAxes (joint,x,y,z,NULL,joint->axis2);
1993 universalComputeInitialRelativeRotations(joint);
1994}
1995
1996
1997extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint,
1998 dVector3 result)
1999{
2000 dUASSERT(joint,"bad joint argument");
2001 dUASSERT(result,"bad result argument");
2002 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2003 if (joint->flags & dJOINT_REVERSE)
2004 getAnchor2 (joint,result,joint->anchor2);
2005 else
2006 getAnchor (joint,result,joint->anchor1);
2007}
2008
2009
2010extern "C" void dJointGetUniversalAnchor2 (dxJointUniversal *joint,
2011 dVector3 result)
2012{
2013 dUASSERT(joint,"bad joint argument");
2014 dUASSERT(result,"bad result argument");
2015 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2016 if (joint->flags & dJOINT_REVERSE)
2017 getAnchor (joint,result,joint->anchor1);
2018 else
2019 getAnchor2 (joint,result,joint->anchor2);
2020}
2021
2022
2023extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint,
2024 dVector3 result)
2025{
2026 dUASSERT(joint,"bad joint argument");
2027 dUASSERT(result,"bad result argument");
2028 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2029 if (joint->flags & dJOINT_REVERSE)
2030 getAxis2 (joint,result,joint->axis2);
2031 else
2032 getAxis (joint,result,joint->axis1);
2033}
2034
2035
2036extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint,
2037 dVector3 result)
2038{
2039 dUASSERT(joint,"bad joint argument");
2040 dUASSERT(result,"bad result argument");
2041 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2042 if (joint->flags & dJOINT_REVERSE)
2043 getAxis (joint,result,joint->axis1);
2044 else
2045 getAxis2 (joint,result,joint->axis2);
2046}
2047
2048
2049extern "C" void dJointSetUniversalParam (dxJointUniversal *joint,
2050 int parameter, dReal value)
2051{
2052 dUASSERT(joint,"bad joint argument");
2053 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2054 if ((parameter & 0xff00) == 0x100) {
2055 joint->limot2.set (parameter & 0xff,value);
2056 }
2057 else {
2058 joint->limot1.set (parameter,value);
2059 }
2060}
2061
2062
2063extern "C" dReal dJointGetUniversalParam (dxJointUniversal *joint, int parameter)
2064{
2065 dUASSERT(joint,"bad joint argument");
2066 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2067 if ((parameter & 0xff00) == 0x100) {
2068 return joint->limot2.get (parameter & 0xff);
2069 }
2070 else {
2071 return joint->limot1.get (parameter);
2072 }
2073}
2074
2075
2076extern "C" dReal dJointGetUniversalAngle1 (dxJointUniversal *joint)
2077{
2078 dUASSERT(joint,"bad joint argument");
2079 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2080 if (joint->flags & dJOINT_REVERSE)
2081 return getUniversalAngle2 (joint);
2082 else
2083 return getUniversalAngle1 (joint);
2084}
2085
2086
2087extern "C" dReal dJointGetUniversalAngle2 (dxJointUniversal *joint)
2088{
2089 dUASSERT(joint,"bad joint argument");
2090 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2091 if (joint->flags & dJOINT_REVERSE)
2092 return getUniversalAngle1 (joint);
2093 else
2094 return getUniversalAngle2 (joint);
2095}
2096
2097
2098extern "C" dReal dJointGetUniversalAngle1Rate (dxJointUniversal *joint)
2099{
2100 dUASSERT(joint,"bad joint argument");
2101 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2102
2103 if (joint->node[0].body) {
2104 dVector3 axis;
2105
2106 if (joint->flags & dJOINT_REVERSE)
2107 getAxis2 (joint,axis,joint->axis2);
2108 else
2109 getAxis (joint,axis,joint->axis1);
2110
2111 dReal rate = dDOT(axis, joint->node[0].body->avel);
2112 if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel);
2113 return rate;
2114 }
2115 return 0;
2116}
2117
2118
2119extern "C" dReal dJointGetUniversalAngle2Rate (dxJointUniversal *joint)
2120{
2121 dUASSERT(joint,"bad joint argument");
2122 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2123
2124 if (joint->node[0].body) {
2125 dVector3 axis;
2126
2127 if (joint->flags & dJOINT_REVERSE)
2128 getAxis (joint,axis,joint->axis1);
2129 else
2130 getAxis2 (joint,axis,joint->axis2);
2131
2132 dReal rate = dDOT(axis, joint->node[0].body->avel);
2133 if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel);
2134 return rate;
2135 }
2136 return 0;
2137}
2138
2139
2140extern "C" void dJointAddUniversalTorques (dxJointUniversal *joint, dReal torque1, dReal torque2)
2141{
2142 dVector3 axis1, axis2;
2143 dAASSERT(joint);
2144 dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
2145
2146 if (joint->flags & dJOINT_REVERSE) {
2147 dReal temp = torque1;
2148 torque1 = - torque2;
2149 torque2 = - temp;
2150 }
2151
2152 getAxis (joint,axis1,joint->axis1);
2153 getAxis2 (joint,axis2,joint->axis2);
2154 axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
2155 axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
2156 axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
2157
2158 if (joint->node[0].body != 0)
2159 dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]);
2160 if (joint->node[1].body != 0)
2161 dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]);
2162}
2163
2164
2165
2166
2167
2168dxJoint::Vtable __duniversal_vtable = {
2169 sizeof(dxJointUniversal),
2170 (dxJoint::init_fn*) universalInit,
2171 (dxJoint::getInfo1_fn*) universalGetInfo1,
2172 (dxJoint::getInfo2_fn*) universalGetInfo2,
2173 dJointTypeUniversal};
2174
2175//****************************************************************************
2176// angular motor
2177
2178static void amotorInit (dxJointAMotor *j)
2179{
2180 int i;
2181 j->num = 0;
2182 j->mode = dAMotorUser;
2183 for (i=0; i<3; i++) {
2184 j->rel[i] = 0;
2185 dSetZero (j->axis[i],4);
2186 j->limot[i].init (j->world);
2187 j->angle[i] = 0;
2188 }
2189 dSetZero (j->reference1,4);
2190 dSetZero (j->reference2,4);
2191
2192 j->flags |= dJOINT_TWOBODIES;
2193}
2194
2195
2196// compute the 3 axes in global coordinates
2197
2198static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3])
2199{
2200 if (joint->mode == dAMotorEuler) {
2201 // special handling for euler mode
2202 dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]);
2203 dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]);
2204 dCROSS (ax[1],=,ax[2],ax[0]);
2205 dNormalize3 (ax[1]);
2206 }
2207 else {
2208 for (int i=0; i < joint->num; i++) {
2209 if (joint->rel[i] == 1) {
2210 // relative to b1
2211 dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]);
2212 }
2213 if (joint->rel[i] == 2) {
2214 // relative to b2
2215 dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]);
2216 }
2217 else {
2218 // global - just copy it
2219 ax[i][0] = joint->axis[i][0];
2220 ax[i][1] = joint->axis[i][1];
2221 ax[i][2] = joint->axis[i][2];
2222 }
2223 }
2224 }
2225}
2226
2227
2228static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3])
2229{
2230 // assumptions:
2231 // global axes already calculated --> ax
2232 // axis[0] is relative to body 1 --> global ax[0]
2233 // axis[2] is relative to body 2 --> global ax[2]
2234 // ax[1] = ax[2] x ax[0]
2235 // original ax[0] and ax[2] are perpendicular
2236 // reference1 is perpendicular to ax[0] (in body 1 frame)
2237 // reference2 is perpendicular to ax[2] (in body 2 frame)
2238 // all ax[] and reference vectors are unit length
2239
2240 // calculate references in global frame
2241 dVector3 ref1,ref2;
2242 dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1);
2243 dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2);
2244
2245 // get q perpendicular to both ax[0] and ref1, get first euler angle
2246 dVector3 q;
2247 dCROSS (q,=,ax[0],ref1);
2248 joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1));
2249
2250 // get q perpendicular to both ax[0] and ax[1], get second euler angle
2251 dCROSS (q,=,ax[0],ax[1]);
2252 joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q));
2253
2254 // get q perpendicular to both ax[1] and ax[2], get third euler angle
2255 dCROSS (q,=,ax[1],ax[2]);
2256 joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q));
2257}
2258
2259
2260// set the reference vectors as follows:
2261// * reference1 = current axis[2] relative to body 1
2262// * reference2 = current axis[0] relative to body 2
2263// this assumes that:
2264// * axis[0] is relative to body 1
2265// * axis[2] is relative to body 2
2266
2267static void amotorSetEulerReferenceVectors (dxJointAMotor *j)
2268{
2269 if (j->node[0].body && j->node[1].body) {
2270 dVector3 r; // axis[2] and axis[0] in global coordinates
2271 dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]);
2272 dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r);
2273 dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]);
2274 dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r);
2275 }
2276}
2277
2278
2279static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info)
2280{
2281 info->m = 0;
2282 info->nub = 0;
2283
2284 // compute the axes and angles, if in euler mode
2285 if (j->mode == dAMotorEuler) {
2286 dVector3 ax[3];
2287 amotorComputeGlobalAxes (j,ax);
2288 amotorComputeEulerAngles (j,ax);
2289 }
2290
2291 // see if we're powered or at a joint limit for each axis
2292 for (int i=0; i < j->num; i++) {
2293 if (j->limot[i].testRotationalLimit (j->angle[i]) ||
2294 j->limot[i].fmax > 0) {
2295 info->m++;
2296 }
2297 }
2298}
2299
2300
2301static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info)
2302{
2303 int i;
2304
2305 // compute the axes (if not global)
2306 dVector3 ax[3];
2307 amotorComputeGlobalAxes (joint,ax);
2308
2309 // in euler angle mode we do not actually constrain the angular velocity
2310 // along the axes axis[0] and axis[2] (although we do use axis[1]) :
2311 //
2312 // to get constrain w2-w1 along ...not
2313 // ------ --------------------- ------
2314 // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
2315 // d(angle[1])/dt = 0 ax[1]
2316 // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
2317 //
2318 // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
2319 // to prove the result for angle[0], write the expression for angle[0] from
2320 // GetInfo1 then take the derivative. to prove this for angle[2] it is
2321 // easier to take the euler rate expression for d(angle[2])/dt with respect
2322 // to the components of w and set that to 0.
2323
2324 dVector3 *axptr[3];
2325 axptr[0] = &ax[0];
2326 axptr[1] = &ax[1];
2327 axptr[2] = &ax[2];
2328
2329 dVector3 ax0_cross_ax1;
2330 dVector3 ax1_cross_ax2;
2331 if (joint->mode == dAMotorEuler) {
2332 dCROSS (ax0_cross_ax1,=,ax[0],ax[1]);
2333 axptr[2] = &ax0_cross_ax1;
2334 dCROSS (ax1_cross_ax2,=,ax[1],ax[2]);
2335 axptr[0] = &ax1_cross_ax2;
2336 }
2337
2338 int row=0;
2339 for (i=0; i < joint->num; i++) {
2340 row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1);
2341 }
2342}
2343
2344
2345extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num)
2346{
2347 dAASSERT(joint && num >= 0 && num <= 3);
2348 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2349 if (joint->mode == dAMotorEuler) {
2350 joint->num = 3;
2351 }
2352 else {
2353 if (num < 0) num = 0;
2354 if (num > 3) num = 3;
2355 joint->num = num;
2356 }
2357}
2358
2359
2360extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel,
2361 dReal x, dReal y, dReal z)
2362{
2363 dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2);
2364 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2365 if (anum < 0) anum = 0;
2366 if (anum > 2) anum = 2;
2367 joint->rel[anum] = rel;
2368
2369 // x,y,z is always in global coordinates regardless of rel, so we may have
2370 // to convert it to be relative to a body
2371 dVector3 r;
2372 r[0] = x;
2373 r[1] = y;
2374 r[2] = z;
2375 r[3] = 0;
2376 if (rel > 0) {
2377 if (rel==1) {
2378 dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r);
2379 }
2380 else {
2381 dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r);
2382 }
2383 }
2384 else {
2385 joint->axis[anum][0] = r[0];
2386 joint->axis[anum][1] = r[1];
2387 joint->axis[anum][2] = r[2];
2388 }
2389 dNormalize3 (joint->axis[anum]);
2390 if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint);
2391}
2392
2393
2394extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum,
2395 dReal angle)
2396{
2397 dAASSERT(joint && anum >= 0 && anum < 3);
2398 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2399 if (joint->mode == dAMotorUser) {
2400 if (anum < 0) anum = 0;
2401 if (anum > 3) anum = 3;
2402 joint->angle[anum] = angle;
2403 }
2404}
2405
2406
2407extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter,
2408 dReal value)
2409{
2410 dAASSERT(joint);
2411 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2412 int anum = parameter >> 8;
2413 if (anum < 0) anum = 0;
2414 if (anum > 2) anum = 2;
2415 parameter &= 0xff;
2416 joint->limot[anum].set (parameter, value);
2417}
2418
2419
2420extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode)
2421{
2422 dAASSERT(joint);
2423 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2424 joint->mode = mode;
2425 if (joint->mode == dAMotorEuler) {
2426 joint->num = 3;
2427 amotorSetEulerReferenceVectors (joint);
2428 }
2429}
2430
2431
2432extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint)
2433{
2434 dAASSERT(joint);
2435 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2436 return joint->num;
2437}
2438
2439
2440extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum,
2441 dVector3 result)
2442{
2443 dAASSERT(joint && anum >= 0 && anum < 3);
2444 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2445 if (anum < 0) anum = 0;
2446 if (anum > 2) anum = 2;
2447 if (joint->rel[anum] > 0) {
2448 if (joint->rel[anum]==1) {
2449 dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]);
2450 }
2451 else {
2452 dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]);
2453 }
2454 }
2455 else {
2456 result[0] = joint->axis[anum][0];
2457 result[1] = joint->axis[anum][1];
2458 result[2] = joint->axis[anum][2];
2459 }
2460}
2461
2462
2463extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum)
2464{
2465 dAASSERT(joint && anum >= 0 && anum < 3);
2466 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2467 if (anum < 0) anum = 0;
2468 if (anum > 2) anum = 2;
2469 return joint->rel[anum];
2470}
2471
2472
2473extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum)
2474{
2475 dAASSERT(joint && anum >= 0 && anum < 3);
2476 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2477 if (anum < 0) anum = 0;
2478 if (anum > 3) anum = 3;
2479 return joint->angle[anum];
2480}
2481
2482
2483extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum)
2484{
2485 // @@@
2486 dDebug (0,"not yet implemented");
2487 return 0;
2488}
2489
2490
2491extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter)
2492{
2493 dAASSERT(joint);
2494 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2495 int anum = parameter >> 8;
2496 if (anum < 0) anum = 0;
2497 if (anum > 2) anum = 2;
2498 parameter &= 0xff;
2499 return joint->limot[anum].get (parameter);
2500}
2501
2502
2503extern "C" int dJointGetAMotorMode (dxJointAMotor *joint)
2504{
2505 dAASSERT(joint);
2506 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2507 return joint->mode;
2508}
2509
2510
2511extern "C" void dJointAddAMotorTorques (dxJointAMotor *joint, dReal torque1, dReal torque2, dReal torque3)
2512{
2513 dVector3 axes[3];
2514 dAASSERT(joint);
2515 dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
2516
2517 if (joint->num == 0)
2518 return;
2519 dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints");
2520
2521 amotorComputeGlobalAxes (joint,axes);
2522 axes[0][0] *= torque1;
2523 axes[0][1] *= torque1;
2524 axes[0][2] *= torque1;
2525 if (joint->num >= 2) {
2526 axes[0][0] += axes[1][0] * torque2;
2527 axes[0][1] += axes[1][0] * torque2;
2528 axes[0][2] += axes[1][0] * torque2;
2529 if (joint->num >= 3) {
2530 axes[0][0] += axes[2][0] * torque3;
2531 axes[0][1] += axes[2][0] * torque3;
2532 axes[0][2] += axes[2][0] * torque3;
2533 }
2534 }
2535
2536 if (joint->node[0].body != 0)
2537 dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]);
2538 if (joint->node[1].body != 0)
2539 dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]);
2540}
2541
2542
2543dxJoint::Vtable __damotor_vtable = {
2544 sizeof(dxJointAMotor),
2545 (dxJoint::init_fn*) amotorInit,
2546 (dxJoint::getInfo1_fn*) amotorGetInfo1,
2547 (dxJoint::getInfo2_fn*) amotorGetInfo2,
2548 dJointTypeAMotor};
2549
2550//****************************************************************************
2551// fixed joint
2552
2553static void fixedInit (dxJointFixed *j)
2554{
2555 dSetZero (j->offset,4);
2556 dSetZero (j->qrel,4);
2557}
2558
2559
2560static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info)
2561{
2562 info->m = 6;
2563 info->nub = 6;
2564}
2565
2566
2567static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info)
2568{
2569 int s = info->rowskip;
2570
2571 // Three rows for orientation
2572 setFixedOrientation(joint, info, joint->qrel, 3);
2573
2574 // Three rows for position.
2575 // set jacobian
2576 info->J1l[0] = 1;
2577 info->J1l[s+1] = 1;
2578 info->J1l[2*s+2] = 1;
2579
2580 dVector3 ofs;
2581 dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset);
2582 if (joint->node[1].body) {
2583 dCROSSMAT (info->J1a,ofs,s,+,-);
2584 info->J2l[0] = -1;
2585 info->J2l[s+1] = -1;
2586 info->J2l[2*s+2] = -1;
2587 }
2588
2589 // set right hand side for the first three rows (linear)
2590 dReal k = info->fps * info->erp;
2591 if (joint->node[1].body) {
2592 for (int j=0; j<3; j++)
2593 info->c[j] = k * (joint->node[1].body->pos[j] -
2594 joint->node[0].body->pos[j] + ofs[j]);
2595 }
2596 else {
2597 for (int j=0; j<3; j++)
2598 info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]);
2599 }
2600}
2601
2602
2603extern "C" void dJointSetFixed (dxJointFixed *joint)
2604{
2605 dUASSERT(joint,"bad joint argument");
2606 dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed");
2607 int i;
2608
2609 // This code is taken from sJointSetSliderAxis(), we should really put the
2610 // common code in its own function.
2611 // compute the offset between the bodies
2612 if (joint->node[0].body) {
2613 if (joint->node[1].body) {
2614 dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
2615 dReal ofs[4];
2616 for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i];
2617 for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i];
2618 dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs);
2619 }
2620 else {
2621 // set joint->qrel to the transpose of the first body's q
2622 joint->qrel[0] = joint->node[0].body->q[0];
2623 for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
2624 for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i];
2625 }
2626 }
2627}
2628
2629
2630dxJoint::Vtable __dfixed_vtable = {
2631 sizeof(dxJointFixed),
2632 (dxJoint::init_fn*) fixedInit,
2633 (dxJoint::getInfo1_fn*) fixedGetInfo1,
2634 (dxJoint::getInfo2_fn*) fixedGetInfo2,
2635 dJointTypeFixed};
2636
2637//****************************************************************************
2638// null joint
2639
2640static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info)
2641{
2642 info->m = 0;
2643 info->nub = 0;
2644}
2645
2646
2647static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info)
2648{
2649 dDebug (0,"this should never get called");
2650}
2651
2652
2653dxJoint::Vtable __dnull_vtable = {
2654 sizeof(dxJointNull),
2655 (dxJoint::init_fn*) 0,
2656 (dxJoint::getInfo1_fn*) nullGetInfo1,
2657 (dxJoint::getInfo2_fn*) nullGetInfo2,
2658 dJointTypeNull};
2659
2660/******************** breakable joint contribution ***********************/
2661extern "C" void dJointSetBreakable (dxJoint *joint, int b) {
2662 dAASSERT(joint);
2663 if (b) {
2664 // we want this joint to be breakable but we must first check if it
2665 // was already breakable
2666 if (!joint->breakInfo) {
2667 // allocate a dxJointBreakInfo struct
2668 joint->breakInfo = new dxJointBreakInfo;
2669 joint->breakInfo->flags = 0;
2670 for (int i = 0; i < 3; i++) {
2671 joint->breakInfo->b1MaxF[0] = 0;
2672 joint->breakInfo->b1MaxT[0] = 0;
2673 joint->breakInfo->b2MaxF[0] = 0;
2674 joint->breakInfo->b2MaxT[0] = 0;
2675 }
2676 joint->breakInfo->callback = 0;
2677 }
2678 else {
2679 // the joint was already breakable
2680 return;
2681 }
2682 }
2683 else {
2684 // we want this joint to be unbreakable mut we must first check if
2685 // it is alreay unbreakable
2686 if (joint->breakInfo) {
2687 // deallocate the dxJointBreakInfo struct
2688 delete joint->breakInfo;
2689 joint->breakInfo = 0;
2690 }
2691 else {
2692 // the joint was already unbreakable
2693 return;
2694 }
2695 }
2696}
2697
2698extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) {
2699 dAASSERT(joint);
2700# ifndef dNODEBUG
2701 // only works for a breakable joint
2702 if (!joint->breakInfo) {
2703 dDebug (0, "dJointSetBreakCallback called on unbreakable joint");
2704 }
2705# endif
2706 joint->breakInfo->callback = callbackFunc;
2707}
2708
2709extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) {
2710 dAASSERT(joint);
2711# ifndef dNODEBUG
2712 // only works for a breakable joint
2713 if (!joint->breakInfo) {
2714 dDebug (0, "dJointSetBreakMode called on unbreakable joint");
2715 }
2716# endif
2717 joint->breakInfo->flags = mode;
2718}
2719
2720extern "C" int dJointGetBreakMode (dxJoint *joint) {
2721 dAASSERT(joint);
2722# ifndef dNODEBUG
2723 // only works for a breakable joint
2724 if (!joint->breakInfo) {
2725 dDebug (0, "dJointGetBreakMode called on unbreakable joint");
2726 }
2727# endif
2728 return joint->breakInfo->flags;
2729}
2730
2731extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
2732 dAASSERT(joint);
2733# ifndef dNODEBUG
2734 // only works for a breakable joint
2735 if (!joint->breakInfo) {
2736 dDebug (0, "dJointSetBreakForce called on unbreakable joint");
2737 }
2738# endif
2739 if (body) {
2740 joint->breakInfo->b2MaxF[0] = x;
2741 joint->breakInfo->b2MaxF[1] = y;
2742 joint->breakInfo->b2MaxF[2] = z;
2743 }
2744 else {
2745 joint->breakInfo->b1MaxF[0] = x;
2746 joint->breakInfo->b1MaxF[1] = y;
2747 joint->breakInfo->b1MaxF[2] = z;
2748 }
2749}
2750
2751extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
2752 dAASSERT(joint);
2753# ifndef dNODEBUG
2754 // only works for a breakable joint
2755 if (!joint->breakInfo) {
2756 dDebug (0, "dJointSetBreakTorque called on unbreakable joint");
2757 }
2758# endif
2759 if (body) {
2760 joint->breakInfo->b2MaxT[0] = x;
2761 joint->breakInfo->b2MaxT[1] = y;
2762 joint->breakInfo->b2MaxT[2] = z;
2763 }
2764 else {
2765 joint->breakInfo->b1MaxT[0] = x;
2766 joint->breakInfo->b1MaxT[1] = y;
2767 joint->breakInfo->b1MaxT[2] = z;
2768 }
2769}
2770
2771extern "C" int dJointIsBreakable (dxJoint *joint) {
2772 dAASSERT(joint);
2773 return joint->breakInfo != 0;
2774}
2775
2776extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) {
2777 dAASSERT(joint);
2778# ifndef dNODEBUG
2779 // only works for a breakable joint
2780 if (!joint->breakInfo) {
2781 dDebug (0, "dJointGetBreakForce called on unbreakable joint");
2782 }
2783# endif
2784 if (body)
2785 for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i];
2786 else
2787 for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i];
2788}
2789
2790extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) {
2791 dAASSERT(joint);
2792# ifndef dNODEBUG
2793 // only works for a breakable joint
2794 if (!joint->breakInfo) {
2795 dDebug (0, "dJointGetBreakTorque called on unbreakable joint");
2796 }
2797# endif
2798 if (body)
2799 for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i];
2800 else
2801 for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i];
2802}
2803/*************************************************************************/
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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_JOINT_H_
24#define _ODE_JOINT_H_
25
26
27#include "objects.h"
28#include <ode/contact.h>
29#include "obstack.h"
30
31
32// joint flags
33enum {
34 // if this flag is set, the joint was allocated in a joint group
35 dJOINT_INGROUP = 1,
36
37 // if this flag is set, the joint was attached with arguments (0,body).
38 // our convention is to treat all attaches as (body,0), i.e. so node[0].body
39 // is always nonzero, so this flag records the fact that the arguments were
40 // swapped.
41 dJOINT_REVERSE = 2,
42
43 // if this flag is set, the joint can not have just one body attached to it,
44 // it must have either zero or two bodies attached.
45 dJOINT_TWOBODIES = 4
46};
47
48
49// there are two of these nodes in the joint, one for each connection to a
50// body. these are node of a linked list kept by each body of it's connecting
51// joints. but note that the body pointer in each node points to the body that
52// makes use of the *other* node, not this node. this trick makes it a bit
53// easier to traverse the body/joint graph.
54
55struct dxJointNode {
56 dxJoint *joint; // pointer to enclosing dxJoint object
57 dxBody *body; // *other* body this joint is connected to
58 dxJointNode *next; // next node in body's list of connected joints
59};
60
61/******************** breakable joint contribution ***********************/
62struct dxJointBreakInfo : public dBase {
63 int flags;
64 dReal b1MaxF[3]; // maximum force on body 1
65 dReal b1MaxT[3]; // maximum torque on body 1
66 dReal b2MaxF[3]; // maximum force on body 2
67 dReal b2MaxT[3]; // maximum torque on body 2
68 dJointBreakCallback *callback; // function that is called when this joint breaks
69};
70/*************************************************************************/
71
72struct dxJoint : public dObject {
73 // naming convention: the "first" body this is connected to is node[0].body,
74 // and the "second" body is node[1].body. if this joint is only connected
75 // to one body then the second body is 0.
76
77 // info returned by getInfo1 function. the constraint dimension is m (<=6).
78 // i.e. that is the total number of rows in the jacobian. `nub' is the
79 // number of unbounded variables (which have lo,hi = -/+ infinity).
80
81 struct Info1 {
82 int m,nub;
83 };
84
85 // info returned by getInfo2 function
86
87 struct Info2 {
88 // integrator parameters: frames per second (1/stepsize), default error
89 // reduction parameter (0..1).
90 dReal fps,erp;
91
92 // for the first and second body, pointers to two (linear and angular)
93 // n*3 jacobian sub matrices, stored by rows. these matrices will have
94 // been initialized to 0 on entry. if the second body is zero then the
95 // J2xx pointers may be 0.
96 dReal *J1l,*J1a,*J2l,*J2a;
97
98 // elements to jump from one row to the next in J's
99 int rowskip;
100
101 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
102 // "constraint force mixing" vector. c is set to zero on entry, cfm is
103 // set to a constant value (typically very small or zero) value on entry.
104 dReal *c,*cfm;
105
106 // lo and hi limits for variables (set to -/+ infinity on entry).
107 dReal *lo,*hi;
108
109 // findex vector for variables. see the LCP solver interface for a
110 // description of what this does. this is set to -1 on entry.
111 // note that the returned indexes are relative to the first index of
112 // the constraint.
113 int *findex;
114 };
115
116 // virtual function table: size of the joint structure, function pointers.
117 // we do it this way instead of using C++ virtual functions because
118 // sometimes we need to allocate joints ourself within a memory pool.
119
120 typedef void init_fn (dxJoint *joint);
121 typedef void getInfo1_fn (dxJoint *joint, Info1 *info);
122 typedef void getInfo2_fn (dxJoint *joint, Info2 *info);
123 struct Vtable {
124 int size;
125 init_fn *init;
126 getInfo1_fn *getInfo1;
127 getInfo2_fn *getInfo2;
128 int typenum; // a dJointTypeXXX type number
129 };
130
131 Vtable *vtable; // virtual function table
132 int flags; // dJOINT_xxx flags
133 dxJointNode node[2]; // connections to bodies. node[1].body can be 0
134 dJointFeedback *feedback; // optional feedback structure
135
136 /******************** breakable joint contribution ***********************/
137 // optional break info structure. if this is not NULL the the joint is
138 // breakable.
139 dxJointBreakInfo *breakInfo;
140 /*************************************************************************/
141};
142
143
144// joint group. NOTE: any joints in the group that have their world destroyed
145// will have their world pointer set to 0.
146
147struct dxJointGroup : public dBase {
148 int num; // number of joints on the stack
149 dObStack stack; // a stack of (possibly differently sized) dxJoint
150}; // objects.
151
152
153// common limit and motor information for a single joint axis of movement
154struct dxJointLimitMotor {
155 dReal vel,fmax; // powered joint: velocity, max force
156 dReal lostop,histop; // joint limits, relative to initial position
157 dReal fudge_factor; // when powering away from joint limits
158 dReal normal_cfm; // cfm to use when not at a stop
159 dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit
160 dReal bounce; // restitution factor
161 // variables used between getInfo1() and getInfo2()
162 int limit; // 0=free, 1=at lo limit, 2=at hi limit
163 dReal limit_err; // if at limit, amount over limit
164
165 void init (dxWorld *);
166 void set (int num, dReal value);
167 dReal get (int num);
168 int testRotationalLimit (dReal angle);
169 int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row,
170 dVector3 ax1, int rotational);
171};
172
173
174// ball and socket
175
176struct dxJointBall : public dxJoint {
177 dVector3 anchor1; // anchor w.r.t first body
178 dVector3 anchor2; // anchor w.r.t second body
179};
180extern struct dxJoint::Vtable __dball_vtable;
181
182
183// hinge
184
185struct dxJointHinge : public dxJoint {
186 dVector3 anchor1; // anchor w.r.t first body
187 dVector3 anchor2; // anchor w.r.t second body
188 dVector3 axis1; // axis w.r.t first body
189 dVector3 axis2; // axis w.r.t second body
190 dQuaternion qrel; // initial relative rotation body1 -> body2
191 dxJointLimitMotor limot; // limit and motor information
192};
193extern struct dxJoint::Vtable __dhinge_vtable;
194
195
196// universal
197
198struct dxJointUniversal : public dxJoint {
199 dVector3 anchor1; // anchor w.r.t first body
200 dVector3 anchor2; // anchor w.r.t second body
201 dVector3 axis1; // axis w.r.t first body
202 dVector3 axis2; // axis w.r.t second body
203 dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece
204 dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2
205 dxJointLimitMotor limot1; // limit and motor information for axis1
206 dxJointLimitMotor limot2; // limit and motor information for axis2
207};
208extern struct dxJoint::Vtable __duniversal_vtable;
209
210
211// slider. if body2 is 0 then qrel is the absolute rotation of body1 and
212// offset is the position of body1 center along axis1.
213
214struct dxJointSlider : public dxJoint {
215 dVector3 axis1; // axis w.r.t first body
216 dQuaternion qrel; // initial relative rotation body1 -> body2
217 dVector3 offset; // point relative to body2 that should be
218 // aligned with body1 center along axis1
219 dxJointLimitMotor limot; // limit and motor information
220};
221extern struct dxJoint::Vtable __dslider_vtable;
222
223
224// contact
225
226struct dxJointContact : public dxJoint {
227 int the_m; // number of rows computed by getInfo1
228 dContact contact;
229};
230extern struct dxJoint::Vtable __dcontact_vtable;
231
232
233// hinge 2
234
235struct dxJointHinge2 : public dxJoint {
236 dVector3 anchor1; // anchor w.r.t first body
237 dVector3 anchor2; // anchor w.r.t second body
238 dVector3 axis1; // axis 1 w.r.t first body
239 dVector3 axis2; // axis 2 w.r.t second body
240 dReal c0,s0; // cos,sin of desired angle between axis 1,2
241 dVector3 v1,v2; // angle ref vectors embedded in first body
242 dxJointLimitMotor limot1; // limit+motor info for axis 1
243 dxJointLimitMotor limot2; // limit+motor info for axis 2
244 dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm)
245};
246extern struct dxJoint::Vtable __dhinge2_vtable;
247
248
249// angular motor
250
251struct dxJointAMotor : public dxJoint {
252 int num; // number of axes (0..3)
253 int mode; // a dAMotorXXX constant
254 int rel[3]; // what the axes are relative to (global,b1,b2)
255 dVector3 axis[3]; // three axes
256 dxJointLimitMotor limot[3]; // limit+motor info for axes
257 dReal angle[3]; // user-supplied angles for axes
258 // these vectors are used for calculating euler angles
259 dVector3 reference1; // original axis[2], relative to body 1
260 dVector3 reference2; // original axis[0], relative to body 2
261};
262extern struct dxJoint::Vtable __damotor_vtable;
263
264
265// fixed
266
267struct dxJointFixed : public dxJoint {
268 dQuaternion qrel; // initial relative rotation body1 -> body2
269 dVector3 offset; // relative offset between the bodies
270};
271extern struct dxJoint::Vtable __dfixed_vtable;
272
273
274// null joint, for testing only
275
276struct dxJointNull : public dxJoint {
277};
278extern struct dxJoint::Vtable __dnull_vtable;
279
280
281
282#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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifndef _ODE_OBJECTS_H_
24#define _ODE_OBJECTS_H_
25
26#include <ode/common.h>
27#include <ode/mass.h>
28#include <ode/contact.h>
29
30#ifdef __cplusplus
31extern "C" {
32#endif
33
34/* world */
35
36dWorldID dWorldCreate();
37void dWorldDestroy (dWorldID);
38
39void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z);
40void dWorldGetGravity (dWorldID, dVector3 gravity);
41void dWorldSetERP (dWorldID, dReal erp);
42dReal dWorldGetERP (dWorldID);
43void dWorldSetCFM (dWorldID, dReal cfm);
44dReal dWorldGetCFM (dWorldID);
45void dWorldStep (dWorldID, dReal stepsize);
46void dWorldImpulseToForce (dWorldID, dReal stepsize,
47 dReal ix, dReal iy, dReal iz, dVector3 force);
48
49/* StepFast1 functions */
50
51void dWorldStepFast1(dWorldID, dReal stepsize, int maxiterations);
52void dWorldSetAutoEnableDepthSF1(dWorldID, int autoEnableDepth);
53
54int dWorldGetAutoEnableDepthSF1(dWorldID);
55
56void dBodySetAutoDisableThresholdSF1(dBodyID, dReal autoDisableThreshold);
57
58/* These functions are not yet implemented by ODE. */
59/*
60dReal dBodyGetAutoDisableThresholdSF1(dBodyID);
61
62void dBodySetAutoDisableStepsSF1(dBodyID, int AutoDisableSteps);
63
64int dBodyGetAutoDisableStepsSF1(dBodyID);
65
66void dBodySetAutoDisableSF1(dBodyID, int doAutoDisable);
67
68int dBodyGetAutoDisableSF1(dBodyID);
69*/
70
71/* bodies */
72
73dBodyID dBodyCreate (dWorldID);
74void dBodyDestroy (dBodyID);
75
76void dBodySetData (dBodyID, void *data);
77void *dBodyGetData (dBodyID);
78
79void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z);
80void dBodySetRotation (dBodyID, const dMatrix3 R);
81void dBodySetQuaternion (dBodyID, const dQuaternion q);
82void dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z);
83void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z);
84const dReal * dBodyGetPosition (dBodyID);
85const dReal * dBodyGetRotation (dBodyID); /* ptr to 4x3 rot matrix */
86const dReal * dBodyGetQuaternion (dBodyID);
87const dReal * dBodyGetLinearVel (dBodyID);
88const dReal * dBodyGetAngularVel (dBodyID);
89
90void dBodySetMass (dBodyID, const dMass *mass);
91void dBodyGetMass (dBodyID, dMass *mass);
92
93void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
94void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
95void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz);
96void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz);
97void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,
98 dReal px, dReal py, dReal pz);
99void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
100 dReal px, dReal py, dReal pz);
101void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,
102 dReal px, dReal py, dReal pz);
103void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
104 dReal px, dReal py, dReal pz);
105
106const dReal * dBodyGetForce (dBodyID);
107const dReal * dBodyGetTorque (dBodyID);
108void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z);
109void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z);
110
111void dBodyGetRelPointPos (dBodyID, dReal px, dReal py, dReal pz,
112 dVector3 result);
113void dBodyGetRelPointVel (dBodyID, dReal px, dReal py, dReal pz,
114 dVector3 result);
115void dBodyGetPointVel (dBodyID, dReal px, dReal py, dReal pz,
116 dVector3 result);
117void dBodyGetPosRelPoint (dBodyID, dReal px, dReal py, dReal pz,
118 dVector3 result);
119void dBodyVectorToWorld (dBodyID, dReal px, dReal py, dReal pz,
120 dVector3 result);
121void dBodyVectorFromWorld (dBodyID, dReal px, dReal py, dReal pz,
122 dVector3 result);
123
124void dBodySetFiniteRotationMode (dBodyID, int mode);
125void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z);
126
127int dBodyGetFiniteRotationMode (dBodyID);
128void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result);
129
130int dBodyGetNumJoints (dBodyID b);
131dJointID dBodyGetJoint (dBodyID, int index);
132
133void dBodyEnable (dBodyID);
134void dBodyDisable (dBodyID);
135int dBodyIsEnabled (dBodyID);
136
137void dBodySetGravityMode (dBodyID b, int mode);
138int dBodyGetGravityMode (dBodyID b);
139
140
141/* joints */
142
143dJointID dJointCreateBall (dWorldID, dJointGroupID);
144dJointID dJointCreateHinge (dWorldID, dJointGroupID);
145dJointID dJointCreateSlider (dWorldID, dJointGroupID);
146dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *);
147dJointID dJointCreateHinge2 (dWorldID, dJointGroupID);
148dJointID dJointCreateUniversal (dWorldID, dJointGroupID);
149dJointID dJointCreateFixed (dWorldID, dJointGroupID);
150dJointID dJointCreateNull (dWorldID, dJointGroupID);
151dJointID dJointCreateAMotor (dWorldID, dJointGroupID);
152
153void dJointDestroy (dJointID);
154
155dJointGroupID dJointGroupCreate (int max_size);
156void dJointGroupDestroy (dJointGroupID);
157void dJointGroupEmpty (dJointGroupID);
158
159void dJointAttach (dJointID, dBodyID body1, dBodyID body2);
160void dJointSetData (dJointID, void *data);
161void *dJointGetData (dJointID);
162int dJointGetType (dJointID);
163dBodyID dJointGetBody (dJointID, int index);
164
165void dJointSetFeedback (dJointID, dJointFeedback *);
166dJointFeedback *dJointGetFeedback (dJointID);
167
168/******************** breakable joint contribution ***********************/
169void dJointSetBreakable (dJointID, int b);
170void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc);
171void dJointSetBreakMode (dJointID, int mode);
172int dJointGetBreakMode (dJointID);
173void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z);
174void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z);
175int dJointIsBreakable (dJointID);
176void dJointGetBreakForce (dJointID, int body, dReal *force);
177void dJointGetBreakTorque (dJointID, int body, dReal *torque);
178/*************************************************************************/
179
180void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z);
181void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z);
182void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z);
183void dJointSetHingeParam (dJointID, int parameter, dReal value);
184void dJointAddHingeTorque(dJointID joint, dReal torque);
185void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z);
186void dJointSetSliderParam (dJointID, int parameter, dReal value);
187void dJointAddSliderForce(dJointID joint, dReal force);
188void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z);
189void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z);
190void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z);
191void dJointSetHinge2Param (dJointID, int parameter, dReal value);
192void dJointAddHinge2Torques(dJointID joint, dReal torque1, dReal torque2);
193void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z);
194void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z);
195void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z);
196void dJointSetUniversalParam (dJointID, int parameter, dReal value);
197void dJointAddUniversalTorques(dJointID joint, dReal torque1, dReal torque2);
198void dJointSetFixed (dJointID);
199void dJointSetAMotorNumAxes (dJointID, int num);
200void dJointSetAMotorAxis (dJointID, int anum, int rel,
201 dReal x, dReal y, dReal z);
202void dJointSetAMotorAngle (dJointID, int anum, dReal angle);
203void dJointSetAMotorParam (dJointID, int parameter, dReal value);
204void dJointSetAMotorMode (dJointID, int mode);
205void dJointAddAMotorTorques (dJointID, dReal torque1, dReal torque2, dReal torque3);
206
207void dJointGetBallAnchor (dJointID, dVector3 result);
208void dJointGetBallAnchor2 (dJointID, dVector3 result);
209void dJointGetHingeAnchor (dJointID, dVector3 result);
210void dJointGetHingeAnchor2 (dJointID, dVector3 result);
211void dJointGetHingeAxis (dJointID, dVector3 result);
212dReal dJointGetHingeParam (dJointID, int parameter);
213dReal dJointGetHingeAngle (dJointID);
214dReal dJointGetHingeAngleRate (dJointID);
215dReal dJointGetSliderPosition (dJointID);
216dReal dJointGetSliderPositionRate (dJointID);
217void dJointGetSliderAxis (dJointID, dVector3 result);
218dReal dJointGetSliderParam (dJointID, int parameter);
219void dJointGetHinge2Anchor (dJointID, dVector3 result);
220void dJointGetHinge2Anchor2 (dJointID, dVector3 result);
221void dJointGetHinge2Axis1 (dJointID, dVector3 result);
222void dJointGetHinge2Axis2 (dJointID, dVector3 result);
223dReal dJointGetHinge2Param (dJointID, int parameter);
224dReal dJointGetHinge2Angle1 (dJointID);
225dReal dJointGetHinge2Angle1Rate (dJointID);
226dReal dJointGetHinge2Angle2Rate (dJointID);
227void dJointGetUniversalAnchor (dJointID, dVector3 result);
228void dJointGetUniversalAnchor2 (dJointID, dVector3 result);
229void dJointGetUniversalAxis1 (dJointID, dVector3 result);
230void dJointGetUniversalAxis2 (dJointID, dVector3 result);
231dReal dJointGetUniversalParam (dJointID, int parameter);
232dReal dJointGetUniversalAngle1 (dJointID);
233dReal dJointGetUniversalAngle2 (dJointID);
234dReal dJointGetUniversalAngle1Rate (dJointID);
235dReal dJointGetUniversalAngle2Rate (dJointID);
236int dJointGetAMotorNumAxes (dJointID);
237void dJointGetAMotorAxis (dJointID, int anum, dVector3 result);
238int dJointGetAMotorAxisRel (dJointID, int anum);
239dReal dJointGetAMotorAngle (dJointID, int anum);
240dReal dJointGetAMotorAngleRate (dJointID, int anum);
241dReal dJointGetAMotorParam (dJointID, int parameter);
242int dJointGetAMotorMode (dJointID);
243
244int dAreConnected (dBodyID, dBodyID);
245int dAreConnectedExcluding (dBodyID, dBodyID, int joint_type);
246
247
248#ifdef __cplusplus
249}
250#endif
251
252#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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#ifdef _MSC_VER
24#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
25#endif
26
27// this source file is mostly concerned with the data structures, not the
28// numerics.
29
30#include "objects.h"
31#include <ode/ode.h>
32#include "joint.h"
33#include <ode/odemath.h>
34#include <ode/matrix.h>
35#include "step.h"
36#include <ode/memory.h>
37#include <ode/error.h>
38
39// misc defines
40#define ALLOCA dALLOCA16
41
42//****************************************************************************
43// utility
44
45static inline void initObject (dObject *obj, dxWorld *w)
46{
47 obj->world = w;
48 obj->next = 0;
49 obj->tome = 0;
50 obj->userdata = 0;
51 obj->tag = 0;
52}
53
54
55// add an object `obj' to the list who's head pointer is pointed to by `first'.
56
57static inline void addObjectToList (dObject *obj, dObject **first)
58{
59 obj->next = *first;
60 obj->tome = first;
61 if (*first) (*first)->tome = &obj->next;
62 (*first) = obj;
63}
64
65
66// remove the object from the linked list
67
68static inline void removeObjectFromList (dObject *obj)
69{
70 if (obj->next) obj->next->tome = obj->tome;
71 *(obj->tome) = obj->next;
72 // safeguard
73 obj->next = 0;
74 obj->tome = 0;
75}
76
77
78// remove the joint from neighbour lists of all connected bodies
79
80static void removeJointReferencesFromAttachedBodies (dxJoint *j)
81{
82 for (int i=0; i<2; i++) {
83 dxBody *body = j->node[i].body;
84 if (body) {
85 dxJointNode *n = body->firstjoint;
86 dxJointNode *last = 0;
87 while (n) {
88 if (n->joint == j) {
89 if (last) last->next = n->next;
90 else body->firstjoint = n->next;
91 break;
92 }
93 last = n;
94 n = n->next;
95 }
96 }
97 }
98 j->node[0].body = 0;
99 j->node[0].next = 0;
100 j->node[1].body = 0;
101 j->node[1].next = 0;
102}
103
104//****************************************************************************
105// island processing
106
107// this groups all joints and bodies in a world into islands. all objects
108// in an island are reachable by going through connected bodies and joints.
109// each island can be simulated separately.
110// note that joints that are not attached to anything will not be included
111// in any island, an so they do not affect the simulation.
112//
113// this function starts new island from unvisited bodies. however, it will
114// never start a new islands from a disabled body. thus islands of disabled
115// bodies will not be included in the simulation. disabled bodies are
116// re-enabled if they are found to be part of an active island.
117
118static void processIslands (dxWorld *world, dReal stepsize)
119{
120 dxBody *b,*bb,**body;
121 dxJoint *j,**joint;
122
123 // nothing to do if no bodies
124 if (world->nb <= 0) return;
125
126 // make arrays for body and joint lists (for a single island) to go into
127 body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
128 joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
129 int bcount = 0; // number of bodies in `body'
130 int jcount = 0; // number of joints in `joint'
131
132 // set all body/joint tags to 0
133 for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
134 for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
135
136 // allocate a stack of unvisited bodies in the island. the maximum size of
137 // the stack can be the lesser of the number of bodies or joints, because
138 // new bodies are only ever added to the stack by going through untagged
139 // joints. all the bodies in the stack must be tagged!
140 int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
141 dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
142
143 for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
144 // get bb = the next enabled, untagged body, and tag it
145 if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
146 bb->tag = 1;
147
148 // tag all bodies and joints starting from bb.
149 int stacksize = 0;
150 b = bb;
151 body[0] = bb;
152 bcount = 1;
153 jcount = 0;
154 goto quickstart;
155 while (stacksize > 0) {
156 b = stack[--stacksize]; // pop body off stack
157 body[bcount++] = b; // put body on body list
158 quickstart:
159
160 // traverse and tag all body's joints, add untagged connected bodies
161 // to stack
162 for (dxJointNode *n=b->firstjoint; n; n=n->next) {
163 if (!n->joint->tag) {
164 n->joint->tag = 1;
165 joint[jcount++] = n->joint;
166 if (n->body && !n->body->tag) {
167 n->body->tag = 1;
168 stack[stacksize++] = n->body;
169 }
170 }
171 }
172 dIASSERT(stacksize <= world->nb);
173 dIASSERT(stacksize <= world->nj);
174 }
175
176 // now do something with body and joint lists
177 dInternalStepIsland (world,body,bcount,joint,jcount,stepsize);
178
179 // what we've just done may have altered the body/joint tag values.
180 // we must make sure that these tags are nonzero.
181 // also make sure all bodies are in the enabled state.
182 int i;
183 for (i=0; i<bcount; i++) {
184 body[i]->tag = 1;
185 body[i]->flags &= ~dxBodyDisabled;
186 }
187 for (i=0; i<jcount; i++) joint[i]->tag = 1;
188 }
189
190 // if debugging, check that all objects (except for disabled bodies,
191 // unconnected joints, and joints that are connected to disabled bodies)
192 // were tagged.
193# ifndef dNODEBUG
194 for (b=world->firstbody; b; b=(dxBody*)b->next) {
195 if (b->flags & dxBodyDisabled) {
196 if (b->tag) dDebug (0,"disabled body tagged");
197 }
198 else {
199 if (!b->tag) dDebug (0,"enabled body not tagged");
200 }
201 }
202 for (j=world->firstjoint; j; j=(dxJoint*)j->next) {
203 if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) ||
204 (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) {
205 if (!j->tag) dDebug (0,"attached enabled joint not tagged");
206 }
207 else {
208 if (j->tag) dDebug (0,"unattached or disabled joint tagged");
209 }
210 }
211# endif
212 /******************** breakable joint contribution ***********************/
213 dxJoint* nextJ;
214 if (!world->firstjoint)
215 nextJ = 0;
216 else
217 nextJ = (dxJoint*)world->firstjoint->next;
218 for (j=world->firstjoint; j; j=nextJ) {
219 nextJ = (dxJoint*)j->next;
220 // check if joint is breakable and broken
221 if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
222 // detach (break) the joint
223 dJointAttach (j, 0, 0);
224 // call the callback function if it is set
225 if (j->breakInfo->callback) j->breakInfo->callback (j);
226 // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
227 if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
228 }
229 }
230 /*************************************************************************/
231}
232
233//****************************************************************************
234// debugging
235
236// see if an object list loops on itself (if so, it's bad).
237
238static int listHasLoops (dObject *first)
239{
240 if (first==0 || first->next==0) return 0;
241 dObject *a=first,*b=first->next;
242 int skip=0;
243 while (b) {
244 if (a==b) return 1;
245 b = b->next;
246 if (skip) a = a->next;
247 skip ^= 1;
248 }
249 return 0;
250}
251
252
253// check the validity of the world data structures
254
255static void checkWorld (dxWorld *w)
256{
257 dxBody *b;
258 dxJoint *j;
259
260 // check there are no loops
261 if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
262 if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
263
264 // check lists are well formed (check `tome' pointers)
265 for (b=w->firstbody; b; b=(dxBody*)b->next) {
266 if (b->next && b->next->tome != &b->next)
267 dDebug (0,"bad tome pointer in body list");
268 }
269 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
270 if (j->next && j->next->tome != &j->next)
271 dDebug (0,"bad tome pointer in joint list");
272 }
273
274 // check counts
275 int n = 0;
276 for (b=w->firstbody; b; b=(dxBody*)b->next) n++;
277 if (w->nb != n) dDebug (0,"body count incorrect");
278 n = 0;
279 for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++;
280 if (w->nj != n) dDebug (0,"joint count incorrect");
281
282 // set all tag values to a known value
283 static int count = 0;
284 count++;
285 for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count;
286 for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count;
287
288 // check all body/joint world pointers are ok
289 for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w)
290 dDebug (0,"bad world pointer in body list");
291 for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w)
292 dDebug (0,"bad world pointer in joint list");
293
294 /*
295 // check for half-connected joints - actually now these are valid
296 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
297 if (j->node[0].body || j->node[1].body) {
298 if (!(j->node[0].body && j->node[1].body))
299 dDebug (0,"half connected joint found");
300 }
301 }
302 */
303
304 // check that every joint node appears in the joint lists of both bodies it
305 // attaches
306 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
307 for (int i=0; i<2; i++) {
308 if (j->node[i].body) {
309 int ok = 0;
310 for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
311 if (n->joint == j) ok = 1;
312 }
313 if (ok==0) dDebug (0,"joint not in joint list of attached body");
314 }
315 }
316 }
317
318 // check all body joint lists (correct body ptrs)
319 for (b=w->firstbody; b; b=(dxBody*)b->next) {
320 for (dxJointNode *n=b->firstjoint; n; n=n->next) {
321 if (&n->joint->node[0] == n) {
322 if (n->joint->node[1].body != b)
323 dDebug (0,"bad body pointer in joint node of body list (1)");
324 }
325 else {
326 if (n->joint->node[0].body != b)
327 dDebug (0,"bad body pointer in joint node of body list (2)");
328 }
329 if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
330 }
331 }
332
333 // check all body pointers in joints, check they are distinct
334 for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
335 if (j->node[0].body && (j->node[0].body == j->node[1].body))
336 dDebug (0,"non-distinct body pointers in joint");
337 if ((j->node[0].body && j->node[0].body->tag != count) ||
338 (j->node[1].body && j->node[1].body->tag != count))
339 dDebug (0,"bad body pointer in joint");
340 }
341}
342
343
344void dWorldCheck (dxWorld *w)
345{
346 checkWorld (w);
347}
348
349//****************************************************************************
350// body
351
352dxBody *dBodyCreate (dxWorld *w)
353{
354 dAASSERT (w);
355 dxBody *b = new dxBody;
356 initObject (b,w);
357 b->firstjoint = 0;
358 b->flags = 0;
359 b->geom = 0;
360 dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
361 dSetZero (b->invI,4*3);
362 b->invI[0] = 1;
363 b->invI[5] = 1;
364 b->invI[10] = 1;
365 b->invMass = 1;
366 dSetZero (b->pos,4);
367 dSetZero (b->q,4);
368 b->q[0] = 1;
369 dRSetIdentity (b->R);
370 dSetZero (b->lvel,4);
371 dSetZero (b->avel,4);
372 dSetZero (b->facc,4);
373 dSetZero (b->tacc,4);
374 dSetZero (b->finite_rot_axis,4);
375 addObjectToList (b,(dObject **) &w->firstbody);
376 w->nb++;
377 return b;
378}
379
380
381void dBodyDestroy (dxBody *b)
382{
383 dAASSERT (b);
384
385 // all geoms that link to this body must be notified that the body is about
386 // to disappear. note that the call to dGeomSetBody(geom,0) will result in
387 // dGeomGetBodyNext() returning 0 for the body, so we must get the next body
388 // before setting the body to 0.
389 dxGeom *next_geom = 0;
390 for (dxGeom *geom = b->geom; geom; geom = next_geom) {
391 next_geom = dGeomGetBodyNext (geom);
392 dGeomSetBody (geom,0);
393 }
394
395 // detach all neighbouring joints, then delete this body.
396 dxJointNode *n = b->firstjoint;
397 while (n) {
398 // sneaky trick to speed up removal of joint references (black magic)
399 n->joint->node[(n == n->joint->node)].body = 0;
400
401 dxJointNode *next = n->next;
402 n->next = 0;
403 removeJointReferencesFromAttachedBodies (n->joint);
404 n = next;
405 }
406 removeObjectFromList (b);
407 b->world->nb--;
408 delete b;
409}
410
411
412void dBodySetData (dBodyID b, void *data)
413{
414 dAASSERT (b);
415 b->userdata = data;
416}
417
418
419void *dBodyGetData (dBodyID b)
420{
421 dAASSERT (b);
422 return b->userdata;
423}
424
425
426void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
427{
428 dAASSERT (b);
429 b->pos[0] = x;
430 b->pos[1] = y;
431 b->pos[2] = z;
432
433 // notify all attached geoms that this body has moved
434 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
435 dGeomMoved (geom);
436}
437
438
439void dBodySetRotation (dBodyID b, const dMatrix3 R)
440{
441 dAASSERT (b && R);
442 dQuaternion q;
443 dRtoQ (R,q);
444 dNormalize4 (q);
445 b->q[0] = q[0];
446 b->q[1] = q[1];
447 b->q[2] = q[2];
448 b->q[3] = q[3];
449 dQtoR (b->q,b->R);
450
451 // notify all attached geoms that this body has moved
452 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
453 dGeomMoved (geom);
454}
455
456
457void dBodySetQuaternion (dBodyID b, const dQuaternion q)
458{
459 dAASSERT (b && q);
460 b->q[0] = q[0];
461 b->q[1] = q[1];
462 b->q[2] = q[2];
463 b->q[3] = q[3];
464 dNormalize4 (b->q);
465 dQtoR (b->q,b->R);
466
467 // notify all attached geoms that this body has moved
468 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
469 dGeomMoved (geom);
470}
471
472
473void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z)
474{
475 dAASSERT (b);
476 b->lvel[0] = x;
477 b->lvel[1] = y;
478 b->lvel[2] = z;
479}
480
481
482void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
483{
484 dAASSERT (b);
485 b->avel[0] = x;
486 b->avel[1] = y;
487 b->avel[2] = z;
488}
489
490
491const dReal * dBodyGetPosition (dBodyID b)
492{
493 dAASSERT (b);
494 return b->pos;
495}
496
497
498const dReal * dBodyGetRotation (dBodyID b)
499{
500 dAASSERT (b);
501 return b->R;
502}
503
504
505const dReal * dBodyGetQuaternion (dBodyID b)
506{
507 dAASSERT (b);
508 return b->q;
509}
510
511
512const dReal * dBodyGetLinearVel (dBodyID b)
513{
514 dAASSERT (b);
515 return b->lvel;
516}
517
518
519const dReal * dBodyGetAngularVel (dBodyID b)
520{
521 dAASSERT (b);
522 return b->avel;
523}
524
525
526void dBodySetMass (dBodyID b, const dMass *mass)
527{
528 dAASSERT (b && mass);
529 memcpy (&b->mass,mass,sizeof(dMass));
530 if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {
531 dDEBUGMSG ("inertia must be positive definite");
532 dRSetIdentity (b->invI);
533 }
534 b->invMass = dRecip(b->mass.mass);
535}
536
537
538void dBodyGetMass (dBodyID b, dMass *mass)
539{
540 dAASSERT (b && mass);
541 memcpy (mass,&b->mass,sizeof(dMass));
542}
543
544
545void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
546{
547 dAASSERT (b);
548 b->facc[0] += fx;
549 b->facc[1] += fy;
550 b->facc[2] += fz;
551}
552
553
554void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
555{
556 dAASSERT (b);
557 b->tacc[0] += fx;
558 b->tacc[1] += fy;
559 b->tacc[2] += fz;
560}
561
562
563void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
564{
565 dAASSERT (b);
566 dVector3 t1,t2;
567 t1[0] = fx;
568 t1[1] = fy;
569 t1[2] = fz;
570 t1[3] = 0;
571 dMULTIPLY0_331 (t2,b->R,t1);
572 b->facc[0] += t2[0];
573 b->facc[1] += t2[1];
574 b->facc[2] += t2[2];
575}
576
577
578void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
579{
580 dAASSERT (b);
581 dVector3 t1,t2;
582 t1[0] = fx;
583 t1[1] = fy;
584 t1[2] = fz;
585 t1[3] = 0;
586 dMULTIPLY0_331 (t2,b->R,t1);
587 b->tacc[0] += t2[0];
588 b->tacc[1] += t2[1];
589 b->tacc[2] += t2[2];
590}
591
592
593void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
594 dReal px, dReal py, dReal pz)
595{
596 dAASSERT (b);
597 b->facc[0] += fx;
598 b->facc[1] += fy;
599 b->facc[2] += fz;
600 dVector3 f,q;
601 f[0] = fx;
602 f[1] = fy;
603 f[2] = fz;
604 q[0] = px - b->pos[0];
605 q[1] = py - b->pos[1];
606 q[2] = pz - b->pos[2];
607 dCROSS (b->tacc,+=,q,f);
608}
609
610
611void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
612 dReal px, dReal py, dReal pz)
613{
614 dAASSERT (b);
615 dVector3 prel,f,p;
616 f[0] = fx;
617 f[1] = fy;
618 f[2] = fz;
619 f[3] = 0;
620 prel[0] = px;
621 prel[1] = py;
622 prel[2] = pz;
623 prel[3] = 0;
624 dMULTIPLY0_331 (p,b->R,prel);
625 b->facc[0] += f[0];
626 b->facc[1] += f[1];
627 b->facc[2] += f[2];
628 dCROSS (b->tacc,+=,p,f);
629}
630
631
632void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
633 dReal px, dReal py, dReal pz)
634{
635 dAASSERT (b);
636 dVector3 frel,f;
637 frel[0] = fx;
638 frel[1] = fy;
639 frel[2] = fz;
640 frel[3] = 0;
641 dMULTIPLY0_331 (f,b->R,frel);
642 b->facc[0] += f[0];
643 b->facc[1] += f[1];
644 b->facc[2] += f[2];
645 dVector3 q;
646 q[0] = px - b->pos[0];
647 q[1] = py - b->pos[1];
648 q[2] = pz - b->pos[2];
649 dCROSS (b->tacc,+=,q,f);
650}
651
652
653void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
654 dReal px, dReal py, dReal pz)
655{
656 dAASSERT (b);
657 dVector3 frel,prel,f,p;
658 frel[0] = fx;
659 frel[1] = fy;
660 frel[2] = fz;
661 frel[3] = 0;
662 prel[0] = px;
663 prel[1] = py;
664 prel[2] = pz;
665 prel[3] = 0;
666 dMULTIPLY0_331 (f,b->R,frel);
667 dMULTIPLY0_331 (p,b->R,prel);
668 b->facc[0] += f[0];
669 b->facc[1] += f[1];
670 b->facc[2] += f[2];
671 dCROSS (b->tacc,+=,p,f);
672}
673
674
675const dReal * dBodyGetForce (dBodyID b)
676{
677 dAASSERT (b);
678 return b->facc;
679}
680
681
682const dReal * dBodyGetTorque (dBodyID b)
683{
684 dAASSERT (b);
685 return b->tacc;
686}
687
688
689void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
690{
691 dAASSERT (b);
692 b->facc[0] = x;
693 b->facc[1] = y;
694 b->facc[2] = z;
695}
696
697
698void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
699{
700 dAASSERT (b);
701 b->tacc[0] = x;
702 b->tacc[1] = y;
703 b->tacc[2] = z;
704}
705
706
707void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
708 dVector3 result)
709{
710 dAASSERT (b);
711 dVector3 prel,p;
712 prel[0] = px;
713 prel[1] = py;
714 prel[2] = pz;
715 prel[3] = 0;
716 dMULTIPLY0_331 (p,b->R,prel);
717 result[0] = p[0] + b->pos[0];
718 result[1] = p[1] + b->pos[1];
719 result[2] = p[2] + b->pos[2];
720}
721
722
723void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
724 dVector3 result)
725{
726 dAASSERT (b);
727 dVector3 prel,p;
728 prel[0] = px;
729 prel[1] = py;
730 prel[2] = pz;
731 prel[3] = 0;
732 dMULTIPLY0_331 (p,b->R,prel);
733 result[0] = b->lvel[0];
734 result[1] = b->lvel[1];
735 result[2] = b->lvel[2];
736 dCROSS (result,+=,b->avel,p);
737}
738
739
740void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
741 dVector3 result)
742{
743 dAASSERT (b);
744 dVector3 p;
745 p[0] = px - b->pos[0];
746 p[1] = py - b->pos[1];
747 p[2] = pz - b->pos[2];
748 p[3] = 0;
749 result[0] = b->lvel[0];
750 result[1] = b->lvel[1];
751 result[2] = b->lvel[2];
752 dCROSS (result,+=,b->avel,p);
753}
754
755
756void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
757 dVector3 result)
758{
759 dAASSERT (b);
760 dVector3 prel;
761 prel[0] = px - b->pos[0];
762 prel[1] = py - b->pos[1];
763 prel[2] = pz - b->pos[2];
764 prel[3] = 0;
765 dMULTIPLY1_331 (result,b->R,prel);
766}
767
768
769void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
770 dVector3 result)
771{
772 dAASSERT (b);
773 dVector3 p;
774 p[0] = px;
775 p[1] = py;
776 p[2] = pz;
777 p[3] = 0;
778 dMULTIPLY0_331 (result,b->R,p);
779}
780
781
782void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
783 dVector3 result)
784{
785 dAASSERT (b);
786 dVector3 p;
787 p[0] = px;
788 p[1] = py;
789 p[2] = pz;
790 p[3] = 0;
791 dMULTIPLY1_331 (result,b->R,p);
792}
793
794
795void dBodySetFiniteRotationMode (dBodyID b, int mode)
796{
797 dAASSERT (b);
798 b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);
799 if (mode) {
800 b->flags |= dxBodyFlagFiniteRotation;
801 if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||
802 b->finite_rot_axis[2] != 0) {
803 b->flags |= dxBodyFlagFiniteRotationAxis;
804 }
805 }
806}
807
808
809void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
810{
811 dAASSERT (b);
812 b->finite_rot_axis[0] = x;
813 b->finite_rot_axis[1] = y;
814 b->finite_rot_axis[2] = z;
815 if (x != 0 || y != 0 || z != 0) {
816 dNormalize3 (b->finite_rot_axis);
817 b->flags |= dxBodyFlagFiniteRotationAxis;
818 }
819 else {
820 b->flags &= ~dxBodyFlagFiniteRotationAxis;
821 }
822}
823
824
825int dBodyGetFiniteRotationMode (dBodyID b)
826{
827 dAASSERT (b);
828 return ((b->flags & dxBodyFlagFiniteRotation) != 0);
829}
830
831
832void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
833{
834 dAASSERT (b);
835 result[0] = b->finite_rot_axis[0];
836 result[1] = b->finite_rot_axis[1];
837 result[2] = b->finite_rot_axis[2];
838}
839
840
841int dBodyGetNumJoints (dBodyID b)
842{
843 dAASSERT (b);
844 int count=0;
845 for (dxJointNode *n=b->firstjoint; n; n=n->next, count++);
846 return count;
847}
848
849
850dJointID dBodyGetJoint (dBodyID b, int index)
851{
852 dAASSERT (b);
853 int i=0;
854 for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {
855 if (i == index) return n->joint;
856 }
857 return 0;
858}
859
860
861void dBodyEnable (dBodyID b)
862{
863 dAASSERT (b);
864 b->flags &= ~dxBodyDisabled;
865}
866
867
868void dBodyDisable (dBodyID b)
869{
870 dAASSERT (b);
871 b->flags |= dxBodyDisabled;
872}
873
874
875int dBodyIsEnabled (dBodyID b)
876{
877 dAASSERT (b);
878 return ((b->flags & dxBodyDisabled) == 0);
879}
880
881
882void dBodySetGravityMode (dBodyID b, int mode)
883{
884 dAASSERT (b);
885 if (mode) b->flags &= ~dxBodyNoGravity;
886 else b->flags |= dxBodyNoGravity;
887}
888
889
890int dBodyGetGravityMode (dBodyID b)
891{
892 dAASSERT (b);
893 return ((b->flags & dxBodyNoGravity) == 0);
894}
895
896//****************************************************************************
897// joints
898
899static void dJointInit (dxWorld *w, dxJoint *j)
900{
901 dIASSERT (w && j);
902 initObject (j,w);
903 j->vtable = 0;
904 j->flags = 0;
905 j->node[0].joint = j;
906 j->node[0].body = 0;
907 j->node[0].next = 0;
908 j->node[1].joint = j;
909 j->node[1].body = 0;
910 j->node[1].next = 0;
911 addObjectToList (j,(dObject **) &w->firstjoint);
912 w->nj++;
913}
914
915
916static dxJoint *createJoint (dWorldID w, dJointGroupID group,
917 dxJoint::Vtable *vtable)
918{
919 dIASSERT (w && vtable);
920 dxJoint *j;
921 if (group) {
922 j = (dxJoint*) group->stack.alloc (vtable->size);
923 group->num++;
924 }
925 else j = (dxJoint*) dAlloc (vtable->size);
926 dJointInit (w,j);
927 j->vtable = vtable;
928 if (group) j->flags |= dJOINT_INGROUP;
929 if (vtable->init) vtable->init (j);
930 j->feedback = 0;
931 /******************** breakable joint contribution ***********************/
932 j->breakInfo = 0;
933 /*************************************************************************/
934 return j;
935}
936
937
938dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
939{
940 dAASSERT (w);
941 return createJoint (w,group,&__dball_vtable);
942}
943
944
945dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
946{
947 dAASSERT (w);
948 return createJoint (w,group,&__dhinge_vtable);
949}
950
951
952dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
953{
954 dAASSERT (w);
955 return createJoint (w,group,&__dslider_vtable);
956}
957
958
959dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
960 const dContact *c)
961{
962 dAASSERT (w && c);
963 dxJointContact *j = (dxJointContact *)
964 createJoint (w,group,&__dcontact_vtable);
965 j->contact = *c;
966 return j;
967}
968
969
970dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
971{
972 dAASSERT (w);
973 return createJoint (w,group,&__dhinge2_vtable);
974}
975
976
977dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
978{
979 dAASSERT (w);
980 return createJoint (w,group,&__duniversal_vtable);
981}
982
983
984dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
985{
986 dAASSERT (w);
987 return createJoint (w,group,&__dfixed_vtable);
988}
989
990
991dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
992{
993 dAASSERT (w);
994 return createJoint (w,group,&__dnull_vtable);
995}
996
997
998dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
999{
1000 dAASSERT (w);
1001 return createJoint (w,group,&__damotor_vtable);
1002}
1003
1004
1005void dJointDestroy (dxJoint *j)
1006{
1007 dAASSERT (j);
1008 if (j->flags & dJOINT_INGROUP) return;
1009 removeJointReferencesFromAttachedBodies (j);
1010 removeObjectFromList (j);
1011 /******************** breakable joint contribution ***********************/
1012 if (j->breakInfo) delete j->breakInfo;
1013 /*************************************************************************/
1014 j->world->nj--;
1015 dFree (j,j->vtable->size);
1016}
1017
1018
1019dJointGroupID dJointGroupCreate (int max_size)
1020{
1021 // not any more ... dUASSERT (max_size > 0,"max size must be > 0");
1022 dxJointGroup *group = new dxJointGroup;
1023 group->num = 0;
1024 return group;
1025}
1026
1027
1028void dJointGroupDestroy (dJointGroupID group)
1029{
1030 dAASSERT (group);
1031 dJointGroupEmpty (group);
1032 delete group;
1033}
1034
1035
1036void dJointGroupEmpty (dJointGroupID group)
1037{
1038 // the joints in this group are detached starting from the most recently
1039 // added (at the top of the stack). this helps ensure that the various
1040 // linked lists are not traversed too much, as the joints will hopefully
1041 // be at the start of those lists.
1042 // if any group joints have their world pointer set to 0, their world was
1043 // previously destroyed. no special handling is required for these joints.
1044
1045 dAASSERT (group);
1046 int i;
1047 dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*));
1048 dxJoint *j = (dxJoint*) group->stack.rewind();
1049 for (i=0; i < group->num; i++) {
1050 jlist[i] = j;
1051 j = (dxJoint*) (group->stack.next (j->vtable->size));
1052 }
1053 for (i=group->num-1; i >= 0; i--) {
1054 if (jlist[i]->world) {
1055 removeJointReferencesFromAttachedBodies (jlist[i]);
1056 removeObjectFromList (jlist[i]);
1057 jlist[i]->world->nj--;
1058 }
1059 }
1060 group->num = 0;
1061 group->stack.freeAll();
1062}
1063
1064
1065void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
1066{
1067 // check arguments
1068 dUASSERT (joint,"bad joint argument");
1069 dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2");
1070 dxWorld *world = joint->world;
1071 dUASSERT ( (!body1 || body1->world == world) &&
1072 (!body2 || body2->world == world),
1073 "joint and bodies must be in same world");
1074
1075 // check if the joint can not be attached to just one body
1076 dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
1077 ((body1 != 0) ^ (body2 != 0))),
1078 "joint can not be attached to just one body");
1079
1080 // remove any existing body attachments
1081 if (joint->node[0].body || joint->node[1].body) {
1082 removeJointReferencesFromAttachedBodies (joint);
1083 }
1084
1085 // if a body is zero, make sure that it is body2, so 0 --> node[1].body
1086 if (body1==0) {
1087 body1 = body2;
1088 body2 = 0;
1089 joint->flags |= dJOINT_REVERSE;
1090 }
1091 else {
1092 joint->flags &= (~dJOINT_REVERSE);
1093 }
1094
1095 // attach to new bodies
1096 joint->node[0].body = body1;
1097 joint->node[1].body = body2;
1098 if (body1) {
1099 joint->node[1].next = body1->firstjoint;
1100 body1->firstjoint = &joint->node[1];
1101 }
1102 else joint->node[1].next = 0;
1103 if (body2) {
1104 joint->node[0].next = body2->firstjoint;
1105 body2->firstjoint = &joint->node[0];
1106 }
1107 else {
1108 joint->node[0].next = 0;
1109 }
1110}
1111
1112
1113void dJointSetData (dxJoint *joint, void *data)
1114{
1115 dAASSERT (joint);
1116 joint->userdata = data;
1117}
1118
1119
1120void *dJointGetData (dxJoint *joint)
1121{
1122 dAASSERT (joint);
1123 return joint->userdata;
1124}
1125
1126
1127int dJointGetType (dxJoint *joint)
1128{
1129 dAASSERT (joint);
1130 return joint->vtable->typenum;
1131}
1132
1133
1134dBodyID dJointGetBody (dxJoint *joint, int index)
1135{
1136 dAASSERT (joint);
1137 if (index >= 0 && index < 2) return joint->node[index].body;
1138 else return 0;
1139}
1140
1141
1142void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)
1143{
1144 dAASSERT (joint);
1145 joint->feedback = f;
1146}
1147
1148
1149dJointFeedback *dJointGetFeedback (dxJoint *joint)
1150{
1151 dAASSERT (joint);
1152 return joint->feedback;
1153}
1154
1155
1156int dAreConnected (dBodyID b1, dBodyID b2)
1157{
1158 dAASSERT (b1 && b2);
1159 // look through b1's neighbour list for b2
1160 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1161 if (n->body == b2) return 1;
1162 }
1163 return 0;
1164}
1165
1166
1167int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type)
1168{
1169 dAASSERT (b1 && b2);
1170 // look through b1's neighbour list for b2
1171 for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
1172 if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1;
1173 }
1174 return 0;
1175}
1176
1177//****************************************************************************
1178// world
1179
1180dxWorld * dWorldCreate()
1181{
1182 dxWorld *w = new dxWorld;
1183 w->firstbody = 0;
1184 w->firstjoint = 0;
1185 w->nb = 0;
1186 w->nj = 0;
1187 dSetZero (w->gravity,4);
1188 w->global_erp = REAL(0.2);
1189#if defined(dSINGLE)
1190 w->global_cfm = 1e-5f;
1191#elif defined(dDOUBLE)
1192 w->global_cfm = 1e-10;
1193#else
1194 #error dSINGLE or dDOUBLE must be defined
1195#endif
1196 return w;
1197}
1198
1199
1200void dWorldDestroy (dxWorld *w)
1201{
1202 // delete all bodies and joints
1203 dAASSERT (w);
1204 dxBody *nextb, *b = w->firstbody;
1205 while (b) {
1206 nextb = (dxBody*) b->next;
1207 delete b;
1208 b = nextb;
1209 }
1210 dxJoint *nextj, *j = w->firstjoint;
1211 while (j) {
1212 nextj = (dxJoint*)j->next;
1213 if (j->flags & dJOINT_INGROUP) {
1214 // the joint is part of a group, so "deactivate" it instead
1215 j->world = 0;
1216 j->node[0].body = 0;
1217 j->node[0].next = 0;
1218 j->node[1].body = 0;
1219 j->node[1].next = 0;
1220 dMessage (0,"warning: destroying world containing grouped joints");
1221 }
1222 else {
1223 dFree (j,j->vtable->size);
1224 }
1225 j = nextj;
1226 }
1227 delete w;
1228}
1229
1230
1231void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
1232{
1233 dAASSERT (w);
1234 w->gravity[0] = x;
1235 w->gravity[1] = y;
1236 w->gravity[2] = z;
1237}
1238
1239
1240void dWorldGetGravity (dWorldID w, dVector3 g)
1241{
1242 dAASSERT (w);
1243 g[0] = w->gravity[0];
1244 g[1] = w->gravity[1];
1245 g[2] = w->gravity[2];
1246}
1247
1248
1249void dWorldSetERP (dWorldID w, dReal erp)
1250{
1251 dAASSERT (w);
1252 w->global_erp = erp;
1253}
1254
1255
1256dReal dWorldGetERP (dWorldID w)
1257{
1258 dAASSERT (w);
1259 return w->global_erp;
1260}
1261
1262
1263void dWorldSetCFM (dWorldID w, dReal cfm)
1264{
1265 dAASSERT (w);
1266 w->global_cfm = cfm;
1267}
1268
1269
1270dReal dWorldGetCFM (dWorldID w)
1271{
1272 dAASSERT (w);
1273 return w->global_cfm;
1274}
1275
1276
1277void dWorldStep (dWorldID w, dReal stepsize)
1278{
1279 dUASSERT (w,"bad world argument");
1280 dUASSERT (stepsize > 0,"stepsize must be > 0");
1281 processIslands (w,stepsize);
1282}
1283
1284
1285void dWorldImpulseToForce (dWorldID w, dReal stepsize,
1286 dReal ix, dReal iy, dReal iz,
1287 dVector3 force)
1288{
1289 dAASSERT (w);
1290 stepsize = dRecip(stepsize);
1291 force[0] = stepsize * ix;
1292 force[1] = stepsize * iy;
1293 force[2] = stepsize * iz;
1294 // @@@ force[3] = 0;
1295}
1296
1297//****************************************************************************
1298// testing
1299
1300#define NUM 100
1301
1302#define DO(x)
1303
1304
1305extern "C" void dTestDataStructures()
1306{
1307 int i;
1308 DO(printf ("testDynamicsStuff()\n"));
1309
1310 dBodyID body [NUM];
1311 int nb = 0;
1312 dJointID joint [NUM];
1313 int nj = 0;
1314
1315 for (i=0; i<NUM; i++) body[i] = 0;
1316 for (i=0; i<NUM; i++) joint[i] = 0;
1317
1318 DO(printf ("creating world\n"));
1319 dWorldID w = dWorldCreate();
1320 checkWorld (w);
1321
1322 for (;;) {
1323 if (nb < NUM && dRandReal() > 0.5) {
1324 DO(printf ("creating body\n"));
1325 body[nb] = dBodyCreate (w);
1326 DO(printf ("\t--> %p\n",body[nb]));
1327 nb++;
1328 checkWorld (w);
1329 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1330 }
1331 if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
1332 dBodyID b1 = body [dRand() % nb];
1333 dBodyID b2 = body [dRand() % nb];
1334 if (b1 != b2) {
1335 DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
1336 joint[nj] = dJointCreateBall (w,0);
1337 DO(printf ("\t-->%p\n",joint[nj]));
1338 checkWorld (w);
1339 dJointAttach (joint[nj],b1,b2);
1340 nj++;
1341 checkWorld (w);
1342 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1343 }
1344 }
1345 if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
1346 dBodyID b1 = body [dRand() % nb];
1347 dBodyID b2 = body [dRand() % nb];
1348 if (b1 != b2) {
1349 int k = dRand() % nj;
1350 DO(printf ("reattaching joint %p\n",joint[k]));
1351 dJointAttach (joint[k],b1,b2);
1352 checkWorld (w);
1353 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1354 }
1355 }
1356 if (nb > 0 && dRandReal() > 0.5) {
1357 int k = dRand() % nb;
1358 DO(printf ("destroying body %p\n",body[k]));
1359 dBodyDestroy (body[k]);
1360 checkWorld (w);
1361 for (; k < (NUM-1); k++) body[k] = body[k+1];
1362 nb--;
1363 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1364 }
1365 if (nj > 0 && dRandReal() > 0.5) {
1366 int k = dRand() % nj;
1367 DO(printf ("destroying joint %p\n",joint[k]));
1368 dJointDestroy (joint[k]);
1369 checkWorld (w);
1370 for (; k < (NUM-1); k++) joint[k] = joint[k+1];
1371 nj--;
1372 DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
1373 }
1374 }
1375
1376 /*
1377 printf ("creating world\n");
1378 dWorldID w = dWorldCreate();
1379 checkWorld (w);
1380 printf ("creating body\n");
1381 dBodyID b1 = dBodyCreate (w);
1382 checkWorld (w);
1383 printf ("creating body\n");
1384 dBodyID b2 = dBodyCreate (w);
1385 checkWorld (w);
1386 printf ("creating joint\n");
1387 dJointID j = dJointCreateBall (w);
1388 checkWorld (w);
1389 printf ("attaching joint\n");
1390 dJointAttach (j,b1,b2);
1391 checkWorld (w);
1392 printf ("destroying joint\n");
1393 dJointDestroy (j);
1394 checkWorld (w);
1395 printf ("destroying body\n");
1396 dBodyDestroy (b1);
1397 checkWorld (w);
1398 printf ("destroying body\n");
1399 dBodyDestroy (b2);
1400 checkWorld (w);
1401 printf ("destroying world\n");
1402 dWorldDestroy (w);
1403 */
1404}
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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23#include "objects.h"
24#include "joint.h"
25#include <ode/config.h>
26#include <ode/odemath.h>
27#include <ode/rotation.h>
28#include <ode/timer.h>
29#include <ode/error.h>
30#include <ode/matrix.h>
31#include "lcp.h"
32
33//****************************************************************************
34// misc defines
35
36#define FAST_FACTOR
37//#define TIMING
38
39#define ALLOCA dALLOCA16
40
41//****************************************************************************
42// debugging - comparison of various vectors and matrices produced by the
43// slow and fast versions of the stepper.
44
45//#define COMPARE_METHODS
46
47#ifdef COMPARE_METHODS
48#include "testing.h"
49dMatrixComparison comparator;
50#endif
51
52//****************************************************************************
53// special matrix multipliers
54
55// this assumes the 4th and 8th rows of B and C are zero.
56
57static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,
58 int p, int r, int Askip)
59{
60 int i,j;
61 dReal sum,*bb,*cc;
62 dIASSERT (p>0 && r>0 && A && B && C);
63 bb = B;
64 for (i=p; i; i--) {
65 cc = C;
66 for (j=r; j; j--) {
67 sum = bb[0]*cc[0];
68 sum += bb[1]*cc[1];
69 sum += bb[2]*cc[2];
70 sum += bb[4]*cc[4];
71 sum += bb[5]*cc[5];
72 sum += bb[6]*cc[6];
73 *(A++) = sum;
74 cc += 8;
75 }
76 A += Askip - r;
77 bb += 8;
78 }
79}
80
81
82// this assumes the 4th and 8th rows of B and C are zero.
83
84static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,
85 int p, int r, int Askip)
86{
87 int i,j;
88 dReal sum,*bb,*cc;
89 dIASSERT (p>0 && r>0 && A && B && C);
90 bb = B;
91 for (i=p; i; i--) {
92 cc = C;
93 for (j=r; j; j--) {
94 sum = bb[0]*cc[0];
95 sum += bb[1]*cc[1];
96 sum += bb[2]*cc[2];
97 sum += bb[4]*cc[4];
98 sum += bb[5]*cc[5];
99 sum += bb[6]*cc[6];
100 *(A++) += sum;
101 cc += 8;
102 }
103 A += Askip - r;
104 bb += 8;
105 }
106}
107
108
109// this assumes the 4th and 8th rows of B are zero.
110
111static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)
112{
113 int i;
114 dIASSERT (p>0 && A && B && C);
115 dReal sum;
116 for (i=p; i; i--) {
117 sum = B[0]*C[0];
118 sum += B[1]*C[1];
119 sum += B[2]*C[2];
120 sum += B[4]*C[4];
121 sum += B[5]*C[5];
122 sum += B[6]*C[6];
123 *(A++) = sum;
124 B += 8;
125 }
126}
127
128
129// this assumes the 4th and 8th rows of B are zero.
130
131static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)
132{
133 int i;
134 dIASSERT (p>0 && A && B && C);
135 dReal sum;
136 for (i=p; i; i--) {
137 sum = B[0]*C[0];
138 sum += B[1]*C[1];
139 sum += B[2]*C[2];
140 sum += B[4]*C[4];
141 sum += B[5]*C[5];
142 sum += B[6]*C[6];
143 *(A++) += sum;
144 B += 8;
145 }
146}
147
148
149// this assumes the 4th and 8th rows of B are zero.
150
151static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)
152{
153 int k;
154 dReal sum;
155 dIASSERT (q>0 && A && B && C);
156 sum = 0;
157 for (k=0; k<q; k++) sum += B[k*8] * C[k];
158 A[0] += sum;
159 sum = 0;
160 for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
161 A[1] += sum;
162 sum = 0;
163 for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
164 A[2] += sum;
165 sum = 0;
166 for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
167 A[4] += sum;
168 sum = 0;
169 for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
170 A[5] += sum;
171 sum = 0;
172 for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
173 A[6] += sum;
174}
175
176
177// this assumes the 4th and 8th rows of B are zero.
178
179static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
180{
181 int k;
182 dReal sum;
183 dIASSERT (q>0 && A && B && C);
184 sum = 0;
185 for (k=0; k<q; k++) sum += B[k*8] * C[k];
186 A[0] = sum;
187 sum = 0;
188 for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
189 A[1] = sum;
190 sum = 0;
191 for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
192 A[2] = sum;
193 sum = 0;
194 for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
195 A[4] = sum;
196 sum = 0;
197 for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
198 A[5] = sum;
199 sum = 0;
200 for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
201 A[6] = sum;
202}
203
204//****************************************************************************
205// body rotation
206
207// return sin(x)/x. this has a singularity at 0 so special handling is needed
208// for small arguments.
209
210static inline dReal sinc (dReal x)
211{
212 // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
213 // is actually accurate to one LS bit within this range if double precision
214 // is being used - so don't worry!
215 if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
216 else return dSin(x)/x;
217}
218
219
220// given a body b, apply its linear and angular rotation over the time
221// interval h, thereby adjusting its position and orientation.
222
223static inline void moveAndRotateBody (dxBody *b, dReal h)
224{
225 int j;
226
227 // handle linear velocity
228 for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
229
230 if (b->flags & dxBodyFlagFiniteRotation) {
231 dVector3 irv; // infitesimal rotation vector
232 dQuaternion q; // quaternion for finite rotation
233
234 if (b->flags & dxBodyFlagFiniteRotationAxis) {
235 // split the angular velocity vector into a component along the finite
236 // rotation axis, and a component orthogonal to it.
237 dVector3 frv,irv; // finite rotation vector
238 dReal k = dDOT (b->finite_rot_axis,b->avel);
239 frv[0] = b->finite_rot_axis[0] * k;
240 frv[1] = b->finite_rot_axis[1] * k;
241 frv[2] = b->finite_rot_axis[2] * k;
242 irv[0] = b->avel[0] - frv[0];
243 irv[1] = b->avel[1] - frv[1];
244 irv[2] = b->avel[2] - frv[2];
245
246 // make a rotation quaternion q that corresponds to frv * h.
247 // compare this with the full-finite-rotation case below.
248 h *= REAL(0.5);
249 dReal theta = k * h;
250 q[0] = dCos(theta);
251 dReal s = sinc(theta) * h;
252 q[1] = frv[0] * s;
253 q[2] = frv[1] * s;
254 q[3] = frv[2] * s;
255 }
256 else {
257 // make a rotation quaternion q that corresponds to w * h
258 dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
259 b->avel[2]*b->avel[2]);
260 h *= REAL(0.5);
261 dReal theta = wlen * h;
262 q[0] = dCos(theta);
263 dReal s = sinc(theta) * h;
264 q[1] = b->avel[0] * s;
265 q[2] = b->avel[1] * s;
266 q[3] = b->avel[2] * s;
267 }
268
269 // do the finite rotation
270 dQuaternion q2;
271 dQMultiply0 (q2,q,b->q);
272 for (j=0; j<4; j++) b->q[j] = q2[j];
273
274 // do the infitesimal rotation if required
275 if (b->flags & dxBodyFlagFiniteRotationAxis) {
276 dReal dq[4];
277 dWtoDQ (irv,b->q,dq);
278 for (j=0; j<4; j++) b->q[j] += h * dq[j];
279 }
280 }
281 else {
282 // the normal way - do an infitesimal rotation
283 dReal dq[4];
284 dWtoDQ (b->avel,b->q,dq);
285 for (j=0; j<4; j++) b->q[j] += h * dq[j];
286 }
287
288 // normalize the quaternion and convert it to a rotation matrix
289 dNormalize4 (b->q);
290 dQtoR (b->q,b->R);
291
292 // notify all attached geoms that this body has moved
293 for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
294 dGeomMoved (geom);
295}
296
297//****************************************************************************
298// the slow, but sure way
299// note that this does not do any joint feedback!
300
301// given lists of bodies and joints that form an island, perform a first
302// order timestep.
303//
304// `body' is the body array, `nb' is the size of the array.
305// `_joint' is the body array, `nj' is the size of the array.
306
307void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
308 dxJoint * const *_joint, int nj, dReal stepsize)
309{
310 int i,j,k;
311 int n6 = 6*nb;
312
313# ifdef TIMING
314 dTimerStart("preprocessing");
315# endif
316
317 // number all bodies in the body list - set their tag values
318 for (i=0; i<nb; i++) body[i]->tag = i;
319
320 // make a local copy of the joint array, because we might want to modify it.
321 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
322 // but not the joint array, because the caller might need it unchanged).
323 dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
324 memcpy (joint,_joint,nj * sizeof(dxJoint*));
325
326 // for all bodies, compute the inertia tensor and its inverse in the global
327 // frame, and compute the rotational force and add it to the torque
328 // accumulator.
329 // @@@ check computation of rotational force.
330 dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
331 dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
332
333 //dSetZero (I,3*nb*4);
334 //dSetZero (invI,3*nb*4);
335 for (i=0; i<nb; i++) {
336 dReal tmp[12];
337 // compute inertia tensor in global frame
338 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
339 dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
340 // compute inverse inertia tensor in global frame
341 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
342 dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
343 // compute rotational force
344 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
345 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
346 }
347
348 // add the gravity force to all bodies
349 for (i=0; i<nb; i++) {
350 if ((body[i]->flags & dxBodyNoGravity)==0) {
351 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
352 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
353 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
354 }
355 }
356
357 // get m = total constraint dimension, nub = number of unbounded variables.
358 // create constraint offset array and number-of-rows array for all joints.
359 // the constraints are re-ordered as follows: the purely unbounded
360 // constraints, the mixed unbounded + LCP constraints, and last the purely
361 // LCP constraints.
362 //
363 // joints with m=0 are inactive and are removed from the joints array
364 // entirely, so that the code that follows does not consider them.
365 int m = 0;
366 dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
367 int *ofs = (int*) ALLOCA (nj*sizeof(int));
368 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
369 joint[j]->vtable->getInfo1 (joint[j],info+i);
370 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
371 info[i].nub >= 0 && info[i].nub <= info[i].m);
372 if (info[i].m > 0) {
373 joint[i] = joint[j];
374 i++;
375 }
376 }
377 nj = i;
378
379 // the purely unbounded constraints
380 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
381 ofs[i] = m;
382 m += info[i].m;
383 }
384 int nub = m;
385 // the mixed unbounded + LCP constraints
386 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
387 ofs[i] = m;
388 m += info[i].m;
389 }
390 // the purely LCP constraints
391 for (i=0; i<nj; i++) if (info[i].nub == 0) {
392 ofs[i] = m;
393 m += info[i].m;
394 }
395
396 // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
397 // parameters
398# ifdef TIMING
399 dTimerNow ("create mass matrix");
400# endif
401 int nskip = dPAD (n6);
402 dReal *invM = (dReal*) ALLOCA (n6*nskip*sizeof(dReal));
403 dSetZero (invM,n6*nskip);
404 for (i=0; i<nb; i++) {
405 dReal *MM = invM+(i*6)*nskip+(i*6);
406 MM[0] = body[i]->invMass;
407 MM[nskip+1] = body[i]->invMass;
408 MM[2*nskip+2] = body[i]->invMass;
409 MM += 3*nskip+3;
410 for (j=0; j<3; j++) for (k=0; k<3; k++) {
411 MM[j*nskip+k] = invI[i*12+j*4+k];
412 }
413 }
414
415 // assemble some body vectors: fe = external forces, v = velocities
416 dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal));
417 dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal));
418 //dSetZero (fe,n6);
419 //dSetZero (v,n6);
420 for (i=0; i<nb; i++) {
421 for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
422 for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
423 for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
424 for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
425 }
426
427 // this will be set to the velocity update
428 dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal));
429 dSetZero (vnew,n6);
430
431 // if there are constraints, compute cforce
432 if (m > 0) {
433 // create a constraint equation right hand side vector `c', a constraint
434 // force mixing vector `cfm', and LCP low and high bound vectors, and an
435 // 'findex' vector.
436 dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
437 dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
438 dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
439 dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
440 int *findex = (int*) alloca (m*sizeof(int));
441 dSetZero (c,m);
442 dSetValue (cfm,m,world->global_cfm);
443 dSetValue (lo,m,-dInfinity);
444 dSetValue (hi,m, dInfinity);
445 for (i=0; i<m; i++) findex[i] = -1;
446
447 // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
448 // data. also fill the c vector.
449# ifdef TIMING
450 dTimerNow ("create J");
451# endif
452 dReal *J = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
453 dSetZero (J,m*nskip);
454 dxJoint::Info2 Jinfo;
455 Jinfo.rowskip = nskip;
456 Jinfo.fps = dRecip(stepsize);
457 Jinfo.erp = world->global_erp;
458 for (i=0; i<nj; i++) {
459 Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
460 Jinfo.J1a = Jinfo.J1l + 3;
461 if (joint[i]->node[1].body) {
462 Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
463 Jinfo.J2a = Jinfo.J2l + 3;
464 }
465 else {
466 Jinfo.J2l = 0;
467 Jinfo.J2a = 0;
468 }
469 Jinfo.c = c + ofs[i];
470 Jinfo.cfm = cfm + ofs[i];
471 Jinfo.lo = lo + ofs[i];
472 Jinfo.hi = hi + ofs[i];
473 Jinfo.findex = findex + ofs[i];
474 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
475 // adjust returned findex values for global index numbering
476 for (j=0; j<info[i].m; j++) {
477 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
478 }
479 }
480
481 // compute A = J*invM*J'
482# ifdef TIMING
483 dTimerNow ("compute A");
484# endif
485 dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
486 //dSetZero (JinvM,m*nskip);
487 dMultiply0 (JinvM,J,invM,m,n6,n6);
488 int mskip = dPAD(m);
489 dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
490 //dSetZero (A,m*mskip);
491 dMultiply2 (A,JinvM,J,m,n6,m);
492
493 // add cfm to the diagonal of A
494 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
495
496# ifdef COMPARE_METHODS
497 comparator.nextMatrix (A,m,m,1,"A");
498# endif
499
500 // compute `rhs', the right hand side of the equation J*a=c
501# ifdef TIMING
502 dTimerNow ("compute rhs");
503# endif
504 dReal *tmp1 = (dReal*) ALLOCA (n6 * sizeof(dReal));
505 //dSetZero (tmp1,n6);
506 dMultiply0 (tmp1,invM,fe,n6,n6,1);
507 for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
508 dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
509 //dSetZero (rhs,m);
510 dMultiply0 (rhs,J,tmp1,m,n6,1);
511 for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
512
513# ifdef COMPARE_METHODS
514 comparator.nextMatrix (c,m,1,0,"c");
515 comparator.nextMatrix (rhs,m,1,0,"rhs");
516# endif
517
518 // solve the LCP problem and get lambda.
519 // this will destroy A but that's okay
520# ifdef TIMING
521 dTimerNow ("solving LCP problem");
522# endif
523 dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
524 dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
525 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
526
527// OLD WAY - direct factor and solve
528//
529// // factorize A (L*L'=A)
530//# ifdef TIMING
531// dTimerNow ("factorize A");
532//# endif
533// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
534// memcpy (L,A,m*mskip*sizeof(dReal));
535// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
536//
537// // compute lambda
538//# ifdef TIMING
539// dTimerNow ("compute lambda");
540//# endif
541// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
542// memcpy (lambda,rhs,m * sizeof(dReal));
543// dSolveCholesky (L,lambda,m);
544
545# ifdef COMPARE_METHODS
546 comparator.nextMatrix (lambda,m,1,0,"lambda");
547# endif
548
549 // compute the velocity update `vnew'
550# ifdef TIMING
551 dTimerNow ("compute velocity update");
552# endif
553 dMultiply1 (tmp1,J,lambda,n6,m,1);
554 for (i=0; i<n6; i++) tmp1[i] += fe[i];
555 dMultiply0 (vnew,invM,tmp1,n6,n6,1);
556 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
557
558 // see if the constraint has worked: compute J*vnew and make sure it equals
559 // `c' (to within a certain tolerance).
560# ifdef TIMING
561 dTimerNow ("verify constraint equation");
562# endif
563 dMultiply0 (tmp1,J,vnew,m,n6,1);
564 dReal err = 0;
565 for (i=0; i<m; i++) err += dFabs(tmp1[i]-c[i]);
566 printf ("%.6e\n",err);
567 }
568 else {
569 // no constraints
570 dMultiply0 (vnew,invM,fe,n6,n6,1);
571 for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
572 }
573
574# ifdef COMPARE_METHODS
575 comparator.nextMatrix (vnew,n6,1,0,"vnew");
576# endif
577
578 // apply the velocity update to the bodies
579# ifdef TIMING
580 dTimerNow ("update velocity");
581# endif
582 for (i=0; i<nb; i++) {
583 for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
584 for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
585 }
586
587 // update the position and orientation from the new linear/angular velocity
588 // (over the given timestep)
589# ifdef TIMING
590 dTimerNow ("update position");
591# endif
592 for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
593
594# ifdef TIMING
595 dTimerNow ("tidy up");
596# endif
597
598 // zero all force accumulators
599 for (i=0; i<nb; i++) {
600 body[i]->facc[0] = 0;
601 body[i]->facc[1] = 0;
602 body[i]->facc[2] = 0;
603 body[i]->facc[3] = 0;
604 body[i]->tacc[0] = 0;
605 body[i]->tacc[1] = 0;
606 body[i]->tacc[2] = 0;
607 body[i]->tacc[3] = 0;
608 }
609
610# ifdef TIMING
611 dTimerEnd();
612 if (m > 0) dTimerReport (stdout,1);
613# endif
614}
615
616//****************************************************************************
617// an optimized version of dInternalStepIsland1()
618
619void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb,
620 dxJoint * const *_joint, int nj, dReal stepsize)
621{
622 int i,j,k;
623# ifdef TIMING
624 dTimerStart("preprocessing");
625# endif
626
627 dReal stepsize1 = dRecip(stepsize);
628
629 // number all bodies in the body list - set their tag values
630 for (i=0; i<nb; i++) body[i]->tag = i;
631
632 // make a local copy of the joint array, because we might want to modify it.
633 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
634 // but not the joint array, because the caller might need it unchanged).
635 dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
636 memcpy (joint,_joint,nj * sizeof(dxJoint*));
637
638 // for all bodies, compute the inertia tensor and its inverse in the global
639 // frame, and compute the rotational force and add it to the torque
640 // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
641 // @@@ check computation of rotational force.
642 dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
643 dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
644
645 //dSetZero (I,3*nb*4);
646 //dSetZero (invI,3*nb*4);
647 for (i=0; i<nb; i++) {
648 dReal tmp[12];
649 // compute inertia tensor in global frame
650 dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
651 dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
652 // compute inverse inertia tensor in global frame
653 dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
654 dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
655 // compute rotational force
656 dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
657 dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
658 }
659
660 // add the gravity force to all bodies
661 for (i=0; i<nb; i++) {
662 if ((body[i]->flags & dxBodyNoGravity)==0) {
663 body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
664 body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
665 body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
666 }
667 }
668
669 // get m = total constraint dimension, nub = number of unbounded variables.
670 // create constraint offset array and number-of-rows array for all joints.
671 // the constraints are re-ordered as follows: the purely unbounded
672 // constraints, the mixed unbounded + LCP constraints, and last the purely
673 // LCP constraints. this assists the LCP solver to put all unbounded
674 // variables at the start for a quick factorization.
675 //
676 // joints with m=0 are inactive and are removed from the joints array
677 // entirely, so that the code that follows does not consider them.
678 // also number all active joints in the joint list (set their tag values).
679 // inactive joints receive a tag value of -1.
680
681 int m = 0;
682 dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
683 int *ofs = (int*) ALLOCA (nj*sizeof(int));
684 for (i=0, j=0; j<nj; j++) { // i=dest, j=src
685 joint[j]->vtable->getInfo1 (joint[j],info+i);
686 dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
687 info[i].nub >= 0 && info[i].nub <= info[i].m);
688 if (info[i].m > 0) {
689 joint[i] = joint[j];
690 joint[i]->tag = i;
691 i++;
692 }
693 else {
694 joint[j]->tag = -1;
695 }
696 }
697 nj = i;
698
699 // the purely unbounded constraints
700 for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
701 ofs[i] = m;
702 m += info[i].m;
703 }
704 int nub = m;
705 // the mixed unbounded + LCP constraints
706 for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
707 ofs[i] = m;
708 m += info[i].m;
709 }
710 // the purely LCP constraints
711 for (i=0; i<nj; i++) if (info[i].nub == 0) {
712 ofs[i] = m;
713 m += info[i].m;
714 }
715
716 // this will be set to the force due to the constraints
717 dReal *cforce = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
718 dSetZero (cforce,nb*8);
719
720 // if there are constraints, compute cforce
721 if (m > 0) {
722 // create a constraint equation right hand side vector `c', a constraint
723 // force mixing vector `cfm', and LCP low and high bound vectors, and an
724 // 'findex' vector.
725 dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
726 dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
727 dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
728 dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
729 int *findex = (int*) alloca (m*sizeof(int));
730 dSetZero (c,m);
731 dSetValue (cfm,m,world->global_cfm);
732 dSetValue (lo,m,-dInfinity);
733 dSetValue (hi,m, dInfinity);
734 for (i=0; i<m; i++) findex[i] = -1;
735
736 // get jacobian data from constraints. a (2*m)x8 matrix will be created
737 // to store the two jacobian blocks from each constraint. it has this
738 // format:
739 //
740 // l l l 0 a a a 0 \ .
741 // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
742 // l l l 0 a a a 0 /
743 // l l l 0 a a a 0 \ .
744 // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
745 // l l l 0 a a a 0 /
746 // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
747 // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
748 // etc...
749 //
750 // (lll) = linear jacobian data
751 // (aaa) = angular jacobian data
752 //
753# ifdef TIMING
754 dTimerNow ("create J");
755# endif
756 dReal *J = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
757 dSetZero (J,2*m*8);
758 dxJoint::Info2 Jinfo;
759 Jinfo.rowskip = 8;
760 Jinfo.fps = stepsize1;
761 Jinfo.erp = world->global_erp;
762 for (i=0; i<nj; i++) {
763 Jinfo.J1l = J + 2*8*ofs[i];
764 Jinfo.J1a = Jinfo.J1l + 4;
765 Jinfo.J2l = Jinfo.J1l + 8*info[i].m;
766 Jinfo.J2a = Jinfo.J2l + 4;
767 Jinfo.c = c + ofs[i];
768 Jinfo.cfm = cfm + ofs[i];
769 Jinfo.lo = lo + ofs[i];
770 Jinfo.hi = hi + ofs[i];
771 Jinfo.findex = findex + ofs[i];
772 joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
773 // adjust returned findex values for global index numbering
774 for (j=0; j<info[i].m; j++) {
775 if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
776 }
777 }
778
779 // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
780 // format as J so we just go through the constraints in J multiplying by
781 // the appropriate scalars and matrices.
782# ifdef TIMING
783 dTimerNow ("compute A");
784# endif
785 dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
786 dSetZero (JinvM,2*m*8);
787 for (i=0; i<nj; i++) {
788 int b = joint[i]->node[0].body->tag;
789 dReal body_invMass = body[b]->invMass;
790 dReal *body_invI = invI + b*12;
791 dReal *Jsrc = J + 2*8*ofs[i];
792 dReal *Jdst = JinvM + 2*8*ofs[i];
793 for (j=info[i].m-1; j>=0; j--) {
794 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
795 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
796 Jsrc += 8;
797 Jdst += 8;
798 }
799 if (joint[i]->node[1].body) {
800 b = joint[i]->node[1].body->tag;
801 body_invMass = body[b]->invMass;
802 body_invI = invI + b*12;
803 for (j=info[i].m-1; j>=0; j--) {
804 for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
805 dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
806 Jsrc += 8;
807 Jdst += 8;
808 }
809 }
810 }
811
812 // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
813 // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
814 // if joints i and j have at least one body in common. this fact suggests
815 // the algorithm used to fill A:
816 //
817 // for b = all bodies
818 // n = number of joints attached to body b
819 // for i = 1..n
820 // for j = i+1..n
821 // ii = actual joint number for i
822 // jj = actual joint number for j
823 // // (ii,jj) will be set to all pairs of joints around body b
824 // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)'
825 //
826 // this algorithm catches all pairs of joints that have at least one body
827 // in common. it does not compute the diagonal blocks of A however -
828 // another similar algorithm does that.
829
830 int mskip = dPAD(m);
831 dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
832 dSetZero (A,m*mskip);
833 for (i=0; i<nb; i++) {
834 for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) {
835 for (dxJointNode *n2=n1->next; n2; n2=n2->next) {
836 // get joint numbers and ensure ofs[j1] >= ofs[j2]
837 int j1 = n1->joint->tag;
838 int j2 = n2->joint->tag;
839 if (ofs[j1] < ofs[j2]) {
840 int tmp = j1;
841 j1 = j2;
842 j2 = tmp;
843 }
844
845 // if either joint was tagged as -1 then it is an inactive (m=0)
846 // joint that should not be considered
847 if (j1==-1 || j2==-1) continue;
848
849 // determine if body i is the 1st or 2nd body of joints j1 and j2
850 int jb1 = (joint[j1]->node[1].body == body[i]);
851 int jb2 = (joint[j2]->node[1].body == body[i]);
852 // jb1/jb2 must be 0 for joints with only one body
853 dIASSERT(joint[j1]->node[1].body || jb1==0);
854 dIASSERT(joint[j2]->node[1].body || jb2==0);
855
856 // set block of A
857 MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
858 JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
859 J + 2*8*ofs[j2] + jb2*8*info[j2].m,
860 info[j1].m,info[j2].m, mskip);
861 }
862 }
863 }
864 // compute diagonal blocks of A
865 for (i=0; i<nj; i++) {
866 Multiply2_p8r (A + ofs[i]*(mskip+1),
867 JinvM + 2*8*ofs[i],
868 J + 2*8*ofs[i],
869 info[i].m,info[i].m, mskip);
870 if (joint[i]->node[1].body) {
871 MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
872 JinvM + 2*8*ofs[i] + 8*info[i].m,
873 J + 2*8*ofs[i] + 8*info[i].m,
874 info[i].m,info[i].m, mskip);
875 }
876 }
877
878 // add cfm to the diagonal of A
879 for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1;
880
881# ifdef COMPARE_METHODS
882 comparator.nextMatrix (A,m,m,1,"A");
883# endif
884
885 // compute the right hand side `rhs'
886# ifdef TIMING
887 dTimerNow ("compute rhs");
888# endif
889 dReal *tmp1 = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
890 //dSetZero (tmp1,nb*8);
891 // put v/h + invM*fe into tmp1
892 for (i=0; i<nb; i++) {
893 dReal body_invMass = body[i]->invMass;
894 dReal *body_invI = invI + i*12;
895 for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass +
896 body[i]->lvel[j] * stepsize1;
897 dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
898 for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1;
899 }
900 // put J*tmp1 into rhs
901 dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
902 //dSetZero (rhs,m);
903 for (i=0; i<nj; i++) {
904 dReal *JJ = J + 2*8*ofs[i];
905 Multiply0_p81 (rhs+ofs[i],JJ,
906 tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
907 if (joint[i]->node[1].body) {
908 MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
909 tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
910 }
911 }
912 // complete rhs
913 for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
914
915# ifdef COMPARE_METHODS
916 comparator.nextMatrix (c,m,1,0,"c");
917 comparator.nextMatrix (rhs,m,1,0,"rhs");
918# endif
919
920 // solve the LCP problem and get lambda.
921 // this will destroy A but that's okay
922# ifdef TIMING
923 dTimerNow ("solving LCP problem");
924# endif
925 dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
926 dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
927 dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
928
929// OLD WAY - direct factor and solve
930//
931// // factorize A (L*L'=A)
932//# ifdef TIMING
933// dTimerNow ("factorize A");
934//# endif
935// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
936// memcpy (L,A,m*mskip*sizeof(dReal));
937//# ifdef FAST_FACTOR
938// dFastFactorCholesky (L,m); // does not report non positive definiteness
939//# else
940// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
941//# endif
942//
943// // compute lambda
944//# ifdef TIMING
945// dTimerNow ("compute lambda");
946//# endif
947// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
948// memcpy (lambda,rhs,m * sizeof(dReal));
949// dSolveCholesky (L,lambda,m);
950
951# ifdef COMPARE_METHODS
952 comparator.nextMatrix (lambda,m,1,0,"lambda");
953# endif
954
955 // compute the constraint force `cforce'
956# ifdef TIMING
957 dTimerNow ("compute constraint force");
958# endif
959 // compute cforce = J'*lambda
960 for (i=0; i<nj; i++) {
961 dReal *JJ = J + 2*8*ofs[i];
962 dxBody* b1 = joint[i]->node[0].body;
963 dxBody* b2 = joint[i]->node[1].body;
964 dJointFeedback *fb = joint[i]->feedback;
965
966/******************** breakable joint contribution ***********************/
967 // this saves us a few dereferences
968 dxJointBreakInfo *jBI = joint[i]->breakInfo;
969 // we need joint feedback if the joint is breakable or if the user
970 // requested feedback.
971 if (jBI||fb) {
972 // we need feedback on the amount of force that this joint is
973 // applying to the bodies. we use a slightly slower computation
974 // that splits out the force components and puts them in the
975 // feedback structure.
976 dJointFeedback temp_fb; // temporary storage for joint feedback
977 dReal data1[8],data2[8];
978 Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
979 dReal *cf1 = cforce + 8*b1->tag;
980 cf1[0] += (temp_fb.f1[0] = data1[0]);
981 cf1[1] += (temp_fb.f1[1] = data1[1]);
982 cf1[2] += (temp_fb.f1[2] = data1[2]);
983 cf1[4] += (temp_fb.t1[0] = data1[4]);
984 cf1[5] += (temp_fb.t1[1] = data1[5]);
985 cf1[6] += (temp_fb.t1[2] = data1[6]);
986 if (b2) {
987 Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
988 dReal *cf2 = cforce + 8*b2->tag;
989 cf2[0] += (temp_fb.f2[0] = data2[0]);
990 cf2[1] += (temp_fb.f2[1] = data2[1]);
991 cf2[2] += (temp_fb.f2[2] = data2[2]);
992 cf2[4] += (temp_fb.t2[0] = data2[4]);
993 cf2[5] += (temp_fb.t2[1] = data2[5]);
994 cf2[6] += (temp_fb.t2[2] = data2[6]);
995 }
996 // if the user requested so we must copy the feedback information to
997 // the feedback struct that the user suplied.
998 if (fb) {
999 // copy temp_fb to fb
1000 fb->f1[0] = temp_fb.f1[0];
1001 fb->f1[1] = temp_fb.f1[1];
1002 fb->f1[2] = temp_fb.f1[2];
1003 fb->t1[0] = temp_fb.t1[0];
1004 fb->t1[1] = temp_fb.t1[1];
1005 fb->t1[2] = temp_fb.t1[2];
1006 if (b2) {
1007 fb->f2[0] = temp_fb.f2[0];
1008 fb->f2[1] = temp_fb.f2[1];
1009 fb->f2[2] = temp_fb.f2[2];
1010 fb->t2[0] = temp_fb.t2[0];
1011 fb->t2[1] = temp_fb.t2[1];
1012 fb->t2[2] = temp_fb.t2[2];
1013 }
1014 }
1015 // if the joint is breakable we need to check the breaking conditions
1016 if (jBI) {
1017 dReal relCF1[3];
1018 dReal relCT1[3];
1019 // multiply the force and torque vectors by the rotation matrix of body 1
1020 dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]);
1021 dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]);
1022 if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
1023 // check if the force is to high
1024 for (int i = 0; i < 3; i++) {
1025 if (relCF1[i] > jBI->b1MaxF[i]) {
1026 jBI->flags |= dJOINT_BROKEN;
1027 goto doneCheckingBreaks;
1028 }
1029 }
1030 }
1031 if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
1032 // check if the torque is to high
1033 for (int i = 0; i < 3; i++) {
1034 if (relCT1[i] > jBI->b1MaxT[i]) {
1035 jBI->flags |= dJOINT_BROKEN;
1036 goto doneCheckingBreaks;
1037 }
1038 }
1039 }
1040 if (b2) {
1041 dReal relCF2[3];
1042 dReal relCT2[3];
1043 // multiply the force and torque vectors by the rotation matrix of body 2
1044 dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]);
1045 dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]);
1046 if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
1047 // check if the force is to high
1048 for (int i = 0; i < 3; i++) {
1049 if (relCF2[i] > jBI->b2MaxF[i]) {
1050 jBI->flags |= dJOINT_BROKEN;
1051 goto doneCheckingBreaks;
1052 }
1053 }
1054 }
1055 if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
1056 // check if the torque is to high
1057 for (int i = 0; i < 3; i++) {
1058 if (relCT2[i] > jBI->b2MaxT[i]) {
1059 jBI->flags |= dJOINT_BROKEN;
1060 goto doneCheckingBreaks;
1061 }
1062 }
1063 }
1064 }
1065 doneCheckingBreaks:
1066 ;
1067 }
1068 }
1069/*************************************************************************/
1070 else {
1071 // no feedback is required, let's compute cforce the faster way
1072 MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
1073 if (b2) {
1074 MultiplyAdd1_8q1 (cforce + 8*b2->tag,
1075 JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
1076 }
1077 }
1078 }
1079 }
1080
1081 // compute the velocity update
1082# ifdef TIMING
1083 dTimerNow ("compute velocity update");
1084# endif
1085
1086 // add fe to cforce
1087 for (i=0; i<nb; i++) {
1088 for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j];
1089 for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j];
1090 }
1091 // multiply cforce by stepsize
1092 for (i=0; i < nb*8; i++) cforce[i] *= stepsize;
1093 // add invM * cforce to the body velocity
1094 for (i=0; i<nb; i++) {
1095 dReal body_invMass = body[i]->invMass;
1096 dReal *body_invI = invI + i*12;
1097 for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j];
1098 dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
1099 }
1100
1101 // update the position and orientation from the new linear/angular velocity
1102 // (over the given timestep)
1103# ifdef TIMING
1104 dTimerNow ("update position");
1105# endif
1106 for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
1107
1108# ifdef COMPARE_METHODS
1109 dReal *tmp_vnew = (dReal*) ALLOCA (nb*6*sizeof(dReal));
1110 for (i=0; i<nb; i++) {
1111 for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j];
1112 for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j];
1113 }
1114 comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
1115# endif
1116
1117# ifdef TIMING
1118 dTimerNow ("tidy up");
1119# endif
1120
1121 // zero all force accumulators
1122 for (i=0; i<nb; i++) {
1123 body[i]->facc[0] = 0;
1124 body[i]->facc[1] = 0;
1125 body[i]->facc[2] = 0;
1126 body[i]->facc[3] = 0;
1127 body[i]->tacc[0] = 0;
1128 body[i]->tacc[1] = 0;
1129 body[i]->tacc[2] = 0;
1130 body[i]->tacc[3] = 0;
1131 }
1132
1133# ifdef TIMING
1134 dTimerEnd();
1135 if (m > 0) dTimerReport (stdout,1);
1136# endif
1137}
1138
1139//****************************************************************************
1140
1141void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
1142 dxJoint * const *joint, int nj, dReal stepsize)
1143{
1144# ifndef COMPARE_METHODS
1145 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1146# endif
1147
1148# ifdef COMPARE_METHODS
1149 int i;
1150
1151 // save body state
1152 dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody));
1153 for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
1154
1155 // take slow step
1156 comparator.reset();
1157 dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
1158 comparator.end();
1159
1160 // restore state
1161 for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
1162
1163 // take fast step
1164 dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
1165 comparator.end();
1166
1167 //comparator.dump();
1168 //_exit (1);
1169# endif
1170}
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp b/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp
deleted file mode 100755
index a0eb9ae..0000000
--- a/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp
+++ /dev/null
@@ -1,1212 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * Fast iterative solver, David Whittaker. Email: david@csworkbench.com *
7 * *
8 * This library is free software; you can redistribute it and/or *
9 * modify it under the terms of EITHER: *
10 * (1) The GNU Lesser General Public License as published by the Free *
11 * Software Foundation; either version 2.1 of the License, or (at *
12 * your option) any later version. The text of the GNU Lesser *
13 * General Public License is included with this library in the *
14 * file LICENSE.TXT. *
15 * (2) The BSD-style license that is included with this library in *
16 * the file LICENSE-BSD.TXT. *
17 * *
18 * This library is distributed in the hope that it will be useful, *
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
21 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
22 * *
23 *************************************************************************/
24
25// This is the StepFast code by David Whittaker. This code is faster, but
26// sometimes less stable than, the original "big matrix" code.
27// Refer to the user's manual for more information.
28// Note that this source file duplicates a lot of stuff from step.cpp,
29// eventually we should move the common code to a third file.
30
31#include "objects.h"
32#include "joint.h"
33#include <ode/config.h>
34#include <ode/objects.h>
35#include <ode/odemath.h>
36#include <ode/rotation.h>
37#include <ode/timer.h>
38#include <ode/error.h>
39#include <ode/matrix.h>
40#include "lcp.h"
41#include "step.h"
42
43
44// misc defines
45
46#define ALLOCA dALLOCA16
47
48#define RANDOM_JOINT_ORDER
49//#define FAST_FACTOR //use a factorization approximation to the LCP solver (fast, theoretically less accurate)
50#define SLOW_LCP //use the old LCP solver
51//#define NO_ISLANDS //does not perform island creation code (3~4% of simulation time), body disabling doesn't work
52//#define TIMING
53
54
55static int autoEnableDepth = 2;
56
57void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth)
58{
59 if (autodepth > 0)
60 autoEnableDepth = autodepth;
61 else
62 autoEnableDepth = 0;
63}
64
65int dWorldGetAutoEnableDepthSF1 (dxWorld *world)
66{
67 return autoEnableDepth;
68}
69
70//little bit of math.... the _sym_ functions assume the return matrix will be symmetric
71static void
72Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip)
73{
74 int i, j;
75 dReal sum, *aa, *ad, *bb, *cc;
76 dIASSERT (p > 0 && A && B && C);
77 bb = B;
78 for (i = 0; i < p; i++)
79 {
80 //aa is going accross the matrix, ad down
81 aa = ad = A;
82 cc = C;
83 for (j = i; j < p; j++)
84 {
85 sum = bb[0] * cc[0];
86 sum += bb[1] * cc[1];
87 sum += bb[2] * cc[2];
88 sum += bb[4] * cc[4];
89 sum += bb[5] * cc[5];
90 sum += bb[6] * cc[6];
91 *(aa++) = *ad = sum;
92 ad += Askip;
93 cc += 8;
94 }
95 bb += 8;
96 A += Askip + 1;
97 C += 8;
98 }
99}
100
101static void
102MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip)
103{
104 int i, j;
105 dReal sum, *aa, *ad, *bb, *cc;
106 dIASSERT (p > 0 && A && B && C);
107 bb = B;
108 for (i = 0; i < p; i++)
109 {
110 //aa is going accross the matrix, ad down
111 aa = ad = A;
112 cc = C;
113 for (j = i; j < p; j++)
114 {
115 sum = bb[0] * cc[0];
116 sum += bb[1] * cc[1];
117 sum += bb[2] * cc[2];
118 sum += bb[4] * cc[4];
119 sum += bb[5] * cc[5];
120 sum += bb[6] * cc[6];
121 *(aa++) += sum;
122 *ad += sum;
123 ad += Askip;
124 cc += 8;
125 }
126 bb += 8;
127 A += Askip + 1;
128 C += 8;
129 }
130}
131
132
133// this assumes the 4th and 8th rows of B are zero.
134
135static void
136Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p)
137{
138 int i;
139 dIASSERT (p > 0 && A && B && C);
140 dReal sum;
141 for (i = p; i; i--)
142 {
143 sum = B[0] * C[0];
144 sum += B[1] * C[1];
145 sum += B[2] * C[2];
146 sum += B[4] * C[4];
147 sum += B[5] * C[5];
148 sum += B[6] * C[6];
149 *(A++) = sum;
150 B += 8;
151 }
152}
153
154
155// this assumes the 4th and 8th rows of B are zero.
156
157static void
158MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p)
159{
160 int i;
161 dIASSERT (p > 0 && A && B && C);
162 dReal sum;
163 for (i = p; i; i--)
164 {
165 sum = B[0] * C[0];
166 sum += B[1] * C[1];
167 sum += B[2] * C[2];
168 sum += B[4] * C[4];
169 sum += B[5] * C[5];
170 sum += B[6] * C[6];
171 *(A++) += sum;
172 B += 8;
173 }
174}
175
176
177// this assumes the 4th and 8th rows of B are zero.
178
179static void
180Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q)
181{
182 int k;
183 dReal sum;
184 dIASSERT (q > 0 && A && B && C);
185 sum = 0;
186 for (k = 0; k < q; k++)
187 sum += B[k * 8] * C[k];
188 A[0] = sum;
189 sum = 0;
190 for (k = 0; k < q; k++)
191 sum += B[1 + k * 8] * C[k];
192 A[1] = sum;
193 sum = 0;
194 for (k = 0; k < q; k++)
195 sum += B[2 + k * 8] * C[k];
196 A[2] = sum;
197 sum = 0;
198 for (k = 0; k < q; k++)
199 sum += B[4 + k * 8] * C[k];
200 A[4] = sum;
201 sum = 0;
202 for (k = 0; k < q; k++)
203 sum += B[5 + k * 8] * C[k];
204 A[5] = sum;
205 sum = 0;
206 for (k = 0; k < q; k++)
207 sum += B[6 + k * 8] * C[k];
208 A[6] = sum;
209}
210
211//****************************************************************************
212// body rotation
213
214// return sin(x)/x. this has a singularity at 0 so special handling is needed
215// for small arguments.
216
217static inline dReal
218sinc (dReal x)
219{
220 // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
221 // is actually accurate to one LS bit within this range if double precision
222 // is being used - so don't worry!
223 if (dFabs (x) < 1.0e-4)
224 return REAL (1.0) - x * x * REAL (0.166666666666666666667);
225 else
226 return dSin (x) / x;
227}
228
229
230// given a body b, apply its linear and angular rotation over the time
231// interval h, thereby adjusting its position and orientation.
232
233static inline void
234moveAndRotateBody (dxBody * b, dReal h)
235{
236 int j;
237
238 // handle linear velocity
239 for (j = 0; j < 3; j++)
240 b->pos[j] += h * b->lvel[j];
241
242 if (b->flags & dxBodyFlagFiniteRotation)
243 {
244 dVector3 irv; // infitesimal rotation vector
245 dQuaternion q; // quaternion for finite rotation
246
247 if (b->flags & dxBodyFlagFiniteRotationAxis)
248 {
249 // split the angular velocity vector into a component along the finite
250 // rotation axis, and a component orthogonal to it.
251 dVector3 frv, irv; // finite rotation vector
252 dReal k = dDOT (b->finite_rot_axis, b->avel);
253 frv[0] = b->finite_rot_axis[0] * k;
254 frv[1] = b->finite_rot_axis[1] * k;
255 frv[2] = b->finite_rot_axis[2] * k;
256 irv[0] = b->avel[0] - frv[0];
257 irv[1] = b->avel[1] - frv[1];
258 irv[2] = b->avel[2] - frv[2];
259
260 // make a rotation quaternion q that corresponds to frv * h.
261 // compare this with the full-finite-rotation case below.
262 h *= REAL (0.5);
263 dReal theta = k * h;
264 q[0] = dCos (theta);
265 dReal s = sinc (theta) * h;
266 q[1] = frv[0] * s;
267 q[2] = frv[1] * s;
268 q[3] = frv[2] * s;
269 }
270 else
271 {
272 // make a rotation quaternion q that corresponds to w * h
273 dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]);
274 h *= REAL (0.5);
275 dReal theta = wlen * h;
276 q[0] = dCos (theta);
277 dReal s = sinc (theta) * h;
278 q[1] = b->avel[0] * s;
279 q[2] = b->avel[1] * s;
280 q[3] = b->avel[2] * s;
281 }
282
283 // do the finite rotation
284 dQuaternion q2;
285 dQMultiply0 (q2, q, b->q);
286 for (j = 0; j < 4; j++)
287 b->q[j] = q2[j];
288
289 // do the infitesimal rotation if required
290 if (b->flags & dxBodyFlagFiniteRotationAxis)
291 {
292 dReal dq[4];
293 dWtoDQ (irv, b->q, dq);
294 for (j = 0; j < 4; j++)
295 b->q[j] += h * dq[j];
296 }
297 }
298 else
299 {
300 // the normal way - do an infitesimal rotation
301 dReal dq[4];
302 dWtoDQ (b->avel, b->q, dq);
303 for (j = 0; j < 4; j++)
304 b->q[j] += h * dq[j];
305 }
306
307 // normalize the quaternion and convert it to a rotation matrix
308 dNormalize4 (b->q);
309 dQtoR (b->q, b->R);
310
311 // notify all attached geoms that this body has moved
312 for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
313 dGeomMoved (geom);
314}
315
316//****************************************************************************
317//This is an implementation of the iterated/relaxation algorithm.
318//Here is a quick overview of the algorithm per Sergi Valverde's posts to the
319//mailing list:
320//
321// for i=0..N-1 do
322// for c = 0..C-1 do
323// Solve constraint c-th
324// Apply forces to constraint bodies
325// next
326// next
327// Integrate bodies
328
329void
330dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize)
331{
332 int i, j, k;
333# ifdef TIMING
334 dTimerNow ("constraint preprocessing");
335# endif
336
337 dReal stepsize1 = dRecip (stepsize);
338
339 int m = info.m;
340 // nothing to do if no constraints.
341 if (m <= 0)
342 return;
343
344 int nub = 0;
345 if (info.nub == info.m)
346 nub = m;
347
348 // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
349 // format as J so we just go through the constraints in J multiplying by
350 // the appropriate scalars and matrices.
351# ifdef TIMING
352 dTimerNow ("compute A");
353# endif
354 dReal JinvM[2 * 6 * 8];
355 //dSetZero (JinvM, 2 * m * 8);
356
357 dReal *Jsrc = Jinfo.J1l;
358 dReal *Jdst = JinvM;
359 if (body[0])
360 {
361 for (j = m - 1; j >= 0; j--)
362 {
363 for (k = 0; k < 3; k++)
364 Jdst[k] = Jsrc[k] * body[0]->invMass;
365 dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]);
366 Jsrc += 8;
367 Jdst += 8;
368 }
369 }
370 if (body[1])
371 {
372 Jsrc = Jinfo.J2l;
373 Jdst = JinvM + 8 * m;
374 for (j = m - 1; j >= 0; j--)
375 {
376 for (k = 0; k < 3; k++)
377 Jdst[k] = Jsrc[k] * body[1]->invMass;
378 dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]);
379 Jsrc += 8;
380 Jdst += 8;
381 }
382 }
383
384
385 // now compute A = JinvM * J'.
386 int mskip = dPAD (m);
387 dReal A[6 * 8];
388 //dSetZero (A, 6 * 8);
389
390 if (body[0])
391 Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip);
392 if (body[1])
393 MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, m, mskip);
394
395 // add cfm to the diagonal of A
396 for (i = 0; i < m; i++)
397 A[i * mskip + i] += Jinfo.cfm[i] * stepsize1;
398
399 // compute the right hand side `rhs'
400# ifdef TIMING
401 dTimerNow ("compute rhs");
402# endif
403 dReal tmp1[16];
404 //dSetZero (tmp1, 16);
405 // put v/h + invM*fe into tmp1
406 for (i = 0; i < 2; i++)
407 {
408 if (!body[i])
409 continue;
410 for (j = 0; j < 3; j++)
411 tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1;
412 dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc);
413 for (j = 0; j < 3; j++)
414 tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1;
415 }
416 // put J*tmp1 into rhs
417 dReal rhs[6];
418 //dSetZero (rhs, 6);
419
420 if (body[0])
421 Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m);
422 if (body[1])
423 MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
424
425 // complete rhs
426 for (i = 0; i < m; i++)
427 rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i];
428
429#ifdef SLOW_LCP
430 // solve the LCP problem and get lambda.
431 // this will destroy A but that's okay
432# ifdef TIMING
433 dTimerNow ("solving LCP problem");
434# endif
435 dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal));
436 dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal));
437 dReal lo[6], hi[6];
438 memcpy (lo, Jinfo.lo, m * sizeof (dReal));
439 memcpy (hi, Jinfo.hi, m * sizeof (dReal));
440 dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex);
441#endif
442
443 // LCP Solver replacement:
444 // This algorithm goes like this:
445 // Do a straightforward LDLT factorization of the matrix A, solving for
446 // A*x = rhs
447 // For each x[i] that is outside of the bounds of lo[i] and hi[i],
448 // clamp x[i] into that range.
449 // Substitute into A the now known x's
450 // subtract the residual away from the rhs.
451 // Remove row and column i from L, updating the factorization
452 // place the known x's at the end of the array, keeping up with location in p
453 // Repeat until all constraints have been clamped or all are within bounds
454 //
455 // This is probably only faster in the single joint case where only one repeat is
456 // the norm.
457
458#ifdef FAST_FACTOR
459 // factorize A (L*D*L'=A)
460# ifdef TIMING
461 dTimerNow ("factorize A");
462# endif
463 dReal d[6];
464 dReal L[6 * 8];
465 memcpy (L, A, m * mskip * sizeof (dReal));
466 dFactorLDLT (L, d, m, mskip);
467
468 // compute lambda
469# ifdef TIMING
470 dTimerNow ("compute lambda");
471# endif
472
473 int left = m; //constraints left to solve.
474 int remove[6];
475 dReal lambda[6];
476 dReal x[6];
477 int p[6];
478 for (i = 0; i < 6; i++)
479 p[i] = i;
480 while (true)
481 {
482 memcpy (x, rhs, left * sizeof (dReal));
483 dSolveLDLT (L, d, x, left, mskip);
484
485 int fixed = 0;
486 for (i = 0; i < left; i++)
487 {
488 j = p[i];
489 remove[i] = false;
490 // This isn't the exact same use of findex as dSolveLCP.... since x[findex]
491 // may change after I've already clamped x[i], but it should be close
492 if (Jinfo.findex[j] > -1)
493 {
494 dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]);
495 if (x[i] > f)
496 x[i] = f;
497 else if (x[i] < -f)
498 x[i] = -f;
499 else
500 continue;
501 }
502 else
503 {
504 if (x[i] > Jinfo.hi[j])
505 x[i] = Jinfo.hi[j];
506 else if (x[i] < Jinfo.lo[j])
507 x[i] = Jinfo.lo[j];
508 else
509 continue;
510 }
511 remove[i] = true;
512 fixed++;
513 }
514 if (fixed == 0 || fixed == left) //no change or all constraints solved
515 break;
516
517 for (i = 0; i < left; i++) //sub in to right hand side.
518 if (remove[i])
519 for (j = 0; j < left; j++)
520 if (!remove[j])
521 rhs[j] -= A[j * mskip + i] * x[i];
522
523 for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables
524 {
525 if (remove[r])
526 {
527 //dRemoveLDLT adapted for use without row pointers.
528 if (r == left - 1)
529 {
530 left--;
531 continue; // deleting last row/col is easy
532 }
533 else if (r == 0)
534 {
535 dReal a[6];
536 for (i = 0; i < left; i++)
537 a[i] = -A[i * mskip];
538 a[0] += REAL (1.0);
539 dLDLTAddTL (L, d, a, left, mskip);
540 }
541 else
542 {
543 dReal t[6];
544 dReal a[6];
545 for (i = 0; i < r; i++)
546 t[i] = L[r * mskip + i] / d[i];
547 for (i = 0; i < left - r; i++)
548 a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r];
549 a[0] += REAL (1.0);
550 dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip);
551 }
552
553 dRemoveRowCol (L, left, mskip, r);
554 //end dRemoveLDLT
555
556 left--;
557 if (r < (left - 1))
558 {
559 dReal tx = x[r];
560 memmove (d + r, d + r + 1, (left - r) * sizeof (dReal));
561 memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal));
562 //x will get written over by rhs anyway, no need to move it around
563 //just store the fixed value we just discovered in it.
564 x[left] = tx;
565 for (i = 0; i < m; i++)
566 if (p[i] > r && p[i] <= left)
567 p[i]--;
568 p[r] = left;
569 }
570 }
571 }
572 }
573
574 for (i = 0; i < m; i++)
575 lambda[i] = x[p[i]];
576# endif
577 // compute the constraint force `cforce'
578# ifdef TIMING
579 dTimerNow ("compute constraint force");
580#endif
581
582 // compute cforce = J'*lambda
583 dJointFeedback *fb = joint->feedback;
584 dReal cforce[16];
585 //dSetZero (cforce, 16);
586
587/******************** breakable joint contribution ***********************/
588 // this saves us a few dereferences
589 dxJointBreakInfo *jBI = joint->breakInfo;
590 // we need joint feedback if the joint is breakable or if the user
591 // requested feedback.
592 if (jBI||fb) {
593 // we need feedback on the amount of force that this joint is
594 // applying to the bodies. we use a slightly slower computation
595 // that splits out the force components and puts them in the
596 // feedback structure.
597 dJointFeedback temp_fb; // temporary storage for joint feedback
598 dReal data1[8],data2[8];
599 if (body[0])
600 {
601 Multiply1_8q1 (data1, Jinfo.J1l, lambda, m);
602 dReal *cf1 = cforce;
603 cf1[0] = (temp_fb.f1[0] = data1[0]);
604 cf1[1] = (temp_fb.f1[1] = data1[1]);
605 cf1[2] = (temp_fb.f1[2] = data1[2]);
606 cf1[4] = (temp_fb.t1[0] = data1[4]);
607 cf1[5] = (temp_fb.t1[1] = data1[5]);
608 cf1[6] = (temp_fb.t1[2] = data1[6]);
609 }
610 if (body[1])
611 {
612 Multiply1_8q1 (data2, Jinfo.J2l, lambda, m);
613 dReal *cf2 = cforce + 8;
614 cf2[0] = (temp_fb.f2[0] = data2[0]);
615 cf2[1] = (temp_fb.f2[1] = data2[1]);
616 cf2[2] = (temp_fb.f2[2] = data2[2]);
617 cf2[4] = (temp_fb.t2[0] = data2[4]);
618 cf2[5] = (temp_fb.t2[1] = data2[5]);
619 cf2[6] = (temp_fb.t2[2] = data2[6]);
620 }
621 // if the user requested so we must copy the feedback information to
622 // the feedback struct that the user suplied.
623 if (fb) {
624 // copy temp_fb to fb
625 fb->f1[0] = temp_fb.f1[0];
626 fb->f1[1] = temp_fb.f1[1];
627 fb->f1[2] = temp_fb.f1[2];
628 fb->t1[0] = temp_fb.t1[0];
629 fb->t1[1] = temp_fb.t1[1];
630 fb->t1[2] = temp_fb.t1[2];
631 if (body[1]) {
632 fb->f2[0] = temp_fb.f2[0];
633 fb->f2[1] = temp_fb.f2[1];
634 fb->f2[2] = temp_fb.f2[2];
635 fb->t2[0] = temp_fb.t2[0];
636 fb->t2[1] = temp_fb.t2[1];
637 fb->t2[2] = temp_fb.t2[2];
638 }
639 }
640 // if the joint is breakable we need to check the breaking conditions
641 if (jBI) {
642 dReal relCF1[3];
643 dReal relCT1[3];
644 // multiply the force and torque vectors by the rotation matrix of body 1
645 dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]);
646 dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]);
647 if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
648 // check if the force is to high
649 for (int i = 0; i < 3; i++) {
650 if (relCF1[i] > jBI->b1MaxF[i]) {
651 jBI->flags |= dJOINT_BROKEN;
652 goto doneCheckingBreaks;
653 }
654 }
655 }
656 if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
657 // check if the torque is to high
658 for (int i = 0; i < 3; i++) {
659 if (relCT1[i] > jBI->b1MaxT[i]) {
660 jBI->flags |= dJOINT_BROKEN;
661 goto doneCheckingBreaks;
662 }
663 }
664 }
665 if (body[1]) {
666 dReal relCF2[3];
667 dReal relCT2[3];
668 // multiply the force and torque vectors by the rotation matrix of body 2
669 dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]);
670 dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]);
671 if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
672 // check if the force is to high
673 for (int i = 0; i < 3; i++) {
674 if (relCF2[i] > jBI->b2MaxF[i]) {
675 jBI->flags |= dJOINT_BROKEN;
676 goto doneCheckingBreaks;
677 }
678 }
679 }
680 if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
681 // check if the torque is to high
682 for (int i = 0; i < 3; i++) {
683 if (relCT2[i] > jBI->b2MaxT[i]) {
684 jBI->flags |= dJOINT_BROKEN;
685 goto doneCheckingBreaks;
686 }
687 }
688 }
689 }
690 doneCheckingBreaks:
691 ;
692 }
693 }
694/*************************************************************************/
695 else
696 {
697 // no feedback is required, let's compute cforce the faster way
698 if (body[0])
699 Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m);
700 if (body[1])
701 Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m);
702 }
703
704 for (i = 0; i < 2; i++)
705 {
706 if (!body[i])
707 continue;
708 for (j = 0; j < 3; j++)
709 {
710 body[i]->facc[j] += cforce[i * 8 + j];
711 body[i]->tacc[j] += cforce[i * 8 + 4 + j];
712 }
713 }
714}
715
716void
717dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations)
718{
719# ifdef TIMING
720 dTimerNow ("preprocessing");
721# endif
722 dxBody *bodyPair[2], *body;
723 dReal *GIPair[2], *GinvIPair[2];
724 dxJoint *joint;
725 int iter, b, j, i;
726 dReal ministep = stepsize / maxiterations;
727
728 // make a local copy of the joint array, because we might want to modify it.
729 // (the "dxJoint *const*" declaration says we're allowed to modify the joints
730 // but not the joint array, because the caller might need it unchanged).
731 dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *));
732 memcpy (joints, _joints, nj * sizeof (dxJoint *));
733
734 // get m = total constraint dimension, nub = number of unbounded variables.
735 // create constraint offset array and number-of-rows array for all joints.
736 // the constraints are re-ordered as follows: the purely unbounded
737 // constraints, the mixed unbounded + LCP constraints, and last the purely
738 // LCP constraints. this assists the LCP solver to put all unbounded
739 // variables at the start for a quick factorization.
740 //
741 // joints with m=0 are inactive and are removed from the joints array
742 // entirely, so that the code that follows does not consider them.
743 // also number all active joints in the joint list (set their tag values).
744 // inactive joints receive a tag value of -1.
745
746 int m = 0;
747 dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1));
748 int *ofs = (int *) ALLOCA (nj * sizeof (int));
749 for (i = 0, j = 0; j < nj; j++)
750 { // i=dest, j=src
751 joints[j]->vtable->getInfo1 (joints[j], info + i);
752 dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
753 if (info[i].m > 0)
754 {
755 joints[i] = joints[j];
756 joints[i]->tag = i;
757 i++;
758 }
759 else
760 {
761 joints[j]->tag = -1;
762 }
763 }
764 nj = i;
765
766 // the purely unbounded constraints
767 for (i = 0; i < nj; i++)
768 {
769 ofs[i] = m;
770 m += info[i].m;
771 }
772 dReal *c = NULL;
773 dReal *cfm = NULL;
774 dReal *lo = NULL;
775 dReal *hi = NULL;
776 int *findex = NULL;
777
778 dReal *J = NULL;
779 dxJoint::Info2 * Jinfo = NULL;
780
781 if (m)
782 {
783 // create a constraint equation right hand side vector `c', a constraint
784 // force mixing vector `cfm', and LCP low and high bound vectors, and an
785 // 'findex' vector.
786 c = (dReal *) ALLOCA (m * sizeof (dReal));
787 cfm = (dReal *) ALLOCA (m * sizeof (dReal));
788 lo = (dReal *) ALLOCA (m * sizeof (dReal));
789 hi = (dReal *) ALLOCA (m * sizeof (dReal));
790 findex = (int *) ALLOCA (m * sizeof (int));
791 dSetZero (c, m);
792 dSetValue (cfm, m, world->global_cfm);
793 dSetValue (lo, m, -dInfinity);
794 dSetValue (hi, m, dInfinity);
795 for (i = 0; i < m; i++)
796 findex[i] = -1;
797
798 // get jacobian data from constraints. a (2*m)x8 matrix will be created
799 // to store the two jacobian blocks from each constraint. it has this
800 // format:
801 //
802 // l l l 0 a a a 0 \ .
803 // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
804 // l l l 0 a a a 0 /
805 // l l l 0 a a a 0 \ .
806 // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
807 // l l l 0 a a a 0 /
808 // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
809 // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
810 // etc...
811 //
812 // (lll) = linear jacobian data
813 // (aaa) = angular jacobian data
814 //
815# ifdef TIMING
816 dTimerNow ("create J");
817# endif
818 J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal));
819 dSetZero (J, 2 * m * 8);
820 Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2));
821 for (i = 0; i < nj; i++)
822 {
823 Jinfo[i].rowskip = 8;
824 Jinfo[i].fps = dRecip (stepsize);
825 Jinfo[i].erp = world->global_erp;
826 Jinfo[i].J1l = J + 2 * 8 * ofs[i];
827 Jinfo[i].J1a = Jinfo[i].J1l + 4;
828 Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m;
829 Jinfo[i].J2a = Jinfo[i].J2l + 4;
830 Jinfo[i].c = c + ofs[i];
831 Jinfo[i].cfm = cfm + ofs[i];
832 Jinfo[i].lo = lo + ofs[i];
833 Jinfo[i].hi = hi + ofs[i];
834 Jinfo[i].findex = findex + ofs[i];
835 //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i);
836 }
837
838 }
839
840 dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
841 dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
842 dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
843 dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
844 for (b = 0; b < nb; b++)
845 {
846 for (i = 0; i < 4; i++)
847 {
848 saveFacc[b * 4 + i] = bodies[b]->facc[i];
849 saveTacc[b * 4 + i] = bodies[b]->tacc[i];
850 bodies[b]->tag = b;
851 }
852 }
853
854 for (iter = 0; iter < maxiterations; iter++)
855 {
856# ifdef TIMING
857 dTimerNow ("applying inertia and gravity");
858# endif
859 dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
860
861 for (b = 0; b < nb; b++)
862 {
863 body = bodies[b];
864
865 // for all bodies, compute the inertia tensor and its inverse in the global
866 // frame, and compute the rotational force and add it to the torque
867 // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
868 // @@@ check computation of rotational force.
869
870 // compute inertia tensor in global frame
871 dMULTIPLY2_333 (tmp, body->mass.I, body->R);
872 dMULTIPLY0_333 (globalI + b * 12, body->R, tmp);
873 // compute inverse inertia tensor in global frame
874 dMULTIPLY2_333 (tmp, body->invI, body->R);
875 dMULTIPLY0_333 (globalInvI + b * 12, body->R, tmp);
876
877 for (i = 0; i < 4; i++)
878 body->tacc[i] = saveTacc[b * 4 + i];
879 // compute rotational force
880 dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel);
881 dCROSS (body->tacc, -=, body->avel, tmp);
882
883 // add the gravity force to all bodies
884 if ((body->flags & dxBodyNoGravity) == 0)
885 {
886 body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0];
887 body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1];
888 body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2];
889 body->facc[3] = 0;
890 }
891
892 }
893
894#ifdef RANDOM_JOINT_ORDER
895#ifdef TIMING
896 dTimerNow ("randomizing joint order");
897#endif
898 //randomize the order of the joints by looping through the array
899 //and swapping the current joint pointer with a random one before it.
900 for (j = 0; j < nj; j++)
901 {
902 joint = joints[j];
903 dxJoint::Info1 i1 = info[j];
904 dxJoint::Info2 i2 = Jinfo[j];
905 int r = rand () % (j + 1);
906 joints[j] = joints[r];
907 info[j] = info[r];
908 Jinfo[j] = Jinfo[r];
909 joints[r] = joint;
910 info[r] = i1;
911 Jinfo[r] = i2;
912 }
913#endif
914
915 //now iterate through the random ordered joint array we created.
916 for (j = 0; j < nj; j++)
917 {
918#ifdef TIMING
919 dTimerNow ("setting up joint");
920#endif
921 joint = joints[j];
922 bodyPair[0] = joint->node[0].body;
923 bodyPair[1] = joint->node[1].body;
924
925 if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled))
926 bodyPair[0] = 0;
927 if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled))
928 bodyPair[1] = 0;
929
930 //if this joint is not connected to any enabled bodies, skip it.
931 if (!bodyPair[0] && !bodyPair[1])
932 continue;
933
934 if (bodyPair[0])
935 {
936 GIPair[0] = globalI + bodyPair[0]->tag * 12;
937 GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12;
938 }
939 if (bodyPair[1])
940 {
941 GIPair[1] = globalI + bodyPair[1]->tag * 12;
942 GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12;
943 }
944
945 joints[j]->vtable->getInfo2 (joints[j], Jinfo + j);
946
947 //dInternalStepIslandFast is an exact copy of the old routine with one
948 //modification: the calculated forces are added back to the facc and tacc
949 //vectors instead of applying them to the bodies and moving them.
950 if (info[j].m > 0)
951 {
952 dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep);
953 }
954 }
955 // }
956# ifdef TIMING
957 dTimerNow ("moving bodies");
958# endif
959 //Now we can simulate all the free floating bodies, and move them.
960 for (b = 0; b < nb; b++)
961 {
962 body = bodies[b];
963
964 for (i = 0; i < 4; i++)
965 {
966 body->facc[i] *= ministep;
967 body->tacc[i] *= ministep;
968 }
969
970 //apply torque
971 dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc);
972
973 //apply force
974 for (i = 0; i < 3; i++)
975 body->lvel[i] += body->invMass * body->facc[i];
976
977 //move It!
978 moveAndRotateBody (body, ministep);
979 }
980 }
981 for (b = 0; b < nb; b++)
982 for (j = 0; j < 4; j++)
983 bodies[b]->facc[j] = bodies[b]->tacc[j] = 0;
984}
985
986
987#ifdef NO_ISLANDS
988
989// Since the iterative algorithm doesn't care about islands of bodies, this is a
990// faster algorithm that just sends it all the joints and bodies in one array.
991// It's downfall is it's inability to handle disabled bodies as well as the old one.
992static void
993processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations)
994{
995 // nothing to do if no bodies
996 if (world->nb <= 0)
997 return;
998
999# ifdef TIMING
1000 dTimerStart ("creating joint and body arrays");
1001# endif
1002 dxBody **bodies, *body;
1003 dxJoint **joints, *joint;
1004 joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *));
1005 bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *));
1006
1007 int nj = 0;
1008 for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next)
1009 joints[nj++] = joint;
1010
1011 int nb = 0;
1012 for (body = world->firstbody; body; body = (dxBody *) body->next)
1013 bodies[nb++] = body;
1014
1015 dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations);
1016# ifdef TIMING
1017 dTimerEnd ();
1018 dTimerReport (stdout, 1);
1019# endif
1020}
1021
1022#else
1023
1024//****************************************************************************
1025// island processing
1026
1027// this groups all joints and bodies in a world into islands. all objects
1028// in an island are reachable by going through connected bodies and joints.
1029// each island can be simulated separately.
1030// note that joints that are not attached to anything will not be included
1031// in any island, an so they do not affect the simulation.
1032//
1033// this function starts new island from unvisited bodies. however, it will
1034// never start a new islands from a disabled body. thus islands of disabled
1035// bodies will not be included in the simulation. disabled bodies are
1036// re-enabled if they are found to be part of an active island.
1037
1038static void
1039processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations)
1040{
1041#ifdef TIMING
1042 dTimerStart ("Island Setup");
1043#endif
1044 dxBody *b, *bb, **body;
1045 dxJoint *j, **joint;
1046
1047 // nothing to do if no bodies
1048 if (world->nb <= 0)
1049 return;
1050
1051 // make arrays for body and joint lists (for a single island) to go into
1052 body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *));
1053 joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *));
1054 int bcount = 0; // number of bodies in `body'
1055 int jcount = 0; // number of joints in `joint'
1056 int tbcount = 0;
1057 int tjcount = 0;
1058
1059 // set all body/joint tags to 0
1060 for (b = world->firstbody; b; b = (dxBody *) b->next)
1061 b->tag = 0;
1062 for (j = world->firstjoint; j; j = (dxJoint *) j->next)
1063 j->tag = 0;
1064
1065 // allocate a stack of unvisited bodies in the island. the maximum size of
1066 // the stack can be the lesser of the number of bodies or joints, because
1067 // new bodies are only ever added to the stack by going through untagged
1068 // joints. all the bodies in the stack must be tagged!
1069 int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
1070 dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *));
1071 int *autostack = (int *) ALLOCA (stackalloc * sizeof (int));
1072
1073 for (bb = world->firstbody; bb; bb = (dxBody *) bb->next)
1074 {
1075#ifdef TIMING
1076 dTimerNow ("Island Processing");
1077#endif
1078 // get bb = the next enabled, untagged body, and tag it
1079 if (bb->tag || (bb->flags & dxBodyDisabled))
1080 continue;
1081 bb->tag = 1;
1082
1083 // tag all bodies and joints starting from bb.
1084 int stacksize = 0;
1085 int autoDepth = autoEnableDepth;
1086 b = bb;
1087 body[0] = bb;
1088 bcount = 1;
1089 jcount = 0;
1090 goto quickstart;
1091 while (stacksize > 0)
1092 {
1093 b = stack[--stacksize]; // pop body off stack
1094 autoDepth = autostack[stacksize];
1095 body[bcount++] = b; // put body on body list
1096 quickstart:
1097
1098 // traverse and tag all body's joints, add untagged connected bodies
1099 // to stack
1100 for (dxJointNode * n = b->firstjoint; n; n = n->next)
1101 {
1102 if (!n->joint->tag)
1103 {
1104 int thisDepth = autoEnableDepth;
1105 n->joint->tag = 1;
1106 joint[jcount++] = n->joint;
1107 if (n->body && !n->body->tag)
1108 {
1109 if (n->body->flags & dxBodyDisabled)
1110 thisDepth = autoDepth - 1;
1111 if (thisDepth < 0)
1112 continue;
1113 n->body->flags &= ~dxBodyDisabled;
1114 n->body->tag = 1;
1115 autostack[stacksize] = thisDepth;
1116 stack[stacksize++] = n->body;
1117 }
1118 }
1119 }
1120 dIASSERT (stacksize <= world->nb);
1121 dIASSERT (stacksize <= world->nj);
1122 }
1123
1124 // now do something with body and joint lists
1125 dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations);
1126
1127 // what we've just done may have altered the body/joint tag values.
1128 // we must make sure that these tags are nonzero.
1129 // also make sure all bodies are in the enabled state.
1130 int i;
1131 for (i = 0; i < bcount; i++)
1132 {
1133 body[i]->tag = 1;
1134 body[i]->flags &= ~dxBodyDisabled;
1135 }
1136 for (i = 0; i < jcount; i++)
1137 joint[i]->tag = 1;
1138
1139 tbcount += bcount;
1140 tjcount += jcount;
1141 }
1142
1143#ifdef TIMING
1144 dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount);
1145#endif
1146
1147 // if debugging, check that all objects (except for disabled bodies,
1148 // unconnected joints, and joints that are connected to disabled bodies)
1149 // were tagged.
1150# ifndef dNODEBUG
1151 for (b = world->firstbody; b; b = (dxBody *) b->next)
1152 {
1153 if (b->flags & dxBodyDisabled)
1154 {
1155 if (b->tag)
1156 dDebug (0, "disabled body tagged");
1157 }
1158 else
1159 {
1160 if (!b->tag)
1161 dDebug (0, "enabled body not tagged");
1162 }
1163 }
1164 for (j = world->firstjoint; j; j = (dxJoint *) j->next)
1165 {
1166 if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0))
1167 {
1168 if (!j->tag)
1169 dDebug (0, "attached enabled joint not tagged");
1170 }
1171 else
1172 {
1173 if (j->tag)
1174 dDebug (0, "unattached or disabled joint tagged");
1175 }
1176 }
1177# endif
1178 /******************** breakable joint contribution ***********************/
1179 dxJoint* nextJ;
1180 if (!world->firstjoint)
1181 nextJ = 0;
1182 else
1183 nextJ = (dxJoint*)world->firstjoint->next;
1184 for (j=world->firstjoint; j; j=nextJ) {
1185 nextJ = (dxJoint*)j->next;
1186 // check if joint is breakable and broken
1187 if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
1188 // detach (break) the joint
1189 dJointAttach (j, 0, 0);
1190 // call the callback function if it is set
1191 if (j->breakInfo->callback) j->breakInfo->callback (j);
1192 // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
1193 if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
1194 }
1195 }
1196 /*************************************************************************/
1197
1198# ifdef TIMING
1199 dTimerEnd ();
1200 dTimerReport (stdout, 1);
1201# endif
1202}
1203
1204#endif
1205
1206
1207void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations)
1208{
1209 dUASSERT (w, "bad world argument");
1210 dUASSERT (stepsize > 0, "stepsize must be > 0");
1211 processIslandsFast (w, stepsize, maxiterations);
1212}
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 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25buggy with suspension.
26this also shows you how to use geom groups.
27
28*/
29
30
31#include <stdlib.h>
32
33#include <ode/ode.h>
34#include <drawstuff/drawstuff.h>
35
36#ifdef _MSC_VER
37#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
38#endif
39
40// select correct drawing functions
41
42#ifdef dDOUBLE
43#define dsDrawBox dsDrawBoxD
44#define dsDrawSphere dsDrawSphereD
45#define dsDrawCylinder dsDrawCylinderD
46#define dsDrawCappedCylinder dsDrawCappedCylinderD
47#endif
48
49
50// some constants
51
52#define LENGTH 0.7 // chassis length
53#define WIDTH 0.4 // chassis width
54#define HEIGHT 0.2 // chassis height
55#define RADIUS 0.22 // wheel radius
56#define STARTZ 0.4 // starting height of chassis
57#define CMASS 1 // chassis mass
58#define WMASS 0.2 // wheel mass
59
60// dynamics and collision objects (chassis, 4 wheels, environment, obstacles, chain)
61static dWorldID world;
62static dSpaceID space;
63
64// chain stuff
65static const float chain_radius = 0.1;
66static const float chain_mass = 0.1;
67static const int chain_num = 10;
68static dBodyID chain_body[chain_num];
69static dGeomID chain_geom[chain_num];
70static dJointID chain_joint[chain_num-1];
71
72// 1 chasses, 4 wheels
73static dBodyID body[5];
74// joint[0] is left front wheel, joint[1] is right front wheel
75static dJointID joint[4];
76static int joint_exists[4];
77static dJointGroupID contactgroup;
78static dGeomID ground;
79static dSpaceID car_space;
80static dGeomID box[1];
81static dGeomID sphere[4];
82static dGeomID ground_box;
83static const int obstacle_num = 25;
84static dGeomID obstacle[obstacle_num];
85
86// things that the user controls
87
88static dReal speed=0,steer=0; // user commands
89
90
91
92// this is called by dSpaceCollide when two objects in space are
93// potentially colliding.
94
95static void nearCallback (void *data, dGeomID o1, dGeomID o2)
96{
97 int i,n;
98
99// // do not collide objects that are connected
100// dBodyID b1 = dGeomGetBody (o1),
101// b2 = dGeomGetBody (o2);
102// if (b1 && b2 && dAreConnected(b1, b2)) return;
103
104 const int N = 10;
105 dContact contact[N];
106 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
107 if (n > 0) {
108 for (i=0; i<n; i++) {
109 contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
110 dContactSoftERP | dContactSoftCFM | dContactApprox1;
111 contact[i].surface.mu = dInfinity;
112 contact[i].surface.slip1 = 0.1;
113 contact[i].surface.slip2 = 0.1;
114 contact[i].surface.soft_erp = 0.5;
115 contact[i].surface.soft_cfm = 0.3;
116 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
117 dJointAttach (c,
118 dGeomGetBody(contact[i].geom.g1),
119 dGeomGetBody(contact[i].geom.g2));
120 }
121 }
122}
123
124// callback function for joints that break
125static void jointBreakCallback (dJointID j)
126{
127 if (j == joint[0]) joint_exists[0] = 0;
128 else if (j == joint[1]) joint_exists[1] = 0;
129 else if (j == joint[2]) joint_exists[2] = 0;
130 else if (j == joint[3]) joint_exists[3] = 0;
131 printf ("A joint just broke\n");
132}
133
134// start simulation - set viewpoint
135
136static void start()
137{
138 static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
139 static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
140 dsSetViewpoint (xyz,hpr);
141 printf ("Press:\t'a' to increase speed.\n"
142 "\t'z' to decrease speed.\n"
143 "\t',' to steer left.\n"
144 "\t'.' to steer right.\n"
145 "\t' ' to reset speed and steering.\n");
146}
147
148
149// called when a key pressed
150
151static void command (int cmd)
152{
153 switch (cmd) {
154 case 'a': case 'A':
155 speed += 0.3;
156 break;
157 case 'z': case 'Z':
158 speed -= 0.3;
159 break;
160 case ',':
161 steer -= 0.5;
162 break;
163 case '.':
164 steer += 0.5;
165 break;
166 case ' ':
167 speed = 0;
168 steer = 0;
169 break;
170 }
171}
172
173
174// simulation loop
175
176static void simLoop (int pause)
177{
178 int i;
179 if (!pause) {
180 for (i=0; i<2; i++) {
181 if (joint_exists[i]) {
182 // motor
183 dJointSetHinge2Param (joint[i],dParamVel2,-speed);
184 dJointSetHinge2Param (joint[i],dParamFMax2,0.1);
185
186 // steering
187 dReal v = steer - dJointGetHinge2Angle1 (joint[i]);
188 if (v > 0.1) v = 0.1;
189 if (v < -0.1) v = -0.1;
190 v *= 10.0;
191 dJointSetHinge2Param (joint[i],dParamVel,v);
192 dJointSetHinge2Param (joint[i],dParamFMax,0.2);
193 dJointSetHinge2Param (joint[i],dParamLoStop,-0.75);
194 dJointSetHinge2Param (joint[i],dParamHiStop,0.75);
195 dJointSetHinge2Param (joint[i],dParamFudgeFactor,0.1);
196 }
197 }
198
199 dSpaceCollide (space,0,&nearCallback);
200 //dWorldStep (world,0.05);
201 dWorldStepFast1 (world,0.05,5);
202
203 // remove all contact joints
204 dJointGroupEmpty (contactgroup);
205 }
206
207 dsSetColor (0,1,1);
208 dsSetTexture (DS_WOOD);
209 dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
210 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
211 dsSetColor (1,1,1);
212 for (i=1; i<=4; i++)
213 dsDrawCylinder (dBodyGetPosition(body[i]),
214 dBodyGetRotation(body[i]),
215 0.2,
216 RADIUS);
217
218 dVector3 ss;
219 dGeomBoxGetLengths (ground_box,ss);
220 dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
221
222 dsSetColor (1,0,0);
223 for (i=0; i<obstacle_num; i++) {
224 dVector3 ss;
225 dGeomBoxGetLengths (obstacle[i],ss);
226 dsDrawBox (dGeomGetPosition(obstacle[i]),dGeomGetRotation(obstacle[i]),ss);
227 }
228
229 dsSetColor (1,1,0);
230 for (i=0; i<chain_num; i++) {
231 dsDrawSphere (dGeomGetPosition(chain_geom[i]),dGeomGetRotation(chain_geom[i]),chain_radius);
232 }
233
234 /*
235 printf ("%.10f %.10f %.10f %.10f\n",
236 dJointGetHingeAngle (joint[1]),
237 dJointGetHingeAngle (joint[2]),
238 dJointGetHingeAngleRate (joint[1]),
239 dJointGetHingeAngleRate (joint[2]));
240 */
241}
242
243int main (int argc, char **argv)
244{
245 int i;
246 dMass m;
247
248 // setup pointers to drawstuff callback functions
249 dsFunctions fn;
250 fn.version = DS_VERSION;
251 fn.start = &start;
252 fn.step = &simLoop;
253 fn.command = &command;
254 fn.stop = 0;
255 fn.path_to_textures = "../../drawstuff/textures";
256 if(argc==2)
257 {
258 fn.path_to_textures = argv[1];
259 }
260 // create world
261
262 world = dWorldCreate();
263 space = dHashSpaceCreate (0);
264 contactgroup = dJointGroupCreate (0);
265 dWorldSetGravity (world,0,0,-0.5);
266 ground = dCreatePlane (space,0,0,1,0);
267
268 // chassis body
269 body[0] = dBodyCreate (world);
270 dBodySetPosition (body[0],0,0,STARTZ);
271 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
272 dMassAdjust (&m,CMASS);
273 dBodySetMass (body[0],&m);
274 box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
275 dGeomSetBody (box[0],body[0]);
276
277 // a chain
278 for (i=0; i<chain_num; i++) {
279 chain_body[i] = dBodyCreate (world);
280 dBodySetPosition (chain_body[i],-LENGTH-(i*2*chain_radius),0,STARTZ-HEIGHT*0.5);
281 dMassSetSphere (&m,1,chain_radius);
282 dMassAdjust (&m,chain_mass);
283 dBodySetMass (chain_body[i],&m);
284 chain_geom[i] = dCreateSphere (space,chain_radius);
285 dGeomSetBody (chain_geom[i],chain_body[i]);
286 }
287
288 // wheel bodies
289 for (i=1; i<=4; i++) {
290 body[i] = dBodyCreate (world);
291 dQuaternion q;
292 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
293 dBodySetQuaternion (body[i],q);
294 dMassSetSphere (&m,1,RADIUS);
295 dMassAdjust (&m,WMASS);
296 dBodySetMass (body[i],&m);
297 sphere[i-1] = dCreateSphere (0,RADIUS);
298 dGeomSetBody (sphere[i-1],body[i]);
299 }
300 dBodySetPosition (body[1], 0.5*LENGTH, WIDTH*1.0, STARTZ-HEIGHT*0.5);
301 dBodySetPosition (body[2], 0.5*LENGTH, -WIDTH*1.0, STARTZ-HEIGHT*0.5);
302 dBodySetPosition (body[3], -0.5*LENGTH, WIDTH*1.0, STARTZ-HEIGHT*0.5);
303 dBodySetPosition (body[4], -0.5*LENGTH, -WIDTH*1.0, STARTZ-HEIGHT*0.5);
304
305 // front wheel hinge
306 /*
307 joint[0] = dJointCreateHinge2 (world,0);
308 dJointAttach (joint[0],body[0],body[1]);
309 const dReal *a = dBodyGetPosition (body[1]);
310 dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
311 dJointSetHinge2Axis1 (joint[0],0,0,1);
312 dJointSetHinge2Axis2 (joint[0],0,1,0);
313 */
314
315 // front and back wheel hinges
316 for (i=0; i<4; i++) {
317 joint[i] = dJointCreateHinge2 (world,0);
318 joint_exists[i] = 1;
319 dJointAttach (joint[i],body[0],body[i+1]);
320 const dReal *a = dBodyGetPosition (body[i+1]);
321 dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
322 dJointSetHinge2Axis1 (joint[i],0,0,1);
323 dJointSetHinge2Axis2 (joint[i],0,1,0);
324
325 // the wheels can break
326 dJointSetBreakable (joint[i], 1);
327 // the wheels wil break at a specific force
328 dJointSetBreakMode (joint[i],
329 dJOINT_BREAK_AT_B1_FORCE |
330 dJOINT_BREAK_AT_B2_FORCE |
331 dJOINT_DELETE_ON_BREAK);
332 // specify the force for the first body connected to the joint ...
333 dJointSetBreakForce (joint[i], 0, 2.5, 2.5, 2.5);
334 // and for the second body
335 dJointSetBreakForce (joint[i], 1, 2.5, 2.5, 2.5);
336 // set the callback function
337 dJointSetBreakCallback (joint[i], &jointBreakCallback);
338 }
339
340 // joints for the chain
341 for (i=0; i<chain_num-1; i++) {
342 chain_joint[i] = dJointCreateFixed (world,0);
343 dJointAttach (chain_joint[i],chain_body[i+1],chain_body[i]);
344 dJointSetFixed (chain_joint[i]);
345 // the chain can break
346 dJointSetBreakable (chain_joint[i], 1);
347 // the chain wil break at a specific force
348 dJointSetBreakMode (chain_joint[i],
349 dJOINT_BREAK_AT_B1_FORCE |
350 dJOINT_BREAK_AT_B2_FORCE |
351 dJOINT_DELETE_ON_BREAK);
352 // specify the force for the first body connected to the joint ...
353 dJointSetBreakForce (chain_joint[i], 0, 0.5, 0.5, 0.5);
354 // and for the second body
355 dJointSetBreakForce (chain_joint[i], 1, 0.5, 0.5, 0.5);
356 // set the callback function
357 dJointSetBreakCallback (chain_joint[i], &jointBreakCallback);
358 }
359
360 // set joint suspension
361 for (i=0; i<4; i++) {
362 dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
363 dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.1);
364 }
365
366 // lock back wheels along the steering axis
367 for (i=1; i<4; i++) {
368 // set stops to make sure wheels always stay in alignment
369 dJointSetHinge2Param (joint[i],dParamLoStop,0);
370 dJointSetHinge2Param (joint[i],dParamHiStop,0);
371 // the following alternative method is no good as the wheels may get out
372 // of alignment:
373 // dJointSetHinge2Param (joint[i],dParamVel,0);
374 // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
375 }
376
377 // create car space and add it to the top level space
378 car_space = dSimpleSpaceCreate (space);
379 dSpaceSetCleanup (car_space,0);
380 dSpaceAdd (car_space,box[0]);
381 dSpaceAdd (car_space,sphere[0]);
382 dSpaceAdd (car_space,sphere[1]);
383 dSpaceAdd (car_space,sphere[2]);
384
385 // environment
386 ground_box = dCreateBox (space,2,1.5,1);
387 dMatrix3 R;
388 dRFromAxisAndAngle (R,0,1,0,-0.15);
389 dGeomSetPosition (ground_box,2,0,-0.34);
390 dGeomSetRotation (ground_box,R);
391
392 // obstacles
393 for (i=0; i<obstacle_num; i++) {
394 dReal height = 0.1+(dReal(rand()%10)/10.0);
395 obstacle[i] = dCreateBox (space,0.2,0.2,height);
396 dGeomSetPosition (
397 obstacle[i],
398 (rand()%20)-10,
399 (rand()%20)-10,
400 height/2.0);
401 }
402
403 // run simulation
404 dsSimulationLoop (argc,argv,352,288,&fn);
405
406 dJointGroupDestroy (contactgroup);
407 dSpaceDestroy (space);
408 dWorldDestroy (world);
409 dGeomDestroy (box[0]);
410 for (i=0; i<4; i++)
411 dGeomDestroy (sphere[i]);
412
413 dCloseODE ();
414
415 return 0;
416}
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp b/libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp
deleted file mode 100644
index 1dc0ab3..0000000
--- a/libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp
+++ /dev/null
@@ -1,327 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23/*
24
25buggy with suspension.
26this also shows you how to use geom groups.
27
28*/
29
30
31#include <ode/ode.h>
32#include <drawstuff/drawstuff.h>
33
34#ifdef _MSC_VER
35#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
36#endif
37
38// select correct drawing functions
39
40#ifdef dDOUBLE
41#define dsDrawBox dsDrawBoxD
42#define dsDrawSphere dsDrawSphereD
43#define dsDrawCylinder dsDrawCylinderD
44#define dsDrawCappedCylinder dsDrawCappedCylinderD
45#endif
46
47
48// some constants
49
50#define LENGTH 0.7 // chassis length
51#define WIDTH 0.5 // chassis width
52#define HEIGHT 0.2 // chassis height
53#define RADIUS 0.18 // wheel radius
54#define STARTZ 0.5 // starting height of chassis
55#define CMASS 1 // chassis mass
56#define WMASS 0.2 // wheel mass
57
58
59// dynamics and collision objects (chassis, 3 wheels, environment)
60
61static dWorldID world;
62static dSpaceID space;
63static dBodyID body[4];
64static dJointID joint[3]; // joint[0] is the front wheel
65static dJointGroupID contactgroup;
66static dGeomID ground;
67static dSpaceID car_space;
68static dGeomID box[1];
69static dGeomID sphere[3];
70static dGeomID ground_box;
71
72
73// things that the user controls
74
75static dReal speed=0,steer=0; // user commands
76
77
78
79// this is called by dSpaceCollide when two objects in space are
80// potentially colliding.
81
82static void nearCallback (void *data, dGeomID o1, dGeomID o2)
83{
84 int i,n;
85
86 // only collide things with the ground
87 int g1 = (o1 == ground || o1 == ground_box);
88 int g2 = (o2 == ground || o2 == ground_box);
89 if (!(g1 ^ g2)) return;
90
91 const int N = 10;
92 dContact contact[N];
93 n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
94 if (n > 0) {
95 for (i=0; i<n; i++) {
96 contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
97 dContactSoftERP | dContactSoftCFM | dContactApprox1;
98 contact[i].surface.mu = dInfinity;
99 contact[i].surface.slip1 = 0.1;
100 contact[i].surface.slip2 = 0.1;
101 contact[i].surface.soft_erp = 0.5;
102 contact[i].surface.soft_cfm = 0.3;
103 dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
104 dJointAttach (c,
105 dGeomGetBody(contact[i].geom.g1),
106 dGeomGetBody(contact[i].geom.g2));
107 }
108 }
109}
110
111
112// start simulation - set viewpoint
113
114static void start()
115{
116 static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
117 static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
118 dsSetViewpoint (xyz,hpr);
119 printf ("Press:\t'a' to increase speed.\n"
120 "\t'z' to decrease speed.\n"
121 "\t',' to steer left.\n"
122 "\t'.' to steer right.\n"
123 "\t' ' to reset speed and steering.\n");
124}
125
126
127// called when a key pressed
128
129static void command (int cmd)
130{
131 switch (cmd) {
132 case 'a': case 'A':
133 speed += 0.3;
134 break;
135 case 'z': case 'Z':
136 speed -= 0.3;
137 break;
138 case ',':
139 steer -= 0.5;
140 break;
141 case '.':
142 steer += 0.5;
143 break;
144 case ' ':
145 speed = 0;
146 steer = 0;
147 break;
148 }
149}
150
151
152// simulation loop
153
154static void simLoop (int pause)
155{
156 int i;
157 if (!pause) {
158 // motor
159 dJointSetHinge2Param (joint[0],dParamVel2,-speed);
160 dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
161
162 // steering
163 dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
164 if (v > 0.1) v = 0.1;
165 if (v < -0.1) v = -0.1;
166 v *= 10.0;
167 dJointSetHinge2Param (joint[0],dParamVel,v);
168 dJointSetHinge2Param (joint[0],dParamFMax,0.2);
169 dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
170 dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
171 dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
172
173 dSpaceCollide (space,0,&nearCallback);
174 dWorldStep (world,0.05);
175
176 // remove all contact joints
177 dJointGroupEmpty (contactgroup);
178 }
179
180 dsSetColor (0,1,1);
181 dsSetTexture (DS_WOOD);
182 dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
183 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
184 dsSetColor (1,1,1);
185 for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
186 dBodyGetRotation(body[i]),0.02f,RADIUS);
187
188 dVector3 ss;
189 dGeomBoxGetLengths (ground_box,ss);
190 dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
191
192 /*
193 printf ("%.10f %.10f %.10f %.10f\n",
194 dJointGetHingeAngle (joint[1]),
195 dJointGetHingeAngle (joint[2]),
196 dJointGetHingeAngleRate (joint[1]),
197 dJointGetHingeAngleRate (joint[2]));
198 */
199}
200
201
202int main (int argc, char **argv)
203{
204 int i;
205 dMass m;
206
207 // setup pointers to drawstuff callback functions
208 dsFunctions fn;
209 fn.version = DS_VERSION;
210 fn.start = &start;
211 fn.step = &simLoop;
212 fn.command = &command;
213 fn.stop = 0;
214 fn.path_to_textures = "../../drawstuff/textures";
215 if(argc==2)
216 {
217 fn.path_to_textures = argv[1];
218 }
219
220 // create world
221
222 world = dWorldCreate();
223 space = dHashSpaceCreate (0);
224 contactgroup = dJointGroupCreate (0);
225 dWorldSetGravity (world,0,0,-0.5);
226 ground = dCreatePlane (space,0,0,1,0);
227
228 // chassis body
229 body[0] = dBodyCreate (world);
230 dBodySetPosition (body[0],0,0,STARTZ);
231 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
232 dMassAdjust (&m,CMASS);
233 dBodySetMass (body[0],&m);
234 box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
235 dGeomSetBody (box[0],body[0]);
236
237 // wheel bodies
238 for (i=1; i<=3; i++) {
239 body[i] = dBodyCreate (world);
240 dQuaternion q;
241 dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
242 dBodySetQuaternion (body[i],q);
243 dMassSetSphere (&m,1,RADIUS);
244 dMassAdjust (&m,WMASS);
245 dBodySetMass (body[i],&m);
246 sphere[i-1] = dCreateSphere (0,RADIUS);
247 dGeomSetBody (sphere[i-1],body[i]);
248 }
249 dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
250 dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
251 dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
252
253 // front wheel hinge
254 /*
255 joint[0] = dJointCreateHinge2 (world,0);
256 dJointAttach (joint[0],body[0],body[1]);
257 const dReal *a = dBodyGetPosition (body[1]);
258 dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
259 dJointSetHinge2Axis1 (joint[0],0,0,1);
260 dJointSetHinge2Axis2 (joint[0],0,1,0);
261 */
262
263 // front and back wheel hinges
264 for (i=0; i<3; i++) {
265 joint[i] = dJointCreateHinge2 (world,0);
266 dJointAttach (joint[i],body[0],body[i+1]);
267 const dReal *a = dBodyGetPosition (body[i+1]);
268 dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
269 dJointSetHinge2Axis1 (joint[i],0,0,1);
270 dJointSetHinge2Axis2 (joint[i],0,1,0);
271
272 // breakable joints contribution
273 // the wheels can break
274 dJointSetBreakable (joint[i], 1);
275 // the wheels wil break at a specific force
276 dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_B1_FORCE|dJOINT_BREAK_AT_B2_FORCE);
277 // specify the force for the first body connected to the joint ...
278 dJointSetBreakForce (joint[i], 0, 1.5, 1.5, 1.5);
279 // and for the second body
280 dJointSetBreakForce (joint[i], 1, 1.5, 1.5, 1.5);
281 }
282
283 // set joint suspension
284 for (i=0; i<3; i++) {
285 dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
286 dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
287 }
288
289 // lock back wheels along the steering axis
290 for (i=1; i<3; i++) {
291 // set stops to make sure wheels always stay in alignment
292 dJointSetHinge2Param (joint[i],dParamLoStop,0);
293 dJointSetHinge2Param (joint[i],dParamHiStop,0);
294 // the following alternative method is no good as the wheels may get out
295 // of alignment:
296 // dJointSetHinge2Param (joint[i],dParamVel,0);
297 // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
298 }
299
300 // create car space and add it to the top level space
301 car_space = dSimpleSpaceCreate (space);
302 dSpaceSetCleanup (car_space,0);
303 dSpaceAdd (car_space,box[0]);
304 dSpaceAdd (car_space,sphere[0]);
305 dSpaceAdd (car_space,sphere[1]);
306 dSpaceAdd (car_space,sphere[2]);
307
308 // environment
309 ground_box = dCreateBox (space,2,1.5,1);
310 dMatrix3 R;
311 dRFromAxisAndAngle (R,0,1,0,-0.15);
312 dGeomSetPosition (ground_box,2,0,-0.34);
313 dGeomSetRotation (ground_box,R);
314
315 // run simulation
316 dsSimulationLoop (argc,argv,352,288,&fn);
317
318 dJointGroupDestroy (contactgroup);
319 dSpaceDestroy (space);
320 dWorldDestroy (world);
321 dGeomDestroy (box[0]);
322 dGeomDestroy (sphere[0]);
323 dGeomDestroy (sphere[1]);
324 dGeomDestroy (sphere[2]);
325
326 return 0;
327}
generated by cgit v1.1 at 2024-11-10 03:02:40 +0000