aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib
diff options
context:
space:
mode:
authordan miller2007-10-21 08:36:32 +0000
committerdan miller2007-10-21 08:36:32 +0000
commit2f8d7092bc2c9609fa98d6888106b96f38b22828 (patch)
treeda6c37579258cc965b52a75aee6135fe44237698 /libraries/ode-0.9/contrib
parent* Committing new PolicyManager based on an ACL system. (diff)
downloadopensim-SC_OLD-2f8d7092bc2c9609fa98d6888106b96f38b22828.zip
opensim-SC_OLD-2f8d7092bc2c9609fa98d6888106b96f38b22828.tar.gz
opensim-SC_OLD-2f8d7092bc2c9609fa98d6888106b96f38b22828.tar.bz2
opensim-SC_OLD-2f8d7092bc2c9609fa98d6888106b96f38b22828.tar.xz
libraries moved to opensim-libs, a new repository
Diffstat (limited to 'libraries/ode-0.9/contrib')
-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
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp58
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Body.cpp322
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Body.h76
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h43
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln21
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj379
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp219
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Geom.h75
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp35
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Joint.h21
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp162
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h62
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp79
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointBall.h38
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp67
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h37
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp53
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h33
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp121
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h195
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp133
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h48
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp102
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h158
-rwxr-xr-xlibraries/ode-0.9/contrib/DotNetManaged/Release/ode.dllbin196608 -> 0 bytes
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Space.cpp53
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Space.h33
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp5
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h12
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/TEST.h17
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/World.cpp74
-rw-r--r--libraries/ode-0.9/contrib/DotNetManaged/World.h67
-rw-r--r--libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp218
-rw-r--r--libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h29
-rw-r--r--libraries/ode-0.9/contrib/GeomTransformGroup/README.txt148
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.binbin46592 -> 0 bytes
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt95
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h6
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h6
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h10
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h13
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h13
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h6
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp281
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h2
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h2
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h52
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp289
-rw-r--r--libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c197
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs18
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs58
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua19
-rwxr-xr-xlibraries/ode-0.9/contrib/Ode.NET/Ode.NET.sln21
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs18
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs1732
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua31
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/README.TXT73
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs260
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua27
-rw-r--r--libraries/ode-0.9/contrib/Ode.NET/premake.lua29
-rw-r--r--libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT37
-rw-r--r--libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT502
-rw-r--r--libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln20
-rw-r--r--libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs354
-rw-r--r--libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj69
-rw-r--r--libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs35
-rwxr-xr-xlibraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs26
-rwxr-xr-xlibraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings6
-rw-r--r--libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT78
-rw-r--r--libraries/ode-0.9/contrib/README19
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h100
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp504
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp662
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp659
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/readme.txt322
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp1375
-rw-r--r--libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp1445
-rw-r--r--libraries/ode-0.9/contrib/dCylinder/dCylinder.h14
-rw-r--r--libraries/ode-0.9/contrib/dCylinder/readme.txt62
-rw-r--r--libraries/ode-0.9/contrib/dRay/Include/dRay.h15
-rw-r--r--libraries/ode-0.9/contrib/dRay/README.txt16
-rw-r--r--libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp1372
-rw-r--r--libraries/ode-0.9/contrib/dRay/dRay.cpp119
-rw-r--r--libraries/ode-0.9/contrib/dRay/dRay_Box.cpp134
-rw-r--r--libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp199
-rw-r--r--libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp35
-rw-r--r--libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp95
-rw-r--r--libraries/ode-0.9/contrib/dRay/dxRay.h32
106 files changed, 0 insertions, 23587 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}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp b/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp
deleted file mode 100644
index e550a32..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
1#include "stdafx.h"
2
3using namespace System::Reflection;
4using namespace System::Runtime::CompilerServices;
5
6//
7// General Information about an assembly is controlled through the following
8// set of attributes. Change these attribute values to modify the information
9// associated with an assembly.
10//
11[assembly:AssemblyTitleAttribute("")];
12[assembly:AssemblyDescriptionAttribute("")];
13[assembly:AssemblyConfigurationAttribute("")];
14[assembly:AssemblyCompanyAttribute("")];
15[assembly:AssemblyProductAttribute("")];
16[assembly:AssemblyCopyrightAttribute("")];
17[assembly:AssemblyTrademarkAttribute("")];
18[assembly:AssemblyCultureAttribute("")];
19
20//
21// Version information for an assembly consists of the following four values:
22//
23// Major Version
24// Minor Version
25// Build Number
26// Revision
27//
28// You can specify all the value or you can default the Revision and Build Numbers
29// by using the '*' as shown below:
30
31[assembly:AssemblyVersionAttribute("1.0.*")];
32
33//
34// In order to sign your assembly you must specify a key to use. Refer to the
35// Microsoft .NET Framework documentation for more information on assembly signing.
36//
37// Use the attributes below to control which key is used for signing.
38//
39// Notes:
40// (*) If no key is specified, the assembly is not signed.
41// (*) KeyName refers to a key that has been installed in the Crypto Service
42// Provider (CSP) on your machine. KeyFile refers to a file which contains
43// a key.
44// (*) If the KeyFile and the KeyName values are both specified, the
45// following processing occurs:
46// (1) If the KeyName can be found in the CSP, that key is used.
47// (2) If the KeyName does not exist and the KeyFile does exist, the key
48// in the KeyFile is installed into the CSP and used.
49// (*) In order to create a KeyFile, you can use the sn.exe (Strong Name) utility.
50// When specifying the KeyFile, the location of the KeyFile should be
51// relative to the project directory.
52// (*) Delay Signing is an advanced option - see the Microsoft .NET Framework
53// documentation for more information on this.
54//
55[assembly:AssemblyDelaySignAttribute(false)];
56[assembly:AssemblyKeyFileAttribute("")];
57[assembly:AssemblyKeyNameAttribute("")];
58
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp
deleted file mode 100644
index c95ae57..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp
+++ /dev/null
@@ -1,322 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "Body.h"
5
6namespace ODEManaged
7{
8
9 //Constructors
10
11 Body::Body(void)
12 {
13 _id = 0;
14 }
15
16 Body::Body(World &world)
17 {
18 _id = dBodyCreate(world.Id());
19 }
20
21
22 //Destructor
23
24 Body::~Body(void)
25 {
26 dBodyDestroy(this->_id);
27 }
28
29
30 //Methods
31
32 //Id
33 dBodyID Body::Id()
34 {
35 return _id;
36 }
37
38
39 //SetData
40 void Body::SetData(void *data)
41 {
42 dBodySetData(this->_id, data);
43 }
44
45 //GetData
46 void *Body::GetData(void)
47 {
48 return dBodyGetData(this->_id);
49 }
50
51
52 //SetPosition
53 void Body::SetPosition (double x, double y, double z)
54 {
55 dBodySetPosition(this->_id, x, y, z);
56 }
57
58
59 //Overloaded GetPosition
60 Vector3 Body::GetPosition(void)
61 {
62 Vector3 retVal;
63 const dReal *temp;
64 temp = dBodyGetPosition(this->_id);
65 retVal.x = temp[0];
66 retVal.y = temp[1];
67 retVal.z = temp[2];
68 return retVal;
69 };
70
71 void Body::GetPosition(double position __gc[])
72 {
73 const dReal *temp;
74 temp = dBodyGetPosition(this->_id);
75 position[0] = temp[0];
76 position[1] = temp[1];
77 position[2] = temp[2];
78 }
79
80
81 //SetRotationIdentity
82 void Body::SetRotationIdentity(void)
83 {
84 dMatrix3 temp;
85 dRSetIdentity(temp);
86 dBodySetRotation(this->_id, temp);
87 }
88
89
90 //SetRotation (left handed system=>transpose)
91 void Body::SetRotation(Matrix3 rotation)
92 {
93 dMatrix3 temp;
94 temp[0] = rotation.m11;
95 temp[4] = rotation.m12;
96 temp[8] = rotation.m13;
97 temp[1] = rotation.m21;
98 temp[5] = rotation.m22;
99 temp[9] = rotation.m23;
100 temp[2] = rotation.m31;
101 temp[6] = rotation.m32;
102 temp[10] = rotation.m33;
103 dBodySetRotation(this->_id, temp);
104 }
105
106 //GetRotation (left handed system=>transpose)
107 Matrix3 Body::GetRotation(void)
108 {
109 Matrix3 retVal;
110 //const dMatrix3 *m;
111 const dReal *temp;
112 temp = dBodyGetRotation(this->_id);
113 retVal.m11 = temp[0];
114 retVal.m12 = temp[4];
115 retVal.m13 = temp[8];
116 retVal.m21 = temp[1];
117 retVal.m22 = temp[5];
118 retVal.m23 = temp[9];
119 retVal.m31 = temp[2];
120 retVal.m32 = temp[6];
121 retVal.m33 = temp[10];
122 return retVal;
123 }
124
125
126 //Overloaded SetMass
127 void Body::SetMass(double mass, Vector3 centerOfGravity, Matrix3 inertia)
128 {
129 dMass *temp = new dMass();
130 dMassSetParameters(temp, mass,
131 centerOfGravity.x,
132 centerOfGravity.y,
133 centerOfGravity.z,
134 inertia.m11, inertia.m22,
135 inertia.m33, inertia.m12,
136 inertia.m13, inertia.m23);
137
138 dBodySetMass(this->_id, temp);
139 }
140
141
142 //SetMassSphere
143 void Body::SetMassSphere(double density, double radius)
144 {
145 dMass *temp = new dMass();
146 dMassSetSphere(temp, density, radius);
147 dBodySetMass(this->_id, temp);
148 }
149
150
151 //SetMassBox
152 void Body::SetMassBox(double density, double sideX, double sideY, double sideZ)
153 {
154 dMass *temp = new dMass();
155 dMassSetBox(temp, density, sideX, sideY, sideZ);
156 dBodySetMass(this->_id, temp);
157 }
158
159
160 //SetMassCappedCylinder
161 void Body::SetMassCappedCylinder(double density, int axis, double cylinderRadius, double cylinderLength)
162 {
163 dMass *temp = new dMass();
164 dMassSetCappedCylinder(temp, density, axis,
165 cylinderRadius,
166 cylinderLength);
167
168 dBodySetMass(this->_id, temp);
169 }
170
171
172 //AddForce
173 void Body::AddForce(double fX, double fY, double fZ)
174 {
175 dBodyAddForce(this->_id, fX, fY, fZ);
176 }
177
178
179 //AddRelForce
180 void Body::AddRelForce(double fX, double fY, double fZ)
181 {
182 dBodyAddRelForce(this->_id, fX,fY,fZ);
183 }
184
185
186 //AddForceAtPos
187 void Body::AddForceAtPos(double fX, double fY, double fZ, double pX, double pY, double pZ)
188 {
189 dBodyAddForceAtPos(this->_id, fX, fY, fZ, pX, pY, pZ);
190 }
191
192
193 //AddRelForceAtPos
194 void Body::AddRelForceAtPos(double fX, double fY, double fZ, double pX, double pY, double pZ)
195 {
196 dBodyAddRelForceAtPos(this->_id, fX, fY, fZ, pX, pY, pZ);
197 }
198
199
200 //AddRelForceAtRelPos
201 void Body::AddRelForceAtRelPos(double fX, double fY, double fZ, double pX, double pY, double pZ)
202 {
203 dBodyAddRelForceAtRelPos(this->_id, fX, fY, fZ, pX, pY, pZ);
204 }
205
206
207 //ApplyLinearVelocityDrag
208 void Body::ApplyLinearVelocityDrag(double dragCoef)
209 {
210 const dReal *temp;
211 double fX;
212 double fY;
213 double fZ;
214 temp = dBodyGetLinearVel(this->_id);
215 fX = temp[0]*dragCoef*-1;
216 fY = temp[1]*dragCoef*-1;
217 fZ = temp[2]*dragCoef*-1;
218 dBodyAddForce(this->_id, fX, fY, fZ);
219 }
220
221
222 //ApplyAngularVelocityDrag
223 void Body::ApplyAngularVelocityDrag(double dragCoef)
224 {
225 const dReal *temp;
226 double fX;
227 double fY;
228 double fZ;
229 temp = dBodyGetAngularVel(this->_id);
230 fX = temp[0]*dragCoef*-1;
231 fY = temp[1]*dragCoef*-1;
232 fZ = temp[2]*dragCoef*-1;
233 dBodyAddTorque(this->_id, fX, fY, fZ);
234 }
235
236
237 //AddTorque
238 void Body::AddTorque(double fX, double fY, double fZ)
239 {
240 dBodyAddTorque(this->_id, fX, fY, fZ);
241 }
242
243
244 //AddRelTorque
245 void Body::AddRelTorque(double fX, double fY, double fZ)
246 {
247 dBodyAddRelTorque(this->_id, fX,fY,fZ);
248 }
249
250
251 //SetLinearVelocity
252 void Body::SetLinearVelocity(double x, double y, double z)
253 {
254 dBodySetLinearVel(this->_id, x, y, z);
255 }
256
257
258 //GetLinearVelocity
259 Vector3 Body::GetLinearVelocity(void)
260 {
261 Vector3 retVal;
262 const dReal *temp;
263 temp = dBodyGetLinearVel(this->_id);
264 retVal.x = temp[0];
265 retVal.y = temp[1];
266 retVal.z = temp[2];
267 return retVal;
268 }
269
270
271 //SetAngularVelocity
272 void Body::SetAngularVelocity(double x, double y, double z)
273 {
274 dBodySetAngularVel(this->_id, x, y, z);
275 }
276
277 //GetAngularVelocity
278 Vector3 Body::GetAngularVelocity(void)
279 {
280 Vector3 retVal;
281 const dReal *temp;
282 temp = dBodyGetAngularVel(this->_id);
283 retVal.x = temp[0];
284 retVal.y = temp[1];
285 retVal.z = temp[2];
286 return retVal;
287 }
288
289
290 //GetRelPointPos
291 Vector3 Body::GetRelPointPos(double pX, double pY, double pZ)
292 {
293 Vector3 retVal;
294 dVector3 temp;
295 dBodyGetRelPointPos(this->_id, pX, pY, pZ, temp);
296 retVal.x = temp[0];
297 retVal.y = temp[1];
298 retVal.z = temp[2];
299 return retVal;
300 }
301
302
303 //GetRelPointVel
304 Vector3 Body::GetRelPointVel(double pX, double pY, double pZ)
305 {
306 Vector3 retVal;
307 dVector3 temp;
308 dBodyGetRelPointVel(this->_id, pX, pY, pZ, temp);
309 retVal.x = temp[0];
310 retVal.y = temp[1];
311 retVal.z = temp[2];
312 return retVal;
313 }
314
315
316 //ConnectedTo
317 int Body::ConnectedTo(const Body &b)
318 {
319 return dAreConnected(this->_id, b._id);
320 }
321
322}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Body.h b/libraries/ode-0.9/contrib/DotNetManaged/Body.h
deleted file mode 100644
index 9347c17..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Body.h
+++ /dev/null
@@ -1,76 +0,0 @@
1#pragma once
2
3#include "World.h"
4#include "CommonMgd.h"
5
6namespace ODEManaged
7{
8 __gc public class Body
9 {
10 public:
11
12 //Constructors and Destructors
13
14 Body(void);
15 Body(World &world);
16
17 ~Body(void);
18
19
20 //Public Methods
21
22 dBodyID Id();
23 void SetData (void *data);
24 void *GetData (void);
25
26 //POSITION
27 void SetPosition(double x, double y, double z);
28 Vector3 GetPosition(void);
29 void GetPosition(double position __gc[]);
30
31 //ROTATION
32 void SetRotationIdentity(void);
33 void SetRotation(Matrix3 rotation);
34 Matrix3 GetRotation(void);
35
36 //MASS
37 void SetMass(double mass, Vector3 centerOfGravity, Matrix3 inertia);
38 void SetMassSphere(double density, double radius);
39 void SetMassBox(double density, double sideX, double sideY, double sideZ);
40 void SetMassCappedCylinder(double density, int axis, double cylinderRadius, double cylinderLength);
41
42 //FORCE AND TORQUE
43 void AddForce(double fX, double fY, double fZ);
44 void AddRelForce(double fX, double fY, double fZ);
45 void AddForceAtPos(double fX, double fY, double fZ,double pX, double pY, double pZ);
46 void AddRelForceAtPos(double fX, double fY, double fZ,double pX, double pY, double pZ);
47 void AddRelForceAtRelPos(double fX, double fY, double fZ,double pX, double pY, double pZ);
48 void ApplyLinearVelocityDrag(double dragCoef);
49 void ApplyAngularVelocityDrag(double dragCoef);
50
51
52 void AddTorque(double fX, double fY, double fZ);
53 void AddRelTorque(double fX, double fY, double fZ);
54
55 //LINEAR VELOCITY
56 void SetLinearVelocity (double x, double y, double z);
57 Vector3 GetLinearVelocity(void);
58
59 //ANGULAR VELOCITY
60 void SetAngularVelocity (double x, double y, double z);
61 Vector3 GetAngularVelocity(void);
62
63 //POINT
64 Vector3 GetRelPointPos(double pX, double pY, double pZ);
65 Vector3 GetRelPointVel(double pX, double pY, double pZ);
66
67 //CONNECTED TO
68 int ConnectedTo (const Body &b);
69
70 private:
71
72 dBodyID _id;
73
74 };
75}
76
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h b/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h
deleted file mode 100644
index 143397d..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h
+++ /dev/null
@@ -1,43 +0,0 @@
1#pragma once
2
3namespace ODEManaged
4{
5
6 __value public struct Vector3
7 {
8 double x;
9 double y;
10 double z;
11 };
12
13
14 __value public struct Vector4
15 {
16 double W;
17 double x;
18 double y;
19 double z;
20 };
21
22
23 __value public struct Matrix3
24 {
25 double m11;
26 double m12;
27 double m13;
28 double m21;
29 double m22;
30 double m23;
31 double m31;
32 double m32;
33 double m33;
34 };
35
36 //__value public struct NearCallback
37 //{
38 // void *data;
39 // dGeomID o1;
40 // dGeomID o2;
41 //};
42
43} \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln
deleted file mode 100644
index 2694a26..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln
+++ /dev/null
@@ -1,21 +0,0 @@
1Microsoft Visual Studio Solution File, Format Version 7.00
2Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DotNetManaged", "DotNetManaged.vcproj", "{4B75AC19-971A-4CC6-A4F5-0695C9F8562F}"
3EndProject
4Global
5 GlobalSection(SolutionConfiguration) = preSolution
6 ConfigName.0 = Debug
7 ConfigName.1 = Release
8 EndGlobalSection
9 GlobalSection(ProjectDependencies) = postSolution
10 EndGlobalSection
11 GlobalSection(ProjectConfiguration) = postSolution
12 {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Debug.ActiveCfg = Debug|Win32
13 {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Debug.Build.0 = Debug|Win32
14 {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Release.ActiveCfg = Release|Win32
15 {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Release.Build.0 = Release|Win32
16 EndGlobalSection
17 GlobalSection(ExtensibilityGlobals) = postSolution
18 EndGlobalSection
19 GlobalSection(ExtensibilityAddIns) = postSolution
20 EndGlobalSection
21EndGlobal
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj
deleted file mode 100644
index 2f5bb6c..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj
+++ /dev/null
@@ -1,379 +0,0 @@
1<?xml version="1.0" encoding = "Windows-1252"?>
2<VisualStudioProject
3 ProjectType="Visual C++"
4 Version="7.00"
5 Name="DotNetManaged"
6 ProjectGUID="{4B75AC19-971A-4CC6-A4F5-0695C9F8562F}"
7 Keyword="ManagedCProj">
8 <Platforms>
9 <Platform
10 Name="Win32"/>
11 </Platforms>
12 <Configurations>
13 <Configuration
14 Name="Debug|Win32"
15 OutputDirectory="Debug"
16 IntermediateDirectory="Debug"
17 ConfigurationType="2"
18 CharacterSet="2"
19 ManagedExtensions="TRUE">
20 <Tool
21 Name="VCCLCompilerTool"
22 Optimization="2"
23 AdditionalIncludeDirectories="&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Contrib\tri-collider&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\ODE\Src&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\ODE&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Include&quot;"
24 PreprocessorDefinitions="WIN32;_DEBUG"
25 MinimalRebuild="FALSE"
26 BasicRuntimeChecks="0"
27 RuntimeLibrary="1"
28 WarningLevel="3"
29 DebugInformationFormat="3"/>
30 <Tool
31 Name="VCCustomBuildTool"/>
32 <Tool
33 Name="VCLinkerTool"
34 OutputFile="$(OutDir)/DotNetManaged.exe"
35 LinkIncremental="2"
36 GenerateDebugInformation="TRUE"/>
37 <Tool
38 Name="VCMIDLTool"/>
39 <Tool
40 Name="VCPostBuildEventTool"/>
41 <Tool
42 Name="VCPreBuildEventTool"/>
43 <Tool
44 Name="VCPreLinkEventTool"/>
45 <Tool
46 Name="VCResourceCompilerTool"/>
47 <Tool
48 Name="VCWebServiceProxyGeneratorTool"/>
49 <Tool
50 Name="VCWebDeploymentTool"/>
51 </Configuration>
52 <Configuration
53 Name="Release|Win32"
54 OutputDirectory="Release"
55 IntermediateDirectory="Release"
56 ConfigurationType="2"
57 CharacterSet="2"
58 ManagedExtensions="TRUE"
59 WholeProgramOptimization="FALSE">
60 <Tool
61 Name="VCCLCompilerTool"
62 Optimization="2"
63 InlineFunctionExpansion="0"
64 OmitFramePointers="TRUE"
65 AdditionalIncludeDirectories="&quot;F:\Documents and Settings\Visual Studio Projects\ODE&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Contrib\DotNetManaged\odeOLd&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Include&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Include\ODE&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\ODE\Src&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Contrib\tri-collider&quot;"
66 PreprocessorDefinitions="WIN32;NDEBUG"
67 MinimalRebuild="FALSE"
68 WarningLevel="3"
69 DebugInformationFormat="3"/>
70 <Tool
71 Name="VCCustomBuildTool"/>
72 <Tool
73 Name="VCLinkerTool"
74 OutputFile="$(OutDir)/ode.dll"
75 SuppressStartupBanner="TRUE"
76 GenerateDebugInformation="FALSE"/>
77 <Tool
78 Name="VCMIDLTool"/>
79 <Tool
80 Name="VCPostBuildEventTool"/>
81 <Tool
82 Name="VCPreBuildEventTool"/>
83 <Tool
84 Name="VCPreLinkEventTool"/>
85 <Tool
86 Name="VCResourceCompilerTool"/>
87 <Tool
88 Name="VCWebServiceProxyGeneratorTool"/>
89 <Tool
90 Name="VCWebDeploymentTool"/>
91 </Configuration>
92 </Configurations>
93 <Files>
94 <Filter
95 Name="Source Files"
96 Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm">
97 <File
98 RelativePath="..\..\ODE\Src\array.cpp">
99 </File>
100 <File
101 RelativePath="..\..\ODE\Src\error.cpp">
102 </File>
103 <File
104 RelativePath="..\..\ODE\Src\fastdot.c">
105 </File>
106 <File
107 RelativePath="..\..\ODE\Src\fastldlt.c">
108 </File>
109 <File
110 RelativePath="..\..\ODE\Src\fastlsolve.c">
111 </File>
112 <File
113 RelativePath="..\..\ODE\Src\fastltsolve.c">
114 </File>
115 <File
116 RelativePath="..\..\ODE\Src\geom.cpp">
117 </File>
118 <File
119 RelativePath="..\..\ODE\Src\joint.cpp">
120 </File>
121 <File
122 RelativePath="..\..\ODE\Src\lcp.cpp">
123 </File>
124 <File
125 RelativePath="..\..\ODE\Src\mass.cpp">
126 </File>
127 <File
128 RelativePath="..\..\ODE\Src\mat.cpp">
129 </File>
130 <File
131 RelativePath="..\..\ODE\Src\matrix.cpp">
132 </File>
133 <File
134 RelativePath="..\..\ODE\Src\memory.cpp">
135 </File>
136 <File
137 RelativePath="..\..\ODE\Src\misc.cpp">
138 </File>
139 <File
140 RelativePath="..\..\ODE\Src\objects.h">
141 </File>
142 <File
143 RelativePath="..\..\ODE\Src\obstack.cpp">
144 </File>
145 <File
146 RelativePath="..\..\ODE\Src\ode.cpp">
147 </File>
148 <File
149 RelativePath="..\..\ODE\Src\odemath.cpp">
150 </File>
151 <File
152 RelativePath="..\..\ODE\Src\rotation.cpp">
153 </File>
154 <File
155 RelativePath="..\..\ODE\Src\space.cpp">
156 </File>
157 <File
158 RelativePath="..\..\ODE\Src\step.cpp">
159 </File>
160 <File
161 RelativePath="..\..\ODE\Src\testing.cpp">
162 </File>
163 <File
164 RelativePath="..\..\ODE\Src\timer.cpp">
165 </File>
166 </Filter>
167 <Filter
168 Name="Header Files"
169 Filter="h;hpp;hxx;hm;inl;inc">
170 <File
171 RelativePath="..\..\ODE\Src\array.h">
172 </File>
173 <File
174 RelativePath="..\..\Include\ODE\common.h">
175 </File>
176 <File
177 RelativePath="..\..\Include\ODE\config.h">
178 </File>
179 <File
180 RelativePath="..\..\Include\ODE\contact.h">
181 </File>
182 <File
183 RelativePath="..\..\Include\ODE\error.h">
184 </File>
185 <File
186 RelativePath="..\..\Include\ODE\geom.h">
187 </File>
188 <File
189 RelativePath="..\..\ODE\Src\geom_internal.h">
190 </File>
191 <File
192 RelativePath="..\..\ODE\Src\joint.h">
193 </File>
194 <File
195 RelativePath="..\..\ODE\Src\lcp.h">
196 </File>
197 <File
198 RelativePath="..\..\Include\ODE\mass.h">
199 </File>
200 <File
201 RelativePath="..\..\ODE\Src\mat.h">
202 </File>
203 <File
204 RelativePath="..\..\Include\ODE\matrix.h">
205 </File>
206 <File
207 RelativePath="..\..\Include\ODE\memory.h">
208 </File>
209 <File
210 RelativePath="..\..\Include\ODE\misc.h">
211 </File>
212 <File
213 RelativePath="..\..\Include\ODE\objects.h">
214 </File>
215 <File
216 RelativePath="..\..\ODE\Src\obstack.h">
217 </File>
218 <File
219 RelativePath="..\..\Include\ODE\ode.h">
220 </File>
221 <File
222 RelativePath="..\..\Include\ODE\odecpp.h">
223 </File>
224 <File
225 RelativePath="..\..\Include\ODE\odemath.h">
226 </File>
227 <File
228 RelativePath="..\..\Include\ODE\rotation.h">
229 </File>
230 <File
231 RelativePath="..\..\Include\ODE\space.h">
232 </File>
233 <File
234 RelativePath="..\..\ODE\Src\step.h">
235 </File>
236 <File
237 RelativePath="..\..\ODE\Src\testing.h">
238 </File>
239 <File
240 RelativePath="..\..\Include\ODE\timer.h">
241 </File>
242 </Filter>
243 <Filter
244 Name="Resource Files"
245 Filter="rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;r">
246 </Filter>
247 <Filter
248 Name="Wrapper"
249 Filter="">
250 <Filter
251 Name="Header Files"
252 Filter="">
253 <File
254 RelativePath="Body.h">
255 </File>
256 <File
257 RelativePath="CommonMgd.h">
258 </File>
259 <File
260 RelativePath="Geom.h">
261 </File>
262 <File
263 RelativePath="Joint.h">
264 </File>
265 <File
266 RelativePath="JointAMotor.h">
267 </File>
268 <File
269 RelativePath="JointBall.h">
270 </File>
271 <File
272 RelativePath="JointFixed.h">
273 </File>
274 <File
275 RelativePath="JointGroup.h">
276 </File>
277 <File
278 RelativePath="JointHinge.h">
279 </File>
280 <File
281 RelativePath="JointHinge2.h">
282 </File>
283 <File
284 RelativePath="JointSlider.h">
285 </File>
286 <File
287 RelativePath="Space.h">
288 </File>
289 <File
290 RelativePath="Stdafx.h">
291 </File>
292 <File
293 RelativePath="World.h">
294 </File>
295 </Filter>
296 <Filter
297 Name="Source Files"
298 Filter="">
299 <File
300 RelativePath="Body.cpp">
301 </File>
302 <File
303 RelativePath="Geom.cpp">
304 <FileConfiguration
305 Name="Debug|Win32">
306 <Tool
307 Name="VCCLCompilerTool"
308 ObjectFile="$(IntDir)/$(InputName)1.obj"/>
309 </FileConfiguration>
310 <FileConfiguration
311 Name="Release|Win32">
312 <Tool
313 Name="VCCLCompilerTool"
314 ObjectFile="$(IntDir)/$(InputName)1.obj"/>
315 </FileConfiguration>
316 </File>
317 <File
318 RelativePath="Joint.cpp">
319 <FileConfiguration
320 Name="Debug|Win32">
321 <Tool
322 Name="VCCLCompilerTool"
323 ObjectFile="$(IntDir)/$(InputName)1.obj"/>
324 </FileConfiguration>
325 <FileConfiguration
326 Name="Release|Win32">
327 <Tool
328 Name="VCCLCompilerTool"
329 ObjectFile="$(IntDir)/$(InputName)1.obj"/>
330 </FileConfiguration>
331 </File>
332 <File
333 RelativePath="JointAMotor.cpp">
334 </File>
335 <File
336 RelativePath="JointBall.cpp">
337 </File>
338 <File
339 RelativePath="JointFixed.cpp">
340 </File>
341 <File
342 RelativePath="JointGroup.cpp">
343 </File>
344 <File
345 RelativePath="JointHinge.cpp">
346 </File>
347 <File
348 RelativePath="JointHinge2.cpp">
349 </File>
350 <File
351 RelativePath="JointSlider.cpp">
352 </File>
353 <File
354 RelativePath="Space.cpp">
355 <FileConfiguration
356 Name="Debug|Win32">
357 <Tool
358 Name="VCCLCompilerTool"
359 ObjectFile="$(IntDir)/$(InputName)1.obj"/>
360 </FileConfiguration>
361 <FileConfiguration
362 Name="Release|Win32">
363 <Tool
364 Name="VCCLCompilerTool"
365 ObjectFile="$(IntDir)/$(InputName)1.obj"/>
366 </FileConfiguration>
367 </File>
368 <File
369 RelativePath="Stdafx.cpp">
370 </File>
371 <File
372 RelativePath="World.cpp">
373 </File>
374 </Filter>
375 </Filter>
376 </Files>
377 <Globals>
378 </Globals>
379</VisualStudioProject>
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp
deleted file mode 100644
index 3655466..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp
+++ /dev/null
@@ -1,219 +0,0 @@
1
2#include "StdAfx.h"
3
4#include <ode/ode.h>
5#include "Geom.h"
6
7
8namespace ODEManaged
9{
10
11 //Constructors
12
13 Geom::Geom(void)
14 {
15 _id = 0;
16 }
17
18
19 //Destructor
20
21 Geom::~Geom(void)
22 {
23 dGeomDestroy(this->_id);
24 }
25
26
27 //Methods
28
29 //Id
30 dGeomID Geom::Id(void)
31 {
32 return _id;
33 }
34
35
36 //GetBody
37 dBodyID Geom::GetBody(void)
38 {
39 return dGeomGetBody(this->_id);
40 }
41
42
43 //Overloaded SetBody
44 void Geom::SetBody(Body &body)
45 {
46 dGeomSetBody(this->_id, body.Id());
47 }
48
49 //void Geom::SetBody(dBodyID b)
50 //{
51 // dGeomSetBody(this->_id, b);
52 //}
53
54
55 //SetPosition
56 void Geom::SetPosition(double x, double y, double z)
57 {
58 dGeomSetPosition(this->_id, x, y, z);
59 }
60
61
62 //SetRotation
63 void Geom::SetRotation(Matrix3 rotation)
64 {
65 dMatrix3 temp;
66 temp[0] = rotation.m11;
67 temp[4] = rotation.m12;
68 temp[8] = rotation.m13;
69 temp[1] = rotation.m21;
70 temp[5] = rotation.m22;
71 temp[9] = rotation.m23;
72 temp[2] = rotation.m31;
73 temp[6] = rotation.m32;
74 temp[10] = rotation.m33;
75 dGeomSetRotation(_id, temp);
76 }
77
78
79 //Destroy
80 void Geom::Destroy()
81 {
82 if(this->_id) dGeomDestroy(this->_id);
83 _id = 0;
84 }
85
86
87 //SetData
88 void Geom::SetData(void *data)
89 {
90 dGeomSetData(this->_id, data);
91 }
92
93
94 //GetData
95 void *Geom::GetData(void)
96 {
97 return dGeomGetData(this->_id);
98 }
99
100
101 //GetPosition
102 Vector3 Geom::GetPosition(void)
103 {
104 Vector3 retVal;
105 const dReal *temp;
106 temp = dGeomGetPosition(this->_id);
107 retVal.x = temp[0];
108 retVal.y = temp[1];
109 retVal.z = temp[2];
110 return retVal;
111 }
112
113
114 //GetRotation (left handed system=>transpose)
115 Matrix3 Geom::GetRotation(void)
116 {
117 Matrix3 retVal;
118 const dReal *temp;
119 temp = dGeomGetRotation(this->_id);
120 retVal.m11 = temp[0];
121 retVal.m12 = temp[4];
122 retVal.m13 = temp[8];
123 retVal.m21 = temp[1];
124 retVal.m22 = temp[5];
125 retVal.m23 = temp[9];
126 retVal.m31 = temp[2];
127 retVal.m32 = temp[6];
128 retVal.m33 = temp[10];
129 return retVal;
130 }
131
132
133 //CreateSphere
134 void Geom::CreateSphere(Space &space, double radius)
135 {
136 if(this->_id) dGeomDestroy(this->_id);
137 _id = dCreateSphere(space.Id(), radius);
138 }
139
140
141 //CreateBox
142 void Geom::CreateBox(Space &space, double lx, double ly, double lz)
143 {
144 if(this->_id) dGeomDestroy(this->_id);
145 _id = dCreateBox(space.Id(), lx, ly, lz);
146 }
147
148
149 //CreatePlane
150 void Geom::CreatePlane(Space &space, double a, double b, double c, double d)
151 {
152 if(this->_id) dGeomDestroy(this->_id);
153 _id = dCreatePlane(space.Id(), a, b, c, d);
154 }
155
156
157 //CreateCCylinder
158 void Geom::CreateCCylinder(Space &space, double radius, double length)
159 {
160 if(this->_id) dGeomDestroy(this->_id);
161 _id = dCreateCCylinder(space.Id(), radius, length);
162 }
163
164
165 //SphereGetRadius
166 double Geom::SphereGetRadius(void)
167 {
168 return dGeomSphereGetRadius(this->_id);
169 }
170
171
172 //BoxGetLengths
173 Vector3 Geom::BoxGetLengths(void)
174 {
175 Vector3 retVal;
176 dVector3 temp;
177 dGeomBoxGetLengths(this->_id, temp);
178 retVal.x = temp[0];
179 retVal.y = temp[1];
180 retVal.z = temp[2];
181 return retVal;
182 }
183
184
185 //PlaneGetParams
186 Vector4 Geom::PlaneGetParams(void)
187 {
188 Vector4 retVal;
189 dVector4 temp;
190 dGeomPlaneGetParams(this->_id, temp);
191 retVal.W = temp[0];
192 retVal.x = temp[1];
193 retVal.y = temp[2];
194 retVal.z = temp[3];
195 return retVal;
196 }
197
198
199 //CCylinderGetParams
200 void Geom::CCylinderGetParams(double *radius, double *length)
201 {
202 dGeomCCylinderGetParams(this->_id, radius, length);
203 }
204
205
206 //GetClass
207 int Geom::GetClass(void)
208 {
209 return dGeomGetClass(this->_id);
210 }
211
212}
213
214
215
216
217
218
219
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Geom.h b/libraries/ode-0.9/contrib/DotNetManaged/Geom.h
deleted file mode 100644
index 83a6faf..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Geom.h
+++ /dev/null
@@ -1,75 +0,0 @@
1#pragma once
2
3#include "Body.h"
4#include "Space.h"
5#include "CommonMgd.h"
6
7namespace ODEManaged
8{
9 __gc public class Geom
10 {
11 public:
12
13
14 //Constructor
15
16 Geom (void);
17
18
19 //Destructor
20
21 ~Geom (void);
22
23
24 //Methods
25
26 //Basic Stuff
27
28 dGeomID Id (void);
29 dBodyID GetBody (void);
30
31 //Overloaded SetBody
32 void SetBody (Body &body);
33 /*void SetBody (dBodyID b);*/
34
35 Vector3 GetPosition (void);
36 void SetPosition (double x, double y, double z);
37
38 Matrix3 GetRotation (void);
39 void SetRotation (Matrix3 rotation);
40
41 void SetData (void *data);
42 void *GetData (void);
43
44
45 //Create Objects
46
47 void CreateSphere (Space &space, double radius);
48 void CreateBox (Space &space, double lx, double ly, double lz);
49 void CreatePlane (Space &space, double a, double b, double c, double d);
50 void CreateCCylinder (Space &space, double radius, double length);
51
52
53 //Destroy Objects
54
55 void Destroy (void);
56
57
58 //Get Object's Parameters
59
60 double SphereGetRadius (void);
61 Vector3 BoxGetLengths (void);
62 Vector4 PlaneGetParams (void);
63 void CCylinderGetParams (double *radius, double *length);
64 int GetClass (void);
65
66
67 //Properties
68
69 private:
70
71 dGeomID _id;
72
73 };
74
75}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp
deleted file mode 100644
index e2d8de6..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "joint.h"
5#include "CommonMgd.h"
6#include "world.h"
7
8namespace ODEManaged
9{
10
11 //Constructor
12
13 Joint::Joint(void)
14 {
15 _id=0;
16 }
17
18
19 //Destructor
20
21 Joint::~Joint(void)
22 {
23 dJointDestroy(this->_id);
24 }
25
26
27 //Methods
28
29 //Id
30 dJointID Joint::Id(void)
31 {
32 return _id;
33 }
34
35}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Joint.h b/libraries/ode-0.9/contrib/DotNetManaged/Joint.h
deleted file mode 100644
index d9ab254..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Joint.h
+++ /dev/null
@@ -1,21 +0,0 @@
1#pragma once
2
3#include "JointGroup.h"
4#include "World.h"
5#include "Body.h"
6
7namespace ODEManaged
8{
9 __gc public class Joint
10 {
11 protected:
12 //Constructor and Destructor Defenition
13 Joint(void);
14 ~Joint(void);
15
16 //Public Methods
17 dJointID Id(void);
18
19 dJointID _id;
20 };
21}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp
deleted file mode 100644
index c5a4543..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp
+++ /dev/null
@@ -1,162 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "JointAMotor.h"
5
6namespace ODEManaged
7{
8
9 //Constructors
10
11 JointAMotor::JointAMotor(void) : Joint(){}
12
13
14 JointAMotor::JointAMotor(World &world)
15 {
16 if(this->_id) dJointDestroy(this->_id);
17 _id = dJointCreateAMotor(world.Id(), 0);
18 }
19
20
21 JointAMotor::JointAMotor(World &world, JointGroup &jointGroup)
22 {
23 if(this->_id) dJointDestroy(this->_id);
24 _id = dJointCreateAMotor(world.Id(), jointGroup.Id());
25 }
26
27
28 //Destructor
29
30 JointAMotor::~JointAMotor(void){}
31
32
33 //Methods
34
35 //Overloaded Create
36 void JointAMotor::Create(World &world, JointGroup &jointGroup)
37 {
38 if(this->_id) dJointDestroy(this->_id);
39 _id = dJointCreateAMotor(world.Id(), jointGroup.Id());
40 }
41
42 void JointAMotor::Create(World &world)
43 {
44 if(this->_id) dJointDestroy(this->_id);
45 _id = dJointCreateAMotor(world.Id(), 0);
46 }
47
48
49 //Overloaded Attach
50 void JointAMotor::Attach(Body &body1, Body &body2)
51 {
52 dJointAttach(this->_id, body1.Id(), body2.Id());
53 }
54
55 void JointAMotor::Attach(Body &body1)
56 {
57 dJointAttach(this->_id, body1.Id(), 0);
58 }
59
60
61 //SetNumAxes
62
63 void JointAMotor::SetNumAxes(int num)
64 {
65 dJointSetAMotorNumAxes(this->_id, num);
66 }
67
68
69 //GetNumAxes
70
71 int JointAMotor::GetNumAxes(void)
72 {
73 return dJointGetAMotorNumAxes(this->_id);
74 }
75
76
77 //SetAxis
78
79 void JointAMotor::SetAxis(int anum, int rel, double x, double y ,double z)
80 {
81 dJointSetAMotorAxis(this->_id, anum, rel, x, y, z);
82 }
83
84
85 //GetAxis
86
87 Vector3 JointAMotor::GetAxis(int anum)
88 {
89 Vector3 retVal;
90 dVector3 temp;
91 dJointGetAMotorAxis(this->_id, anum, temp);
92 retVal.x = temp[0];
93 retVal.y = temp[1];
94 retVal.z = temp[2];
95 return retVal;
96 }
97
98
99 //SetAngle
100
101 void JointAMotor::SetAngle(int anum, double angle)
102 {
103 dJointSetAMotorAngle(this->_id, anum, angle);
104 }
105
106
107 //GetAngle
108
109 double JointAMotor::GetAngle(int anum)
110 {
111 return dJointGetAMotorAngle(this->_id, anum);
112 }
113
114
115 //SetParam
116
117 void JointAMotor::SetParam(int parameter, double value)
118 {
119 dJointSetAMotorParam(this->_id, parameter, value);
120 }
121
122
123 //GetParam
124
125 double JointAMotor::GetParam(int parameter)
126 {
127 return dJointGetAMotorParam(this->_id, parameter);
128 }
129
130
131 //SetMode
132
133 void JointAMotor::SetMode(int mode)
134 {
135 dJointSetAMotorMode(this->_id, mode);
136 }
137
138
139 //GetMode
140
141 int JointAMotor::GetMode(void)
142 {
143 return dJointGetAMotorMode(this->_id);
144 }
145
146
147 //GetAxisRel
148
149 int JointAMotor::GetAxisRel(int anum)
150 {
151 return dJointGetAMotorAxisRel(this->_id, anum);
152 }
153
154
155 //GetAngleRate
156
157 double JointAMotor::GetAngleRate(int anum)
158 {
159 return dJointGetAMotorAngleRate(this->_id, anum);
160 }
161
162}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h
deleted file mode 100644
index aa3ca4b..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h
+++ /dev/null
@@ -1,62 +0,0 @@
1#pragma once
2
3#include "Joint.h"
4
5namespace ODEManaged
6{
7 __gc public class JointAMotor : public Joint
8 {
9 public:
10
11
12 //Constructors
13
14 JointAMotor (void);
15 JointAMotor (World &world);
16 JointAMotor (World &world, JointGroup &jointGroup);
17
18
19 //Destructor
20
21 virtual ~JointAMotor (void);
22
23
24 //Methods
25
26 //Basic Stuff
27
28 //Overloaded Create
29 void Create (World &world, JointGroup &jointGroup);
30 void Create (World &world);
31
32 void SetNumAxes (int num);
33 int GetNumAxes (void);
34
35 void SetAxis (int anum, int rel, double x, double y, double z);
36 Vector3 GetAxis (int anum);
37
38 void SetAngle (int anum, double angle);
39 double GetAngle (int anum);
40
41 void SetMode (int mode);
42 int GetMode (void);
43
44 int GetAxisRel (int anum);
45 double GetAngleRate (int anum);
46
47 //Overloaded Attach
48 void Attach (Body &body1, Body &body2);
49 void Attach (Body &body1);
50
51
52 //Movement Parameters
53
54 void SetParam (int parameter, double value);
55 double GetParam (int parameter);
56
57
58
59
60
61 };
62}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp
deleted file mode 100644
index d9336c9..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "jointball.h"
5
6namespace ODEManaged
7{
8
9 //Constructors
10
11 JointBall::JointBall(void) : Joint(){}
12
13
14 JointBall::JointBall(World &world)
15 {
16 if(this->_id) dJointDestroy(this->_id);
17 _id = dJointCreateBall(world.Id(), 0);
18 }
19
20
21 JointBall::JointBall(World &world, JointGroup &jointGroup)
22 {
23 if(this->_id) dJointDestroy(this->_id);
24 _id = dJointCreateBall(world.Id(), jointGroup.Id());
25 }
26
27
28 //Destructor
29
30 JointBall::~JointBall(void){}
31
32
33 //Methods
34
35 //Overloaded Create
36 void JointBall::Create(World &world, JointGroup &jointGroup)
37 {
38 if(this->_id) dJointDestroy(this->_id);
39 _id = dJointCreateBall(world.Id(), jointGroup.Id());
40 }
41
42 void JointBall::Create(World &world)
43 {
44 if(this->_id) dJointDestroy(this->_id);
45 _id = dJointCreateBall(world.Id(), 0);
46 }
47
48
49 //Overloaded Attach
50 void JointBall::Attach(Body &body1, Body &body2)
51 {
52 dJointAttach(this->_id, body1.Id(), body2.Id());
53 }
54
55 void JointBall::Attach(Body &body1)
56 {
57 dJointAttach(this->_id, body1.Id(), 0);
58 }
59
60
61 //SetAnchor
62 void JointBall::SetAnchor(double x, double y ,double z)
63 {
64 dJointSetBallAnchor(this->_id, x, y, z);
65 }
66
67 //GetAnchor
68 Vector3 JointBall::GetAnchor(void)
69 {
70 Vector3 retVal;
71 dVector3 temp;
72 dJointGetBallAnchor(this->_id,temp);
73 retVal.x = temp[0];
74 retVal.y = temp[1];
75 retVal.z = temp[2];
76 return retVal;
77 }
78
79}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h
deleted file mode 100644
index 2355bdd..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h
+++ /dev/null
@@ -1,38 +0,0 @@
1#pragma once
2
3#include "Joint.h"
4
5namespace ODEManaged
6{
7 __gc public class JointBall : public Joint
8 {
9 public:
10
11 //Constructors
12
13 JointBall(void);
14 JointBall(World &world);
15 JointBall(World &world, JointGroup &jointGroup);
16
17
18 //Destructors
19
20 virtual ~JointBall(void);
21
22
23 //Methods
24
25 //Overloaded Create
26 void Create(World &world, JointGroup &jointGroup);
27 void Create(World &world);
28
29 //Overloaded Attach
30 void Attach(Body &body1, Body &body2);
31 void Attach(Body &body1);
32
33 void SetAnchor(double x, double y, double z);
34 Vector3 GetAnchor(void);
35
36 };
37
38}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp
deleted file mode 100644
index afe9222..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "jointfixed.h"
5
6namespace ODEManaged
7{
8
9 //Constructors
10
11 JointFixed::JointFixed(void) : Joint(){}
12
13
14 JointFixed::JointFixed(World &world)
15 {
16 if(this->_id) dJointDestroy(this->_id);
17 _id = dJointCreateFixed(world.Id(),0);
18 }
19
20
21 JointFixed::JointFixed(World &world, JointGroup &jointGroup)
22 {
23 if(this->_id) dJointDestroy(this->_id);
24 _id = dJointCreateFixed(world.Id(), jointGroup.Id());
25 }
26
27
28 //Destructor
29
30 JointFixed::~JointFixed(void){}
31
32
33 //Methods
34
35 //Overloaded Create
36 void JointFixed::Create(World &world, JointGroup &jointGroup)
37 {
38 if(this->_id) dJointDestroy(this->_id);
39 _id = dJointCreateFixed(world.Id(), jointGroup.Id());
40 }
41
42 void JointFixed::Create(World &world)
43 {
44 if(this->_id) dJointDestroy(this->_id);
45 _id = dJointCreateFixed(world.Id(), 0);
46 }
47
48
49 //Overloaded Attach
50 void JointFixed::Attach(Body &body1, Body &body2)
51 {
52 dJointAttach(this->_id, body1.Id(), body2.Id());
53 }
54
55 void JointFixed::Attach(Body &body1)
56 {
57 dJointAttach(this->_id, body1.Id(), 0);
58 }
59
60
61 //Fixed
62 void JointFixed::SetFixed(void)
63 {
64 dJointSetFixed(this->_id);
65 }
66
67}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h
deleted file mode 100644
index 5ca50dc..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h
+++ /dev/null
@@ -1,37 +0,0 @@
1#pragma once
2
3#include "Joint.h"
4
5namespace ODEManaged
6{
7 __gc public class JointFixed : public Joint
8 {
9 public:
10
11 //Constructors
12
13 JointFixed(void);
14 JointFixed(World &world);
15 JointFixed(World &world, JointGroup &jointGroup);
16
17
18 //Destructor
19
20 virtual ~JointFixed(void);
21
22
23 //Methods
24
25 //Overloaded Create
26 void Create(World &world, JointGroup &jointGroup);
27 void Create(World &world);
28
29 //Overloaded Attach
30 void Attach(Body &body1, Body &body2);
31 void Attach(Body &body1);
32
33 void SetFixed(void);
34
35 };
36
37}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp
deleted file mode 100644
index 925751f..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "jointgroup.h"
5
6namespace ODEManaged
7{
8
9 //Constructors
10
11 JointGroup::JointGroup(void)
12 {
13 _id=0;
14 }
15
16 JointGroup::JointGroup (int maxSize)
17 {
18 _id = dJointGroupCreate(maxSize);
19 }
20
21
22 //Destructor
23
24 JointGroup::~JointGroup(void)
25 {
26 dJointGroupDestroy(this->_id);
27 }
28
29
30 //Methods
31
32 //ID
33 dJointGroupID JointGroup::Id()
34 {
35 return _id;
36 }
37
38
39 //Create
40 void JointGroup::Create (int maxSize)
41 {
42 if(_id) dJointGroupDestroy(_id);
43 _id = dJointGroupCreate(maxSize);
44 }
45
46
47 //Empty
48 void JointGroup::Empty (void)
49 {
50 dJointGroupEmpty(this->_id);
51 }
52
53}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h
deleted file mode 100644
index b62ced0..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h
+++ /dev/null
@@ -1,33 +0,0 @@
1#pragma once
2
3namespace ODEManaged
4{
5 __gc public class JointGroup
6 {
7 public:
8
9 //Constructors
10
11 JointGroup(void);
12 JointGroup(int maxSize);
13
14
15 //Destructor
16
17 ~JointGroup(void);
18
19
20 //Methods
21
22 dJointGroupID Id(void);
23 void Create(int maxSize);
24 void Empty(void);
25
26
27 private:
28
29 dJointGroupID _id;
30
31 };
32
33}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp
deleted file mode 100644
index 85d420b..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp
+++ /dev/null
@@ -1,121 +0,0 @@
1#include "stdafx.h"
2
3#include <ode/ode.h>
4#include "jointhinge.h"
5
6namespace ODEManaged
7{
8
9 //Constructors
10
11 JointHinge::JointHinge(void) : Joint(){}
12
13
14 JointHinge::JointHinge(World &world)
15 {
16 if(this->_id) dJointDestroy(this->_id);
17 _id = dJointCreateHinge(world.Id(), 0);
18 }
19
20
21 JointHinge::JointHinge(World &world, JointGroup &jointGroup)
22 {
23 if(this->_id) dJointDestroy(this->_id);
24 _id = dJointCreateHinge(world.Id(), jointGroup.Id());
25 }
26
27
28 //Destructor
29
30 JointHinge::~JointHinge(void){}
31
32
33 //Methods
34
35 //Overloaded Create
36 void JointHinge::Create(World &world, JointGroup &jointGroup)
37 {
38 if(this->_id) dJointDestroy(this->_id);
39 _id = dJointCreateHinge(world.Id(), jointGroup.Id());
40 }
41
42 void JointHinge::Create(World &world)
43 {
44 if(this->_id) dJointDestroy(this->_id);
45 _id = dJointCreateHinge(world.Id(), 0);
46 }
47
48
49 //Overloaded Attach
50 void JointHinge::Attach(Body &body1, Body &body2)
51 {
52 dJointAttach(this->_id, body1.Id(), body2.Id());
53 }
54
55 void JointHinge::Attach(Body &body1)
56 {
57 dJointAttach(this->_id, body1.Id(), 0);
58 }
59
60
61 //SetAxis
62 void JointHinge::SetAxis(double x, double y, double z)
63 {
64 dJointSetHingeAxis(this->_id, x, y, z);
65 }
66
67 //GetAxis
68 Vector3 JointHinge::GetAxis(void)
69 {
70 Vector3 retVal;
71 dVector3 temp;
72 dJointGetHingeAxis(this->_id, temp);
73 retVal.x = temp[0];
74 retVal.y = temp[1];
75 retVal.z = temp[2];
76 return retVal;
77 }
78
79
80 //SetAnchor
81 void JointHinge::SetAnchor(double x, double y, double z)
82 {
83 dJointSetHingeAnchor(this->_id, x, y, z);
84 }
85
86 //GetAnchor
87 Vector3 JointHinge::GetAnchor(void)
88 {
89 Vector3 retVal;
90 dVector3 temp;
91 dJointGetHingeAnchor(this->_id, temp);
92 retVal.x = temp[0];
93 retVal.y = temp[1];
94 retVal.z = temp[2];
95 return retVal;
96 }
97
98
99 //Movement Parameters
100
101 //SetAllMovParams
102 void JointHinge::SetAllMovParams(double LoStop, double HiStop,
103 double Velocity, double MaxForce,
104 double FudgeFactor, double Bounce,
105 double StopERP, double StopCFM)
106 {
107 if (LoStop > -3.141592653 && LoStop <= 0)
108 dJointSetHingeParam(this->_id, dParamLoStop, LoStop);
109
110 if (HiStop < 3.141592653 && HiStop >= 0)
111 dJointSetHingeParam(this->_id, dParamHiStop, HiStop);
112
113 dJointSetHingeParam(this->_id, dParamVel, Velocity);
114 dJointSetHingeParam(this->_id, dParamFMax, MaxForce);
115 dJointSetHingeParam(this->_id, dParamFudgeFactor, FudgeFactor);
116 dJointSetHingeParam(this->_id, dParamBounce, Bounce);
117 dJointSetHingeParam(this->_id, dParamStopERP, StopERP);
118 dJointSetHingeParam(this->_id, dParamStopCFM, StopCFM);
119 }
120
121}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h
deleted file mode 100644
index 3115845..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h
+++ /dev/null
@@ -1,195 +0,0 @@
1#pragma once
2
3#include "Joint.h"
4#include "CommonMgd.h"
5
6namespace ODEManaged
7{
8 __gc public class JointHinge : public Joint
9 {
10 public:
11
12 //Constructors
13
14 JointHinge(void);
15 JointHinge(World &world);
16 JointHinge(World &world, JointGroup &jointGroup);
17
18
19 //Destructor
20
21 virtual~JointHinge(void);
22
23
24 //Methods
25
26 //Overloaded Create
27 void Create(World &world, JointGroup &jointGroup);
28 void Create(World &world);
29
30 //Overloaded Attach
31 void Attach(Body &body1, Body &body2);
32 void Attach(Body &body1);
33
34 void SetAnchor(double x, double y, double z);
35 Vector3 GetAnchor(void);
36
37 void SetAxis(double x, double y, double z);
38 Vector3 GetAxis(void);
39
40 void SetAllMovParams(double LoStop, double HiStop,
41 double Velocity, double MaxForce,
42 double FudgeFactor, double Bounce,
43 double StopERP, double StopCFM);
44
45
46 //Properties
47
48 //LoStop
49 __property double get_LoStop(void)
50 {
51 return dJointGetHingeParam(this->_id, dParamLoStop);
52 }
53
54 __property void set_LoStop(double value)
55 {
56 if (value > -3.141592653 && value <= 0)
57 dJointSetHingeParam(this->_id, dParamLoStop, value);
58 }
59
60
61 //HiStop
62 __property double get_HiStop(void)
63 {
64 return dJointGetHingeParam(this->_id, dParamHiStop);
65 }
66
67 __property void set_HiStop(double value)
68 {
69 if (value < 3.141592653 && value >= 0)
70 dJointSetHingeParam(this->_id, dParamHiStop, value);
71 }
72
73
74 //Velocity
75 __property double get_Velocity(void)
76 {
77 return dJointGetHingeParam(this->_id, dParamVel);
78 }
79
80 __property void set_Velocity(double value)
81 {
82 dJointSetHingeParam(this->_id, dParamVel, value);
83 }
84
85
86 //MaxForce
87 __property double get_MaxForce(void)
88 {
89 return dJointGetHingeParam(this->_id, dParamFMax);
90 }
91
92 __property void set_MaxForce(double value)
93 {
94 dJointSetHingeParam(this->_id, dParamFMax, value);
95 }
96
97
98 //FudgeFactor
99 __property double get_FudgeFactor(void)
100 {
101 return dJointGetHingeParam(this->_id, dParamFudgeFactor);
102 }
103
104 __property void set_FudgeFactor(double value)
105 {
106 dJointSetHingeParam(this->_id, dParamFudgeFactor, value);
107 }
108
109
110 //Bounce
111 __property double get_Bounce(void)
112 {
113 return dJointGetHingeParam(this->_id, dParamBounce);
114 }
115
116 __property void set_Bounce(double value)
117 {
118 dJointSetHingeParam(this->_id, dParamBounce, value);
119 }
120
121
122 //StopERP
123 __property double get_StopERP(void)
124 {
125 return dJointGetHingeParam(this->_id, dParamStopERP);
126 }
127
128 __property void set_StopERP(double value)
129 {
130 dJointSetHingeParam(this->_id, dParamStopERP, value);
131 }
132
133
134 //StopCFM
135 __property double get_StopCFM(void)
136 {
137 return dJointGetHingeParam(this->_id, dParamStopCFM);
138 }
139
140 __property void set_StopCFM(double value)
141 {
142 dJointSetHingeParam(this->_id, dParamStopCFM, value);
143 }
144
145
146 //GetAngle
147 __property double get_Angle(void)
148 {
149 return dJointGetHingeAngle(this->_id);
150 }
151
152
153 //GetAngleRate
154 __property double get_AngleRate(void)
155 {
156 return dJointGetHingeAngleRate(this->_id);
157 }
158
159 };
160
161}
162
163// void SetSuspensionERP(double value);
164// double GetSuspensionERP(void);
165
166// void SetSuspensionCFM(double value);
167// double GetSuspensionCFM(void);
168
169/*
170 //SetSuspensionERP
171 void JointHinge::SetSuspensionERP(double value)
172 {
173 dJointSetHingeParam(this->_id, dParamSuspensionERP, value);
174 }
175
176 //GetSuspensionERP
177 double JointHinge::GetSuspensionERP(void)
178 {
179 return dJointGetHingeParam(this->_id, dParamSuspensionERP);
180 }
181
182
183 //SetSuspensionCFM
184 void JointHinge::SetSuspensionCFM(double value)
185 {
186 dJointSetHingeParam(this->_id, dParamSuspensionCFM, value);
187 }
188
189 //GetSuspensionCFM
190 double JointHinge::GetSuspensionCFM(void)
191 {
192 return dJointGetHingeParam(this->_id, dParamSuspensionCFM);
193 }
194
195*/
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp
deleted file mode 100644
index 94fd7a7..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "jointhinge2.h"
5
6namespace ODEManaged
7{
8 //Constructors
9 JointHinge2::JointHinge2(void) : Joint(){}
10
11 JointHinge2::JointHinge2(World &world)
12 {
13 if(this->_id) dJointDestroy(this->_id);
14 _id = dJointCreateHinge2(world.Id(),0);
15 }
16
17 JointHinge2::JointHinge2(World &world, JointGroup &jointGroup)
18 {
19 if(this->_id) dJointDestroy(this->_id);
20 _id = dJointCreateHinge2(world.Id(), jointGroup.Id());
21 }
22
23 //Destructor
24 JointHinge2::~JointHinge2(void){}
25
26 //CreateHinge2 (overload 1)
27 void JointHinge2::Create(World &world, JointGroup &jointGroup)
28 {
29 if(this->_id) dJointDestroy(this->_id);
30 _id = dJointCreateHinge2(world.Id(), jointGroup.Id());
31 }
32
33 //CreateHinge2 (overload 2)
34 void JointHinge2::Create(World &world)
35 {
36 if(this->_id) dJointDestroy(this->_id);
37 _id = dJointCreateHinge2(world.Id(),0);
38 }
39
40 //SetAnchor1
41 void JointHinge2::SetAnchor (double x, double y ,double z)
42 {
43 dJointSetHinge2Anchor(_id, x,y,z);
44 }
45
46 //GetAnchor1
47 Vector3 JointHinge2::GetAnchor()
48 {
49 Vector3 retVal;
50 dVector3 temp;
51 dJointGetHinge2Anchor(_id,temp);
52 retVal.x = temp[0];
53 retVal.y = temp[1];
54 retVal.z = temp[2];
55 return retVal;
56 }
57
58 //SetAxis1
59 void JointHinge2::SetAxis1 (double x, double y ,double z)
60 {
61 dJointSetHinge2Axis1(_id, x,y,z);
62 }
63
64 //GetAxis1
65 Vector3 JointHinge2::GetAxis1()
66 {
67 Vector3 retVal;
68 dVector3 temp;
69 dJointGetHinge2Axis1(_id,temp);
70 retVal.x = temp[0];
71 retVal.y = temp[1];
72 retVal.z = temp[2];
73 return retVal;
74 }
75
76 //SetAxis2
77 void JointHinge2::SetAxis2 (double x, double y ,double z)
78 {
79 dJointSetHinge2Axis2(_id, x,y,z);
80 }
81
82 //GetAxis2
83 Vector3 JointHinge2::GetAxis2()
84 {
85 Vector3 retVal;
86 dVector3 temp;
87 dJointGetHinge2Axis2(_id,temp);
88 retVal.x = temp[0];
89 retVal.y = temp[1];
90 retVal.z = temp[2];
91 return retVal;
92 }
93
94 //GetAngle1
95 double JointHinge2::GetAngle1 ()
96 {
97 return dJointGetHinge2Angle1(this->_id);
98 }
99
100 //GetAngle1Rate
101 double JointHinge2::GetAngle1Rate ()
102 {
103 return dJointGetHinge2Angle1Rate(this->_id);
104 }
105
106 ////GetAngle hmm, this doesn't exist
107 //double JointHinge2::GetAngle2 ()
108 //{
109 // return dJointGetHinge2Angle2(this->_id);
110 //}
111
112 //GetAngle2Rate
113 double JointHinge2::GetAngle2Rate ()
114 {
115 return dJointGetHinge2Angle2Rate(this->_id);
116 }
117
118
119 //Attach (overload 1)
120 void JointHinge2::Attach (Body &body1, Body &body2)
121 {
122 dJointAttach(_id, body1.Id(),body2.Id());
123 }
124
125 //Attach (overload 2)
126 //TODO: possibly add an overload that takes anchor as a param also.
127 void JointHinge2::Attach (Body &body1)
128 {
129 dJointAttach(_id, body1.Id(),0);
130 }
131
132
133}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h
deleted file mode 100644
index e883ea8..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h
+++ /dev/null
@@ -1,48 +0,0 @@
1#pragma once
2
3#include "Joint.h"
4#include "CommonMgd.h"
5
6namespace ODEManaged
7{
8 __gc public class JointHinge2 : public Joint
9 {
10 public:
11
12
13 //Constructors
14
15 JointHinge2 (void);
16 JointHinge2 (World &world);
17 JointHinge2 (World &world, JointGroup &jointGroup);
18
19 //Destructors
20
21 virtual ~JointHinge2 (void);
22
23
24 //Methods
25
26 //Overloaded Hinge.Create
27 void Create (World &world, JointGroup &jointGroup);
28 void Create (World &world);
29
30 void SetAnchor (double x, double y, double z);
31 Vector3 GetAnchor (void);
32
33 void SetAxis1 (double x, double y, double z);
34 Vector3 GetAxis1 (void);
35
36 void SetAxis2 (double x, double y, double z);
37 Vector3 GetAxis2 (void);
38
39 double GetAngle1 (void);
40 double GetAngle1Rate (void);
41
42 //double GetAngle2 (void);
43 double GetAngle2Rate (void);
44
45 void Attach (Body &body1, Body &body2);
46 void Attach( Body &body1);
47 };
48}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp
deleted file mode 100644
index ab7ebd6..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp
+++ /dev/null
@@ -1,102 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "jointslider.h"
5
6namespace ODEManaged
7{
8
9 //Constructors
10
11 JointSlider::JointSlider(void) : Joint(){}
12
13
14 JointSlider::JointSlider(World &world)
15 {
16 if(this->_id) dJointDestroy(this->_id);
17 _id = dJointCreateSlider(world.Id(), 0);
18 }
19
20
21 JointSlider::JointSlider(World &world, JointGroup &jointGroup)
22 {
23 if(this->_id) dJointDestroy(this->_id);
24 _id = dJointCreateSlider(world.Id(), jointGroup.Id());
25 }
26
27
28 //Destructor
29
30 JointSlider::~JointSlider(void){}
31
32
33 //Methods
34
35 //Overloaded Create
36 void JointSlider::Create(World &world, JointGroup &jointGroup)
37 {
38 if(this->_id) dJointDestroy(this->_id);
39 _id = dJointCreateSlider(world.Id(), jointGroup.Id());
40 }
41
42 void JointSlider::Create(World &world)
43 {
44 if(this->_id) dJointDestroy(this->_id);
45 _id = dJointCreateSlider(world.Id(), 0);
46 }
47
48
49 //Overloaded Attach
50 void JointSlider::Attach(Body &body1, Body &body2)
51 {
52 dJointAttach(this->_id, body1.Id(), body2.Id());
53 }
54
55 void JointSlider::Attach(Body &body1)
56 {
57 dJointAttach(this->_id, body1.Id(), 0);
58 }
59
60
61 //SetAxis
62 void JointSlider::SetAxis(double x, double y, double z)
63 {
64 dJointSetSliderAxis(this->_id, x, y, z);
65 }
66
67 //GetAxis
68 Vector3 JointSlider::GetAxis(void)
69 {
70 Vector3 retVal;
71 dVector3 temp;
72 dJointGetSliderAxis(this->_id, temp);
73 retVal.x = temp[0];
74 retVal.y = temp[1];
75 retVal.z = temp[2];
76 return retVal;
77 }
78
79
80 //Movement Parameters
81
82 //SetAllMovParams
83 void JointSlider::SetAllMovParams(double LoStop, double HiStop,
84 double Velocity, double MaxForce,
85 double FudgeFactor, double Bounce,
86 double StopERP, double StopCFM)
87 {
88 if (LoStop <= 0)
89 dJointSetHingeParam(this->_id, dParamLoStop, LoStop);
90
91 if (HiStop >= 0)
92 dJointSetHingeParam(this->_id, dParamHiStop, HiStop);
93
94 dJointSetSliderParam(this->_id, dParamVel, Velocity);
95 dJointSetSliderParam(this->_id, dParamFMax, MaxForce);
96 dJointSetSliderParam(this->_id, dParamFudgeFactor, FudgeFactor);
97 dJointSetSliderParam(this->_id, dParamBounce, Bounce);
98 dJointSetSliderParam(this->_id, dParamStopERP, StopERP);
99 dJointSetSliderParam(this->_id, dParamStopCFM, StopCFM);
100 }
101
102}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h
deleted file mode 100644
index 7e96e59..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h
+++ /dev/null
@@ -1,158 +0,0 @@
1#pragma once
2
3#include "Joint.h"
4#include "CommonMgd.h"
5
6namespace ODEManaged
7{
8 __gc public class JointSlider : public Joint
9 {
10 public:
11
12
13 //Constructors
14
15 JointSlider(void);
16 JointSlider(World &world);
17 JointSlider(World &world, JointGroup &jointGroup);
18
19
20 //Destructors
21
22 virtual ~JointSlider(void);
23
24
25 //Methods
26
27 //Overloaded Create
28 void Create(World &world, JointGroup &jointGroup);
29 void Create(World &world);
30
31 //Overloaded Attach
32 void Attach(Body &body1, Body &body2);
33 void Attach(Body &body1);
34
35 void SetAxis(double x, double y, double z);
36 Vector3 GetAxis(void);
37
38 void SetAllMovParams(double LoStop, double HiStop,
39 double Velocity, double MaxForce,
40 double FudgeFactor, double Bounce,
41 double StopERP, double StopCFM);
42
43
44 //Properties
45
46 //LoStop
47 __property double get_LoStop(void)
48 {
49 return dJointGetSliderParam(this->_id, dParamLoStop);
50 }
51
52 __property void set_LoStop(double value)
53 {
54 if (value <=0)
55 dJointSetSliderParam(this->_id, dParamLoStop, value);
56 }
57
58
59 //HiStop
60 __property double get_HiStop(void)
61 {
62 return dJointGetSliderParam(this->_id, dParamHiStop);
63 }
64
65 __property void set_HiStop(double value)
66 {
67 if (value >= 0)
68 dJointSetSliderParam(this->_id, dParamHiStop, value);
69 }
70
71
72 //Velocity
73 __property double get_Velocity(void)
74 {
75 return dJointGetSliderParam(this->_id, dParamVel);
76 }
77
78 __property void set_Velocity(double value)
79 {
80 dJointSetSliderParam(this->_id, dParamVel, value);
81 }
82
83
84 //MaxForce
85 __property double get_MaxForce(void)
86 {
87 return dJointGetSliderParam(this->_id, dParamFMax);
88 }
89
90 __property void set_MaxForce(double value)
91 {
92 dJointSetSliderParam(this->_id, dParamFMax, value);
93 }
94
95
96 //FudgeFactor
97 __property double get_FudgeFactor(void)
98 {
99 return dJointGetSliderParam(this->_id, dParamFudgeFactor);
100 }
101
102 __property void set_FudgeFactor(double value)
103 {
104 dJointSetSliderParam(this->_id, dParamFudgeFactor, value);
105 }
106
107
108 //Bounce
109 __property double get_Bounce(void)
110 {
111 return dJointGetSliderParam(this->_id, dParamBounce);
112 }
113
114 __property void set_Bounce(double value)
115 {
116 dJointSetSliderParam(this->_id, dParamBounce, value);
117 }
118
119
120 //StopERP
121 __property double get_StopERP(void)
122 {
123 return dJointGetSliderParam(this->_id, dParamStopERP);
124 }
125
126 __property void set_StopERP(double value)
127 {
128 dJointSetSliderParam(this->_id, dParamStopERP, value);
129 }
130
131
132 //StopCFM
133 __property double get_StopCFM(void)
134 {
135 return dJointGetSliderParam(this->_id, dParamStopCFM);
136 }
137
138 __property void set_StopCFM(double value)
139 {
140 dJointSetSliderParam(this->_id, dParamStopCFM, value);
141 }
142
143
144 //GetAngle
145 __property double get_Position(void)
146 {
147 return dJointGetSliderPosition(this->_id);
148 }
149
150
151 //GetAngleRate
152 __property double get_PositionRate(void)
153 {
154 return dJointGetSliderPositionRate(this->_id);
155 }
156
157 };
158}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll b/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll
deleted file mode 100755
index ccf2a41..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll
+++ /dev/null
Binary files differ
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp
deleted file mode 100644
index c9a7e19..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "Space.h"
5#include "TEST.h"
6
7namespace ODEManaged
8{
9
10 //Constructor
11
12 Space::Space(void)
13 {
14 _id = dSimpleSpaceCreate();
15 }
16
17 Space::Space(int minlevel, int maxlevel)
18 {
19 _id = dHashSpaceCreate();
20 dHashSpaceSetLevels(this->_id, minlevel, maxlevel);
21 }
22
23
24 //Destructor
25
26 Space::~Space(void)
27 {
28 dSpaceDestroy(this->_id);
29 }
30
31
32 //Methods
33
34 //Id
35 dSpaceID Space::Id()
36 {
37 return _id;
38 }
39
40
41 //Collide
42 void Space::Collide(void *data, dNearCallback *callback)
43 {
44 dSpaceCollide(this->_id, data, callback);
45 }
46
47
48
49
50
51
52
53}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Space.h b/libraries/ode-0.9/contrib/DotNetManaged/Space.h
deleted file mode 100644
index 78e81ad..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Space.h
+++ /dev/null
@@ -1,33 +0,0 @@
1#pragma once
2
3#include "CommonMgd.h"
4
5namespace ODEManaged
6{
7 __gc public class Space
8 {
9 public:
10
11 //Constructor
12
13 Space(void);
14 Space(int minlevel, int maxlevel);
15
16 //Destructor
17
18 ~Space(void);
19
20
21 //Methods
22
23 dSpaceID Id(void);
24 void Collide(void *data, dNearCallback *callback);
25
26
27 private:
28
29 dSpaceID _id;
30
31 };
32
33}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp
deleted file mode 100644
index b6c9d98..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp
+++ /dev/null
@@ -1,5 +0,0 @@
1// stdafx.cpp : source file that includes just the standard includes
2// ODEManaged.pch will be the pre-compiled header
3// stdafx.obj will contain the pre-compiled type information
4
5#include "stdafx.h"
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h
deleted file mode 100644
index 2222759..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h
+++ /dev/null
@@ -1,12 +0,0 @@
1// stdafx.h : include file for standard system include files,
2// or project specific include files that are used frequently,
3// but are changed infrequently
4
5#pragma once
6
7#using <mscorlib.dll>
8
9
10
11
12
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/TEST.h b/libraries/ode-0.9/contrib/DotNetManaged/TEST.h
deleted file mode 100644
index e2cdbc3..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/TEST.h
+++ /dev/null
@@ -1,17 +0,0 @@
1
2#pragma once
3
4#include "CommonMgd.h"
5
6namespace ODEManaged
7{
8
9void RnearCallback(void *data, dGeomID o1, dGeomID o2)
10 {
11 }
12
13}
14
15
16
17
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/World.cpp b/libraries/ode-0.9/contrib/DotNetManaged/World.cpp
deleted file mode 100644
index beab21a..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/World.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
1#include "StdAfx.h"
2
3#include <ode/ode.h>
4#include "World.h"
5
6namespace ODEManaged
7{
8
9 //Constructor
10
11 World::World(void)
12 {
13 /*dWorldID _temp = dWorldCreate();
14 _id = _temp;*/
15 _id = dWorldCreate();
16 }
17
18
19 //Destructor
20
21 World::~World(void)
22 {
23 dWorldDestroy(this->_id);
24 }
25
26
27 //Methods
28
29 //Id
30 dWorldID World::Id()
31 {
32 return _id;
33 }
34
35
36 //SetGravity
37 void World::SetGravity(double x, double y, double z)
38 {
39 dWorldSetGravity(this->_id, x, y, z);
40 }
41
42
43 //Overloaded GetGravity
44 Vector3 World::GetGravity(void)
45 {
46 Vector3 retVal;
47 dVector3 temp;
48 dWorldGetGravity(this->_id, temp);
49 retVal.x = temp[0];
50 retVal.y = temp[1];
51 retVal.z = temp[2];
52 return retVal;
53 }
54
55 void World::GetGravity(double gravity __gc[])
56 {
57 dVector3 temp;
58 dWorldGetGravity(this->_id, temp);
59 gravity[0] = temp[0];
60 gravity[1] = temp[1];
61 gravity[2] = temp[2];
62 }
63
64
65 //Step
66 void World::Step(double stepSize)
67 {
68 dWorldStep(this->_id, stepSize);
69 }
70
71}
72
73
74
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/World.h b/libraries/ode-0.9/contrib/DotNetManaged/World.h
deleted file mode 100644
index c4c60e5..0000000
--- a/libraries/ode-0.9/contrib/DotNetManaged/World.h
+++ /dev/null
@@ -1,67 +0,0 @@
1#pragma once
2
3#include "CommonMgd.h"
4
5namespace ODEManaged
6{
7 __gc public class World
8 {
9 public:
10
11 //Constructor
12
13 World(void);
14
15
16 //Destructor
17
18 ~World(void);
19
20
21 // Methods
22
23 dWorldID Id(void);
24
25 void SetGravity(double x, double y, double z);
26
27 //Overloaded GetGravity
28 Vector3 GetGravity(void);
29 void GetGravity(double gravity __gc[]);
30
31 void Step(double stepSize);
32
33
34 //Properties
35
36 //Constraint Force Mixing
37 __property void set_CFM(double cfm)
38 {
39 dWorldSetCFM(this->_id,cfm);
40 }
41
42 __property double get_CFM(void)
43 {
44 return dWorldGetCFM(this->_id);
45 }
46
47
48 //Error Reduction Parameter
49 __property void set_ERP(double erp)
50 {
51 dWorldSetERP(this->_id,erp);
52 }
53
54 __property double get_ERP(void)
55 {
56 return dWorldGetERP(this->_id);
57 }
58
59
60 private:
61
62 dWorldID _id;
63
64 };
65
66}
67
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
deleted file mode 100644
index 26b77b0..0000000
--- a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
1
2/* ************************************************************************ */
3/*
4 grouped and transformed geometry functions
5 author: Tim Schmidt tisch@uni-paderborn.de
6*/
7
8
9#include <ode/common.h>
10#include <ode/geom.h>
11#include <ode/rotation.h>
12#include <ode/odemath.h>
13#include <ode/memory.h>
14#include <ode/misc.h>
15#include <ode/objects.h>
16#include <ode/matrix.h>
17#include <ode/GeomTransformGroup.h>
18#include "objects.h"
19#include "array.h"
20#include "geom_internal.h"
21
22// given a pointer `p' to a dContactGeom, return the dContactGeom at
23// p + skip bytes.
24
25#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
26
27
28// ############################################################################
29
30int dGeomTransformGroupClass = -1;
31// ############################################################################
32
33struct dxGeomTransformGroup {
34 dArray<dxGeom*> parts; // all the geoms that make up the group
35 dVector3 relativePosition;
36 dMatrix3 relativeRotation;
37};
38// ############################################################################
39
40void dGeomTransformGroupSetRelativePosition (dxGeom *g, dReal x, dReal y, dReal z)
41{
42 dAASSERT (g);
43 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
44 transformGroup->relativePosition[0] = x;
45 transformGroup->relativePosition[1] = y;
46 transformGroup->relativePosition[2] = z;
47}
48// ############################################################################
49
50void dGeomTransformGroupSetRelativeRotation (dxGeom *g, const dMatrix3 R)
51{
52 dAASSERT (g);
53 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
54 memcpy (transformGroup->relativeRotation,R,sizeof(dMatrix3));
55}
56// ############################################################################
57
58const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g)
59{
60 dAASSERT (g);
61 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
62 return transformGroup->relativePosition;
63}
64// ############################################################################
65
66const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g)
67{
68 dAASSERT (g);
69 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
70 return transformGroup->relativeRotation;
71}
72// ############################################################################
73
74static void computeFinalTransformation (const dxGeom *tg, const dxGeom *part)
75{
76 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(tg);
77 dMULTIPLY0_331 (part->pos,tg->R,transformGroup->relativePosition);
78 part->pos[0] += tg->pos[0];
79 part->pos[1] += tg->pos[1];
80 part->pos[2] += tg->pos[2];
81 dMULTIPLY0_333 (part->R,tg->R,transformGroup->relativeRotation);
82}
83// ############################################################################
84
85int dCollideTransformGroup (const dxGeom *o1, const dxGeom *o2, int flags,
86 dContactGeom *contact, int skip)
87{
88 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(o1);
89 if (transformGroup->parts.size() == 0)
90 {
91 return 0;
92 }
93 int numleft = flags & NUMC_MASK;
94 if (numleft == 0) numleft = 1;
95 flags &= ~NUMC_MASK;
96 int num=0, i=0;
97 while (i < transformGroup->parts.size() && numleft > 0)
98 {
99 dUASSERT (transformGroup->parts[i]->spaceid==0,
100 "GeomTransformGroup encapsulated object must not be in a space");
101 dUASSERT (transformGroup->parts[i]->body==0,
102 "GeomTransformGroup encapsulated object must not be attached to a body");
103 if (!o1->space_aabb)
104 {
105 computeFinalTransformation (o1, transformGroup->parts[i]);
106 }
107 dxBody *bodyBackup = transformGroup->parts[i]->body;
108 transformGroup->parts[i]->body = o1->body;
109 int n = dCollide (transformGroup->parts[i],const_cast<dxGeom*>(o2),
110 flags | numleft,contact,skip);
111 transformGroup->parts[i]->body = bodyBackup;
112 contact = CONTACT (contact,skip*n);
113 numleft -= n;
114 num += n;
115 i++;
116 }
117 return num;
118}
119// ############################################################################
120
121static dColliderFn * dGeomTransformGroupColliderFn (int num)
122{
123 return (dColliderFn *) &dCollideTransformGroup;
124}
125// ############################################################################
126
127static void dGeomTransformGroupAABB (dxGeom *geom, dReal aabb[6])
128{
129 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom);
130 aabb[0] = dInfinity;
131 aabb[1] = -dInfinity;
132 aabb[2] = dInfinity;
133 aabb[3] = -dInfinity;
134 aabb[4] = dInfinity;
135 aabb[5] = -dInfinity;
136 int i,j;
137 for (i=0; i < transformGroup->parts.size(); i++)
138 {
139 computeFinalTransformation (geom, transformGroup->parts[i]);
140 dReal aabb2[6];
141 transformGroup->parts[i]->_class->aabb (transformGroup->parts[i],aabb2);
142 for (j=0; j<6; j += 2) if (aabb2[j] < aabb[j]) aabb[j] = aabb2[j];
143 for (j=1; j<6; j += 2) if (aabb2[j] > aabb[j]) aabb[j] = aabb2[j];
144 }
145}
146// ############################################################################
147
148static void dGeomTransformGroupDtor (dxGeom *geom)
149{
150 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom);
151 transformGroup->parts.~dArray();
152}
153// ############################################################################
154
155dxGeom *dCreateGeomTransformGroup (dSpaceID space)
156{
157 if (dGeomTransformGroupClass == -1) {
158 dGeomClass c;
159 c.bytes = sizeof (dxGeomTransformGroup);
160 c.collider = &dGeomTransformGroupColliderFn;
161 c.aabb = &dGeomTransformGroupAABB;
162 c.aabb_test = 0;
163 c.dtor = dGeomTransformGroupDtor;
164 dGeomTransformGroupClass = dCreateGeomClass (&c);
165 }
166 dxGeom *g = dCreateGeom (dGeomTransformGroupClass);
167 if (space)
168 {
169 dSpaceAdd (space,g);
170 }
171 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
172 transformGroup->parts.constructor();
173 dSetZero (transformGroup->relativePosition,4);
174 dRSetIdentity (transformGroup->relativeRotation);
175 return g;
176}
177// ############################################################################
178
179void dGeomTransformGroupAddGeom (dxGeom *g, dxGeom *obj)
180{
181 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
182 "argument not a geom TransformGroup");
183 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
184 transformGroup->parts.push (obj);
185}
186// ############################################################################
187
188void dGeomTransformGroupRemoveGeom (dxGeom *g, dxGeom *obj)
189{
190 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
191 "argument not a geom TransformGroup");
192 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
193 for (int i=0; i < transformGroup->parts.size(); i++) {
194 if (transformGroup->parts[i] == obj) {
195 transformGroup->parts.remove (i);
196 return;
197 }
198 }
199}
200// ############################################################################
201
202dxGeom * dGeomTransformGroupGetGeom (dxGeom *g, int i)
203{
204 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
205 "argument not a geom TransformGroup");
206 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
207 dAASSERT (i >= 0 && i < transformGroup->parts.size());
208 return transformGroup->parts[i];
209}
210// ############################################################################
211
212int dGeomTransformGroupGetNumGeoms (dxGeom *g)
213{
214 dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
215 "argument not a geom TransformGroup");
216 dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
217 return transformGroup->parts.size();
218}
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h
deleted file mode 100644
index 705fdb9..0000000
--- a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h
+++ /dev/null
@@ -1,29 +0,0 @@
1
2/* ************************************************************************ */
3/*
4 grouped and transformed geometry functions
5 author: Tim Schmidt tisch@uni-paderborn.de
6*/
7
8
9#ifdef __cplusplus
10extern "C" {
11#endif
12
13
14extern int dGeomTransformGroupClass;
15
16void dGeomTransformGroupSetRelativePosition (dGeomID g, dReal x, dReal y, dReal z);
17void dGeomTransformGroupSetRelativeRotation (dGeomID g, const dMatrix3 R);
18const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g);
19const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g);
20dGeomID dCreateGeomTransformGroup (dSpaceID space);
21void dGeomTransformGroupAddGeom (dGeomID tg, dGeomID obj);
22void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj);
23dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i);
24int dGeomTransformGroupGetNumGeoms (dGeomID tg);
25
26
27#ifdef __cplusplus
28}
29#endif
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt b/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt
deleted file mode 100644
index bca0e66..0000000
--- a/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt
+++ /dev/null
@@ -1,148 +0,0 @@
1README for GeomTransformGroup by Tim Schmidt.
2---------------------------------------------
3
4This is a patch to add the dGeomTransformGroup object to the list of geometry
5objects.
6
7It should work with the cvs version of the ode library from 07/24/2002.
8
9++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10
11comment by russ smith: this code is easy to use with the rest of ODE.
12simply copy GeomTransformGroup.cpp to ode/src and copy GeomTransformGroup.h
13to include/ode. then add GeomTransformGroup.cpp to the ODE_SRC variable
14in the makefile. rebuild, and you're done! of course i could have done all
15this for you, but i prefer to keep GeomTransformGroup separated from the
16rest of ODE for now while other issues with the collision system are
17resolved.
18
19++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
21
22Description:
23
24The dGeomTransformGroup is an adaption of the TransformGroup known from
25Java3D (and maybe other libraries with a similar scene graph representation).
26It can be used to build an arbitrarily structured tree of objects that are
27each positioned relative to the particular parent node.
28
29If you have a plane for example, there is one root node associated with the
30plane's body and another three transformgroups placed 'under' this node. One
31with the fuselage (cappedcylinder) under it and two with the underlying wings
32(flat boxes). And if you want to add engines, simply put them 'next to' the
33wings under another two transformgroups.
34
35bodyTG ---> associated with dBody
36 |
37 +--fuselageTG
38 | |
39 | +--fuselageCylinder
40 |
41 +--leftwingTG
42 | |
43 | +--wingBox
44 | |
45 | +--leftengineTG
46 | |
47 | +--leftengineCylinder
48 |
49 +--rightwingTG
50 |
51 +--wingBox
52 |
53 +--rightengineTG
54 |
55 +--rightengineCylinder
56
57This is a method to easily compose objects without the necessity of always
58calculating global coordinates. But apart from this there is something else
59that makes dGeomTransformGroups very valuable.
60
61Maybe you remember that some users reported the problem of acquiring the
62correct bodies to be attached by a contactjoint in the nearCallback when
63using dGeomGroups and dGeomTransforms at the same time. This results from the
64fact that dGeomGroups are not associated with bodies while all other
65geometries are.
66
67So, as you can see in the nearCallback of the the test_buggy demo you have to
68attach the contactjoint with the bodies that you get from the geometries that
69are stored in the contact struct (-> dGeomGetBody(contacts[i].geom.g1)).
70Normally you would do this by asking o1 and o2 directly with dGeomGetBody(o1)
71and dGeomGetBody(o2) respectively.
72
73As a first approach you can overcome that problem by testing o1 and o2 if
74they are groups or not to find out how to get the corresponding bodies.
75
76However this will fail if you want grouped transforms that are constructed
77out of dGeomTransforms encapsulated in a dGeomGroup. According to the test
78you use contacts[i].geom.g1 to get the right body. Unfortunately g1 is
79encapsulated in a transform and therefore not attached to any body. In this
80case the dGeomTransform 'in the middle' would have been the right object to
81be asked for the body.
82
83You may now conclude that it is a good idea to unwrap the group encapsulated
84geoms at the beginning of the nearcallback and use dGeomGetBody(o1)
85consistently. But keep in mind that this also means not to invoke
86dCollide(..) on groups at all and therefore not to expoit the capability of
87dGeomGroups to speed up collision detection by the creation of bounding boxes
88around the encapsulated geometry.
89
90Everything becomes even worse if you create a dGeomTransform that contains a
91dGeomGroup of geoms. The function that cares about the collision of
92transforms with other objects uses the position and rotation of the
93respective encapsulated object to compute its final position and orientation.
94Unfortunately dGeomGroups do not have a position and rotation, so the result
95will not be what you have expected.
96
97Here the dGeomTransformGroups comes into operation, because it combines the
98advantages and capabilities of the dGeomGroup and the dGeomTransform.
99And as an effect of synergy it is now even possible to set the position of a
100group of geoms with one single command.
101Even nested encapsulations of dGeomTransformGroups in dGeomTransformGroups
102should be possible (to be honest, I have not tried that so far ;-) ).
103
104++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
105
106API:
107
108dGeomID dCreateGeomTransformGroup (dSpaceID space);
109 - create a GeomTransformGroup
110
111void dGeomTransformGroupAddGeom (dGeomID tg, dGeomID obj);
112 - Comparable to dGeomTransformSetGeom or dGeomGroupAdd
113 - add objects to this group
114
115void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj);
116 - remove objects from this group
117
118void dGeomTransformGroupSetRelativePosition
119 (dGeomID g, dReal x, dReal y, dReal z);
120void dGeomTransformGroupSetRelativeRotation
121 (dGeomID g, const dMatrix3 R);
122 - Comparable to setting the position and rotation of all the
123 dGeomTransform encapsulated geometry. The difference
124 is that it is global with respect to this group and therefore
125 affects all geoms in this group.
126 - The relative position and rotation are attributes of the
127 transformgroup, so the position and rotation of the individual
128 geoms are not changed
129
130const dReal * dGeomTransformGroupGetRelativePosition (dGeomID g);
131const dReal * dGeomTransformGroupGetRelativeRotation (dGeomID g);
132 - get the relative position and rotation
133
134dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i);
135 - Comparable to dGeomGroupGetGeom
136 - get a specific geom of the group
137
138int dGeomTransformGroupGetNumGeoms (dGeomID tg);
139 - Comparable to dGeomGroupGetNumGeoms
140 - get the number of geoms in the group
141
142
143++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
144
145Tim Schmidt
146student of computer science
147University of Paderborn, Germany
148tisch@uni-paderborn.de
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin b/libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin
deleted file mode 100644
index c06768c..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin
+++ /dev/null
Binary files differ
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt b/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt
deleted file mode 100644
index 7c1f8a4..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt
+++ /dev/null
@@ -1,95 +0,0 @@
1-----------------------------
2ODE - Mac CFM Carbon Port
3(contact Frank Condello <pox@planetquake.com> with questions regarding this port)
4
5Although ODE contains a MacOSX makefile, and some individuals have implemented ODE in
6Cocoa, I opted to use (and prefer) CodeWarrior. This also opens up ODE to MacOS8 & 9
7users, without scarfing functionality in MacOSX (same binaries run on both platforms).
8
9The 'ode_CW7.mcp' project contains release and debug targets to create static ODE and
10DrawStuff libraries.
11
12'examples_CW7.mcp' contains targets for the entire ODE test suite, plus a couple other
13test programs which were posted to the ODE mailing list.
14
15
16-----------------------------
17Compiling Notes:
18
19You'll need to extract the CodeWarrior projects from the 'CW7_projects.sit.bin' archive
20(They're nearly a meg uncompressed so this was done to be bandwith friendly on the CVS).
21
22Projects require CodeWarrior 7 or above (recreating them with earlier versions shouldn't
23be too difficult). The projects use relative paths and are meant to be compiled from
24'contrib/Mac_CFMCarbon/'. Don't move them!
25
26All the libraries build into the 'lib/' directory, all test applications build into
27'contrib/Mac_CFMCarbon/mac_testbin/' (and must be run from that directory since the
28texture path is hard-coded).
29
30You'll need to compile the release ODE library, and the DrawStuff library before
31compiling the examples.
32
33The ODE 'configurator' has not been ported, but a Mac-friendly 'config.h' header has been
34manually hacked together (all PPC Macs should be fine with this header). Single or double
35precision can be defined in the 'CommonPrefix.h' header found in
36'contrib/Mac_CFMCarbon/mac_source/'.
37
38'contrib/Mac_CFMCarbon/mac_source/' also contains any mac specific additions to the main source.
39The directory structure here matches the main source tree, and I would recommend that this
40format is maintained when making additions, since the access paths are touchy (more below...)
41
42Some issues were encountered with duplicate header names. CodeWarrior tends to be
43unforgiving about this sort of thing but fudging with the access paths eventually
44cleared up the problem. If ODE fails to compile, make sure the <ode/objects.h> and
45"objects.h" or <timer.h> and <Timer.h> are actually pointing to the correct header.
46
47You'll need Apple's OpenGL SDK (with GLUT) in your compiler path to build DrawStuff. I've
48added redirection headers in 'contrib/Mac_CFMCarbon/mac_source/include/GL/' to properly
49link with the Apple headers (since the projects are set to follow DOS paths).
50
51The examples link against a crapload of static libraries, but my initial builds using
52ODE, MSL, GLUT, and DrawStuff shared/merged DLL's proved unstable (mostly problems with
53SIOUX spawning multiple sessions, and crashes in Classic). Static libs just worked better
54in the end, but the test apps are a little bloated as a result, and need to be re-linked
55whenever a change to a library is made.
56
57IMPORTANT: You must use the same 'CommonPrefix.h' settings for libraries, and test apps
58(i.e. double or single precision).
59
60
61-----------------------------
62Running the test apps:
63
64The test apps will show the SIOUX CLI prompt when run. Just hit OK to ignore it, or add any
65DrawStuff arguments. You'll want to log output to a file for 'test_ode'.
66
67There are two extra test programs in the 'mac_source' directory. Both were posted to the ODE
68mailing list by OSX users. 'test_stability1' visualizes some internal issues with ODE, and
69'test_stacktest' is a standalone GLUT program (doesn't use DrawStuff) that can be useful
70to stress test the library, and give you an idea of just how much stack memory you're
71going to need for large systems.
72
73ISSUES:
74
75The carbon DrawStuff lib uses GLUT to make life easy, but GLUT isn't exactly bug-free
76or stable on the Mac... Try moving the mouse around if a simulation is running slowly
77on OS9 (it's not ODE's fault, but rather a poor carbon GLUT implementation - seems GLUT stalls
78when it's not getting system events - I haven't seen this problem on OSX).
79
80The 3D view may not update if typing in the SIOUX console window.
81
82You cannot pass startup args to GLUT due to the way the DrawStuff library initializes.
83
84'Write Frames' doesn't actually do anything at the moment.
85
86The 'test_joints' app seems broken (though I don't know what the intended effect should be)
87
88
89-----------------------------
90TODO:
91
92- Re-add shared library targets (if stability issues are resolved).
93- Implement 'Write Frames' in DrawStuff.
94- Write a Carbon compatible configurator
95- Create CodeWarrior 8 projects (once I scrounge up enough dough for the update). \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h
deleted file mode 100644
index 5948b3e..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h
+++ /dev/null
@@ -1,6 +0,0 @@
1#define TARGET_API_MAC_CARBON 1
2#define finite isfinite
3#define dNODEBUG 1
4
5// Comment out for single precision
6#define PRECISION_DOUBLE 1 \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h
deleted file mode 100644
index 6122528..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h
+++ /dev/null
@@ -1,6 +0,0 @@
1#ifndef prefix_h
2#define prefix_h
3
4#include "CommonPrefix.h"
5
6#endif // prefix_h \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h
deleted file mode 100644
index 0e328a9..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h
+++ /dev/null
@@ -1,10 +0,0 @@
1#ifndef prefix_h
2#define prefix_h
3
4#include "CommonPrefix.h"
5
6#ifdef dNODEBUG
7#undef dNODEBUG
8#endif
9
10#endif // prefix_h \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h
deleted file mode 100644
index 1dedfc9..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h
+++ /dev/null
@@ -1,13 +0,0 @@
1#ifndef prefix_h
2#define prefix_h
3
4#include "CommonPrefix.h"
5
6// Hack to automatically call SIOUX's CLI interface for the test apps
7#include <console.h>
8#include <SIOUX.h>
9int fmain (int argc, char **argv);
10int main (int argc, char **argv) { argc = ccommand(&argv); return fmain(argc, argv); }
11#define main(argc, argv) fmain(argc, argv)
12
13#endif // prefix_h \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h
deleted file mode 100644
index f8f5022..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h
+++ /dev/null
@@ -1,13 +0,0 @@
1#ifndef prefix_h
2#define prefix_h
3
4#include "CommonPrefix.h"
5
6// Hack to automatically call SIOUX's CLI interface for the test apps
7#include <console.h>
8#include <SIOUX.h>
9int fmain ();
10int main (int argc, char **argv) { argc = ccommand(&argv); return fmain(); }
11#define main() fmain()
12
13#endif // prefix_h \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h
deleted file mode 100644
index 6122528..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h
+++ /dev/null
@@ -1,6 +0,0 @@
1#ifndef prefix_h
2#define prefix_h
3
4#include "CommonPrefix.h"
5
6#endif // prefix_h \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp
deleted file mode 100644
index eb0b144..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp
+++ /dev/null
@@ -1,281 +0,0 @@
1/*************************************************************************
2 * *
3 * DrawStuff Library, Copyright (C) 2001 Russell L. Smith. *
4 * 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 the GNU Lesser General Public *
8 * License as published by the Free Software Foundation; either *
9 * version 2.1 of the License, or (at your option) any later version. *
10 * *
11 * This library is distributed in the hope that it will be useful, *
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
14 * Lesser General Public License for more details. *
15 * *
16 * You should have received a copy of the GNU Lesser General Public *
17 * License along with this library (see the file LICENSE.TXT); if not, *
18 * write to the Free Software Foundation, Inc., 59 Temple Place, *
19 * Suite 330, Boston, MA 02111-1307 USA. *
20 * *
21 *************************************************************************/
22
23// main window and event handling for Mac CFM Carbon
24
25#include <ode/config.h>
26#include <stdlib.h>
27#include <string.h>
28#include <stdarg.h>
29#include <stdio.h>
30#include <glut.h>
31#include <SIOUX.h>
32
33#include <MacTypes.h>
34#include <Timer.h>
35
36#include <drawstuff/drawstuff.h>
37#include <drawstuff/version.h>
38#include "internal.h"
39
40
41//***************************************************************************
42// error handling for unix (works just fine with SIOUX)
43
44static void printMessage (char *msg1, char *msg2, va_list ap)
45{
46 fflush (stderr);
47 fflush (stdout);
48 fprintf (stderr,"\n%s: ",msg1);
49 vfprintf (stderr,msg2,ap);
50 fprintf (stderr,"\n");
51 fflush (stderr);
52}
53
54extern "C" void dsError (char *msg, ...)
55{
56 va_list ap;
57 va_start (ap,msg);
58 printMessage ("Error",msg,ap);
59 exit (1);
60}
61
62
63extern "C" void dsDebug (char *msg, ...)
64{
65 va_list ap;
66 va_start (ap,msg);
67 printMessage ("INTERNAL ERROR",msg,ap);
68 // *((char *)0) = 0; ... commit SEGVicide ?
69 abort();
70}
71
72extern "C" void dsPrint (char *msg, ...)
73{
74 va_list ap;
75 va_start (ap,msg);
76 vprintf (msg,ap);
77}
78
79//***************************************************************************
80// openGL window
81
82// window and openGL
83static int width=0,height=0; // window size
84static int last_key_pressed=0; // last key pressed in the window
85static int pause=0; // 1 if in `pause' mode
86static int singlestep=0; // 1 if single step key pressed
87static int writeframes=0; // 1 if frame files to be written
88static dsFunctions *gfn;
89static int frame = 1;
90
91float getTime (void)
92{
93 UnsignedWide ms;
94
95 Microseconds(&ms);
96 return ms.lo / 1000000.0;
97}
98
99
100static void captureFrame (int num)
101{
102// TODO
103}
104
105static void reshape(int w, int h)
106{
107 width = w;
108 height = h;
109}
110
111static void draw(void)
112{
113 dsDrawFrame (width,height,gfn,pause && !singlestep);
114 singlestep = 0;
115 glutSwapBuffers();
116
117 if (pause==0 && writeframes) {
118 captureFrame (frame);
119 frame++;
120 }
121}
122
123static void idle(void)
124{
125 static float lasttime=0;
126 float t;
127
128 // Try to maintain a reasonable rate (good enough for testing anyway)
129 t = getTime();
130 if (lasttime < t) {
131 lasttime = t+0.005;
132 draw();
133 }
134}
135
136static void key(unsigned char key, int x, int y)
137{
138 if (!glutGetModifiers()) {
139
140 if (key >= ' ' && key <= 126 && gfn->command) gfn->command (key);
141
142 // GLUT_ACTIVE_CTRL doesn't seem to be working, so we use Alt
143 } else if (glutGetModifiers()&GLUT_ACTIVE_ALT) {
144
145 switch (key) {
146 case 't': case 'T':
147 dsSetTextures (dsGetTextures() ^ 1);
148 break;
149 case 's': case 'S':
150 dsSetShadows (dsGetShadows() ^ 1);
151 break;
152 case 'p': case 'P':
153 pause ^= 1;
154 singlestep = 0;
155 break;
156 case 'o': case 'O':
157 if (pause) singlestep = 1;
158 break;
159 case 'v': case 'V': {
160 float xyz[3],hpr[3];
161 dsGetViewpoint (xyz,hpr);
162 printf ("Viewpoint = (%.4f,%.4f,%.4f,%.4f,%.4f,%.4f)\n",
163 xyz[0],xyz[1],xyz[2],hpr[0],hpr[1],hpr[2]);
164 break;
165 }
166 // No case 'X' - Quit works through the Mac system menu, or cmd-q
167 case 'w': case 'W':
168 writeframes ^= 1;
169 if (writeframes) printf ("Write frames not done yet!\n");// TODO
170 break;
171 }
172 }
173
174 last_key_pressed = key;
175}
176
177static int mx=0,my=0; // mouse position
178static int mode = 0; // mouse button bits
179
180static void MouseDown(int button, int state, int x, int y)
181{
182 if(button == GLUT_LEFT_BUTTON)
183 {
184 if(state == GLUT_DOWN)
185 mode |= 1;
186 else if(state == GLUT_UP)
187 mode &= (~1);
188 }
189 else if (button == GLUT_MIDDLE_BUTTON)
190 {
191 if(state == GLUT_DOWN)
192 mode |= 3;
193 else if(state == GLUT_UP)
194 mode &= (~3);
195 }
196 else if (button == GLUT_RIGHT_BUTTON)
197 {
198 if(state == GLUT_DOWN)
199 mode |= 2;
200 else if(state == GLUT_UP)
201 mode &= (~2);
202 }
203
204 mx = x;
205 my = y;
206}
207
208static void MouseMove(int x, int y)
209{
210 dsMotion (mode, x - mx, y - my);
211 mx = x;
212 my = y;
213}
214
215static void createMainWindow (int _width, int _height)
216{
217 // So GLUT doesn't complain
218 int argc = 0;
219 char **argv = NULL;
220
221 // initialize variables
222 width = _width;
223 height = _height;
224 last_key_pressed = 0;
225
226 if (width < 1 || height < 1) dsDebug (0,"bad window width or height");
227
228 glutInit(&argc, argv);
229 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
230 glutInitWindowSize(_width, _height);
231 glutInitWindowPosition(100, 100);
232 glutCreateWindow("ODE Simulation");
233
234 glutKeyboardFunc(key);
235 glutMotionFunc(MouseMove);
236 glutMouseFunc(MouseDown);
237 glutReshapeFunc(reshape);
238 glutDisplayFunc(idle);
239 glutIdleFunc(idle);
240}
241
242void dsPlatformSimLoop (int window_width, int window_height, dsFunctions *fn,
243 int initial_pause)
244{
245 SIOUXSettings.initializeTB = false;
246 SIOUXSettings.standalone = false;
247 SIOUXSettings.setupmenus = false;
248 SIOUXSettings.autocloseonquit = true;
249 SIOUXSettings.asktosaveonclose = false;
250
251 gfn = fn;
252 pause = initial_pause;
253
254 printf (
255 "\n"
256 "Simulation test environment v%d.%02d\n"
257 " Option-P : pause / unpause (or say `-pause' on command line).\n"
258 " Option-O : single step when paused.\n"
259 " Option-T : toggle textures (or say `-notex' on command line).\n"
260 " Option-S : toggle shadows (or say `-noshadow' on command line).\n"
261 " Option-V : print current viewpoint coordinates (x,y,z,h,p,r).\n"
262 " Option-W : write frames to ppm files: frame/frameNNN.ppm\n"
263 "\n"
264 "Change the camera position by clicking + dragging in the window.\n"
265 " Left button - pan and tilt.\n"
266 " Right button - forward and sideways.\n"
267 " Left + Right button (or middle button) - sideways and up.\n"
268 "\n",DS_VERSION >> 8,DS_VERSION & 0xff);
269
270 createMainWindow (window_width, window_height);
271 dsStartGraphics (window_width,window_height,fn);
272
273 if (fn->start) fn->start();
274
275 glutMainLoop();
276
277 if (fn->stop) fn->stop();
278 dsStopGraphics();
279}
280
281extern "C" void dsStop(){ }// GLUT/MSL hooks into the system to exit
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h
deleted file mode 100644
index 4acaeed..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h
+++ /dev/null
@@ -1,2 +0,0 @@
1// A little hackaround (Apple use / in the FILENAME, which doesn't work when following DOS paths)
2#include <gl.h> \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h
deleted file mode 100644
index 5b4a791..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h
+++ /dev/null
@@ -1,2 +0,0 @@
1// A little hackaround (Apple use / in the FILENAME, which doesn't work when following DOS paths)
2#include <glu.h> \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h
deleted file mode 100644
index bb889f9..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h
+++ /dev/null
@@ -1,52 +0,0 @@
1/* This file has been manually hacked together for the Mac CFM Carbon build - Frank. */
2
3#ifndef _ODE_CONFIG_H_
4#define _ODE_CONFIG_H_
5
6/* standard system headers */
7#include <stdio.h>
8#include <stdlib.h>
9#include <math.h>
10#include <string.h>
11#include <stdarg.h>
12#include <malloc.h>
13#include <alloca.h>
14#include <float.h>
15
16#ifdef __cplusplus
17extern "C" {
18#endif
19
20/* #define PENTIUM 1 -- not a pentium */
21
22/* integer types (we assume int >= 32 bits) */
23typedef char int8;
24typedef unsigned char uint8;
25typedef int int32;
26typedef unsigned int uint32;
27
28/* an integer type that we can safely cast a pointer to and from without loss of bits. */
29typedef unsigned int intP;
30
31#ifdef PRECISION_DOUBLE
32
33 /*select the base floating point type*/
34 #define dDOUBLE 1
35
36 /* the floating point infinity */
37 #define dInfinity DBL_MAX
38
39#else
40
41 /* select the base floating point type */
42 #define dSINGLE 1
43
44 /* the floating point infinity */
45 #define dInfinity FLT_MAX
46
47#endif
48
49#ifdef __cplusplus
50}
51#endif
52#endif \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp
deleted file mode 100644
index 79c066a..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp
+++ /dev/null
@@ -1,289 +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 <ode/ode.h>
24#include <drawstuff/drawstuff.h>
25
26#ifdef _MSC_VER
27#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
28#endif
29
30// select correct drawing functions
31
32#ifdef dDOUBLE
33#define dsDrawBox dsDrawBoxD
34#define dsDrawSphere dsDrawSphereD
35#define dsDrawCylinder dsDrawCylinderD
36#define dsDrawCappedCylinder dsDrawCappedCylinderD
37#endif
38
39
40// some constants
41
42#define DENSITY (5.0) // density of all objects
43
44// dynamics and collision objects
45
46struct MyObject {
47 dBodyID body; // the body
48 dGeomID geom; // geometry representing this body
49};
50
51static dWorldID world;
52static dSpaceID space;
53static MyObject fallingObject;
54static dGeomID box1, box2;
55static dJointGroupID contactgroup;
56
57
58// this is called by dSpaceCollide when two objects in space are
59// potentially colliding.
60
61static void nearCallback (void *data, dGeomID o1, dGeomID o2)
62{
63 int i;
64 // if (o1->body && o2->body) return;
65
66 // exit without doing anything if the two bodies are connected by a joint
67 dBodyID b1 = dGeomGetBody(o1);
68 dBodyID b2 = dGeomGetBody(o2);
69 if (b1 && b2 && dAreConnected (b1,b2)) return;
70
71 dContact contact[4]; // up to 3 contacts per box
72 for (i=0; i<4; i++) {
73 contact[i].surface.mode = dContactBounce; //dContactMu2;
74 contact[i].surface.mu = dInfinity;
75 contact[i].surface.mu2 = 0;
76 contact[i].surface.bounce = 0.5;
77 contact[i].surface.bounce_vel = 0.1;
78 }
79 if (int numc = dCollide (o1,o2,4,&contact[0].geom,sizeof(dContact))) {
80 // dMatrix3 RI;
81 // dRSetIdentity (RI);
82 // const dReal ss[3] = {0.02,0.02,0.02};
83 for (i=0; i<numc; i++) {
84 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
85 dJointAttach (c,b1,b2);
86 // dsDrawBox (contact[i].geom.pos,RI,ss);
87 }
88 }
89}
90
91
92// start simulation - set viewpoint
93
94static void start()
95{
96 static float xyz[3] = {-4.0f, 0.0f, 3.0f};
97 static float hpr[3] = {0.0f,-15.0f,0.0f};
98 dsSetViewpoint (xyz,hpr);
99 printf ("To drop another object, press:\n");
100 printf (" b for box.\n");
101 printf (" s for sphere.\n");
102 printf (" c for cylinder.\n");
103 printf ("To select an object, press space.\n");
104}
105
106
107char locase (char c)
108{
109 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
110 else return c;
111}
112
113
114// called when a key pressed
115
116static void command (int cmd)
117{
118 int i,k;
119 dReal sides[3];
120 dMass m;
121
122 cmd = locase (cmd);
123 if (cmd == 'b' || cmd == 's' || cmd == 'c') {
124 // Destroy the currently falling object and replace it by an instance of the requested type
125 if (fallingObject.body) {
126 dBodyDestroy (fallingObject.body);
127 dGeomDestroy (fallingObject.geom);
128 memset (&fallingObject, 0, sizeof(fallingObject));
129 }
130
131 fallingObject.body = dBodyCreate (world);
132 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
133
134 // Start out centered above the V-gap
135 dBodySetPosition (fallingObject.body, 0,0,5);
136
137#if 0
138 dMatrix3 R;
139 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
140 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
141 dBodySetRotation (fallingObject.body,R);
142 dBodySetData (fallingObject.body,(void*) i);
143#endif
144
145 if (cmd == 'b') {
146 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
147 fallingObject.geom = dCreateBox (space,sides[0],sides[1],sides[2]);
148 }
149 else if (cmd == 'c') {
150 sides[0] *= 0.5;
151 dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
152 fallingObject.geom = dCreateCCylinder (space,sides[0],sides[1]);
153 }
154 else if (cmd == 's') {
155 sides[0] *= 0.5;
156 dMassSetSphere (&m,DENSITY,sides[0]);
157 fallingObject.geom = dCreateSphere (space,sides[0]);
158 }
159
160 dGeomSetBody (fallingObject.geom,fallingObject.body);
161
162 dBodySetMass (fallingObject.body,&m);
163 }
164}
165
166
167// draw a geom
168
169void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
170{
171 if (!g) return;
172 if (!pos) pos = dGeomGetPosition (g);
173 if (!R) R = dGeomGetRotation (g);
174
175 int type = dGeomGetClass (g);
176 if (type == dBoxClass) {
177 dVector3 sides;
178 dGeomBoxGetLengths (g,sides);
179 dsDrawBox (pos,R,sides);
180 }
181 else if (type == dSphereClass) {
182 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
183 }
184 else if (type == dCCylinderClass) {
185 dReal radius,length;
186 dGeomCCylinderGetParams (g,&radius,&length);
187 dsDrawCappedCylinder (pos,R,length,radius);
188 }
189 /*
190 else if (type == dGeomTransformClass) {
191 dGeomID g2 = dGeomTransformGetGeom (g);
192 const dReal *pos2 = dGeomGetPosition (g2);
193 const dReal *R2 = dGeomGetRotation (g2);
194 dVector3 actual_pos;
195 dMatrix3 actual_R;
196 dMULTIPLY0_331 (actual_pos,R,pos2);
197 actual_pos[0] += pos[0];
198 actual_pos[1] += pos[1];
199 actual_pos[2] += pos[2];
200 dMULTIPLY0_333 (actual_R,R,R2);
201 drawGeom (g2,actual_pos,actual_R);
202 }
203 */
204}
205
206
207// simulation loop
208
209static void simLoop (int pause)
210{
211 dsSetColor (0,0,2);
212 dSpaceCollide (space,0,&nearCallback);
213 if (!pause) dWorldStep (world,0.0005);
214
215 // remove all contact joints
216 dJointGroupEmpty (contactgroup);
217
218 dsSetColor (1,1,0);
219 dsSetTexture (DS_WOOD);
220
221 // draw the falling object
222 dsSetColor (1,0,0);
223 drawGeom (fallingObject.geom,0,0);
224
225 // draw the constraining boxes
226 dsSetColor(0.8, 1, 0.8);
227 drawGeom (box1,0,0);
228 drawGeom (box2,0,0);
229}
230
231
232int main (int argc, char **argv)
233{
234 // setup pointers to drawstuff callback functions
235 dsFunctions fn;
236 fn.version = DS_VERSION;
237 fn.start = &start;
238 fn.step = &simLoop;
239 fn.command = &command;
240 fn.stop = 0;
241 fn.path_to_textures = "../../drawstuff/textures";
242 if(argc==2)
243 {
244 fn.path_to_textures = argv[1];
245 }
246
247 // create world
248
249 world = dWorldCreate();
250 space = dHashSpaceCreate();
251 contactgroup = dJointGroupCreate (0);
252 dWorldSetGravity (world,0,0,-0.5);
253 dWorldSetCFM (world,1e-5);
254 dCreatePlane (space,0,0,1,0);
255 memset (&fallingObject,0,sizeof(fallingObject));
256
257 // Create two flat boxes, just slightly off vertical and a bit apart for stuff to fall in between.
258 // Don't create bodies for these boxes -- they'll be immovable instead.
259 {
260 dReal sides[3];
261 dMatrix3 R;
262
263 sides[0] = 4;
264 sides[1] = 0.2;
265 sides[2] = 3;
266
267 box1 = dCreateBox (space,sides[0],sides[1],sides[2]);
268 dGeomSetPosition (box1, 0, sides[1], sides[2]/2);
269 dRFromAxisAndAngle (R, 1, 0, 0, -0.1);
270 dGeomSetRotation (box1, R);
271
272 box2 = dCreateBox (space,sides[0],sides[1],sides[2]);
273 dGeomSetPosition (box2, 0, -sides[1], sides[2]/2);
274 dRFromAxisAndAngle (R, 1, 0, 0, 0.1);
275 dGeomSetRotation (box2, R);
276 }
277
278 // Pretend to drop a box to start
279 command('b');
280
281 // run simulation
282 dsSimulationLoop (argc,argv,640,480,&fn);
283
284 dJointGroupDestroy (contactgroup);
285 dSpaceDestroy (space);
286 dWorldDestroy (world);
287
288 return 0;
289}
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c
deleted file mode 100644
index e49fd73..0000000
--- a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c
+++ /dev/null
@@ -1,197 +0,0 @@
1#include <stdio.h>
2#include <glut.h>
3#include "ode.h"
4
5#define NUMBODIES 80
6
7#define USE_SPHERE 0
8#define USE_HELIX 1
9#define USE_TORQUE 1
10#define USE_WEIRD_MATRIX_OPS 0
11
12#define CONTACTS 1
13
14dWorldID aWorld;
15dSpaceID aSpace;
16float cycle = 0, fade;
17dJointGroupID aContactGroup;
18dBodyID bodies[NUMBODIES];
19dGeomID geoms[NUMBODIES];
20GLfloat colors[NUMBODIES][4];
21unsigned int contactsThisFrame;
22
23void kglTransformByODEGeom(dGeomID geom) {
24 const dReal *p = dGeomGetPosition(geom);
25 const dReal *R = dGeomGetRotation(geom);
26 GLdouble glm[16];
27
28 glm[0] = R[0]; glm[1] = R[4]; glm[2] = R[8]; glm[3] = 0;
29 glm[4] = R[1]; glm[5] = R[5]; glm[6] = R[9]; glm[7] = 0;
30 glm[8] = R[2]; glm[9] = R[6]; glm[10] = R[10];glm[11] = 0;
31 glm[12] = p[0]; glm[13] = p[1]; glm[14] = p[2]; glm[15] = 1;
32
33 glMultMatrixd(glm);
34}
35
36static void odeNearCallback(void *data, dGeomID g1, dGeomID g2) {
37 dBodyID b1 = dGeomGetBody(g1),
38 b2 = dGeomGetBody(g2);
39 dContact contact[CONTACTS];
40 int contactsUsed, i;
41
42 if (b1 && b2 && dAreConnected(b1, b2)) return;
43
44 contactsUsed = dCollide(g1, g2, CONTACTS, &contact[0].geom,
45 sizeof(dContact));
46 if (contactsUsed > CONTACTS) contactsUsed = CONTACTS;
47
48 for (i = 0; i < contactsUsed; i++) {
49 contact[i].surface.mode = 0;
50 contact[i].surface.mu = 20.0;
51
52 dJointAttach(dJointCreateContact(aWorld, aContactGroup,
53 &(contact[i])), b1, b2);
54 contactsThisFrame++;
55 }
56}
57
58void myGlutResize(int w, int h) {
59 glViewport(0, 0, w, h);
60 glMatrixMode(GL_PROJECTION);
61 glLoadIdentity();
62 gluPerspective(45.0, (GLfloat)w / h, 1.0, 120.0);
63 glMatrixMode(GL_MODELVIEW);
64 glLoadIdentity();
65 glTranslatef(0, -6, -20);
66}
67
68void myGlutIdle(void) {
69 const float step = 1.0/120;
70 int i;
71
72 cycle = fmod(cycle + step / 4, 1);
73 fade = fabs(cycle * 2 - 1);
74
75 contactsThisFrame = 0;
76 dSpaceCollide(aSpace, NULL, &odeNearCallback);
77 //printf("%u\n", contactsThisFrame);
78 dWorldStep(aWorld, step);
79 dJointGroupEmpty(aContactGroup);
80
81 for (i = 0; i < NUMBODIES; i++) {
82 const dReal *cvel = dBodyGetLinearVel(bodies[i]);
83 dBodyAddForce(bodies[i],
84 -cvel[0] * 0.5,
85 -cvel[1] * 0.5,
86 -cvel[2] * 0.5
87 );
88 }
89
90 glutPostRedisplay();
91}
92
93void myGlutDisplay(void) {
94 int i;
95
96 glClearColor(fade * 0.15, 0, 0, 1);
97 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
98
99 if (USE_WEIRD_MATRIX_OPS) glPushMatrix();
100 for (i = 0; i < NUMBODIES; i++) {
101 if (!USE_WEIRD_MATRIX_OPS) glPushMatrix();
102 kglTransformByODEGeom(geoms[i]);
103 glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, colors[i]);
104 glColor3f(fade * 1.5, 0, 0);
105#if USE_SPHERE
106 glRotatef(90, 1, 0, 0);
107 glutSolidSphere(0.5, 9, 6);
108 glDisable(GL_LIGHTING);
109 glutWireSphere(0.5, 9, 6);
110#else
111 glutSolidCube(1);
112 glDisable(GL_LIGHTING);
113 glutWireCube(1);
114#endif
115 glEnable(GL_LIGHTING);
116 if (!USE_WEIRD_MATRIX_OPS) glPopMatrix();
117 }
118 if (USE_WEIRD_MATRIX_OPS) glPopMatrix();
119
120 glutSwapBuffers();
121}
122
123int main(int argc, char **argv) {
124 printf("Initializing GLUT\n");
125
126 glutInit(&argc, argv);
127 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
128 glutInitWindowSize(400, 300);
129 glutInitWindowPosition(100, 100);
130 glutCreateWindow("ODE Crash Test");
131
132 glutDisplayFunc(myGlutDisplay);
133 glutReshapeFunc(myGlutResize);
134 glutIdleFunc(myGlutIdle);
135
136 glPolygonOffset(1, 1);
137 glDepthFunc(GL_LEQUAL);
138 glEnable(GL_POLYGON_OFFSET_FILL);
139 glEnable(GL_DEPTH_TEST);
140 glEnable(GL_CULL_FACE);
141 glEnable(GL_LIGHTING);
142 glEnable(GL_LIGHT0);
143 myGlutResize(400, 300);
144
145 printf("Creating ODE world\n");
146 aWorld = dWorldCreate();
147 aSpace = dHashSpaceCreate();
148 aContactGroup = dJointGroupCreate(0);
149 dCreatePlane(aSpace, 0, 1, 0, 0);
150 dWorldSetGravity(aWorld, 0, -9.81, 0);
151 dWorldSetERP(aWorld, 0.4);
152 dWorldSetCFM(aWorld, 1e-10);
153
154 printf("Creating objects\n");
155 {
156 int i;
157 dMass mass;
158
159 dMassSetBox(&mass, 1.0, 1, 1, 1);
160
161 for (i = 0; i < NUMBODIES; i++) {
162 float fraction = (float)i / NUMBODIES;
163
164 bodies[i] = dBodyCreate(aWorld);
165 dBodySetMass(bodies[i], &mass);
166#if USE_SPHERE
167 geoms[i] = dCreateSphere(aSpace, 0.5);
168#else
169 geoms[i] = dCreateBox(aSpace, 1, 1, 1);
170#endif
171 dGeomSetBody(geoms[i], bodies[i]);
172
173 if (USE_HELIX) {
174 float r = (i % 3 - 1) * (1.5+4*(1 - fraction)),
175 theta = (float)i / 4;
176 dBodySetPosition(bodies[i],
177 sin(theta) * r,
178 (float)i + 1,
179 cos(theta) * r
180 );
181 } else {
182 dBodySetPosition(bodies[i], 0, (float)i * 2 + 1, 0);
183 }
184 if (USE_TORQUE) dBodyAddTorque(bodies[i], fraction*10, fraction*20, fraction*30);
185
186 colors[i][0] = fraction;
187 colors[i][1] = 1 - fraction;
188 colors[i][2] = 1 - fabs(fraction * 2 - 1);
189 colors[i][3] = 1;
190 }
191 }
192
193 printf("Starting simulation\n");
194 glutMainLoop();
195
196 return 0;
197} \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs
deleted file mode 100644
index 8d2b86a..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs
+++ /dev/null
@@ -1,18 +0,0 @@
1using System;
2using System.Reflection;
3using System.Runtime.CompilerServices;
4using System.Runtime.InteropServices;
5
6[assembly: AssemblyTitle("Drawstuff.NET")]
7[assembly: AssemblyDescription("")]
8[assembly: AssemblyConfiguration("")]
9[assembly: AssemblyCompany("")]
10[assembly: AssemblyProduct("Ode.NET")]
11[assembly: AssemblyCopyright("")]
12[assembly: AssemblyTrademark("")]
13[assembly: AssemblyCulture("")]
14[assembly: ComVisible(false)]
15[assembly: Guid("b2a39dd4-dd67-4e8a-af70-d3b412da8850")]
16[assembly: AssemblyVersion("0.7.0.0")]
17[assembly: AssemblyFileVersion("0.7.0.0")]
18[assembly: CLSCompliantAttribute(true)]
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs
deleted file mode 100644
index aa84966..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs
+++ /dev/null
@@ -1,58 +0,0 @@
1using System;
2using System.Runtime.InteropServices;
3using Ode.NET;
4
5namespace Drawstuff.NET
6{
7#if dDOUBLE
8 using dReal = System.Double;
9#else
10 using dReal = System.Single;
11#endif
12
13 public static class ds
14 {
15 public const int VERSION = 2;
16
17 public enum Texture
18 {
19 None,
20 Wood
21 }
22
23 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
24 public delegate void CallbackFunction(int arg);
25
26 [StructLayout(LayoutKind.Sequential)]
27 public struct Functions
28 {
29 public int version;
30 public CallbackFunction start;
31 public CallbackFunction step;
32 public CallbackFunction command;
33 public CallbackFunction stop;
34 public string path_to_textures;
35 }
36
37 [DllImport("drawstuff", EntryPoint="dsDrawBox")]
38 public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);
39
40 [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]
41 public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);
42
43 [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]
44 public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
45
46 [DllImport("drawstuff", EntryPoint="dsSetColor")]
47 public static extern void SetColor(float red, float green, float blue);
48
49 [DllImport("drawstuff", EntryPoint="dsSetTexture")]
50 public static extern void SetTexture(Texture texture);
51
52 [DllImport("drawstuff", EntryPoint="dsSetViewpoint")]
53 public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);
54
55 [DllImport("drawstuff", EntryPoint="dsSimulationLoop")]
56 public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);
57 }
58}
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua
deleted file mode 100644
index d777ffb..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua
+++ /dev/null
@@ -1,19 +0,0 @@
1package.name = "Drawstuff.NET"
2package.kind = "dll"
3package.language = "c#"
4
5if (options["with-doubles"]) then
6 package.defines = { "dDOUBLE" }
7else
8 package.defines = { "dSINGLE " }
9end
10
11package.links = {
12 "System",
13 "Ode.NET"
14}
15
16package.files = {
17 "AssemblyInfo.cs",
18 "Drawstuff.cs"
19}
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode.NET.sln b/libraries/ode-0.9/contrib/Ode.NET/Ode.NET.sln
deleted file mode 100755
index ecc6d9d..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Ode.NET.sln
+++ /dev/null
@@ -1,21 +0,0 @@
1Microsoft Visual Studio Solution File, Format Version 9.00
2# Visual Studio 2005
3Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Ode.NET", "Ode/Ode.NET.csproj", "{637D89E7-AA43-EB43-9D2C-5210B740F36B}"
4 ProjectSection(ProjectDependencies) = postProject
5 EndProjectSection
6EndProject
7Global
8 GlobalSection(SolutionConfigurationPlatforms) = preSolution
9 Debug|Any CPU = Debug|Any CPU
10 Release|Any CPU = Release|Any CPU
11 EndGlobalSection
12 GlobalSection(ProjectConfigurationPlatforms) = postSolution
13 {637D89E7-AA43-EB43-9D2C-5210B740F36B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
14 {637D89E7-AA43-EB43-9D2C-5210B740F36B}.Debug|Any CPU.Build.0 = Debug|Any CPU
15 {637D89E7-AA43-EB43-9D2C-5210B740F36B}.Release|Any CPU.ActiveCfg = Release|Any CPU
16 {637D89E7-AA43-EB43-9D2C-5210B740F36B}.Release|Any CPU.Build.0 = Release|Any CPU
17 EndGlobalSection
18 GlobalSection(SolutionProperties) = preSolution
19 HideSolutionNode = FALSE
20 EndGlobalSection
21EndGlobal
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs b/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs
deleted file mode 100644
index 6a8c2b6..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs
+++ /dev/null
@@ -1,18 +0,0 @@
1using System;
2using System.Reflection;
3using System.Runtime.CompilerServices;
4using System.Runtime.InteropServices;
5
6[assembly: AssemblyTitle("Ode.NET")]
7[assembly: AssemblyDescription("")]
8[assembly: AssemblyConfiguration("")]
9[assembly: AssemblyCompany("")]
10[assembly: AssemblyProduct("Ode.NET")]
11[assembly: AssemblyCopyright("")]
12[assembly: AssemblyTrademark("")]
13[assembly: AssemblyCulture("")]
14[assembly: ComVisible(false)]
15[assembly: Guid("1347a35e-c32b-4ff6-8064-7d10b2cc113b")]
16[assembly: AssemblyVersion("0.7.0.0")]
17[assembly: AssemblyFileVersion("0.7.0.0")]
18[assembly: CLSCompliantAttribute(true)]
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs b/libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs
deleted file mode 100644
index 0603810..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs
+++ /dev/null
@@ -1,1732 +0,0 @@
1using System;
2using System.Runtime.InteropServices;
3using System.Security;
4
5namespace Ode.NET
6{
7#if dDOUBLE
8 using dReal = System.Double;
9#else
10 using dReal = System.Single;
11#endif
12
13 public static class d
14 {
15 public static dReal Infinity = dReal.MaxValue;
16
17 #region Flags and Enumerations
18
19 [Flags]
20 public enum ContactFlags : int
21 {
22 Mu2 = 0x001,
23 FDir1 = 0x002,
24 Bounce = 0x004,
25 SoftERP = 0x008,
26 SoftCFM = 0x010,
27 Motion1 = 0x020,
28 Motion2 = 0x040,
29 Slip1 = 0x080,
30 Slip2 = 0x100,
31 Approx0 = 0x0000,
32 Approx1_1 = 0x1000,
33 Approx1_2 = 0x2000,
34 Approx1 = 0x3000
35 }
36
37 public enum GeomClassID : int
38 {
39 SphereClass,
40 BoxClass,
41 CapsuleClass,
42 CylinderClass,
43 PlaneClass,
44 RayClass,
45 ConvexClass,
46 GeomTransformClass,
47 TriMeshClass,
48 HeightfieldClass,
49 FirstSpaceClass,
50 SimpleSpaceClass = FirstSpaceClass,
51 HashSpaceClass,
52 QuadTreeSpaceClass,
53 LastSpaceClass = QuadTreeSpaceClass,
54 FirstUserClass,
55 LastUserClass = FirstUserClass + MaxUserClasses - 1,
56 NumClasses,
57 MaxUserClasses = 4
58 }
59
60 public enum JointType : int
61 {
62 None,
63 Ball,
64 Hinge,
65 Slider,
66 Contact,
67 Universal,
68 Hinge2,
69 Fixed,
70 Null,
71 AMotor,
72 LMotor,
73 Plane2D
74 }
75
76 public enum JointParam : int
77 {
78 LoStop,
79 HiStop,
80 Vel,
81 FMax,
82 FudgeFactor,
83 Bounce,
84 CFM,
85 StopERP,
86 StopCFM,
87 SuspensionERP,
88 SuspensionCFM,
89 LoStop2 = 256,
90 HiStop2,
91 Vel2,
92 FMax2,
93 FudgeFactor2,
94 Bounce2,
95 CFM2,
96 StopERP2,
97 StopCFM2,
98 SuspensionERP2,
99 SuspensionCFM2,
100 LoStop3 = 512,
101 HiStop3,
102 Vel3,
103 FMax3,
104 FudgeFactor3,
105 Bounce3,
106 CFM3,
107 StopERP3,
108 StopCFM3,
109 SuspensionERP3,
110 SuspensionCFM3
111 }
112
113 #endregion
114
115 #region Callbacks
116
117 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
118 public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
119
120 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
121 public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
122
123 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
124 public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
125
126 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
127 public delegate ColliderFn GetColliderFnFn(int num);
128
129 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
130 public delegate void GeomDtorFn(IntPtr o);
131
132 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
133 public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
134
135 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
136 public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
137
138 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
139 public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
140
141 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
142 public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
143
144 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
145 public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
146
147 #endregion
148
149 #region Structs
150
151 [StructLayout(LayoutKind.Sequential)]
152 public struct AABB
153 {
154 public dReal MinX, MaxX;
155 public dReal MinY, MaxY;
156 public dReal MinZ, MaxZ;
157 }
158
159
160 [StructLayout(LayoutKind.Sequential)]
161 public struct Contact
162 {
163 public SurfaceParameters surface;
164 public ContactGeom geom;
165 public Vector3 fdir1;
166 }
167
168
169 [StructLayout(LayoutKind.Sequential)]
170 public struct ContactGeom
171 {
172 public static readonly int SizeOf = Marshal.SizeOf(typeof(ContactGeom));
173
174 public Vector3 pos;
175 public Vector3 normal;
176 public dReal depth;
177 public IntPtr g1;
178 public IntPtr g2;
179 public int side1;
180 public int side2;
181 }
182
183 [StructLayout(LayoutKind.Sequential)]
184 public struct GeomClass
185 {
186 public int bytes;
187 public GetColliderFnFn collider;
188 public GetAABBFn aabb;
189 public AABBTestFn aabb_test;
190 public GeomDtorFn dtor;
191 }
192
193
194 [StructLayout(LayoutKind.Sequential)]
195 public struct JointFeedback
196 {
197 public Vector3 f1;
198 public Vector3 t1;
199 public Vector3 f2;
200 public Vector3 t2;
201 }
202
203
204 [StructLayout(LayoutKind.Sequential)]
205 public struct Mass
206 {
207 public dReal mass;
208 public Vector4 c;
209 public Matrix3 I;
210 }
211
212
213 [StructLayout(LayoutKind.Sequential)]
214 public struct Matrix3
215 {
216 public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
217 {
218 M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f;
219 M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f;
220 M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f;
221 }
222 public dReal M00, M10, M20;
223 private dReal _m30;
224 public dReal M01, M11, M21;
225 private dReal _m31;
226 public dReal M02, M12, M22;
227 private dReal _m32;
228 }
229
230 [StructLayout(LayoutKind.Sequential)]
231 public struct Matrix4
232 {
233 public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
234 dReal m01, dReal m11, dReal m21, dReal m31,
235 dReal m02, dReal m12, dReal m22, dReal m32,
236 dReal m03, dReal m13, dReal m23, dReal m33)
237 {
238 M00 = m00; M10 = m10; M20 = m20; M30 = m30;
239 M01 = m01; M11 = m11; M21 = m21; M31 = m31;
240 M02 = m02; M12 = m12; M22 = m22; M32 = m32;
241 M03 = m03; M13 = m13; M23 = m23; M33 = m33;
242 }
243 public dReal M00, M10, M20, M30;
244 public dReal M01, M11, M21, M31;
245 public dReal M02, M12, M22, M32;
246 public dReal M03, M13, M23, M33;
247 }
248
249 [StructLayout(LayoutKind.Sequential)]
250 public struct Quaternion
251 {
252 public dReal W, X, Y, Z;
253 }
254
255
256 [StructLayout(LayoutKind.Sequential)]
257 public struct SurfaceParameters
258 {
259 public ContactFlags mode;
260 public dReal mu;
261 public dReal mu2;
262 public dReal bounce;
263 public dReal bounce_vel;
264 public dReal soft_erp;
265 public dReal soft_cfm;
266 public dReal motion1;
267 public dReal motion2;
268 public dReal slip1;
269 public dReal slip2;
270 }
271
272
273 [StructLayout(LayoutKind.Sequential)]
274 public struct Vector3
275 {
276 public Vector3(dReal x, dReal y, dReal z)
277 {
278 X = x; Y = y; Z = z; _w = 0.0f;
279 }
280 public dReal X, Y, Z;
281 private dReal _w;
282 }
283
284
285 [StructLayout(LayoutKind.Sequential)]
286 public struct Vector4
287 {
288 public Vector4(dReal x, dReal y, dReal z, dReal w)
289 {
290 X = x; Y = y; Z = z; W = w;
291 }
292 public dReal X, Y, Z, W;
293 }
294
295 #endregion
296
297 [DllImport("ode", EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
298 public static extern bool AreConnected(IntPtr b1, IntPtr b2);
299
300 [DllImport("ode", EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
301 public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
302
303 [DllImport("ode", EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
304 public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
305
306 [DllImport("ode", EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
307 public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
308
309 [DllImport("ode", EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
310 public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
311
312 [DllImport("ode", EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
313 public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
314
315 [DllImport("ode", EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
316 public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
317
318 [DllImport("ode", EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
319 public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
320
321 [DllImport("ode", EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
322 public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
323
324 [DllImport("ode", EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
325 public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
326
327 [DllImport("ode", EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
328 public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
329
330 [DllImport("ode", EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
331 public static extern void BodyCopyPosition(IntPtr body, out dReal X);
332
333 [DllImport("ode", EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
334 public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
335
336 [DllImport("ode", EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
337 public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
338
339 [DllImport("ode", EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
340 public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
341
342 [DllImport("ode", EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
343 public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
344
345 [DllImport("ode", EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
346 public static extern IntPtr BodyCreate(IntPtr world);
347
348 [DllImport("ode", EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
349 public static extern void BodyDestroy(IntPtr body);
350
351 [DllImport("ode", EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
352 public static extern void BodyDisable(IntPtr body);
353
354 [DllImport("ode", EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
355 public static extern void BodyEnable(IntPtr body);
356
357 [DllImport("ode", EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
358 public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
359
360 [DllImport("ode", EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
361 public static extern bool BodyGetAutoDisableFlag(IntPtr body);
362
363 [DllImport("ode", EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
364 public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
365
366 [DllImport("ode", EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
367 public static extern int BodyGetAutoDisableSteps(IntPtr body);
368
369 [DllImport("ode", EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
370 public static extern dReal BodyGetAutoDisableTime(IntPtr body);
371
372#if !dNO_UNSAFE_CODE
373 [CLSCompliant(false)]
374 [DllImport("ode", EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
375 public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
376 public static Vector3 BodyGetAngularVel(IntPtr body)
377 {
378 unsafe { return *(BodyGetAngularVelUnsafe(body)); }
379 }
380#endif
381
382 [DllImport("ode", EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
383 public static extern IntPtr BodyGetData(IntPtr body);
384
385 [DllImport("ode", EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
386 public static extern int BodyGetFiniteRotationMode(IntPtr body);
387
388 [DllImport("ode", EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
389 public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
390
391#if !dNO_UNSAFE_CODE
392 [CLSCompliant(false)]
393 [DllImport("ode", EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
394 public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
395 public static Vector3 BodyGetForce(IntPtr body)
396 {
397 unsafe { return *(BodyGetForceUnsafe(body)); }
398 }
399#endif
400
401 [DllImport("ode", EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
402 public static extern bool BodyGetGravityMode(IntPtr body);
403
404 [DllImport("ode", EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
405 public static extern IntPtr BodyGetJoint(IntPtr body, int index);
406
407#if !dNO_UNSAFE_CODE
408 [CLSCompliant(false)]
409 [DllImport("ode", EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
410 public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
411 public static Vector3 BodyGetLinearVel(IntPtr body)
412 {
413 unsafe { return *(BodyGetLinearVelUnsafe(body)); }
414 }
415#endif
416
417 [DllImport("ode", EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
418 public static extern void BodyGetMass(IntPtr body, out Mass mass);
419
420 [DllImport("ode", EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
421 public static extern int BodyGetNumJoints(IntPtr body);
422
423 [DllImport("ode", EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
424 public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
425
426#if !dNO_UNSAFE_CODE
427 [CLSCompliant(false)]
428 [DllImport("ode", EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
429 public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
430 public static Vector3 BodyGetPosition(IntPtr body)
431 {
432 unsafe { return *(BodyGetPositionUnsafe(body)); }
433 }
434#endif
435
436 [DllImport("ode", EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
437 public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
438
439#if !dNO_UNSAFE_CODE
440 [CLSCompliant(false)]
441 [DllImport("ode", EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
442 public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
443 public static Quaternion BodyGetQuaternion(IntPtr body)
444 {
445 unsafe { return *(BodyGetQuaternionUnsafe(body)); }
446 }
447#endif
448
449 [DllImport("ode", EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
450 public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
451
452 [DllImport("ode", EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
453 public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
454
455#if !dNO_UNSAFE_CODE
456 [CLSCompliant(false)]
457 [DllImport("ode", EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
458 public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
459 public static Matrix3 BodyGetRotation(IntPtr body)
460 {
461 unsafe { return *(BodyGetRotationUnsafe(body)); }
462 }
463#endif
464
465#if !dNO_UNSAFE_CODE
466 [CLSCompliant(false)]
467 [DllImport("ode", EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
468 public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
469 public static Vector3 BodyGetTorque(IntPtr body)
470 {
471 unsafe { return *(BodyGetTorqueUnsafe(body)); }
472 }
473#endif
474
475 [DllImport("ode", EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
476 public static extern bool BodyIsEnabled(IntPtr body);
477
478 [DllImport("ode", EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
479 public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
480
481 [DllImport("ode", EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
482 public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
483
484 [DllImport("ode", EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
485 public static extern void BodySetAutoDisableDefaults(IntPtr body);
486
487 [DllImport("ode", EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
488 public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
489
490 [DllImport("ode", EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
491 public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
492
493 [DllImport("ode", EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
494 public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
495
496 [DllImport("ode", EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
497 public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
498
499 [DllImport("ode", EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
500 public static extern void BodySetData(IntPtr body, IntPtr data);
501
502 [DllImport("ode", EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
503 public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
504
505 [DllImport("ode", EntryPoint = "dBodySetFiniteRotationModeAxis"), SuppressUnmanagedCodeSecurity]
506 public static extern void BodySetFiniteRotationModeAxis(IntPtr body, dReal x, dReal y, dReal z);
507
508 [DllImport("ode", EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
509 public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
510
511 [DllImport("ode", EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
512 public static extern void BodySetGravityMode(IntPtr body, bool mode);
513
514 [DllImport("ode", EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
515 public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
516
517 [DllImport("ode", EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
518 public static extern void BodySetMass(IntPtr body, ref Mass mass);
519
520 [DllImport("ode", EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
521 public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
522
523 [DllImport("ode", EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
524 public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
525
526 [DllImport("ode", EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
527 public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
528
529 [DllImport("ode", EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
530 public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
531
532 [DllImport("ode", EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
533 public static extern void BodySetRotation(IntPtr body, ref dReal M00);
534
535 [DllImport("ode", EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
536 public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
537
538 [DllImport("ode", EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
539 public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
540
541 [DllImport("ode", EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
542 public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
543
544 [DllImport("ode", EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
545 public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
546 ref Vector3 side1, ref Vector3 p2,
547 ref Matrix3 R2, ref Vector3 side2,
548 ref Vector3 normal, out dReal depth, out int return_code,
549 int maxc, out ContactGeom contact, int skip);
550
551 [DllImport("ode", EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
552 public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
553 ref Vector3 side1, ref Vector3 _p2,
554 ref Matrix3 R2, ref Vector3 side2);
555
556 [DllImport("ode", EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
557 public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
558 ref Vector3 b1, ref Vector3 b2,
559 ref Vector3 cp1, ref Vector3 cp2);
560
561 [DllImport("ode", EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
562 public static extern void CloseODE();
563
564 [DllImport("ode", EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
565 public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
566
567 [DllImport("ode", EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
568 public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
569
570 [DllImport("ode", EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
571 public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz);
572
573 [DllImport("ode", EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
574 public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length);
575
576 [DllImport("ode", EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
577 public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
578
579 [DllImport("ode", EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
580 public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length);
581
582 [DllImport("ode", EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
583 public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable);
584
585 [DllImport("ode", EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
586 public static extern IntPtr CreateGeom(int classnum);
587
588 [DllImport("ode", EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
589 public static extern int CreateGeomClass(ref GeomClass classptr);
590
591 [DllImport("ode", EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
592 public static extern IntPtr CreateGeomTransform(IntPtr space);
593
594 [DllImport("ode", EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
595 public static extern IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
596
597 [DllImport("ode", EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
598 public static extern IntPtr CreateRay(IntPtr space, dReal length);
599
600 [DllImport("ode", EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
601 public static extern IntPtr CreateSphere(IntPtr space, dReal radius);
602
603 [DllImport("ode", EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
604 public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data,
605 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
606
607 [DllImport("ode", EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
608 public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
609
610 [DllImport("ode", EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
611 public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
612
613 [DllImport("ode", EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
614 public static extern int FactorCholesky(ref dReal A00, int n);
615
616 [DllImport("ode", EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
617 public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
618
619 [DllImport("ode", EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
620 public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
621
622 [DllImport("ode", EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
623 public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
624
625 [DllImport("ode", EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
626 public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
627
628 [DllImport("ode", EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
629 public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
630
631 [DllImport("ode", EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
632 public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
633
634 [DllImport("ode", EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
635 public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
636
637 [DllImport("ode", EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
638 public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
639
640 [DllImport("ode", EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
641 public static extern void GeomClearOffset(IntPtr geom);
642
643 [DllImport("ode", EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
644 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
645
646 [DllImport("ode", EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
647 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
648
649 [DllImport("ode", EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
650 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
651
652 [DllImport("ode", EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
653 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
654
655 [DllImport("ode", EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
656 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
657
658 [DllImport("ode", EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
659 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
660
661 [DllImport("ode", EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
662 public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
663
664 [DllImport("ode", EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
665 public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
666
667 [DllImport("ode", EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
668 public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
669
670 [DllImport("ode", EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
671 public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
672
673 [DllImport("ode", EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
674 public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
675
676 [DllImport("ode", EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
677 public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
678
679 [DllImport("ode", EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
680 public static extern void GeomDestroy(IntPtr geom);
681
682 [DllImport("ode", EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
683 public static extern void GeomDisable(IntPtr geom);
684
685 [DllImport("ode", EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
686 public static extern void GeomEnable(IntPtr geom);
687
688 [DllImport("ode", EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
689 public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
690
691 [DllImport("ode", EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
692 public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
693
694 [DllImport("ode", EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
695 public static extern IntPtr GeomGetBody(IntPtr geom);
696
697 [DllImport("ode", EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
698 public static extern int GeomGetCategoryBits(IntPtr geom);
699
700 [DllImport("ode", EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
701 public static extern IntPtr GeomGetClassData(IntPtr geom);
702
703 [DllImport("ode", EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
704 public static extern int GeomGetCollideBits(IntPtr geom);
705
706 [DllImport("ode", EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
707 public static extern GeomClassID GeomGetClass(IntPtr geom);
708
709 [DllImport("ode", EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
710 public static extern IntPtr GeomGetData(IntPtr geom);
711
712#if !dNO_UNSAFE_CODE
713 [CLSCompliant(false)]
714 [DllImport("ode", EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
715 public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
716 public static Vector3 GeomGetOffsetPosition(IntPtr geom)
717 {
718 unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
719 }
720#endif
721
722#if !dNO_UNSAFE_CODE
723 [CLSCompliant(false)]
724 [DllImport("ode", EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
725 public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
726 public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
727 {
728 unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
729 }
730#endif
731
732#if !dNO_UNSAFE_CODE
733 [CLSCompliant(false)]
734 [DllImport("ode", EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
735 public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
736 public static Vector3 GeomGetPosition(IntPtr geom)
737 {
738 unsafe { return *(GeomGetPositionUnsafe(geom)); }
739 }
740#endif
741
742 [DllImport("ode", EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
743 public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
744
745 [DllImport("ode", EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
746 public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
747
748#if !dNO_UNSAFE_CODE
749 [CLSCompliant(false)]
750 [DllImport("ode", EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
751 public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
752 public static Matrix3 GeomGetRotation(IntPtr geom)
753 {
754 unsafe { return *(GeomGetRotationUnsafe(geom)); }
755 }
756#endif
757
758 [DllImport("ode", EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
759 public static extern IntPtr GeomGetSpace(IntPtr geom);
760
761 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
762 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
763 dReal width, dReal depth, int widthSamples, int depthSamples,
764 dReal scale, dReal offset, dReal thickness, int bWrap);
765
766 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
767 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
768 dReal width, dReal depth, int widthSamples, int depthSamples,
769 dReal scale, dReal offset, dReal thickness, int bWrap);
770
771 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
772 public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
773 dReal width, dReal depth, int widthSamples, int depthSamples,
774 dReal scale, dReal offset, dReal thickness, int bWrap);
775
776 [CLSCompliant(false)]
777 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
778 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
779 dReal width, dReal depth, int widthSamples, int depthSamples,
780 dReal scale, dReal offset, dReal thickness, int bWrap);
781
782 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
783 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
784 dReal width, dReal depth, int widthSamples, int depthSamples,
785 dReal scale, dReal offset, dReal thickness, int bWrap);
786
787 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
788 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
789 dReal width, dReal depth, int widthSamples, int depthSamples,
790 dReal scale, dReal offset, dReal thickness, int bWrap);
791
792 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
793 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
794 dReal width, dReal depth, int widthSamples, int depthSamples,
795 dReal scale, dReal offset, dReal thickness, int bWrap);
796
797 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
798 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
799 dReal width, dReal depth, int widthSamples, int depthSamples,
800 dReal scale, dReal offset, dReal thickness, int bWrap);
801
802 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
803 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
804 dReal width, dReal depth, int widthSamples, int depthSamples,
805 dReal scale, dReal offset, dReal thickness, int bWrap);
806
807 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
808 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
809 dReal width, dReal depth, int widthSamples, int depthSamples,
810 dReal scale, dReal offset, dReal thickness, int bWrap);
811
812 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
813 public static extern IntPtr GeomHeightfieldDataCreate();
814
815 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
816 public static extern void GeomHeightfieldDataDestroy(IntPtr d);
817
818 [DllImport("ode", EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
819 public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
820
821 [DllImport("ode", EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
822 public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
823
824 [DllImport("ode", EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
825 public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
826
827 [DllImport("ode", EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
828 public static extern bool GeomIsEnabled(IntPtr geom);
829
830 [DllImport("ode", EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
831 public static extern bool GeomIsOffset(IntPtr geom);
832
833 [DllImport("ode", EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
834 public static extern bool GeomIsSpace(IntPtr geom);
835
836 [DllImport("ode", EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
837 public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
838
839 [DllImport("ode", EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
840 public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
841
842 [DllImport("ode", EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
843 public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
844
845 [DllImport("ode", EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
846 public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
847
848 [DllImport("ode", EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
849 public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
850
851 [DllImport("ode", EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
852 public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
853
854 [DllImport("ode", EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
855 public static extern int GeomRayGetClosestHit(IntPtr ray);
856
857 [DllImport("ode", EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
858 public static extern dReal GeomRayGetLength(IntPtr ray);
859
860 [DllImport("ode", EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
861 public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
862
863 [DllImport("ode", EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
864 public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
865
866 [DllImport("ode", EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
867 public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
868
869 [DllImport("ode", EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
870 public static extern void GeomRaySetLength(IntPtr ray, dReal length);
871
872 [DllImport("ode", EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
873 public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
874
875 [DllImport("ode", EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
876 public static extern void GeomSetBody(IntPtr geom, IntPtr body);
877
878 [DllImport("ode", EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
879 public static extern void GeomSetCategoryBits(IntPtr geom, int bits);
880
881 [DllImport("ode", EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
882 public static extern void GeomSetCollideBits(IntPtr geom, int bits);
883
884 [DllImport("ode", EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
885 public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
886
887 [DllImport("ode", EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
888 public static extern void GeomSetData(IntPtr geom, IntPtr data);
889
890 [DllImport("ode", EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
891 public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
892
893 [DllImport("ode", EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
894 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
895
896 [DllImport("ode", EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
897 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
898
899 [DllImport("ode", EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
900 public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
901
902 [DllImport("ode", EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
903 public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
904
905 [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
906 public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
907
908 [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
909 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
910
911 [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
912 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
913
914 [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
915 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
916
917 [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
918 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
919
920 [DllImport("ode", EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
921 public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
922
923 [DllImport("ode", EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
924 public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
925
926 [DllImport("ode", EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
927 public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
928
929 [DllImport("ode", EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
930 public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
931
932 [DllImport("ode", EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
933 public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
934
935 [DllImport("ode", EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
936 public static extern dReal GeomSphereGetRadius(IntPtr geom);
937
938 [DllImport("ode", EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
939 public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
940
941 [DllImport("ode", EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
942 public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
943
944 [DllImport("ode", EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
945 public static extern int GeomTransformGetCleanup(IntPtr geom);
946
947 [DllImport("ode", EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
948 public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
949
950 [DllImport("ode", EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
951 public static extern int GeomTransformGetInfo(IntPtr geom);
952
953 [DllImport("ode", EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
954 public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
955
956 [DllImport("ode", EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
957 public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
958
959 [DllImport("ode", EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
960 public static extern void GeomTransformSetInfo(IntPtr geom, int info);
961
962 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
963 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
964 double[] vertices, int vertexStride, int vertexCount,
965 int[] indices, int indexCount, int triStride);
966
967 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
968 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
969 IntPtr vertices, int vertexStride, int vertexCount,
970 IntPtr indices, int indexCount, int triStride);
971
972 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
973 public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
974 double[] vertices, int vertexStride, int vertexCount,
975 int[] indices, int indexCount, int triStride,
976 double[] normals);
977
978 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
979 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
980 IntPtr vertices, int vertexStride, int vertexCount,
981 IntPtr indices, int indexCount, int triStride,
982 IntPtr normals);
983
984 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
985 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
986 dReal[] vertices, int vertexStride, int vertexCount,
987 int[] indices, int indexCount, int triStride);
988
989 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
990 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
991 IntPtr vertices, int vertexStride, int vertexCount,
992 IntPtr indices, int indexCount, int triStride);
993
994 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
995 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
996 dReal[] vertices, int vertexStride, int vertexCount,
997 int[] indices, int indexCount, int triStride,
998 dReal[] normals);
999
1000 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1001 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1002 IntPtr vertices, int vertexStride, int vertexCount,
1003 IntPtr indices, int indexCount, int triStride,
1004 IntPtr normals);
1005
1006 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1007 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1008 float[] vertices, int vertexStride, int vertexCount,
1009 int[] indices, int indexCount, int triStride);
1010
1011 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1012 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1013 IntPtr vertices, int vertexStride, int vertexCount,
1014 IntPtr indices, int indexCount, int triStride);
1015
1016 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1017 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1018 float[] vertices, int vertexStride, int vertexCount,
1019 int[] indices, int indexCount, int triStride,
1020 float[] normals);
1021
1022 [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1023 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1024 IntPtr vertices, int vertexStride, int vertexCount,
1025 IntPtr indices, int indexCount, int triStride,
1026 IntPtr normals);
1027
1028 [DllImport("ode", EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
1029 public static extern void GeomTriMeshClearTCCache(IntPtr g);
1030
1031 [DllImport("ode", EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
1032 public static extern IntPtr GeomTriMeshDataCreate();
1033
1034 [DllImport("ode", EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
1035 public static extern void GeomTriMeshDataDestroy(IntPtr d);
1036
1037 [DllImport("ode", EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
1038 public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
1039
1040 [DllImport("ode", EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
1041 public static extern void GeomTriMeshDataPreprocess(IntPtr d);
1042
1043 [DllImport("ode", EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
1044 public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
1045
1046 [DllImport("ode", EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
1047 public static extern void GeomTriMeshDataUpdate(IntPtr d);
1048
1049 [DllImport("ode", EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
1050 public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
1051
1052 [DllImport("ode", EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
1053 public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
1054
1055 [DllImport("ode", EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
1056 public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
1057
1058 [DllImport("ode", EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
1059 public static extern IntPtr GeomTriMeshGetData(IntPtr g);
1060
1061#if !dNO_UNSAFE_CODE
1062 [CLSCompliant(false)]
1063 [DllImport("ode", EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
1064 public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
1065 public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
1066 {
1067 unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
1068 }
1069#endif
1070
1071 [DllImport("ode", EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
1072 public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
1073
1074 [DllImport("ode", EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
1075 public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
1076
1077 [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
1078 public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
1079
1080 [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
1081 public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
1082
1083 [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
1084 public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
1085
1086 [DllImport("ode", EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
1087 public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
1088
1089 [DllImport("ode", EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
1090 public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
1091
1092 [DllImport("ode", EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
1093 public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
1094
1095 [DllImport("ode", EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
1096 public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
1097
1098 [DllImport("ode", EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1099 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
1100
1101 [DllImport("ode", EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1102 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
1103
1104 [DllImport("ode", EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
1105 public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
1106
1107 [DllImport("ode", EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
1108 public static extern IntPtr HashSpaceCreate(IntPtr space);
1109
1110 [DllImport("ode", EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
1111 public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
1112
1113 [DllImport("ode", EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
1114 public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
1115
1116 [DllImport("ode", EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
1117 public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
1118
1119 [DllImport("ode", EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
1120 public static extern void InitODE();
1121
1122 [DllImport("ode", EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
1123 public static extern int IsPositiveDefinite(ref dReal A, int n);
1124
1125 [DllImport("ode", EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
1126 public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
1127
1128 [DllImport("ode", EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
1129 public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
1130
1131 [DllImport("ode", EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
1132 public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
1133
1134 [DllImport("ode", EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
1135 public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
1136
1137 [DllImport("ode", EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
1138 public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
1139
1140 [DllImport("ode", EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
1141 public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
1142
1143 [DllImport("ode", EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
1144 public static extern void JointAddSliderForce(IntPtr joint, dReal force);
1145
1146 [DllImport("ode", EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
1147 public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
1148
1149 [DllImport("ode", EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
1150 public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
1151
1152 [DllImport("ode", EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
1153 public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
1154
1155 [DllImport("ode", EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1156 public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
1157
1158 [DllImport("ode", EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
1159 public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
1160
1161 [DllImport("ode", EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
1162 public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
1163
1164 [DllImport("ode", EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
1165 public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
1166
1167 [DllImport("ode", EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
1168 public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
1169
1170 [DllImport("ode", EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
1171 public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
1172
1173 [DllImport("ode", EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
1174 public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
1175
1176 [DllImport("ode", EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
1177 public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
1178
1179 [DllImport("ode", EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
1180 public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
1181
1182 [DllImport("ode", EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
1183 public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
1184
1185 [DllImport("ode", EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
1186 public static extern void JointDestroy(IntPtr j);
1187
1188 [DllImport("ode", EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1189 public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
1190
1191 [DllImport("ode", EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
1192 public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
1193
1194 [DllImport("ode", EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1195 public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
1196
1197 [DllImport("ode", EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
1198 public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
1199
1200 [DllImport("ode", EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
1201 public static extern int JointGetAMotorMode(IntPtr j);
1202
1203 [DllImport("ode", EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1204 public static extern int JointGetAMotorNumAxes(IntPtr j);
1205
1206 [DllImport("ode", EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
1207 public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
1208
1209 [DllImport("ode", EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
1210 public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
1211
1212 [DllImport("ode", EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1213 public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
1214
1215 [DllImport("ode", EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
1216 public static extern IntPtr JointGetBody(IntPtr j);
1217
1218 [DllImport("ode", EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
1219 public static extern IntPtr JointGetData(IntPtr j);
1220
1221#if !dNO_UNSAFE_CODE
1222 [CLSCompliant(false)]
1223 [DllImport("ode", EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
1224 public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
1225 public static JointFeedback JointGetFeedback(IntPtr j)
1226 {
1227 unsafe { return *(JointGetFeedbackUnsafe(j)); }
1228 }
1229#endif
1230
1231 [DllImport("ode", EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1232 public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
1233
1234 [DllImport("ode", EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
1235 public static extern dReal JointGetHingeAngle(IntPtr j);
1236
1237 [DllImport("ode", EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
1238 public static extern dReal JointGetHingeAngleRate(IntPtr j);
1239
1240 [DllImport("ode", EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
1241 public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
1242
1243 [DllImport("ode", EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
1244 public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
1245
1246 [DllImport("ode", EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
1247 public static extern dReal JointGetHinge2Angle1(IntPtr j);
1248
1249 [DllImport("ode", EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
1250 public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
1251
1252 [DllImport("ode", EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
1253 public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
1254
1255 [DllImport("ode", EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
1256 public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
1257
1258 [DllImport("ode", EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1259 public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
1260
1261 [DllImport("ode", EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
1262 public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
1263
1264 [DllImport("ode", EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1265 public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
1266
1267 [DllImport("ode", EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1268 public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
1269
1270 [DllImport("ode", EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
1271 public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
1272
1273 [DllImport("ode", EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1274 public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
1275
1276 [DllImport("ode", EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1277 public static extern int JointGetLMotorNumAxes(IntPtr j);
1278
1279 [DllImport("ode", EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
1280 public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
1281
1282 [DllImport("ode", EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
1283 public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
1284
1285 [DllImport("ode", EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
1286 public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
1287
1288 [DllImport("ode", EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
1289 public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
1290
1291 [DllImport("ode", EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
1292 public static extern dReal JointGetPRParam(IntPtr j, int parameter);
1293
1294 [DllImport("ode", EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
1295 public static extern dReal JointGetPRPosition(IntPtr j);
1296
1297 [DllImport("ode", EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
1298 public static extern dReal JointGetPRPositionRate(IntPtr j);
1299
1300 [DllImport("ode", EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
1301 public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
1302
1303 [DllImport("ode", EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
1304 public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
1305
1306 [DllImport("ode", EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
1307 public static extern dReal JointGetSliderPosition(IntPtr j);
1308
1309 [DllImport("ode", EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
1310 public static extern dReal JointGetSliderPositionRate(IntPtr j);
1311
1312 [DllImport("ode", EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
1313 public static extern JointType JointGetType(IntPtr j);
1314
1315 [DllImport("ode", EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1316 public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
1317
1318 [DllImport("ode", EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
1319 public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
1320
1321 [DllImport("ode", EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
1322 public static extern dReal JointGetUniversalAngle1(IntPtr j);
1323
1324 [DllImport("ode", EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
1325 public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
1326
1327 [DllImport("ode", EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
1328 public static extern dReal JointGetUniversalAngle2(IntPtr j);
1329
1330 [DllImport("ode", EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
1331 public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
1332
1333 [DllImport("ode", EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
1334 public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
1335
1336 [DllImport("ode", EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1337 public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
1338
1339 [DllImport("ode", EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1340 public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
1341
1342 [DllImport("ode", EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
1343 public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
1344
1345 [DllImport("ode", EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
1346 public static extern IntPtr JointGroupCreate(int max_size);
1347
1348 [DllImport("ode", EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
1349 public static extern void JointGroupDestroy(IntPtr group);
1350
1351 [DllImport("ode", EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
1352 public static extern void JointGroupEmpty(IntPtr group);
1353
1354 [DllImport("ode", EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1355 public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
1356
1357 [DllImport("ode", EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1358 public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1359
1360 [DllImport("ode", EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
1361 public static extern void JointSetAMotorMode(IntPtr j, int mode);
1362
1363 [DllImport("ode", EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1364 public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
1365
1366 [DllImport("ode", EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
1367 public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
1368
1369 [DllImport("ode", EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
1370 public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
1371
1372 [DllImport("ode", EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1373 public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
1374
1375 [DllImport("ode", EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
1376 public static extern void JointSetData(IntPtr j, IntPtr data);
1377
1378 [DllImport("ode", EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
1379 public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
1380
1381 [DllImport("ode", EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
1382 public static extern void JointSetFixed(IntPtr j);
1383
1384 [DllImport("ode", EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1385 public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
1386
1387 [DllImport("ode", EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
1388 public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1389
1390 [DllImport("ode", EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
1391 public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
1392
1393 [DllImport("ode", EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
1394 public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
1395
1396 [DllImport("ode", EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1397 public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
1398
1399 [DllImport("ode", EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1400 public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
1401
1402 [DllImport("ode", EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1403 public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
1404
1405 [DllImport("ode", EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
1406 public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
1407
1408 [DllImport("ode", EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1409 public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1410
1411 [DllImport("ode", EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1412 public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
1413
1414 [DllImport("ode", EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
1415 public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
1416
1417 [DllImport("ode", EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
1418 public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
1419
1420 [DllImport("ode", EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
1421 public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
1422
1423 [DllImport("ode", EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
1424 public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
1425
1426 [DllImport("ode", EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
1427 public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
1428
1429 [DllImport("ode", EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
1430 public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
1431
1432 [DllImport("ode", EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
1433 public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
1434
1435 [DllImport("ode", EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
1436 public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
1437
1438 [DllImport("ode", EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
1439 public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
1440
1441 [DllImport("ode", EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
1442 public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1443
1444 [DllImport("ode", EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
1445 public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
1446
1447 [DllImport("ode", EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1448 public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
1449
1450 [DllImport("ode", EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1451 public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
1452
1453 [DllImport("ode", EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1454 public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
1455
1456 [DllImport("ode", EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
1457 public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
1458
1459 [DllImport("ode", EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
1460 public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
1461
1462 [DllImport("ode", EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
1463 public static extern void MassAdd(ref Mass a, ref Mass b);
1464
1465 [DllImport("ode", EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
1466 public static extern void MassAdjust(ref Mass m, dReal newmass);
1467
1468 [DllImport("ode", EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
1469 public static extern bool MassCheck(ref Mass m);
1470
1471 [DllImport("ode", EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1472 public static extern void MassRotate(out Mass mass, ref Matrix3 R);
1473
1474 [DllImport("ode", EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1475 public static extern void MassRotate(out Mass mass, ref dReal M00);
1476
1477 [DllImport("ode", EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
1478 public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
1479
1480 [DllImport("ode", EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
1481 public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
1482
1483 [DllImport("ode", EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
1484 public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1485
1486 [DllImport("ode", EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
1487 public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1488
1489 [DllImport("ode", EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
1490 public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1491
1492 [DllImport("ode", EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
1493 public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1494
1495 [DllImport("ode", EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
1496 public static extern void MassSetParameters(out Mass mass, dReal themass,
1497 dReal cgx, dReal cgy, dReal cgz,
1498 dReal i11, dReal i22, dReal i33,
1499 dReal i12, dReal i13, dReal i23);
1500
1501 [DllImport("ode", EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
1502 public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
1503
1504 [DllImport("ode", EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
1505 public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
1506
1507 [DllImport("ode", EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
1508 public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
1509
1510 [DllImport("ode", EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
1511 public static extern void MassSetZero(out Mass mass);
1512
1513 [DllImport("ode", EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
1514 public static extern void MassTranslate(out Mass mass, dReal x, dReal y, dReal z);
1515
1516 [DllImport("ode", EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1517 public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1518
1519 [DllImport("ode", EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
1520 public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1521
1522 [DllImport("ode", EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
1523 public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1524
1525 [DllImport("ode", EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1526 public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
1527
1528 [DllImport("ode", EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
1529 public static extern void QfromR(out Quaternion q, ref Matrix3 R);
1530
1531 [DllImport("ode", EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
1532 public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1533
1534 [DllImport("ode", EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
1535 public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1536
1537 [DllImport("ode", EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
1538 public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1539
1540 [DllImport("ode", EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
1541 public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1542
1543 [DllImport("ode", EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
1544 public static extern void QSetIdentity(out Quaternion q);
1545
1546 [DllImport("ode", EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1547 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
1548
1549 [DllImport("ode", EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1550 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
1551
1552 [DllImport("ode", EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
1553 public static extern dReal RandReal();
1554
1555 [DllImport("ode", EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
1556 public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
1557
1558 [DllImport("ode", EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1559 public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
1560
1561 [DllImport("ode", EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
1562 public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
1563
1564 [DllImport("ode", EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
1565 public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
1566
1567 [DllImport("ode", EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
1568 public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
1569
1570 [DllImport("ode", EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
1571 public static extern void RSetIdentity(out Matrix3 R);
1572
1573 [DllImport("ode", EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
1574 public static extern void SetValue(out dReal a, int n);
1575
1576 [DllImport("ode", EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
1577 public static extern void SetZero(out dReal a, int n);
1578
1579 [DllImport("ode", EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
1580 public static extern IntPtr SimpleSpaceCreate(IntPtr space);
1581
1582 [DllImport("ode", EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
1583 public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
1584
1585 [DllImport("ode", EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
1586 public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
1587
1588 [DllImport("ode", EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
1589 public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
1590
1591 [DllImport("ode", EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
1592 public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
1593
1594 [DllImport("ode", EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
1595 public static extern void SpaceAdd(IntPtr space, IntPtr geom);
1596
1597 [DllImport("ode", EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
1598 public static extern void SpaceClean(IntPtr space);
1599
1600 [DllImport("ode", EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
1601 public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
1602
1603 [DllImport("ode", EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
1604 public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
1605
1606 [DllImport("ode", EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
1607 public static extern void SpaceDestroy(IntPtr space);
1608
1609 [DllImport("ode", EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
1610 public static extern bool SpaceGetCleanup(IntPtr space);
1611
1612 [DllImport("ode", EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
1613 public static extern int SpaceGetNumGeoms(IntPtr space);
1614
1615 [DllImport("ode", EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
1616 public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
1617
1618 [DllImport("ode", EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
1619 public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
1620
1621 [DllImport("ode", EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
1622 public static extern void SpaceRemove(IntPtr space, IntPtr geom);
1623
1624 [DllImport("ode", EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
1625 public static extern void SpaceSetCleanup(IntPtr space, bool mode);
1626
1627 [DllImport("ode", EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
1628 public static extern void VectorScale(out dReal a, ref dReal d, int n);
1629
1630 [DllImport("ode", EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
1631 public static extern IntPtr WorldCreate();
1632
1633 [DllImport("ode", EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
1634 public static extern void WorldDestroy(IntPtr world);
1635
1636 [DllImport("ode", EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1637 public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
1638
1639 [DllImport("ode", EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1640 public static extern bool WorldGetAutoDisableFlag(IntPtr world);
1641
1642 [DllImport("ode", EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1643 public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
1644
1645 [DllImport("ode", EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1646 public static extern int WorldGetAutoDisableSteps(IntPtr world);
1647
1648 [DllImport("ode", EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1649 public static extern dReal WorldGetAutoDisableTime(IntPtr world);
1650
1651 [DllImport("ode", EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1652 public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
1653
1654 [DllImport("ode", EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
1655 public static extern dReal WorldGetCFM(IntPtr world);
1656
1657 [DllImport("ode", EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
1658 public static extern dReal WorldGetERP(IntPtr world);
1659
1660 [DllImport("ode", EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1661 public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
1662
1663 [DllImport("ode", EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1664 public static extern void WorldGetGravity(IntPtr world, out dReal X);
1665
1666 [DllImport("ode", EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1667 public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
1668
1669 [DllImport("ode", EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1670 public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
1671
1672 [DllImport("ode", EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1673 public static extern int WorldGetQuickStepNumIterations(IntPtr world);
1674
1675 [DllImport("ode", EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
1676 public static extern dReal WorldGetQuickStepW(IntPtr world);
1677
1678 [DllImport("ode", EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1679 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
1680
1681 [DllImport("ode", EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1682 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
1683
1684 [DllImport("ode", EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
1685 public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
1686
1687 [DllImport("ode", EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1688 public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
1689
1690 [DllImport("ode", EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1691 public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
1692
1693 [DllImport("ode", EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1694 public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
1695
1696 [DllImport("ode", EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1697 public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
1698
1699 [DllImport("ode", EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1700 public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
1701
1702 [DllImport("ode", EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1703 public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
1704
1705 [DllImport("ode", EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
1706 public static extern void WorldSetCFM(IntPtr world, dReal cfm);
1707
1708 [DllImport("ode", EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1709 public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
1710
1711 [DllImport("ode", EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1712 public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
1713
1714 [DllImport("ode", EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
1715 public static extern void WorldSetERP(IntPtr world, dReal erp);
1716
1717 [DllImport("ode", EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
1718 public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
1719
1720 [DllImport("ode", EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1721 public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
1722
1723 [DllImport("ode", EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
1724 public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
1725
1726 [DllImport("ode", EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
1727 public static extern void WorldStep(IntPtr world, dReal stepsize);
1728
1729 [DllImport("ode", EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
1730 public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
1731 }
1732}
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua
deleted file mode 100644
index b53d54c..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua
+++ /dev/null
@@ -1,31 +0,0 @@
1package.name = "Ode.NET"
2package.kind = "dll"
3package.language = "c#"
4
5-- Build options
6
7 package.defines = { }
8
9 if (options["with-doubles"]) then
10 table.insert(package.defines, "dDOUBLE")
11 else
12 table.insert(package.defines, "dSINGLE")
13 end
14
15 if (options["no-unsafe"]) then
16 table.insert(package.defines, "dNO_UNSAFE_CODE")
17 else
18 package.buildflags = { "unsafe" }
19 end
20
21
22-- Files & Libraries
23
24 package.files = {
25 "AssemblyInfo.cs",
26 "Ode.cs"
27 }
28
29 package.links = {
30 "System"
31 }
diff --git a/libraries/ode-0.9/contrib/Ode.NET/README.TXT b/libraries/ode-0.9/contrib/Ode.NET/README.TXT
deleted file mode 100644
index e6f1262..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/README.TXT
+++ /dev/null
@@ -1,73 +0,0 @@
1Ode.NET - .NET bindings for ODE
2Jason Perkins (starkos@gmail.com)
3
4 THIS IS A WORK IN PROGRESS! I'm not done yet!
5
6
7---------------------------------------------------------------------
8 INSTALLATION
9---------------------------------------------------------------------
10
11 Note that this binding uses a C# 2.0 feature (the
12 UnmanagedFunctionPointer attribute). You will need to use
13 Visual Studio 2005 (C# Express is fine) or Mono's gmcs
14 compiler.
15
16 Start by getting or building ODE as a shared library (DLL).
17
18 The simplest way to build the bindings is probably to create a
19 new library assembly in your tool of choice and drop in the files
20 Ode/Ode.cs and Ode/AssemblyInfo.cs. Define the symbol`dDOUBLE` if
21 you used double-precision math in your ode.dll. Build, done.
22
23 For testing purposes, I have also created bindings for the
24 Drawstuff library and a C# version of the BoxStack demo. You can
25 throw all of these files into a console executable and run it to
26 see the demo.
27
28 If you happen to have Premake installed (http://premake.sf.net/),
29 you can generate build scripts for the library with:
30
31 premake --target (toolset) # for single precision
32 premake --with-doubles --target (toolset) # for double precision
33
34 To build the test application too, use:
35
36 premake --with-tests --target (toolset)
37
38 To build with Mono, you must add the --dotnet parameter to enable
39 support .NET 2.0:
40
41 premake --dotnet mono2 --target gnu
42
43
44---------------------------------------------------------------------
45 USAGE
46---------------------------------------------------------------------
47
48 I have tried to keep things as close to the original C API as I can,
49 rather than forcing a class structure on everyone. Everything is
50 contained within the `Ode.NET` namespace inside a static class
51 named `d`. All ODE IDs are replaced with IntPtrs. A quick example:
52
53 using Ode.NET;
54
55 IntPtr world = d.WorldCreate();
56 IntPtr body = d.BodyCreate(world);
57
58 Take a look at Tests/BoxStack.cs for a more complete example.
59
60
61---------------------------------------------------------------------
62 KNOWN ISSUES
63---------------------------------------------------------------------
64
65 I'm not done yet, so many functions are still missing.
66
67 It is not possible to implement dBodyGetPosition(), dBodyGetRotation(),
68 etc. without resorting to unsafe code, which I was trying to avoid.
69 This binding uses the .NET friendly dBodyCopyPosition(),
70 dBodyCopyRotation(), etc. instead.
71
72 Collision response (contact joints) do not work when built under
73 Mono as double-precision. I have not tried to track down why.
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs b/libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs
deleted file mode 100644
index 1077124..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs
+++ /dev/null
@@ -1,260 +0,0 @@
1using System;
2using System.Collections.Generic;
3using Drawstuff.NET;
4
5namespace Ode.NET
6{
7#if dDOUBLE
8 using dReal = System.Double;
9#else
10 using dReal = System.Single;
11#endif
12
13 public class TestBoxStack
14 {
15 #region Description of convex shape
16
17 static dReal[] planes =
18 {
19 1.0f, 0.0f, 0.0f, 0.25f,
20 0.0f, 1.0f, 0.0f, 0.25f,
21 0.0f, 0.0f, 1.0f, 0.25f,
22 0.0f, 0.0f, -1.0f, 0.25f,
23 0.0f, -1.0f, 0.0f, 0.25f,
24 -1.0f, 0.0f , 0.0f, 0.25f
25 };
26
27 static dReal[] points =
28 {
29 0.25f, 0.25f, 0.25f,
30 -0.25f, 0.25f, 0.25f,
31 0.25f, -0.25f, 0.25f,
32 -0.25f, -0.25f, 0.25f,
33 0.25f, 0.25f, -0.25f,
34 -0.25f,0.25f,-0.25f,
35 0.25f,-0.25f,-0.25f,
36 -0.25f,-0.25f,-0.25f,
37 };
38
39 static int[] polygons =
40 {
41 4, 0, 2, 6, 4,
42 4, 1, 0, 4, 5,
43 4, 0, 1, 3, 2,
44 4, 3, 1, 5, 7,
45 4, 2, 3, 7, 6,
46 4, 5, 4, 6, 7,
47 };
48
49 #endregion
50
51 const int NUM = 100;
52 const float DENSITY = 5.0f;
53 const int MAX_CONTACTS = 8;
54
55 static IntPtr world;
56 static IntPtr space;
57 static IntPtr contactgroup;
58
59 static Queue<IntPtr> obj = new Queue<IntPtr>();
60
61 static d.Vector3 xyz = new d.Vector3(2.1640f, -1.3079f, 1.7600f);
62 static d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
63
64 static d.NearCallback nearCallback = near;
65 static d.ContactGeom[] contacts = new d.ContactGeom[MAX_CONTACTS];
66 static d.Contact contact;
67
68
69 // Called when window is opened - sets up viewpoint and prints usage
70 static void start(int unused)
71 {
72 ds.SetViewpoint(ref xyz, ref hpr);
73 Console.WriteLine("To drop another object, press:");
74 Console.WriteLine(" b for box.");
75 Console.WriteLine(" s for sphere.");
76 Console.WriteLine(" c for capsule.");
77 Console.WriteLine(" y for cylinder.");
78 Console.WriteLine(" v for a convex object.");
79 Console.WriteLine(" x for a composite object.");
80 Console.WriteLine("To select an object, press space.");
81 Console.WriteLine("To disable the selected object, press d.");
82 Console.WriteLine("To enable the selected object, press e.");
83 Console.WriteLine("To toggle showing the geom AABBs, press a.");
84 Console.WriteLine("To toggle showing the contact points, press t.");
85 Console.WriteLine("To toggle dropping from random position/orientation, press r.");
86 Console.WriteLine("To save the current state to 'state.dif', press 1.");
87 }
88
89
90 // Near callback - creates contact joints
91 static void near(IntPtr space, IntPtr g1, IntPtr g2)
92 {
93 IntPtr b1 = d.GeomGetBody(g1);
94 IntPtr b2 = d.GeomGetBody(g2);
95 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
96 return;
97
98 int count = d.Collide(g1, g2, MAX_CONTACTS, contacts, d.ContactGeom.SizeOf);
99 for (int i = 0; i < count; ++i)
100 {
101 contact.geom = contacts[i];
102 IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);
103 d.JointAttach(joint, b1, b2);
104 }
105 }
106
107
108 // Adds a new object to the scene - attaches a body to the geom and
109 // sets the initial position and orientation
110 static void addObject(IntPtr geom, d.Mass mass)
111 {
112 // Create a body for this object
113 IntPtr body = d.BodyCreate(world);
114 d.GeomSetBody(geom, body);
115 d.BodySetMass(body, ref mass);
116 obj.Enqueue(geom);
117
118 // Set the position of the new object
119 d.Matrix3 R;
120 d.BodySetPosition(body, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() + 2);
121 d.RFromAxisAndAngle(out R, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 10 - 5);
122 d.BodySetRotation(body, ref R);
123
124 // Cap the total number of objects
125 if (obj.Count > NUM)
126 {
127 geom = obj.Dequeue();
128 body = d.GeomGetBody(geom);
129 d.BodyDestroy(body);
130 d.GeomDestroy(geom);
131 }
132 }
133
134
135 // Keyboard callback
136 static void command(int cmd)
137 {
138 IntPtr geom;
139 d.Mass mass;
140 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
141
142 Char ch = Char.ToLower((Char)cmd);
143 switch ((Char)ch)
144 {
145 case 'b':
146 d.MassSetBox(out mass, DENSITY, sides.X, sides.Y, sides.Z);
147 geom = d.CreateBox(space, sides.X, sides.Y, sides.Z);
148 addObject(geom, mass);
149 break;
150
151 case 'c':
152 sides.X *= 0.5f;
153 d.MassSetCapsule(out mass, DENSITY, 3, sides.X, sides.Y);
154 geom = d.CreateCapsule(space, sides.X, sides.Y);
155 addObject(geom, mass);
156 break;
157
158 case 'v':
159 d.MassSetBox(out mass, DENSITY, 0.25f, 0.25f, 0.25f);
160 geom = d.CreateConvex(space, planes, planes.Length / 4, points, points.Length / 3, polygons);
161 addObject(geom, mass);
162 break;
163 }
164 }
165
166
167 // Draw an object in the scene
168 static void drawGeom(IntPtr geom)
169 {
170 IntPtr body = d.GeomGetBody(geom);
171
172 d.Vector3 pos;
173 d.BodyCopyPosition(body, out pos);
174
175 d.Matrix3 R;
176 d.BodyCopyRotation(body, out R);
177
178 d.GeomClassID type = d.GeomGetClass(geom);
179 switch (type)
180 {
181 case d.GeomClassID.BoxClass:
182 d.Vector3 sides;
183 d.GeomBoxGetLengths(geom, out sides);
184 ds.DrawBox(ref pos, ref R, ref sides);
185 break;
186 case d.GeomClassID.CapsuleClass:
187 dReal radius, length;
188 d.GeomCapsuleGetParams(geom, out radius, out length);
189 ds.DrawCapsule(ref pos, ref R, length, radius);
190 break;
191 case d.GeomClassID.ConvexClass:
192 ds.DrawConvex(ref pos, ref R, planes, planes.Length / 4, points, points.Length / 3, polygons);
193 break;
194 }
195 }
196
197
198 // Called once per frame; updates the scene
199 static void step(int pause)
200 {
201 d.SpaceCollide(space, IntPtr.Zero, nearCallback);
202 if (pause == 0)
203 d.WorldQuickStep(world, 0.02f);
204 d.JointGroupEmpty(contactgroup);
205
206 ds.SetColor(1.0f, 1.0f, 0.0f);
207 ds.SetTexture(ds.Texture.Wood);
208
209 foreach (IntPtr geom in obj)
210 {
211 drawGeom(geom);
212 }
213 }
214
215
216 static void Main(string[] args)
217 {
218 // Setup pointers to drawstuff callback functions
219 ds.Functions fn;
220 fn.version = ds.VERSION;
221 fn.start = new ds.CallbackFunction(start);
222 fn.step = new ds.CallbackFunction(step);
223 fn.command = new ds.CallbackFunction(command);
224 fn.stop = null;
225 fn.path_to_textures = "../../../../drawstuff/textures";
226 if (args.Length > 0)
227 {
228 fn.path_to_textures = args[0];
229 }
230
231 // Set up contact response parameters
232 contact.surface.mode = d.ContactFlags.Bounce | d.ContactFlags.SoftCFM;
233 contact.surface.mu = d.Infinity;
234 contact.surface.mu2 = 0.0f;
235 contact.surface.bounce = 0.1f;
236 contact.surface.bounce_vel = 0.1f;
237 contact.surface.soft_cfm = 0.01f;
238
239 // Initialize the scene
240 world = d.WorldCreate();
241 space = d.HashSpaceCreate(IntPtr.Zero);
242 contactgroup = d.JointGroupCreate(0);
243 d.WorldSetGravity(world, 0.0f, 0.0f, -0.5f);
244 d.WorldSetCFM(world, 1e-5f);
245 d.WorldSetAutoDisableFlag(world, true);
246 d.WorldSetContactMaxCorrectingVel(world, 0.1f);
247 d.WorldSetContactSurfaceLayer(world, 0.001f);
248 d.CreatePlane(space, 0, 0, 1, 0);
249
250 // Run the scene
251 ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
252
253 // Clean up
254 d.JointGroupDestroy(contactgroup);
255 d.SpaceDestroy(space);
256 d.WorldDestroy(world);
257 d.CloseODE();
258 }
259 }
260}
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua
deleted file mode 100644
index 5253ae1..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua
+++ /dev/null
@@ -1,27 +0,0 @@
1-- This function creates the test packages
2function maketest(name)
3
4 package = newpackage()
5 package.name = name
6 package.kind = "exe"
7 package.language = "c#"
8
9 if (options["with-doubles"]) then
10 package.defines = { "dDOUBLE" }
11 else
12 package.defines = { "dSINGLE " }
13 end
14
15 package.links = {
16 "System",
17 "Ode.NET",
18 "Drawstuff.NET"
19 }
20
21 package.files = {
22 name .. ".cs"
23 }
24
25end
26
27maketest("BoxStack")
diff --git a/libraries/ode-0.9/contrib/Ode.NET/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/premake.lua
deleted file mode 100644
index c25a017..0000000
--- a/libraries/ode-0.9/contrib/Ode.NET/premake.lua
+++ /dev/null
@@ -1,29 +0,0 @@
1project.name = "Ode.NET"
2
3-- Target checking
4
5 if (target and target ~= "vs2005" and target ~= "gnu") then
6 error("Ode.NET requires a .NET 2.0 compiler")
7 end
8
9
10-- Project options
11
12 addoption("with-doubles", "Use double instead of float as base numeric type")
13 addoption("with-tests", "Builds the test applications and DrawStuff library")
14 addoption("no-unsafe", "Exclude functions using unsafe code (dBodyGetPosition, etc.)")
15
16
17-- Build settings
18
19 project.config["Debug"].bindir = "bin/Debug"
20 project.config["Release"].bindir = "bin/Release"
21
22
23-- Packages
24
25 if (options["with-tests"]) then
26 dopackage("Tests")
27 dopackage("Drawstuff")
28 end
29 dopackage("Ode")
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT
deleted file mode 100644
index b2b1995..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT
+++ /dev/null
@@ -1,37 +0,0 @@
1
2This is the BSD-style license for The ODE Model Processor
3----------------------------------------------------------
4
5The ODE Model Processor
6Copyright (c) 2007, Department Of Information Science,
7University of Otago, Dunedin, New Zealand.
8All rights reserved.
9
10Author: Richard Barrington <barri662@student.otago.ac.nz>
11
12Redistribution and use in source and binary forms, with or without
13modification, are permitted provided that the following conditions
14are met:
15
16Redistributions of source code must retain the above copyright notice,
17this list of conditions and the following disclaimer.
18
19Redistributions in binary form must reproduce the above copyright notice,
20this list of conditions and the following disclaimer in the documentation
21and/or other materials provided with the distribution.
22
23Neither the names of the copyright owner nor the names of its
24contributors may be used to endorse or promote products derived from
25this software without specific prior written permission.
26
27THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
28"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
29LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
30FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
31OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
32SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
33TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
34PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
35LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
36NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
37SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT
deleted file mode 100644
index cfe59bc..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT
+++ /dev/null
@@ -1,502 +0,0 @@
1 GNU LESSER GENERAL PUBLIC LICENSE
2 Version 2.1, February 1999
3
4 Copyright (C) 1991, 1999 Free Software Foundation, Inc.
5 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
6 Everyone is permitted to copy and distribute verbatim copies
7 of this license document, but changing it is not allowed.
8
9[This is the first released version of the Lesser GPL. It also counts
10 as the successor of the GNU Library Public License, version 2, hence
11 the version number 2.1.]
12
13 Preamble
14
15 The licenses for most software are designed to take away your
16freedom to share and change it. By contrast, the GNU General Public
17Licenses are intended to guarantee your freedom to share and change
18free software--to make sure the software is free for all its users.
19
20 This license, the Lesser General Public License, applies to some
21specially designated software packages--typically libraries--of the
22Free Software Foundation and other authors who decide to use it. You
23can use it too, but we suggest you first think carefully about whether
24this license or the ordinary General Public License is the better
25strategy to use in any particular case, based on the explanations below.
26
27 When we speak of free software, we are referring to freedom of use,
28not price. Our General Public Licenses are designed to make sure that
29you have the freedom to distribute copies of free software (and charge
30for this service if you wish); that you receive source code or can get
31it if you want it; that you can change the software and use pieces of
32it in new free programs; and that you are informed that you can do
33these things.
34
35 To protect your rights, we need to make restrictions that forbid
36distributors to deny you these rights or to ask you to surrender these
37rights. These restrictions translate to certain responsibilities for
38you if you distribute copies of the library or if you modify it.
39
40 For example, if you distribute copies of the library, whether gratis
41or for a fee, you must give the recipients all the rights that we gave
42you. You must make sure that they, too, receive or can get the source
43code. If you link other code with the library, you must provide
44complete object files to the recipients, so that they can relink them
45with the library after making changes to the library and recompiling
46it. And you must show them these terms so they know their rights.
47
48 We protect your rights with a two-step method: (1) we copyright the
49library, and (2) we offer you this license, which gives you legal
50permission to copy, distribute and/or modify the library.
51
52 To protect each distributor, we want to make it very clear that
53there is no warranty for the free library. Also, if the library is
54modified by someone else and passed on, the recipients should know
55that what they have is not the original version, so that the original
56author's reputation will not be affected by problems that might be
57introduced by others.
58
59 Finally, software patents pose a constant threat to the existence of
60any free program. We wish to make sure that a company cannot
61effectively restrict the users of a free program by obtaining a
62restrictive license from a patent holder. Therefore, we insist that
63any patent license obtained for a version of the library must be
64consistent with the full freedom of use specified in this license.
65
66 Most GNU software, including some libraries, is covered by the
67ordinary GNU General Public License. This license, the GNU Lesser
68General Public License, applies to certain designated libraries, and
69is quite different from the ordinary General Public License. We use
70this license for certain libraries in order to permit linking those
71libraries into non-free programs.
72
73 When a program is linked with a library, whether statically or using
74a shared library, the combination of the two is legally speaking a
75combined work, a derivative of the original library. The ordinary
76General Public License therefore permits such linking only if the
77entire combination fits its criteria of freedom. The Lesser General
78Public License permits more lax criteria for linking other code with
79the library.
80
81 We call this license the "Lesser" General Public License because it
82does Less to protect the user's freedom than the ordinary General
83Public License. It also provides other free software developers Less
84of an advantage over competing non-free programs. These disadvantages
85are the reason we use the ordinary General Public License for many
86libraries. However, the Lesser license provides advantages in certain
87special circumstances.
88
89 For example, on rare occasions, there may be a special need to
90encourage the widest possible use of a certain library, so that it becomes
91a de-facto standard. To achieve this, non-free programs must be
92allowed to use the library. A more frequent case is that a free
93library does the same job as widely used non-free libraries. In this
94case, there is little to gain by limiting the free library to free
95software only, so we use the Lesser General Public License.
96
97 In other cases, permission to use a particular library in non-free
98programs enables a greater number of people to use a large body of
99free software. For example, permission to use the GNU C Library in
100non-free programs enables many more people to use the whole GNU
101operating system, as well as its variant, the GNU/Linux operating
102system.
103
104 Although the Lesser General Public License is Less protective of the
105users' freedom, it does ensure that the user of a program that is
106linked with the Library has the freedom and the wherewithal to run
107that program using a modified version of the Library.
108
109 The precise terms and conditions for copying, distribution and
110modification follow. Pay close attention to the difference between a
111"work based on the library" and a "work that uses the library". The
112former contains code derived from the library, whereas the latter must
113be combined with the library in order to run.
114
115 GNU LESSER GENERAL PUBLIC LICENSE
116 TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
117
118 0. This License Agreement applies to any software library or other
119program which contains a notice placed by the copyright holder or
120other authorized party saying it may be distributed under the terms of
121this Lesser General Public License (also called "this License").
122Each licensee is addressed as "you".
123
124 A "library" means a collection of software functions and/or data
125prepared so as to be conveniently linked with application programs
126(which use some of those functions and data) to form executables.
127
128 The "Library", below, refers to any such software library or work
129which has been distributed under these terms. A "work based on the
130Library" means either the Library or any derivative work under
131copyright law: that is to say, a work containing the Library or a
132portion of it, either verbatim or with modifications and/or translated
133straightforwardly into another language. (Hereinafter, translation is
134included without limitation in the term "modification".)
135
136 "Source code" for a work means the preferred form of the work for
137making modifications to it. For a library, complete source code means
138all the source code for all modules it contains, plus any associated
139interface definition files, plus the scripts used to control compilation
140and installation of the library.
141
142 Activities other than copying, distribution and modification are not
143covered by this License; they are outside its scope. The act of
144running a program using the Library is not restricted, and output from
145such a program is covered only if its contents constitute a work based
146on the Library (independent of the use of the Library in a tool for
147writing it). Whether that is true depends on what the Library does
148and what the program that uses the Library does.
149
150 1. You may copy and distribute verbatim copies of the Library's
151complete source code as you receive it, in any medium, provided that
152you conspicuously and appropriately publish on each copy an
153appropriate copyright notice and disclaimer of warranty; keep intact
154all the notices that refer to this License and to the absence of any
155warranty; and distribute a copy of this License along with the
156Library.
157
158 You may charge a fee for the physical act of transferring a copy,
159and you may at your option offer warranty protection in exchange for a
160fee.
161
162 2. You may modify your copy or copies of the Library or any portion
163of it, thus forming a work based on the Library, and copy and
164distribute such modifications or work under the terms of Section 1
165above, provided that you also meet all of these conditions:
166
167 a) The modified work must itself be a software library.
168
169 b) You must cause the files modified to carry prominent notices
170 stating that you changed the files and the date of any change.
171
172 c) You must cause the whole of the work to be licensed at no
173 charge to all third parties under the terms of this License.
174
175 d) If a facility in the modified Library refers to a function or a
176 table of data to be supplied by an application program that uses
177 the facility, other than as an argument passed when the facility
178 is invoked, then you must make a good faith effort to ensure that,
179 in the event an application does not supply such function or
180 table, the facility still operates, and performs whatever part of
181 its purpose remains meaningful.
182
183 (For example, a function in a library to compute square roots has
184 a purpose that is entirely well-defined independent of the
185 application. Therefore, Subsection 2d requires that any
186 application-supplied function or table used by this function must
187 be optional: if the application does not supply it, the square
188 root function must still compute square roots.)
189
190These requirements apply to the modified work as a whole. If
191identifiable sections of that work are not derived from the Library,
192and can be reasonably considered independent and separate works in
193themselves, then this License, and its terms, do not apply to those
194sections when you distribute them as separate works. But when you
195distribute the same sections as part of a whole which is a work based
196on the Library, the distribution of the whole must be on the terms of
197this License, whose permissions for other licensees extend to the
198entire whole, and thus to each and every part regardless of who wrote
199it.
200
201Thus, it is not the intent of this section to claim rights or contest
202your rights to work written entirely by you; rather, the intent is to
203exercise the right to control the distribution of derivative or
204collective works based on the Library.
205
206In addition, mere aggregation of another work not based on the Library
207with the Library (or with a work based on the Library) on a volume of
208a storage or distribution medium does not bring the other work under
209the scope of this License.
210
211 3. You may opt to apply the terms of the ordinary GNU General Public
212License instead of this License to a given copy of the Library. To do
213this, you must alter all the notices that refer to this License, so
214that they refer to the ordinary GNU General Public License, version 2,
215instead of to this License. (If a newer version than version 2 of the
216ordinary GNU General Public License has appeared, then you can specify
217that version instead if you wish.) Do not make any other change in
218these notices.
219
220 Once this change is made in a given copy, it is irreversible for
221that copy, so the ordinary GNU General Public License applies to all
222subsequent copies and derivative works made from that copy.
223
224 This option is useful when you wish to copy part of the code of
225the Library into a program that is not a library.
226
227 4. You may copy and distribute the Library (or a portion or
228derivative of it, under Section 2) in object code or executable form
229under the terms of Sections 1 and 2 above provided that you accompany
230it with the complete corresponding machine-readable source code, which
231must be distributed under the terms of Sections 1 and 2 above on a
232medium customarily used for software interchange.
233
234 If distribution of object code is made by offering access to copy
235from a designated place, then offering equivalent access to copy the
236source code from the same place satisfies the requirement to
237distribute the source code, even though third parties are not
238compelled to copy the source along with the object code.
239
240 5. A program that contains no derivative of any portion of the
241Library, but is designed to work with the Library by being compiled or
242linked with it, is called a "work that uses the Library". Such a
243work, in isolation, is not a derivative work of the Library, and
244therefore falls outside the scope of this License.
245
246 However, linking a "work that uses the Library" with the Library
247creates an executable that is a derivative of the Library (because it
248contains portions of the Library), rather than a "work that uses the
249library". The executable is therefore covered by this License.
250Section 6 states terms for distribution of such executables.
251
252 When a "work that uses the Library" uses material from a header file
253that is part of the Library, the object code for the work may be a
254derivative work of the Library even though the source code is not.
255Whether this is true is especially significant if the work can be
256linked without the Library, or if the work is itself a library. The
257threshold for this to be true is not precisely defined by law.
258
259 If such an object file uses only numerical parameters, data
260structure layouts and accessors, and small macros and small inline
261functions (ten lines or less in length), then the use of the object
262file is unrestricted, regardless of whether it is legally a derivative
263work. (Executables containing this object code plus portions of the
264Library will still fall under Section 6.)
265
266 Otherwise, if the work is a derivative of the Library, you may
267distribute the object code for the work under the terms of Section 6.
268Any executables containing that work also fall under Section 6,
269whether or not they are linked directly with the Library itself.
270
271 6. As an exception to the Sections above, you may also combine or
272link a "work that uses the Library" with the Library to produce a
273work containing portions of the Library, and distribute that work
274under terms of your choice, provided that the terms permit
275modification of the work for the customer's own use and reverse
276engineering for debugging such modifications.
277
278 You must give prominent notice with each copy of the work that the
279Library is used in it and that the Library and its use are covered by
280this License. You must supply a copy of this License. If the work
281during execution displays copyright notices, you must include the
282copyright notice for the Library among them, as well as a reference
283directing the user to the copy of this License. Also, you must do one
284of these things:
285
286 a) Accompany the work with the complete corresponding
287 machine-readable source code for the Library including whatever
288 changes were used in the work (which must be distributed under
289 Sections 1 and 2 above); and, if the work is an executable linked
290 with the Library, with the complete machine-readable "work that
291 uses the Library", as object code and/or source code, so that the
292 user can modify the Library and then relink to produce a modified
293 executable containing the modified Library. (It is understood
294 that the user who changes the contents of definitions files in the
295 Library will not necessarily be able to recompile the application
296 to use the modified definitions.)
297
298 b) Use a suitable shared library mechanism for linking with the
299 Library. A suitable mechanism is one that (1) uses at run time a
300 copy of the library already present on the user's computer system,
301 rather than copying library functions into the executable, and (2)
302 will operate properly with a modified version of the library, if
303 the user installs one, as long as the modified version is
304 interface-compatible with the version that the work was made with.
305
306 c) Accompany the work with a written offer, valid for at
307 least three years, to give the same user the materials
308 specified in Subsection 6a, above, for a charge no more
309 than the cost of performing this distribution.
310
311 d) If distribution of the work is made by offering access to copy
312 from a designated place, offer equivalent access to copy the above
313 specified materials from the same place.
314
315 e) Verify that the user has already received a copy of these
316 materials or that you have already sent this user a copy.
317
318 For an executable, the required form of the "work that uses the
319Library" must include any data and utility programs needed for
320reproducing the executable from it. However, as a special exception,
321the materials to be distributed need not include anything that is
322normally distributed (in either source or binary form) with the major
323components (compiler, kernel, and so on) of the operating system on
324which the executable runs, unless that component itself accompanies
325the executable.
326
327 It may happen that this requirement contradicts the license
328restrictions of other proprietary libraries that do not normally
329accompany the operating system. Such a contradiction means you cannot
330use both them and the Library together in an executable that you
331distribute.
332
333 7. You may place library facilities that are a work based on the
334Library side-by-side in a single library together with other library
335facilities not covered by this License, and distribute such a combined
336library, provided that the separate distribution of the work based on
337the Library and of the other library facilities is otherwise
338permitted, and provided that you do these two things:
339
340 a) Accompany the combined library with a copy of the same work
341 based on the Library, uncombined with any other library
342 facilities. This must be distributed under the terms of the
343 Sections above.
344
345 b) Give prominent notice with the combined library of the fact
346 that part of it is a work based on the Library, and explaining
347 where to find the accompanying uncombined form of the same work.
348
349 8. You may not copy, modify, sublicense, link with, or distribute
350the Library except as expressly provided under this License. Any
351attempt otherwise to copy, modify, sublicense, link with, or
352distribute the Library is void, and will automatically terminate your
353rights under this License. However, parties who have received copies,
354or rights, from you under this License will not have their licenses
355terminated so long as such parties remain in full compliance.
356
357 9. You are not required to accept this License, since you have not
358signed it. However, nothing else grants you permission to modify or
359distribute the Library or its derivative works. These actions are
360prohibited by law if you do not accept this License. Therefore, by
361modifying or distributing the Library (or any work based on the
362Library), you indicate your acceptance of this License to do so, and
363all its terms and conditions for copying, distributing or modifying
364the Library or works based on it.
365
366 10. Each time you redistribute the Library (or any work based on the
367Library), the recipient automatically receives a license from the
368original licensor to copy, distribute, link with or modify the Library
369subject to these terms and conditions. You may not impose any further
370restrictions on the recipients' exercise of the rights granted herein.
371You are not responsible for enforcing compliance by third parties with
372this License.
373
374 11. If, as a consequence of a court judgment or allegation of patent
375infringement or for any other reason (not limited to patent issues),
376conditions are imposed on you (whether by court order, agreement or
377otherwise) that contradict the conditions of this License, they do not
378excuse you from the conditions of this License. If you cannot
379distribute so as to satisfy simultaneously your obligations under this
380License and any other pertinent obligations, then as a consequence you
381may not distribute the Library at all. For example, if a patent
382license would not permit royalty-free redistribution of the Library by
383all those who receive copies directly or indirectly through you, then
384the only way you could satisfy both it and this License would be to
385refrain entirely from distribution of the Library.
386
387If any portion of this section is held invalid or unenforceable under any
388particular circumstance, the balance of the section is intended to apply,
389and the section as a whole is intended to apply in other circumstances.
390
391It is not the purpose of this section to induce you to infringe any
392patents or other property right claims or to contest validity of any
393such claims; this section has the sole purpose of protecting the
394integrity of the free software distribution system which is
395implemented by public license practices. Many people have made
396generous contributions to the wide range of software distributed
397through that system in reliance on consistent application of that
398system; it is up to the author/donor to decide if he or she is willing
399to distribute software through any other system and a licensee cannot
400impose that choice.
401
402This section is intended to make thoroughly clear what is believed to
403be a consequence of the rest of this License.
404
405 12. If the distribution and/or use of the Library is restricted in
406certain countries either by patents or by copyrighted interfaces, the
407original copyright holder who places the Library under this License may add
408an explicit geographical distribution limitation excluding those countries,
409so that distribution is permitted only in or among countries not thus
410excluded. In such case, this License incorporates the limitation as if
411written in the body of this License.
412
413 13. The Free Software Foundation may publish revised and/or new
414versions of the Lesser General Public License from time to time.
415Such new versions will be similar in spirit to the present version,
416but may differ in detail to address new problems or concerns.
417
418Each version is given a distinguishing version number. If the Library
419specifies a version number of this License which applies to it and
420"any later version", you have the option of following the terms and
421conditions either of that version or of any later version published by
422the Free Software Foundation. If the Library does not specify a
423license version number, you may choose any version ever published by
424the Free Software Foundation.
425
426 14. If you wish to incorporate parts of the Library into other free
427programs whose distribution conditions are incompatible with these,
428write to the author to ask for permission. For software which is
429copyrighted by the Free Software Foundation, write to the Free
430Software Foundation; we sometimes make exceptions for this. Our
431decision will be guided by the two goals of preserving the free status
432of all derivatives of our free software and of promoting the sharing
433and reuse of software generally.
434
435 NO WARRANTY
436
437 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
438WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
439EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
440OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
441KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
442IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
443PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
444LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
445THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
446
447 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
448WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
449AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
450FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
451CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
452LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
453RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
454FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
455SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
456DAMAGES.
457
458 END OF TERMS AND CONDITIONS
459
460 How to Apply These Terms to Your New Libraries
461
462 If you develop a new library, and you want it to be of the greatest
463possible use to the public, we recommend making it free software that
464everyone can redistribute and change. You can do so by permitting
465redistribution under these terms (or, alternatively, under the terms of the
466ordinary General Public License).
467
468 To apply these terms, attach the following notices to the library. It is
469safest to attach them to the start of each source file to most effectively
470convey the exclusion of warranty; and each file should have at least the
471"copyright" line and a pointer to where the full notice is found.
472
473 <one line to give the library's name and a brief idea of what it does.>
474 Copyright (C) <year> <name of author>
475
476 This library is free software; you can redistribute it and/or
477 modify it under the terms of the GNU Lesser General Public
478 License as published by the Free Software Foundation; either
479 version 2.1 of the License, or (at your option) any later version.
480
481 This library is distributed in the hope that it will be useful,
482 but WITHOUT ANY WARRANTY; without even the implied warranty of
483 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
484 Lesser General Public License for more details.
485
486 You should have received a copy of the GNU Lesser General Public
487 License along with this library; if not, write to the Free Software
488 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
489
490Also add information on how to contact you by electronic and paper mail.
491
492You should also get your employer (if you work as a programmer) or your
493school, if any, to sign a "copyright disclaimer" for the library, if
494necessary. Here is a sample; alter the names:
495
496 Yoyodyne, Inc., hereby disclaims all copyright interest in the
497 library `Frob' (a library for tweaking knobs) written by James Random Hacker.
498
499 <signature of Ty Coon>, 1 April 1990
500 Ty Coon, President of Vice
501
502That's all there is to it!
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln
deleted file mode 100644
index 6a3f521..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln
+++ /dev/null
@@ -1,20 +0,0 @@
1
2Microsoft Visual Studio Solution File, Format Version 9.00
3# Visual C# Express 2005
4Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "OdeModelProcessor", "OdeModelProcessor\OdeModelProcessor.csproj", "{246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}"
5EndProject
6Global
7 GlobalSection(SolutionConfigurationPlatforms) = preSolution
8 Debug|Any CPU = Debug|Any CPU
9 Release|Any CPU = Release|Any CPU
10 EndGlobalSection
11 GlobalSection(ProjectConfigurationPlatforms) = postSolution
12 {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
13 {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Debug|Any CPU.Build.0 = Debug|Any CPU
14 {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Release|Any CPU.ActiveCfg = Release|Any CPU
15 {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Release|Any CPU.Build.0 = Release|Any CPU
16 EndGlobalSection
17 GlobalSection(SolutionProperties) = preSolution
18 HideSolutionNode = FALSE
19 EndGlobalSection
20EndGlobal
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs
deleted file mode 100644
index 9c974eb..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs
+++ /dev/null
@@ -1,354 +0,0 @@
1/*
2 * The ODE Model Processor
3 * -----------------------
4 *
5 * Copyright 2007, Department of Information Science,
6 * University of Otago, Dunedin, New Zealand.
7 *
8 * Author: Richard Barrington <barri662@student.otago.ac.nz>
9 *
10 * This is a Content Processor and Tag library written for use with
11 * Microsoft Visual C# 2005 Express Edition and Microsoft XNA Game
12 * Studio Express 1.0.
13 *
14 * It can be used to read .x model vertex and index data before
15 * insertion into the content pipeline. This is used to build ODE
16 * Triangle Meshes which are then used for collision detection that
17 * is more accurate than the default XNA bounding boxes or spheres.
18 *
19 * Usage is simple:
20 * Build the library and reference the DLL in your project.
21 * Add the DLL to the Content Pipeline
22 * Set the content processor for you .x models to OdeModelProcessor.
23 *
24 * Create triangle meshes as follows:
25 * 1) Create a space, but only one for all of models.
26 * 2) Create a triangle data.
27 * 3) Load the model.
28 * 4) Retreive the tag from the model.
29 * 6) Build the triangle mesh by calling d.GeomTriMeshDataBuildSimple.
30 *
31 * Eg:
32 * IntPtr space = d.SimpleSpaceCreate(IntPtr.Zero);
33 * IntPtr triangleData = d.GeomTriMeshDataCreate();
34 * Model obj = content.Load<Model>("Content\\mycube");
35 * OdeTag tag = (OdeTag)obj.Tag;
36 * IntPtr vertexArray = tag.getVertices();
37 * IntPtr indexArray = tag.getIndices();
38 * d.GeomTriMeshDataBuildSimple
39 * (
40 * triangleData,
41 * vertexArray, tag.getVertexStride(), tag.getVertexCount(),
42 * indexArray, tag.getIndexCount(), tag.getIndexStride()
43 * );
44 * IntPtr triangleMesh = d.CreateTriMesh(space, triangleData, null, null, null);
45 *
46 * You can load multiple models and test for collisions with something
47 * like this in the update method:
48 *
49 * d.GeomSetPosition(odeTri1, obj1Position.X, obj1Position.Y, obj1Position.Z);
50 * d.GeomSetPosition(odeTri2, obj2Position.X, obj2Position.Y, obj2Position.Z);
51 * int numberOfContacts = d.Collide(odeTri1, odeTri2, ODE_CONTACTS,
52 * contactGeom, d.ContactGeom.SizeOf);
53 *
54 * Where odeTri1 and odeTri2 are triangle meshes you've created, obj1Position
55 * and obj2Position are the positions of your rendered models in the scene,
56 * ODE_CONTACTS is a constant defining the maximum number of contacts
57 * to test for, contactGeom is a d.ContactGeom[] of length ODE_CONTACTS.
58 *
59 * If numberOfContacts is greater than 0, you have a collision.
60 *
61 * Other ODE functions such as d.SpaceCollide() also work; see ODE.NET BoxTest.cs.
62 *
63 * This library is free software; you can redistribute it and/or
64 * modify it under the same terms as the ODE and ODE.Net libraries.
65 * Specifically, the terms are one of EITHER:
66 *
67 * (1) The GNU Lesser General Public License as published by the Free
68 * Software Foundation; either version 2.1 of the License, or (at
69 * your option) any later version. The text of the GNU Lesser
70 * General Public License is included with this library in the
71 * file LICENSE.TXT.
72 *
73 * (2) The BSD-style license that is included with this library in
74 * the file LICENSE-BSD.TXT.
75 *
76 * This library is distributed in the hope that it will be useful,
77 * but WITHOUT ANY WARRANTY; without even the implied warranty of
78 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
79 * LICENSE.TXT and LICENSE-BSD.TXT for more details.
80 *
81 */
82
83using System;
84using System.Collections.Generic;
85using System.Text;
86using Microsoft.Xna.Framework;
87using Microsoft.Xna.Framework.Content;
88using Microsoft.Xna.Framework.Graphics;
89using Microsoft.Xna.Framework.Design;
90using Microsoft.Xna.Framework.Content.Pipeline;
91using Microsoft.Xna.Framework.Content.Pipeline.Graphics;
92using Microsoft.Xna.Framework.Content.Pipeline.Processors;
93using Microsoft.Xna.Framework.Content.Pipeline.Serialization.Compiler;
94using Ode.NET;
95using System.Runtime.InteropServices;
96
97namespace OdeModelProcessor
98{
99 /*
100 * Container for vertex and index data in a format
101 * that ODE.Net can use
102 */
103 public class OdeTag
104 {
105
106 private float[] vertexData;
107 private int[] indexData;
108 private const int indexStride = (sizeof(int));
109 private const int vertexStride = (3 * sizeof(float));
110
111 /* Constructors */
112 public OdeTag()
113 {
114 vertexData = new float[0];
115 indexData = new int[0];
116 }
117
118 public OdeTag(float[] vertexData, int[] indexData)
119 {
120 this.vertexData = vertexData;
121 this.indexData = indexData;
122 }
123
124 /* Data setter */
125 public void setData(float[] vertexData, int[] indexData)
126 {
127 this.vertexData = vertexData;
128 this.indexData = indexData;
129 }
130
131 /* Data appenders */
132 public void appendVertexData(float[] vertexData)
133 {
134 int newVertexDataLength = vertexData.Length;
135 float[] tempVertexArray = new float[newVertexDataLength + this.vertexData.Length];
136 this.vertexData.CopyTo(tempVertexArray, 0);
137 vertexData.CopyTo(tempVertexArray, this.vertexData.Length);
138 this.vertexData = tempVertexArray;
139 }
140
141 public void appendIndexData(int[] indexData)
142 {
143 int newIndexDataLength = indexData.Length;
144 int[] tempIndexArray = new int[newIndexDataLength + this.indexData.Length];
145 this.indexData.CopyTo(tempIndexArray, 0);
146 indexData.CopyTo(tempIndexArray, this.indexData.Length);
147 this.indexData = tempIndexArray;
148 }
149
150 /* Data getters */
151 public float[] getVertexData()
152 {
153 return this.vertexData;
154 }
155
156 public int[] getIndexData()
157 {
158 return this.indexData;
159 }
160
161 /* Native data getters */
162 public IntPtr getVertices()
163 {
164 int count = getVertexData().Length;
165 int memsize = count * Marshal.SizeOf(getVertexData()[0].GetType());
166 IntPtr pointer = Marshal.AllocCoTaskMem(memsize);
167 Marshal.Copy(getVertexData(), 0, pointer, count);
168 return pointer;
169 }
170
171 public IntPtr getIndices()
172 {
173 int count = getIndexData().Length;
174 int memsize = count * Marshal.SizeOf(getIndexData()[0].GetType());
175 IntPtr pointer = Marshal.AllocCoTaskMem(memsize);
176 Marshal.Copy(getIndexData(), 0, pointer, count);
177 return pointer;
178 }
179
180 /* Count getters */
181 public int getVertexCount()
182 {
183 return vertexData.Length/3;
184 }
185
186 public int getIndexCount()
187 {
188 return indexData.Length;
189 }
190
191 /* Stride getters */
192 public int getVertexStride()
193 {
194 return vertexStride;
195 }
196
197 public int getIndexStride()
198 {
199 return indexStride;
200 }
201
202 /*
203 * Convienience method to build the mesh and return it. The triangleData
204 * is passed in to allow the calling application to delete it afterwards.
205 *
206 * Be sure to destroy the returned TriangleMesh in the client application.
207 *
208 * Can't destroy the index and vertex arrays here though, so best to handle
209 * this manually - only use this method if nothing else makes sense.
210 */
211 public IntPtr getTriangleMesh(IntPtr space, IntPtr triangleData)
212 {
213 d.GeomTriMeshDataBuildSimple(
214 triangleData,
215 getVertices(), getVertexStride(), getVertexCount(),
216 getIndices(), getIndexCount(), getIndexStride()
217 );
218 return d.CreateTriMesh(space, triangleData, null, null, null);
219 }
220
221 }
222
223 /*
224 * Subclass of the XNA .x model processor, which creates and appends a tag
225 * containing vertex and index data for ODE.Net to use.
226 */
227 [ContentProcessor]
228 public class OdeModelProcessor : ModelProcessor
229 {
230 private OdeTag tag;
231 private int indexOffset = 0;
232
233 public override ModelContent Process(NodeContent input, ContentProcessorContext context)
234 {
235 tag = new OdeTag();
236 GenerateVerticesRecursive( input );
237 ModelContent model = base.Process(input, context);
238 model.Tag = tag;
239 indexOffset = 0;
240 return model;
241 }
242
243 public void GenerateVerticesRecursive(NodeContent input)
244 {
245
246 MeshContent mesh = input as MeshContent;
247
248 if (mesh != null)
249 {
250 GeometryContentCollection gc = mesh.Geometry;
251 foreach (GeometryContent g in gc)
252 {
253 VertexContent vc = g.Vertices;
254 IndirectPositionCollection ipc = vc.Positions;
255 IndexCollection ic = g.Indices;
256
257 float[] vertexData = new float[ipc.Count * 3];
258 for (int i = 0; i < ipc.Count; i++)
259 {
260
261 Vector3 v0 = ipc[i];
262 vertexData[(i * 3) + 0] = v0.X;
263 vertexData[(i * 3) + 1] = v0.Y;
264 vertexData[(i * 3) + 2] = v0.Z;
265
266 }
267
268 int[] indexData = new int[ic.Count];
269 for (int j = 0; j < ic.Count; j ++)
270 {
271
272 indexData[j] = ic[j] + indexOffset;
273
274 }
275
276 tag.appendVertexData(vertexData);
277 tag.appendIndexData(indexData);
278 indexOffset += ipc.Count;
279 }
280
281 }
282
283 foreach (NodeContent child in input.Children)
284 {
285 GenerateVerticesRecursive(child);
286 }
287
288 }
289
290 }
291
292 /* Writer for the OdeTag class */
293 [ContentTypeWriter]
294 public class OdeTagWriter : ContentTypeWriter<OdeTag>
295 {
296
297 protected override void Write(ContentWriter output, OdeTag value)
298 {
299 float[] vertexData = value.getVertexData();
300 int[] indexData = value.getIndexData();
301 output.Write(vertexData.Length);
302 output.Write(indexData.Length);
303 for (int j = 0; j < vertexData.Length; j++)
304 {
305 output.Write(vertexData[j]);
306 }
307 for (int i = 0; i < indexData.Length; i++)
308 {
309 output.Write(indexData[i]);
310 }
311 }
312
313 public override string GetRuntimeType(TargetPlatform targetPlatform)
314 {
315 return typeof(OdeTag).AssemblyQualifiedName;
316 }
317
318 public override string GetRuntimeReader(TargetPlatform targetPlatform)
319 {
320 return "OdeModelProcessor.OdeTagReader, OdeModelProcessor, Version=1.0.0.0, Culture=neutral";
321 }
322
323 }
324
325 /* Reader for the OdeTag class */
326 public class OdeTagReader : ContentTypeReader<OdeTag>
327 {
328 protected override OdeTag Read(ContentReader input, OdeTag existingInstance)
329 {
330 float[] vertexData = new float[input.ReadInt32()];
331 int[] indexData = new int[input.ReadInt32()];
332 for (int j = 0; j < vertexData.Length; j++)
333 {
334 vertexData[j] = input.ReadSingle();
335 }
336 for (int i = 0; i < indexData.Length; i++)
337 {
338 indexData[i] = input.ReadInt32();
339 }
340
341 OdeTag tag = null;
342 if (existingInstance == null)
343 {
344 tag = new OdeTag(vertexData, indexData);
345 }
346 else
347 {
348 tag = existingInstance;
349 tag.setData(vertexData, indexData);
350 }
351 return tag;
352 }
353 }
354}
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj
deleted file mode 100644
index 3a36a12..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj
+++ /dev/null
@@ -1,69 +0,0 @@
1<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
2 <PropertyGroup>
3 <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
4 <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
5 <ProductVersion>8.0.50727</ProductVersion>
6 <SchemaVersion>2.0</SchemaVersion>
7 <ProjectGuid>{246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}</ProjectGuid>
8 <OutputType>Library</OutputType>
9 <AppDesignerFolder>Properties</AppDesignerFolder>
10 <RootNamespace>OdeModelProcessor</RootNamespace>
11 <AssemblyName>OdeModelProcessor</AssemblyName>
12 </PropertyGroup>
13 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
14 <DebugSymbols>true</DebugSymbols>
15 <DebugType>full</DebugType>
16 <Optimize>false</Optimize>
17 <OutputPath>bin\Debug\</OutputPath>
18 <DefineConstants>DEBUG</DefineConstants>
19 <ErrorReport>prompt</ErrorReport>
20 <WarningLevel>4</WarningLevel>
21 <AllowUnsafeBlocks>false</AllowUnsafeBlocks>
22 <UseVSHostingProcess>false</UseVSHostingProcess>
23 </PropertyGroup>
24 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
25 <DebugType>pdbonly</DebugType>
26 <Optimize>true</Optimize>
27 <OutputPath>bin\Release\</OutputPath>
28 <DefineConstants>
29 </DefineConstants>
30 <ErrorReport>prompt</ErrorReport>
31 <WarningLevel>4</WarningLevel>
32 <AllowUnsafeBlocks>false</AllowUnsafeBlocks>
33 <UseVSHostingProcess>false</UseVSHostingProcess>
34 </PropertyGroup>
35 <ItemGroup>
36 <Reference Include="Microsoft.Xna.Framework, Version=1.0.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=x86" />
37 <Reference Include="Microsoft.Xna.Framework.Content.Pipeline, Version=1.0.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=x86" />
38 <Reference Include="Ode.NET, Version=0.7.0.0, Culture=neutral, processorArchitecture=MSIL">
39 <SpecificVersion>False</SpecificVersion>
40 <HintPath>..\..\ODE\Ode.NET-0.8\bin\Release\Ode.NET.dll</HintPath>
41 </Reference>
42 <Reference Include="System" />
43 <Reference Include="System.Data" />
44 <Reference Include="System.Xml" />
45 </ItemGroup>
46 <ItemGroup>
47 <Compile Include="OdeModelProcessor.cs" />
48 <Compile Include="Properties\AssemblyInfo.cs" />
49 <Compile Include="Properties\Settings.Designer.cs">
50 <AutoGen>True</AutoGen>
51 <DesignTimeSharedInput>True</DesignTimeSharedInput>
52 <DependentUpon>Settings.settings</DependentUpon>
53 </Compile>
54 </ItemGroup>
55 <ItemGroup>
56 <None Include="Properties\Settings.settings">
57 <Generator>SettingsSingleFileGenerator</Generator>
58 <LastGenOutput>Settings.Designer.cs</LastGenOutput>
59 </None>
60 </ItemGroup>
61 <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
62 <!-- To modify your build process, add your task inside one of the targets below and uncomment it.
63 Other similar extension points exist, see Microsoft.Common.targets.
64 <Target Name="BeforeBuild">
65 </Target>
66 <Target Name="AfterBuild">
67 </Target>
68 -->
69</Project> \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs
deleted file mode 100644
index 1ecad3e..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs
+++ /dev/null
@@ -1,35 +0,0 @@
1using System.Reflection;
2using System.Runtime.CompilerServices;
3using System.Runtime.InteropServices;
4
5// General Information about an assembly is controlled through the following
6// set of attributes. Change these attribute values to modify the information
7// associated with an assembly.
8[assembly: AssemblyTitle("OdeModelProcessor")]
9[assembly: AssemblyDescription("")]
10[assembly: AssemblyConfiguration("")]
11[assembly: AssemblyCompany("")]
12[assembly: AssemblyProduct("OdeModelProcessor")]
13[assembly: AssemblyCopyright("Copyright © 2007")]
14[assembly: AssemblyTrademark("")]
15[assembly: AssemblyCulture("")]
16
17// Setting ComVisible to false makes the types in this assembly not visible
18// to COM components. If you need to access a type in this assembly from
19// COM, set the ComVisible attribute to true on that type.
20[assembly: ComVisible(false)]
21
22// The following GUID is for the ID of the typelib if this project is exposed to COM
23[assembly: Guid("29c5609a-cd5f-480b-b4ef-5c11de022268")]
24
25// Version information for an assembly consists of the following four values:
26//
27// Major Version
28// Minor Version
29// Build Number
30// Revision
31//
32// You can specify all the values or you can default the Revision and Build Numbers
33// by using the '*' as shown below:
34[assembly: AssemblyVersion("1.0.0.0")]
35[assembly: AssemblyFileVersion("1.0.0.0")]
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs
deleted file mode 100755
index 6ab60a8..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs
+++ /dev/null
@@ -1,26 +0,0 @@
1//------------------------------------------------------------------------------
2// <auto-generated>
3// This code was generated by a tool.
4// Runtime Version:2.0.50727.832
5//
6// Changes to this file may cause incorrect behavior and will be lost if
7// the code is regenerated.
8// </auto-generated>
9//------------------------------------------------------------------------------
10
11namespace OdeModelProcessor.Properties {
12
13
14 [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]
15 [global::System.CodeDom.Compiler.GeneratedCodeAttribute("Microsoft.VisualStudio.Editors.SettingsDesigner.SettingsSingleFileGenerator", "8.0.0.0")]
16 internal sealed partial class Settings : global::System.Configuration.ApplicationSettingsBase {
17
18 private static Settings defaultInstance = ((Settings)(global::System.Configuration.ApplicationSettingsBase.Synchronized(new Settings())));
19
20 public static Settings Default {
21 get {
22 return defaultInstance;
23 }
24 }
25 }
26}
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings
deleted file mode 100755
index 15034e7..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings
+++ /dev/null
@@ -1,6 +0,0 @@
1<?xml version='1.0' encoding='utf-8'?>
2<SettingsFile xmlns="http://schemas.microsoft.com/VisualStudio/2004/01/settings" CurrentProfile="(Default)">
3 <Profiles>
4 <Profile Name="(Default)" />
5 </Profiles>
6</SettingsFile>
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT
deleted file mode 100644
index 97683f9..0000000
--- a/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT
+++ /dev/null
@@ -1,78 +0,0 @@
1The ODE Model Processor
2-----------------------
3
4Copyright 2007, Department of Information Science,
5University of Otago, Dunedin, New Zealand.
6
7Author: Richard Barrington <barri662@student.otago.ac.nz>
8
9This is a Content Processor and Tag library written for use with
10Microsoft Visual C# 2005 Express Edition and Microsoft XNA Game
11Studio Express 1.0.
12
13It can be used to read .x model vertex and index data before
14insertion into the content pipeline. This is used to build ODE
15Triangle Meshes which are then used for collision detection that
16is more accurate than the default XNA bounding boxes or spheres.
17
18Usage is fairly simple:
19Build the library and reference the DLL in your project.
20Add the DLL to the Content Pipeline
21Set the content processor for you .x models to OdeModelProcessor.
22
23Create triangle meshes as follows:
241) Create a space, but only one for all of models.
252) Create a triangle data.
263) Load the model.
274) Retreive the tag from the model.
286) Build the triangle mesh by calling d.GeomTriMeshDataBuildSimple.
29
30Eg:
31IntPtr space = d.SimpleSpaceCreate(IntPtr.Zero);
32IntPtr triangleData = d.GeomTriMeshDataCreate();
33Model obj = content.Load<Model>("Content\\mycube");
34OdeTag tag = (OdeTag)obj.Tag;
35IntPtr vertexArray = tag.getVertices();
36IntPtr indexArray = tag.getIndices();
37d.GeomTriMeshDataBuildSimple
38(
39 triangleData,
40 vertexArray, tag.getVertexStride(), tag.getVertexCount(),
41 indexArray, tag.getIndexCount(), tag.getIndexStride()
42);
43IntPtr triangleMesh = d.CreateTriMesh(space, triangleData, null, null, null);
44
45You can load multiple models and test for collisions with something
46like this in the update method:
47
48d.GeomSetPosition(odeTri1, obj1Position.X, obj1Position.Y, obj1Position.Z);
49d.GeomSetPosition(odeTri2, obj2Position.X, obj2Position.Y, obj2Position.Z);
50int numberOfContacts = d.Collide(odeTri1, odeTri2, ODE_CONTACTS,
51 contactGeom, d.ContactGeom.SizeOf);
52
53Where odeTri1 and odeTri2 are triangle meshes you've created, obj1Position
54and obj2Position are the positions of your rendered models in the scene,
55ODE_CONTACTS is a constant defining the maximum number of contacts
56to test for, contactGeom is a d.ContactGeom[] of length ODE_CONTACTS.
57
58If numberOfContacts is greater than 0, you have a collision.
59
60Other ODE functions such as d.SpaceCollide() also work; see ODE.NET BoxTest.cs.
61
62This library is free software; you can redistribute it and/or
63modify it under the same terms as the ODE and ODE.Net libraries.
64Specifically, the terms are one of EITHER:
65
66 (1) The GNU Lesser General Public License as published by the Free
67 Software Foundation; either version 2.1 of the License, or (at
68 your option) any later version. The text of the GNU Lesser
69 General Public License is included with this library in the
70 file LICENSE.TXT.
71
72 (2) The BSD-style license that is included with this library in
73 the file LICENSE-BSD.TXT.
74
75This library is distributed in the hope that it will be useful,
76but WITHOUT ANY WARRANTY; without even the implied warranty of
77MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
78LICENSE.TXT and LICENSE-BSD.TXT for more details.
diff --git a/libraries/ode-0.9/contrib/README b/libraries/ode-0.9/contrib/README
deleted file mode 100644
index dbfaf7f..0000000
--- a/libraries/ode-0.9/contrib/README
+++ /dev/null
@@ -1,19 +0,0 @@
1This directory contains ODE-related things that have been generously
2contributed by ODE's users. Why is this stuff here and not integrated
3into the main ODE source tree? There may be several reasons:
4
5 * The author(s) and ODE maintainers(s) may not have had time to do
6 the job.
7
8 * It may not be finished.
9
10 * It may contribute functionality that is useful but not considered
11 to be part of ODE's core.
12
13No guarantees are made about the code in this directory - it may not
14be documented, it may not have been tested, and it may not even
15compile for you.
16
17Each package has its own subdirectory, with a README file in that
18directory explaining what the package is.
19
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h b/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h
deleted file mode 100644
index b445353..0000000
--- a/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h
+++ /dev/null
@@ -1,100 +0,0 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2#ifndef _ODE_COLLISION_STD_INTERNAL_H_
3#define _ODE_COLLISION_STD_INTERNAL_H_
4
5#include <ode/common.h>
6#include "collision_kernel.h"
7
8struct dxSphere : public dxGeom {
9 dReal radius; // sphere radius
10 dxSphere (dSpaceID space, dReal _radius);
11 void computeAABB();
12};
13
14
15struct dxBox : public dxGeom {
16 dVector3 side; // side lengths (x,y,z)
17 dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
18 void computeAABB();
19};
20
21
22struct dxCCylinder : public dxGeom {
23 dReal radius,lz; // radius, length along z axis
24 dxCCylinder (dSpaceID space, dReal _radius, dReal _length);
25 void computeAABB();
26};
27
28
29struct dxPlane : public dxGeom {
30 dReal p[4];
31 dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
32 void computeAABB();
33};
34
35struct dxCylinder : public dxGeom {
36 dReal radius,lz; // radius, length along z axis
37 dxCylinder (dSpaceID space, dReal _radius, dReal _length);
38 void computeAABB();
39};
40
41struct dxCone : public dxGeom {
42 dReal radius,lz;
43 dxCone(dSpaceID space, dReal _radius,dReal _length);
44 ~dxCone();
45 void computeAABB();
46};
47
48struct dxRay : public dxGeom {
49 dReal length;
50 dxRay (dSpaceID space, dReal _length);
51 void computeAABB();
52};
53
54struct dxTerrainY : public dxGeom {
55 dReal m_vLength;
56 dReal *m_pHeights;
57 dReal m_vMinHeight;
58 dReal m_vMaxHeight;
59 dReal m_vNodeLength;
60 int m_nNumNodesPerSide;
61 int m_nNumNodesPerSideShift;
62 int m_nNumNodesPerSideMask;
63 int m_bFinite;
64 dxTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
65 ~dxTerrainY();
66 void computeAABB();
67 dReal GetHeight(dReal x,dReal z);
68 dReal GetHeight(int x,int z);
69 int dCollideTerrainUnit(int x,int z,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip);
70 bool IsOnTerrain(int nx,int nz,int w,dReal *pos);
71};
72
73struct dxTerrainZ : public dxGeom {
74 dReal m_vLength;
75 dReal *m_pHeights;
76 dReal m_vMinHeight;
77 dReal m_vMaxHeight;
78 dReal m_vNodeLength;
79 int m_nNumNodesPerSide;
80 int m_nNumNodesPerSideShift;
81 int m_nNumNodesPerSideMask;
82 int m_bFinite;
83 dxTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
84 ~dxTerrainZ();
85 void computeAABB();
86 dReal GetHeight(dReal x,dReal y);
87 dReal GetHeight(int x,int y);
88 int dCollideTerrainUnit(int x,int y,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip);
89 bool IsOnTerrain(int nx,int ny,int w,dReal *pos);
90};
91
92#ifndef MIN
93#define MIN(a,b) ((a<b)?a:b)
94#endif
95
96#ifndef MAX
97#define MAX(a,b) ((a>b)?a:b)
98#endif
99
100#endif //_ODE_COLLISION_STD_INTERNAL_H_ \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
deleted file mode 100644
index 8c5c3be..0000000
--- a/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
+++ /dev/null
@@ -1,504 +0,0 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2//some code inspired by Magic Software
3#include <ode/common.h>
4#include <ode/collision.h>
5#include <ode/matrix.h>
6#include <ode/rotation.h>
7#include <ode/odemath.h>
8#include "collision_kernel.h"
9#include "collision_std.h"
10#include "collision_std_internal.h"
11#include "collision_util.h"
12#include <drawstuff/drawstuff.h>
13#include "windows.h"
14#include "ode\ode.h"
15
16#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
17const dReal fEPSILON = 1e-9f;
18
19dxCone::dxCone (dSpaceID space, dReal _radius,dReal _length) :
20dxGeom (space,1)
21{
22 dAASSERT(_radius > 0.f);
23 dAASSERT(_length > 0.f);
24 type = dConeClass;
25 radius = _radius;
26 lz = _length;
27}
28
29dxCone::~dxCone()
30{
31}
32
33void dxCone::computeAABB()
34{
35 const dMatrix3& R = final_posr->R;
36 const dVector3& pos = final_posr->pos;
37
38 dReal xrange = dFabs(R[2] * lz) + radius;
39 dReal yrange = dFabs(R[6] * lz) + radius;
40 dReal zrange = dFabs(R[10] * lz) + radius;
41 aabb[0] = pos[0] - xrange;
42 aabb[1] = pos[0] + xrange;
43 aabb[2] = pos[1] - yrange;
44 aabb[3] = pos[1] + yrange;
45 aabb[4] = pos[2] - zrange;
46 aabb[5] = pos[2] + zrange;
47}
48
49dGeomID dCreateCone(dSpaceID space, dReal _radius,dReal _length)
50{
51 return new dxCone(space,_radius,_length);
52}
53
54void dGeomConeSetParams (dGeomID g, dReal _radius, dReal _length)
55{
56 dUASSERT (g && g->type == dConeClass,"argument not a cone");
57 dAASSERT (_radius > 0.f);
58 dAASSERT (_length > 0.f);
59 g->recomputePosr();
60 dxCone *c = (dxCone*) g;
61 c->radius = _radius;
62 c->lz = _length;
63 dGeomMoved (g);
64}
65
66
67void dGeomConeGetParams (dGeomID g, dReal *_radius, dReal *_length)
68{
69 dUASSERT (g && g->type == dConeClass,"argument not a cone");
70 g->recomputePosr();
71 dxCone *c = (dxCone*) g;
72 *_radius = c->radius;
73 *_length = c->lz;
74}
75
76//positive inside
77dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z)
78{
79 dUASSERT (g && g->type == dConeClass,"argument not a cone");
80
81 g->recomputePosr();
82 dxCone *cone = (dxCone*) g;
83
84 dVector3 tmp,q;
85 tmp[0] = x - cone->final_posr->pos[0];
86 tmp[1] = y - cone->final_posr->pos[1];
87 tmp[2] = z - cone->final_posr->pos[2];
88 dMULTIPLY1_331 (q,cone->final_posr->R,tmp);
89
90 dReal r = cone->radius;
91 dReal h = cone->lz;
92
93 dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]);
94 dReal d1 = q[2];
95 dReal d2 = h-q[2];
96
97 if (d0 < d1) {
98 if (d0 < d2) return d0; else return d2;
99 }
100 else {
101 if (d1 < d2) return d1; else return d2;
102 }
103}
104
105//plane plane
106bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
107 dVector3 LinePos,dVector3 LineDir)
108{
109 // If Cross(N0,N1) is zero, then either planes are parallel and separated
110 // or the same plane. In both cases, 'false' is returned. Otherwise,
111 // the intersection line is
112 //
113 // L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
114 //
115 // for some coefficients c0 and c1 and for t any real number (the line
116 // parameter). Taking dot products with the normals,
117 //
118 // d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
119 // d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
120 //
121 // which are two equations in two unknowns. The solution is
122 //
123 // c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
124 // c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
125 //
126 // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
127/*
128 Real fN00 = rkPlane0.Normal().SquaredLength();
129 Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
130 Real fN11 = rkPlane1.Normal().SquaredLength();
131 Real fDet = fN00*fN11 - fN01*fN01;
132
133 if ( Math::FAbs(fDet) < gs_fEpsilon )
134 return false;
135
136 Real fInvDet = 1.0f/fDet;
137 Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
138 Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;
139
140 rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
141 rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
142 return true;
143*/
144 dReal fN00 = dLENGTHSQUARED(Plane0);
145 dReal fN01 = dDOT(Plane0,Plane1);
146 dReal fN11 = dLENGTHSQUARED(Plane1);
147 dReal fDet = fN00*fN11 - fN01*fN01;
148
149 if ( fabs(fDet) < fEPSILON)
150 return false;
151
152 dReal fInvDet = 1.0f/fDet;
153 dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
154 dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;
155
156 dCROSS(LineDir,=,Plane0,Plane1);
157 dNormalize3(LineDir);
158
159 dVector3 Temp0,Temp1;
160 dOPC(Temp0,*,Plane0,fC0);
161 dOPC(Temp1,*,Plane1,fC1);
162 dOP(LinePos,+,Temp0,Temp1);
163
164 return true;
165}
166
167//plane ray
168bool FindIntersectionPlaneRay(const dReal Plane[4],
169 const dVector3 &LinePos,const dVector3 &LineDir,
170 dReal &u,dVector3 &Pos)
171{
172/*
173 u = (A*X1 + B*Y1 + C*Z1 + D) / (A*(X1-X2) + B*(Y1-Y2)+C*(Z1-Z2))
174*/
175 dReal fDet = -dDot(Plane,LineDir,3);
176
177 if ( fabs(fDet) < fEPSILON)
178 return false;
179
180 u = (dDot(Plane,LinePos,3) - Plane[3]) / fDet;
181 dOPC(Pos,*,LineDir,u);
182 dOPE(Pos,+=,LinePos);
183
184 return true;
185}
186
187int SolveQuadraticPolynomial(dReal a,dReal b,dReal c,dReal &x0,dReal &x1)
188{
189 dReal d = b*b - 4*a*c;
190 int NumRoots = 0;
191 dReal dr;
192
193 if (d < 0.f)
194 return NumRoots;
195
196 if (d == 0.f)
197 {
198 NumRoots = 1;
199 dr = 0.f;
200 }
201 else
202 {
203 NumRoots = 2;
204 dr = sqrtf(d);
205 }
206
207 x0 = (-b -dr) / (2.f * a);
208 x1 = (-b +dr) / (2.f * a);
209
210 return NumRoots;
211}
212/*
213const int VALID_INTERSECTION = 1<<0;
214const int POS_TEST_FAILEDT0 = 1<<0;
215const int POS_TEST_FAILEDT1 = 1<<1;
216*/
217int ProcessConeRayIntersectionPoint( dReal r,dReal h,
218 const dVector3 &q,const dVector3 &v,dReal t,
219 dVector3 &p,
220 dVector3 &n,
221 int &f)
222{
223 dOPC(p,*,v,t);
224 dOPE(p,+=,q);
225 n[0] = 2*p[0];
226 n[1] = 2*p[1];
227 n[2] = -2*p[2]*r*r/(h*h);
228
229 f = 0;
230 if (p[2] > h) return 0;
231 if (p[2] < 0) return 0;
232 if (t > 1) return 0;
233 if (t < 0) return 0;
234
235 return 1;
236}
237
238//cone ray
239//line in cone space (position,direction)
240//distance from line position (direction normalized)(if any)
241//return the number of intersection
242int FindIntersectionConeRay(dReal r,dReal h,
243 const dVector3 &q,const dVector3 &v,dContactGeom *pContact)
244{
245 dVector3 qp,vp;
246 dOPE(qp,=,q);
247 dOPE(vp,=,v);
248 qp[2] = h-q[2];
249 vp[2] = -v[2];
250 dReal ts = (r/h);
251 ts *= ts;
252 dReal a = vp[0]*vp[0] + vp[1]*vp[1] - ts*vp[2]*vp[2];
253 dReal b = 2.f*qp[0]*vp[0] + 2.f*qp[1]*vp[1] - 2.f*ts*qp[2]*vp[2];
254 dReal c = qp[0]*qp[0] + qp[1]*qp[1] - ts*qp[2]*qp[2];
255
256/*
257 dReal a = v[0]*v[0] + v[1]*v[1] - (v[2]*v[2]*r*r) / (h*h);
258 dReal b = 2.f*q[0]*v[0] + 2.f*q[1]*v[1] + 2.f*r*r*v[2]/h - 2*r*r*q[0]*v[0]/(h*h);
259 dReal c = q[0]*q[0] + q[1]*q[1] + 2*r*r*q[2]/h - r*r*q[2]/(h*h) - r*r;
260*/
261 int nNumRoots=SolveQuadraticPolynomial(a,b,c,pContact[0].depth,pContact[1].depth);
262 int flag = 0;
263
264 dContactGeom ValidContact[2];
265
266 int nNumValidContacts = 0;
267 for (int i=0;i<nNumRoots;i++)
268 {
269 if (ProcessConeRayIntersectionPoint(r,h,q,v,pContact[i].depth,pContact[i].pos,
270 pContact[i].normal,flag))
271 {
272 ValidContact[nNumValidContacts] = pContact[i];
273 nNumValidContacts++;
274 }
275 }
276
277 dOP(qp,+,q,v);
278
279 if ((nNumValidContacts < 2) && (v[2] != 0.f))
280 {
281 dReal d = (0.f-q[2]) / (v[2]);
282 if ((d>=0) && (d<=1))
283 {
284 dOPC(vp,*,v,d);
285 dOP(qp,+,q,vp);
286
287 if (qp[0]*qp[0]+qp[1]*qp[1] < r*r)
288 {
289 dOPE(ValidContact[nNumValidContacts].pos,=,qp);
290 ValidContact[nNumValidContacts].normal[0] = 0.f;
291 ValidContact[nNumValidContacts].normal[1] = 0.f;
292 ValidContact[nNumValidContacts].normal[2] = -1.f;
293 ValidContact[nNumValidContacts].depth = d;
294 nNumValidContacts++;
295 }
296 }
297 }
298
299 if (nNumValidContacts == 2)
300 {
301 if (ValidContact[0].depth > ValidContact[1].depth)
302 {
303 pContact[0] = ValidContact[1];
304 pContact[1] = ValidContact[0];
305 }
306 else
307 {
308 pContact[0] = ValidContact[0];
309 pContact[1] = ValidContact[1];
310 }
311 }
312 else if (nNumValidContacts == 1)
313 {
314 pContact[0] = ValidContact[0];
315 }
316
317 return nNumValidContacts;
318}
319
320int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags,
321 dContactGeom *contact, int skip)
322{
323 dIASSERT (skip >= (int)sizeof(dContactGeom));
324 dIASSERT (o1->type == dConeClass);
325 dIASSERT (o2->type == dPlaneClass);
326 dxCone *cone = (dxCone*) o1;
327 dxPlane *plane = (dxPlane*) o2;
328
329 contact->g1 = o1;
330 contact->g2 = o2;
331
332 dVector3 p0,p1,pp0,pp1;
333 dOPE(p0,=,cone->final_posr->pos);
334 p1[0] = cone->final_posr->R[0*4+2] * cone->lz + p0[0];
335 p1[1] = cone->final_posr->R[1*4+2] * cone->lz + p0[1];
336 p1[2] = cone->final_posr->R[2*4+2] * cone->lz + p0[2];
337
338 dReal u;
339 FindIntersectionPlaneRay(plane->p,p0,plane->p,u,pp0);
340 FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);
341
342 if (dDISTANCE(pp0,pp1) < fEPSILON)
343 {
344 p1[0] = cone->final_posr->R[0*4+0] * cone->lz + p0[0];
345 p1[1] = cone->final_posr->R[1*4+0] * cone->lz + p0[1];
346 p1[2] = cone->final_posr->R[2*4+0] * cone->lz + p0[2];
347 FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);
348 dIASSERT(dDISTANCE(pp0,pp1) >= fEPSILON);
349 }
350 dVector3 h,r0,r1;
351 h[0] = cone->final_posr->R[0*4+2];
352 h[1] = cone->final_posr->R[1*4+2];
353 h[2] = cone->final_posr->R[2*4+2];
354
355 dOP(r0,-,pp0,pp1);
356 dCROSS(r1,=,h,r0);
357 dCROSS(r0,=,r1,h);
358 dNormalize3(r0);
359 dOPEC(h,*=,cone->lz);
360 dOPEC(r0,*=,cone->radius);
361
362 dVector3 p[3];
363 dOP(p[0],+,cone->final_posr->pos,h);
364 dOP(p[1],+,cone->final_posr->pos,r0);
365 dOP(p[2],-,cone->final_posr->pos,r0);
366
367 int numMaxContacts = flags & 0xffff;
368 if (numMaxContacts == 0)
369 numMaxContacts = 1;
370
371 int n=0;
372 for (int i=0;i<3;i++)
373 {
374 dReal d = dGeomPlanePointDepth(o2, p[i][0], p[i][1], p[i][2]);
375
376 if (d>0.f)
377 {
378 CONTACT(contact,n*skip)->g1 = o1;
379 CONTACT(contact,n*skip)->g2 = o2;
380 dOPE(CONTACT(contact,n*skip)->normal,=,plane->p);
381 dOPE(CONTACT(contact,n*skip)->pos,=,p[i]);
382 CONTACT(contact,n*skip)->depth = d;
383 n++;
384
385 if (n == numMaxContacts)
386 return n;
387 }
388 }
389
390 return n;
391}
392
393int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags,
394 dContactGeom *contact, int skip)
395{
396 dIASSERT (skip >= (int)sizeof(dContactGeom));
397 dIASSERT (o1->type == dRayClass);
398 dIASSERT (o2->type == dConeClass);
399 dxRay *ray = (dxRay*) o1;
400 dxCone *cone = (dxCone*) o2;
401
402 contact->g1 = o1;
403 contact->g2 = o2;
404
405 dVector3 tmp,q,v;
406 tmp[0] = ray->final_posr->pos[0] - cone->final_posr->pos[0];
407 tmp[1] = ray->final_posr->pos[1] - cone->final_posr->pos[1];
408 tmp[2] = ray->final_posr->pos[2] - cone->final_posr->pos[2];
409 dMULTIPLY1_331 (q,cone->final_posr->R,tmp);
410 tmp[0] = ray->final_posr->R[0*4+2] * ray->length;
411 tmp[1] = ray->final_posr->R[1*4+2] * ray->length;
412 tmp[2] = ray->final_posr->R[2*4+2] * ray->length;
413 dMULTIPLY1_331 (v,cone->final_posr->R,tmp);
414
415 dReal r = cone->radius;
416 dReal h = cone->lz;
417
418 dContactGeom Contact[2];
419
420 if (FindIntersectionConeRay(r,h,q,v,Contact))
421 {
422 dMULTIPLY0_331(contact->normal,cone->final_posr->R,Contact[0].normal);
423 dMULTIPLY0_331(contact->pos,cone->final_posr->R,Contact[0].pos);
424 dOPE(contact->pos,+=,cone->final_posr->pos);
425 contact->depth = Contact[0].depth * dLENGTH(v);
426/*
427 dMatrix3 RI;
428 dRSetIdentity (RI);
429 dVector3 ss;
430 ss[0] = 0.01f;
431 ss[1] = 0.01f;
432 ss[2] = 0.01f;
433
434 dsSetColorAlpha (1,0,0,0.8f);
435 dsDrawBox(contact->pos,RI,ss);
436*/
437 return 1;
438 }
439
440 return 0;
441}
442
443int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
444{
445 dIASSERT (skip >= (int)sizeof(dContactGeom));
446 dIASSERT (o1->type == dConeClass);
447 dIASSERT (o2->type == dSphereClass);
448 dxCone *cone = (dxCone*) o1;
449
450 dxSphere ASphere(0,cone->radius);
451 dGeomSetRotation(&ASphere,cone->final_posr->R);
452 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
453
454 return dCollideSphereSphere(&ASphere, o2, flags, contact, skip);
455}
456
457int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
458{
459 dIASSERT (skip >= (int)sizeof(dContactGeom));
460 dIASSERT (o1->type == dConeClass);
461 dIASSERT (o2->type == dBoxClass);
462 dxCone *cone = (dxCone*) o1;
463
464 dxSphere ASphere(0,cone->radius);
465 dGeomSetRotation(&ASphere,cone->final_posr->R);
466 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
467
468 return dCollideSphereBox(&ASphere, o2, flags, contact, skip);
469}
470
471int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
472{
473 dIASSERT (skip >= (int)sizeof(dContactGeom));
474 dIASSERT (o1->type == dCCylinderClass);
475 dIASSERT (o2->type == dConeClass);
476 dxCone *cone = (dxCone*) o2;
477
478 dxSphere ASphere(0,cone->radius);
479 dGeomSetRotation(&ASphere,cone->final_posr->R);
480 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
481
482 return dCollideCCylinderSphere(o1, &ASphere, flags, contact, skip);
483}
484
485extern int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
486
487int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
488{
489 dIASSERT (skip >= (int)sizeof(dContactGeom));
490 dIASSERT (o1->type == dTriMeshClass);
491 dIASSERT (o2->type == dConeClass);
492 dxCone *cone = (dxCone*) o2;
493
494 dxSphere ASphere(0,cone->radius);
495 dGeomSetRotation(&ASphere,cone->final_posr->R);
496 dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
497
498 return dCollideSTL(o1, &ASphere, flags, contact, skip);
499}
500
501
502
503
504
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
deleted file mode 100644
index ff779ac..0000000
--- a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
+++ /dev/null
@@ -1,662 +0,0 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2//some code inspired by Magic Software
3#include <ode/common.h>
4#include <ode/collision.h>
5#include <ode/matrix.h>
6#include <ode/rotation.h>
7#include <ode/odemath.h>
8#include "collision_kernel.h"
9#include "collision_std.h"
10#include "collision_std_internal.h"
11#include "collision_util.h"
12//#include <drawstuff/drawstuff.h>
13#include "windows.h"
14#include "ode\ode.h"
15
16#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
17#define MAXCONTACT 10
18#define TERRAINTOL 0.0f
19
20static bool IsAPowerOfTwo(int f)
21{
22 dAASSERT(f!=0);
23 while ((f&1) != 1)
24 f >>= 1;
25
26 return (f == 1);
27}
28
29static int GetPowerOfTwo(int f)
30{
31 dAASSERT(f!=0);
32 int n = 0;
33 while ((f&1) != 1)
34 {
35 n++;
36 f >>= 1;
37 }
38
39 return n;
40}
41
42dxTerrainY::dxTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
43dxGeom (space,bPlaceable)
44{
45 dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
46 dIASSERT(pHeights);
47 dIASSERT(vLength > 0.f);
48 dIASSERT(nNumNodesPerSide > 0);
49 type = dTerrainYClass;
50 m_vLength = vLength;
51 m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
52 dIASSERT(m_pHeights);
53 m_nNumNodesPerSide = nNumNodesPerSide;
54 m_vNodeLength = m_vLength / m_nNumNodesPerSide;
55 m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
56 m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1;
57 m_vMinHeight = dInfinity;
58 m_vMaxHeight = -dInfinity;
59 m_bFinite = bFinite;
60
61 for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
62 {
63 m_pHeights[i] = pHeights[i];
64 if (m_pHeights[i] < m_vMinHeight) m_vMinHeight = m_pHeights[i];
65 if (m_pHeights[i] > m_vMaxHeight) m_vMaxHeight = m_pHeights[i];
66 }
67}
68
69dxTerrainY::~dxTerrainY()
70{
71 dIASSERT(m_pHeights);
72 delete [] m_pHeights;
73}
74
75void dxTerrainY::computeAABB()
76{
77 if (m_bFinite)
78 {
79 if (gflags & GEOM_PLACEABLE)
80 {
81 dReal dx[6],dy[6],dz[6];
82 dx[0] = 0;
83 dx[1] = final_posr->R[0] * m_vLength;
84 dx[2] = final_posr->R[1] * m_vMinHeight;
85 dx[3] = final_posr->R[1] * m_vMaxHeight;
86 dx[4] = 0;
87 dx[5] = final_posr->R[2] * m_vLength;
88
89 dy[0] = 0;
90 dy[1] = final_posr->R[4] * m_vLength;
91 dy[2] = final_posr->R[5] * m_vMinHeight;
92 dy[3] = final_posr->R[5] * m_vMaxHeight;
93 dy[4] = 0;
94 dy[5] = final_posr->R[6] * m_vLength;
95
96 dz[0] = 0;
97 dz[1] = final_posr->R[8] * m_vLength;
98 dz[2] = final_posr->R[9] * m_vMinHeight;
99 dz[3] = final_posr->R[9] * m_vMaxHeight;
100 dz[4] = 0;
101 dz[5] = final_posr->R[10] * m_vLength;
102
103 aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
104 aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
105 aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
106 aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
107 aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
108 aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
109 }
110 else
111 {
112 aabb[0] = 0;
113 aabb[1] = m_vLength;
114 aabb[2] = m_vMinHeight;
115 aabb[3] = m_vMaxHeight;
116 aabb[4] = 0;
117 aabb[5] = m_vLength;
118 }
119 }
120 else
121 {
122 if (gflags & GEOM_PLACEABLE)
123 {
124 aabb[0] = -dInfinity;
125 aabb[1] = dInfinity;
126 aabb[2] = -dInfinity;
127 aabb[3] = dInfinity;
128 aabb[4] = -dInfinity;
129 aabb[5] = dInfinity;
130 }
131 else
132 {
133 aabb[0] = -dInfinity;
134 aabb[1] = dInfinity;
135 aabb[2] = m_vMinHeight;
136 aabb[3] = m_vMaxHeight;
137 aabb[4] = -dInfinity;
138 aabb[5] = dInfinity;
139 }
140 }
141}
142
143dReal dxTerrainY::GetHeight(int x,int z)
144{
145 return m_pHeights[ (((unsigned int)(z) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
146 + ((unsigned int)(x) & m_nNumNodesPerSideMask)];
147}
148
149dReal dxTerrainY::GetHeight(dReal x,dReal z)
150{
151 int nX = int(floor(x / m_vNodeLength));
152 int nZ = int(floor(z / m_vNodeLength));
153 dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
154 dReal dz = (z - (dReal(nZ) * m_vNodeLength)) / m_vNodeLength;
155 dIASSERT((dx >= 0.f) && (dx <= 1.f));
156 dIASSERT((dz >= 0.f) && (dz <= 1.f));
157
158 dReal y,y0;
159
160 if (dx + dz < 1.f)
161 {
162 y0 = GetHeight(nX,nZ);
163 y = y0
164 + (GetHeight(nX+1,nZ) - y0) * dx
165 + (GetHeight(nX,nZ+1) - y0) * dz;
166 }
167 else
168 {
169 y0 = GetHeight(nX+1,nZ+1);
170 y = y0
171 + (GetHeight(nX+1,nZ) - y0) * (1.f - dz)
172 + (GetHeight(nX,nZ+1) - y0) * (1.f - dx);
173 }
174
175 return y;
176}
177
178bool dxTerrainY::IsOnTerrain(int nx,int nz,int w,dReal *pos)
179{
180 dVector3 Min,Max;
181 Min[0] = nx * m_vNodeLength;
182 Min[2] = nz * m_vNodeLength;
183 Max[0] = (nx+1) * m_vNodeLength;
184 Max[2] = (nz+1) * m_vNodeLength;
185 dReal Tol = m_vNodeLength * TERRAINTOL;
186
187 if ((pos[0]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
188 return false;
189
190 if ((pos[2]<Min[2]-Tol) || (pos[2]>Max[2]+Tol))
191 return false;
192
193 dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
194 dReal dz = (pos[2] - (dReal(nz) * m_vNodeLength)) / m_vNodeLength;
195
196 if ((w == 0) && (dx + dz > 1.f+TERRAINTOL))
197 return false;
198
199 if ((w == 1) && (dx + dz < 1.f-TERRAINTOL))
200 return false;
201
202 return true;
203}
204
205dGeomID dCreateTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
206{
207 return new dxTerrainY(space, pHeights,vLength,nNumNodesPerSide,bFinite,bPlaceable);
208}
209
210dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z)
211{
212 dUASSERT (g && g->type == dTerrainYClass,"argument not a terrain");
213 g->recomputePosr();
214 dxTerrainY *t = (dxTerrainY*) g;
215 return t->GetHeight(x,z) - y;
216}
217
218typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z);
219#define RECOMPUTE_RAYNORMAL
220//#define DO_RAYDEPTH
221
222#define DMESS(A) \
223 dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
224 x,z,A, \
225 pContact->depth, \
226 dGeomSphereGetRadius(o2), \
227 pContact->pos[0], \
228 pContact->pos[1], \
229 pContact->pos[2], \
230 pContact->normal[0], \
231 pContact->normal[1], \
232 pContact->normal[2]);
233/*
234(y is up)
235
236A-B-E.x
237|/|
238C-D
239|
240F
241.
242z
243*/
244int dxTerrainY::dCollideTerrainUnit(
245 int x,int z,dxGeom *o2,int numMaxContacts,
246 int flags,dContactGeom *contact, int skip)
247{
248 dColliderFn *CollideRayN;
249 dColliderFn *CollideNPlane;
250 dGetDepthFn *GetDepth;
251 int numContacts = 0;
252 int numPlaneContacts = 0;
253 int i;
254
255 if (numContacts == numMaxContacts)
256 return numContacts;
257
258 dContactGeom PlaneContact[MAXCONTACT];
259 flags = (flags & 0xffff0000) | MAXCONTACT;
260
261 switch (o2->type)
262 {
263 case dSphereClass:
264 CollideRayN = dCollideRaySphere;
265 CollideNPlane = dCollideSpherePlane;
266 GetDepth = dGeomSpherePointDepth;
267 break;
268 case dBoxClass:
269 CollideRayN = dCollideRayBox;
270 CollideNPlane = dCollideBoxPlane;
271 GetDepth = dGeomBoxPointDepth;
272 break;
273 case dCCylinderClass:
274 CollideRayN = dCollideRayCCylinder;
275 CollideNPlane = dCollideCCylinderPlane;
276 GetDepth = dGeomCCylinderPointDepth;
277 break;
278 case dRayClass:
279 CollideRayN = NULL;
280 CollideNPlane = dCollideRayPlane;
281 GetDepth = NULL;
282 break;
283 case dConeClass:
284 CollideRayN = dCollideRayCone;
285 CollideNPlane = dCollideConePlane;
286 GetDepth = dGeomConePointDepth;
287 break;
288 default:
289 dIASSERT(0);
290 }
291
292 dReal Plane[4],lBD,lCD,lBC;
293 dVector3 A,B,C,D,BD,CD,BC,AB,AC;
294 A[0] = x * m_vNodeLength;
295 A[2] = z* m_vNodeLength;
296 A[1] = GetHeight(x,z);
297 B[0] = (x+1) * m_vNodeLength;
298 B[2] = z * m_vNodeLength;
299 B[1] = GetHeight(x+1,z);
300 C[0] = x * m_vNodeLength;
301 C[2] = (z+1) * m_vNodeLength;
302 C[1] = GetHeight(x,z+1);
303 D[0] = (x+1) * m_vNodeLength;
304 D[2] = (z+1) * m_vNodeLength;
305 D[1] = GetHeight(x+1,z+1);
306
307 dOP(BC,-,C,B);
308 lBC = dLENGTH(BC);
309 dOPEC(BC,/=,lBC);
310
311 dOP(BD,-,D,B);
312 lBD = dLENGTH(BD);
313 dOPEC(BD,/=,lBD);
314
315 dOP(CD,-,D,C);
316 lCD = dLENGTH(CD);
317 dOPEC(CD,/=,lCD);
318
319 dOP(AB,-,B,A);
320 dNormalize3(AB);
321
322 dOP(AC,-,C,A);
323 dNormalize3(AC);
324
325 if (CollideRayN)
326 {
327#ifdef RECOMPUTE_RAYNORMAL
328 dVector3 E,F;
329 dVector3 CE,FB,AD;
330 dVector3 Normal[3];
331 E[0] = (x+2) * m_vNodeLength;
332 E[2] = z * m_vNodeLength;
333 E[1] = GetHeight(x+2,z);
334 F[0] = x * m_vNodeLength;
335 F[2] = (z+2) * m_vNodeLength;
336 F[1] = GetHeight(x,z+2);
337 dOP(AD,-,D,A);
338 dNormalize3(AD);
339 dOP(CE,-,E,C);
340 dNormalize3(CE);
341 dOP(FB,-,B,F);
342 dNormalize3(FB);
343
344 //BC
345 dCROSS(Normal[0],=,BC,AD);
346 dNormalize3(Normal[0]);
347
348 //BD
349 dCROSS(Normal[1],=,BD,CE);
350 dNormalize3(Normal[1]);
351
352 //CD
353 dCROSS(Normal[2],=,CD,FB);
354 dNormalize3(Normal[2]);
355#endif
356 int nA[3],nB[3];
357 dContactGeom ContactA[3],ContactB[3];
358 dxRay rayBC(0,lBC);
359 dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
360 nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom));
361 dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
362 nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom));
363
364 dxRay rayBD(0,lBD);
365 dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
366 nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom));
367 dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
368 nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom));
369
370 dxRay rayCD(0,lCD);
371 dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
372 nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom));
373 dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
374 nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom));
375
376 for (i=0;i<3;i++)
377 {
378 if (nA[i] & nB[i])
379 {
380 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
381 pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2;
382 pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2;
383 pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2;
384#ifdef RECOMPUTE_RAYNORMAL
385 pContact->normal[0] = -Normal[i][0];
386 pContact->normal[1] = -Normal[i][1];
387 pContact->normal[2] = -Normal[i][2];
388#else
389 pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f;
390 pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f;
391 pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f;
392 dNormalize3(pContact->normal);
393#endif
394#ifdef DO_RAYDEPTH
395 dxRay rayV(0,1000.f);
396 dGeomRaySet(&rayV, pContact->pos[0],
397 pContact->pos[1],
398 pContact->pos[2],
399 -pContact->normal[0],
400 -pContact->normal[1],
401 -pContact->normal[2]);
402
403 dContactGeom ContactV;
404 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
405 {
406 pContact->depth = ContactV.depth;
407 numContacts++;
408 }
409#else
410
411 if (GetDepth == NULL)
412 {
413 dxRay rayV(0,1000.f);
414 dGeomRaySet(&rayV, pContact->pos[0],
415 pContact->pos[1],
416 pContact->pos[2],
417 -pContact->normal[0],
418 -pContact->normal[1],
419 -pContact->normal[2]);
420
421 dContactGeom ContactV;
422
423 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
424 {
425 pContact->depth = ContactV.depth;
426 numContacts++;
427 }
428 }
429 else
430 {
431 pContact->depth = GetDepth(o2,
432 pContact->pos[0],
433 pContact->pos[1],
434 pContact->pos[2]);
435 numContacts++;
436 }
437
438#endif
439 if (numContacts == numMaxContacts)
440 return numContacts;
441
442 }
443 }
444 }
445
446 dCROSS(Plane,=,AC,AB);
447 dNormalize3(Plane);
448 Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
449 dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
450 numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
451
452 for (i=0;i<numPlaneContacts;i++)
453 {
454 if (IsOnTerrain(x,z,0,PlaneContact[i].pos))
455 {
456 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
457 pContact->pos[0] = PlaneContact[i].pos[0];
458 pContact->pos[1] = PlaneContact[i].pos[1];
459 pContact->pos[2] = PlaneContact[i].pos[2];
460 pContact->normal[0] = -PlaneContact[i].normal[0];
461 pContact->normal[1] = -PlaneContact[i].normal[1];
462 pContact->normal[2] = -PlaneContact[i].normal[2];
463 pContact->depth = PlaneContact[i].depth;
464
465 //DMESS(0);
466 numContacts++;
467
468 if (numContacts == numMaxContacts)
469 return numContacts;
470 }
471 }
472
473 dCROSS(Plane,=,BD,CD);
474 dNormalize3(Plane);
475 Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
476 dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
477 numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
478
479 for (i=0;i<numPlaneContacts;i++)
480 {
481 if (IsOnTerrain(x,z,1,PlaneContact[i].pos))
482 {
483 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
484 pContact->pos[0] = PlaneContact[i].pos[0];
485 pContact->pos[1] = PlaneContact[i].pos[1];
486 pContact->pos[2] = PlaneContact[i].pos[2];
487 pContact->normal[0] = -PlaneContact[i].normal[0];
488 pContact->normal[1] = -PlaneContact[i].normal[1];
489 pContact->normal[2] = -PlaneContact[i].normal[2];
490 pContact->depth = PlaneContact[i].depth;
491 //DMESS(1);
492 numContacts++;
493
494 if (numContacts == numMaxContacts)
495 return numContacts;
496 }
497 }
498
499 return numContacts;
500}
501
502int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip)
503{
504 dIASSERT (skip >= (int)sizeof(dContactGeom));
505 dIASSERT (o1->type == dTerrainYClass);
506 int i,j;
507
508 if ((flags & 0xffff) == 0)
509 flags = (flags & 0xffff0000) | 1;
510
511 int numMaxTerrainContacts = (flags & 0xffff);
512 dxTerrainY *terrain = (dxTerrainY*) o1;
513
514 dReal aabbbak[6];
515 int gflagsbak;
516
517 dVector3 pos0;
518 int numTerrainContacts = 0;
519
520 dxPosR *bak;
521 dxPosR X1;
522
523 if (terrain->gflags & GEOM_PLACEABLE)
524 {
525 dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
526 dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
527 dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
528 bak = o2->final_posr;
529 o2->final_posr = &X1;
530 memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
531 gflagsbak = o2->gflags;
532 o2->computeAABB();
533 }
534
535 int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
536 int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
537 int nMinZ = int(floor(o2->aabb[4] / terrain->m_vNodeLength));
538 int nMaxZ = int(floor(o2->aabb[5] / terrain->m_vNodeLength)) + 1;
539
540 if (terrain->m_bFinite)
541 {
542 nMinX = MAX(nMinX,0);
543 nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
544 nMinZ = MAX(nMinZ,0);
545 nMaxZ = MIN(nMaxZ,terrain->m_nNumNodesPerSide);
546
547 if ((nMinX >= nMaxX) || (nMinZ >= nMaxZ))
548 goto dCollideTerrainYExit;
549 }
550
551 dVector3 AabbTop;
552 AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
553 AabbTop[2] = (o2->aabb[4]+o2->aabb[5]) / 2;
554 AabbTop[1] = o2->aabb[3];
555 if (o2->type != dRayClass)
556 {
557 dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[2]) - AabbTop[1];
558 if (AabbTopDepth > 0.f)
559 {
560 contact->depth = AabbTopDepth;
561 dReal MaxDepth = (o2->aabb[3]-o2->aabb[2]) / 2;
562 if (contact->depth > MaxDepth)
563 contact->depth = MaxDepth;
564 contact->g1 = o1;
565 contact->g2 = o2;
566 dOPE(contact->pos,=,AabbTop);
567 contact->normal[0] = 0.f;
568 contact->normal[1] = -1.f;
569 contact->normal[2] = 0.f;
570
571 numTerrainContacts = 1;
572 goto dCollideTerrainYExit;
573 }
574 }
575
576 for (i=nMinX;i<nMaxX;i++)
577 {
578 for (j=nMinZ;j<nMaxZ;j++)
579 {
580 numTerrainContacts += terrain->dCollideTerrainUnit(
581 i,j,o2,numMaxTerrainContacts - numTerrainContacts,
582 flags,CONTACT(contact,numTerrainContacts*skip),skip );
583 }
584 }
585
586 dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
587
588 for (i=0; i<numTerrainContacts; i++)
589 {
590 CONTACT(contact,i*skip)->g1 = o1;
591 CONTACT(contact,i*skip)->g2 = o2;
592 }
593
594dCollideTerrainYExit:
595
596 if (terrain->gflags & GEOM_PLACEABLE)
597 {
598 o2->final_posr = bak;
599 memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
600 o2->gflags = gflagsbak;
601
602 for (i=0; i<numTerrainContacts; i++)
603 {
604 dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
605 dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
606 dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos);
607
608 dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
609 dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
610 }
611 }
612
613 return numTerrainContacts;
614}
615/*
616void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
617{
618 float A[3],B[3],C[3],D[3];
619 float R[12];
620 float pos[3];
621 if (pR)
622 memcpy(R,pR,sizeof(R));
623 else
624 {
625 memset(R,0,sizeof(R));
626 R[0] = 1.f;
627 R[5] = 1.f;
628 R[10] = 1.f;
629 }
630
631 if (ppos)
632 memcpy(pos,ppos,sizeof(pos));
633 else
634 memset(pos,0,sizeof(pos));
635
636 float vx,vz;
637 vx = vLength * x;
638 vz = vLength * z;
639
640 int i;
641 for (i=0;i<nNumNodesPerSide;i++)
642 {
643 for (int j=0;j<nNumNodesPerSide;j++)
644 {
645 A[0] = i * vNodeLength + vx;
646 A[2] = j * vNodeLength + vz;
647 A[1] = GetHeight(i,j,nNumNodesPerSide,pHeights);
648 B[0] = (i+1) * vNodeLength + vx;
649 B[2] = j * vNodeLength + vz;
650 B[1] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
651 C[0] = i * vNodeLength + vx;
652 C[2] = (j+1) * vNodeLength + vz;
653 C[1] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
654 D[0] = (i+1) * vNodeLength + vx;
655 D[2] = (j+1) * vNodeLength + vz;
656 D[1] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
657 dsDrawTriangle(pos,R,C,B,A,1);
658 dsDrawTriangle(pos,R,D,B,C,1);
659 }
660 }
661}
662*/
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
deleted file mode 100644
index d47c402..0000000
--- a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
+++ /dev/null
@@ -1,659 +0,0 @@
1//Benoit CHAPEROT 2003-2004 www.jstarlab.com
2//some code inspired by Magic Software
3#include <ode/common.h>
4#include <ode/collision.h>
5#include <ode/matrix.h>
6#include <ode/rotation.h>
7#include <ode/odemath.h>
8#include "collision_kernel.h"
9#include "collision_std.h"
10#include "collision_std_internal.h"
11#include "collision_util.h"
12//#include <drawstuff/drawstuff.h>
13#include "windows.h"
14#include "ode\ode.h"
15
16#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
17#define MAXCONTACT 10
18#define TERRAINTOL 0.0f
19
20static bool IsAPowerOfTwo(int f)
21{
22 dAASSERT(f!=0);
23 while ((f&1) != 1)
24 f >>= 1;
25
26 return (f == 1);
27}
28
29static int GetPowerOfTwo(int f)
30{
31 dAASSERT(f!=0);
32 int n = 0;
33 while ((f&1) != 1)
34 {
35 n++;
36 f >>= 1;
37 }
38
39 return n;
40}
41
42dxTerrainZ::dxTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
43dxGeom (space,bPlaceable)
44{
45 dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
46 dIASSERT(pHeights);
47 dIASSERT(vLength > 0.f);
48 dIASSERT(nNumNodesPerSide > 0);
49 type = dTerrainZClass;
50 m_vLength = vLength;
51 m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
52 dIASSERT(m_pHeights);
53 m_nNumNodesPerSide = nNumNodesPerSide;
54 m_vNodeLength = m_vLength / m_nNumNodesPerSide;
55 m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
56 m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1;
57 m_vMinHeight = dInfinity;
58 m_vMaxHeight = -dInfinity;
59 m_bFinite = bFinite;
60
61 for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
62 {
63 m_pHeights[i] = pHeights[i];
64 if (m_pHeights[i] < m_vMinHeight) m_vMinHeight = m_pHeights[i];
65 if (m_pHeights[i] > m_vMaxHeight) m_vMaxHeight = m_pHeights[i];
66 }
67}
68
69dxTerrainZ::~dxTerrainZ()
70{
71 dIASSERT(m_pHeights);
72 delete [] m_pHeights;
73}
74
75void dxTerrainZ::computeAABB()
76{
77 if (m_bFinite)
78 {
79 if (gflags & GEOM_PLACEABLE)
80 {
81 dReal dx[6],dy[6],dz[6];
82 dx[0] = 0;
83 dx[1] = final_posr->R[0] * m_vLength;
84 dx[2] = 0;
85 dx[3] = final_posr->R[1] * m_vLength;
86 dx[4] = final_posr->R[2] * m_vMinHeight;
87 dx[5] = final_posr->R[2] * m_vMaxHeight;
88
89 dy[0] = 0;
90 dy[1] = final_posr->R[4] * m_vLength;
91 dy[2] = 0;
92 dy[3] = final_posr->R[5] * m_vLength;
93 dy[4] = final_posr->R[6] * m_vMinHeight;
94 dy[5] = final_posr->R[6] * m_vMaxHeight;
95
96 dz[0] = 0;
97 dz[1] = final_posr->R[8] * m_vLength;
98 dz[2] = 0;
99 dz[3] = final_posr->R[9] * m_vLength;
100 dz[4] = final_posr->R[10] * m_vMinHeight;
101 dz[5] = final_posr->R[10] * m_vMaxHeight;
102
103 aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
104 aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
105 aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
106 aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
107 aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
108 aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
109 }
110 else
111 {
112 aabb[0] = 0;
113 aabb[1] = m_vLength;
114 aabb[2] = 0;
115 aabb[3] = m_vLength;
116 aabb[4] = m_vMinHeight;
117 aabb[5] = m_vMaxHeight;
118 }
119 }
120 else
121 {
122 if (gflags & GEOM_PLACEABLE)
123 {
124 aabb[0] = -dInfinity;
125 aabb[1] = dInfinity;
126 aabb[2] = -dInfinity;
127 aabb[3] = dInfinity;
128 aabb[4] = -dInfinity;
129 aabb[5] = dInfinity;
130 }
131 else
132 {
133 aabb[0] = -dInfinity;
134 aabb[1] = dInfinity;
135 aabb[2] = -dInfinity;
136 aabb[3] = dInfinity;
137 aabb[4] = m_vMinHeight;
138 aabb[5] = m_vMaxHeight;
139 }
140 }
141}
142
143dReal dxTerrainZ::GetHeight(int x,int y)
144{
145 return m_pHeights[ (((unsigned int)(y) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
146 + ((unsigned int)(x) & m_nNumNodesPerSideMask)];
147}
148
149dReal dxTerrainZ::GetHeight(dReal x,dReal y)
150{
151 int nX = int(floor(x / m_vNodeLength));
152 int nY = int(floor(y / m_vNodeLength));
153 dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
154 dReal dy = (y - (dReal(nY) * m_vNodeLength)) / m_vNodeLength;
155 dIASSERT((dx >= 0.f) && (dx <= 1.f));
156 dIASSERT((dy >= 0.f) && (dy <= 1.f));
157
158 dReal z,z0;
159
160 if (dx + dy < 1.f)
161 {
162 z0 = GetHeight(nX,nY);
163 z = z0
164 + (GetHeight(nX+1,nY) - z0) * dx
165 + (GetHeight(nX,nY+1) - z0) * dy;
166 }
167 else
168 {
169 z0 = GetHeight(nX+1,nY+1);
170 z = z0
171 + (GetHeight(nX+1,nY) - z0) * (1.f - dy)
172 + (GetHeight(nX,nY+1) - z0) * (1.f - dx);
173 }
174
175 return z;
176}
177
178bool dxTerrainZ::IsOnTerrain(int nx,int ny,int w,dReal *pos)
179{
180 dVector3 Min,Max;
181 Min[0] = nx * m_vNodeLength;
182 Min[1] = ny * m_vNodeLength;
183 Max[0] = (nx+1) * m_vNodeLength;
184 Max[1] = (ny+1) * m_vNodeLength;
185 dReal Tol = m_vNodeLength * TERRAINTOL;
186
187 if ((pos[0]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
188 return false;
189
190 if ((pos[1]<Min[1]-Tol) || (pos[1]>Max[1]+Tol))
191 return false;
192
193 dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
194 dReal dy = (pos[1] - (dReal(ny) * m_vNodeLength)) / m_vNodeLength;
195
196 if ((w == 0) && (dx + dy > 1.f+TERRAINTOL))
197 return false;
198
199 if ((w == 1) && (dx + dy < 1.f-TERRAINTOL))
200 return false;
201
202 return true;
203}
204
205dGeomID dCreateTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
206{
207 return new dxTerrainZ(space, pHeights,vLength,nNumNodesPerSide, bFinite, bPlaceable);
208}
209
210dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z)
211{
212 dUASSERT (g && g->type == dTerrainZClass,"argument not a terrain");
213 g->recomputePosr();
214 dxTerrainZ *t = (dxTerrainZ*) g;
215 return t->GetHeight(x,y) - z;
216}
217
218typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z);
219#define RECOMPUTE_RAYNORMAL
220//#define DO_RAYDEPTH
221
222#define DMESS(A) \
223 dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
224 x,y,A, \
225 pContact->depth, \
226 dGeomSphereGetRadius(o2), \
227 pContact->pos[0], \
228 pContact->pos[1], \
229 pContact->pos[2], \
230 pContact->normal[0], \
231 pContact->normal[1], \
232 pContact->normal[2]);
233/*
234(z is up)
235
236y
237.
238F
239|
240C-D
241|\|
242A-B-E.x
243*/
244int dxTerrainZ::dCollideTerrainUnit(
245 int x,int y,dxGeom *o2,int numMaxContacts,
246 int flags,dContactGeom *contact, int skip)
247{
248 dColliderFn *CollideRayN;
249 dColliderFn *CollideNPlane;
250 dGetDepthFn *GetDepth;
251 int numContacts = 0;
252 int numPlaneContacts = 0;
253 int i;
254
255 if (numContacts == numMaxContacts)
256 return numContacts;
257
258 dContactGeom PlaneContact[MAXCONTACT];
259 flags = (flags & 0xffff0000) | MAXCONTACT;
260
261 switch (o2->type)
262 {
263 case dSphereClass:
264 CollideRayN = dCollideRaySphere;
265 CollideNPlane = dCollideSpherePlane;
266 GetDepth = dGeomSpherePointDepth;
267 break;
268 case dBoxClass:
269 CollideRayN = dCollideRayBox;
270 CollideNPlane = dCollideBoxPlane;
271 GetDepth = dGeomBoxPointDepth;
272 break;
273 case dCCylinderClass:
274 CollideRayN = dCollideRayCCylinder;
275 CollideNPlane = dCollideCCylinderPlane;
276 GetDepth = dGeomCCylinderPointDepth;
277 break;
278 case dRayClass:
279 CollideRayN = NULL;
280 CollideNPlane = dCollideRayPlane;
281 GetDepth = NULL;
282 break;
283 case dConeClass:
284 CollideRayN = dCollideRayCone;
285 CollideNPlane = dCollideConePlane;
286 GetDepth = dGeomConePointDepth;
287 break;
288 default:
289 dIASSERT(0);
290 }
291
292 dReal Plane[4],lBD,lCD,lBC;
293 dVector3 A,B,C,D,BD,CD,BC,AB,AC;
294 A[0] = x * m_vNodeLength;
295 A[1] = y * m_vNodeLength;
296 A[2] = GetHeight(x,y);
297 B[0] = (x+1) * m_vNodeLength;
298 B[1] = y * m_vNodeLength;
299 B[2] = GetHeight(x+1,y);
300 C[0] = x * m_vNodeLength;
301 C[1] = (y+1) * m_vNodeLength;
302 C[2] = GetHeight(x,y+1);
303 D[0] = (x+1) * m_vNodeLength;
304 D[1] = (y+1) * m_vNodeLength;
305 D[2] = GetHeight(x+1,y+1);
306
307 dOP(BC,-,C,B);
308 lBC = dLENGTH(BC);
309 dOPEC(BC,/=,lBC);
310
311 dOP(BD,-,D,B);
312 lBD = dLENGTH(BD);
313 dOPEC(BD,/=,lBD);
314
315 dOP(CD,-,D,C);
316 lCD = dLENGTH(CD);
317 dOPEC(CD,/=,lCD);
318
319 dOP(AB,-,B,A);
320 dNormalize3(AB);
321
322 dOP(AC,-,C,A);
323 dNormalize3(AC);
324
325 if (CollideRayN)
326 {
327#ifdef RECOMPUTE_RAYNORMAL
328 dVector3 E,F;
329 dVector3 CE,FB,AD;
330 dVector3 Normal[3];
331 E[0] = (x+2) * m_vNodeLength;
332 E[1] = y * m_vNodeLength;
333 E[2] = GetHeight(x+2,y);
334 F[0] = x * m_vNodeLength;
335 F[1] = (y+2) * m_vNodeLength;
336 F[2] = GetHeight(x,y+2);
337 dOP(AD,-,D,A);
338 dNormalize3(AD);
339 dOP(CE,-,E,C);
340 dNormalize3(CE);
341 dOP(FB,-,B,F);
342 dNormalize3(FB);
343
344 //BC
345 dCROSS(Normal[0],=,AD,BC);
346 dNormalize3(Normal[0]);
347
348 //BD
349 dCROSS(Normal[1],=,CE,BD);
350 dNormalize3(Normal[1]);
351
352 //CD
353 dCROSS(Normal[2],=,FB,CD);
354 dNormalize3(Normal[2]);
355#endif
356 int nA[3],nB[3];
357 dContactGeom ContactA[3],ContactB[3];
358 dxRay rayBC(0,lBC);
359 dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
360 nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom));
361 dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
362 nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom));
363
364 dxRay rayBD(0,lBD);
365 dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
366 nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom));
367 dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
368 nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom));
369
370 dxRay rayCD(0,lCD);
371 dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
372 nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom));
373 dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
374 nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom));
375
376 for (i=0;i<3;i++)
377 {
378 if (nA[i] & nB[i])
379 {
380 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
381 pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2;
382 pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2;
383 pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2;
384#ifdef RECOMPUTE_RAYNORMAL
385 pContact->normal[0] = -Normal[i][0];
386 pContact->normal[1] = -Normal[i][1];
387 pContact->normal[2] = -Normal[i][2];
388#else
389 pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f;
390 pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f;
391 pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f;
392 dNormalize3(pContact->normal);
393#endif
394#ifdef DO_RAYDEPTH
395 dxRay rayV(0,1000.f);
396 dGeomRaySet(&rayV, pContact->pos[0],
397 pContact->pos[1],
398 pContact->pos[2],
399 -pContact->normal[0],
400 -pContact->normal[1],
401 -pContact->normal[2]);
402
403 dContactGeom ContactV;
404 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
405 {
406 pContact->depth = ContactV.depth;
407 numContacts++;
408 }
409#else
410 if (GetDepth == NULL)
411 {
412 dxRay rayV(0,1000.f);
413 dGeomRaySet(&rayV, pContact->pos[0],
414 pContact->pos[1],
415 pContact->pos[2],
416 -pContact->normal[0],
417 -pContact->normal[1],
418 -pContact->normal[2]);
419
420 dContactGeom ContactV;
421 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
422 {
423 pContact->depth = ContactV.depth;
424 numContacts++;
425 }
426 }
427 else
428 {
429 pContact->depth = GetDepth(o2,
430 pContact->pos[0],
431 pContact->pos[1],
432 pContact->pos[2]);
433 numContacts++;
434 }
435#endif
436 if (numContacts == numMaxContacts)
437 return numContacts;
438
439 }
440 }
441 }
442
443 dCROSS(Plane,=,AB,AC);
444 dNormalize3(Plane);
445 Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
446 dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
447 numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
448
449 for (i=0;i<numPlaneContacts;i++)
450 {
451 if (IsOnTerrain(x,y,0,PlaneContact[i].pos))
452 {
453 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
454 pContact->pos[0] = PlaneContact[i].pos[0];
455 pContact->pos[1] = PlaneContact[i].pos[1];
456 pContact->pos[2] = PlaneContact[i].pos[2];
457 pContact->normal[0] = -PlaneContact[i].normal[0];
458 pContact->normal[1] = -PlaneContact[i].normal[1];
459 pContact->normal[2] = -PlaneContact[i].normal[2];
460 pContact->depth = PlaneContact[i].depth;
461
462 //DMESS(0);
463 numContacts++;
464
465 if (numContacts == numMaxContacts)
466 return numContacts;
467 }
468 }
469
470 dCROSS(Plane,=,CD,BD);
471 dNormalize3(Plane);
472 Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
473 dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
474 numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
475
476 for (i=0;i<numPlaneContacts;i++)
477 {
478 if (IsOnTerrain(x,y,1,PlaneContact[i].pos))
479 {
480 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
481 pContact->pos[0] = PlaneContact[i].pos[0];
482 pContact->pos[1] = PlaneContact[i].pos[1];
483 pContact->pos[2] = PlaneContact[i].pos[2];
484 pContact->normal[0] = -PlaneContact[i].normal[0];
485 pContact->normal[1] = -PlaneContact[i].normal[1];
486 pContact->normal[2] = -PlaneContact[i].normal[2];
487 pContact->depth = PlaneContact[i].depth;
488 //DMESS(1);
489 numContacts++;
490
491 if (numContacts == numMaxContacts)
492 return numContacts;
493 }
494 }
495
496 return numContacts;
497}
498
499int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip)
500{
501 dIASSERT (skip >= (int)sizeof(dContactGeom));
502 dIASSERT (o1->type == dTerrainZClass);
503 int i,j;
504
505 if ((flags & 0xffff) == 0)
506 flags = (flags & 0xffff0000) | 1;
507
508 int numMaxTerrainContacts = (flags & 0xffff);
509 dxTerrainZ *terrain = (dxTerrainZ*) o1;
510
511 dReal aabbbak[6];
512 int gflagsbak;
513
514 dVector3 pos0;
515 int numTerrainContacts = 0;
516
517 dxPosR *bak;
518 dxPosR X1;
519
520 if (terrain->gflags & GEOM_PLACEABLE)
521 {
522 dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
523 dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
524 dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
525 bak = o2->final_posr;
526 o2->final_posr = &X1;
527 memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
528 gflagsbak = o2->gflags;
529 o2->computeAABB();
530 }
531
532 int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
533 int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
534 int nMinY = int(floor(o2->aabb[2] / terrain->m_vNodeLength));
535 int nMaxY = int(floor(o2->aabb[3] / terrain->m_vNodeLength)) + 1;
536
537 if (terrain->m_bFinite)
538 {
539 nMinX = MAX(nMinX,0);
540 nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
541 nMinY = MAX(nMinY,0);
542 nMaxY = MIN(nMaxY,terrain->m_nNumNodesPerSide);
543
544 if ((nMinX >= nMaxX) || (nMinY >= nMaxY))
545 goto dCollideTerrainZExit;
546 }
547
548 dVector3 AabbTop;
549 AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
550 AabbTop[1] = (o2->aabb[2]+o2->aabb[3]) / 2;
551 AabbTop[2] = o2->aabb[5];
552 if (o2->type != dRayClass)
553 {
554 dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[1]) - AabbTop[2];
555 if (AabbTopDepth > 0.f)
556 {
557 contact->depth = AabbTopDepth;
558 dReal MaxDepth = (o2->aabb[5]-o2->aabb[4]) / 2;
559 if (contact->depth > MaxDepth)
560 contact->depth = MaxDepth;
561 contact->g1 = o1;
562 contact->g2 = o2;
563 dOPE(contact->pos,=,AabbTop);
564 contact->normal[0] = 0.f;
565 contact->normal[1] = 0.f;
566 contact->normal[2] = -1.f;
567
568 numTerrainContacts = 1;
569 goto dCollideTerrainZExit;
570 }
571 }
572
573 for (i=nMinX;i<nMaxX;i++)
574 {
575 for (j=nMinY;j<nMaxY;j++)
576 {
577 numTerrainContacts += terrain->dCollideTerrainUnit(
578 i,j,o2,numMaxTerrainContacts - numTerrainContacts,
579 flags,CONTACT(contact,numTerrainContacts*skip),skip );
580 }
581 }
582
583 dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
584
585 for (i=0; i<numTerrainContacts; i++)
586 {
587 CONTACT(contact,i*skip)->g1 = o1;
588 CONTACT(contact,i*skip)->g2 = o2;
589 }
590
591dCollideTerrainZExit:
592
593 if (terrain->gflags & GEOM_PLACEABLE)
594 {
595 o2->final_posr = bak;
596 memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
597 o2->gflags = gflagsbak;
598
599 for (i=0; i<numTerrainContacts; i++)
600 {
601 dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
602 dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
603 dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos);
604
605 dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
606 dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
607 }
608 }
609
610 return numTerrainContacts;
611}
612/*
613void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
614{
615 float A[3],B[3],C[3],D[3];
616 float R[12];
617 float pos[3];
618 if (pR)
619 memcpy(R,pR,sizeof(R));
620 else
621 {
622 memset(R,0,sizeof(R));
623 R[0] = 1.f;
624 R[5] = 1.f;
625 R[10] = 1.f;
626 }
627
628 if (ppos)
629 memcpy(pos,ppos,sizeof(pos));
630 else
631 memset(pos,0,sizeof(pos));
632
633 float vx,vz;
634 vx = vLength * x;
635 vz = vLength * z;
636
637 int i;
638 for (i=0;i<nNumNodesPerSide;i++)
639 {
640 for (int j=0;j<nNumNodesPerSide;j++)
641 {
642 A[0] = i * vNodeLength + vx;
643 A[1] = j * vNodeLength + vz;
644 A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
645 B[0] = (i+1) * vNodeLength + vx;
646 B[1] = j * vNodeLength + vz;
647 B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
648 C[0] = i * vNodeLength + vx;
649 C[1] = (j+1) * vNodeLength + vz;
650 C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
651 D[0] = (i+1) * vNodeLength + vx;
652 D[1] = (j+1) * vNodeLength + vz;
653 D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
654 dsDrawTriangle(pos,R,C,A,B,1);
655 dsDrawTriangle(pos,R,D,C,B,1);
656 }
657 }
658}
659*/
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt b/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt
deleted file mode 100644
index 0428846..0000000
--- a/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt
+++ /dev/null
@@ -1,322 +0,0 @@
1Benoit CHAPEROT 2003-2004 www.jstarlab.com
2Support for terrain and cones, collision and drawing.
3
4Terrains can be with z up (dTerrainZ) or y up (dTerrainY).
5Terrains are defined by a height field.
6Terrains are now placeable.
7Terrains can now be finite or infinite (repeat themselve in the x and (y or z) directions).
8
9Terrains can potentially collide with everything that collides with planes and rays;
10see the switch statement.
11
12Cones currently collides only with terrain and planes and rays.
13Cones, with high radius to height ratios are perfect to simulate vehicle wheels on terrains.
14
15
16
17There was an error in the depths returned by dCollideTerrain.
18Plus now contacts are not sorted according to their depths.
19Contact sorting is now supposed to be done externally.
20Not all dCollide functions seem to sort contacts according to depth.
21Requesting a high number of contacts, sorting them and then considering only the most significant contacts
22is a good way I think to improve stability.
23* Cones Collisions with spheres, boxes, ccylinder and trimesh are now roughly approximated using sphere collisions.
24
25You will need to complete the following operations (with ODE 0.039):
26
27*** add to folder ode\src:
28
29dCone.cpp
30dTerrainY.cpp
31dTerrainZ.cpp
32collision_std_internal.h
33
34On linux => edit each .cpp file and comment out #include "windows.h" & #include "ode\ode.h"
35
36
37*** add to drawstuff\src\drawstuff.cpp:
38
39static void drawCone(float l, float r)
40{
41 int i;
42 float tmp,ny,nz,a,ca,sa;
43 const int n = 24; // number of sides to the cone (divisible by 4)
44
45 a = float(M_PI*2.0)/float(n);
46 sa = (float) sin(a);
47 ca = (float) cos(a);
48
49 // draw top
50 glShadeModel (GL_FLAT);
51 ny=1; nz=0; // normal vector = (0,ny,nz)
52 glBegin (GL_TRIANGLE_FAN);
53 glNormal3d (0,0,1);
54 glVertex3d (0,0,l);
55 for (i=0; i<=n; i++) {
56 if (i==1 || i==n/2+1)
57 setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
58 glNormal3d (ny*r,nz*r,0);
59 glVertex3d (ny*r,nz*r,0);
60 if (i==1 || i==n/2+1)
61 setColor (color[0],color[1],color[2],color[3]);
62
63 // rotate ny,nz
64 tmp = ca*ny - sa*nz;
65 nz = sa*ny + ca*nz;
66 ny = tmp;
67 }
68 glEnd();
69
70 // draw bottom
71 ny=1; nz=0; // normal vector = (0,ny,nz)
72 glBegin (GL_TRIANGLE_FAN);
73 glNormal3d (0,0,-1);
74 glVertex3d (0,0,0);
75 for (i=0; i<=n; i++) {
76 if (i==1 || i==n/2+1)
77 setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
78 glNormal3d (0,0,-1);
79 glVertex3d (ny*r,nz*r,0);
80 if (i==1 || i==n/2+1)
81 setColor (color[0],color[1],color[2],color[3]);
82
83 // rotate ny,nz
84 tmp = ca*ny + sa*nz;
85 nz = -sa*ny + ca*nz;
86 ny = tmp;
87 }
88 glEnd();
89}
90
91void dsDrawCone (const float pos[3], const float R[12], float length, float radius)
92{
93 if (current_state != 2) dsError ("drawing function called outside simulation loop");
94 setupDrawingMode();
95 glShadeModel (GL_SMOOTH);
96 setTransform (pos,R);
97 drawCone (length,radius);
98 glPopMatrix();
99
100 if (use_shadows) {
101 setShadowDrawingMode();
102 setShadowTransform();
103 setTransform (pos,R);
104 drawCone (length,radius);
105 glPopMatrix();
106 glPopMatrix();
107 glDepthRange (0,1);
108 }
109}
110
111void dsDrawConeD (const double pos[3], const double R[12], float length, float radius)
112{
113 int i;
114 float pos2[3],R2[12];
115 for (i=0; i<3; i++) pos2[i]=(float)pos[i];
116 for (i=0; i<12; i++) R2[i]=(float)R[i];
117 dsDrawCone(pos2,R2,length,radius);
118}
119
120static float GetHeight(int x,int y,int nNumNodesPerSide,float *pHeights)
121{
122 int nNumNodesPerSideMask = nNumNodesPerSide - 1;
123 return pHeights[ (((unsigned int)(y) & nNumNodesPerSideMask) * nNumNodesPerSide)
124 + ((unsigned int)(x) & nNumNodesPerSideMask)];
125}
126
127void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
128{
129 float A[3],B[3],C[3],D[3];
130 float R[12];
131 float pos[3];
132 if (pR)
133 memcpy(R,pR,sizeof(R));
134 else
135 {
136 memset(R,0,sizeof(R));
137 R[0] = 1.f;
138 R[5] = 1.f;
139 R[10] = 1.f;
140 }
141
142 if (ppos)
143 memcpy(pos,ppos,sizeof(pos));
144 else
145 memset(pos,0,sizeof(pos));
146
147 float vx,vz;
148 vx = vLength * x;
149 vz = vLength * z;
150
151 int i;
152 for (i=0;i<nNumNodesPerSide;i++)
153 {
154 for (int j=0;j<nNumNodesPerSide;j++)
155 {
156 A[0] = i * vNodeLength + vx;
157 A[2] = j * vNodeLength + vz;
158 A[1] = GetHeight(i,j,nNumNodesPerSide,pHeights);
159 B[0] = (i+1) * vNodeLength + vx;
160 B[2] = j * vNodeLength + vz;
161 B[1] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
162 C[0] = i * vNodeLength + vx;
163 C[2] = (j+1) * vNodeLength + vz;
164 C[1] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
165 D[0] = (i+1) * vNodeLength + vx;
166 D[2] = (j+1) * vNodeLength + vz;
167 D[1] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
168 dsDrawTriangle(pos,R,C,B,A,1);
169 dsDrawTriangle(pos,R,D,B,C,1);
170 }
171 }
172}
173
174void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
175{
176 float A[3],B[3],C[3],D[3];
177 float R[12];
178 float pos[3];
179 if (pR)
180 memcpy(R,pR,sizeof(R));
181 else
182 {
183 memset(R,0,sizeof(R));
184 R[0] = 1.f;
185 R[5] = 1.f;
186 R[10] = 1.f;
187 }
188
189 if (ppos)
190 memcpy(pos,ppos,sizeof(pos));
191 else
192 memset(pos,0,sizeof(pos));
193
194 float vx,vz;
195 vx = vLength * x;
196 vz = vLength * z;
197
198 int i;
199 for (i=0;i<nNumNodesPerSide;i++)
200 {
201 for (int j=0;j<nNumNodesPerSide;j++)
202 {
203 A[0] = i * vNodeLength + vx;
204 A[1] = j * vNodeLength + vz;
205 A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
206 B[0] = (i+1) * vNodeLength + vx;
207 B[1] = j * vNodeLength + vz;
208 B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
209 C[0] = i * vNodeLength + vx;
210 C[1] = (j+1) * vNodeLength + vz;
211 C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
212 D[0] = (i+1) * vNodeLength + vx;
213 D[1] = (j+1) * vNodeLength + vz;
214 D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
215 dsDrawTriangle(pos,R,C,A,B,1);
216 dsDrawTriangle(pos,R,D,C,B,1);
217 }
218 }
219}
220
221*** add to include\drawstuff\drawstuff.h:
222void dsDrawCone (const float pos[3], const float R[12], float length, float radius);
223void dsDrawConeD (const double pos[3], const double R[12], float length, float radius);
224void dsDrawTerrainY(int x,int y,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos);
225void dsDrawTerrainZ(int x,int y,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos);
226
227*** add in include\ode\collision.h line 77:
228/* class numbers - each geometry object needs a unique number */
229enum {
230 dSphereClass = 0,
231 dBoxClass,
232 dCCylinderClass,
233 dCylinderClass,
234 dPlaneClass,
235 dRayClass,
236 dGeomTransformClass,
237 dTriMeshClass,
238
239 dTerrainYClass, //here
240 dTerrainZClass, //here
241 dConeClass, //here
242
243 dFirstSpaceClass,
244 dSimpleSpaceClass = dFirstSpaceClass,
245 dHashSpaceClass,
246 dQuadTreeSpaceClass,
247
248 dLastSpaceClass = dQuadTreeSpaceClass,
249
250 dFirstUserClass,
251 dLastUserClass = dFirstUserClass + dMaxUserClasses - 1,
252 dGeomNumClasses
253};
254
255dGeomID dCreateTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
256dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z);
257dGeomID dCreateTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
258dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z);
259
260dGeomID dCreateCone(dSpaceID space, dReal radius, dReal length);
261void dGeomConeSetParams (dGeomID cone, dReal radius, dReal length);
262void dGeomConeGetParams (dGeomID cone, dReal *radius, dReal *length);
263dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z);
264
265*** add in include\ode\odemath.h:
266#define dOP(a,op,b,c) \
267 (a)[0] = ((b)[0]) op ((c)[0]); \
268 (a)[1] = ((b)[1]) op ((c)[1]); \
269 (a)[2] = ((b)[2]) op ((c)[2]);
270#define dOPC(a,op,b,c) \
271 (a)[0] = ((b)[0]) op (c); \
272 (a)[1] = ((b)[1]) op (c); \
273 (a)[2] = ((b)[2]) op (c);
274#define dOPE(a,op,b) \
275 (a)[0] op ((b)[0]); \
276 (a)[1] op ((b)[1]); \
277 (a)[2] op ((b)[2]);
278#define dOPEC(a,op,c) \
279 (a)[0] op (c); \
280 (a)[1] op (c); \
281 (a)[2] op (c);
282#define dLENGTH(a) \
283 (dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) ));
284#define dLENGTHSQUARED(a) \
285 (((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]));
286
287*** add in ode\src\collision_kernel.cpp function 'static void initColliders()' next to other 'setCollider' calls:
288 setCollider (dTerrainYClass,dSphereClass,&dCollideTerrainY);
289 setCollider (dTerrainYClass,dBoxClass,&dCollideTerrainY);
290 setCollider (dTerrainYClass,dCCylinderClass,&dCollideTerrainY);
291 setCollider (dTerrainYClass,dRayClass,&dCollideTerrainY);
292 setCollider (dTerrainYClass,dConeClass,&dCollideTerrainY);
293
294 setCollider (dTerrainZClass,dSphereClass,&dCollideTerrainZ);
295 setCollider (dTerrainZClass,dBoxClass,&dCollideTerrainZ);
296 setCollider (dTerrainZClass,dCCylinderClass,&dCollideTerrainZ);
297 setCollider (dTerrainZClass,dRayClass,&dCollideTerrainZ);
298 setCollider (dTerrainZClass,dConeClass,&dCollideTerrainZ);
299
300 setCollider (dRayClass,dConeClass,&dCollideRayCone);
301 setCollider (dConeClass,dPlaneClass,&dCollideConePlane);
302 setCollider (dConeClass,dSphereClass,&dCollideConeSphere);
303 setCollider (dConeClass,dBoxClass,&dCollideConeBox);
304 setCollider (dCCylinderClass,dConeClass,&dCollideCCylinderCone);
305 setCollider (dTriMeshClass,dConeClass,&dCollideTriMeshCone);
306
307*** add in ode\src\collision_std.h:
308int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
309int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
310
311int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
312int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
313int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
314int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
315int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
316int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
317
318*** add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the ODE_SRC variable in the makefile
319On Linux => add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the libode_a_SOURCES variable in the ode/src/Makefile.am file.
320
321*** now you can now test using file test_boxstackb.cpp (to add in folder ode\test).
322
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp
deleted file mode 100644
index f1fa592..0000000
--- a/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp
+++ /dev/null
@@ -1,1375 +0,0 @@
1/*************************************************************************
2
3
4* *
5
6
7* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
8
9
10* All rights reserved. Email: russ@q12.org Web: www.q12.org *
11
12
13* *
14
15
16* This library is free software; you can redistribute it and/or *
17
18
19* modify it under the terms of EITHER: *
20
21
22* (1) The GNU Lesser General Public License as published by the Free *
23
24
25* Software Foundation; either version 2.1 of the License, or (at *
26
27
28* your option) any later version. The text of the GNU Lesser *
29
30
31* General Public License is included with this library in the *
32
33
34* file LICENSE.TXT. *
35
36
37* (2) The BSD-style license that is included with this library in *
38
39
40* the file LICENSE-BSD.TXT. *
41
42
43* *
44
45
46* This library is distributed in the hope that it will be useful, *
47
48
49* but WITHOUT ANY WARRANTY; without even the implied warranty of *
50
51
52* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
53
54
55* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
56
57
58* *
59
60
61*************************************************************************/
62
63
64
65
66
67#include <ode/ode.h>
68
69
70#include <drawstuff/drawstuff.h>
71
72
73
74
75
76#ifdef _MSC_VER
77
78
79#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
80
81
82#endif
83
84
85
86
87
88// select correct drawing functions
89
90
91
92
93
94#ifdef dDOUBLE
95
96
97#define dsDrawBox dsDrawBoxD
98
99
100#define dsDrawSphere dsDrawSphereD
101
102
103#define dsDrawCylinder dsDrawCylinderD
104
105
106#define dsDrawCappedCylinder dsDrawCappedCylinderD
107
108
109#endif
110
111
112
113
114
115
116
117
118// some constants
119
120
121
122
123
124const dReal vTerrainLength = 4.f;
125
126
127const dReal vTerrainHeight = 0.5f;
128
129
130const int TERRAINNODES = 4;
131
132
133dReal pTerrainHeights[TERRAINNODES*TERRAINNODES];
134
135
136
137
138
139dGeomID terrainZ = NULL;
140
141
142dGeomID terrainY = NULL;
143
144
145
146
147
148#define NUM 20 // max number of objects
149
150
151#define DENSITY (5.0) // density of all objects
152
153
154#define GPB 3 // maximum number of geometries per body
155
156
157#define MAX_CONTACTS 4 // maximum number of contact points per body
158
159
160
161
162
163
164
165
166// dynamics and collision objects
167
168
169
170
171
172struct MyObject {
173
174
175 dBodyID body; // the body
176
177
178 dGeomID geom[GPB]; // geometries representing this body
179
180
181};
182
183
184
185
186
187static int num=0; // number of objects in simulation
188
189
190static int nextobj=0; // next object to recycle if num==NUM
191
192
193static dWorldID world;
194
195
196static dSpaceID space;
197
198
199static MyObject obj[NUM];
200
201
202static dJointGroupID contactgroup;
203
204
205static int selected = -1; // selected object
206
207
208static int show_aabb = 0; // show geom AABBs?
209
210
211static int show_contacts = 0; // show contact points?
212
213
214static int random_pos = 1; // drop objects from random position?
215
216
217
218
219
220
221
222
223// this is called by dSpaceCollide when two objects in space are
224
225
226// potentially colliding.
227
228
229
230
231
232static void nearCallback (void *data, dGeomID o1, dGeomID o2)
233
234
235{
236
237
238 int i;
239
240
241 // if (o1->body && o2->body) return;
242
243
244
245
246
247 // exit without doing anything if the two bodies are connected by a joint
248
249
250 dBodyID b1 = dGeomGetBody(o1);
251
252
253 dBodyID b2 = dGeomGetBody(o2);
254
255
256 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
257
258
259
260
261
262 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
263
264
265 for (i=0; i<MAX_CONTACTS; i++) {
266
267
268 contact[i].surface.mode = dContactBounce | dContactApprox1; //dContactSoftCFM;
269
270
271 contact[i].surface.mu = dInfinity;
272
273
274 contact[i].surface.mu2 = 0;
275
276
277 contact[i].surface.bounce = 0.1;
278
279
280 contact[i].surface.bounce_vel = 0.1;
281
282
283 contact[i].surface.soft_cfm = 0.01;
284
285
286 }
287
288
289 if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
290
291
292 sizeof(dContact))) {
293
294
295 dMatrix3 RI;
296
297
298 dRSetIdentity (RI);
299
300
301 const dReal ss[3] = {0.02,0.02,0.02};
302
303
304 for (i=0; i<numc; i++) {
305
306
307 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
308
309
310 dJointAttach (c,b1,b2);
311
312
313 if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
314
315
316 }
317
318
319 }
320
321
322}
323
324
325
326
327
328
329
330
331// start simulation - set viewpoint
332
333
334
335
336
337static void start()
338
339
340{
341
342
343 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
344
345
346 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
347
348
349 dsSetViewpoint (xyz,hpr);
350
351
352 printf ("To drop another object, press:\n");
353
354
355 printf (" b for box.\n");
356
357
358 printf (" s for sphere.\n");
359
360
361 printf (" c for cylinder.\n");
362
363
364 printf (" x for a composite object.\n");
365
366
367 printf ("To select an object, press space.\n");
368
369
370 printf ("To disable the selected object, press d.\n");
371
372
373 printf ("To enable the selected object, press e.\n");
374
375
376 printf ("To toggle showing the geom AABBs, press a.\n");
377
378
379 printf ("To toggle showing the contact points, press t.\n");
380
381
382 printf ("To toggle dropping from random position/orientation, press r.\n");
383
384
385}
386
387
388
389
390
391
392
393
394char locase (char c)
395
396
397{
398
399
400 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
401
402
403 else return c;
404
405
406}
407
408
409
410
411
412
413
414
415// called when a key pressed
416
417
418
419
420
421static void command (int cmd)
422
423
424{
425
426
427 int i,j,k;
428
429
430 dReal sides[3];
431
432
433 dMass m;
434
435
436
437
438
439 cmd = locase (cmd);
440
441
442 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
443
444
445 /* || cmd == 'l' */) {
446
447
448 if (num < NUM) {
449
450
451 i = num;
452
453
454 num++;
455
456
457 }
458
459
460 else {
461
462
463 i = nextobj;
464
465
466 nextobj++;
467
468
469 if (nextobj >= num) nextobj = 0;
470
471
472
473
474
475 // destroy the body and geoms for slot i
476
477
478 dBodyDestroy (obj[i].body);
479
480
481 for (k=0; k < GPB; k++) {
482
483
484 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
485
486
487 }
488
489
490 memset (&obj[i],0,sizeof(obj[i]));
491
492
493 }
494
495
496
497
498
499 obj[i].body = dBodyCreate (world);
500
501
502 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
503
504
505
506
507
508 dMatrix3 R;
509
510
511 if (random_pos) {
512
513
514 dBodySetPosition (obj[i].body,
515
516
517 dRandReal()*2-1,dRandReal()*2+1,dRandReal()+3);
518
519
520 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
521
522
523 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
524
525
526 }
527
528
529 else {
530
531
532 dReal maxheight = 0;
533
534
535 for (k=0; k<num; k++) {
536
537
538 const dReal *pos = dBodyGetPosition (obj[k].body);
539
540
541 if (pos[2] > maxheight) maxheight = pos[2];
542
543
544 }
545
546
547 dBodySetPosition (obj[i].body, 0,maxheight+1,maxheight+3);
548
549
550 dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
551
552
553 }
554
555
556 dBodySetRotation (obj[i].body,R);
557
558
559 dBodySetData (obj[i].body,(void*) i);
560
561
562
563
564
565 if (cmd == 'b') {
566
567
568 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
569
570
571 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
572
573
574 }
575
576
577 else if (cmd == 'c') {
578
579
580 sides[0] *= 0.5;
581
582
583 dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
584
585
586 obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
587
588
589 }
590
591
592 /*
593
594
595 // cylinder option not yet implemented
596
597
598 else if (cmd == 'l') {
599
600
601 sides[1] *= 0.5;
602
603
604 dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
605
606
607 obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
608
609
610 }
611
612
613 */
614
615
616 else if (cmd == 's') {
617
618
619 sides[0] *= 0.5;
620
621
622 dMassSetSphere (&m,DENSITY,sides[0]);
623
624
625 obj[i].geom[0] = dCreateSphere (space,sides[0]);
626
627
628 }
629
630
631 else if (cmd == 'x') {
632
633
634 dGeomID g2[GPB]; // encapsulated geometries
635
636
637 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
638
639
640
641
642
643 // start accumulating masses for the encapsulated geometries
644
645
646 dMass m2;
647
648
649 dMassSetZero (&m);
650
651
652
653
654
655 // set random delta positions
656
657
658 for (j=0; j<GPB; j++) {
659
660
661 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
662
663
664 }
665
666
667
668
669
670 for (k=0; k<GPB; k++) {
671
672
673 obj[i].geom[k] = dCreateGeomTransform (space);
674
675
676 dGeomTransformSetCleanup (obj[i].geom[k],1);
677
678
679 if (k==0) {
680
681
682 dReal radius = dRandReal()*0.25+0.05;
683
684
685 g2[k] = dCreateSphere (0,radius);
686
687
688 dMassSetSphere (&m2,DENSITY,radius);
689
690
691 }
692
693
694 else if (k==1) {
695
696
697 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
698
699
700 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
701
702
703 }
704
705
706 else {
707
708
709 dReal radius = dRandReal()*0.1+0.05;
710
711
712 dReal length = dRandReal()*1.0+0.1;
713
714
715 g2[k] = dCreateCCylinder (0,radius,length);
716
717
718 dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
719
720
721 }
722
723
724 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
725
726
727
728
729
730 // set the transformation (adjust the mass too)
731
732
733 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
734
735
736 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
737
738
739 dMatrix3 Rtx;
740
741
742 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
743
744
745 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
746
747
748 dGeomSetRotation (g2[k],Rtx);
749
750
751 dMassRotate (&m2,Rtx);
752
753
754
755
756
757 // add to the total mass
758
759
760 dMassAdd (&m,&m2);
761
762
763 }
764
765
766
767
768
769 // move all encapsulated objects so that the center of mass is (0,0,0)
770
771
772 for (k=0; k<2; k++) {
773
774
775 dGeomSetPosition (g2[k],
776
777
778 dpos[k][0]-m.c[0],
779
780
781 dpos[k][1]-m.c[1],
782
783
784 dpos[k][2]-m.c[2]);
785
786
787 }
788
789
790 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
791
792
793 }
794
795
796
797
798
799 for (k=0; k < GPB; k++) {
800
801
802 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
803
804
805 }
806
807
808
809
810
811 dBodySetMass (obj[i].body,&m);
812
813
814 }
815
816
817
818
819
820 if (cmd == ' ') {
821
822
823 selected++;
824
825
826 if (selected >= num) selected = 0;
827
828
829 if (selected < 0) selected = 0;
830
831
832 }
833
834
835 else if (cmd == 'd' && selected >= 0 && selected < num) {
836
837
838 dBodyDisable (obj[selected].body);
839
840
841 }
842
843
844 else if (cmd == 'e' && selected >= 0 && selected < num) {
845
846
847 dBodyEnable (obj[selected].body);
848
849
850 }
851
852
853 else if (cmd == 'a') {
854
855
856 show_aabb ^= 1;
857
858
859 }
860
861
862 else if (cmd == 't') {
863
864
865 show_contacts ^= 1;
866
867
868 }
869
870
871 else if (cmd == 'r') {
872
873
874 random_pos ^= 1;
875
876
877 }
878
879
880}
881
882
883
884
885
886
887
888
889// draw a geom
890
891
892
893
894
895void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
896
897
898{
899
900
901 int i;
902
903
904
905
906
907 if (!g) return;
908
909
910 if (!pos) pos = dGeomGetPosition (g);
911
912
913 if (!R) R = dGeomGetRotation (g);
914
915
916
917
918
919 int type = dGeomGetClass (g);
920
921
922 if (type == dBoxClass) {
923
924
925 dVector3 sides;
926
927
928 dGeomBoxGetLengths (g,sides);
929
930
931 dsDrawBox (pos,R,sides);
932
933
934 }
935
936
937 else if (type == dSphereClass) {
938
939
940 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
941
942
943 }
944
945
946 else if (type == dCCylinderClass) {
947
948
949 dReal radius,length;
950
951
952 dGeomCCylinderGetParams (g,&radius,&length);
953
954
955 dsDrawCappedCylinder (pos,R,length,radius);
956
957
958 }
959
960
961 /*
962
963
964 // cylinder option not yet implemented
965
966
967 else if (type == dCylinderClass) {
968
969
970 dReal radius,length;
971
972
973 dGeomCylinderGetParams (g,&radius,&length);
974
975
976 dsDrawCylinder (pos,R,length,radius);
977
978
979 }
980
981
982 */
983
984
985 else if (type == dGeomTransformClass) {
986
987
988 dGeomID g2 = dGeomTransformGetGeom (g);
989
990
991 const dReal *pos2 = dGeomGetPosition (g2);
992
993
994 const dReal *R2 = dGeomGetRotation (g2);
995
996
997 dVector3 actual_pos;
998
999
1000 dMatrix3 actual_R;
1001
1002
1003 dMULTIPLY0_331 (actual_pos,R,pos2);
1004
1005
1006 actual_pos[0] += pos[0];
1007
1008
1009 actual_pos[1] += pos[1];
1010
1011
1012 actual_pos[2] += pos[2];
1013
1014
1015 dMULTIPLY0_333 (actual_R,R,R2);
1016
1017
1018 drawGeom (g2,actual_pos,actual_R,0);
1019
1020
1021 }
1022
1023
1024
1025
1026
1027 if (show_aabb) {
1028
1029
1030 // draw the bounding box for this geom
1031
1032
1033 dReal aabb[6];
1034
1035
1036 dGeomGetAABB (g,aabb);
1037
1038
1039 dVector3 bbpos;
1040
1041
1042 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
1043
1044
1045 dVector3 bbsides;
1046
1047
1048 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
1049
1050
1051 dMatrix3 RI;
1052
1053
1054 dRSetIdentity (RI);
1055
1056
1057 dsSetColorAlpha (1,0,0,0.5);
1058
1059
1060 dsDrawBox (bbpos,RI,bbsides);
1061
1062
1063 }
1064
1065
1066}
1067
1068
1069
1070
1071
1072
1073
1074
1075// simulation loop
1076
1077
1078
1079
1080
1081static void simLoop (int pause)
1082
1083
1084{
1085
1086
1087 dsSetColor (0,0,2);
1088
1089
1090 dSpaceCollide (space,0,&nearCallback);
1091
1092
1093 if (!pause) dWorldStep (world,0.05);
1094
1095
1096
1097
1098
1099 dAASSERT(terrainY);
1100
1101
1102 dAASSERT(terrainZ);
1103
1104
1105 dsSetColor (0,1,0);
1106
1107
1108 dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));
1109
1110
1111 dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));
1112
1113
1114
1115
1116
1117 if (show_aabb)
1118
1119
1120 {
1121
1122
1123 dReal aabb[6];
1124
1125
1126 dGeomGetAABB (terrainY,aabb);
1127
1128
1129 dVector3 bbpos;
1130
1131
1132 int i;
1133
1134
1135 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
1136
1137
1138 dVector3 bbsides;
1139
1140
1141 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
1142
1143
1144 dMatrix3 RI;
1145
1146
1147 dRSetIdentity (RI);
1148
1149
1150 dsSetColorAlpha (1,0,0,0.5);
1151
1152
1153 dsDrawBox (bbpos,RI,bbsides);
1154
1155
1156
1157
1158
1159 dGeomGetAABB (terrainZ,aabb);
1160
1161
1162 for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
1163
1164
1165 for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
1166
1167
1168 dsDrawBox (bbpos,RI,bbsides);
1169
1170
1171 }
1172
1173
1174
1175
1176
1177 dsSetColor (1,1,0);
1178
1179
1180
1181
1182
1183 // remove all contact joints
1184
1185
1186 dJointGroupEmpty (contactgroup);
1187
1188
1189
1190
1191
1192 dsSetColor (1,1,0);
1193
1194
1195 dsSetTexture (DS_WOOD);
1196
1197
1198 for (int i=0; i<num; i++) {
1199
1200
1201 for (int j=0; j < GPB; j++) {
1202
1203
1204 if (i==selected) {
1205
1206
1207 dsSetColor (0,0.7,1);
1208
1209
1210 }
1211
1212
1213 else if (! dBodyIsEnabled (obj[i].body)) {
1214
1215
1216 dsSetColor (1,0,0);
1217
1218
1219 }
1220
1221
1222 else {
1223
1224
1225 dsSetColor (1,1,0);
1226
1227
1228 }
1229
1230
1231 drawGeom (obj[i].geom[j],0,0,show_aabb);
1232
1233
1234 }
1235
1236
1237 }
1238
1239
1240}
1241
1242
1243
1244
1245
1246
1247
1248
1249int main (int argc, char **argv)
1250
1251
1252{
1253
1254
1255 // setup pointers to drawstuff callback functions
1256
1257
1258 dsFunctions fn;
1259
1260
1261 fn.version = DS_VERSION;
1262
1263
1264 fn.start = &start;
1265
1266
1267 fn.step = &simLoop;
1268
1269
1270 fn.command = &command;
1271
1272
1273 fn.stop = 0;
1274
1275
1276 fn.path_to_textures = "../../drawstuff/textures";
1277
1278
1279 if(argc==2)
1280 {
1281 fn.path_to_textures = argv[1];
1282 }
1283
1284
1285
1286 // create world
1287
1288
1289
1290
1291
1292 world = dWorldCreate();
1293
1294
1295 space = dHashSpaceCreate (0);
1296
1297
1298 contactgroup = dJointGroupCreate (0);
1299
1300
1301 dWorldSetGravity (world,0,0,-0.5); //-0.5
1302
1303
1304 dWorldSetCFM (world,1e-5);
1305
1306
1307 dCreatePlane (space,0,0,1,0);
1308
1309
1310 memset (obj,0,sizeof(obj));
1311
1312
1313
1314
1315
1316 for (int i=0;i<TERRAINNODES*TERRAINNODES;i++) pTerrainHeights[i] = vTerrainHeight * dRandReal();
1317
1318
1319 terrainY = dCreateTerrainY(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
1320
1321
1322 terrainZ = dCreateTerrainZ(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
1323
1324
1325
1326
1327
1328 dMatrix3 R;
1329
1330
1331 dRFromZAxis(R, 0.2f, 0.2f, 0.2f);
1332
1333
1334 dGeomSetPosition(terrainY,0.f,0.f,0.5f);
1335
1336
1337 dGeomSetRotation(terrainY,R);
1338
1339
1340 dGeomSetPosition(terrainZ,0.f,0.f,0.5f);
1341
1342
1343 dGeomSetRotation(terrainZ,R);
1344
1345
1346
1347
1348
1349 // run simulation
1350
1351
1352 dsSimulationLoop (argc,argv,352,288,&fn);
1353
1354
1355
1356
1357
1358 dJointGroupDestroy (contactgroup);
1359
1360
1361 dSpaceDestroy (space);
1362
1363
1364 dWorldDestroy (world);
1365
1366
1367
1368
1369
1370 return 0;
1371
1372
1373}
1374
1375
diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp b/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp
deleted file mode 100644
index 215da25..0000000
--- a/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp
+++ /dev/null
@@ -1,1445 +0,0 @@
1#include <ode/ode.h>
2#include "dCylinder.h"
3// given a pointer `p' to a dContactGeom, return the dContactGeom at
4// p + skip bytes.
5
6struct dxCylinder { // cylinder
7 dReal radius,lz; // radius, length along y axis //
8};
9
10int dCylinderClassUser = -1;
11
12#define NUMC_MASK (0xffff)
13
14#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
15
16/////////////////////////////////////////////////////////////////////////////////////////////////
17/////////////////////////////circleIntersection//////////////////////////////////////////////////
18//this does following:
19//takes two circles as normals to planes n1,n2, center points cp1,cp2,and radiuses r1,r2
20//finds line on which circles' planes intersect
21//finds four points O1,O2 - intersection between the line and sphere with center cp1 radius r1
22// O3,O4 - intersection between the line and sphere with center cp2 radius r2
23//returns false if there is no intersection
24//computes distances O1-O3, O1-O4, O2-O3, O2-O4
25//in "point" returns mean point between intersection points with smallest distance
26/////////////////////////////////////////////////////////////////////////////////////////////////
27inline bool circleIntersection(const dReal* n1,const dReal* cp1,dReal r1,const dReal* n2,const dReal* cp2,dReal r2,dVector3 point){
28dReal c1=dDOT14(cp1,n1);
29dReal c2=dDOT14(cp2,n2);
30dReal cos=dDOT44(n1,n2);
31dReal cos_2=cos*cos;
32dReal sin_2=1-cos_2;
33dReal p1=(c1-c2*cos)/sin_2;
34dReal p2=(c2-c1*cos)/sin_2;
35dVector3 lp={p1*n1[0]+p2*n2[0],p1*n1[4]+p2*n2[4],p1*n1[8]+p2*n2[8]};
36dVector3 n;
37dCROSS144(n,=,n1,n2);
38dVector3 LC1={lp[0]-cp1[0],lp[1]-cp1[1],lp[2]-cp1[2]};
39dVector3 LC2={lp[0]-cp2[0],lp[1]-cp2[1],lp[2]-cp2[2]};
40dReal A,B,C,B_A,B_A_2,D;
41dReal t1,t2,t3,t4;
42A=dDOT(n,n);
43B=dDOT(LC1,n);
44C=dDOT(LC1,LC1)-r1*r1;
45B_A=B/A;
46B_A_2=B_A*B_A;
47D=B_A_2-C;
48if(D<0.f){ //somewhat strange solution
49 //- it is needed to set some
50 //axis to sepparate cylinders
51 //when their edges approach
52 t1=-B_A+sqrtf(-D);
53 t2=-B_A-sqrtf(-D);
54// return false;
55 }
56else{
57t1=-B_A-sqrtf(D);
58t2=-B_A+sqrtf(D);
59}
60B=dDOT(LC2,n);
61C=dDOT(LC2,LC2)-r2*r2;
62B_A=B/A;
63B_A_2=B_A*B_A;
64D=B_A_2-C;
65
66if(D<0.f) {
67 t3=-B_A+sqrtf(-D);
68 t4=-B_A-sqrtf(-D);
69// return false;
70 }
71else{
72t3=-B_A-sqrtf(D);
73t4=-B_A+sqrtf(D);
74}
75dVector3 O1={lp[0]+n[0]*t1,lp[1]+n[1]*t1,lp[2]+n[2]*t1};
76dVector3 O2={lp[0]+n[0]*t2,lp[1]+n[1]*t2,lp[2]+n[2]*t2};
77
78dVector3 O3={lp[0]+n[0]*t3,lp[1]+n[1]*t3,lp[2]+n[2]*t3};
79dVector3 O4={lp[0]+n[0]*t4,lp[1]+n[1]*t4,lp[2]+n[2]*t4};
80
81dVector3 L1_3={O3[0]-O1[0],O3[1]-O1[1],O3[2]-O1[2]};
82dVector3 L1_4={O4[0]-O1[0],O4[1]-O1[1],O4[2]-O1[2]};
83
84dVector3 L2_3={O3[0]-O2[0],O3[1]-O2[1],O3[2]-O2[2]};
85dVector3 L2_4={O4[0]-O2[0],O4[1]-O2[1],O4[2]-O2[2]};
86
87
88dReal l1_3=dDOT(L1_3,L1_3);
89dReal l1_4=dDOT(L1_4,L1_4);
90
91dReal l2_3=dDOT(L2_3,L2_3);
92dReal l2_4=dDOT(L2_4,L2_4);
93
94
95if (l1_3<l1_4)
96 if(l2_3<l2_4)
97 if(l1_3<l2_3)
98 {
99 //l1_3;
100 point[0]=0.5f*(O1[0]+O3[0]);
101 point[1]=0.5f*(O1[1]+O3[1]);
102 point[2]=0.5f*(O1[2]+O3[2]);
103 }
104 else{
105 //l2_3;
106 point[0]=0.5f*(O2[0]+O3[0]);
107 point[1]=0.5f*(O2[1]+O3[1]);
108 point[2]=0.5f*(O2[2]+O3[2]);
109 }
110 else
111 if(l1_3<l2_4)
112 {
113 //l1_3;
114 point[0]=0.5f*(O1[0]+O3[0]);
115 point[1]=0.5f*(O1[1]+O3[1]);
116 point[2]=0.5f*(O1[2]+O3[2]);
117 }
118 else{
119 //l2_4;
120 point[0]=0.5f*(O2[0]+O4[0]);
121 point[1]=0.5f*(O2[1]+O4[1]);
122 point[2]=0.5f*(O2[2]+O4[2]);
123 }
124
125else
126 if(l2_3<l2_4)
127 if(l1_4<l2_3)
128 {
129 //l1_4;
130 point[0]=0.5f*(O1[0]+O4[0]);
131 point[1]=0.5f*(O1[1]+O4[1]);
132 point[2]=0.5f*(O1[2]+O4[2]);
133 }
134 else{
135 //l2_3;
136 point[0]=0.5f*(O2[0]+O3[0]);
137 point[1]=0.5f*(O2[1]+O3[1]);
138 point[2]=0.5f*(O2[2]+O3[2]);
139 }
140 else
141 if(l1_4<l2_4)
142 {
143 //l1_4;
144 point[0]=0.5f*(O1[0]+O4[0]);
145 point[1]=0.5f*(O1[1]+O4[1]);
146 point[2]=0.5f*(O1[2]+O4[2]);
147 }
148 else{
149 //l2_4;
150 point[0]=0.5f*(O2[0]+O4[0]);
151 point[1]=0.5f*(O2[1]+O4[1]);
152 point[2]=0.5f*(O2[2]+O4[2]);
153 }
154
155return true;
156}
157
158
159
160
161void lineClosestApproach (const dVector3 pa, const dVector3 ua,
162 const dVector3 pb, const dVector3 ub,
163 dReal *alpha, dReal *beta)
164{
165 dVector3 p;
166 p[0] = pb[0] - pa[0];
167 p[1] = pb[1] - pa[1];
168 p[2] = pb[2] - pa[2];
169 dReal uaub = dDOT(ua,ub);
170 dReal q1 = dDOT(ua,p);
171 dReal q2 = -dDOT(ub,p);
172 dReal d = 1-uaub*uaub;
173 if (d <= 0) {
174 // @@@ this needs to be made more robust
175 *alpha = 0;
176 *beta = 0;
177 }
178 else {
179 d = dRecip(d);
180 *alpha = (q1 + uaub*q2)*d;
181 *beta = (uaub*q1 + q2)*d;
182 }
183}
184
185
186// @@@ some stuff to optimize here, reuse code in contact point calculations.
187
188extern "C" int dCylBox (const dVector3 p1, const dMatrix3 R1,
189 const dReal radius,const dReal lz, const dVector3 p2,
190 const dMatrix3 R2, const dVector3 side2,
191 dVector3 normal, dReal *depth, int *code,
192 int maxc, dContactGeom *contact, int skip)
193{
194 dVector3 p,pp,normalC;
195 const dReal *normalR = 0;
196 dReal B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
197 Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,sQ21,sQ22,sQ23;
198 int i,invert_normal;
199
200 // get vector from centers of box 1 to box 2, relative to box 1
201 p[0] = p2[0] - p1[0];
202 p[1] = p2[1] - p1[1];
203 p[2] = p2[2] - p1[2];
204 dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
205
206 // get side lengths / 2
207 //A1 =radius; A2 = lz*REAL(0.5); A3 = radius;
208 dReal hlz=lz/2.f;
209 B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
210
211 // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
212 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
213 R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
214 R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
215
216 Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
217 Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
218 Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
219
220
221 // * see if the axis separates the box with cylinder. if so, return 0.
222 // * find the depth of the penetration along the separating axis (s2)
223 // * if this is the largest depth so far, record it.
224 // the normal vector will be set to the separating axis with the smallest
225 // depth. note: normalR is set to point to a column of R1 or R2 if that is
226 // the smallest depth normal so far. otherwise normalR is 0 and normalC is
227 // set to a vector relative to body 1. invert_normal is 1 if the sign of
228 // the normal should be flipped.
229
230#define TEST(expr1,expr2,norm,cc) \
231 s2 = dFabs(expr1) - (expr2); \
232 if (s2 > 0) return 0; \
233 if (s2 > s) { \
234 s = s2; \
235 normalR = norm; \
236 invert_normal = ((expr1) < 0); \
237 *code = (cc); \
238 }
239
240 s = -dInfinity;
241 invert_normal = 0;
242 *code = 0;
243
244 // separating axis = cylinder ax u2
245 //used when a box vertex touches a flat face of the cylinder
246 TEST (pp[1],(hlz + B1*Q21 + B2*Q22 + B3*Q23),R1+1,0);
247
248
249 // separating axis = box axis v1,v2,v3
250 //used when cylinder edge touches box face
251 //there is two ways to compute sQ: sQ21=sqrtf(1.f-Q21*Q21); or sQ21=sqrtf(Q23*Q23+Q22*Q22);
252 //if we did not need Q23 and Q22 the first way might be used to quiken the routine but then it need to
253 //check if Q21<=1.f, becouse it may slightly exeed 1.f.
254
255
256 sQ21=sqrtf(Q23*Q23+Q22*Q22);
257 TEST (dDOT41(R2+0,p),(radius*sQ21 + hlz*Q21 + B1),R2+0,1);
258
259 sQ22=sqrtf(Q23*Q23+Q21*Q21);
260 TEST (dDOT41(R2+1,p),(radius*sQ22 + hlz*Q22 + B2),R2+1,2);
261
262 sQ23=sqrtf(Q22*Q22+Q21*Q21);
263 TEST (dDOT41(R2+2,p),(radius*sQ23 + hlz*Q23 + B3),R2+2,3);
264
265
266#undef TEST
267#define TEST(expr1,expr2,n1,n2,n3,cc) \
268 s2 = dFabs(expr1) - (expr2); \
269 if (s2 > 0) return 0; \
270 if (s2 > s) { \
271 s = s2; \
272 normalR = 0; \
273 normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
274 invert_normal = ((expr1) < 0); \
275 *code = (cc); \
276 }
277
278
279
280// separating axis is a normal to the cylinder axis passing across the nearest box vertex
281//used when a box vertex touches the lateral surface of the cylinder
282
283dReal proj,boxProj,cos,sin,cos1,cos3;
284dVector3 tAx,Ax,pb;
285{
286//making Ax which is perpendicular to cyl ax to box position//
287proj=dDOT14(p2,R1+1)-dDOT14(p1,R1+1);
288
289Ax[0]=p2[0]-p1[0]-R1[1]*proj;
290Ax[1]=p2[1]-p1[1]-R1[5]*proj;
291Ax[2]=p2[2]-p1[2]-R1[9]*proj;
292dNormalize3(Ax);
293//using Ax find box vertex which is nearest to the cylinder axis
294 dReal sign;
295
296 for (i=0; i<3; i++) pb[i] = p2[i];
297 sign = (dDOT14(Ax,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
298 for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
299 sign = (dDOT14(Ax,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
300 for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
301 sign = (dDOT14(Ax,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
302 for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];
303
304//building axis which is normal to cylinder ax to the nearest box vertex
305proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1);
306
307Ax[0]=pb[0]-p1[0]-R1[1]*proj;
308Ax[1]=pb[1]-p1[1]-R1[5]*proj;
309Ax[2]=pb[2]-p1[2]-R1[9]*proj;
310dNormalize3(Ax);
311}
312
313boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
314 dFabs(dDOT14(Ax,R2+1)*B2)+
315 dFabs(dDOT14(Ax,R2+2)*B3);
316
317TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(radius+boxProj),Ax[0],Ax[1],Ax[2],4);
318
319
320//next three test used to handle collisions between cylinder circles and box ages
321proj=dDOT14(p1,R2+0)-dDOT14(p2,R2+0);
322
323tAx[0]=-p1[0]+p2[0]+R2[0]*proj;
324tAx[1]=-p1[1]+p2[1]+R2[4]*proj;
325tAx[2]=-p1[2]+p2[2]+R2[8]*proj;
326dNormalize3(tAx);
327
328//now tAx is normal to first ax of the box to cylinder center
329//making perpendicular to tAx lying in the plane which is normal to the cylinder axis
330//it is tangent in the point where projection of tAx on cylinder's ring intersect edge circle
331
332cos=dDOT14(tAx,R1+0);
333sin=dDOT14(tAx,R1+2);
334tAx[0]=R1[2]*cos-R1[0]*sin;
335tAx[1]=R1[6]*cos-R1[4]*sin;
336tAx[2]=R1[10]*cos-R1[8]*sin;
337
338
339//use cross between tAx and first ax of the box as separating axix
340
341dCROSS114(Ax,=,tAx,R2+0);
342dNormalize3(Ax);
343
344boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
345 dFabs(dDOT14(Ax,R2+0)*B1)+
346 dFabs(dDOT14(Ax,R2+2)*B3);
347
348 cos=dFabs(dDOT14(Ax,R1+1));
349 cos1=dDOT14(Ax,R1+0);
350 cos3=dDOT14(Ax,R1+2);
351 sin=sqrtf(cos1*cos1+cos3*cos3);
352
353TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],5);
354
355
356//same thing with the second axis of the box
357proj=dDOT14(p1,R2+1)-dDOT14(p2,R2+1);
358
359tAx[0]=-p1[0]+p2[0]+R2[1]*proj;
360tAx[1]=-p1[1]+p2[1]+R2[5]*proj;
361tAx[2]=-p1[2]+p2[2]+R2[9]*proj;
362dNormalize3(tAx);
363
364
365cos=dDOT14(tAx,R1+0);
366sin=dDOT14(tAx,R1+2);
367tAx[0]=R1[2]*cos-R1[0]*sin;
368tAx[1]=R1[6]*cos-R1[4]*sin;
369tAx[2]=R1[10]*cos-R1[8]*sin;
370
371dCROSS114(Ax,=,tAx,R2+1);
372dNormalize3(Ax);
373
374boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
375 dFabs(dDOT14(Ax,R2+1)*B2)+
376 dFabs(dDOT14(Ax,R2+2)*B3);
377
378 cos=dFabs(dDOT14(Ax,R1+1));
379 cos1=dDOT14(Ax,R1+0);
380 cos3=dDOT14(Ax,R1+2);
381 sin=sqrtf(cos1*cos1+cos3*cos3);
382TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],6);
383
384//same thing with the third axis of the box
385proj=dDOT14(p1,R2+2)-dDOT14(p2,R2+2);
386
387Ax[0]=-p1[0]+p2[0]+R2[2]*proj;
388Ax[1]=-p1[1]+p2[1]+R2[6]*proj;
389Ax[2]=-p1[2]+p2[2]+R2[10]*proj;
390dNormalize3(tAx);
391
392cos=dDOT14(tAx,R1+0);
393sin=dDOT14(tAx,R1+2);
394tAx[0]=R1[2]*cos-R1[0]*sin;
395tAx[1]=R1[6]*cos-R1[4]*sin;
396tAx[2]=R1[10]*cos-R1[8]*sin;
397
398dCROSS114(Ax,=,tAx,R2+2);
399dNormalize3(Ax);
400boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
401 dFabs(dDOT14(Ax,R2+2)*B3)+
402 dFabs(dDOT14(Ax,R2+0)*B1);
403
404 cos=dFabs(dDOT14(Ax,R1+1));
405 cos1=dDOT14(Ax,R1+0);
406 cos3=dDOT14(Ax,R1+2);
407 sin=sqrtf(cos1*cos1+cos3*cos3);
408TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],7);
409
410
411#undef TEST
412
413// note: cross product axes need to be scaled when s is computed.
414// normal (n1,n2,n3) is relative to box 1.
415
416#define TEST(expr1,expr2,n1,n2,n3,cc) \
417 s2 = dFabs(expr1) - (expr2); \
418 if (s2 > 0) return 0; \
419 l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
420 if (l > 0) { \
421 s2 /= l; \
422 if (s2 > s) { \
423 s = s2; \
424 normalR = 0; \
425 normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
426 invert_normal = ((expr1) < 0); \
427 *code = (cc); \
428 } \
429 }
430
431//crosses between cylinder axis and box axes
432 // separating axis = u2 x (v1,v2,v3)
433 TEST(pp[0]*R31-pp[2]*R11,(radius+B2*Q23+B3*Q22),R31,0,-R11,8);
434 TEST(pp[0]*R32-pp[2]*R12,(radius+B1*Q23+B3*Q21),R32,0,-R12,9);
435 TEST(pp[0]*R33-pp[2]*R13,(radius+B1*Q22+B2*Q21),R33,0,-R13,10);
436
437
438#undef TEST
439
440 // if we get to this point, the boxes interpenetrate. compute the normal
441 // in global coordinates.
442 if (normalR) {
443 normal[0] = normalR[0];
444 normal[1] = normalR[4];
445 normal[2] = normalR[8];
446 }
447 else {
448 if(*code>7) dMULTIPLY0_331 (normal,R1,normalC);
449 else {normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];}
450 }
451 if (invert_normal) {
452 normal[0] = -normal[0];
453 normal[1] = -normal[1];
454 normal[2] = -normal[2];
455 }
456 *depth = -s;
457
458 // compute contact point(s)
459
460 if (*code > 7) {
461 //find point on the cylinder pa deepest along normal
462 dVector3 pa;
463 dReal sign, cos1,cos3,factor;
464
465
466 for (i=0; i<3; i++) pa[i] = p1[i];
467
468 cos1 = dDOT14(normal,R1+0);
469 cos3 = dDOT14(normal,R1+2) ;
470 factor=sqrtf(cos1*cos1+cos3*cos3);
471
472 cos1/=factor;
473 cos3/=factor;
474
475 for (i=0; i<3; i++) pa[i] += cos1 * radius * R1[i*4];
476
477 sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
478 for (i=0; i<3; i++) pa[i] += sign * hlz * R1[i*4+1];
479
480
481 for (i=0; i<3; i++) pa[i] += cos3 * radius * R1[i*4+2];
482
483 // find vertex of the box deepest along normal
484 dVector3 pb;
485 for (i=0; i<3; i++) pb[i] = p2[i];
486 sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
487 for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
488 sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
489 for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
490 sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
491 for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];
492
493
494 dReal alpha,beta;
495 dVector3 ua,ub;
496 for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
497 for (i=0; i<3; i++) ub[i] = R2[*code-8 + i*4];
498
499 lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
500 for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
501 for (i=0; i<3; i++) pb[i] += ub[i]*beta;
502
503 for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
504 contact[0].depth = *depth;
505 return 1;
506 }
507
508
509 if(*code==4){
510 for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
511 contact[0].depth = *depth;
512 return 1;
513 }
514
515
516 dVector3 vertex;
517 if (*code == 0) {
518
519 dReal sign;
520 for (i=0; i<3; i++) vertex[i] = p2[i];
521 sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
522 for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4];
523 sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
524 for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1];
525 sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
526 for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2];
527 }
528 else {
529
530 dReal sign,cos1,cos3,factor;
531 for (i=0; i<3; i++) vertex[i] = p1[i];
532 cos1 = dDOT14(normal,R1+0) ;
533 cos3 = dDOT14(normal,R1+2);
534 factor=sqrtf(cos1*cos1+cos3*cos3);
535 factor= factor ? factor : 1.f;
536 cos1/=factor;
537 cos3/=factor;
538 for (i=0; i<3; i++) vertex[i] += cos1 * radius * R1[i*4];
539
540 sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
541 for (i=0; i<3; i++) vertex[i] += sign * hlz * R1[i*4+1];
542
543 for (i=0; i<3; i++) vertex[i] += cos3 * radius * R1[i*4+2];
544 }
545 for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
546 contact[0].depth = *depth;
547 return 1;
548}
549
550//****************************************************************************
551
552extern "C" int dCylCyl (const dVector3 p1, const dMatrix3 R1,
553 const dReal radius1,const dReal lz1, const dVector3 p2,
554 const dMatrix3 R2, const dReal radius2,const dReal lz2,
555 dVector3 normal, dReal *depth, int *code,
556 int maxc, dContactGeom *contact, int skip)
557{
558 dVector3 p,pp1,pp2,normalC;
559 const dReal *normalR = 0;
560 dReal hlz1,hlz2,s,s2;
561 int i,invert_normal;
562
563 // get vector from centers of box 1 to box 2, relative to box 1
564 p[0] = p2[0] - p1[0];
565 p[1] = p2[1] - p1[1];
566 p[2] = p2[2] - p1[2];
567 dMULTIPLY1_331 (pp1,R1,p); // get pp1 = p relative to body 1
568 dMULTIPLY1_331 (pp2,R2,p);
569 // get side lengths / 2
570 hlz1 = lz1*REAL(0.5);
571 hlz2 = lz2*REAL(0.5);
572
573 dReal proj,cos,sin,cos1,cos3;
574
575
576
577#define TEST(expr1,expr2,norm,cc) \
578 s2 = dFabs(expr1) - (expr2); \
579 if (s2 > 0) return 0; \
580 if (s2 > s) { \
581 s = s2; \
582 normalR = norm; \
583 invert_normal = ((expr1) < 0); \
584 *code = (cc); \
585 }
586
587 s = -dInfinity;
588 invert_normal = 0;
589 *code = 0;
590
591 cos=dFabs(dDOT44(R1+1,R2+1));
592 sin=sqrtf(1.f-(cos>1.f ? 1.f : cos));
593
594 TEST (pp1[1],(hlz1 + radius2*sin + hlz2*cos ),R1+1,0);//pp
595
596 TEST (pp2[1],(radius1*sin + hlz1*cos + hlz2),R2+1,1);
597
598
599
600 // note: cross product axes need to be scaled when s is computed.
601
602#undef TEST
603#define TEST(expr1,expr2,n1,n2,n3,cc) \
604 s2 = dFabs(expr1) - (expr2); \
605 if (s2 > 0) return 0; \
606 if (s2 > s) { \
607 s = s2; \
608 normalR = 0; \
609 normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
610 invert_normal = ((expr1) < 0); \
611 *code = (cc); \
612 }
613
614
615dVector3 tAx,Ax,pa,pb;
616
617//cross between cylinders' axes
618dCROSS144(Ax,=,R1+1,R2+1);
619dNormalize3(Ax);
620TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+radius2,Ax[0],Ax[1],Ax[2],6);
621
622
623{
624
625 dReal sign, factor;
626
627 //making ax which is perpendicular to cyl1 ax passing across cyl2 position//
628 //(project p on cyl1 flat surface )
629 for (i=0; i<3; i++) pb[i] = p2[i];
630 //cos1 = dDOT14(p,R1+0);
631 //cos3 = dDOT14(p,R1+2) ;
632 tAx[0]=pp1[0]*R1[0]+pp1[2]*R1[2];
633 tAx[1]=pp1[0]*R1[4]+pp1[2]*R1[6];
634 tAx[2]=pp1[0]*R1[8]+pp1[2]*R1[10];
635 dNormalize3(tAx);
636
637//find deepest point pb of cyl2 on opposite direction of tAx
638 cos1 = dDOT14(tAx,R2+0);
639 cos3 = dDOT14(tAx,R2+2) ;
640 factor=sqrtf(cos1*cos1+cos3*cos3);
641 cos1/=factor;
642 cos3/=factor;
643 for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];
644
645 sign = (dDOT14(tAx,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
646 for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];
647
648 for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];
649
650//making perpendicular to cyl1 ax passing across pb
651 proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1);
652
653 Ax[0]=pb[0]-p1[0]-R1[1]*proj;
654 Ax[1]=pb[1]-p1[1]-R1[5]*proj;
655 Ax[2]=pb[2]-p1[2]-R1[9]*proj;
656
657}
658
659dNormalize3(Ax);
660
661
662 cos=dFabs(dDOT14(Ax,R2+1));
663 cos1=dDOT14(Ax,R2+0);
664 cos3=dDOT14(Ax,R2+2);
665 sin=sqrtf(cos1*cos1+cos3*cos3);
666
667TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+cos*hlz2+sin*radius2,Ax[0],Ax[1],Ax[2],3);
668
669
670
671{
672
673 dReal sign, factor;
674
675 for (i=0; i<3; i++) pa[i] = p1[i];
676
677 //making ax which is perpendicular to cyl2 ax passing across cyl1 position//
678 //(project p on cyl2 flat surface )
679 //cos1 = dDOT14(p,R2+0);
680 //cos3 = dDOT14(p,R2+2) ;
681 tAx[0]=pp2[0]*R2[0]+pp2[2]*R2[2];
682 tAx[1]=pp2[0]*R2[4]+pp2[2]*R2[6];
683 tAx[2]=pp2[0]*R2[8]+pp2[2]*R2[10];
684 dNormalize3(tAx);
685
686 cos1 = dDOT14(tAx,R1+0);
687 cos3 = dDOT14(tAx,R1+2) ;
688 factor=sqrtf(cos1*cos1+cos3*cos3);
689 cos1/=factor;
690 cos3/=factor;
691
692//find deepest point pa of cyl2 on direction of tAx
693 for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];
694
695 sign = (dDOT14(tAx,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
696 for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];
697
698
699 for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];
700
701 proj=dDOT14(pa,R2+1)-dDOT14(p2,R2+1);
702
703 Ax[0]=pa[0]-p2[0]-R2[1]*proj;
704 Ax[1]=pa[1]-p2[1]-R2[5]*proj;
705 Ax[2]=pa[2]-p2[2]-R2[9]*proj;
706
707}
708dNormalize3(Ax);
709
710
711
712 cos=dFabs(dDOT14(Ax,R1+1));
713 cos1=dDOT14(Ax,R1+0);
714 cos3=dDOT14(Ax,R1+2);
715 sin=sqrtf(cos1*cos1+cos3*cos3);
716
717TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius2+cos*hlz1+sin*radius1,Ax[0],Ax[1],Ax[2],4);
718
719
720////test circl
721
722//@ this needed to set right normal when cylinders edges intersect
723//@ the most precise axis for this test may be found as a line between nearest points of two
724//@ circles. But it needs comparatively a lot of computation.
725//@ I use a trick which lets not to solve quadric equation.
726//@ In the case when cylinder eidges touches the test below rather accurate.
727//@ I still not sure about problems with sepparation but they have not been revealed during testing.
728dVector3 point;
729{
730 dVector3 ca,cb;
731 dReal sign;
732 for (i=0; i<3; i++) ca[i] = p1[i];
733 for (i=0; i<3; i++) cb[i] = p2[i];
734//find two nearest flat rings
735 sign = (pp1[1] > 0) ? REAL(1.0) : REAL(-1.0);
736 for (i=0; i<3; i++) ca[i] += sign * hlz1 * R1[i*4+1];
737
738 sign = (pp2[1] > 0) ? REAL(1.0) : REAL(-1.0);
739 for (i=0; i<3; i++) cb[i] -= sign * hlz2 * R2[i*4+1];
740
741 dVector3 tAx,tAx1;
742 circleIntersection(R1+1,ca,radius1,R2+1,cb,radius2,point);
743
744 Ax[0]=point[0]-ca[0];
745 Ax[1]=point[1]-ca[1];
746 Ax[2]=point[2]-ca[2];
747
748 cos1 = dDOT14(Ax,R1+0);
749 cos3 = dDOT14(Ax,R1+2) ;
750
751 tAx[0]=cos3*R1[0]-cos1*R1[2];
752 tAx[1]=cos3*R1[4]-cos1*R1[6];
753 tAx[2]=cos3*R1[8]-cos1*R1[10];
754
755 Ax[0]=point[0]-cb[0];
756 Ax[1]=point[1]-cb[1];
757 Ax[2]=point[2]-cb[2];
758
759
760 cos1 = dDOT14(Ax,R2+0);
761 cos3 = dDOT14(Ax,R2+2) ;
762
763 tAx1[0]=cos3*R2[0]-cos1*R2[2];
764 tAx1[1]=cos3*R2[4]-cos1*R2[6];
765 tAx1[2]=cos3*R2[8]-cos1*R2[10];
766 dCROSS(Ax,=,tAx,tAx1);
767
768
769
770
771dNormalize3(Ax);
772dReal cyl1Pr,cyl2Pr;
773
774 cos=dFabs(dDOT14(Ax,R1+1));
775 cos1=dDOT14(Ax,R1+0);
776 cos3=dDOT14(Ax,R1+2);
777 sin=sqrtf(cos1*cos1+cos3*cos3);
778 cyl1Pr=cos*hlz1+sin*radius1;
779
780 cos=dFabs(dDOT14(Ax,R2+1));
781 cos1=dDOT14(Ax,R2+0);
782 cos3=dDOT14(Ax,R2+2);
783 sin=sqrtf(cos1*cos1+cos3*cos3);
784 cyl2Pr=cos*hlz2+sin*radius2;
785TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],cyl1Pr+cyl2Pr,Ax[0],Ax[1],Ax[2],5);
786
787
788}
789
790
791#undef TEST
792
793
794
795 // if we get to this point, the cylinders interpenetrate. compute the normal
796 // in global coordinates.
797 if (normalR) {
798 normal[0] = normalR[0];
799 normal[1] = normalR[4];
800 normal[2] = normalR[8];
801 }
802 else {
803 normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];
804 }
805 if (invert_normal) {
806 normal[0] = -normal[0];
807 normal[1] = -normal[1];
808 normal[2] = -normal[2];
809 }
810
811 *depth = -s;
812
813 // compute contact point(s)
814
815 if(*code==3){
816 for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
817 contact[0].depth = *depth;
818 return 1;
819 }
820
821 if(*code==4){
822 for (i=0; i<3; i++) contact[0].pos[i] = pa[i];
823 contact[0].depth = *depth;
824 return 1;
825 }
826
827 if(*code==5){
828 for (i=0; i<3; i++) contact[0].pos[i] = point[i];
829 contact[0].depth = *depth;
830 return 1;
831 }
832
833if (*code == 6) {
834 dVector3 pa;
835 dReal sign, cos1,cos3,factor;
836
837
838 for (i=0; i<3; i++) pa[i] = p1[i];
839
840 cos1 = dDOT14(normal,R1+0);
841 cos3 = dDOT14(normal,R1+2) ;
842 factor=sqrtf(cos1*cos1+cos3*cos3);
843
844 cos1/=factor;
845 cos3/=factor;
846
847 for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];
848
849 sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
850 for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];
851
852
853 for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];
854
855 // find a point pb on the intersecting edge of cylinder 2
856 dVector3 pb;
857 for (i=0; i<3; i++) pb[i] = p2[i];
858 cos1 = dDOT14(normal,R2+0);
859 cos3 = dDOT14(normal,R2+2) ;
860 factor=sqrtf(cos1*cos1+cos3*cos3);
861
862 cos1/=factor;
863 cos3/=factor;
864
865 for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];
866
867 sign = (dDOT14(normal,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
868 for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];
869
870
871 for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];
872
873
874 dReal alpha,beta;
875 dVector3 ua,ub;
876 for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
877 for (i=0; i<3; i++) ub[i] = R2[1 + i*4];
878 lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
879 for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
880 for (i=0; i<3; i++) pb[i] += ub[i]*beta;
881
882 for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
883 contact[0].depth = *depth;
884 return 1;
885 }
886
887 // okay, we have a face-something intersection (because the separating
888 // axis is perpendicular to a face).
889
890 // @@@ temporary: make deepest point on the "other" cylinder the contact point.
891 // @@@ this kind of works, but we need multiple contact points for stability,
892 // @@@ especially for face-face contact.
893
894 dVector3 vertex;
895 if (*code == 0) {
896 // flat face from cylinder 1 touches a edge/face from cylinder 2.
897 dReal sign,cos1,cos3,factor;
898 for (i=0; i<3; i++) vertex[i] = p2[i];
899 cos1 = dDOT14(normal,R2+0) ;
900 cos3 = dDOT14(normal,R2+2);
901 factor=sqrtf(cos1*cos1+cos3*cos3);
902
903 cos1/=factor;
904 cos3/=factor;
905 for (i=0; i<3; i++) vertex[i] -= cos1 * radius2 * R2[i*4];
906
907 sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
908 for (i=0; i<3; i++) vertex[i] -= sign * hlz2 * R2[i*4+1];
909
910 for (i=0; i<3; i++) vertex[i] -= cos3 * radius2 * R2[i*4+2];
911 }
912 else {
913 // flat face from cylinder 2 touches a edge/face from cylinder 1.
914 dReal sign,cos1,cos3,factor;
915 for (i=0; i<3; i++) vertex[i] = p1[i];
916 cos1 = dDOT14(normal,R1+0) ;
917 cos3 = dDOT14(normal,R1+2);
918 factor=sqrtf(cos1*cos1+cos3*cos3);
919
920 cos1/=factor;
921 cos3/=factor;
922 for (i=0; i<3; i++) vertex[i] += cos1 * radius1 * R1[i*4];
923
924 sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
925 for (i=0; i<3; i++) vertex[i] += sign * hlz1 * R1[i*4+1];
926
927 for (i=0; i<3; i++) vertex[i] += cos3 * radius1 * R1[i*4+2];
928 }
929 for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
930 contact[0].depth = *depth;
931 return 1;
932}
933
934//****************************************************************************
935
936
937int dCollideCylS (dxGeom *o1, dxGeom *o2, int flags,
938 dContactGeom *contact, int skip)
939{
940
941
942 dIASSERT (skip >= (int)sizeof(dContactGeom));
943 dIASSERT (dGeomGetClass(o2) == dSphereClass);
944 dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
945 const dReal* p1=dGeomGetPosition(o1);
946 const dReal* p2=dGeomGetPosition(o2);
947 const dReal* R=dGeomGetRotation(o1);
948 dVector3 p,normalC,normal;
949 const dReal *normalR = 0;
950 dReal cylRadius;
951 dReal hl;
952 dGeomCylinderGetParams(o1,&cylRadius,&hl);
953 dReal sphereRadius;
954 sphereRadius=dGeomSphereGetRadius(o2);
955
956 int i,invert_normal;
957
958 // get vector from centers of cyl to shere
959 p[0] = p2[0] - p1[0];
960 p[1] = p2[1] - p1[1];
961 p[2] = p2[2] - p1[2];
962
963dReal s,s2;
964unsigned char code;
965#define TEST(expr1,expr2,norm,cc) \
966 s2 = dFabs(expr1) - (expr2); \
967 if (s2 > 0) return 0; \
968 if (s2 > s) { \
969 s = s2; \
970 normalR = norm; \
971 invert_normal = ((expr1) < 0); \
972 code = (cc); \
973 }
974
975 s = -dInfinity;
976 invert_normal = 0;
977 code = 0;
978
979 // separating axis cyl ax
980
981 TEST (dDOT14(p,R+1),sphereRadius+hl,R+1,2);
982 // note: cross product axes need to be scaled when s is computed.
983 // normal (n1,n2,n3) is relative to
984#undef TEST
985#define TEST(expr1,expr2,n1,n2,n3,cc) \
986 s2 = dFabs(expr1) - (expr2); \
987 if (s2 > 0) return 0; \
988 if (s2 > s) { \
989 s = s2; \
990 normalR = 0; \
991 normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
992 invert_normal = ((expr1) < 0); \
993 code = (cc); \
994 }
995
996//making ax which is perpendicular to cyl1 ax to sphere center//
997
998dReal proj,cos,sin,cos1,cos3;
999dVector3 Ax;
1000 proj=dDOT14(p2,R+1)-dDOT14(p1,R+1);
1001
1002 Ax[0]=p2[0]-p1[0]-R[1]*proj;
1003 Ax[1]=p2[1]-p1[1]-R[5]*proj;
1004 Ax[2]=p2[2]-p1[2]-R[9]*proj;
1005dNormalize3(Ax);
1006TEST(dDOT(p,Ax),sphereRadius+cylRadius,Ax[0],Ax[1],Ax[2],9);
1007
1008
1009Ax[0]=p[0];
1010Ax[1]=p[1];
1011Ax[2]=p[2];
1012dNormalize3(Ax);
1013
1014 dVector3 pa;
1015 dReal sign, factor;
1016 for (i=0; i<3; i++) pa[i] = p1[i];
1017
1018 cos1 = dDOT14(Ax,R+0);
1019 cos3 = dDOT14(Ax,R+2) ;
1020 factor=sqrtf(cos1*cos1+cos3*cos3);
1021 cos1/=factor;
1022 cos3/=factor;
1023 for (i=0; i<3; i++) pa[i] += cos1 * cylRadius * R[i*4];
1024 sign = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0);
1025 for (i=0; i<3; i++) pa[i] += sign * hl * R[i*4+1];
1026 for (i=0; i<3; i++) pa[i] += cos3 * cylRadius * R[i*4+2];
1027
1028Ax[0]=p2[0]-pa[0];
1029Ax[1]=p2[1]-pa[1];
1030Ax[2]=p2[2]-pa[2];
1031dNormalize3(Ax);
1032
1033 cos=dFabs(dDOT14(Ax,R+1));
1034 cos1=dDOT14(Ax,R+0);
1035 cos3=dDOT14(Ax,R+2);
1036 sin=sqrtf(cos1*cos1+cos3*cos3);
1037TEST(dDOT(p,Ax),sphereRadius+cylRadius*sin+hl*cos,Ax[0],Ax[1],Ax[2],14);
1038
1039
1040#undef TEST
1041
1042 if (normalR) {
1043 normal[0] = normalR[0];
1044 normal[1] = normalR[4];
1045 normal[2] = normalR[8];
1046 }
1047 else {
1048
1049 normal[0] = normalC[0];
1050 normal[1] = normalC[1];
1051 normal[2] = normalC[2];
1052 }
1053 if (invert_normal) {
1054 normal[0] = -normal[0];
1055 normal[1] = -normal[1];
1056 normal[2] = -normal[2];
1057 }
1058 // compute contact point(s)
1059contact->depth=-s;
1060contact->normal[0]=-normal[0];
1061contact->normal[1]=-normal[1];
1062contact->normal[2]=-normal[2];
1063contact->g1=const_cast<dxGeom*> (o1);
1064contact->g2=const_cast<dxGeom*> (o2);
1065contact->pos[0]=p2[0]-normal[0]*sphereRadius;
1066contact->pos[1]=p2[1]-normal[1]*sphereRadius;
1067contact->pos[2]=p2[2]-normal[2]*sphereRadius;
1068return 1;
1069}
1070
1071
1072
1073int dCollideCylB (dxGeom *o1, dxGeom *o2, int flags,
1074 dContactGeom *contact, int skip)
1075{
1076 dVector3 normal;
1077 dReal depth;
1078 int code;
1079 dReal cylRadius,cylLength;
1080 dVector3 boxSides;
1081 dGeomCylinderGetParams(o1,&cylRadius,&cylLength);
1082 dGeomBoxGetLengths(o2,boxSides);
1083 int num = dCylBox(dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius,cylLength,
1084 dGeomGetPosition(o2),dGeomGetRotation(o2),boxSides,
1085 normal,&depth,&code,flags & NUMC_MASK,contact,skip);
1086 for (int i=0; i<num; i++) {
1087 CONTACT(contact,i*skip)->normal[0] = -normal[0];
1088 CONTACT(contact,i*skip)->normal[1] = -normal[1];
1089 CONTACT(contact,i*skip)->normal[2] = -normal[2];
1090 CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
1091 CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
1092 }
1093 return num;
1094}
1095
1096int dCollideCylCyl (dxGeom *o1, dxGeom *o2, int flags,
1097 dContactGeom *contact, int skip)
1098{
1099 dVector3 normal;
1100 dReal depth;
1101 int code;
1102dReal cylRadius1,cylRadius2;
1103dReal cylLength1,cylLength2;
1104dGeomCylinderGetParams(o1,&cylRadius1,&cylLength1);
1105dGeomCylinderGetParams(o2,&cylRadius2,&cylLength2);
1106int num = dCylCyl (dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius1,cylLength1,
1107 dGeomGetPosition(o2),dGeomGetRotation(o2),cylRadius2,cylLength2,
1108 normal,&depth,&code,flags & NUMC_MASK,contact,skip);
1109
1110 for (int i=0; i<num; i++) {
1111 CONTACT(contact,i*skip)->normal[0] = -normal[0];
1112 CONTACT(contact,i*skip)->normal[1] = -normal[1];
1113 CONTACT(contact,i*skip)->normal[2] = -normal[2];
1114 CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
1115 CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
1116 }
1117 return num;
1118}
1119
1120struct dxPlane {
1121 dReal p[4];
1122};
1123
1124
1125int dCollideCylPlane
1126 (
1127 dxGeom *o1, dxGeom *o2, int flags,
1128 dContactGeom *contact, int skip){
1129 dIASSERT (skip >= (int)sizeof(dContactGeom));
1130 dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
1131 dIASSERT (dGeomGetClass(o2) == dPlaneClass);
1132 contact->g1 = const_cast<dxGeom*> (o1);
1133 contact->g2 = const_cast<dxGeom*> (o2);
1134
1135 unsigned int ret = 0;
1136
1137 dReal radius;
1138 dReal hlz;
1139 dGeomCylinderGetParams(o1,&radius,&hlz);
1140 hlz /= 2;
1141
1142 const dReal *R = dGeomGetRotation(o1);// rotation of cylinder
1143 const dReal* p = dGeomGetPosition(o1);
1144 dVector4 n; // normal vector
1145 dReal pp;
1146 dGeomPlaneGetParams (o2, n);
1147 pp=n[3];
1148 dReal cos1,sin1;
1149 cos1=dFabs(dDOT14(n,R+1));
1150
1151cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
1152sin1=sqrtf(REAL(1.)-cos1*cos1);
1153//////////////////////////////
1154
1155dReal sidePr=cos1*hlz+sin1*radius;
1156
1157dReal dist=-pp+dDOT(n,p);
1158dReal outDepth=sidePr-dist;
1159
1160if(outDepth<0.f) return 0;
1161
1162dVector3 pos;
1163
1164
1165/////////////////////////////////////////// from geom.cpp dCollideBP
1166 dReal Q1 = dDOT14(n,R+0);
1167 dReal Q2 = dDOT14(n,R+1);
1168 dReal Q3 = dDOT14(n,R+2);
1169 dReal factor =sqrtf(Q1*Q1+Q3*Q3);
1170 factor= factor ? factor :1.f;
1171 dReal A1 = radius * Q1/factor;
1172 dReal A2 = hlz*Q2;
1173 dReal A3 = radius * Q3/factor;
1174
1175 pos[0]=p[0];
1176 pos[1]=p[1];
1177 pos[2]=p[2];
1178
1179 pos[0]-= A1*R[0];
1180 pos[1]-= A1*R[4];
1181 pos[2]-= A1*R[8];
1182
1183 pos[0]-= A3*R[2];
1184 pos[1]-= A3*R[6];
1185 pos[2]-= A3*R[10];
1186
1187 pos[0]-= A2>0 ? hlz*R[1]:-hlz*R[1];
1188 pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5];
1189 pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9];
1190
1191
1192
1193 contact->pos[0] = pos[0];
1194 contact->pos[1] = pos[1];
1195 contact->pos[2] = pos[2];
1196 contact->depth = outDepth;
1197 ret=1;
1198
1199if(dFabs(Q2)>M_SQRT1_2){
1200
1201 CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0];
1202 CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4];
1203 CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8];
1204 CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1);
1205
1206 if(CONTACT(contact,ret*skip)->depth>0.f)
1207 ret++;
1208
1209
1210 CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2];
1211 CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6];
1212 CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10];
1213 CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3);
1214
1215 if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
1216} else {
1217
1218 CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]);
1219 CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]);
1220 CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]);
1221 CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2);
1222
1223 if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
1224}
1225
1226
1227
1228 for (unsigned int i=0; i<ret; i++) {
1229 CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
1230 CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
1231 CONTACT(contact,i*skip)->normal[0] =n[0];
1232 CONTACT(contact,i*skip)->normal[1] =n[1];
1233 CONTACT(contact,i*skip)->normal[2] =n[2];
1234 }
1235 return ret;
1236}
1237
1238int dCollideCylRay(dxGeom *o1, dxGeom *o2, int flags,
1239 dContactGeom *contact, int skip) {
1240 dIASSERT (skip >= (int)sizeof(dContactGeom));
1241 dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
1242 dIASSERT (dGeomGetClass(o2) == dRayClass);
1243 contact->g1 = const_cast<dxGeom*> (o1);
1244 contact->g2 = const_cast<dxGeom*> (o2);
1245 dReal radius;
1246 dReal lz;
1247 dGeomCylinderGetParams(o1,&radius,&lz);
1248 dReal lz2=lz*REAL(0.5);
1249 const dReal *R = dGeomGetRotation(o1); // rotation of the cylinder
1250 const dReal *p = dGeomGetPosition(o1); // position of the cylinder
1251 dVector3 start,dir;
1252 dGeomRayGet(o2,start,dir); // position and orientation of the ray
1253 dReal length = dGeomRayGetLength(o2);
1254
1255 // compute some useful info
1256 dVector3 cs,q,r;
1257 dReal C,k;
1258 cs[0] = start[0] - p[0];
1259 cs[1] = start[1] - p[1];
1260 cs[2] = start[2] - p[2];
1261 k = dDOT41(R+1,cs); // position of ray start along cyl axis (Y)
1262 q[0] = k*R[0*4+1] - cs[0];
1263 q[1] = k*R[1*4+1] - cs[1];
1264 q[2] = k*R[2*4+1] - cs[2];
1265 C = dDOT(q,q) - radius*radius;
1266 // if C < 0 then ray start position within infinite extension of cylinder
1267 // if ray start position is inside the cylinder
1268 int inside_cyl=0;
1269 if (C<0 && !(k<-lz2 || k>lz2)) inside_cyl=1;
1270 // compute ray collision with infinite cylinder, except for the case where
1271 // the ray is outside the cylinder but within the infinite cylinder
1272 // (it that case the ray can only hit endcaps)
1273 if (!inside_cyl && C < 0) {
1274 // set k to cap position to check
1275 if (k < 0) k = -lz2; else k = lz2;
1276 }
1277 else {
1278 dReal uv = dDOT41(R+1,dir);
1279 r[0] = uv*R[0*4+1] - dir[0];
1280 r[1] = uv*R[1*4+1] - dir[1];
1281 r[2] = uv*R[2*4+1] - dir[2];
1282 dReal A = dDOT(r,r);
1283 dReal B = 2*dDOT(q,r);
1284 k = B*B-4*A*C;
1285 if (k < 0) {
1286 // the ray does not intersect the infinite cylinder, but if the ray is
1287 // inside and parallel to the cylinder axis it may intersect the end
1288 // caps. set k to cap position to check.
1289 if (!inside_cyl) return 0;
1290 if (uv < 0) k = -lz2; else k = lz2;
1291 }
1292 else {
1293 k = dSqrt(k);
1294 A = dRecip (2*A);
1295 dReal alpha = (-B-k)*A;
1296 if (alpha < 0) {
1297 alpha = (-B+k)*A;
1298 if (alpha<0) return 0;
1299 }
1300 if (alpha>length) return 0;
1301 // the ray intersects the infinite cylinder. check to see if the
1302 // intersection point is between the caps
1303 contact->pos[0] = start[0] + alpha*dir[0];
1304 contact->pos[1] = start[1] + alpha*dir[1];
1305 contact->pos[2] = start[2] + alpha*dir[2];
1306 q[0] = contact->pos[0] - p[0];
1307 q[1] = contact->pos[1] - p[1];
1308 q[2] = contact->pos[2] - p[2];
1309 k = dDOT14(q,R+1);
1310 dReal nsign = inside_cyl ? -1 : 1;
1311 if (k >= -lz2 && k <= lz2) {
1312 contact->normal[0] = nsign * (contact->pos[0] -
1313 (p[0] + k*R[0*4+1]));
1314 contact->normal[1] = nsign * (contact->pos[1] -
1315 (p[1] + k*R[1*4+1]));
1316 contact->normal[2] = nsign * (contact->pos[2] -
1317 (p[2] + k*R[2*4+1]));
1318 dNormalize3 (contact->normal);
1319 contact->depth = alpha;
1320 return 1;
1321 }
1322 // the infinite cylinder intersection point is not between the caps.
1323 // set k to cap position to check.
1324 if (k < 0) k = -lz2; else k = lz2;
1325 }
1326 }
1327 // check for ray intersection with the caps. k must indicate the cap
1328 // position to check
1329 // perform a ray plan interesection
1330 // R+1 is the plan normal
1331 q[0] = start[0] - (p[0] + k*R[0*4+1]);
1332 q[1] = start[1] - (p[1] + k*R[1*4+1]);
1333 q[2] = start[2] - (p[2] + k*R[2*4+1]);
1334 dReal alpha = -dDOT14(q,R+1);
1335 dReal k2 = dDOT14(dir,R+1);
1336 if (k2==0) return 0; // ray parallel to the plane
1337 alpha/=k2;
1338 if (alpha<0 || alpha>length) return 0; // too short
1339 contact->pos[0]=start[0]+alpha*dir[0];
1340 contact->pos[1]=start[1]+alpha*dir[1];
1341 contact->pos[2]=start[2]+alpha*dir[2];
1342 dReal nsign = (k<0)?-1:1;
1343 contact->normal[0]=nsign*R[0*4+1];
1344 contact->normal[1]=nsign*R[1*4+1];
1345 contact->normal[2]=nsign*R[2*4+1];
1346 contact->depth=alpha;
1347 return 1;
1348}
1349
1350static dColliderFn * dCylinderColliderFn (int num)
1351{
1352 if (num == dBoxClass) return (dColliderFn *) &dCollideCylB;
1353 else if (num == dSphereClass) return (dColliderFn *) &dCollideCylS;
1354 else if (num == dCylinderClassUser) return (dColliderFn *) &dCollideCylCyl;
1355 else if (num == dPlaneClass) return (dColliderFn *) &dCollideCylPlane;
1356 else if (num == dRayClass) return (dColliderFn *) &dCollideCylRay;
1357 return 0;
1358}
1359
1360
1361static void dCylinderAABB (dxGeom *geom, dReal aabb[6])
1362{
1363 dReal radius,lz;
1364 dGeomCylinderGetParams(geom,&radius,&lz);
1365const dReal* R= dGeomGetRotation(geom);
1366const dReal* pos= dGeomGetPosition(geom);
1367 dReal xrange = dFabs (R[0] *radius) +
1368 REAL(0.5) *dFabs (R[1] * lz) + dFabs (R[2] * radius);
1369
1370 dReal yrange = dFabs (R[4] *radius) +
1371 REAL(0.5) * dFabs (R[5] * lz) + dFabs (R[6] * radius);
1372
1373 dReal zrange = dFabs (R[8] * radius) +
1374 REAL(0.5) *dFabs (R[9] * lz) + dFabs (R[10] * radius);
1375
1376 aabb[0] = pos[0] - xrange;
1377 aabb[1] = pos[0] + xrange;
1378 aabb[2] = pos[1] - yrange;
1379 aabb[3] = pos[1] + yrange;
1380 aabb[4] = pos[2] - zrange;
1381 aabb[5] = pos[2] + zrange;
1382}
1383
1384dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz)
1385{
1386 dAASSERT (r > 0 && lz > 0);
1387 if (dCylinderClassUser == -1)
1388 {
1389 dGeomClass c;
1390 c.bytes = sizeof (dxCylinder);
1391 c.collider = &dCylinderColliderFn;
1392 c.aabb = &dCylinderAABB;
1393 c.aabb_test = 0;
1394 c.dtor = 0;
1395 dCylinderClassUser=dCreateGeomClass (&c);
1396
1397 }
1398
1399 dGeomID g = dCreateGeom (dCylinderClassUser);
1400 if (space) dSpaceAdd (space,g);
1401 dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
1402
1403 c->radius = r;
1404 c->lz = lz;
1405 return g;
1406}
1407
1408
1409
1410void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length)
1411{
1412 dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser,"argument not a cylinder");
1413 dAASSERT (radius > 0 && length > 0);
1414 dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
1415 c->radius = radius;
1416 c->lz = length;
1417}
1418
1419
1420
1421void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length)
1422{
1423 dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser ,"argument not a cylinder");
1424 dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
1425 *radius = c->radius;
1426 *length = c->lz;
1427}
1428
1429/*
1430void dMassSetCylinder (dMass *m, dReal density,
1431 dReal radius, dReal length)
1432{
1433 dAASSERT (m);
1434 dMassSetZero (m);
1435 dReal M = length*M_PI*radius*radius*density;
1436 m->mass = M;
1437 m->_I(0,0) = M/REAL(4.0) * (ly*ly + lz*lz);
1438 m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz);
1439 m->_I(2,2) = M/REAL(4.0) * (lx*lx + ly*ly);
1440
1441# ifndef dNODEBUG
1442 checkMass (m);
1443# endif
1444}
1445*/
diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.h b/libraries/ode-0.9/contrib/dCylinder/dCylinder.h
deleted file mode 100644
index 06e3a0b..0000000
--- a/libraries/ode-0.9/contrib/dCylinder/dCylinder.h
+++ /dev/null
@@ -1,14 +0,0 @@
1
2#ifndef dCylinder_h
3#define dCylinder_h
4
5struct dxCylinder;
6extern int dCylinderClassUser;
7
8
9dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz);
10void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length);
11
12void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length);
13#endif //dCylinder_h
14
diff --git a/libraries/ode-0.9/contrib/dCylinder/readme.txt b/libraries/ode-0.9/contrib/dCylinder/readme.txt
deleted file mode 100644
index facd13e..0000000
--- a/libraries/ode-0.9/contrib/dCylinder/readme.txt
+++ /dev/null
@@ -1,62 +0,0 @@
1readme.txt
2
3WARNING: THIS IS NOT VERY RELIABLE CODE. IT HAS BUGS. YOUR
4 SUCCESS MAY VARY. CONTRIBUTIONS OF FIXES/REWRITES ARE
5 WELCOME.
6
7///////////////////////////////////////////////////////////////////////
8
9Cylinder geometry class.
10
11New in this version:
12
13Cylinder class implemented as User Geometry Class so it now can be
14used with old and new ODE collision detection.
15
16Cylinder - Ray has been contributed by Olivier Michel.
17
18THE IDENTIFIER dCylinderClass HAS BEEN REPLACED BY dCylinderClassUser
19
20to avoid conflict with dCylinderClass in the enum definite in collision.h
21
22///////////////////////////////////////////////////////////////////////
23The dCylinder class includes the following collisions:
24
25Cylinder - Box
26Cylinder - Cylinder
27Cylinder - Sphere
28Cylinder - Plane
29Cylinder - Ray (contributed by Olivier Michel)
30
31Cylinder aligned along axis - Y when created. (Not like Capped
32Cylinder which aligned along axis - Z).
33
34Interface is just the same as Capped Cylinder has.
35
36Use functions which have one "C" instead of double "C".
37
38to create:
39dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length);
40
41to set params:
42void dGeomCylinderSetParams (dGeomID cylinder,
43 dReal radius, dReal length);
44
45
46to get params:
47void dGeomCylinderGetParams (dGeomID cylinder,
48 dReal *radius, dReal *length);
49
50Return in radius and length the parameters of the given cylinder.
51
52Identification number of the class:
53 dCylinderClassUser
54
55 I do not include a function that sets inertia tensor for cylinder.
56 One may use existing ODE functions dMassSetCappedCylinder or dMassSetBox.
57 To set exact tensor for cylinder use dMassSetParameters.
58 Remember cylinder aligned along axis - Y.
59
60 ///////////////////////////////////////////////////////////////////////////
61 Konstantin Slipchenko
62 February 5, 2002
diff --git a/libraries/ode-0.9/contrib/dRay/Include/dRay.h b/libraries/ode-0.9/contrib/dRay/Include/dRay.h
deleted file mode 100644
index f6caea8..0000000
--- a/libraries/ode-0.9/contrib/dRay/Include/dRay.h
+++ /dev/null
@@ -1,15 +0,0 @@
1#include "ode\ode.h"
2
3/* Class ID */
4extern int dRayClass;
5
6/* Creates a ray */
7dxGeom* dGeomCreateRay(dSpaceID space, dReal Length);
8
9/* Set/Get length */
10void dGeomRaySetLength(dxGeom* g, dReal Length);
11dReal dGeomRayGetLength(dxGeom* g);
12
13/* Utility function to override the ray's pos + rot */
14void dGeomRaySet(dxGeom* g, dVector3 Origin, dVector3 Direction);
15void dGeomRayGet(dxGeom* g, dVector3 Origin, dVector3 Direction);
diff --git a/libraries/ode-0.9/contrib/dRay/README.txt b/libraries/ode-0.9/contrib/dRay/README.txt
deleted file mode 100644
index 8997208..0000000
--- a/libraries/ode-0.9/contrib/dRay/README.txt
+++ /dev/null
@@ -1,16 +0,0 @@
1From: "Erwin de Vries" <erwin@vo.com>
2To: <ode@q12.org>
3Subject: [ODE] dRay class
4Date: Thu, 25 Jul 2002 13:05:28 +0200
5
6Yesterday and today i've written a dRay class. It interacts with dPlane,
7dSphere, dBox and dCCylinder. It does not generate full contact information.
8It only generates the pos member. I dont think its useful to anyone to go
9through hoops and find a reasonable normal and penetration depth, as i dont
10think anyone will want to use it for dynamics. Just for CD.
11
12It should compile in single and double precision mode, and should be
13platform independant. I hope.
14
15The next Tri-Collider release using Opcode 1.1 will also implement a ray
16collision function along with some other not too interesting improvements.
diff --git a/libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp b/libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp
deleted file mode 100644
index faa8b14..0000000
--- a/libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp
+++ /dev/null
@@ -1,1372 +0,0 @@
1/*************************************************************************
2
3
4 * *
5
6
7 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
8
9
10 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
11
12
13 * *
14
15
16 * This library is free software; you can redistribute it and/or *
17
18
19 * modify it under the terms of EITHER: *
20
21
22 * (1) The GNU Lesser General Public License as published by the Free *
23
24
25 * Software Foundation; either version 2.1 of the License, or (at *
26
27
28 * your option) any later version. The text of the GNU Lesser *
29
30
31 * General Public License is included with this library in the *
32
33
34 * file LICENSE.TXT. *
35
36
37 * (2) The BSD-style license that is included with this library in *
38
39
40 * the file LICENSE-BSD.TXT. *
41
42
43 * *
44
45
46 * This library is distributed in the hope that it will be useful, *
47
48
49 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
50
51
52 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
53
54
55 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
56
57
58 * *
59
60
61 *************************************************************************/
62
63
64
65
66
67#include <ode/ode.h>
68
69
70#include <dRay.h>
71
72
73#include <drawstuff/drawstuff.h>
74
75
76
77
78
79#ifdef _MSC_VER
80
81
82#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
83
84
85#endif
86
87
88
89
90
91// select correct drawing functions
92
93
94
95
96
97#ifdef dDOUBLE
98
99
100#define dsDrawBox dsDrawBoxD
101
102
103#define dsDrawSphere dsDrawSphereD
104
105
106#define dsDrawCylinder dsDrawCylinderD
107
108
109#define dsDrawCappedCylinder dsDrawCappedCylinderD
110
111
112#endif
113
114
115
116
117
118
119
120
121// some constants
122
123
124
125
126
127#define NUM 20 // max number of objects
128
129
130#define DENSITY (5.0) // density of all objects
131
132
133#define GPB 3 // maximum number of geometries per body
134
135
136
137
138
139
140
141
142// dynamics and collision objects
143
144
145
146
147
148struct MyObject {
149
150
151 dBodyID body; // the body
152
153
154 dGeomID geom[GPB]; // geometries representing this body
155
156
157};
158
159
160
161
162
163static int num=0; // number of objects in simulation
164
165
166static int nextobj=0; // next object to recycle if num==NUM
167
168
169static dWorldID world;
170
171
172static dSpaceID space;
173
174
175static MyObject obj[NUM];
176
177
178static dJointGroupID contactgroup;
179
180
181static int selected = -1; // selected object
182
183
184
185
186
187static dGeomID* Rays;
188
189
190static int RayCount;
191
192
193
194
195
196// this is called by dSpaceCollide when two objects in space are
197
198
199// potentially colliding.
200
201
202
203
204
205static void nearCallback (void *data, dGeomID o1, dGeomID o2)
206
207
208{
209
210
211 int i;
212
213
214 // if (o1->body && o2->body) return;
215
216
217
218
219
220 // exit without doing anything if the two bodies are connected by a joint
221
222
223 dBodyID b1 = dGeomGetBody(o1);
224
225
226 dBodyID b2 = dGeomGetBody(o2);
227
228
229 if (b1 && b2 && dAreConnected (b1,b2)) return;
230
231
232
233
234
235 dContact contact[32]; // up to 3 contacts per box
236
237
238 for (i=0; i<32; i++) {
239
240
241 contact[i].surface.mode = dContactBounce; //dContactMu2;
242
243
244 contact[i].surface.mu = dInfinity;
245
246
247 contact[i].surface.mu2 = 0;
248
249
250 contact[i].surface.bounce = 0.5;
251
252
253 contact[i].surface.bounce_vel = 0.1;
254
255
256 }
257
258
259 if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
260
261
262 dMatrix3 RI;
263
264
265 dRSetIdentity (RI);
266
267
268 const dReal ss[3] = {0.02,0.02,0.02};
269
270
271 for (i=0; i<numc; i++) {
272
273
274 if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
275
276
277 dMatrix3 Rotation;
278
279
280 dRSetIdentity(Rotation);
281
282
283 dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
284
285
286 continue;
287
288
289 }
290
291
292
293
294
295 dJointID c = dJointCreateContact (world,contactgroup,contact+i);
296
297
298 dJointAttach (c,b1,b2);
299
300
301 //dsDrawBox (contact[i].geom.pos,RI,ss);
302
303
304
305
306
307
308
309
310 }
311
312
313 }
314
315
316}
317
318
319
320
321
322// start simulation - set viewpoint
323
324
325
326
327
328static void start()
329
330
331{
332
333
334 static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
335
336
337 static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
338
339
340 dsSetViewpoint (xyz,hpr);
341
342
343 printf ("To drop another object, press:\n");
344
345
346 printf (" b for box.\n");
347
348
349 printf (" s for sphere.\n");
350
351
352 printf (" c for cylinder.\n");
353
354
355 printf (" x for a composite object.\n");
356
357
358 printf ("To select an object, press space.\n");
359
360
361 printf ("To disable the selected object, press d.\n");
362
363
364 printf ("To enable the selected object, press e.\n");
365
366
367}
368
369
370
371
372
373
374
375
376char locase (char c)
377
378
379{
380
381
382 if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
383
384
385 else return c;
386
387
388}
389
390
391
392
393
394
395
396
397// called when a key pressed
398
399
400
401
402
403static void command (int cmd)
404
405
406{
407
408
409 int i,j,k;
410
411
412 dReal sides[3];
413
414
415 dMass m;
416
417
418
419
420
421 cmd = locase (cmd);
422
423
424 if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x') {
425
426
427 if (num < NUM) {
428
429
430 i = num;
431
432
433 num++;
434
435
436 }
437
438
439 else {
440
441
442 i = nextobj;
443
444
445 nextobj++;
446
447
448 if (nextobj >= num) nextobj = 0;
449
450
451
452
453
454 // destroy the body and geoms for slot i
455
456
457 dBodyDestroy (obj[i].body);
458
459
460 for (k=0; k < GPB; k++) {
461
462
463 if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
464
465
466 }
467
468
469 memset (&obj[i],0,sizeof(obj[i]));
470
471
472 }
473
474
475
476
477
478 obj[i].body = dBodyCreate (world);
479
480
481 for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
482
483
484
485
486
487 dBodySetPosition (obj[i].body,
488
489
490 dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
491
492
493 dMatrix3 R;
494
495
496 dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
497
498
499 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
500
501
502 dBodySetRotation (obj[i].body,R);
503
504
505 dBodySetData (obj[i].body,(void*) i);
506
507
508
509
510
511 if (cmd == 'b') {
512
513
514 dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
515
516
517 obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
518
519
520 }
521
522
523 else if (cmd == 'c') {
524
525
526 sides[0] *= 0.5;
527
528
529 dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
530
531
532 obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
533
534
535 }
536
537
538 else if (cmd == 's') {
539
540
541 sides[0] *= 0.5;
542
543
544 dMassSetSphere (&m,DENSITY,sides[0]);
545
546
547 obj[i].geom[0] = dCreateSphere (space,sides[0]);
548
549
550 }
551
552
553 else if (cmd == 'x') {
554
555
556 dGeomID g2[GPB]; // encapsulated geometries
557
558
559 dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
560
561
562
563
564
565 // start accumulating masses for the encapsulated geometries
566
567
568 dMass m2;
569
570
571 dMassSetZero (&m);
572
573
574
575
576
577 // set random delta positions
578
579
580 for (j=0; j<GPB; j++) {
581
582
583 for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
584
585
586 }
587
588
589
590
591
592 for (k=0; k<3; k++) {
593
594
595 obj[i].geom[k] = dCreateGeomTransform (space);
596
597
598 dGeomTransformSetCleanup (obj[i].geom[k],1);
599
600
601 if (k==0) {
602
603
604 dReal radius = dRandReal()*0.25+0.05;
605
606
607 g2[k] = dCreateSphere (0,radius);
608
609
610 dMassSetSphere (&m2,DENSITY,radius);
611
612
613 }
614
615
616 else if (k==1) {
617
618
619 g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
620
621
622 dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
623
624
625 }
626
627
628 else {
629
630
631 dReal radius = dRandReal()*0.1+0.05;
632
633
634 dReal length = dRandReal()*1.0+0.1;
635
636
637 g2[k] = dCreateCCylinder (0,radius,length);
638
639
640 dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
641
642
643 }
644
645
646 dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
647
648
649
650
651
652 // set the transformation (adjust the mass too)
653
654
655 dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
656
657
658 dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
659
660
661 dMatrix3 Rtx;
662
663
664 dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
665
666
667 dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
668
669
670 dGeomSetRotation (g2[k],Rtx);
671
672
673 dMassRotate (&m2,Rtx);
674
675
676
677
678
679 // add to the total mass
680
681
682 dMassAdd (&m,&m2);
683
684
685 }
686
687
688
689
690
691 // move all encapsulated objects so that the center of mass is (0,0,0)
692
693
694 for (k=0; k<2; k++) {
695
696
697 dGeomSetPosition (g2[k],
698
699
700 dpos[k][0]-m.c[0],
701
702
703 dpos[k][1]-m.c[1],
704
705
706 dpos[k][2]-m.c[2]);
707
708
709 }
710
711
712 dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
713
714
715 }
716
717
718
719
720
721 for (k=0; k < GPB; k++) {
722
723
724 if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
725
726
727 }
728
729
730
731
732
733 dBodySetMass (obj[i].body,&m);
734
735
736 }
737
738
739
740
741
742 if (cmd == ' ') {
743
744
745 selected++;
746
747
748 if (selected >= num) selected = 0;
749
750
751 if (selected < 0) selected = 0;
752
753
754 }
755
756
757 else if (cmd == 'd' && selected >= 0 && selected < num) {
758
759
760 dBodyDisable (obj[selected].body);
761
762
763 }
764
765
766 else if (cmd == 'e' && selected >= 0 && selected < num) {
767
768
769 dBodyEnable (obj[selected].body);
770
771
772 }
773
774
775}
776
777
778
779
780
781
782
783
784// draw a geom
785
786
787
788
789
790void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
791
792
793{
794
795
796 if (!g) return;
797
798
799 if (!pos) pos = dGeomGetPosition (g);
800
801
802 if (!R) R = dGeomGetRotation (g);
803
804
805
806
807
808 int type = dGeomGetClass (g);
809
810
811 if (type == dBoxClass) {
812
813
814 dVector3 sides;
815
816
817 dGeomBoxGetLengths (g,sides);
818
819
820 dsDrawBox (pos,R,sides);
821
822
823 }
824
825
826 else if (type == dSphereClass) {
827
828
829 dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
830
831
832 }
833
834
835 else if (type == dCCylinderClass) {
836
837
838 dReal radius,length;
839
840
841 dGeomCCylinderGetParams (g,&radius,&length);
842
843
844 dsDrawCappedCylinder (pos,R,length,radius);
845
846
847 }
848
849
850 else if (type == dGeomTransformClass) {
851
852
853 dGeomID g2 = dGeomTransformGetGeom (g);
854
855
856 const dReal *pos2 = dGeomGetPosition (g2);
857
858
859 const dReal *R2 = dGeomGetRotation (g2);
860
861
862 dVector3 actual_pos;
863
864
865 dMatrix3 actual_R;
866
867
868 dMULTIPLY0_331 (actual_pos,R,pos2);
869
870
871 actual_pos[0] += pos[0];
872
873
874 actual_pos[1] += pos[1];
875
876
877 actual_pos[2] += pos[2];
878
879
880 dMULTIPLY0_333 (actual_R,R,R2);
881
882
883 drawGeom (g2,actual_pos,actual_R);
884
885
886 }
887
888
889}
890
891
892
893
894
895
896
897
898// simulation loop
899
900
901
902
903
904static void simLoop (int pause)
905
906
907{
908
909
910 dsSetColor (0,0,2);
911
912
913 dSpaceCollide (space,0,&nearCallback);
914
915
916 if (!pause) dWorldStep (world,0.05);
917
918
919
920
921
922 // remove all contact joints
923
924
925 dJointGroupEmpty (contactgroup);
926
927
928
929
930
931 dsSetColor (1,1,0);
932
933
934 dsSetTexture (DS_WOOD);
935
936
937 for (int i=0; i<num; i++) {
938
939
940 int color_changed = 0;
941
942
943 if (i==selected) {
944
945
946 dsSetColor (0,0.7,1);
947
948
949 color_changed = 1;
950
951
952 }
953
954
955 else if (! dBodyIsEnabled (obj[i].body)) {
956
957
958 dsSetColor (1,0,0);
959
960
961 color_changed = 1;
962
963
964 }
965
966
967 for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
968
969
970 if (color_changed) dsSetColor (1,1,0);
971
972
973 }
974
975
976
977
978
979 {for (int i = 0; i < RayCount; i++){
980
981
982 dVector3 Origin, Direction;
983
984
985 dGeomRayGet(Rays[i], Origin, Direction);
986
987
988
989
990
991 dReal Length = dGeomRayGetLength(Rays[i]);
992
993
994
995
996
997 dVector3 End;
998
999
1000 End[0] = Origin[0] + (Direction[0] * Length);
1001
1002
1003 End[1] = Origin[1] + (Direction[1] * Length);
1004
1005
1006 End[2] = Origin[2] + (Direction[2] * Length);
1007
1008
1009 End[3] = Origin[3] + (Direction[3] * Length);
1010
1011
1012
1013
1014
1015 dsDrawLine(Origin, End);
1016
1017
1018 }}
1019
1020
1021}
1022
1023
1024
1025
1026
1027
1028
1029
1030int main (int argc, char **argv)
1031
1032
1033{
1034
1035
1036 // setup pointers to drawstuff callback functions
1037
1038
1039 dsFunctions fn;
1040
1041
1042 fn.version = DS_VERSION;
1043
1044
1045 fn.start = &start;
1046
1047
1048 fn.step = &simLoop;
1049
1050
1051 fn.command = &command;
1052
1053
1054 fn.stop = 0;
1055
1056
1057 fn.path_to_textures = "../../drawstuff/textures";
1058
1059
1060 if(argc==2)
1061 {
1062 fn.path_to_textures = argv[1];
1063 }
1064
1065
1066
1067 // create world
1068
1069
1070
1071
1072
1073 world = dWorldCreate();
1074
1075
1076 space = dHashSpaceCreate();
1077
1078
1079 contactgroup = dJointGroupCreate (0);
1080
1081
1082 dWorldSetGravity (world,0,0,-0.5);
1083
1084
1085 dWorldSetCFM (world,1e-5);
1086
1087
1088 dCreatePlane (space,0,0,1,0);
1089
1090
1091 memset (obj,0,sizeof(obj));
1092
1093
1094
1095
1096
1097 dVector3 Origin, Direction;
1098
1099
1100
1101
1102
1103 RayCount = 5;
1104
1105
1106 Rays = new dGeomID[RayCount];
1107
1108
1109
1110
1111
1112 /* Ray 0 */
1113
1114
1115 Origin[0] = 1;
1116
1117
1118 Origin[1] = 1;
1119
1120
1121 Origin[2] = 1.5;
1122
1123
1124 Origin[3] = 0;
1125
1126
1127
1128
1129
1130 Direction[0] = 0.0f;
1131
1132
1133 Direction[1] = 0.0f;
1134
1135
1136 Direction[2] = -1;
1137
1138
1139 Direction[3] = 0;
1140
1141
1142
1143
1144
1145 dNormalize3(Direction);
1146
1147
1148
1149
1150
1151 Rays[0] = dGeomCreateRay(space, 5.0f);
1152
1153
1154 dGeomRaySet(Rays[0], Origin, Direction);
1155
1156
1157
1158
1159
1160 /* Ray 1 */
1161
1162
1163 Origin[0] = 0;
1164
1165
1166 Origin[1] = 10;
1167
1168
1169 Origin[2] = 0.25;
1170
1171
1172 Origin[3] = 0;
1173
1174
1175
1176
1177
1178 Direction[0] = 0.0f;
1179
1180
1181 Direction[1] = -1.0f;
1182
1183
1184 Direction[2] = 0.0f;
1185
1186
1187 Direction[3] = 0;
1188
1189
1190
1191
1192
1193 dNormalize3(Direction);
1194
1195
1196
1197
1198
1199 Rays[1] = dGeomCreateRay(space, 20.0f);
1200
1201
1202 dGeomRaySet(Rays[1], Origin, Direction);
1203
1204
1205
1206
1207
1208 /* Ray 2 */
1209
1210
1211 Origin[0] = -10;
1212
1213
1214 Origin[1] = 0;
1215
1216
1217 Origin[2] = 0.20;
1218
1219
1220 Origin[3] = 0;
1221
1222
1223
1224
1225
1226 Direction[0] = 1.0f;
1227
1228
1229 Direction[1] = 0.0f;
1230
1231
1232 Direction[2] = 0.0f;
1233
1234
1235 Direction[3] = 0;
1236
1237
1238
1239
1240
1241 dNormalize3(Direction);
1242
1243
1244
1245
1246
1247 Rays[2] = dGeomCreateRay(space, 20.0f);
1248
1249
1250 dGeomRaySet(Rays[2], Origin, Direction);
1251
1252
1253
1254
1255
1256 /* Ray 3 */
1257
1258
1259 Origin[0] = -9;
1260
1261
1262 Origin[1] = 11;
1263
1264
1265 Origin[2] = 0.15;
1266
1267
1268 Origin[3] = 0;
1269
1270
1271
1272
1273
1274 Direction[0] = 1.0f;
1275
1276
1277 Direction[1] = -1.0f;
1278
1279
1280 Direction[2] = 0.0f;
1281
1282
1283 Direction[3] = 0;
1284
1285
1286
1287
1288
1289 dNormalize3(Direction);
1290
1291
1292
1293
1294
1295 Rays[3] = dGeomCreateRay(space, 20.0f);
1296
1297
1298 dGeomRaySet(Rays[3], Origin, Direction);
1299
1300
1301
1302
1303
1304 /* Ray 4 */
1305
1306
1307 Origin[0] = -0.1;
1308
1309
1310 Origin[1] = 0.3;
1311
1312
1313 Origin[2] = 0.30;
1314
1315
1316 Origin[3] = 0;
1317
1318
1319
1320
1321
1322 Direction[0] = 0.3f;
1323
1324
1325 Direction[1] = 0.5f;
1326
1327
1328 Direction[2] = 1.0f;
1329
1330
1331 Direction[3] = 0;
1332
1333
1334
1335
1336
1337 Rays[4] = dGeomCreateRay(space, 5.0f);
1338
1339
1340 dGeomRaySet(Rays[4], Origin, Direction);
1341
1342
1343
1344
1345
1346 // run simulation
1347
1348
1349 dsSimulationLoop (argc,argv,352,288,&fn);
1350
1351
1352
1353
1354
1355 dJointGroupDestroy (contactgroup);
1356
1357
1358 dSpaceDestroy (space);
1359
1360
1361 dWorldDestroy (world);
1362
1363
1364
1365
1366
1367 return 0;
1368
1369
1370}
1371
1372
diff --git a/libraries/ode-0.9/contrib/dRay/dRay.cpp b/libraries/ode-0.9/contrib/dRay/dRay.cpp
deleted file mode 100644
index e3a426e..0000000
--- a/libraries/ode-0.9/contrib/dRay/dRay.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
1#include "Include\dRay.h"
2#include "dxRay.h"
3
4int dRayClass = -1;
5
6void dAABBRay(dxGeom* Ray, dReal AABB[6]){
7 dVector3 Start, End;
8 dGeomRayGet(Ray, Start, End);
9 dReal Length = dGeomRayGetLength(Ray);
10
11 End[0] = Start[0] + End[0] * Length;
12 End[1] = Start[1] + End[1] * Length;
13 End[2] = Start[2] + End[2] * Length;
14 End[3] = Start[3] + End[3] * Length;
15
16 if (Start[0] < End[0]){
17 AABB[0] = Start[0];
18 AABB[1] = End[0];
19 }
20 else{
21 AABB[0] = End[0];
22 AABB[1] = Start[0];
23 }
24
25 if (Start[1] < End[1]){
26 AABB[2] = Start[1];
27 AABB[3] = End[1];
28 }
29 else{
30 AABB[2] = End[1];
31 AABB[3] = Start[1];
32 }
33
34 if (Start[2] < End[2]){
35 AABB[4] = Start[2];
36 AABB[5] = End[2];
37 }
38 else{
39 AABB[4] = End[2];
40 AABB[5] = Start[2];
41 }
42 // Should we tweak the box to have a minimum size for axis aligned lines? How small should it be?
43}
44
45dColliderFn* dRayColliderFn(int num){
46 if (num == dPlaneClass) return (dColliderFn*)&dCollidePR;
47 if (num == dSphereClass) return (dColliderFn*)&dCollideSR;
48 if (num == dBoxClass) return (dColliderFn*)&dCollideBR;
49 if (num == dCCylinderClass) return (dColliderFn*)&dCollideCCR;
50 return 0;
51}
52
53dxGeom* dGeomCreateRay(dSpaceID space, dReal Length){
54 if (dRayClass == -1){
55 dGeomClass c;
56 c.bytes = sizeof(dxRay);
57 c.collider = &dRayColliderFn;
58 c.aabb = &dAABBRay;
59 c.aabb_test = 0;
60 c.dtor = 0;
61
62 dRayClass = dCreateGeomClass(&c);
63 }
64
65 dxGeom* g = dCreateGeom(dRayClass);
66 if (space) dSpaceAdd(space, g);
67
68 dGeomRaySetLength(g, Length);
69 return g;
70}
71
72void dGeomRaySetLength(dxGeom* g, dReal Length){
73 ((dxRay*)dGeomGetClassData(g))->Length = Length;
74}
75
76dReal dGeomRayGetLength(dxGeom* g){
77 return ((dxRay*)dGeomGetClassData(g))->Length;
78}
79
80void dGeomRaySet(dxGeom* g, dVector3 Origin, dVector3 Direction){
81 dGeomSetPosition(g, Origin[0], Origin[1], Origin[2]);
82
83 dVector3 Up, Right;
84 dPlaneSpace(Direction, Up, Right);
85
86 Origin[3] = Up[3] = Right[3] = REAL(0.0);
87
88 dMatrix3 Rotation;
89 Rotation[0 * 4 + 0] = Right[0];
90 Rotation[1 * 4 + 0] = Right[1];
91 Rotation[2 * 4 + 0] = Right[2];
92 Rotation[3 * 4 + 0] = Right[3];
93
94 Rotation[0 * 4 + 1] = Up[0];
95 Rotation[1 * 4 + 1] = Up[1];
96 Rotation[2 * 4 + 1] = Up[2];
97 Rotation[3 * 4 + 1] = Up[3];
98
99 Rotation[0 * 4 + 2] = Direction[0];
100 Rotation[1 * 4 + 2] = Direction[1];
101 Rotation[2 * 4 + 2] = Direction[2];
102 Rotation[3 * 4 + 2] = Direction[3];
103
104 dGeomSetRotation(g, Rotation);
105}
106
107void dGeomRayGet(dxGeom* g, dVector3 Origin, dVector3 Direction){
108 const dReal* Position = dGeomGetPosition(g);
109 Origin[0] = Position[0];
110 Origin[1] = Position[1];
111 Origin[2] = Position[2];
112 Origin[3] = Position[3];
113
114 const dReal* Rotation = dGeomGetRotation(g);
115 Direction[0] = Rotation[0 * 4 + 2];
116 Direction[1] = Rotation[1 * 4 + 2];
117 Direction[2] = Rotation[2 * 4 + 2];
118 Direction[3] = Rotation[3 * 4 + 2];
119} \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_Box.cpp b/libraries/ode-0.9/contrib/dRay/dRay_Box.cpp
deleted file mode 100644
index d2a0d9c..0000000
--- a/libraries/ode-0.9/contrib/dRay/dRay_Box.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
1// Ripped from Magic Software
2
3#include "Include\dRay.h"
4#include "dxRay.h"
5
6bool Clip(dReal Denom, dReal Numer, dReal& T0, dReal& T1){
7 // Return value is 'true' if line segment intersects the current test
8 // plane. Otherwise 'false' is returned in which case the line segment
9 // is entirely clipped.
10
11 if (Denom > REAL(0.0)){
12 if (Numer > Denom * T1){
13 return false;
14 }
15
16 if (Numer > Denom * T0){
17 T0 = Numer / Denom;
18 }
19 return true;
20 }
21 else if (Denom < REAL(0.0)){
22 if (Numer > Denom * T0){
23 return false;
24 }
25
26 if (Numer > Denom * T1){
27 T1 = Numer / Denom;
28 }
29 return true;
30 }
31 else return Numer <= REAL(0.0);
32}
33
34bool FindIntersection(const dVector3 Origin, const dVector3 Direction, const dVector3 Extents, dReal& T0, dReal& T1){
35 dReal SaveT0 = T0;
36 dReal SaveT1 = T1;
37
38 bool NotEntirelyClipped =
39 Clip(+Direction[0], -Origin[0] - Extents[0], T0, T1) &&
40 Clip(-Direction[0], +Origin[0] - Extents[0], T0, T1) &&
41 Clip(+Direction[1], -Origin[1] - Extents[1], T0, T1) &&
42 Clip(-Direction[1], +Origin[1] - Extents[1], T0, T1) &&
43 Clip(+Direction[2], -Origin[2] - Extents[2], T0, T1) &&
44 Clip(-Direction[2], +Origin[2] - Extents[2], T0, T1);
45
46 return NotEntirelyClipped && (T0 != SaveT0 || T1 != SaveT1);
47}
48
49int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
50 const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom);
51 const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom);
52 dVector3 Extents;
53 dGeomBoxGetLengths(BoxGeom, Extents);
54 Extents[0] /= 2;
55 Extents[1] /= 2;
56 Extents[2] /= 2;
57 Extents[3] /= 2;
58
59 dVector3 Origin, Direction;
60 dGeomRayGet(RayGeom, Origin, Direction);
61 dReal Length = dGeomRayGetLength(RayGeom);
62
63 dVector3 Diff;
64 Diff[0] = Origin[0] - Position[0];
65 Diff[1] = Origin[1] - Position[1];
66 Diff[2] = Origin[2] - Position[2];
67 Diff[3] = Origin[3] - Position[3];
68
69 Direction[0] *= Length;
70 Direction[1] *= Length;
71 Direction[2] *= Length;
72 Direction[3] *= Length;
73
74 dVector3 Rot[3];
75 Decompose(Rotation, Rot);
76
77 dVector3 TransOrigin;
78 TransOrigin[0] = dDOT(Diff, Rot[0]);
79 TransOrigin[1] = dDOT(Diff, Rot[1]);
80 TransOrigin[2] = dDOT(Diff, Rot[2]);
81 TransOrigin[3] = REAL(0.0);
82
83 dVector3 TransDirection;
84 TransDirection[0] = dDOT(Direction, Rot[0]);
85 TransDirection[1] = dDOT(Direction, Rot[1]);
86 TransDirection[2] = dDOT(Direction, Rot[2]);
87 TransDirection[3] = REAL(0.0);
88
89 dReal T[2];
90 T[0] = 0.0f;
91 T[1] = dInfinity;
92
93 bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]);
94
95 if (Intersect){
96 if (T[0] > REAL(0.0)){
97 dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
98 Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
99 Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
100 Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
101 Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
102 //Contact0->normal = 0;
103 Contact0->depth = 0.0f;
104 Contact0->g1 = RayGeom;
105 Contact0->g2 = BoxGeom;
106
107 dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
108 Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
109 Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
110 Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
111 Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
112 //Contact1->normal = 0;
113 Contact1->depth = 0.0f;
114 Contact1->g1 = RayGeom;
115 Contact1->g2 = BoxGeom;
116
117 return 2;
118 }
119 else{
120 dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
121 Contact->pos[0] = Origin[0] + T[1] * Direction[0];
122 Contact->pos[1] = Origin[1] + T[1] * Direction[1];
123 Contact->pos[2] = Origin[2] + T[1] * Direction[2];
124 Contact->pos[3] = Origin[3] + T[1] * Direction[3];
125 //Contact->normal = 0;
126 Contact->depth = 0.0f;
127 Contact->g1 = RayGeom;
128 Contact->g2 = BoxGeom;
129
130 return 1;
131 }
132 }
133 else return 0;
134} \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp b/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
deleted file mode 100644
index b9ea0c0..0000000
--- a/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
+++ /dev/null
@@ -1,199 +0,0 @@
1// Ripped from Magic Software
2
3#include "Include\dRay.h"
4#include "dxRay.h"
5
6int Find(const dVector3 Origin, dVector3 Direction, dReal Length, const dVector3 CCPos, const dMatrix3 CCRot, dReal CCRadius, dReal CCLength, dReal T[2]){
7 dVector3 U, V, W;
8 Decompose(CCRot, U, V, W);
9
10 dVector3 CCOrigin;
11 CCOrigin[0] = CCPos[0] - (W[0] * CCLength / 2);
12 CCOrigin[1] = CCPos[1] - (W[1] * CCLength / 2);
13 CCOrigin[2] = CCPos[2] - (W[2] * CCLength / 2);
14 CCOrigin[3] = CCPos[3] - (W[3] * CCLength / 2);
15
16 dVector3 D;
17 D[0] = dDOT(U, Direction);
18 D[1] = dDOT(V, Direction);
19 D[2] = dDOT(W, Direction);
20
21 dReal DMag = Length;
22 dReal InvDMag = REAL(1.0) / DMag;
23
24 dVector3 Diff;
25 Diff[0] = Origin[0] - CCOrigin[0];
26 Diff[1] = Origin[1] - CCOrigin[1];
27 Diff[2] = Origin[2] - CCOrigin[2];
28 Diff[3] = Origin[3] - CCOrigin[3];
29
30 dVector3 P;
31 P[0] = dDOT(U, Diff);
32 P[1] = dDOT(V, Diff);
33 P[2] = dDOT(W, Diff);
34
35 dReal CCRadiusSq = CCRadius * CCRadius;
36
37 dReal Epsilon = 1e-12f;
38
39 if (dFabs(D[2]) >= REAL(1.0) - Epsilon){ // line is parallel to capsule axis
40 dReal Discr = CCRadiusSq - P[0] * P[0] - P[1] * P[1];
41
42 if (Discr >= REAL(0.0)){
43 dReal Root = dSqrt(Discr);
44 T[0] = (-P[2] + Root) * InvDMag;
45 T[1] = (CCLength - P[2] + Root) * InvDMag;
46 return 2;
47 }
48 else return 0;
49 }
50
51 // test intersection with infinite cylinder
52 dReal A = D[0] * D[0] + D[1] * D[1];
53 dReal B = P[0] * D[0] + P[1] * D[1];
54 dReal C = P[0] * P[0] + P[1] * P[1] - CCRadiusSq;
55 dReal Discr = B * B - A * C;
56 if (Discr < REAL(0.0)){ // line does not intersect infinite cylinder
57 return 0;
58 }
59
60 int Count = 0;
61
62 if (Discr > REAL(0.0)){ // line intersects infinite cylinder in two places
63 dReal Root = dSqrt(Discr);
64 dReal Inv = REAL(1.0) / A;
65
66 dReal TTemp = (-B - Root) * Inv;
67
68 dReal Tmp = P[2] + TTemp * D[2];
69 if (REAL(0.0) <= Tmp && Tmp <= CCLength){
70 T[Count++] = TTemp * InvDMag;
71 }
72
73
74 TTemp = (-B + Root) * Inv;
75 Tmp = P[2] + TTemp * D[2];
76 if (REAL(0.0) <= Tmp && Tmp <= CCLength){
77 T[Count++] = TTemp * InvDMag;
78 }
79
80 if (Count == 2){ // line intersects capsule wall in two places
81 return 2;
82 }
83 }
84 else{ // line is tangent to infinite cylinder
85 dReal TTemp = -B / A;
86 dReal Tmp = P[2] + TTemp * D[2];
87 if (REAL(0.0) <= Tmp && Tmp <= CCLength){
88 T[0] = TTemp * InvDMag;
89 return 1;
90 }
91 }
92
93 // test intersection with bottom hemisphere
94 // fA = 1
95 B += P[2] * D[2];
96 C += P[2] * P[2];
97 Discr = B * B - C;
98 if (Discr > REAL(0.0)){
99 dReal Root = dSqrt(Discr);
100 dReal TTemp = -B - Root;
101 dReal Tmp = P[2] + TTemp * D[2];
102 if (Tmp <= REAL(0.0)){
103 T[Count++] = TTemp * InvDMag;
104 if (Count == 2){
105 return 2;
106 }
107 }
108
109 TTemp = -B + Root;
110 Tmp = P[2] + TTemp * D[2];
111 if (Tmp <= REAL(0.0)){
112 T[Count++] = TTemp * InvDMag;
113 if (Count == 2){
114 return 2;
115 }
116 }
117 }
118 else if (Discr == REAL(0.0)){
119 dReal TTemp = -B;
120 dReal Tmp = P[2] + TTemp * D[2];
121 if (Tmp <= REAL(0.0)){
122 T[Count++] = TTemp * InvDMag;
123 if (Count == 2){
124 return 2;
125 }
126 }
127 }
128
129 // test intersection with top hemisphere
130 // fA = 1
131 B -= D[2] * CCLength;
132 C += CCLength * (CCLength - REAL(2.0) * P[2]);
133
134 Discr = B * B - C;
135 if (Discr > REAL(0.0)){
136 dReal Root = dSqrt(Discr);
137 dReal TTemp = -B - Root;
138 dReal Tmp = P[2] + TTemp * D[2];
139 if (Tmp >= CCLength){
140
141 T[Count++] = TTemp * InvDMag;
142 if (Count == 2){
143 return 2;
144 }
145 }
146
147 TTemp = -B + Root;
148 Tmp = P[2] + TTemp * D[2];
149 if (Tmp >= CCLength){
150 T[Count++] = TTemp * InvDMag;
151 if (Count == 2){
152 return 2;
153 }
154 }
155 }
156 else if (Discr == REAL(0.0)){
157 dReal TTemp = -B;
158 dReal Tmp = P[2] + TTemp * D[2];
159 if (Tmp >= CCLength){
160 T[Count++] = TTemp * InvDMag;
161 if (Count == 2){
162 return 2;
163 }
164 }
165 }
166 return Count;
167}
168
169int dCollideCCR(dxGeom* RayGeom, dxGeom* CCGeom, int Flags, dContactGeom* Contacts, int Stride){
170 const dVector3& CCPos = *(const dVector3*)dGeomGetPosition(CCGeom);
171 const dMatrix3& CCRot = *(const dMatrix3*)dGeomGetRotation(CCGeom);
172
173 dReal CCRadius, CCLength;
174 dGeomCCylinderGetParams(CCGeom, &CCRadius, &CCLength);
175
176 dVector3 Origin, Direction;
177 dGeomRayGet(RayGeom, Origin, Direction);
178 dReal Length = dGeomRayGetLength(RayGeom);
179
180 dReal T[2];
181 int Count = Find(Origin, Direction, Length, CCPos, CCRot, CCRadius, CCLength, T);
182 int ContactCount = 0;
183 for (int i = 0; i < Count; i++){
184 if (T[i] >= 0.0){
185 dContactGeom* Contact = CONTACT(Flags, Contacts, ContactCount, Stride);
186 Contact->pos[0] = Origin[0] + T[i] * Direction[0] * Length;
187 Contact->pos[1] = Origin[1] + T[i] * Direction[1] * Length;
188 Contact->pos[2] = Origin[2] + T[i] * Direction[2] * Length;
189 Contact->pos[3] = Origin[3] + T[i] * Direction[3] * Length;
190 //Contact->normal = 0;
191 Contact->depth = 0.0f;
192 Contact->g1 = RayGeom;
193 Contact->g2 = CCGeom;
194
195 ContactCount++;
196 }
197 }
198 return ContactCount;
199} \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp b/libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp
deleted file mode 100644
index cf03c5b..0000000
--- a/libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
1// Ripped from Paul Bourke
2
3#include "Include\dRay.h"
4#include "dxRay.h"
5
6int dCollidePR(dxGeom* RayGeom, dxGeom* PlaneGeom, int Flags, dContactGeom* Contact, int Stride){
7 dVector3 Plane;
8 dGeomPlaneGetParams(PlaneGeom, Plane);
9
10 dVector3 Origin, Direction;
11 dGeomRayGet(RayGeom, Origin, Direction);
12
13 dReal Length = dGeomRayGetLength(RayGeom);
14
15 dReal Denom = Plane[0] * Direction[0] + Plane[1] * Direction[1] + Plane[2] * Direction[2];
16 if (dFabs(Denom) < 0.00001f){
17 return 0; // Ray never hits
18 }
19
20 float T = -(Plane[3] + Plane[0] * Origin[0] + Plane[1] * Origin[1] + Plane[2] * Origin[2]) / Denom;
21
22 if (T < 0 || T > Length){
23 return 0; // Ray hits but not within boundaries
24 }
25
26 Contact->pos[0] = Origin[0] + T * Direction[0];
27 Contact->pos[1] = Origin[1] + T * Direction[1];
28 Contact->pos[2] = Origin[2] + T * Direction[2];
29 Contact->pos[3] = REAL(0.0);
30 //Contact->normal = 0;
31 Contact->depth = 0.0f;
32 Contact->g1 = RayGeom;
33 Contact->g2 = PlaneGeom;
34 return 1;
35} \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp b/libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp
deleted file mode 100644
index 8e1ac39..0000000
--- a/libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
1// Ripped from Magic Software
2
3#include "Include\dRay.h"
4#include "dxRay.h"
5
6int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
7 const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
8 dReal Radius = dGeomSphereGetRadius(SphereGeom);
9
10 dVector3 Origin, Direction;
11 dGeomRayGet(RayGeom, Origin, Direction);
12 dReal Length = dGeomRayGetLength(RayGeom);
13
14 dVector3 Diff;
15 Diff[0] = Origin[0] - Position[0];
16 Diff[1] = Origin[1] - Position[1];
17 Diff[2] = Origin[2] - Position[2];
18 Diff[3] = Origin[3] - Position[3];
19
20 Direction[0] *= Length;
21 Direction[1] *= Length;
22 Direction[2] *= Length;
23 Direction[3] *= Length;
24
25 dReal A = Length * Length;
26 dReal B = dDOT(Diff, Direction);
27 dReal C = dDOT(Diff, Diff) - (Radius * Radius);
28
29 dReal Discr = B * B - A * C;
30 if (Discr < REAL(0.0)){
31 return 0;
32 }
33 else if (Discr > REAL(0.0)){
34 dReal T[2];
35 dReal Root = dSqrt(Discr);
36 dReal InvA = REAL(1.0) / A;
37 T[0] = (-B - Root) * InvA;
38 T[1] = (-B + Root) * InvA;
39
40 if (T[0] >= REAL(0.0)){
41 dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
42 Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
43 Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
44 Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
45 Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
46 //Contact0->normal = 0;
47 Contact0->depth = 0.0f;
48 Contact0->g1 = RayGeom;
49 Contact0->g2 = SphereGeom;
50
51 dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
52 Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
53 Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
54 Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
55 Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
56 //Contact1->normal = 0;
57 Contact1->depth = 0.0f;
58 Contact1->g1 = RayGeom;
59 Contact1->g2 = SphereGeom;
60
61 return 2;
62 }
63 else if (T[1] >= REAL(0.0)){
64 dContactGeom* Contact = CONTACT(Flags, Contacts, 1, Stride);
65 Contact->pos[0] = Origin[0] + T[1] * Direction[0];
66 Contact->pos[1] = Origin[1] + T[1] * Direction[1];
67 Contact->pos[2] = Origin[2] + T[1] * Direction[2];
68 Contact->pos[3] = Origin[3] + T[1] * Direction[3];
69 //Contact->normal = 0;
70 Contact->depth = 0.0f;
71 Contact->g1 = RayGeom;
72 Contact->g2 = SphereGeom;
73
74 return 1;
75 }
76 else return 0;
77 }
78 else{
79 dReal T;
80 T = -B / A;
81 if (T >= REAL(0.0)){
82 dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
83 Contact->pos[0] = Origin[0] + T * Direction[0];
84 Contact->pos[1] = Origin[1] + T * Direction[1];
85 Contact->pos[2] = Origin[2] + T * Direction[2];
86 Contact->pos[3] = Origin[3] + T * Direction[3];
87 //Contact->normal = 0;
88 Contact->depth = 0.0f;
89 Contact->g1 = RayGeom;
90 Contact->g2 = SphereGeom;
91 return 1;
92 }
93 else return 0;
94 }
95} \ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dxRay.h b/libraries/ode-0.9/contrib/dRay/dxRay.h
deleted file mode 100644
index 0fd1d2d..0000000
--- a/libraries/ode-0.9/contrib/dRay/dxRay.h
+++ /dev/null
@@ -1,32 +0,0 @@
1struct dxRay{
2 dReal Length;
3};
4
5inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
6 Right[0] = Matrix[0 * 4 + 0];
7 Right[1] = Matrix[1 * 4 + 0];
8 Right[2] = Matrix[2 * 4 + 0];
9 Right[3] = Matrix[3 * 4 + 0];
10 Up[0] = Matrix[0 * 4 + 1];
11 Up[1] = Matrix[1 * 4 + 1];
12 Up[2] = Matrix[2 * 4 + 1];
13 Up[3] = Matrix[3 * 4 + 1];
14 Direction[0] = Matrix[0 * 4 + 2];
15 Direction[1] = Matrix[1 * 4 + 2];
16 Direction[2] = Matrix[2 * 4 + 2];
17 Direction[3] = Matrix[3 * 4 + 2];
18}
19
20inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
21 Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
22}
23
24inline dContactGeom* CONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
25 dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff));
26 return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
27}
28
29int dCollidePR(dxGeom* RayGeom, dxGeom* PlaneGeom, int Flags, dContactGeom* Contacts, int Stride);
30int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride);
31int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride);
32int dCollideCCR(dxGeom* RayGeom, dxGeom* CCylinderGeom, int Flags, dContactGeom* Contacts, int Stride); \ No newline at end of file