From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- .../ode-0.9/contrib/BreakableJoints/README.txt | 110 + libraries/ode-0.9/contrib/BreakableJoints/common.h | 337 +++ .../contrib/BreakableJoints/diff/common.h.diff | 21 + .../contrib/BreakableJoints/diff/joint.cpp.diff | 148 ++ .../contrib/BreakableJoints/diff/joint.h.diff | 18 + .../contrib/BreakableJoints/diff/objects.h.diff | 13 + .../contrib/BreakableJoints/diff/ode.cpp.diff | 28 + .../contrib/BreakableJoints/diff/step.cpp.diff | 130 + .../contrib/BreakableJoints/diff/stepfast.cpp.diff | 143 + .../BreakableJoints/diff/test_buggy.cpp.diff | 16 + .../ode-0.9/contrib/BreakableJoints/joint.cpp | 2803 ++++++++++++++++++++ libraries/ode-0.9/contrib/BreakableJoints/joint.h | 282 ++ .../ode-0.9/contrib/BreakableJoints/objects.h | 252 ++ libraries/ode-0.9/contrib/BreakableJoints/ode.cpp | 1404 ++++++++++ libraries/ode-0.9/contrib/BreakableJoints/step.cpp | 1170 ++++++++ .../ode-0.9/contrib/BreakableJoints/stepfast.cpp | 1212 +++++++++ .../contrib/BreakableJoints/test_breakable.cpp | 416 +++ .../ode-0.9/contrib/BreakableJoints/test_buggy.cpp | 327 +++ .../ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp | 58 + libraries/ode-0.9/contrib/DotNetManaged/Body.cpp | 322 +++ libraries/ode-0.9/contrib/DotNetManaged/Body.h | 76 + .../ode-0.9/contrib/DotNetManaged/CommonMgd.h | 43 + .../contrib/DotNetManaged/DotNetManaged.sln | 21 + .../contrib/DotNetManaged/DotNetManaged.vcproj | 379 +++ libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp | 219 ++ libraries/ode-0.9/contrib/DotNetManaged/Geom.h | 75 + libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp | 35 + libraries/ode-0.9/contrib/DotNetManaged/Joint.h | 21 + .../ode-0.9/contrib/DotNetManaged/JointAMotor.cpp | 162 ++ .../ode-0.9/contrib/DotNetManaged/JointAMotor.h | 62 + .../ode-0.9/contrib/DotNetManaged/JointBall.cpp | 79 + .../ode-0.9/contrib/DotNetManaged/JointBall.h | 38 + .../ode-0.9/contrib/DotNetManaged/JointFixed.cpp | 67 + .../ode-0.9/contrib/DotNetManaged/JointFixed.h | 37 + .../ode-0.9/contrib/DotNetManaged/JointGroup.cpp | 53 + .../ode-0.9/contrib/DotNetManaged/JointGroup.h | 33 + .../ode-0.9/contrib/DotNetManaged/JointHinge.cpp | 121 + .../ode-0.9/contrib/DotNetManaged/JointHinge.h | 195 ++ .../ode-0.9/contrib/DotNetManaged/JointHinge2.cpp | 133 + .../ode-0.9/contrib/DotNetManaged/JointHinge2.h | 48 + .../ode-0.9/contrib/DotNetManaged/JointSlider.cpp | 102 + .../ode-0.9/contrib/DotNetManaged/JointSlider.h | 158 ++ .../ode-0.9/contrib/DotNetManaged/Release/ode.dll | Bin 0 -> 196608 bytes libraries/ode-0.9/contrib/DotNetManaged/Space.cpp | 53 + libraries/ode-0.9/contrib/DotNetManaged/Space.h | 33 + libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp | 5 + libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h | 12 + libraries/ode-0.9/contrib/DotNetManaged/TEST.h | 17 + libraries/ode-0.9/contrib/DotNetManaged/World.cpp | 74 + libraries/ode-0.9/contrib/DotNetManaged/World.h | 67 + .../GeomTransformGroup/GeomTransformGroup.cpp | 218 ++ .../GeomTransformGroup/GeomTransformGroup.h | 29 + .../ode-0.9/contrib/GeomTransformGroup/README.txt | 148 ++ .../contrib/Mac_CFMCarbon/CW7_projects.sit.bin | Bin 0 -> 46592 bytes libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt | 95 + .../Mac_CFMCarbon/mac_source/CommonPrefix.h | 6 + .../contrib/Mac_CFMCarbon/mac_source/DSPrefix.h | 6 + .../contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h | 10 + .../Mac_CFMCarbon/mac_source/ExamplesPrefix.h | 13 + .../Mac_CFMCarbon/mac_source/ODETestPrefix.h | 13 + .../Mac_CFMCarbon/mac_source/ReleasePrefix.h | 6 + .../mac_source/drawstuff/src/mac_glut_carbon.cpp | 281 ++ .../Mac_CFMCarbon/mac_source/include/GL/gl.h | 2 + .../Mac_CFMCarbon/mac_source/include/GL/glu.h | 2 + .../Mac_CFMCarbon/mac_source/include/ode/config.h | 52 + .../mac_source/ode/test/test_stability1.cpp | 289 ++ .../mac_source/ode/test/test_stacktest.c | 197 ++ .../contrib/Ode.NET/Drawstuff/AssemblyInfo.cs | 18 + .../ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs | 58 + .../ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua | 19 + .../ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs | 18 + libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs | 1732 ++++++++++++ libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua | 31 + libraries/ode-0.9/contrib/Ode.NET/README.TXT | 73 + .../ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs | 260 ++ .../ode-0.9/contrib/Ode.NET/Tests/premake.lua | 27 + libraries/ode-0.9/contrib/Ode.NET/premake.lua | 29 + .../contrib/OdeModelProcessor/LICENSE-BSD.TXT | 37 + .../ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT | 502 ++++ .../OdeModelProcessor/OdeModelProcessor.sln | 20 + .../OdeModelProcessor/OdeModelProcessor.cs | 354 +++ .../OdeModelProcessor/OdeModelProcessor.csproj | 69 + .../OdeModelProcessor/Properties/AssemblyInfo.cs | 35 + .../Properties/Settings.Designer.cs | 26 + .../OdeModelProcessor/Properties/Settings.settings | 6 + .../ode-0.9/contrib/OdeModelProcessor/README.TXT | 78 + libraries/ode-0.9/contrib/README | 19 + .../TerrainAndCone/collision_std_internal.h | 100 + libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp | 504 ++++ .../ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp | 662 +++++ .../ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp | 659 +++++ .../ode-0.9/contrib/TerrainAndCone/readme.txt | 322 +++ .../contrib/TerrainAndCone/test_boxstackb.cpp | 1375 ++++++++++ libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp | 1445 ++++++++++ libraries/ode-0.9/contrib/dCylinder/dCylinder.h | 14 + libraries/ode-0.9/contrib/dCylinder/readme.txt | 62 + libraries/ode-0.9/contrib/dRay/Include/dRay.h | 15 + libraries/ode-0.9/contrib/dRay/README.txt | 16 + libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp | 1372 ++++++++++ libraries/ode-0.9/contrib/dRay/dRay.cpp | 119 + libraries/ode-0.9/contrib/dRay/dRay_Box.cpp | 134 + libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp | 199 ++ libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp | 35 + libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp | 95 + libraries/ode-0.9/contrib/dRay/dxRay.h | 32 + 105 files changed, 23566 insertions(+) create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/README.txt create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/common.h create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/joint.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/joint.h create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/objects.h create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/ode.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/step.cpp create mode 100755 libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp create mode 100644 libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Body.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Body.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Geom.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Joint.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointBall.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h create mode 100755 libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Space.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Space.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/TEST.h create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/World.cpp create mode 100644 libraries/ode-0.9/contrib/DotNetManaged/World.h create mode 100644 libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp create mode 100644 libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h create mode 100644 libraries/ode-0.9/contrib/GeomTransformGroup/README.txt create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp create mode 100644 libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua create mode 100644 libraries/ode-0.9/contrib/Ode.NET/README.TXT create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs create mode 100644 libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua create mode 100644 libraries/ode-0.9/contrib/Ode.NET/premake.lua create mode 100644 libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT create mode 100644 libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT create mode 100644 libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln create mode 100644 libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs create mode 100644 libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj create mode 100644 libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs create mode 100755 libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs create mode 100755 libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings create mode 100644 libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT create mode 100644 libraries/ode-0.9/contrib/README create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/readme.txt create mode 100644 libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp create mode 100644 libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp create mode 100644 libraries/ode-0.9/contrib/dCylinder/dCylinder.h create mode 100644 libraries/ode-0.9/contrib/dCylinder/readme.txt create mode 100644 libraries/ode-0.9/contrib/dRay/Include/dRay.h create mode 100644 libraries/ode-0.9/contrib/dRay/README.txt create mode 100644 libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp create mode 100644 libraries/ode-0.9/contrib/dRay/dRay.cpp create mode 100644 libraries/ode-0.9/contrib/dRay/dRay_Box.cpp create mode 100644 libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp create mode 100644 libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp create mode 100644 libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp create mode 100644 libraries/ode-0.9/contrib/dRay/dxRay.h (limited to 'libraries/ode-0.9/contrib') diff --git a/libraries/ode-0.9/contrib/BreakableJoints/README.txt b/libraries/ode-0.9/contrib/BreakableJoints/README.txt new file mode 100644 index 0000000..e996816 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/README.txt @@ -0,0 +1,110 @@ +Breakable Joints + +================================================================================ + +Description: +This is a small addition to ODE that makes joints breakable. Breakable means +that if a force on a joint is to high it wil break. I have included a modified +version of test_buggy.cpp (test_breakable.cpp) so you can see it for your self. +Just drive your buggy into an obstacle and enjoy! + +================================================================================ + +Installation instructions: +- copy joint.h, joint.cpp, ode.cpp and step.cpp to the ode/src/ directory +- copy common.h and object.h to the include/ directory +- copy test_breakable.cpp to the ode/test/ directory +- add test_breakable.cpp to the ODE_TEST_SRC_CPP object in the makefile. +- make ode-lib +- make ode-test +You can also use the diffs. The above files will quickly go out of sync with the +rest of ODE but the diffs wil remain valid longer. + +================================================================================ + +Functions: +dJointSetBreakable (dJointID joint, int b) + If b is 1 the joint is made breakable. If b is 0 the joint is made + unbreakable. + +void dJointSetBreakCallback (dJointID joint, dJointBreakCallback *callbackFunc) + Sets the callback function for this joint. If a funtion is set it will be + called if the joint is broken but before it is actually detached or deleted. + +void dJointSetBreakMode (dJointID joint, int mode) + Use this functions to set some flags. These flags can be ORred ( | ) + together; ie. dJointSetBreakMode (someJoint, +dJOINT_BREAK_AT_B1_FORCE|dJOINT_DELETE_ON_BREAK) + dJOINT_DELETE_ON_BREAK - If the joint breaks it wil be deleted. + dJOINT_BREAK_AT_B1_FORCE - If the force on body 1 is to high the joint will + break + dJOINT_BREAK_AT_B1_TORQUE - If the torque on body 1 is to high the joint will + break + dJOINT_BREAK_AT_B2_FORCE - If the force on body 2 is to high the joint will + break + dJOINT_BREAK_AT_B2_TORQUE - If the torque on body 2 is to high the joint will + break + +void dJointSetBreakForce (dJointID joint, int body, dReal x, dReal y, dReal z) + With this function you can set the maximum force for a body connected to this + joint. A value of 0 for body means body 1, 1 means body 2. The force is + relative to the bodies rotation. + +void dJointSetBreakTorque (dJointID joint, int body, dReal x, dReal y, dReal z) + With this function you can set the maximum torque for a body connected to this + joint. A value of 0 for body means body 1, 1 means body 2. The torque is + relative to the bodies rotation. + +int dJointIsBreakable (dJointID joint) + Returns 1 if this joint is breakable, 0 otherwise. + +int dJointGetBreakMode (dJointID joint) + Returns the breakmode flag. + +void dJointGetBreakForce (dJointID joint, int body, dReal *force) + Returns the force at what this joint will break. A value of 0 for body means + body 1, 1 means body 2. force must have enough space for 3 dReal values. + +void dJointGetBreakTorque (dJointID joint, int body, dReal *torque) + Returns the torque at what this joint will break. A value of 0 for body + means body 1, 1 means body 2. force must have enough space for 3 dReal + values. + +================================================================================ + +The callback function is defined like this (in common.h): +void dJointBreakCallback (dJointID); + +================================================================================ + +Problems, known bugs & other issues: +- If the timestep is very small then joints get a lot weaker. They can even fall + apart! +- I have tested all this with the latest checkout from CVS (at the time of + writing ofcourse). I haven't tested it with earlier versions of ODE. +- I have modified the code that fills the jointfeedback struct. I haven't tested + if it still works. +- I'm not sure if the forces are really relative to the connected bodies. +- There are some memory leaks in the test_breakable.cpp example. + +================================================================================ + +Bugfixes and changes: +09/08/2003 +- I fixed a bug when there where 0 joints in the simulation + +06/12/2003 +- dJointGetBreakMode() added, by vadim_mcagon@hotmail.com + +11/03/2004 +- Updated files to work with latest CVS checkout. +- Added support for dWorldStepFast1() +- Added separate test_breakable.cpp example. +- Updated the code that breaks and destroys a joint. + +================================================================================ + +Send me an e-mail if you have any suggestions, ideas, bugs, bug-fixes, anything! +e-mail: roelvandijk@home.nl + +Roel van Dijk - 11/03/2004 diff --git a/libraries/ode-0.9/contrib/BreakableJoints/common.h b/libraries/ode-0.9/contrib/BreakableJoints/common.h new file mode 100644 index 0000000..bc4272d --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/common.h @@ -0,0 +1,337 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_COMMON_H_ +#define _ODE_COMMON_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +/* configuration stuff */ + +/* the efficient alignment. most platforms align data structures to some + * number of bytes, but this is not always the most efficient alignment. + * for example, many x86 compilers align to 4 bytes, but on a pentium it + * is important to align doubles to 8 byte boundaries (for speed), and + * the 4 floats in a SIMD register to 16 byte boundaries. many other + * platforms have similar behavior. setting a larger alignment can waste + * a (very) small amount of memory. NOTE: this number must be a power of + * two. this is set to 16 by default. + */ +#define EFFICIENT_ALIGNMENT 16 + + +/* constants */ + +/* pi and 1/sqrt(2) are defined here if necessary because they don't get + * defined in on some platforms (like MS-Windows) + */ + +#ifndef M_PI +#define M_PI REAL(3.1415926535897932384626433832795029) +#endif +#ifndef M_SQRT1_2 +#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490) +#endif + + +/* debugging: + * IASSERT is an internal assertion, i.e. a consistency check. if it fails + * we want to know where. + * UASSERT is a user assertion, i.e. if it fails a nice error message + * should be printed for the user. + * AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)" + * is printed. + * DEBUGMSG just prints out a message + */ + +#ifndef dNODEBUG +#ifdef __GNUC__ +#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__); +#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ + msg " in %s()", __FUNCTION__); +#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ + msg " in %s()", __FUNCTION__); +#else +#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ + "assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__); +#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ + msg " (%s:%d)", __FILE__,__LINE__); +#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ + msg " (%s:%d)", __FILE__,__LINE__); +#endif +#else +#define dIASSERT(a) ; +#define dUASSERT(a,msg) ; +#define dDEBUGMSG(msg) ; +#endif +#define dAASSERT(a) dUASSERT(a,"Bad argument(s)") + +/* floating point data type, vector, matrix and quaternion types */ + +#if defined(dSINGLE) +typedef float dReal; +#elif defined(dDOUBLE) +typedef double dReal; +#else +#error You must #define dSINGLE or dDOUBLE +#endif + + +/* round an integer up to a multiple of 4, except that 0 and 1 are unmodified + * (used to compute matrix leading dimensions) + */ +#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a)) + +/* these types are mainly just used in headers */ +typedef dReal dVector3[4]; +typedef dReal dVector4[4]; +typedef dReal dMatrix3[4*3]; +typedef dReal dMatrix4[4*4]; +typedef dReal dMatrix6[8*6]; +typedef dReal dQuaternion[4]; + + +/* precision dependent scalar math functions */ + +#if defined(dSINGLE) + +#define REAL(x) (x ## f) /* form a constant */ +#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ +#define dSqrt(x) ((float)sqrt(x)) /* square root */ +#define dRecipSqrt(x) ((float)(1.0f/sqrt(x))) /* reciprocal square root */ +#define dSin(x) ((float)sin(x)) /* sine */ +#define dCos(x) ((float)cos(x)) /* cosine */ +#define dFabs(x) ((float)fabs(x)) /* absolute value */ +#define dAtan2(y,x) ((float)atan2((y),(x))) /* arc tangent with 2 args */ + +#elif defined(dDOUBLE) + +#define REAL(x) (x) +#define dRecip(x) (1.0/(x)) +#define dSqrt(x) sqrt(x) +#define dRecipSqrt(x) (1.0/sqrt(x)) +#define dSin(x) sin(x) +#define dCos(x) cos(x) +#define dFabs(x) fabs(x) +#define dAtan2(y,x) atan2((y),(x)) + +#else +#error You must #define dSINGLE or dDOUBLE +#endif + + +/* utility */ + + +/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */ + +#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) + + +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ + +#define dALLOCA16(n) \ + ((char*)dEFFICIENT_SIZE(((int)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) + + +/* internal object types (all prefixed with `dx') */ + +struct dxWorld; /* dynamics world */ +struct dxSpace; /* collision space */ +struct dxBody; /* rigid body (dynamics object) */ +struct dxGeom; /* geometry (collision object) */ +struct dxJoint; +struct dxJointNode; +struct dxJointGroup; + +typedef struct dxWorld *dWorldID; +typedef struct dxSpace *dSpaceID; +typedef struct dxBody *dBodyID; +typedef struct dxGeom *dGeomID; +typedef struct dxJoint *dJointID; +typedef struct dxJointGroup *dJointGroupID; + + +/* error numbers */ + +enum { + d_ERR_UNKNOWN = 0, /* unknown error */ + d_ERR_IASSERT, /* internal assertion failed */ + d_ERR_UASSERT, /* user assertion failed */ + d_ERR_LCP /* user assertion failed */ +}; + + +/* joint type numbers */ + +enum { + dJointTypeNone = 0, /* or "unknown" */ + dJointTypeBall, + dJointTypeHinge, + dJointTypeSlider, + dJointTypeContact, + dJointTypeUniversal, + dJointTypeHinge2, + dJointTypeFixed, + dJointTypeNull, + dJointTypeAMotor +}; + +/******************** breakable joint contribution ***********************/ +/* joint break callback function */ +typedef void dJointBreakCallback (dJointID joint); + +/* joint break modes */ +enum { + // if this flag is set, the joint wil break + dJOINT_BROKEN = 0x0001, + // if this flag is set, the joint wil be deleted when it breaks + dJOINT_DELETE_ON_BREAK = 0x0002, + // if this flag is set, the joint can break at a certain force on body 1 + dJOINT_BREAK_AT_B1_FORCE = 0x0004, + // if this flag is set, the joint can break at a certain torque on body 1 + dJOINT_BREAK_AT_B1_TORQUE = 0x0008, + // if this flag is set, the joint can break at a certain force on body 2 + dJOINT_BREAK_AT_B2_FORCE = 0x0010, + // if this flag is set, the joint can break at a certain torque on body 2 + dJOINT_BREAK_AT_B2_TORQUE = 0x0020 +}; +/*************************************************************************/ + +/* an alternative way of setting joint parameters, using joint parameter + * structures and member constants. we don't actually do this yet. + */ + +/* +typedef struct dLimot { + int mode; + dReal lostop, histop; + dReal vel, fmax; + dReal fudge_factor; + dReal bounce, soft; + dReal suspension_erp, suspension_cfm; +} dLimot; + +enum { + dLimotLoStop = 0x0001, + dLimotHiStop = 0x0002, + dLimotVel = 0x0004, + dLimotFMax = 0x0008, + dLimotFudgeFactor = 0x0010, + dLimotBounce = 0x0020, + dLimotSoft = 0x0040 +}; +*/ + + +/* standard joint parameter names. why are these here? - because we don't want + * to include all the joint function definitions in joint.cpp. hmmmm. + * MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second argument, + * which is why we have the D_ALL_PARAM_NAMES macro as well. please copy and + * paste between these two. + */ + +#define D_ALL_PARAM_NAMES(start) \ + /* parameters for limits and motors */ \ + dParamLoStop = start, \ + dParamHiStop, \ + dParamVel, \ + dParamFMax, \ + dParamFudgeFactor, \ + dParamBounce, \ + dParamCFM, \ + dParamStopERP, \ + dParamStopCFM, \ + /* parameters for suspension */ \ + dParamSuspensionERP, \ + dParamSuspensionCFM, + +#define D_ALL_PARAM_NAMES_X(start,x) \ + /* parameters for limits and motors */ \ + dParamLoStop ## x = start, \ + dParamHiStop ## x, \ + dParamVel ## x, \ + dParamFMax ## x, \ + dParamFudgeFactor ## x, \ + dParamBounce ## x, \ + dParamCFM ## x, \ + dParamStopERP ## x, \ + dParamStopCFM ## x, \ + /* parameters for suspension */ \ + dParamSuspensionERP ## x, \ + dParamSuspensionCFM ## x, + +enum { + D_ALL_PARAM_NAMES(0) + D_ALL_PARAM_NAMES_X(0x100,2) + D_ALL_PARAM_NAMES_X(0x200,3) + + /* add a multiple of this constant to the basic parameter numbers to get + * the parameters for the second, third etc axes. + */ + dParamGroup=0x100 +}; + + +/* angular motor mode numbers */ + +enum{ + dAMotorUser = 0, + dAMotorEuler = 1 +}; + + +/* joint force feedback information */ + +typedef struct dJointFeedback { + dVector3 f1; /* force applied to body 1 */ + dVector3 t1; /* torque applied to body 1 */ + dVector3 f2; /* force applied to body 2 */ + dVector3 t2; /* torque applied to body 2 */ +} dJointFeedback; + + +/* private functions that must be implemented by the collision library: + * (1) indicate that a geom has moved, (2) get the next geom in a body list. + * these functions are called whenever the position of geoms connected to a + * body have changed, e.g. with dBodySetPosition(), dBodySetRotation(), or + * when the ODE step function updates the body state. + */ + +void dGeomMoved (dGeomID); +dGeomID dGeomGetBodyNext (dGeomID); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff new file mode 100644 index 0000000..24415a1 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff @@ -0,0 +1,21 @@ +208,227d207 +< /******************** breakable joint contribution ***********************/ +< /* joint break callback function */ +< typedef void dJointBreakCallback (dJointID joint); +< +< /* joint break modes */ +< enum { +< // if this flag is set, the joint wil break +< dJOINT_BROKEN = 0x0001, +< // if this flag is set, the joint wil be deleted when it breaks +< dJOINT_DELETE_ON_BREAK = 0x0002, +< // if this flag is set, the joint can break at a certain force on body 1 +< dJOINT_BREAK_AT_B1_FORCE = 0x0004, +< // if this flag is set, the joint can break at a certain torque on body 1 +< dJOINT_BREAK_AT_B1_TORQUE = 0x0008, +< // if this flag is set, the joint can break at a certain force on body 2 +< dJOINT_BREAK_AT_B2_FORCE = 0x0010, +< // if this flag is set, the joint can break at a certain torque on body 2 +< dJOINT_BREAK_AT_B2_TORQUE = 0x0020 +< }; +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff new file mode 100644 index 0000000..80397f0 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff @@ -0,0 +1,148 @@ +2659,2804d2658 +< +< /******************** breakable joint contribution ***********************/ +< extern "C" void dJointSetBreakable (dxJoint *joint, int b) { +< dAASSERT(joint); +< if (b) { +< // we want this joint to be breakable but we must first check if it +< // was already breakable +< if (!joint->breakInfo) { +< // allocate a dxJointBreakInfo struct +< joint->breakInfo = new dxJointBreakInfo; +< joint->breakInfo->flags = 0; +< for (int i = 0; i < 3; i++) { +< joint->breakInfo->b1MaxF[0] = 0; +< joint->breakInfo->b1MaxT[0] = 0; +< joint->breakInfo->b2MaxF[0] = 0; +< joint->breakInfo->b2MaxT[0] = 0; +< } +< joint->breakInfo->callback = 0; +< } +< else { +< // the joint was already breakable +< return; +< } +< } +< else { +< // we want this joint to be unbreakable mut we must first check if +< // it is alreay unbreakable +< if (joint->breakInfo) { +< // deallocate the dxJointBreakInfo struct +< delete joint->breakInfo; +< joint->breakInfo = 0; +< } +< else { +< // the joint was already unbreakable +< return; +< } +< } +< } +< +< extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakCallback called on unbreakable joint"); +< } +< # endif +< joint->breakInfo->callback = callbackFunc; +< } +< +< extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakMode called on unbreakable joint"); +< } +< # endif +< joint->breakInfo->flags = mode; +< } +< +< extern "C" int dJointGetBreakMode (dxJoint *joint) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointGetBreakMode called on unbreakable joint"); +< } +< # endif +< return joint->breakInfo->flags; +< } +< +< extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakForce called on unbreakable joint"); +< } +< # endif +< if (body) { +< joint->breakInfo->b2MaxF[0] = x; +< joint->breakInfo->b2MaxF[1] = y; +< joint->breakInfo->b2MaxF[2] = z; +< } +< else { +< joint->breakInfo->b1MaxF[0] = x; +< joint->breakInfo->b1MaxF[1] = y; +< joint->breakInfo->b1MaxF[2] = z; +< } +< } +< +< extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointSetBreakTorque called on unbreakable joint"); +< } +< # endif +< if (body) { +< joint->breakInfo->b2MaxT[0] = x; +< joint->breakInfo->b2MaxT[1] = y; +< joint->breakInfo->b2MaxT[2] = z; +< } +< else { +< joint->breakInfo->b1MaxT[0] = x; +< joint->breakInfo->b1MaxT[1] = y; +< joint->breakInfo->b1MaxT[2] = z; +< } +< } +< +< extern "C" int dJointIsBreakable (dxJoint *joint) { +< dAASSERT(joint); +< return joint->breakInfo != 0; +< } +< +< extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointGetBreakForce called on unbreakable joint"); +< } +< # endif +< if (body) +< for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i]; +< else +< for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i]; +< } +< +< extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) { +< dAASSERT(joint); +< # ifndef dNODEBUG +< // only works for a breakable joint +< if (!joint->breakInfo) { +< dDebug (0, "dJointGetBreakTorque called on unbreakable joint"); +< } +< # endif +< if (body) +< for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i]; +< else +< for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i]; +< } +< /*************************************************************************/ +< +\ No newline at end of file diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff new file mode 100644 index 0000000..eed3c24 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff @@ -0,0 +1,18 @@ +61,70d60 +< /******************** breakable joint contribution ***********************/ +< struct dxJointBreakInfo : public dBase { +< int flags; +< dReal b1MaxF[3]; // maximum force on body 1 +< dReal b1MaxT[3]; // maximum torque on body 1 +< dReal b2MaxF[3]; // maximum force on body 2 +< dReal b2MaxT[3]; // maximum torque on body 2 +< dJointBreakCallback *callback; // function that is called when this joint breaks +< }; +< /*************************************************************************/ +135,140d124 +< +< /******************** breakable joint contribution ***********************/ +< // optional break info structure. if this is not NULL the the joint is +< // breakable. +< dxJointBreakInfo *breakInfo; +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff new file mode 100644 index 0000000..fd2129e --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff @@ -0,0 +1,13 @@ +168,179d167 +< /******************** breakable joint contribution ***********************/ +< void dJointSetBreakable (dJointID, int b); +< void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc); +< void dJointSetBreakMode (dJointID, int mode); +< int dJointGetBreakMode (dJointID); +< void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z); +< void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z); +< int dJointIsBreakable (dJointID); +< void dJointGetBreakForce (dJointID, int body, dReal *force); +< void dJointGetBreakTorque (dJointID, int body, dReal *torque); +< /*************************************************************************/ +< diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff new file mode 100644 index 0000000..761b7be --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff @@ -0,0 +1,28 @@ +212,230d211 +< /******************** breakable joint contribution ***********************/ +< dxJoint* nextJ; +< if (!world->firstjoint) +< nextJ = 0; +< else +< nextJ = (dxJoint*)world->firstjoint->next; +< for (j=world->firstjoint; j; j=nextJ) { +< nextJ = (dxJoint*)j->next; +< // check if joint is breakable and broken +< if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { +< // detach (break) the joint +< dJointAttach (j, 0, 0); +< // call the callback function if it is set +< if (j->breakInfo->callback) j->breakInfo->callback (j); +< // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set +< if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); +< } +< } +< /*************************************************************************/ +931,933d911 +< /******************** breakable joint contribution ***********************/ +< j->breakInfo = 0; +< /*************************************************************************/ +1011,1013d988 +< /******************** breakable joint contribution ***********************/ +< if (j->breakInfo) delete j->breakInfo; +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff new file mode 100644 index 0000000..dfc8c2f --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff @@ -0,0 +1,130 @@ +966,1066c966,989 +< /******************** breakable joint contribution ***********************/ +< // this saves us a few dereferences +< dxJointBreakInfo *jBI = joint[i]->breakInfo; +< // we need joint feedback if the joint is breakable or if the user +< // requested feedback. +< if (jBI||fb) { +< // we need feedback on the amount of force that this joint is +< // applying to the bodies. we use a slightly slower computation +< // that splits out the force components and puts them in the +< // feedback structure. +< dJointFeedback temp_fb; // temporary storage for joint feedback +< dReal data1[8],data2[8]; +< Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); +< dReal *cf1 = cforce + 8*b1->tag; +< cf1[0] += (temp_fb.f1[0] = data1[0]); +< cf1[1] += (temp_fb.f1[1] = data1[1]); +< cf1[2] += (temp_fb.f1[2] = data1[2]); +< cf1[4] += (temp_fb.t1[0] = data1[4]); +< cf1[5] += (temp_fb.t1[1] = data1[5]); +< cf1[6] += (temp_fb.t1[2] = data1[6]); +< if (b2) { +< Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); +< dReal *cf2 = cforce + 8*b2->tag; +< cf2[0] += (temp_fb.f2[0] = data2[0]); +< cf2[1] += (temp_fb.f2[1] = data2[1]); +< cf2[2] += (temp_fb.f2[2] = data2[2]); +< cf2[4] += (temp_fb.t2[0] = data2[4]); +< cf2[5] += (temp_fb.t2[1] = data2[5]); +< cf2[6] += (temp_fb.t2[2] = data2[6]); +< } +< // if the user requested so we must copy the feedback information to +< // the feedback struct that the user suplied. +< if (fb) { +< // copy temp_fb to fb +< fb->f1[0] = temp_fb.f1[0]; +< fb->f1[1] = temp_fb.f1[1]; +< fb->f1[2] = temp_fb.f1[2]; +< fb->t1[0] = temp_fb.t1[0]; +< fb->t1[1] = temp_fb.t1[1]; +< fb->t1[2] = temp_fb.t1[2]; +< if (b2) { +< fb->f2[0] = temp_fb.f2[0]; +< fb->f2[1] = temp_fb.f2[1]; +< fb->f2[2] = temp_fb.f2[2]; +< fb->t2[0] = temp_fb.t2[0]; +< fb->t2[1] = temp_fb.t2[1]; +< fb->t2[2] = temp_fb.t2[2]; +< } +< } +< // if the joint is breakable we need to check the breaking conditions +< if (jBI) { +< dReal relCF1[3]; +< dReal relCT1[3]; +< // multiply the force and torque vectors by the rotation matrix of body 1 +< dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]); +< dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF1[i] > jBI->b1MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT1[i] > jBI->b1MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (b2) { +< dReal relCF2[3]; +< dReal relCT2[3]; +< // multiply the force and torque vectors by the rotation matrix of body 2 +< dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]); +< dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF2[i] > jBI->b2MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT2[i] > jBI->b2MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< } +< doneCheckingBreaks: +< ; +--- +> if (fb) { +> // the user has requested feedback on the amount of force that this +> // joint is applying to the bodies. we use a slightly slower +> // computation that splits out the force components and puts them +> // in the feedback structure. +> dReal data1[8],data2[8]; +> Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); +> dReal *cf1 = cforce + 8*b1->tag; +> cf1[0] += (fb->f1[0] = data1[0]); +> cf1[1] += (fb->f1[1] = data1[1]); +> cf1[2] += (fb->f1[2] = data1[2]); +> cf1[4] += (fb->t1[0] = data1[4]); +> cf1[5] += (fb->t1[1] = data1[5]); +> cf1[6] += (fb->t1[2] = data1[6]); +> if (b2){ +> Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); +> dReal *cf2 = cforce + 8*b2->tag; +> cf2[0] += (fb->f2[0] = data2[0]); +> cf2[1] += (fb->f2[1] = data2[1]); +> cf2[2] += (fb->f2[2] = data2[2]); +> cf2[4] += (fb->t2[0] = data2[4]); +> cf2[5] += (fb->t2[1] = data2[5]); +> cf2[6] += (fb->t2[2] = data2[6]); +> } +1068,1069d990 +< } +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff new file mode 100644 index 0000000..ed64cba --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff @@ -0,0 +1,143 @@ +587,598c587,593 +< /******************** breakable joint contribution ***********************/ +< // this saves us a few dereferences +< dxJointBreakInfo *jBI = joint->breakInfo; +< // we need joint feedback if the joint is breakable or if the user +< // requested feedback. +< if (jBI||fb) { +< // we need feedback on the amount of force that this joint is +< // applying to the bodies. we use a slightly slower computation +< // that splits out the force components and puts them in the +< // feedback structure. +< dJointFeedback temp_fb; // temporary storage for joint feedback +< dReal data1[8],data2[8]; +--- +> if (fb) +> { +> // the user has requested feedback on the amount of force that this +> // joint is applying to the bodies. we use a slightly slower +> // computation that splits out the force components and puts them +> // in the feedback structure. +> dReal data1[8], data2[8]; +603,608c598,603 +< cf1[0] = (temp_fb.f1[0] = data1[0]); +< cf1[1] = (temp_fb.f1[1] = data1[1]); +< cf1[2] = (temp_fb.f1[2] = data1[2]); +< cf1[4] = (temp_fb.t1[0] = data1[4]); +< cf1[5] = (temp_fb.t1[1] = data1[5]); +< cf1[6] = (temp_fb.t1[2] = data1[6]); +--- +> cf1[0] = (fb->f1[0] = data1[0]); +> cf1[1] = (fb->f1[1] = data1[1]); +> cf1[2] = (fb->f1[2] = data1[2]); +> cf1[4] = (fb->t1[0] = data1[4]); +> cf1[5] = (fb->t1[1] = data1[5]); +> cf1[6] = (fb->t1[2] = data1[6]); +614,691c609,614 +< cf2[0] = (temp_fb.f2[0] = data2[0]); +< cf2[1] = (temp_fb.f2[1] = data2[1]); +< cf2[2] = (temp_fb.f2[2] = data2[2]); +< cf2[4] = (temp_fb.t2[0] = data2[4]); +< cf2[5] = (temp_fb.t2[1] = data2[5]); +< cf2[6] = (temp_fb.t2[2] = data2[6]); +< } +< // if the user requested so we must copy the feedback information to +< // the feedback struct that the user suplied. +< if (fb) { +< // copy temp_fb to fb +< fb->f1[0] = temp_fb.f1[0]; +< fb->f1[1] = temp_fb.f1[1]; +< fb->f1[2] = temp_fb.f1[2]; +< fb->t1[0] = temp_fb.t1[0]; +< fb->t1[1] = temp_fb.t1[1]; +< fb->t1[2] = temp_fb.t1[2]; +< if (body[1]) { +< fb->f2[0] = temp_fb.f2[0]; +< fb->f2[1] = temp_fb.f2[1]; +< fb->f2[2] = temp_fb.f2[2]; +< fb->t2[0] = temp_fb.t2[0]; +< fb->t2[1] = temp_fb.t2[1]; +< fb->t2[2] = temp_fb.t2[2]; +< } +< } +< // if the joint is breakable we need to check the breaking conditions +< if (jBI) { +< dReal relCF1[3]; +< dReal relCT1[3]; +< // multiply the force and torque vectors by the rotation matrix of body 1 +< dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]); +< dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF1[i] > jBI->b1MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT1[i] > jBI->b1MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (body[1]) { +< dReal relCF2[3]; +< dReal relCT2[3]; +< // multiply the force and torque vectors by the rotation matrix of body 2 +< dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]); +< dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]); +< if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { +< // check if the force is to high +< for (int i = 0; i < 3; i++) { +< if (relCF2[i] > jBI->b2MaxF[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { +< // check if the torque is to high +< for (int i = 0; i < 3; i++) { +< if (relCT2[i] > jBI->b2MaxT[i]) { +< jBI->flags |= dJOINT_BROKEN; +< goto doneCheckingBreaks; +< } +< } +< } +< } +< doneCheckingBreaks: +< ; +--- +> cf2[0] = (fb->f2[0] = data2[0]); +> cf2[1] = (fb->f2[1] = data2[1]); +> cf2[2] = (fb->f2[2] = data2[2]); +> cf2[4] = (fb->t2[0] = data2[4]); +> cf2[5] = (fb->t2[1] = data2[5]); +> cf2[6] = (fb->t2[2] = data2[6]); +694d616 +< /*************************************************************************/ +1178,1196d1099 +< /******************** breakable joint contribution ***********************/ +< dxJoint* nextJ; +< if (!world->firstjoint) +< nextJ = 0; +< else +< nextJ = (dxJoint*)world->firstjoint->next; +< for (j=world->firstjoint; j; j=nextJ) { +< nextJ = (dxJoint*)j->next; +< // check if joint is breakable and broken +< if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { +< // detach (break) the joint +< dJointAttach (j, 0, 0); +< // call the callback function if it is set +< if (j->breakInfo->callback) j->breakInfo->callback (j); +< // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set +< if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); +< } +< } +< /*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff new file mode 100644 index 0000000..65770da --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff @@ -0,0 +1,16 @@ +266,270d265 +< +< // breakable joints contribution +< dJointSetBreakable (joint[i], 1); +< dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_FORCE); +< dJointSetBreakForce (joint[i], 0.5); +298c293 +< ground_box = dCreateBox (space,2,1.5,5); +--- +> ground_box = dCreateBox (space,2,1.5,1); +300,301c295,296 +< dRFromAxisAndAngle (R,0,1,0,-0.85); +< dGeomSetPosition (ground_box,5,0,-1); +--- +> dRFromAxisAndAngle (R,0,1,0,-0.15); +> dGeomSetPosition (ground_box,2,0,-0.34); diff --git a/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp b/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp new file mode 100644 index 0000000..2c724f8 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp @@ -0,0 +1,2803 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +design note: the general principle for giving a joint the option of connecting +to the static environment (i.e. the absolute frame) is to check the second +body (joint->node[1].body), and if it is zero then behave as if its body +transform is the identity. + +*/ + +#include +#include +#include +#include "joint.h" + +//**************************************************************************** +// externs + +extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); +extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); + +//**************************************************************************** +// utility + +// set three "ball-and-socket" rows in the constraint equation, and the +// corresponding right hand side. + +static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int s = info->rowskip; + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dCROSSMAT (info->J1a,a1,s,-,+); + if (joint->node[1].body) { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dCROSSMAT (info->J2a,a2,s,+,-); + } + + // set right hand side + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) { + info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] - + a1[j] - joint->node[0].body->pos[j]); + } + } + else { + for (int j=0; j<3; j++) { + info->c[j] = k * (anchor2[j] - a1[j] - + joint->node[0].body->pos[j]); + } + } +} + + +// this is like setBall(), except that `axis' is a unit length vector +// (in global coordinates) that should be used for the first jacobian +// position row (the other two row vectors will be derived from this). +// `erp1' is the erp value to use along the axis. + +static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, + dVector3 anchor1, dVector3 anchor2, + dVector3 axis, dReal erp1) +{ + // anchor points in global coordinates with respect to body PORs. + dVector3 a1,a2; + + int i,s = info->rowskip; + + // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], + // [0 1 0] and [0 0 1], which makes everything much easier. + dVector3 q1,q2; + dPlaneSpace (axis,q1,q2); + + // set jacobian + for (i=0; i<3; i++) info->J1l[i] = axis[i]; + for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; + for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; + dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dCROSS (info->J1a,=,a1,axis); + dCROSS (info->J1a+s,=,a1,q1); + dCROSS (info->J1a+2*s,=,a1,q2); + if (joint->node[1].body) { + for (i=0; i<3; i++) info->J2l[i] = -axis[i]; + for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; + for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; + dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dCROSS (info->J2a,= -,a2,axis); + dCROSS (info->J2a+s,= -,a2,q1); + dCROSS (info->J2a+2*s,= -,a2,q2); + } + + // set right hand side - measure error along (axis,q1,q2) + dReal k1 = info->fps * erp1; + dReal k = info->fps * info->erp; + + for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i]; + if (joint->node[1].body) { + for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i]; + info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); + } + else { + info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); + info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); + info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); + } +} + + +// set three orientation rows in the constraint equation, and the +// corresponding right hand side. + +static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) +{ + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->J1a[start_index] = 1; + info->J1a[start_index + s + 1] = 1; + info->J1a[start_index + s*2+2] = 1; + if (joint->node[1].body) { + info->J2a[start_index] = -1; + info->J2a[start_index + s+1] = -1; + info->J2a[start_index + s*2+2] = -1; + } + + // compute the right hand side. the first three elements will result in + // relative angular velocity of the two bodies - this is set to bring them + // back into alignment. the correcting angular velocity is + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * u + // = (erp*fps) * theta * u + // where rotation along unit length axis u by theta brings body 2's frame + // to qrel with respect to body 1's frame. using a small angle approximation + // for sin(), this gives + // angular_velocity = (erp*fps) * 2 * v + // where the quaternion of the relative rotation between the two bodies is + // q = [cos(theta/2) sin(theta/2)*u] = [s v] + + // get qerr = relative rotation (rotation error) between two bodies + dQuaternion qerr,e; + if (joint->node[1].body) { + dQuaternion qq; + dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); + dQMultiply2 (qerr,qq,qrel); + } + else { + dQMultiply3 (qerr,joint->node[0].body->q,qrel); + } + if (qerr[0] < 0) { + qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small + qerr[2] = -qerr[2]; + qerr[3] = -qerr[3]; + } + dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! + dReal k = info->fps * info->erp; + info->c[start_row] = 2*k * e[0]; + info->c[start_row+1] = 2*k * e[1]; + info->c[start_row+2] = 2*k * e[2]; +} + + +// compute anchor points relative to bodies + +static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 anchor1, dVector3 anchor2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x - j->node[0].body->pos[0]; + q[1] = y - j->node[0].body->pos[1]; + q[2] = z - j->node[0].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); + if (j->node[1].body) { + q[0] = x - j->node[1].body->pos[0]; + q[1] = y - j->node[1].body->pos[1]; + q[2] = z - j->node[1].body->pos[2]; + q[3] = 0; + dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); + } + else { + anchor2[0] = x; + anchor2[1] = y; + anchor2[2] = z; + } + } + anchor1[3] = 0; + anchor2[3] = 0; +} + + +// compute axes relative to bodies. either axis1 or axis2 can be 0. + +static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, + dVector3 axis1, dVector3 axis2) +{ + if (j->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + if (axis1) { + dMULTIPLY1_331 (axis1,j->node[0].body->R,q); + axis1[3] = 0; + } + if (axis2) { + if (j->node[1].body) { + dMULTIPLY1_331 (axis2,j->node[1].body->R,q); + } + else { + axis2[0] = x; + axis2[1] = y; + axis2[2] = z; + } + axis2[3] = 0; + } + } +} + + +static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->R,anchor1); + result[0] += j->node[0].body->pos[0]; + result[1] += j->node[0].body->pos[1]; + result[2] += j->node[0].body->pos[2]; + } +} + + +static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->R,anchor2); + result[0] += j->node[1].body->pos[0]; + result[1] += j->node[1].body->pos[1]; + result[2] += j->node[1].body->pos[2]; + } + else { + result[0] = anchor2[0]; + result[1] = anchor2[1]; + result[2] = anchor2[2]; + } +} + + +static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) +{ + if (j->node[0].body) { + dMULTIPLY0_331 (result,j->node[0].body->R,axis1); + } +} + + +static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) +{ + if (j->node[1].body) { + dMULTIPLY0_331 (result,j->node[1].body->R,axis2); + } + else { + result[0] = axis2[0]; + result[1] = axis2[1]; + result[2] = axis2[2]; + } +} + + +static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) +{ + // the angle between the two bodies is extracted from the quaternion that + // represents the relative rotation between them. recall that a quaternion + // q is: + // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] + // where s is a scalar and v is a 3-vector. u is a unit length axis and + // theta is a rotation along that axis. we can get theta/2 by: + // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) + // but we can't get sin(theta/2) directly, only its absolute value, i.e.: + // |v| = |sin(theta/2)| * |u| + // = |sin(theta/2)| + // using this value will have a strange effect. recall that there are two + // quaternion representations of a given rotation, q and -q. typically as + // a body rotates along the axis it will go through a complete cycle using + // one representation and then the next cycle will use the other + // representation. this corresponds to u pointing in the direction of the + // hinge axis and then in the opposite direction. the result is that theta + // will appear to go "backwards" every other cycle. here is a fix: if u + // points "away" from the direction of the hinge (motor) axis (i.e. more + // than 90 degrees) then use -q instead of q. this represents the same + // rotation, but results in the cos(theta/2) value being sign inverted. + + // extract the angle from the quaternion. cost2 = cos(theta/2), + // sint2 = |sin(theta/2)| + dReal cost2 = qrel[0]; + dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); + dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions + (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis + (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction + + // the angle we get will be between 0..2*pi, but we want to return angles + // between -pi..pi + if (theta > M_PI) theta -= 2*M_PI; + + // the angle we've just extracted has the wrong sign + theta = -theta; + + return theta; +} + + +// given two bodies (body1,body2), the hinge axis that they are connected by +// w.r.t. body1 (axis), and the initial relative orientation between them +// (q_initial), return the relative rotation angle. the initial relative +// orientation corresponds to an angle of zero. if body2 is 0 then measure the +// angle between body1 and the static frame. +// +// this will not return the correct angle if the bodies rotate along any axis +// other than the given hinge axis. + +static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, + dQuaternion q_initial) +{ + // get qrel = relative rotation between the two bodies + dQuaternion qrel; + if (body2) { + dQuaternion qq; + dQMultiply1 (qq,body1->q,body2->q); + dQMultiply2 (qrel,qq,q_initial); + } + else { + // pretend body2->q is the identity + dQMultiply3 (qrel,body1->q,q_initial); + } + + return getHingeAngleFromRelativeQuat (qrel,axis); +} + +//**************************************************************************** +// dxJointLimitMotor + +void dxJointLimitMotor::init (dxWorld *world) +{ + vel = 0; + fmax = 0; + lostop = -dInfinity; + histop = dInfinity; + fudge_factor = 1; + normal_cfm = world->global_cfm; + stop_erp = world->global_erp; + stop_cfm = world->global_cfm; + bounce = 0; + limit = 0; + limit_err = 0; +} + + +void dxJointLimitMotor::set (int num, dReal value) +{ + switch (num) { + case dParamLoStop: + if (value <= histop) lostop = value; + break; + case dParamHiStop: + if (value >= lostop) histop = value; + break; + case dParamVel: + vel = value; + break; + case dParamFMax: + if (value >= 0) fmax = value; + break; + case dParamFudgeFactor: + if (value >= 0 && value <= 1) fudge_factor = value; + break; + case dParamBounce: + bounce = value; + break; + case dParamCFM: + normal_cfm = value; + break; + case dParamStopERP: + stop_erp = value; + break; + case dParamStopCFM: + stop_cfm = value; + break; + } +} + + +dReal dxJointLimitMotor::get (int num) +{ + switch (num) { + case dParamLoStop: return lostop; + case dParamHiStop: return histop; + case dParamVel: return vel; + case dParamFMax: return fmax; + case dParamFudgeFactor: return fudge_factor; + case dParamBounce: return bounce; + case dParamCFM: return normal_cfm; + case dParamStopERP: return stop_erp; + case dParamStopCFM: return stop_cfm; + default: return 0; + } +} + + +int dxJointLimitMotor::testRotationalLimit (dReal angle) +{ + if (angle <= lostop) { + limit = 1; + limit_err = angle - lostop; + return 1; + } + else if (angle >= histop) { + limit = 2; + limit_err = angle - histop; + return 1; + } + else { + limit = 0; + return 0; + } +} + + +int dxJointLimitMotor::addLimot (dxJoint *joint, + dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational) +{ + int srow = row * info->rowskip; + + // if the joint is powered, or has joint limits, add in the extra row + int powered = fmax > 0; + if (powered || limit) { + dReal *J1 = rotational ? info->J1a : info->J1l; + dReal *J2 = rotational ? info->J2a : info->J2l; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if (joint->node[1].body) { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), we have to be careful + // that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (!rotational && joint->node[1].body) { + dVector3 c; + c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]); + c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]); + c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]); + dCROSS (ltd,=,c,ax1); + info->J1a[srow+0] = ltd[0]; + info->J1a[srow+1] = ltd[1]; + info->J1a[srow+2] = ltd[2]; + info->J2a[srow+0] = ltd[0]; + info->J2a[srow+1] = ltd[1]; + info->J2a[srow+2] = ltd[2]; + } + + // if we're limited low and high simultaneously, the joint motor is + // ineffective + if (limit && (lostop == histop)) powered = 0; + + if (powered) { + info->cfm[row] = normal_cfm; + if (! limit) { + info->c[row] = vel; + info->lo[row] = -fmax; + info->hi[row] = fmax; + } + else { + // the joint is at a limit, AND is being powered. if the joint is + // being powered into the limit then we apply the maximum motor force + // in that direction, because the motor is working against the + // immovable limit. if the joint is being powered away from the limit + // then we have problems because actually we need *two* lcp + // constraints to handle this case. so we fake it and apply some + // fraction of the maximum force. the fraction to use can be set as + // a fudge factor. + + dReal fm = fmax; + if (vel > 0) fm = -fm; + + // if we're powering away from the limit, apply the fudge factor + if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; + + if (rotational) { + dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1], + -fm*ax1[2]); + if (joint->node[1].body) + dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + } + else { + dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]); + if (joint->node[1].body) { + dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); + + // linear limot torque decoupling step: refer to above discussion + dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], + -fm*ltd[2]); + } + } + } + } + + if (limit) { + dReal k = info->fps * stop_erp; + info->c[row] = -k * limit_err; + info->cfm[row] = stop_cfm; + + if (lostop == histop) { + // limited low and high simultaneously + info->lo[row] = -dInfinity; + info->hi[row] = dInfinity; + } + else { + if (limit == 1) { + // low limit + info->lo[row] = 0; + info->hi[row] = dInfinity; + } + else { + // high limit + info->lo[row] = -dInfinity; + info->hi[row] = 0; + } + + // deal with bounce + if (bounce > 0) { + // calculate joint velocity + dReal vel; + if (rotational) { + vel = dDOT(joint->node[0].body->avel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->avel,ax1); + } + else { + vel = dDOT(joint->node[0].body->lvel,ax1); + if (joint->node[1].body) + vel -= dDOT(joint->node[1].body->lvel,ax1); + } + + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if (limit == 1) { + // low limit + if (vel < 0) { + dReal newc = -bounce * vel; + if (newc > info->c[row]) info->c[row] = newc; + } + } + else { + // high limit - all those computations are reversed + if (vel > 0) { + dReal newc = -bounce * vel; + if (newc < info->c[row]) info->c[row] = newc; + } + } + } + } + } + return 1; + } + else return 0; +} + +//**************************************************************************** +// ball and socket + +static void ballInit (dxJointBall *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); +} + + +static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info) +{ + info->m = 3; + info->nub = 3; +} + + +static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) +{ + setBall (joint,info,joint->anchor1,joint->anchor2); +} + + +extern "C" void dJointSetBallAnchor (dxJointBall *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); +} + + +extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetBallAnchor2 (dxJointBall *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +dxJoint::Vtable __dball_vtable = { + sizeof(dxJointBall), + (dxJoint::init_fn*) ballInit, + (dxJoint::getInfo1_fn*) ballGetInfo1, + (dxJoint::getInfo2_fn*) ballGetInfo2, + dJointTypeBall}; + +//**************************************************************************** +// hinge + +static void hingeInit (dxJointHinge *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[0] = 1; + dSetZero (j->qrel,4); + j->limot.init (j->world); +} + + +static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered hinge needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && + j->limot.lostop <= j->limot.histop) { + dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, + j->qrel); + if (j->limot.testRotationalLimit (angle)) info->m = 6; + } +} + + +static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) +{ + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + // set the two hinge rows. the hinge axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the hinge axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the hinge axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body + dVector3 p,q; // plane space vectors for ax1 + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dPlaneSpace (ax1,p,q); + + int s3=3*info->rowskip; + int s4=4*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + info->J1a[s4+0] = q[0]; + info->J1a[s4+1] = q[1]; + info->J1a[s4+2] = q[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + info->J2a[s4+0] = -q[0]; + info->J2a[s4+1] = -q[1]; + info->J2a[s4+2] = -q[2]; + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the hinge back into alignment. + // if ax1,ax2 are the unit length hinge axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if `theta' is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + + dVector3 ax2,b; + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } + dCROSS (b,=,ax1,ax2); + dReal k = info->fps * info->erp; + info->c[3] = k * dDOT(b,p); + info->c[4] = k * dDOT(b,q); + + // if the hinge is powered, or has joint limits, add in the stuff + joint->limot.addLimot (joint,info,5,ax1,1); +} + + +// compute initial relative rotation body1 -> body2, or env -> body1 + +static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) +{ + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + } + else { + // set joint->qrel to the transpose of the first body q + joint->qrel[0] = joint->node[0].body->q[0]; + for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + } + } +} + + +extern "C" void dJointSetHingeAnchor (dxJointHinge *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + hingeComputeInitialRelativeRotation (joint); +} + + +extern "C" void dJointSetHingeAxis (dxJointHinge *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + setAxes (joint,x,y,z,joint->axis1,joint->axis2); + hingeComputeInitialRelativeRotation (joint); +} + + +extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetHingeAnchor2 (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointSetHingeParam (dxJointHinge *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + joint->limot.set (parameter,value); +} + + +extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + return joint->limot.get (parameter); +} + + +extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); + if (joint->node[0].body) { + dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, + joint->qrel); + if (joint->flags & dJOINT_REVERSE) + return -ang; + else + return ang; + } + else return 0; +} + + +extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + if (joint->flags & dJOINT_REVERSE) rate = - rate; + return rate; + } + else return 0; +} + + +extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque) +{ + dVector3 axis; + dAASSERT(joint); + dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); + + if (joint->flags & dJOINT_REVERSE) + torque = -torque; + + getAxis (joint,axis,joint->axis1); + axis[0] *= torque; + axis[1] *= torque; + axis[2] *= torque; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dhinge_vtable = { + sizeof(dxJointHinge), + (dxJoint::init_fn*) hingeInit, + (dxJoint::getInfo1_fn*) hingeGetInfo1, + (dxJoint::getInfo2_fn*) hingeGetInfo2, + dJointTypeHinge}; + +//**************************************************************************** +// slider + +static void sliderInit (dxJointSlider *j) +{ + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->qrel,4); + dSetZero (j->offset,4); + j->limot.init (j->world); +} + + +extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1,q; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + // get body2 + offset point in global coordinates + dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset); + for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] - + joint->node[1].body->pos[i]; + } + else { + for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - + joint->offset[i]; + + } + return dDOT(ax1,q); +} + + +extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + // get axis1 in global coordinates + dVector3 ax1; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + return dDOT(ax1,joint->node[0].body->lvel) - + dDOT(ax1,joint->node[1].body->lvel); + } + else { + return dDOT(ax1,joint->node[0].body->lvel); + } +} + + +static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) +{ + info->nub = 5; + + // see if joint is powered + if (j->limot.fmax > 0) + info->m = 6; // powered slider needs an extra constraint row + else info->m = 5; + + // see if we're at a joint limit. + j->limot.limit = 0; + if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && + j->limot.lostop <= j->limot.histop) { + // measure joint position + dReal pos = dJointGetSliderPosition (j); + if (pos <= j->limot.lostop) { + j->limot.limit = 1; + j->limot.limit_err = pos - j->limot.lostop; + info->m = 6; + } + else if (pos >= j->limot.histop) { + j->limot.limit = 2; + j->limot.limit_err = pos - j->limot.histop; + info->m = 6; + } + } +} + + +static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) +{ + int i,s = info->rowskip; + int s2=2*s,s3=3*s,s4=4*s; + + // pull out pos and R for both bodies. also get the `connection' + // vector pos2-pos1. + + dReal *pos1,*pos2,*R1,*R2; + dVector3 c; + pos1 = joint->node[0].body->pos; + R1 = joint->node[0].body->R; + if (joint->node[1].body) { + pos2 = joint->node[1].body->pos; + R2 = joint->node[1].body->R; + for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; + } + else { + pos2 = 0; + R2 = 0; + } + + // 3 rows to make body rotations equal + setFixedOrientation(joint, info, joint->qrel, 0); + + // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. + + dVector3 ax1; // joint axis in global coordinates (unit length) + dVector3 p,q; // plane space of ax1 + dMULTIPLY0_331 (ax1,R1,joint->axis1); + dPlaneSpace (ax1,p,q); + if (joint->node[1].body) { + dVector3 tmp; + dCROSS (tmp, = REAL(0.5) * ,c,p); + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + dCROSS (tmp, = REAL(0.5) * ,c,q); + for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; + for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; + for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; + } + for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; + for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; + + // compute last two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + dVector3 ofs; // offset point in global coordinates + dMULTIPLY0_331 (ofs,R2,joint->offset); + for (i=0; i<3; i++) c[i] += ofs[i]; + info->c[3] = k * dDOT(p,c); + info->c[4] = k * dDOT(q,c); + } + else { + dVector3 ofs; // offset point in global coordinates + for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; + info->c[3] = k * dDOT(p,ofs); + info->c[4] = k * dDOT(q,ofs); + } + + // if the slider is powered, or has joint limits, add in the extra row + joint->limot.addLimot (joint,info,5,ax1,0); +} + + +extern "C" void dJointSetSliderAxis (dxJointSlider *joint, + dReal x, dReal y, dReal z) +{ + int i; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + setAxes (joint,x,y,z,joint->axis1,0); + + // compute initial relative rotation body1 -> body2, or env -> body1 + // also compute center of body1 w.r.t body 2 + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dVector3 c; + for (i=0; i<3; i++) + c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i]; + } +} + + +extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointSetSliderParam (dxJointSlider *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + joint->limot.set (parameter,value); +} + + +extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + return joint->limot.get (parameter); +} + + +extern "C" void dJointAddSliderForce (dxJointSlider *joint, dReal force) +{ + dVector3 axis; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); + + if (joint->flags & dJOINT_REVERSE) + force -= force; + + getAxis (joint,axis,joint->axis1); + axis[0] *= force; + axis[1] *= force; + axis[2] *= force; + + if (joint->node[0].body != 0) + dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); + if (joint->node[1].body != 0) + dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); +} + + +dxJoint::Vtable __dslider_vtable = { + sizeof(dxJointSlider), + (dxJoint::init_fn*) sliderInit, + (dxJoint::getInfo1_fn*) sliderGetInfo1, + (dxJoint::getInfo2_fn*) sliderGetInfo2, + dJointTypeSlider}; + +//**************************************************************************** +// contact + +static void contactInit (dxJointContact *j) +{ + // default frictionless contact. hmmm, this info gets overwritten straight + // away anyway, so why bother? +#if 0 /* so don't bother ;) */ + j->contact.surface.mode = 0; + j->contact.surface.mu = 0; + dSetZero (j->contact.geom.pos,4); + dSetZero (j->contact.geom.normal,4); + j->contact.geom.depth = 0; +#endif +} + + +static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) +{ + // make sure mu's >= 0, then calculate number of constraint rows and number + // of unbounded rows. + int m = 1, nub=0; + if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; + if (j->contact.surface.mode & dContactMu2) { + if (j->contact.surface.mu > 0) m++; + if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; + if (j->contact.surface.mu2 > 0) m++; + if (j->contact.surface.mu == dInfinity) nub ++; + if (j->contact.surface.mu2 == dInfinity) nub ++; + } + else { + if (j->contact.surface.mu > 0) m += 2; + if (j->contact.surface.mu == dInfinity) nub += 2; + } + + j->the_m = m; + info->m = m; + info->nub = nub; +} + + +static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) +{ + int i,s = info->rowskip; + int s2 = 2*s; + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + if (j->flags & dJOINT_REVERSE) { + normal[0] = - j->contact.geom.normal[0]; + normal[1] = - j->contact.geom.normal[1]; + normal[2] = - j->contact.geom.normal[2]; + } + else { + normal[0] = j->contact.geom.normal[0]; + normal[1] = j->contact.geom.normal[1]; + normal[2] = j->contact.geom.normal[2]; + } + normal[3] = 0; // @@@ hmmm + + // c1,c2 = contact points with respect to body PORs + dVector3 c1,c2; + for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i]; + + // set jacobian for normal + info->J1l[0] = normal[0]; + info->J1l[1] = normal[1]; + info->J1l[2] = normal[2]; + dCROSS (info->J1a,=,c1,normal); + if (j->node[1].body) { + for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - + j->node[1].body->pos[i]; + info->J2l[0] = -normal[0]; + info->J2l[1] = -normal[1]; + info->J2l[2] = -normal[2]; + dCROSS (info->J2a,= -,c2,normal); + } + + // set right hand side and cfm value for normal + dReal erp = info->erp; + if (j->contact.surface.mode & dContactSoftERP) + erp = j->contact.surface.soft_erp; + dReal k = info->fps * erp; + info->c[0] = k*j->contact.geom.depth; + if (j->contact.surface.mode & dContactSoftCFM) + info->cfm[0] = j->contact.surface.soft_cfm; + + // deal with bounce + if (j->contact.surface.mode & dContactBounce) { + // calculate outgoing velocity (-ve for incoming contact) + dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + + dDOT(info->J1a,j->node[0].body->avel); + if (j->node[1].body) { + outgoing += dDOT(info->J2l,j->node[1].body->lvel) + + dDOT(info->J2a,j->node[1].body->avel); + } + // only apply bounce if the outgoing velocity is greater than the + // threshold, and if the resulting c[0] exceeds what we already have. + if (j->contact.surface.bounce_vel >= 0 && + (-outgoing) > j->contact.surface.bounce_vel) { + dReal newc = - j->contact.surface.bounce * outgoing; + if (newc > info->c[0]) info->c[0] = newc; + } + } + + // set LCP limits for normal + info->lo[0] = 0; + info->hi[0] = dInfinity; + + // now do jacobian for tangential forces + dVector3 t1,t2; // two vectors tangential to normal + + // first friction direction + if (j->the_m >= 2) { + if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? + t1[0] = j->contact.fdir1[0]; + t1[1] = j->contact.fdir1[1]; + t1[2] = j->contact.fdir1[2]; + dCROSS (t2,=,normal,t1); + } + else { + dPlaneSpace (normal,t1,t2); + } + info->J1l[s+0] = t1[0]; + info->J1l[s+1] = t1[1]; + info->J1l[s+2] = t1[2]; + dCROSS (info->J1a+s,=,c1,t1); + if (j->node[1].body) { + info->J2l[s+0] = -t1[0]; + info->J2l[s+1] = -t1[1]; + info->J2l[s+2] = -t1[2]; + dCROSS (info->J2a+s,= -,c2,t1); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion1) { + info->c[1] = j->contact.surface.motion1; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + info->lo[1] = -j->contact.surface.mu; + info->hi[1] = j->contact.surface.mu; + if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip1) + info->cfm[1] = j->contact.surface.slip1; + } + + // second friction direction + if (j->the_m >= 3) { + info->J1l[s2+0] = t2[0]; + info->J1l[s2+1] = t2[1]; + info->J1l[s2+2] = t2[2]; + dCROSS (info->J1a+s2,=,c1,t2); + if (j->node[1].body) { + info->J2l[s2+0] = -t2[0]; + info->J2l[s2+1] = -t2[1]; + info->J2l[s2+2] = -t2[2]; + dCROSS (info->J2a+s2,= -,c2,t2); + } + // set right hand side + if (j->contact.surface.mode & dContactMotion2) { + info->c[2] = j->contact.surface.motion2; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + if (j->contact.surface.mode & dContactMu2) { + info->lo[2] = -j->contact.surface.mu2; + info->hi[2] = j->contact.surface.mu2; + } + else { + info->lo[2] = -j->contact.surface.mu; + info->hi[2] = j->contact.surface.mu; + } + if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; + + // set slip (constraint force mixing) + if (j->contact.surface.mode & dContactSlip2) + info->cfm[2] = j->contact.surface.slip2; + } +} + + +dxJoint::Vtable __dcontact_vtable = { + sizeof(dxJointContact), + (dxJoint::init_fn*) contactInit, + (dxJoint::getInfo1_fn*) contactGetInfo1, + (dxJoint::getInfo2_fn*) contactGetInfo2, + dJointTypeContact}; + +//**************************************************************************** +// hinge 2. note that this joint must be attached to two bodies for it to work + +static dReal measureHinge2Angle (dxJointHinge2 *joint) +{ + dVector3 a1,a2; + dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2); + dMULTIPLY1_331 (a2,joint->node[0].body->R,a1); + dReal x = dDOT(joint->v1,a2); + dReal y = dDOT(joint->v2,a2); + return -dAtan2 (y,x); +} + + +static void hinge2Init (dxJointHinge2 *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + j->c0 = 0; + j->s0 = 0; + + dSetZero (j->v1,4); + j->v1[0] = 1; + dSetZero (j->v2,4); + j->v2[1] = 1; + + j->limot1.init (j->world); + j->limot2.init (j->world); + + j->susp_erp = j->world->global_erp; + j->susp_cfm = j->world->global_cfm; + + j->flags |= dJOINT_TWOBODIES; +} + + +static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) +{ + info->m = 4; + info->nub = 4; + + // see if we're powered or at a joint limit for axis 1 + int atlimit=0; + if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop) { + dReal angle = measureHinge2Angle (j); + if (j->limot1.testRotationalLimit (angle)) atlimit = 1; + } + if (atlimit || j->limot1.fmax > 0) info->m++; + + // see if we're powering axis 2 (we currently never limit this axis) + j->limot2.limit = 0; + if (j->limot2.fmax > 0) info->m++; +} + + +// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are +// relative to body 1 and 2 initially) and then computes the constrained +// rotational axis as the cross product of ax1 and ax2. +// the sin and cos of the angle between axis 1 and 2 is computed, this comes +// from dot and cross product rules. + +#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ + dVector3 ax1,ax2; \ + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \ + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \ + dCROSS (axis,=,ax1,ax2); \ + sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ + cos_angle = dDOT (ax1,ax2); + + +static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) +{ + // get information we need to set the hinge row + dReal s,c; + dVector3 q; + HINGE2_GET_AXIS_INFO (q,s,c); + dNormalize3 (q); // @@@ quicker: divide q by s ? + + // set the three ball-and-socket rows (aligned to the suspension axis ax1) + setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); + + // set the hinge row + int s3=3*info->rowskip; + info->J1a[s3+0] = q[0]; + info->J1a[s3+1] = q[1]; + info->J1a[s3+2] = q[2]; + if (joint->node[1].body) { + info->J2a[s3+0] = -q[0]; + info->J2a[s3+1] = -q[1]; + info->J2a[s3+2] = -q[2]; + } + + // compute the right hand side for the constrained rotational DOF. + // axis 1 and axis 2 are separated by an angle `theta'. the desired + // separation angle is theta0. sin(theta0) and cos(theta0) are recorded + // in the joint structure. the correcting angular velocity is: + // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize + // = (erp*fps) * (theta0-theta) + // (theta0-theta) can be computed using the following small-angle-difference + // approximation: + // theta0-theta ~= tan(theta0-theta) + // = sin(theta0-theta)/cos(theta0-theta) + // = (c*s0 - s*c0) / (c*c0 + s*s0) + // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 + // where c = cos(theta), s = sin(theta) + // c0 = cos(theta0), s0 = sin(theta0) + + dReal k = info->fps * info->erp; + info->c[3] = k * (joint->c0 * s - joint->s0 * c); + + // if the axis1 hinge is powered, or has joint limits, add in more stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the axis2 hinge is powered, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); + + // set parameter for the suspension + info->cfm[0] = joint->susp_cfm; +} + + +// compute vectors v1 and v2 (embedded in body1), used to measure angle +// between body 1 and body 2 + +static void makeHinge2V1andV2 (dxJointHinge2 *joint) +{ + if (joint->node[0].body) { + // get axis 1 and 2 in global coords + dVector3 ax1,ax2,v; + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + + // don't do anything if the axis1 or axis2 vectors are zero or the same + if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || + (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || + (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; + + // modify axis 2 so it's perpendicular to axis 1 + dReal k = dDOT(ax1,ax2); + for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; + dNormalize3 (ax2); + + // make v1 = modified axis2, v2 = axis1 x (modified axis2) + dCROSS (v,=,ax1,ax2); + dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2); + dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v); + } +} + + +extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dReal q[4]; + q[0] = x; + q[1] = y; + q[2] = z; + q[3] = 0; + dNormalize3 (q); + dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); + joint->axis1[3] = 0; + + // compute the sin and cos of the angle between axis 1 and axis 2 + dVector3 ax; + HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); + } + makeHinge2V1andV2 (joint); +} + + +extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + if (parameter == dParamSuspensionERP) joint->susp_erp = value; + else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; + else joint->limot1.set (parameter,value); + } +} + + +extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetHinge2Anchor2 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1); + } +} + + +extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[1].body) { + dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2); + } +} + + +extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + if (parameter == dParamSuspensionERP) return joint->susp_erp; + else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; + else return joint->limot1.get (parameter); + } +} + + +extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) return measureHinge2Angle (joint); + else return 0; +} + + +extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + if (joint->node[0].body && joint->node[1].body) { + dVector3 axis; + dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2); + dReal rate = dDOT(axis,joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); + return rate; + } + else return 0; +} + + +extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dReal torque2) +{ + dVector3 axis1, axis2; + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); + + if (joint->node[0].body && joint->node[1].body) { + dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); + } +} + + +dxJoint::Vtable __dhinge2_vtable = { + sizeof(dxJointHinge2), + (dxJoint::init_fn*) hinge2Init, + (dxJoint::getInfo1_fn*) hinge2GetInfo1, + (dxJoint::getInfo2_fn*) hinge2GetInfo2, + dJointTypeHinge2}; + +//**************************************************************************** +// universal + +// I just realized that the universal joint is equivalent to a hinge 2 joint with +// perfectly stiff suspension. By comparing the hinge 2 implementation to +// the universal implementation, you may be able to improve this +// implementation (or, less likely, the hinge2 implementation). + +static void universalInit (dxJointUniversal *j) +{ + dSetZero (j->anchor1,4); + dSetZero (j->anchor2,4); + dSetZero (j->axis1,4); + j->axis1[0] = 1; + dSetZero (j->axis2,4); + j->axis2[1] = 1; + dSetZero(j->qrel1,4); + dSetZero(j->qrel2,4); + j->limot1.init (j->world); + j->limot2.init (j->world); +} + + +static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) +{ + // This says "ax1 = joint->node[0].body->R * joint->axis1" + dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + + if (joint->node[1].body) { + dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + } + else { + ax2[0] = joint->axis2[0]; + ax2[1] = joint->axis2[1]; + ax2[2] = joint->axis2[2]; + } +} + + +static dReal getUniversalAngle1(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ (R,qcross); + + // This code is essential the same as getHingeAngle(), see the comments + // there for details. + + // get qrel = relative rotation between node[0] and the cross + dQMultiply1 (qq,joint->node[0].body->q,qcross); + dQMultiply2 (qrel,qq,joint->qrel1); + + return getHingeAngleFromRelativeQuat(qrel, joint->axis1); + } + return 0; +} + + +static dReal getUniversalAngle2(dxJointUniversal *joint) +{ + if (joint->node[0].body) { + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross, qq, qrel; + + getUniversalAxes (joint,ax1,ax2); + + // It should be possible to get both angles without explicitly + // constructing the rotation matrix of the cross. Basically, + // orientation of the cross about axis1 comes from body 2, + // about axis 2 comes from body 1, and the perpendicular + // axis can come from the two bodies somehow. (We don't really + // want to assume it's 90 degrees, because in general the + // constraints won't be perfectly satisfied, or even very well + // satisfied.) + // + // However, we'd need a version of getHingeAngleFromRElativeQuat() + // that CAN handle when its relative quat is rotated along a direction + // other than the given axis. What I have here works, + // although it's probably much slower than need be. + + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + + if (joint->node[1].body) { + dQMultiply1 (qq, joint->node[1].body->q, qcross); + dQMultiply2 (qrel,qq,joint->qrel2); + } + else { + // pretend joint->node[1].body->q is the identity + dQMultiply2 (qrel,qcross, joint->qrel2); + } + + return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); + } + return 0; +} + + +static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) +{ + info->nub = 4; + info->m = 4; + + // see if we're powered or at a joint limit. + bool constraint1 = j->limot1.fmax > 0; + bool constraint2 = j->limot2.fmax > 0; + + bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && + j->limot1.lostop <= j->limot1.histop; + bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) && + j->limot2.lostop <= j->limot2.histop; + + // We need to call testRotationLimit() even if we're motored, since it + // records the result. + if (limiting1 || limiting2) { + dReal angle1, angle2; + angle1 = getUniversalAngle1(j); + angle2 = getUniversalAngle2(j); + if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; + if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; + } + if (constraint1) + info->m++; + if (constraint2) + info->m++; +} + + +static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) +{ + // set the three ball-and-socket rows + setBall (joint,info,joint->anchor1,joint->anchor2); + + // set the universal joint row. the angular velocity about an axis + // perpendicular to both joint axes should be equal. thus the constraint + // equation is + // p*w1 - p*w2 = 0 + // where p is a vector normal to both joint axes, and w1 and w2 + // are the angular velocity vectors of the two bodies. + + // length 1 joint axis in global coordinates, from each body + dVector3 ax1, ax2; + dVector3 ax2_temp; + // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate + // about this. + dVector3 p; + dReal k; + + getUniversalAxes(joint, ax1, ax2); + k = dDOT(ax1, ax2); + ax2_temp[0] = ax2[0] - k*ax1[0]; + ax2_temp[1] = ax2[1] - k*ax1[1]; + ax2_temp[2] = ax2[2] - k*ax1[2]; + dCROSS(p, =, ax1, ax2_temp); + dNormalize3(p); + + int s3=3*info->rowskip; + + info->J1a[s3+0] = p[0]; + info->J1a[s3+1] = p[1]; + info->J1a[s3+2] = p[2]; + + if (joint->node[1].body) { + info->J2a[s3+0] = -p[0]; + info->J2a[s3+1] = -p[1]; + info->J2a[s3+2] = -p[2]; + } + + // compute the right hand side of the constraint equation. set relative + // body velocities along p to bring the axes back to perpendicular. + // If ax1, ax2 are unit length joint axes as computed from body1 and + // body2, we need to rotate both bodies along the axis p. If theta + // is the angle between ax1 and ax2, we need an angular velocity + // along p to cover the angle erp * (theta - Pi/2) in one step: + // + // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize + // = (erp*fps) * (theta - Pi/2) + // + // if theta is close to Pi/2, + // theta - Pi/2 ~= cos(theta), so + // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) + + info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); + + // if the first angle is powered, or has joint limits, add in the stuff + int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); + + // if the second angle is powered, or has joint limits, add in more stuff + joint->limot2.addLimot (joint,info,row,ax2,1); +} + + +static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) +{ + if (joint->node[0].body) { + dVector3 ax1, ax2; + dMatrix3 R; + dQuaternion qcross; + + getUniversalAxes(joint, ax1, ax2); + + // Axis 1. + dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); + dRtoQ(R, qcross); + dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); + + // Axis 2. + dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); + dRtoQ(R, qcross); + if (joint->node[1].body) { + dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); + } + else { + // set joint->qrel to qcross + for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; + } + } +} + + +extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,NULL,joint->axis2); + else + setAxes (joint,x,y,z,joint->axis1,NULL); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint, + dReal x, dReal y, dReal z) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + setAxes (joint,x,y,z,joint->axis1,NULL); + else + setAxes (joint,x,y,z,NULL,joint->axis2); + universalComputeInitialRelativeRotations(joint); +} + + +extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor2 (joint,result,joint->anchor2); + else + getAnchor (joint,result,joint->anchor1); +} + + +extern "C" void dJointGetUniversalAnchor2 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAnchor (joint,result,joint->anchor1); + else + getAnchor2 (joint,result,joint->anchor2); +} + + +extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,result,joint->axis2); + else + getAxis (joint,result,joint->axis1); +} + + +extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint, + dVector3 result) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(result,"bad result argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,result,joint->axis1); + else + getAxis2 (joint,result,joint->axis2); +} + + +extern "C" void dJointSetUniversalParam (dxJointUniversal *joint, + int parameter, dReal value) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + joint->limot2.set (parameter & 0xff,value); + } + else { + joint->limot1.set (parameter,value); + } +} + + +extern "C" dReal dJointGetUniversalParam (dxJointUniversal *joint, int parameter) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if ((parameter & 0xff00) == 0x100) { + return joint->limot2.get (parameter & 0xff); + } + else { + return joint->limot1.get (parameter); + } +} + + +extern "C" dReal dJointGetUniversalAngle1 (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle2 (joint); + else + return getUniversalAngle1 (joint); +} + + +extern "C" dReal dJointGetUniversalAngle2 (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + if (joint->flags & dJOINT_REVERSE) + return getUniversalAngle1 (joint); + else + return getUniversalAngle2 (joint); +} + + +extern "C" dReal dJointGetUniversalAngle1Rate (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis2 (joint,axis,joint->axis2); + else + getAxis (joint,axis,joint->axis1); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +extern "C" dReal dJointGetUniversalAngle2Rate (dxJointUniversal *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->node[0].body) { + dVector3 axis; + + if (joint->flags & dJOINT_REVERSE) + getAxis (joint,axis,joint->axis1); + else + getAxis2 (joint,axis,joint->axis2); + + dReal rate = dDOT(axis, joint->node[0].body->avel); + if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); + return rate; + } + return 0; +} + + +extern "C" void dJointAddUniversalTorques (dxJointUniversal *joint, dReal torque1, dReal torque2) +{ + dVector3 axis1, axis2; + dAASSERT(joint); + dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); + + if (joint->flags & dJOINT_REVERSE) { + dReal temp = torque1; + torque1 = - torque2; + torque2 = - temp; + } + + getAxis (joint,axis1,joint->axis1); + getAxis2 (joint,axis2,joint->axis2); + axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; + axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; + axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); +} + + + + + +dxJoint::Vtable __duniversal_vtable = { + sizeof(dxJointUniversal), + (dxJoint::init_fn*) universalInit, + (dxJoint::getInfo1_fn*) universalGetInfo1, + (dxJoint::getInfo2_fn*) universalGetInfo2, + dJointTypeUniversal}; + +//**************************************************************************** +// angular motor + +static void amotorInit (dxJointAMotor *j) +{ + int i; + j->num = 0; + j->mode = dAMotorUser; + for (i=0; i<3; i++) { + j->rel[i] = 0; + dSetZero (j->axis[i],4); + j->limot[i].init (j->world); + j->angle[i] = 0; + } + dSetZero (j->reference1,4); + dSetZero (j->reference2,4); + + j->flags |= dJOINT_TWOBODIES; +} + + +// compute the 3 axes in global coordinates + +static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) +{ + if (joint->mode == dAMotorEuler) { + // special handling for euler mode + dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]); + dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]); + dCROSS (ax[1],=,ax[2],ax[0]); + dNormalize3 (ax[1]); + } + else { + for (int i=0; i < joint->num; i++) { + if (joint->rel[i] == 1) { + // relative to b1 + dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]); + } + if (joint->rel[i] == 2) { + // relative to b2 + dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]); + } + else { + // global - just copy it + ax[i][0] = joint->axis[i][0]; + ax[i][1] = joint->axis[i][1]; + ax[i][2] = joint->axis[i][2]; + } + } + } +} + + +static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) +{ + // assumptions: + // global axes already calculated --> ax + // axis[0] is relative to body 1 --> global ax[0] + // axis[2] is relative to body 2 --> global ax[2] + // ax[1] = ax[2] x ax[0] + // original ax[0] and ax[2] are perpendicular + // reference1 is perpendicular to ax[0] (in body 1 frame) + // reference2 is perpendicular to ax[2] (in body 2 frame) + // all ax[] and reference vectors are unit length + + // calculate references in global frame + dVector3 ref1,ref2; + dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1); + dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2); + + // get q perpendicular to both ax[0] and ref1, get first euler angle + dVector3 q; + dCROSS (q,=,ax[0],ref1); + joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); + + // get q perpendicular to both ax[0] and ax[1], get second euler angle + dCROSS (q,=,ax[0],ax[1]); + joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); + + // get q perpendicular to both ax[1] and ax[2], get third euler angle + dCROSS (q,=,ax[1],ax[2]); + joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); +} + + +// set the reference vectors as follows: +// * reference1 = current axis[2] relative to body 1 +// * reference2 = current axis[0] relative to body 2 +// this assumes that: +// * axis[0] is relative to body 1 +// * axis[2] is relative to body 2 + +static void amotorSetEulerReferenceVectors (dxJointAMotor *j) +{ + if (j->node[0].body && j->node[1].body) { + dVector3 r; // axis[2] and axis[0] in global coordinates + dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]); + dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r); + dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]); + dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r); + } +} + + +static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; + + // compute the axes and angles, if in euler mode + if (j->mode == dAMotorEuler) { + dVector3 ax[3]; + amotorComputeGlobalAxes (j,ax); + amotorComputeEulerAngles (j,ax); + } + + // see if we're powered or at a joint limit for each axis + for (int i=0; i < j->num; i++) { + if (j->limot[i].testRotationalLimit (j->angle[i]) || + j->limot[i].fmax > 0) { + info->m++; + } + } +} + + +static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) +{ + int i; + + // compute the axes (if not global) + dVector3 ax[3]; + amotorComputeGlobalAxes (joint,ax); + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + dVector3 *axptr[3]; + axptr[0] = &ax[0]; + axptr[1] = &ax[1]; + axptr[2] = &ax[2]; + + dVector3 ax0_cross_ax1; + dVector3 ax1_cross_ax2; + if (joint->mode == dAMotorEuler) { + dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); + axptr[2] = &ax0_cross_ax1; + dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); + axptr[0] = &ax1_cross_ax2; + } + + int row=0; + for (i=0; i < joint->num; i++) { + row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); + } +} + + +extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num) +{ + dAASSERT(joint && num >= 0 && num <= 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorEuler) { + joint->num = 3; + } + else { + if (num < 0) num = 0; + if (num > 3) num = 3; + joint->num = num; + } +} + + +extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel, + dReal x, dReal y, dReal z) +{ + dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + joint->rel[anum] = rel; + + // x,y,z is always in global coordinates regardless of rel, so we may have + // to convert it to be relative to a body + dVector3 r; + r[0] = x; + r[1] = y; + r[2] = z; + r[3] = 0; + if (rel > 0) { + if (rel==1) { + dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r); + } + else { + dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r); + } + } + else { + joint->axis[anum][0] = r[0]; + joint->axis[anum][1] = r[1]; + joint->axis[anum][2] = r[2]; + } + dNormalize3 (joint->axis[anum]); + if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); +} + + +extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum, + dReal angle) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (joint->mode == dAMotorUser) { + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + joint->angle[anum] = angle; + } +} + + +extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter, + dReal value) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + joint->limot[anum].set (parameter, value); +} + + +extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + joint->mode = mode; + if (joint->mode == dAMotorEuler) { + joint->num = 3; + amotorSetEulerReferenceVectors (joint); + } +} + + +extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->num; +} + + +extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum, + dVector3 result) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + if (joint->rel[anum] > 0) { + if (joint->rel[anum]==1) { + dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]); + } + else { + dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]); + } + } + else { + result[0] = joint->axis[anum][0]; + result[1] = joint->axis[anum][1]; + result[2] = joint->axis[anum][2]; + } +} + + +extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + return joint->rel[anum]; +} + + +extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum) +{ + dAASSERT(joint && anum >= 0 && anum < 3); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + if (anum < 0) anum = 0; + if (anum > 3) anum = 3; + return joint->angle[anum]; +} + + +extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum) +{ + // @@@ + dDebug (0,"not yet implemented"); + return 0; +} + + +extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + int anum = parameter >> 8; + if (anum < 0) anum = 0; + if (anum > 2) anum = 2; + parameter &= 0xff; + return joint->limot[anum].get (parameter); +} + + +extern "C" int dJointGetAMotorMode (dxJointAMotor *joint) +{ + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + return joint->mode; +} + + +extern "C" void dJointAddAMotorTorques (dxJointAMotor *joint, dReal torque1, dReal torque2, dReal torque3) +{ + dVector3 axes[3]; + dAASSERT(joint); + dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); + + if (joint->num == 0) + return; + dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints"); + + amotorComputeGlobalAxes (joint,axes); + axes[0][0] *= torque1; + axes[0][1] *= torque1; + axes[0][2] *= torque1; + if (joint->num >= 2) { + axes[0][0] += axes[1][0] * torque2; + axes[0][1] += axes[1][0] * torque2; + axes[0][2] += axes[1][0] * torque2; + if (joint->num >= 3) { + axes[0][0] += axes[2][0] * torque3; + axes[0][1] += axes[2][0] * torque3; + axes[0][2] += axes[2][0] * torque3; + } + } + + if (joint->node[0].body != 0) + dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); + if (joint->node[1].body != 0) + dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); +} + + +dxJoint::Vtable __damotor_vtable = { + sizeof(dxJointAMotor), + (dxJoint::init_fn*) amotorInit, + (dxJoint::getInfo1_fn*) amotorGetInfo1, + (dxJoint::getInfo2_fn*) amotorGetInfo2, + dJointTypeAMotor}; + +//**************************************************************************** +// fixed joint + +static void fixedInit (dxJointFixed *j) +{ + dSetZero (j->offset,4); + dSetZero (j->qrel,4); +} + + +static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) +{ + info->m = 6; + info->nub = 6; +} + + +static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) +{ + int s = info->rowskip; + + // Three rows for orientation + setFixedOrientation(joint, info, joint->qrel, 3); + + // Three rows for position. + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + dVector3 ofs; + dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset); + if (joint->node[1].body) { + dCROSSMAT (info->J1a,ofs,s,+,-); + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + } + + // set right hand side for the first three rows (linear) + dReal k = info->fps * info->erp; + if (joint->node[1].body) { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->node[1].body->pos[j] - + joint->node[0].body->pos[j] + ofs[j]); + } + else { + for (int j=0; j<3; j++) + info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]); + } +} + + +extern "C" void dJointSetFixed (dxJointFixed *joint) +{ + dUASSERT(joint,"bad joint argument"); + dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); + int i; + + // This code is taken from sJointSetSliderAxis(), we should really put the + // common code in its own function. + // compute the offset between the bodies + if (joint->node[0].body) { + if (joint->node[1].body) { + dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); + dReal ofs[4]; + for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; + for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); + } + else { + // set joint->qrel to the transpose of the first body's q + joint->qrel[0] = joint->node[0].body->q[0]; + for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; + for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; + } + } +} + + +dxJoint::Vtable __dfixed_vtable = { + sizeof(dxJointFixed), + (dxJoint::init_fn*) fixedInit, + (dxJoint::getInfo1_fn*) fixedGetInfo1, + (dxJoint::getInfo2_fn*) fixedGetInfo2, + dJointTypeFixed}; + +//**************************************************************************** +// null joint + +static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info) +{ + info->m = 0; + info->nub = 0; +} + + +static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) +{ + dDebug (0,"this should never get called"); +} + + +dxJoint::Vtable __dnull_vtable = { + sizeof(dxJointNull), + (dxJoint::init_fn*) 0, + (dxJoint::getInfo1_fn*) nullGetInfo1, + (dxJoint::getInfo2_fn*) nullGetInfo2, + dJointTypeNull}; + +/******************** breakable joint contribution ***********************/ +extern "C" void dJointSetBreakable (dxJoint *joint, int b) { + dAASSERT(joint); + if (b) { + // we want this joint to be breakable but we must first check if it + // was already breakable + if (!joint->breakInfo) { + // allocate a dxJointBreakInfo struct + joint->breakInfo = new dxJointBreakInfo; + joint->breakInfo->flags = 0; + for (int i = 0; i < 3; i++) { + joint->breakInfo->b1MaxF[0] = 0; + joint->breakInfo->b1MaxT[0] = 0; + joint->breakInfo->b2MaxF[0] = 0; + joint->breakInfo->b2MaxT[0] = 0; + } + joint->breakInfo->callback = 0; + } + else { + // the joint was already breakable + return; + } + } + else { + // we want this joint to be unbreakable mut we must first check if + // it is alreay unbreakable + if (joint->breakInfo) { + // deallocate the dxJointBreakInfo struct + delete joint->breakInfo; + joint->breakInfo = 0; + } + else { + // the joint was already unbreakable + return; + } + } +} + +extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakCallback called on unbreakable joint"); + } +# endif + joint->breakInfo->callback = callbackFunc; +} + +extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakMode called on unbreakable joint"); + } +# endif + joint->breakInfo->flags = mode; +} + +extern "C" int dJointGetBreakMode (dxJoint *joint) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointGetBreakMode called on unbreakable joint"); + } +# endif + return joint->breakInfo->flags; +} + +extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakForce called on unbreakable joint"); + } +# endif + if (body) { + joint->breakInfo->b2MaxF[0] = x; + joint->breakInfo->b2MaxF[1] = y; + joint->breakInfo->b2MaxF[2] = z; + } + else { + joint->breakInfo->b1MaxF[0] = x; + joint->breakInfo->b1MaxF[1] = y; + joint->breakInfo->b1MaxF[2] = z; + } +} + +extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointSetBreakTorque called on unbreakable joint"); + } +# endif + if (body) { + joint->breakInfo->b2MaxT[0] = x; + joint->breakInfo->b2MaxT[1] = y; + joint->breakInfo->b2MaxT[2] = z; + } + else { + joint->breakInfo->b1MaxT[0] = x; + joint->breakInfo->b1MaxT[1] = y; + joint->breakInfo->b1MaxT[2] = z; + } +} + +extern "C" int dJointIsBreakable (dxJoint *joint) { + dAASSERT(joint); + return joint->breakInfo != 0; +} + +extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointGetBreakForce called on unbreakable joint"); + } +# endif + if (body) + for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i]; + else + for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i]; +} + +extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) { + dAASSERT(joint); +# ifndef dNODEBUG + // only works for a breakable joint + if (!joint->breakInfo) { + dDebug (0, "dJointGetBreakTorque called on unbreakable joint"); + } +# endif + if (body) + for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i]; + else + for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i]; +} +/*************************************************************************/ diff --git a/libraries/ode-0.9/contrib/BreakableJoints/joint.h b/libraries/ode-0.9/contrib/BreakableJoints/joint.h new file mode 100644 index 0000000..0573119 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/joint.h @@ -0,0 +1,282 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_JOINT_H_ +#define _ODE_JOINT_H_ + + +#include "objects.h" +#include +#include "obstack.h" + + +// joint flags +enum { + // if this flag is set, the joint was allocated in a joint group + dJOINT_INGROUP = 1, + + // if this flag is set, the joint was attached with arguments (0,body). + // our convention is to treat all attaches as (body,0), i.e. so node[0].body + // is always nonzero, so this flag records the fact that the arguments were + // swapped. + dJOINT_REVERSE = 2, + + // if this flag is set, the joint can not have just one body attached to it, + // it must have either zero or two bodies attached. + dJOINT_TWOBODIES = 4 +}; + + +// there are two of these nodes in the joint, one for each connection to a +// body. these are node of a linked list kept by each body of it's connecting +// joints. but note that the body pointer in each node points to the body that +// makes use of the *other* node, not this node. this trick makes it a bit +// easier to traverse the body/joint graph. + +struct dxJointNode { + dxJoint *joint; // pointer to enclosing dxJoint object + dxBody *body; // *other* body this joint is connected to + dxJointNode *next; // next node in body's list of connected joints +}; + +/******************** breakable joint contribution ***********************/ +struct dxJointBreakInfo : public dBase { + int flags; + dReal b1MaxF[3]; // maximum force on body 1 + dReal b1MaxT[3]; // maximum torque on body 1 + dReal b2MaxF[3]; // maximum force on body 2 + dReal b2MaxT[3]; // maximum torque on body 2 + dJointBreakCallback *callback; // function that is called when this joint breaks +}; +/*************************************************************************/ + +struct dxJoint : public dObject { + // naming convention: the "first" body this is connected to is node[0].body, + // and the "second" body is node[1].body. if this joint is only connected + // to one body then the second body is 0. + + // info returned by getInfo1 function. the constraint dimension is m (<=6). + // i.e. that is the total number of rows in the jacobian. `nub' is the + // number of unbounded variables (which have lo,hi = -/+ infinity). + + struct Info1 { + int m,nub; + }; + + // info returned by getInfo2 function + + struct Info2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + dReal fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + dReal *J1l,*J1a,*J2l,*J2a; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + dReal *c,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + dReal *lo,*hi; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + + // virtual function table: size of the joint structure, function pointers. + // we do it this way instead of using C++ virtual functions because + // sometimes we need to allocate joints ourself within a memory pool. + + typedef void init_fn (dxJoint *joint); + typedef void getInfo1_fn (dxJoint *joint, Info1 *info); + typedef void getInfo2_fn (dxJoint *joint, Info2 *info); + struct Vtable { + int size; + init_fn *init; + getInfo1_fn *getInfo1; + getInfo2_fn *getInfo2; + int typenum; // a dJointTypeXXX type number + }; + + Vtable *vtable; // virtual function table + int flags; // dJOINT_xxx flags + dxJointNode node[2]; // connections to bodies. node[1].body can be 0 + dJointFeedback *feedback; // optional feedback structure + + /******************** breakable joint contribution ***********************/ + // optional break info structure. if this is not NULL the the joint is + // breakable. + dxJointBreakInfo *breakInfo; + /*************************************************************************/ +}; + + +// joint group. NOTE: any joints in the group that have their world destroyed +// will have their world pointer set to 0. + +struct dxJointGroup : public dBase { + int num; // number of joints on the stack + dObStack stack; // a stack of (possibly differently sized) dxJoint +}; // objects. + + +// common limit and motor information for a single joint axis of movement +struct dxJointLimitMotor { + dReal vel,fmax; // powered joint: velocity, max force + dReal lostop,histop; // joint limits, relative to initial position + dReal fudge_factor; // when powering away from joint limits + dReal normal_cfm; // cfm to use when not at a stop + dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit + dReal bounce; // restitution factor + // variables used between getInfo1() and getInfo2() + int limit; // 0=free, 1=at lo limit, 2=at hi limit + dReal limit_err; // if at limit, amount over limit + + void init (dxWorld *); + void set (int num, dReal value); + dReal get (int num); + int testRotationalLimit (dReal angle); + int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row, + dVector3 ax1, int rotational); +}; + + +// ball and socket + +struct dxJointBall : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body +}; +extern struct dxJoint::Vtable __dball_vtable; + + +// hinge + +struct dxJointHinge : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dhinge_vtable; + + +// universal + +struct dxJointUniversal : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis w.r.t first body + dVector3 axis2; // axis w.r.t second body + dQuaternion qrel1; // initial relative rotation body1 -> virtual cross piece + dQuaternion qrel2; // initial relative rotation virtual cross piece -> body2 + dxJointLimitMotor limot1; // limit and motor information for axis1 + dxJointLimitMotor limot2; // limit and motor information for axis2 +}; +extern struct dxJoint::Vtable __duniversal_vtable; + + +// slider. if body2 is 0 then qrel is the absolute rotation of body1 and +// offset is the position of body1 center along axis1. + +struct dxJointSlider : public dxJoint { + dVector3 axis1; // axis w.r.t first body + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // point relative to body2 that should be + // aligned with body1 center along axis1 + dxJointLimitMotor limot; // limit and motor information +}; +extern struct dxJoint::Vtable __dslider_vtable; + + +// contact + +struct dxJointContact : public dxJoint { + int the_m; // number of rows computed by getInfo1 + dContact contact; +}; +extern struct dxJoint::Vtable __dcontact_vtable; + + +// hinge 2 + +struct dxJointHinge2 : public dxJoint { + dVector3 anchor1; // anchor w.r.t first body + dVector3 anchor2; // anchor w.r.t second body + dVector3 axis1; // axis 1 w.r.t first body + dVector3 axis2; // axis 2 w.r.t second body + dReal c0,s0; // cos,sin of desired angle between axis 1,2 + dVector3 v1,v2; // angle ref vectors embedded in first body + dxJointLimitMotor limot1; // limit+motor info for axis 1 + dxJointLimitMotor limot2; // limit+motor info for axis 2 + dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm) +}; +extern struct dxJoint::Vtable __dhinge2_vtable; + + +// angular motor + +struct dxJointAMotor : public dxJoint { + int num; // number of axes (0..3) + int mode; // a dAMotorXXX constant + int rel[3]; // what the axes are relative to (global,b1,b2) + dVector3 axis[3]; // three axes + dxJointLimitMotor limot[3]; // limit+motor info for axes + dReal angle[3]; // user-supplied angles for axes + // these vectors are used for calculating euler angles + dVector3 reference1; // original axis[2], relative to body 1 + dVector3 reference2; // original axis[0], relative to body 2 +}; +extern struct dxJoint::Vtable __damotor_vtable; + + +// fixed + +struct dxJointFixed : public dxJoint { + dQuaternion qrel; // initial relative rotation body1 -> body2 + dVector3 offset; // relative offset between the bodies +}; +extern struct dxJoint::Vtable __dfixed_vtable; + + +// null joint, for testing only + +struct dxJointNull : public dxJoint { +}; +extern struct dxJoint::Vtable __dnull_vtable; + + + +#endif diff --git a/libraries/ode-0.9/contrib/BreakableJoints/objects.h b/libraries/ode-0.9/contrib/BreakableJoints/objects.h new file mode 100644 index 0000000..de08391 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/objects.h @@ -0,0 +1,252 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifndef _ODE_OBJECTS_H_ +#define _ODE_OBJECTS_H_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* world */ + +dWorldID dWorldCreate(); +void dWorldDestroy (dWorldID); + +void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z); +void dWorldGetGravity (dWorldID, dVector3 gravity); +void dWorldSetERP (dWorldID, dReal erp); +dReal dWorldGetERP (dWorldID); +void dWorldSetCFM (dWorldID, dReal cfm); +dReal dWorldGetCFM (dWorldID); +void dWorldStep (dWorldID, dReal stepsize); +void dWorldImpulseToForce (dWorldID, dReal stepsize, + dReal ix, dReal iy, dReal iz, dVector3 force); + +/* StepFast1 functions */ + +void dWorldStepFast1(dWorldID, dReal stepsize, int maxiterations); +void dWorldSetAutoEnableDepthSF1(dWorldID, int autoEnableDepth); + +int dWorldGetAutoEnableDepthSF1(dWorldID); + +void dBodySetAutoDisableThresholdSF1(dBodyID, dReal autoDisableThreshold); + +/* These functions are not yet implemented by ODE. */ +/* +dReal dBodyGetAutoDisableThresholdSF1(dBodyID); + +void dBodySetAutoDisableStepsSF1(dBodyID, int AutoDisableSteps); + +int dBodyGetAutoDisableStepsSF1(dBodyID); + +void dBodySetAutoDisableSF1(dBodyID, int doAutoDisable); + +int dBodyGetAutoDisableSF1(dBodyID); +*/ + +/* bodies */ + +dBodyID dBodyCreate (dWorldID); +void dBodyDestroy (dBodyID); + +void dBodySetData (dBodyID, void *data); +void *dBodyGetData (dBodyID); + +void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z); +void dBodySetRotation (dBodyID, const dMatrix3 R); +void dBodySetQuaternion (dBodyID, const dQuaternion q); +void dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z); +void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z); +const dReal * dBodyGetPosition (dBodyID); +const dReal * dBodyGetRotation (dBodyID); /* ptr to 4x3 rot matrix */ +const dReal * dBodyGetQuaternion (dBodyID); +const dReal * dBodyGetLinearVel (dBodyID); +const dReal * dBodyGetAngularVel (dBodyID); + +void dBodySetMass (dBodyID, const dMass *mass); +void dBodyGetMass (dBodyID, dMass *mass); + +void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz); +void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); +void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz); + +const dReal * dBodyGetForce (dBodyID); +const dReal * dBodyGetTorque (dBodyID); +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z); +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z); + +void dBodyGetRelPointPos (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetRelPointVel (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetPointVel (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyGetPosRelPoint (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyVectorToWorld (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); +void dBodyVectorFromWorld (dBodyID, dReal px, dReal py, dReal pz, + dVector3 result); + +void dBodySetFiniteRotationMode (dBodyID, int mode); +void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z); + +int dBodyGetFiniteRotationMode (dBodyID); +void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result); + +int dBodyGetNumJoints (dBodyID b); +dJointID dBodyGetJoint (dBodyID, int index); + +void dBodyEnable (dBodyID); +void dBodyDisable (dBodyID); +int dBodyIsEnabled (dBodyID); + +void dBodySetGravityMode (dBodyID b, int mode); +int dBodyGetGravityMode (dBodyID b); + + +/* joints */ + +dJointID dJointCreateBall (dWorldID, dJointGroupID); +dJointID dJointCreateHinge (dWorldID, dJointGroupID); +dJointID dJointCreateSlider (dWorldID, dJointGroupID); +dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *); +dJointID dJointCreateHinge2 (dWorldID, dJointGroupID); +dJointID dJointCreateUniversal (dWorldID, dJointGroupID); +dJointID dJointCreateFixed (dWorldID, dJointGroupID); +dJointID dJointCreateNull (dWorldID, dJointGroupID); +dJointID dJointCreateAMotor (dWorldID, dJointGroupID); + +void dJointDestroy (dJointID); + +dJointGroupID dJointGroupCreate (int max_size); +void dJointGroupDestroy (dJointGroupID); +void dJointGroupEmpty (dJointGroupID); + +void dJointAttach (dJointID, dBodyID body1, dBodyID body2); +void dJointSetData (dJointID, void *data); +void *dJointGetData (dJointID); +int dJointGetType (dJointID); +dBodyID dJointGetBody (dJointID, int index); + +void dJointSetFeedback (dJointID, dJointFeedback *); +dJointFeedback *dJointGetFeedback (dJointID); + +/******************** breakable joint contribution ***********************/ +void dJointSetBreakable (dJointID, int b); +void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc); +void dJointSetBreakMode (dJointID, int mode); +int dJointGetBreakMode (dJointID); +void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z); +void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z); +int dJointIsBreakable (dJointID); +void dJointGetBreakForce (dJointID, int body, dReal *force); +void dJointGetBreakTorque (dJointID, int body, dReal *torque); +/*************************************************************************/ + +void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z); +void dJointSetHingeParam (dJointID, int parameter, dReal value); +void dJointAddHingeTorque(dJointID joint, dReal torque); +void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z); +void dJointSetSliderParam (dJointID, int parameter, dReal value); +void dJointAddSliderForce(dJointID joint, dReal force); +void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z); +void dJointSetHinge2Param (dJointID, int parameter, dReal value); +void dJointAddHinge2Torques(dJointID joint, dReal torque1, dReal torque2); +void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z); +void dJointSetUniversalParam (dJointID, int parameter, dReal value); +void dJointAddUniversalTorques(dJointID joint, dReal torque1, dReal torque2); +void dJointSetFixed (dJointID); +void dJointSetAMotorNumAxes (dJointID, int num); +void dJointSetAMotorAxis (dJointID, int anum, int rel, + dReal x, dReal y, dReal z); +void dJointSetAMotorAngle (dJointID, int anum, dReal angle); +void dJointSetAMotorParam (dJointID, int parameter, dReal value); +void dJointSetAMotorMode (dJointID, int mode); +void dJointAddAMotorTorques (dJointID, dReal torque1, dReal torque2, dReal torque3); + +void dJointGetBallAnchor (dJointID, dVector3 result); +void dJointGetBallAnchor2 (dJointID, dVector3 result); +void dJointGetHingeAnchor (dJointID, dVector3 result); +void dJointGetHingeAnchor2 (dJointID, dVector3 result); +void dJointGetHingeAxis (dJointID, dVector3 result); +dReal dJointGetHingeParam (dJointID, int parameter); +dReal dJointGetHingeAngle (dJointID); +dReal dJointGetHingeAngleRate (dJointID); +dReal dJointGetSliderPosition (dJointID); +dReal dJointGetSliderPositionRate (dJointID); +void dJointGetSliderAxis (dJointID, dVector3 result); +dReal dJointGetSliderParam (dJointID, int parameter); +void dJointGetHinge2Anchor (dJointID, dVector3 result); +void dJointGetHinge2Anchor2 (dJointID, dVector3 result); +void dJointGetHinge2Axis1 (dJointID, dVector3 result); +void dJointGetHinge2Axis2 (dJointID, dVector3 result); +dReal dJointGetHinge2Param (dJointID, int parameter); +dReal dJointGetHinge2Angle1 (dJointID); +dReal dJointGetHinge2Angle1Rate (dJointID); +dReal dJointGetHinge2Angle2Rate (dJointID); +void dJointGetUniversalAnchor (dJointID, dVector3 result); +void dJointGetUniversalAnchor2 (dJointID, dVector3 result); +void dJointGetUniversalAxis1 (dJointID, dVector3 result); +void dJointGetUniversalAxis2 (dJointID, dVector3 result); +dReal dJointGetUniversalParam (dJointID, int parameter); +dReal dJointGetUniversalAngle1 (dJointID); +dReal dJointGetUniversalAngle2 (dJointID); +dReal dJointGetUniversalAngle1Rate (dJointID); +dReal dJointGetUniversalAngle2Rate (dJointID); +int dJointGetAMotorNumAxes (dJointID); +void dJointGetAMotorAxis (dJointID, int anum, dVector3 result); +int dJointGetAMotorAxisRel (dJointID, int anum); +dReal dJointGetAMotorAngle (dJointID, int anum); +dReal dJointGetAMotorAngleRate (dJointID, int anum); +dReal dJointGetAMotorParam (dJointID, int parameter); +int dJointGetAMotorMode (dJointID); + +int dAreConnected (dBodyID, dBodyID); +int dAreConnectedExcluding (dBodyID, dBodyID, int joint_type); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp b/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp new file mode 100644 index 0000000..7137960 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp @@ -0,0 +1,1404 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#ifdef _MSC_VER +#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found" +#endif + +// this source file is mostly concerned with the data structures, not the +// numerics. + +#include "objects.h" +#include +#include "joint.h" +#include +#include +#include "step.h" +#include +#include + +// misc defines +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// utility + +static inline void initObject (dObject *obj, dxWorld *w) +{ + obj->world = w; + obj->next = 0; + obj->tome = 0; + obj->userdata = 0; + obj->tag = 0; +} + + +// add an object `obj' to the list who's head pointer is pointed to by `first'. + +static inline void addObjectToList (dObject *obj, dObject **first) +{ + obj->next = *first; + obj->tome = first; + if (*first) (*first)->tome = &obj->next; + (*first) = obj; +} + + +// remove the object from the linked list + +static inline void removeObjectFromList (dObject *obj) +{ + if (obj->next) obj->next->tome = obj->tome; + *(obj->tome) = obj->next; + // safeguard + obj->next = 0; + obj->tome = 0; +} + + +// remove the joint from neighbour lists of all connected bodies + +static void removeJointReferencesFromAttachedBodies (dxJoint *j) +{ + for (int i=0; i<2; i++) { + dxBody *body = j->node[i].body; + if (body) { + dxJointNode *n = body->firstjoint; + dxJointNode *last = 0; + while (n) { + if (n->joint == j) { + if (last) last->next = n->next; + else body->firstjoint = n->next; + break; + } + last = n; + n = n->next; + } + } + } + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; +} + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +static void processIslands (dxWorld *world, dReal stepsize) +{ + dxBody *b,*bb,**body; + dxJoint *j,**joint; + + // nothing to do if no bodies + if (world->nb <= 0) return; + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*)); + joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + + // set all body/joint tags to 0 + for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; + for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*)); + + for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) { + b = stack[--stacksize]; // pop body off stack + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (!n->joint->tag) { + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) { + n->body->tag = 1; + stack[stacksize++] = n->body; + } + } + } + dIASSERT(stacksize <= world->nb); + dIASSERT(stacksize <= world->nj); + } + + // now do something with body and joint lists + dInternalStepIsland (world,body,bcount,joint,jcount,stepsize); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i=0; itag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i=0; itag = 1; + } + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b=world->firstbody; b; b=(dxBody*)b->next) { + if (b->flags & dxBodyDisabled) { + if (b->tag) dDebug (0,"disabled body tagged"); + } + else { + if (!b->tag) dDebug (0,"enabled body not tagged"); + } + } + for (j=world->firstjoint; j; j=(dxJoint*)j->next) { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) || + (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) { + if (!j->tag) dDebug (0,"attached enabled joint not tagged"); + } + else { + if (j->tag) dDebug (0,"unattached or disabled joint tagged"); + } + } +# endif + /******************** breakable joint contribution ***********************/ + dxJoint* nextJ; + if (!world->firstjoint) + nextJ = 0; + else + nextJ = (dxJoint*)world->firstjoint->next; + for (j=world->firstjoint; j; j=nextJ) { + nextJ = (dxJoint*)j->next; + // check if joint is breakable and broken + if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { + // detach (break) the joint + dJointAttach (j, 0, 0); + // call the callback function if it is set + if (j->breakInfo->callback) j->breakInfo->callback (j); + // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set + if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); + } + } + /*************************************************************************/ +} + +//**************************************************************************** +// debugging + +// see if an object list loops on itself (if so, it's bad). + +static int listHasLoops (dObject *first) +{ + if (first==0 || first->next==0) return 0; + dObject *a=first,*b=first->next; + int skip=0; + while (b) { + if (a==b) return 1; + b = b->next; + if (skip) a = a->next; + skip ^= 1; + } + return 0; +} + + +// check the validity of the world data structures + +static void checkWorld (dxWorld *w) +{ + dxBody *b; + dxJoint *j; + + // check there are no loops + if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops"); + if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops"); + + // check lists are well formed (check `tome' pointers) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + if (b->next && b->next->tome != &b->next) + dDebug (0,"bad tome pointer in body list"); + } + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->next && j->next->tome != &j->next) + dDebug (0,"bad tome pointer in joint list"); + } + + // check counts + int n = 0; + for (b=w->firstbody; b; b=(dxBody*)b->next) n++; + if (w->nb != n) dDebug (0,"body count incorrect"); + n = 0; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++; + if (w->nj != n) dDebug (0,"joint count incorrect"); + + // set all tag values to a known value + static int count = 0; + count++; + for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count; + for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count; + + // check all body/joint world pointers are ok + for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w) + dDebug (0,"bad world pointer in body list"); + for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w) + dDebug (0,"bad world pointer in joint list"); + + /* + // check for half-connected joints - actually now these are valid + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body || j->node[1].body) { + if (!(j->node[0].body && j->node[1].body)) + dDebug (0,"half connected joint found"); + } + } + */ + + // check that every joint node appears in the joint lists of both bodies it + // attaches + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + for (int i=0; i<2; i++) { + if (j->node[i].body) { + int ok = 0; + for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) { + if (n->joint == j) ok = 1; + } + if (ok==0) dDebug (0,"joint not in joint list of attached body"); + } + } + } + + // check all body joint lists (correct body ptrs) + for (b=w->firstbody; b; b=(dxBody*)b->next) { + for (dxJointNode *n=b->firstjoint; n; n=n->next) { + if (&n->joint->node[0] == n) { + if (n->joint->node[1].body != b) + dDebug (0,"bad body pointer in joint node of body list (1)"); + } + else { + if (n->joint->node[0].body != b) + dDebug (0,"bad body pointer in joint node of body list (2)"); + } + if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body"); + } + } + + // check all body pointers in joints, check they are distinct + for (j=w->firstjoint; j; j=(dxJoint*)j->next) { + if (j->node[0].body && (j->node[0].body == j->node[1].body)) + dDebug (0,"non-distinct body pointers in joint"); + if ((j->node[0].body && j->node[0].body->tag != count) || + (j->node[1].body && j->node[1].body->tag != count)) + dDebug (0,"bad body pointer in joint"); + } +} + + +void dWorldCheck (dxWorld *w) +{ + checkWorld (w); +} + +//**************************************************************************** +// body + +dxBody *dBodyCreate (dxWorld *w) +{ + dAASSERT (w); + dxBody *b = new dxBody; + initObject (b,w); + b->firstjoint = 0; + b->flags = 0; + b->geom = 0; + dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0); + dSetZero (b->invI,4*3); + b->invI[0] = 1; + b->invI[5] = 1; + b->invI[10] = 1; + b->invMass = 1; + dSetZero (b->pos,4); + dSetZero (b->q,4); + b->q[0] = 1; + dRSetIdentity (b->R); + dSetZero (b->lvel,4); + dSetZero (b->avel,4); + dSetZero (b->facc,4); + dSetZero (b->tacc,4); + dSetZero (b->finite_rot_axis,4); + addObjectToList (b,(dObject **) &w->firstbody); + w->nb++; + return b; +} + + +void dBodyDestroy (dxBody *b) +{ + dAASSERT (b); + + // all geoms that link to this body must be notified that the body is about + // to disappear. note that the call to dGeomSetBody(geom,0) will result in + // dGeomGetBodyNext() returning 0 for the body, so we must get the next body + // before setting the body to 0. + dxGeom *next_geom = 0; + for (dxGeom *geom = b->geom; geom; geom = next_geom) { + next_geom = dGeomGetBodyNext (geom); + dGeomSetBody (geom,0); + } + + // detach all neighbouring joints, then delete this body. + dxJointNode *n = b->firstjoint; + while (n) { + // sneaky trick to speed up removal of joint references (black magic) + n->joint->node[(n == n->joint->node)].body = 0; + + dxJointNode *next = n->next; + n->next = 0; + removeJointReferencesFromAttachedBodies (n->joint); + n = next; + } + removeObjectFromList (b); + b->world->nb--; + delete b; +} + + +void dBodySetData (dBodyID b, void *data) +{ + dAASSERT (b); + b->userdata = data; +} + + +void *dBodyGetData (dBodyID b) +{ + dAASSERT (b); + return b->userdata; +} + + +void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->pos[0] = x; + b->pos[1] = y; + b->pos[2] = z; + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetRotation (dBodyID b, const dMatrix3 R) +{ + dAASSERT (b && R); + dQuaternion q; + dRtoQ (R,q); + dNormalize4 (q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetQuaternion (dBodyID b, const dQuaternion q) +{ + dAASSERT (b && q); + b->q[0] = q[0]; + b->q[1] = q[1]; + b->q[2] = q[2]; + b->q[3] = q[3]; + dNormalize4 (b->q); + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + + +void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->lvel[0] = x; + b->lvel[1] = y; + b->lvel[2] = z; +} + + +void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->avel[0] = x; + b->avel[1] = y; + b->avel[2] = z; +} + + +const dReal * dBodyGetPosition (dBodyID b) +{ + dAASSERT (b); + return b->pos; +} + + +const dReal * dBodyGetRotation (dBodyID b) +{ + dAASSERT (b); + return b->R; +} + + +const dReal * dBodyGetQuaternion (dBodyID b) +{ + dAASSERT (b); + return b->q; +} + + +const dReal * dBodyGetLinearVel (dBodyID b) +{ + dAASSERT (b); + return b->lvel; +} + + +const dReal * dBodyGetAngularVel (dBodyID b) +{ + dAASSERT (b); + return b->avel; +} + + +void dBodySetMass (dBodyID b, const dMass *mass) +{ + dAASSERT (b && mass); + memcpy (&b->mass,mass,sizeof(dMass)); + if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) { + dDEBUGMSG ("inertia must be positive definite"); + dRSetIdentity (b->invI); + } + b->invMass = dRecip(b->mass.mass); +} + + +void dBodyGetMass (dBodyID b, dMass *mass) +{ + dAASSERT (b && mass); + memcpy (mass,&b->mass,sizeof(dMass)); +} + + +void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; +} + + +void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + b->tacc[0] += fx; + b->tacc[1] += fy; + b->tacc[2] += fz; +} + + +void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->R,t1); + b->facc[0] += t2[0]; + b->facc[1] += t2[1]; + b->facc[2] += t2[2]; +} + + +void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz) +{ + dAASSERT (b); + dVector3 t1,t2; + t1[0] = fx; + t1[1] = fy; + t1[2] = fz; + t1[3] = 0; + dMULTIPLY0_331 (t2,b->R,t1); + b->tacc[0] += t2[0]; + b->tacc[1] += t2[1]; + b->tacc[2] += t2[2]; +} + + +void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + b->facc[0] += fx; + b->facc[1] += fy; + b->facc[2] += fz; + dVector3 f,q; + f[0] = fx; + f[1] = fy; + f[2] = fz; + q[0] = px - b->pos[0]; + q[1] = py - b->pos[1]; + q[2] = pz - b->pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 prel,f,p; + f[0] = fx; + f[1] = fy; + f[2] = fz; + f[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,f; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + dMULTIPLY0_331 (f,b->R,frel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dVector3 q; + q[0] = px - b->pos[0]; + q[1] = py - b->pos[1]; + q[2] = pz - b->pos[2]; + dCROSS (b->tacc,+=,q,f); +} + + +void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, + dReal px, dReal py, dReal pz) +{ + dAASSERT (b); + dVector3 frel,prel,f,p; + frel[0] = fx; + frel[1] = fy; + frel[2] = fz; + frel[3] = 0; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (f,b->R,frel); + dMULTIPLY0_331 (p,b->R,prel); + b->facc[0] += f[0]; + b->facc[1] += f[1]; + b->facc[2] += f[2]; + dCROSS (b->tacc,+=,p,f); +} + + +const dReal * dBodyGetForce (dBodyID b) +{ + dAASSERT (b); + return b->facc; +} + + +const dReal * dBodyGetTorque (dBodyID b) +{ + dAASSERT (b); + return b->tacc; +} + + +void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->facc[0] = x; + b->facc[1] = y; + b->facc[2] = z; +} + + +void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->tacc[0] = x; + b->tacc[1] = y; + b->tacc[2] = z; +} + + +void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + result[0] = p[0] + b->pos[0]; + result[1] = p[1] + b->pos[1]; + result[2] = p[2] + b->pos[2]; +} + + +void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel,p; + prel[0] = px; + prel[1] = py; + prel[2] = pz; + prel[3] = 0; + dMULTIPLY0_331 (p,b->R,prel); + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px - b->pos[0]; + p[1] = py - b->pos[1]; + p[2] = pz - b->pos[2]; + p[3] = 0; + result[0] = b->lvel[0]; + result[1] = b->lvel[1]; + result[2] = b->lvel[2]; + dCROSS (result,+=,b->avel,p); +} + + +void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 prel; + prel[0] = px - b->pos[0]; + prel[1] = py - b->pos[1]; + prel[2] = pz - b->pos[2]; + prel[3] = 0; + dMULTIPLY1_331 (result,b->R,prel); +} + + +void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY0_331 (result,b->R,p); +} + + +void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz, + dVector3 result) +{ + dAASSERT (b); + dVector3 p; + p[0] = px; + p[1] = py; + p[2] = pz; + p[3] = 0; + dMULTIPLY1_331 (result,b->R,p); +} + + +void dBodySetFiniteRotationMode (dBodyID b, int mode) +{ + dAASSERT (b); + b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis); + if (mode) { + b->flags |= dxBodyFlagFiniteRotation; + if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 || + b->finite_rot_axis[2] != 0) { + b->flags |= dxBodyFlagFiniteRotationAxis; + } + } +} + + +void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z) +{ + dAASSERT (b); + b->finite_rot_axis[0] = x; + b->finite_rot_axis[1] = y; + b->finite_rot_axis[2] = z; + if (x != 0 || y != 0 || z != 0) { + dNormalize3 (b->finite_rot_axis); + b->flags |= dxBodyFlagFiniteRotationAxis; + } + else { + b->flags &= ~dxBodyFlagFiniteRotationAxis; + } +} + + +int dBodyGetFiniteRotationMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyFlagFiniteRotation) != 0); +} + + +void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result) +{ + dAASSERT (b); + result[0] = b->finite_rot_axis[0]; + result[1] = b->finite_rot_axis[1]; + result[2] = b->finite_rot_axis[2]; +} + + +int dBodyGetNumJoints (dBodyID b) +{ + dAASSERT (b); + int count=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, count++); + return count; +} + + +dJointID dBodyGetJoint (dBodyID b, int index) +{ + dAASSERT (b); + int i=0; + for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) { + if (i == index) return n->joint; + } + return 0; +} + + +void dBodyEnable (dBodyID b) +{ + dAASSERT (b); + b->flags &= ~dxBodyDisabled; +} + + +void dBodyDisable (dBodyID b) +{ + dAASSERT (b); + b->flags |= dxBodyDisabled; +} + + +int dBodyIsEnabled (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyDisabled) == 0); +} + + +void dBodySetGravityMode (dBodyID b, int mode) +{ + dAASSERT (b); + if (mode) b->flags &= ~dxBodyNoGravity; + else b->flags |= dxBodyNoGravity; +} + + +int dBodyGetGravityMode (dBodyID b) +{ + dAASSERT (b); + return ((b->flags & dxBodyNoGravity) == 0); +} + +//**************************************************************************** +// joints + +static void dJointInit (dxWorld *w, dxJoint *j) +{ + dIASSERT (w && j); + initObject (j,w); + j->vtable = 0; + j->flags = 0; + j->node[0].joint = j; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].joint = j; + j->node[1].body = 0; + j->node[1].next = 0; + addObjectToList (j,(dObject **) &w->firstjoint); + w->nj++; +} + + +static dxJoint *createJoint (dWorldID w, dJointGroupID group, + dxJoint::Vtable *vtable) +{ + dIASSERT (w && vtable); + dxJoint *j; + if (group) { + j = (dxJoint*) group->stack.alloc (vtable->size); + group->num++; + } + else j = (dxJoint*) dAlloc (vtable->size); + dJointInit (w,j); + j->vtable = vtable; + if (group) j->flags |= dJOINT_INGROUP; + if (vtable->init) vtable->init (j); + j->feedback = 0; + /******************** breakable joint contribution ***********************/ + j->breakInfo = 0; + /*************************************************************************/ + return j; +} + + +dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dball_vtable); +} + + +dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge_vtable); +} + + +dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dslider_vtable); +} + + +dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group, + const dContact *c) +{ + dAASSERT (w && c); + dxJointContact *j = (dxJointContact *) + createJoint (w,group,&__dcontact_vtable); + j->contact = *c; + return j; +} + + +dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dhinge2_vtable); +} + + +dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__duniversal_vtable); +} + + +dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dfixed_vtable); +} + + +dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__dnull_vtable); +} + + +dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group) +{ + dAASSERT (w); + return createJoint (w,group,&__damotor_vtable); +} + + +void dJointDestroy (dxJoint *j) +{ + dAASSERT (j); + if (j->flags & dJOINT_INGROUP) return; + removeJointReferencesFromAttachedBodies (j); + removeObjectFromList (j); + /******************** breakable joint contribution ***********************/ + if (j->breakInfo) delete j->breakInfo; + /*************************************************************************/ + j->world->nj--; + dFree (j,j->vtable->size); +} + + +dJointGroupID dJointGroupCreate (int max_size) +{ + // not any more ... dUASSERT (max_size > 0,"max size must be > 0"); + dxJointGroup *group = new dxJointGroup; + group->num = 0; + return group; +} + + +void dJointGroupDestroy (dJointGroupID group) +{ + dAASSERT (group); + dJointGroupEmpty (group); + delete group; +} + + +void dJointGroupEmpty (dJointGroupID group) +{ + // the joints in this group are detached starting from the most recently + // added (at the top of the stack). this helps ensure that the various + // linked lists are not traversed too much, as the joints will hopefully + // be at the start of those lists. + // if any group joints have their world pointer set to 0, their world was + // previously destroyed. no special handling is required for these joints. + + dAASSERT (group); + int i; + dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*)); + dxJoint *j = (dxJoint*) group->stack.rewind(); + for (i=0; i < group->num; i++) { + jlist[i] = j; + j = (dxJoint*) (group->stack.next (j->vtable->size)); + } + for (i=group->num-1; i >= 0; i--) { + if (jlist[i]->world) { + removeJointReferencesFromAttachedBodies (jlist[i]); + removeObjectFromList (jlist[i]); + jlist[i]->world->nj--; + } + } + group->num = 0; + group->stack.freeAll(); +} + + +void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2) +{ + // check arguments + dUASSERT (joint,"bad joint argument"); + dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2"); + dxWorld *world = joint->world; + dUASSERT ( (!body1 || body1->world == world) && + (!body2 || body2->world == world), + "joint and bodies must be in same world"); + + // check if the joint can not be attached to just one body + dUASSERT (!((joint->flags & dJOINT_TWOBODIES) && + ((body1 != 0) ^ (body2 != 0))), + "joint can not be attached to just one body"); + + // remove any existing body attachments + if (joint->node[0].body || joint->node[1].body) { + removeJointReferencesFromAttachedBodies (joint); + } + + // if a body is zero, make sure that it is body2, so 0 --> node[1].body + if (body1==0) { + body1 = body2; + body2 = 0; + joint->flags |= dJOINT_REVERSE; + } + else { + joint->flags &= (~dJOINT_REVERSE); + } + + // attach to new bodies + joint->node[0].body = body1; + joint->node[1].body = body2; + if (body1) { + joint->node[1].next = body1->firstjoint; + body1->firstjoint = &joint->node[1]; + } + else joint->node[1].next = 0; + if (body2) { + joint->node[0].next = body2->firstjoint; + body2->firstjoint = &joint->node[0]; + } + else { + joint->node[0].next = 0; + } +} + + +void dJointSetData (dxJoint *joint, void *data) +{ + dAASSERT (joint); + joint->userdata = data; +} + + +void *dJointGetData (dxJoint *joint) +{ + dAASSERT (joint); + return joint->userdata; +} + + +int dJointGetType (dxJoint *joint) +{ + dAASSERT (joint); + return joint->vtable->typenum; +} + + +dBodyID dJointGetBody (dxJoint *joint, int index) +{ + dAASSERT (joint); + if (index >= 0 && index < 2) return joint->node[index].body; + else return 0; +} + + +void dJointSetFeedback (dxJoint *joint, dJointFeedback *f) +{ + dAASSERT (joint); + joint->feedback = f; +} + + +dJointFeedback *dJointGetFeedback (dxJoint *joint) +{ + dAASSERT (joint); + return joint->feedback; +} + + +int dAreConnected (dBodyID b1, dBodyID b2) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (n->body == b2) return 1; + } + return 0; +} + + +int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type) +{ + dAASSERT (b1 && b2); + // look through b1's neighbour list for b2 + for (dxJointNode *n=b1->firstjoint; n; n=n->next) { + if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1; + } + return 0; +} + +//**************************************************************************** +// world + +dxWorld * dWorldCreate() +{ + dxWorld *w = new dxWorld; + w->firstbody = 0; + w->firstjoint = 0; + w->nb = 0; + w->nj = 0; + dSetZero (w->gravity,4); + w->global_erp = REAL(0.2); +#if defined(dSINGLE) + w->global_cfm = 1e-5f; +#elif defined(dDOUBLE) + w->global_cfm = 1e-10; +#else + #error dSINGLE or dDOUBLE must be defined +#endif + return w; +} + + +void dWorldDestroy (dxWorld *w) +{ + // delete all bodies and joints + dAASSERT (w); + dxBody *nextb, *b = w->firstbody; + while (b) { + nextb = (dxBody*) b->next; + delete b; + b = nextb; + } + dxJoint *nextj, *j = w->firstjoint; + while (j) { + nextj = (dxJoint*)j->next; + if (j->flags & dJOINT_INGROUP) { + // the joint is part of a group, so "deactivate" it instead + j->world = 0; + j->node[0].body = 0; + j->node[0].next = 0; + j->node[1].body = 0; + j->node[1].next = 0; + dMessage (0,"warning: destroying world containing grouped joints"); + } + else { + dFree (j,j->vtable->size); + } + j = nextj; + } + delete w; +} + + +void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z) +{ + dAASSERT (w); + w->gravity[0] = x; + w->gravity[1] = y; + w->gravity[2] = z; +} + + +void dWorldGetGravity (dWorldID w, dVector3 g) +{ + dAASSERT (w); + g[0] = w->gravity[0]; + g[1] = w->gravity[1]; + g[2] = w->gravity[2]; +} + + +void dWorldSetERP (dWorldID w, dReal erp) +{ + dAASSERT (w); + w->global_erp = erp; +} + + +dReal dWorldGetERP (dWorldID w) +{ + dAASSERT (w); + return w->global_erp; +} + + +void dWorldSetCFM (dWorldID w, dReal cfm) +{ + dAASSERT (w); + w->global_cfm = cfm; +} + + +dReal dWorldGetCFM (dWorldID w) +{ + dAASSERT (w); + return w->global_cfm; +} + + +void dWorldStep (dWorldID w, dReal stepsize) +{ + dUASSERT (w,"bad world argument"); + dUASSERT (stepsize > 0,"stepsize must be > 0"); + processIslands (w,stepsize); +} + + +void dWorldImpulseToForce (dWorldID w, dReal stepsize, + dReal ix, dReal iy, dReal iz, + dVector3 force) +{ + dAASSERT (w); + stepsize = dRecip(stepsize); + force[0] = stepsize * ix; + force[1] = stepsize * iy; + force[2] = stepsize * iz; + // @@@ force[3] = 0; +} + +//**************************************************************************** +// testing + +#define NUM 100 + +#define DO(x) + + +extern "C" void dTestDataStructures() +{ + int i; + DO(printf ("testDynamicsStuff()\n")); + + dBodyID body [NUM]; + int nb = 0; + dJointID joint [NUM]; + int nj = 0; + + for (i=0; i 0.5) { + DO(printf ("creating body\n")); + body[nb] = dBodyCreate (w); + DO(printf ("\t--> %p\n",body[nb])); + nb++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj < NUM && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + DO(printf ("creating joint, attaching to %p,%p\n",b1,b2)); + joint[nj] = dJointCreateBall (w,0); + DO(printf ("\t-->%p\n",joint[nj])); + checkWorld (w); + dJointAttach (joint[nj],b1,b2); + nj++; + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nj > 0 && nb > 2 && dRandReal() > 0.5) { + dBodyID b1 = body [dRand() % nb]; + dBodyID b2 = body [dRand() % nb]; + if (b1 != b2) { + int k = dRand() % nj; + DO(printf ("reattaching joint %p\n",joint[k])); + dJointAttach (joint[k],b1,b2); + checkWorld (w); + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + if (nb > 0 && dRandReal() > 0.5) { + int k = dRand() % nb; + DO(printf ("destroying body %p\n",body[k])); + dBodyDestroy (body[k]); + checkWorld (w); + for (; k < (NUM-1); k++) body[k] = body[k+1]; + nb--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + if (nj > 0 && dRandReal() > 0.5) { + int k = dRand() % nj; + DO(printf ("destroying joint %p\n",joint[k])); + dJointDestroy (joint[k]); + checkWorld (w); + for (; k < (NUM-1); k++) joint[k] = joint[k+1]; + nj--; + DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); + } + } + + /* + printf ("creating world\n"); + dWorldID w = dWorldCreate(); + checkWorld (w); + printf ("creating body\n"); + dBodyID b1 = dBodyCreate (w); + checkWorld (w); + printf ("creating body\n"); + dBodyID b2 = dBodyCreate (w); + checkWorld (w); + printf ("creating joint\n"); + dJointID j = dJointCreateBall (w); + checkWorld (w); + printf ("attaching joint\n"); + dJointAttach (j,b1,b2); + checkWorld (w); + printf ("destroying joint\n"); + dJointDestroy (j); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b1); + checkWorld (w); + printf ("destroying body\n"); + dBodyDestroy (b2); + checkWorld (w); + printf ("destroying world\n"); + dWorldDestroy (w); + */ +} diff --git a/libraries/ode-0.9/contrib/BreakableJoints/step.cpp b/libraries/ode-0.9/contrib/BreakableJoints/step.cpp new file mode 100644 index 0000000..38aed6c --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/step.cpp @@ -0,0 +1,1170 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include "objects.h" +#include "joint.h" +#include +#include +#include +#include +#include +#include +#include "lcp.h" + +//**************************************************************************** +// misc defines + +#define FAST_FACTOR +//#define TIMING + +#define ALLOCA dALLOCA16 + +//**************************************************************************** +// debugging - comparison of various vectors and matrices produced by the +// slow and fast versions of the stepper. + +//#define COMPARE_METHODS + +#ifdef COMPARE_METHODS +#include "testing.h" +dMatrixComparison comparator; +#endif + +//**************************************************************************** +// special matrix multipliers + +// this assumes the 4th and 8th rows of B and C are zero. + +static void Multiply2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) = sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B and C are zero. + +static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C, + int p, int r, int Askip) +{ + int i,j; + dReal sum,*bb,*cc; + dIASSERT (p>0 && r>0 && A && B && C); + bb = B; + for (i=p; i; i--) { + cc = C; + for (j=r; j; j--) { + sum = bb[0]*cc[0]; + sum += bb[1]*cc[1]; + sum += bb[2]*cc[2]; + sum += bb[4]*cc[4]; + sum += bb[5]*cc[5]; + sum += bb[6]*cc[6]; + *(A++) += sum; + cc += 8; + } + A += Askip - r; + bb += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p) +{ + int i; + dIASSERT (p>0 && A && B && C); + dReal sum; + for (i=p; i; i--) { + sum = B[0]*C[0]; + sum += B[1]*C[1]; + sum += B[2]*C[2]; + sum += B[4]*C[4]; + sum += B[5]*C[5]; + sum += B[6]*C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q) +{ + int k; + dReal sum; + dIASSERT (q>0 && A && B && C); + sum = 0; + for (k=0; k0 && A && B && C); + sum = 0; + for (k=0; kpos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv,irv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis,b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL(0.5); + dReal theta = k * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + + b->avel[2]*b->avel[2]); + h *= REAL(0.5); + dReal theta = wlen * h; + q[0] = dCos(theta); + dReal s = sinc(theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2,q,b->q); + for (j=0; j<4; j++) b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) { + dReal dq[4]; + dWtoDQ (irv,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + } + } + else { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel,b->q,dq); + for (j=0; j<4; j++) b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q,b->R); + + // notify all attached geoms that this body has moved + for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +// the slow, but sure way +// note that this does not do any joint feedback! + +// given lists of bodies and joints that form an island, perform a first +// order timestep. +// +// `body' is the body array, `nb' is the size of the array. +// `_joint' is the body array, `nj' is the size of the array. + +void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j,k; + int n6 = 6*nb; + +# ifdef TIMING + dTimerStart("preprocessing"); +# endif + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*)); + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. + // @@@ check computation of rotational force. + dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; imass.I,body[i]->R); + dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); + dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + int m = 0; + dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1)); + int *ofs = (int*) ALLOCA (nj*sizeof(int)); + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; iinvMass; + MM[nskip+1] = body[i]->invMass; + MM[2*nskip+2] = body[i]->invMass; + MM += 3*nskip+3; + for (j=0; j<3; j++) for (k=0; k<3; k++) { + MM[j*nskip+k] = invI[i*12+j*4+k]; + } + } + + // assemble some body vectors: fe = external forces, v = velocities + dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal)); + dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal)); + //dSetZero (fe,n6); + //dSetZero (v,n6); + for (i=0; ifacc[j]; + for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j]; + for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j]; + for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j]; + } + + // this will be set to the velocity update + dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal)); + dSetZero (vnew,n6); + + // if there are constraints, compute cforce + if (m > 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dReal *c = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal)); + int *findex = (int*) alloca (m*sizeof(int)); + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; inode[0].body->tag; + Jinfo.J1a = Jinfo.J1l + 3; + if (joint[i]->node[1].body) { + Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag; + Jinfo.J2a = Jinfo.J2l + 3; + } + else { + Jinfo.J2l = 0; + Jinfo.J2a = 0; + } + Jinfo.c = c + ofs[i]; + Jinfo.cfm = cfm + ofs[i]; + Jinfo.lo = lo + ofs[i]; + Jinfo.hi = hi + ofs[i]; + Jinfo.findex = findex + ofs[i]; + joint[i]->vtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J' +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal)); + //dSetZero (JinvM,m*nskip); + dMultiply0 (JinvM,J,invM,m,n6,n6); + int mskip = dPAD(m); + dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); + //dSetZero (A,m*mskip); + dMultiply2 (A,JinvM,J,m,n6,m); + + // add cfm to the diagonal of A + for (i=0; ilvel[j] = vnew[i*6+j]; + for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j]; + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +# ifdef TIMING + dTimerNow ("update position"); +# endif + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +# ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +# endif +} + +//**************************************************************************** +// an optimized version of dInternalStepIsland1() + +void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *_joint, int nj, dReal stepsize) +{ + int i,j,k; +# ifdef TIMING + dTimerStart("preprocessing"); +# endif + + dReal stepsize1 = dRecip(stepsize); + + // number all bodies in the body list - set their tag values + for (i=0; itag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*)); + memcpy (joint,_joint,nj * sizeof(dxJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. + dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); + + //dSetZero (I,3*nb*4); + //dSetZero (invI,3*nb*4); + for (i=0; imass.I,body[i]->R); + dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); + dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); + dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); + } + + // add the gravity force to all bodies + for (i=0; iflags & dxBodyNoGravity)==0) { + body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; + body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; + body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; + } + } + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1)); + int *ofs = (int*) ALLOCA (nj*sizeof(int)); + for (i=0, j=0; jvtable->getInfo1 (joint[j],info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && + info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + joint[i]->tag = i; + i++; + } + else { + joint[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i=0; i 0 && info[i].nub < info[i].m) { + ofs[i] = m; + m += info[i].m; + } + // the purely LCP constraints + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dReal *c = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal)); + dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal)); + int *findex = (int*) alloca (m*sizeof(int)); + dSetZero (c,m); + dSetValue (cfm,m,world->global_cfm); + dSetValue (lo,m,-dInfinity); + dSetValue (hi,m, dInfinity); + for (i=0; iglobal_erp; + for (i=0; ivtable->getInfo2 (joint[i],&Jinfo); + // adjust returned findex values for global index numbering + for (j=0; j= 0) findex[ofs[i] + j] += ofs[i]; + } + } + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal)); + dSetZero (JinvM,2*m*8); + for (i=0; inode[0].body->tag; + dReal body_invMass = body[b]->invMass; + dReal *body_invI = invI + b*12; + dReal *Jsrc = J + 2*8*ofs[i]; + dReal *Jdst = JinvM + 2*8*ofs[i]; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + if (joint[i]->node[1].body) { + b = joint[i]->node[1].body->tag; + body_invMass = body[b]->invMass; + body_invI = invI + b*12; + for (j=info[i].m-1; j>=0; j--) { + for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; + dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); + Jsrc += 8; + Jdst += 8; + } + } + } + + // now compute A = JinvM * J'. A's rows and columns are grouped by joint, + // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero + // if joints i and j have at least one body in common. this fact suggests + // the algorithm used to fill A: + // + // for b = all bodies + // n = number of joints attached to body b + // for i = 1..n + // for j = i+1..n + // ii = actual joint number for i + // jj = actual joint number for j + // // (ii,jj) will be set to all pairs of joints around body b + // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)' + // + // this algorithm catches all pairs of joints that have at least one body + // in common. it does not compute the diagonal blocks of A however - + // another similar algorithm does that. + + int mskip = dPAD(m); + dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); + dSetZero (A,m*mskip); + for (i=0; ifirstjoint; n1; n1=n1->next) { + for (dxJointNode *n2=n1->next; n2; n2=n2->next) { + // get joint numbers and ensure ofs[j1] >= ofs[j2] + int j1 = n1->joint->tag; + int j2 = n2->joint->tag; + if (ofs[j1] < ofs[j2]) { + int tmp = j1; + j1 = j2; + j2 = tmp; + } + + // if either joint was tagged as -1 then it is an inactive (m=0) + // joint that should not be considered + if (j1==-1 || j2==-1) continue; + + // determine if body i is the 1st or 2nd body of joints j1 and j2 + int jb1 = (joint[j1]->node[1].body == body[i]); + int jb2 = (joint[j2]->node[1].body == body[i]); + // jb1/jb2 must be 0 for joints with only one body + dIASSERT(joint[j1]->node[1].body || jb1==0); + dIASSERT(joint[j2]->node[1].body || jb2==0); + + // set block of A + MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2], + JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m, + J + 2*8*ofs[j2] + jb2*8*info[j2].m, + info[j1].m,info[j2].m, mskip); + } + } + } + // compute diagonal blocks of A + for (i=0; inode[1].body) { + MultiplyAdd2_p8r (A + ofs[i]*(mskip+1), + JinvM + 2*8*ofs[i] + 8*info[i].m, + J + 2*8*ofs[i] + 8*info[i].m, + info[i].m,info[i].m, mskip); + } + } + + // add cfm to the diagonal of A + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass + + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc); + for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal)); + //dSetZero (rhs,m); + for (i=0; inode[0].body->tag, info[i].m); + if (joint[i]->node[1].body) { + MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m, + tmp1 + 8*joint[i]->node[1].body->tag, info[i].m); + } + } + // complete rhs + for (i=0; inode[0].body; + dxBody* b2 = joint[i]->node[1].body; + dJointFeedback *fb = joint[i]->feedback; + +/******************** breakable joint contribution ***********************/ + // this saves us a few dereferences + dxJointBreakInfo *jBI = joint[i]->breakInfo; + // we need joint feedback if the joint is breakable or if the user + // requested feedback. + if (jBI||fb) { + // we need feedback on the amount of force that this joint is + // applying to the bodies. we use a slightly slower computation + // that splits out the force components and puts them in the + // feedback structure. + dJointFeedback temp_fb; // temporary storage for joint feedback + dReal data1[8],data2[8]; + Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); + dReal *cf1 = cforce + 8*b1->tag; + cf1[0] += (temp_fb.f1[0] = data1[0]); + cf1[1] += (temp_fb.f1[1] = data1[1]); + cf1[2] += (temp_fb.f1[2] = data1[2]); + cf1[4] += (temp_fb.t1[0] = data1[4]); + cf1[5] += (temp_fb.t1[1] = data1[5]); + cf1[6] += (temp_fb.t1[2] = data1[6]); + if (b2) { + Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + dReal *cf2 = cforce + 8*b2->tag; + cf2[0] += (temp_fb.f2[0] = data2[0]); + cf2[1] += (temp_fb.f2[1] = data2[1]); + cf2[2] += (temp_fb.f2[2] = data2[2]); + cf2[4] += (temp_fb.t2[0] = data2[4]); + cf2[5] += (temp_fb.t2[1] = data2[5]); + cf2[6] += (temp_fb.t2[2] = data2[6]); + } + // if the user requested so we must copy the feedback information to + // the feedback struct that the user suplied. + if (fb) { + // copy temp_fb to fb + fb->f1[0] = temp_fb.f1[0]; + fb->f1[1] = temp_fb.f1[1]; + fb->f1[2] = temp_fb.f1[2]; + fb->t1[0] = temp_fb.t1[0]; + fb->t1[1] = temp_fb.t1[1]; + fb->t1[2] = temp_fb.t1[2]; + if (b2) { + fb->f2[0] = temp_fb.f2[0]; + fb->f2[1] = temp_fb.f2[1]; + fb->f2[2] = temp_fb.f2[2]; + fb->t2[0] = temp_fb.t2[0]; + fb->t2[1] = temp_fb.t2[1]; + fb->t2[2] = temp_fb.t2[2]; + } + } + // if the joint is breakable we need to check the breaking conditions + if (jBI) { + dReal relCF1[3]; + dReal relCT1[3]; + // multiply the force and torque vectors by the rotation matrix of body 1 + dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]); + dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]); + if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF1[i] > jBI->b1MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT1[i] > jBI->b1MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (b2) { + dReal relCF2[3]; + dReal relCT2[3]; + // multiply the force and torque vectors by the rotation matrix of body 2 + dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]); + dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]); + if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF2[i] > jBI->b2MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT2[i] > jBI->b2MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + } + doneCheckingBreaks: + ; + } + } +/*************************************************************************/ + else { + // no feedback is required, let's compute cforce the faster way + MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m); + if (b2) { + MultiplyAdd1_8q1 (cforce + 8*b2->tag, + JJ + 8*info[i].m, lambda+ofs[i], info[i].m); + } + } + } + } + + // compute the velocity update +# ifdef TIMING + dTimerNow ("compute velocity update"); +# endif + + // add fe to cforce + for (i=0; ifacc[j]; + for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j]; + } + // multiply cforce by stepsize + for (i=0; i < nb*8; i++) cforce[i] *= stepsize; + // add invM * cforce to the body velocity + for (i=0; iinvMass; + dReal *body_invI = invI + i*12; + for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j]; + dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4); + } + + // update the position and orientation from the new linear/angular velocity + // (over the given timestep) +# ifdef TIMING + dTimerNow ("update position"); +# endif + for (i=0; ilvel[j]; + for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j]; + } + comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew"); +# endif + +# ifdef TIMING + dTimerNow ("tidy up"); +# endif + + // zero all force accumulators + for (i=0; ifacc[0] = 0; + body[i]->facc[1] = 0; + body[i]->facc[2] = 0; + body[i]->facc[3] = 0; + body[i]->tacc[0] = 0; + body[i]->tacc[1] = 0; + body[i]->tacc[2] = 0; + body[i]->tacc[3] = 0; + } + +# ifdef TIMING + dTimerEnd(); + if (m > 0) dTimerReport (stdout,1); +# endif +} + +//**************************************************************************** + +void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb, + dxJoint * const *joint, int nj, dReal stepsize) +{ +# ifndef COMPARE_METHODS + dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize); +# endif + +# ifdef COMPARE_METHODS + int i; + + // save body state + dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody)); + for (i=0; i +#include +#include +#include +#include +#include +#include +#include "lcp.h" +#include "step.h" + + +// misc defines + +#define ALLOCA dALLOCA16 + +#define RANDOM_JOINT_ORDER +//#define FAST_FACTOR //use a factorization approximation to the LCP solver (fast, theoretically less accurate) +#define SLOW_LCP //use the old LCP solver +//#define NO_ISLANDS //does not perform island creation code (3~4% of simulation time), body disabling doesn't work +//#define TIMING + + +static int autoEnableDepth = 2; + +void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth) +{ + if (autodepth > 0) + autoEnableDepth = autodepth; + else + autoEnableDepth = 0; +} + +int dWorldGetAutoEnableDepthSF1 (dxWorld *world) +{ + return autoEnableDepth; +} + +//little bit of math.... the _sym_ functions assume the return matrix will be symmetric +static void +Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) = *ad = sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + +static void +MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) +{ + int i, j; + dReal sum, *aa, *ad, *bb, *cc; + dIASSERT (p > 0 && A && B && C); + bb = B; + for (i = 0; i < p; i++) + { + //aa is going accross the matrix, ad down + aa = ad = A; + cc = C; + for (j = i; j < p; j++) + { + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + *(aa++) += sum; + *ad += sum; + ad += Askip; + cc += 8; + } + bb += 8; + A += Askip + 1; + C += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) = sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p) +{ + int i; + dIASSERT (p > 0 && A && B && C); + dReal sum; + for (i = p; i; i--) + { + sum = B[0] * C[0]; + sum += B[1] * C[1]; + sum += B[2] * C[2]; + sum += B[4] * C[4]; + sum += B[5] * C[5]; + sum += B[6] * C[6]; + *(A++) += sum; + B += 8; + } +} + + +// this assumes the 4th and 8th rows of B are zero. + +static void +Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q) +{ + int k; + dReal sum; + dIASSERT (q > 0 && A && B && C); + sum = 0; + for (k = 0; k < q; k++) + sum += B[k * 8] * C[k]; + A[0] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[1 + k * 8] * C[k]; + A[1] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[2 + k * 8] * C[k]; + A[2] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[4 + k * 8] * C[k]; + A[4] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[5 + k * 8] * C[k]; + A[5] = sum; + sum = 0; + for (k = 0; k < q; k++) + sum += B[6 + k * 8] * C[k]; + A[6] = sum; +} + +//**************************************************************************** +// body rotation + +// return sin(x)/x. this has a singularity at 0 so special handling is needed +// for small arguments. + +static inline dReal +sinc (dReal x) +{ + // if |x| < 1e-4 then use a taylor series expansion. this two term expansion + // is actually accurate to one LS bit within this range if double precision + // is being used - so don't worry! + if (dFabs (x) < 1.0e-4) + return REAL (1.0) - x * x * REAL (0.166666666666666666667); + else + return dSin (x) / x; +} + + +// given a body b, apply its linear and angular rotation over the time +// interval h, thereby adjusting its position and orientation. + +static inline void +moveAndRotateBody (dxBody * b, dReal h) +{ + int j; + + // handle linear velocity + for (j = 0; j < 3; j++) + b->pos[j] += h * b->lvel[j]; + + if (b->flags & dxBodyFlagFiniteRotation) + { + dVector3 irv; // infitesimal rotation vector + dQuaternion q; // quaternion for finite rotation + + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + // split the angular velocity vector into a component along the finite + // rotation axis, and a component orthogonal to it. + dVector3 frv, irv; // finite rotation vector + dReal k = dDOT (b->finite_rot_axis, b->avel); + frv[0] = b->finite_rot_axis[0] * k; + frv[1] = b->finite_rot_axis[1] * k; + frv[2] = b->finite_rot_axis[2] * k; + irv[0] = b->avel[0] - frv[0]; + irv[1] = b->avel[1] - frv[1]; + irv[2] = b->avel[2] - frv[2]; + + // make a rotation quaternion q that corresponds to frv * h. + // compare this with the full-finite-rotation case below. + h *= REAL (0.5); + dReal theta = k * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = frv[0] * s; + q[2] = frv[1] * s; + q[3] = frv[2] * s; + } + else + { + // make a rotation quaternion q that corresponds to w * h + dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]); + h *= REAL (0.5); + dReal theta = wlen * h; + q[0] = dCos (theta); + dReal s = sinc (theta) * h; + q[1] = b->avel[0] * s; + q[2] = b->avel[1] * s; + q[3] = b->avel[2] * s; + } + + // do the finite rotation + dQuaternion q2; + dQMultiply0 (q2, q, b->q); + for (j = 0; j < 4; j++) + b->q[j] = q2[j]; + + // do the infitesimal rotation if required + if (b->flags & dxBodyFlagFiniteRotationAxis) + { + dReal dq[4]; + dWtoDQ (irv, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + } + else + { + // the normal way - do an infitesimal rotation + dReal dq[4]; + dWtoDQ (b->avel, b->q, dq); + for (j = 0; j < 4; j++) + b->q[j] += h * dq[j]; + } + + // normalize the quaternion and convert it to a rotation matrix + dNormalize4 (b->q); + dQtoR (b->q, b->R); + + // notify all attached geoms that this body has moved + for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) + dGeomMoved (geom); +} + +//**************************************************************************** +//This is an implementation of the iterated/relaxation algorithm. +//Here is a quick overview of the algorithm per Sergi Valverde's posts to the +//mailing list: +// +// for i=0..N-1 do +// for c = 0..C-1 do +// Solve constraint c-th +// Apply forces to constraint bodies +// next +// next +// Integrate bodies + +void +dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) +{ + int i, j, k; +# ifdef TIMING + dTimerNow ("constraint preprocessing"); +# endif + + dReal stepsize1 = dRecip (stepsize); + + int m = info.m; + // nothing to do if no constraints. + if (m <= 0) + return; + + int nub = 0; + if (info.nub == info.m) + nub = m; + + // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same + // format as J so we just go through the constraints in J multiplying by + // the appropriate scalars and matrices. +# ifdef TIMING + dTimerNow ("compute A"); +# endif + dReal JinvM[2 * 6 * 8]; + //dSetZero (JinvM, 2 * m * 8); + + dReal *Jsrc = Jinfo.J1l; + dReal *Jdst = JinvM; + if (body[0]) + { + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[0]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); + Jsrc += 8; + Jdst += 8; + } + } + if (body[1]) + { + Jsrc = Jinfo.J2l; + Jdst = JinvM + 8 * m; + for (j = m - 1; j >= 0; j--) + { + for (k = 0; k < 3; k++) + Jdst[k] = Jsrc[k] * body[1]->invMass; + dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); + Jsrc += 8; + Jdst += 8; + } + } + + + // now compute A = JinvM * J'. + int mskip = dPAD (m); + dReal A[6 * 8]; + //dSetZero (A, 6 * 8); + + if (body[0]) + Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip); + if (body[1]) + MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, m, mskip); + + // add cfm to the diagonal of A + for (i = 0; i < m; i++) + A[i * mskip + i] += Jinfo.cfm[i] * stepsize1; + + // compute the right hand side `rhs' +# ifdef TIMING + dTimerNow ("compute rhs"); +# endif + dReal tmp1[16]; + //dSetZero (tmp1, 16); + // put v/h + invM*fe into tmp1 + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1; + dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc); + for (j = 0; j < 3; j++) + tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1; + } + // put J*tmp1 into rhs + dReal rhs[6]; + //dSetZero (rhs, 6); + + if (body[0]) + Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m); + if (body[1]) + MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); + + // complete rhs + for (i = 0; i < m; i++) + rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i]; + +#ifdef SLOW_LCP + // solve the LCP problem and get lambda. + // this will destroy A but that's okay +# ifdef TIMING + dTimerNow ("solving LCP problem"); +# endif + dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal)); + dReal lo[6], hi[6]; + memcpy (lo, Jinfo.lo, m * sizeof (dReal)); + memcpy (hi, Jinfo.hi, m * sizeof (dReal)); + dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); +#endif + + // LCP Solver replacement: + // This algorithm goes like this: + // Do a straightforward LDLT factorization of the matrix A, solving for + // A*x = rhs + // For each x[i] that is outside of the bounds of lo[i] and hi[i], + // clamp x[i] into that range. + // Substitute into A the now known x's + // subtract the residual away from the rhs. + // Remove row and column i from L, updating the factorization + // place the known x's at the end of the array, keeping up with location in p + // Repeat until all constraints have been clamped or all are within bounds + // + // This is probably only faster in the single joint case where only one repeat is + // the norm. + +#ifdef FAST_FACTOR + // factorize A (L*D*L'=A) +# ifdef TIMING + dTimerNow ("factorize A"); +# endif + dReal d[6]; + dReal L[6 * 8]; + memcpy (L, A, m * mskip * sizeof (dReal)); + dFactorLDLT (L, d, m, mskip); + + // compute lambda +# ifdef TIMING + dTimerNow ("compute lambda"); +# endif + + int left = m; //constraints left to solve. + int remove[6]; + dReal lambda[6]; + dReal x[6]; + int p[6]; + for (i = 0; i < 6; i++) + p[i] = i; + while (true) + { + memcpy (x, rhs, left * sizeof (dReal)); + dSolveLDLT (L, d, x, left, mskip); + + int fixed = 0; + for (i = 0; i < left; i++) + { + j = p[i]; + remove[i] = false; + // This isn't the exact same use of findex as dSolveLCP.... since x[findex] + // may change after I've already clamped x[i], but it should be close + if (Jinfo.findex[j] > -1) + { + dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]); + if (x[i] > f) + x[i] = f; + else if (x[i] < -f) + x[i] = -f; + else + continue; + } + else + { + if (x[i] > Jinfo.hi[j]) + x[i] = Jinfo.hi[j]; + else if (x[i] < Jinfo.lo[j]) + x[i] = Jinfo.lo[j]; + else + continue; + } + remove[i] = true; + fixed++; + } + if (fixed == 0 || fixed == left) //no change or all constraints solved + break; + + for (i = 0; i < left; i++) //sub in to right hand side. + if (remove[i]) + for (j = 0; j < left; j++) + if (!remove[j]) + rhs[j] -= A[j * mskip + i] * x[i]; + + for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables + { + if (remove[r]) + { + //dRemoveLDLT adapted for use without row pointers. + if (r == left - 1) + { + left--; + continue; // deleting last row/col is easy + } + else if (r == 0) + { + dReal a[6]; + for (i = 0; i < left; i++) + a[i] = -A[i * mskip]; + a[0] += REAL (1.0); + dLDLTAddTL (L, d, a, left, mskip); + } + else + { + dReal t[6]; + dReal a[6]; + for (i = 0; i < r; i++) + t[i] = L[r * mskip + i] / d[i]; + for (i = 0; i < left - r; i++) + a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r]; + a[0] += REAL (1.0); + dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip); + } + + dRemoveRowCol (L, left, mskip, r); + //end dRemoveLDLT + + left--; + if (r < (left - 1)) + { + dReal tx = x[r]; + memmove (d + r, d + r + 1, (left - r) * sizeof (dReal)); + memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal)); + //x will get written over by rhs anyway, no need to move it around + //just store the fixed value we just discovered in it. + x[left] = tx; + for (i = 0; i < m; i++) + if (p[i] > r && p[i] <= left) + p[i]--; + p[r] = left; + } + } + } + } + + for (i = 0; i < m; i++) + lambda[i] = x[p[i]]; +# endif + // compute the constraint force `cforce' +# ifdef TIMING + dTimerNow ("compute constraint force"); +#endif + + // compute cforce = J'*lambda + dJointFeedback *fb = joint->feedback; + dReal cforce[16]; + //dSetZero (cforce, 16); + +/******************** breakable joint contribution ***********************/ + // this saves us a few dereferences + dxJointBreakInfo *jBI = joint->breakInfo; + // we need joint feedback if the joint is breakable or if the user + // requested feedback. + if (jBI||fb) { + // we need feedback on the amount of force that this joint is + // applying to the bodies. we use a slightly slower computation + // that splits out the force components and puts them in the + // feedback structure. + dJointFeedback temp_fb; // temporary storage for joint feedback + dReal data1[8],data2[8]; + if (body[0]) + { + Multiply1_8q1 (data1, Jinfo.J1l, lambda, m); + dReal *cf1 = cforce; + cf1[0] = (temp_fb.f1[0] = data1[0]); + cf1[1] = (temp_fb.f1[1] = data1[1]); + cf1[2] = (temp_fb.f1[2] = data1[2]); + cf1[4] = (temp_fb.t1[0] = data1[4]); + cf1[5] = (temp_fb.t1[1] = data1[5]); + cf1[6] = (temp_fb.t1[2] = data1[6]); + } + if (body[1]) + { + Multiply1_8q1 (data2, Jinfo.J2l, lambda, m); + dReal *cf2 = cforce + 8; + cf2[0] = (temp_fb.f2[0] = data2[0]); + cf2[1] = (temp_fb.f2[1] = data2[1]); + cf2[2] = (temp_fb.f2[2] = data2[2]); + cf2[4] = (temp_fb.t2[0] = data2[4]); + cf2[5] = (temp_fb.t2[1] = data2[5]); + cf2[6] = (temp_fb.t2[2] = data2[6]); + } + // if the user requested so we must copy the feedback information to + // the feedback struct that the user suplied. + if (fb) { + // copy temp_fb to fb + fb->f1[0] = temp_fb.f1[0]; + fb->f1[1] = temp_fb.f1[1]; + fb->f1[2] = temp_fb.f1[2]; + fb->t1[0] = temp_fb.t1[0]; + fb->t1[1] = temp_fb.t1[1]; + fb->t1[2] = temp_fb.t1[2]; + if (body[1]) { + fb->f2[0] = temp_fb.f2[0]; + fb->f2[1] = temp_fb.f2[1]; + fb->f2[2] = temp_fb.f2[2]; + fb->t2[0] = temp_fb.t2[0]; + fb->t2[1] = temp_fb.t2[1]; + fb->t2[2] = temp_fb.t2[2]; + } + } + // if the joint is breakable we need to check the breaking conditions + if (jBI) { + dReal relCF1[3]; + dReal relCT1[3]; + // multiply the force and torque vectors by the rotation matrix of body 1 + dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]); + dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]); + if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF1[i] > jBI->b1MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT1[i] > jBI->b1MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (body[1]) { + dReal relCF2[3]; + dReal relCT2[3]; + // multiply the force and torque vectors by the rotation matrix of body 2 + dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]); + dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]); + if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { + // check if the force is to high + for (int i = 0; i < 3; i++) { + if (relCF2[i] > jBI->b2MaxF[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { + // check if the torque is to high + for (int i = 0; i < 3; i++) { + if (relCT2[i] > jBI->b2MaxT[i]) { + jBI->flags |= dJOINT_BROKEN; + goto doneCheckingBreaks; + } + } + } + } + doneCheckingBreaks: + ; + } + } +/*************************************************************************/ + else + { + // no feedback is required, let's compute cforce the faster way + if (body[0]) + Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m); + if (body[1]) + Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m); + } + + for (i = 0; i < 2; i++) + { + if (!body[i]) + continue; + for (j = 0; j < 3; j++) + { + body[i]->facc[j] += cforce[i * 8 + j]; + body[i]->tacc[j] += cforce[i * 8 + 4 + j]; + } + } +} + +void +dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations) +{ +# ifdef TIMING + dTimerNow ("preprocessing"); +# endif + dxBody *bodyPair[2], *body; + dReal *GIPair[2], *GinvIPair[2]; + dxJoint *joint; + int iter, b, j, i; + dReal ministep = stepsize / maxiterations; + + // make a local copy of the joint array, because we might want to modify it. + // (the "dxJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); + memcpy (joints, _joints, nj * sizeof (dxJoint *)); + + // get m = total constraint dimension, nub = number of unbounded variables. + // create constraint offset array and number-of-rows array for all joints. + // the constraints are re-ordered as follows: the purely unbounded + // constraints, the mixed unbounded + LCP constraints, and last the purely + // LCP constraints. this assists the LCP solver to put all unbounded + // variables at the start for a quick factorization. + // + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + // also number all active joints in the joint list (set their tag values). + // inactive joints receive a tag value of -1. + + int m = 0; + dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); + int *ofs = (int *) ALLOCA (nj * sizeof (int)); + for (i = 0, j = 0; j < nj; j++) + { // i=dest, j=src + joints[j]->vtable->getInfo1 (joints[j], info + i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) + { + joints[i] = joints[j]; + joints[i]->tag = i; + i++; + } + else + { + joints[j]->tag = -1; + } + } + nj = i; + + // the purely unbounded constraints + for (i = 0; i < nj; i++) + { + ofs[i] = m; + m += info[i].m; + } + dReal *c = NULL; + dReal *cfm = NULL; + dReal *lo = NULL; + dReal *hi = NULL; + int *findex = NULL; + + dReal *J = NULL; + dxJoint::Info2 * Jinfo = NULL; + + if (m) + { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + c = (dReal *) ALLOCA (m * sizeof (dReal)); + cfm = (dReal *) ALLOCA (m * sizeof (dReal)); + lo = (dReal *) ALLOCA (m * sizeof (dReal)); + hi = (dReal *) ALLOCA (m * sizeof (dReal)); + findex = (int *) ALLOCA (m * sizeof (int)); + dSetZero (c, m); + dSetValue (cfm, m, world->global_cfm); + dSetValue (lo, m, -dInfinity); + dSetValue (hi, m, dInfinity); + for (i = 0; i < m; i++) + findex[i] = -1; + + // get jacobian data from constraints. a (2*m)x8 matrix will be created + // to store the two jacobian blocks from each constraint. it has this + // format: + // + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) + // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) + // etc... + // + // (lll) = linear jacobian data + // (aaa) = angular jacobian data + // +# ifdef TIMING + dTimerNow ("create J"); +# endif + J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); + dSetZero (J, 2 * m * 8); + Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); + for (i = 0; i < nj; i++) + { + Jinfo[i].rowskip = 8; + Jinfo[i].fps = dRecip (stepsize); + Jinfo[i].erp = world->global_erp; + Jinfo[i].J1l = J + 2 * 8 * ofs[i]; + Jinfo[i].J1a = Jinfo[i].J1l + 4; + Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; + Jinfo[i].J2a = Jinfo[i].J2l + 4; + Jinfo[i].c = c + ofs[i]; + Jinfo[i].cfm = cfm + ofs[i]; + Jinfo[i].lo = lo + ofs[i]; + Jinfo[i].hi = hi + ofs[i]; + Jinfo[i].findex = findex + ofs[i]; + //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); + } + + } + + dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); + dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); + for (b = 0; b < nb; b++) + { + for (i = 0; i < 4; i++) + { + saveFacc[b * 4 + i] = bodies[b]->facc[i]; + saveTacc[b * 4 + i] = bodies[b]->tacc[i]; + bodies[b]->tag = b; + } + } + + for (iter = 0; iter < maxiterations; iter++) + { +# ifdef TIMING + dTimerNow ("applying inertia and gravity"); +# endif + dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. + // @@@ check computation of rotational force. + + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->mass.I, body->R); + dMULTIPLY0_333 (globalI + b * 12, body->R, tmp); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp, body->invI, body->R); + dMULTIPLY0_333 (globalInvI + b * 12, body->R, tmp); + + for (i = 0; i < 4; i++) + body->tacc[i] = saveTacc[b * 4 + i]; + // compute rotational force + dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel); + dCROSS (body->tacc, -=, body->avel, tmp); + + // add the gravity force to all bodies + if ((body->flags & dxBodyNoGravity) == 0) + { + body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0]; + body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1]; + body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2]; + body->facc[3] = 0; + } + + } + +#ifdef RANDOM_JOINT_ORDER +#ifdef TIMING + dTimerNow ("randomizing joint order"); +#endif + //randomize the order of the joints by looping through the array + //and swapping the current joint pointer with a random one before it. + for (j = 0; j < nj; j++) + { + joint = joints[j]; + dxJoint::Info1 i1 = info[j]; + dxJoint::Info2 i2 = Jinfo[j]; + int r = rand () % (j + 1); + joints[j] = joints[r]; + info[j] = info[r]; + Jinfo[j] = Jinfo[r]; + joints[r] = joint; + info[r] = i1; + Jinfo[r] = i2; + } +#endif + + //now iterate through the random ordered joint array we created. + for (j = 0; j < nj; j++) + { +#ifdef TIMING + dTimerNow ("setting up joint"); +#endif + joint = joints[j]; + bodyPair[0] = joint->node[0].body; + bodyPair[1] = joint->node[1].body; + + if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled)) + bodyPair[0] = 0; + if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled)) + bodyPair[1] = 0; + + //if this joint is not connected to any enabled bodies, skip it. + if (!bodyPair[0] && !bodyPair[1]) + continue; + + if (bodyPair[0]) + { + GIPair[0] = globalI + bodyPair[0]->tag * 12; + GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12; + } + if (bodyPair[1]) + { + GIPair[1] = globalI + bodyPair[1]->tag * 12; + GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12; + } + + joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); + + //dInternalStepIslandFast is an exact copy of the old routine with one + //modification: the calculated forces are added back to the facc and tacc + //vectors instead of applying them to the bodies and moving them. + if (info[j].m > 0) + { + dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep); + } + } + // } +# ifdef TIMING + dTimerNow ("moving bodies"); +# endif + //Now we can simulate all the free floating bodies, and move them. + for (b = 0; b < nb; b++) + { + body = bodies[b]; + + for (i = 0; i < 4; i++) + { + body->facc[i] *= ministep; + body->tacc[i] *= ministep; + } + + //apply torque + dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc); + + //apply force + for (i = 0; i < 3; i++) + body->lvel[i] += body->invMass * body->facc[i]; + + //move It! + moveAndRotateBody (body, ministep); + } + } + for (b = 0; b < nb; b++) + for (j = 0; j < 4; j++) + bodies[b]->facc[j] = bodies[b]->tacc[j] = 0; +} + + +#ifdef NO_ISLANDS + +// Since the iterative algorithm doesn't care about islands of bodies, this is a +// faster algorithm that just sends it all the joints and bodies in one array. +// It's downfall is it's inability to handle disabled bodies as well as the old one. +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ + // nothing to do if no bodies + if (world->nb <= 0) + return; + +# ifdef TIMING + dTimerStart ("creating joint and body arrays"); +# endif + dxBody **bodies, *body; + dxJoint **joints, *joint; + joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + + int nj = 0; + for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next) + joints[nj++] = joint; + + int nb = 0; + for (body = world->firstbody; body; body = (dxBody *) body->next) + bodies[nb++] = body; + + dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations); +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#else + +//**************************************************************************** +// island processing + +// this groups all joints and bodies in a world into islands. all objects +// in an island are reachable by going through connected bodies and joints. +// each island can be simulated separately. +// note that joints that are not attached to anything will not be included +// in any island, an so they do not affect the simulation. +// +// this function starts new island from unvisited bodies. however, it will +// never start a new islands from a disabled body. thus islands of disabled +// bodies will not be included in the simulation. disabled bodies are +// re-enabled if they are found to be part of an active island. + +static void +processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) +{ +#ifdef TIMING + dTimerStart ("Island Setup"); +#endif + dxBody *b, *bb, **body; + dxJoint *j, **joint; + + // nothing to do if no bodies + if (world->nb <= 0) + return; + + // make arrays for body and joint lists (for a single island) to go into + body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); + joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); + int bcount = 0; // number of bodies in `body' + int jcount = 0; // number of joints in `joint' + int tbcount = 0; + int tjcount = 0; + + // set all body/joint tags to 0 + for (b = world->firstbody; b; b = (dxBody *) b->next) + b->tag = 0; + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + j->tag = 0; + + // allocate a stack of unvisited bodies in the island. the maximum size of + // the stack can be the lesser of the number of bodies or joints, because + // new bodies are only ever added to the stack by going through untagged + // joints. all the bodies in the stack must be tagged! + int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; + dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *)); + int *autostack = (int *) ALLOCA (stackalloc * sizeof (int)); + + for (bb = world->firstbody; bb; bb = (dxBody *) bb->next) + { +#ifdef TIMING + dTimerNow ("Island Processing"); +#endif + // get bb = the next enabled, untagged body, and tag it + if (bb->tag || (bb->flags & dxBodyDisabled)) + continue; + bb->tag = 1; + + // tag all bodies and joints starting from bb. + int stacksize = 0; + int autoDepth = autoEnableDepth; + b = bb; + body[0] = bb; + bcount = 1; + jcount = 0; + goto quickstart; + while (stacksize > 0) + { + b = stack[--stacksize]; // pop body off stack + autoDepth = autostack[stacksize]; + body[bcount++] = b; // put body on body list + quickstart: + + // traverse and tag all body's joints, add untagged connected bodies + // to stack + for (dxJointNode * n = b->firstjoint; n; n = n->next) + { + if (!n->joint->tag) + { + int thisDepth = autoEnableDepth; + n->joint->tag = 1; + joint[jcount++] = n->joint; + if (n->body && !n->body->tag) + { + if (n->body->flags & dxBodyDisabled) + thisDepth = autoDepth - 1; + if (thisDepth < 0) + continue; + n->body->flags &= ~dxBodyDisabled; + n->body->tag = 1; + autostack[stacksize] = thisDepth; + stack[stacksize++] = n->body; + } + } + } + dIASSERT (stacksize <= world->nb); + dIASSERT (stacksize <= world->nj); + } + + // now do something with body and joint lists + dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations); + + // what we've just done may have altered the body/joint tag values. + // we must make sure that these tags are nonzero. + // also make sure all bodies are in the enabled state. + int i; + for (i = 0; i < bcount; i++) + { + body[i]->tag = 1; + body[i]->flags &= ~dxBodyDisabled; + } + for (i = 0; i < jcount; i++) + joint[i]->tag = 1; + + tbcount += bcount; + tjcount += jcount; + } + +#ifdef TIMING + dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount); +#endif + + // if debugging, check that all objects (except for disabled bodies, + // unconnected joints, and joints that are connected to disabled bodies) + // were tagged. +# ifndef dNODEBUG + for (b = world->firstbody; b; b = (dxBody *) b->next) + { + if (b->flags & dxBodyDisabled) + { + if (b->tag) + dDebug (0, "disabled body tagged"); + } + else + { + if (!b->tag) + dDebug (0, "enabled body not tagged"); + } + } + for (j = world->firstjoint; j; j = (dxJoint *) j->next) + { + if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0)) + { + if (!j->tag) + dDebug (0, "attached enabled joint not tagged"); + } + else + { + if (j->tag) + dDebug (0, "unattached or disabled joint tagged"); + } + } +# endif + /******************** breakable joint contribution ***********************/ + dxJoint* nextJ; + if (!world->firstjoint) + nextJ = 0; + else + nextJ = (dxJoint*)world->firstjoint->next; + for (j=world->firstjoint; j; j=nextJ) { + nextJ = (dxJoint*)j->next; + // check if joint is breakable and broken + if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { + // detach (break) the joint + dJointAttach (j, 0, 0); + // call the callback function if it is set + if (j->breakInfo->callback) j->breakInfo->callback (j); + // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set + if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); + } + } + /*************************************************************************/ + +# ifdef TIMING + dTimerEnd (); + dTimerReport (stdout, 1); +# endif +} + +#endif + + +void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations) +{ + dUASSERT (w, "bad world argument"); + dUASSERT (stepsize > 0, "stepsize must be > 0"); + processIslandsFast (w, stepsize, maxiterations); +} diff --git a/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp b/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp new file mode 100644 index 0000000..bfed3a3 --- /dev/null +++ b/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp @@ -0,0 +1,416 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +/* + +buggy with suspension. +this also shows you how to use geom groups. + +*/ + + +#include + +#include +#include + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCappedCylinder dsDrawCappedCylinderD +#endif + + +// some constants + +#define LENGTH 0.7 // chassis length +#define WIDTH 0.4 // chassis width +#define HEIGHT 0.2 // chassis height +#define RADIUS 0.22 // wheel radius +#define STARTZ 0.4 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 0.2 // wheel mass + +// dynamics and collision objects (chassis, 4 wheels, environment, obstacles, chain) +static dWorldID world; +static dSpaceID space; + +// chain stuff +static const float chain_radius = 0.1; +static const float chain_mass = 0.1; +static const int chain_num = 10; +static dBodyID chain_body[chain_num]; +static dGeomID chain_geom[chain_num]; +static dJointID chain_joint[chain_num-1]; + +// 1 chasses, 4 wheels +static dBodyID body[5]; +// joint[0] is left front wheel, joint[1] is right front wheel +static dJointID joint[4]; +static int joint_exists[4]; +static dJointGroupID contactgroup; +static dGeomID ground; +static dSpaceID car_space; +static dGeomID box[1]; +static dGeomID sphere[4]; +static dGeomID ground_box; +static const int obstacle_num = 25; +static dGeomID obstacle[obstacle_num]; + +// things that the user controls + +static dReal speed=0,steer=0; // user commands + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + int i,n; + +// // do not collide objects that are connected +// dBodyID b1 = dGeomGetBody (o1), +// b2 = dGeomGetBody (o2); +// if (b1 && b2 && dAreConnected(b1, b2)) return; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i 0.1) v = 0.1; + if (v < -0.1) v = -0.1; + v *= 10.0; + dJointSetHinge2Param (joint[i],dParamVel,v); + dJointSetHinge2Param (joint[i],dParamFMax,0.2); + dJointSetHinge2Param (joint[i],dParamLoStop,-0.75); + dJointSetHinge2Param (joint[i],dParamHiStop,0.75); + dJointSetHinge2Param (joint[i],dParamFudgeFactor,0.1); + } + } + + dSpaceCollide (space,0,&nearCallback); + //dWorldStep (world,0.05); + dWorldStepFast1 (world,0.05,5); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + } + + dsSetColor (0,1,1); + dsSetTexture (DS_WOOD); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); + dsSetColor (1,1,1); + for (i=1; i<=4; i++) + dsDrawCylinder (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]), + 0.2, + RADIUS); + + dVector3 ss; + dGeomBoxGetLengths (ground_box,ss); + dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); + + dsSetColor (1,0,0); + for (i=0; i +#include + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCappedCylinder dsDrawCappedCylinderD +#endif + + +// some constants + +#define LENGTH 0.7 // chassis length +#define WIDTH 0.5 // chassis width +#define HEIGHT 0.2 // chassis height +#define RADIUS 0.18 // wheel radius +#define STARTZ 0.5 // starting height of chassis +#define CMASS 1 // chassis mass +#define WMASS 0.2 // wheel mass + + +// dynamics and collision objects (chassis, 3 wheels, environment) + +static dWorldID world; +static dSpaceID space; +static dBodyID body[4]; +static dJointID joint[3]; // joint[0] is the front wheel +static dJointGroupID contactgroup; +static dGeomID ground; +static dSpaceID car_space; +static dGeomID box[1]; +static dGeomID sphere[3]; +static dGeomID ground_box; + + +// things that the user controls + +static dReal speed=0,steer=0; // user commands + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + int i,n; + + // only collide things with the ground + int g1 = (o1 == ground || o1 == ground_box); + int g2 = (o2 == ground || o2 == ground_box); + if (!(g1 ^ g2)) return; + + const int N = 10; + dContact contact[N]; + n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); + if (n > 0) { + for (i=0; i 0.1) v = 0.1; + if (v < -0.1) v = -0.1; + v *= 10.0; + dJointSetHinge2Param (joint[0],dParamVel,v); + dJointSetHinge2Param (joint[0],dParamFMax,0.2); + dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); + dJointSetHinge2Param (joint[0],dParamHiStop,0.75); + dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); + + dSpaceCollide (space,0,&nearCallback); + dWorldStep (world,0.05); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + } + + dsSetColor (0,1,1); + dsSetTexture (DS_WOOD); + dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; + dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); + dsSetColor (1,1,1); + for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), + dBodyGetRotation(body[i]),0.02f,RADIUS); + + dVector3 ss; + dGeomBoxGetLengths (ground_box,ss); + dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); + + /* + printf ("%.10f %.10f %.10f %.10f\n", + dJointGetHingeAngle (joint[1]), + dJointGetHingeAngle (joint[2]), + dJointGetHingeAngleRate (joint[1]), + dJointGetHingeAngleRate (joint[2])); + */ +} + + +int main (int argc, char **argv) +{ + int i; + dMass m; + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = "../../drawstuff/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + ground = dCreatePlane (space,0,0,1,0); + + // chassis body + body[0] = dBodyCreate (world); + dBodySetPosition (body[0],0,0,STARTZ); + dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); + dMassAdjust (&m,CMASS); + dBodySetMass (body[0],&m); + box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT); + dGeomSetBody (box[0],body[0]); + + // wheel bodies + for (i=1; i<=3; i++) { + body[i] = dBodyCreate (world); + dQuaternion q; + dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); + dBodySetQuaternion (body[i],q); + dMassSetSphere (&m,1,RADIUS); + dMassAdjust (&m,WMASS); + dBodySetMass (body[i],&m); + sphere[i-1] = dCreateSphere (0,RADIUS); + dGeomSetBody (sphere[i-1],body[i]); + } + dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5); + dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5); + + // front wheel hinge + /* + joint[0] = dJointCreateHinge2 (world,0); + dJointAttach (joint[0],body[0],body[1]); + const dReal *a = dBodyGetPosition (body[1]); + dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]); + dJointSetHinge2Axis1 (joint[0],0,0,1); + dJointSetHinge2Axis2 (joint[0],0,1,0); + */ + + // front and back wheel hinges + for (i=0; i<3; i++) { + joint[i] = dJointCreateHinge2 (world,0); + dJointAttach (joint[i],body[0],body[i+1]); + const dReal *a = dBodyGetPosition (body[i+1]); + dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]); + dJointSetHinge2Axis1 (joint[i],0,0,1); + dJointSetHinge2Axis2 (joint[i],0,1,0); + + // breakable joints contribution + // the wheels can break + dJointSetBreakable (joint[i], 1); + // the wheels wil break at a specific force + dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_B1_FORCE|dJOINT_BREAK_AT_B2_FORCE); + // specify the force for the first body connected to the joint ... + dJointSetBreakForce (joint[i], 0, 1.5, 1.5, 1.5); + // and for the second body + dJointSetBreakForce (joint[i], 1, 1.5, 1.5, 1.5); + } + + // set joint suspension + for (i=0; i<3; i++) { + dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4); + dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8); + } + + // lock back wheels along the steering axis + for (i=1; i<3; i++) { + // set stops to make sure wheels always stay in alignment + dJointSetHinge2Param (joint[i],dParamLoStop,0); + dJointSetHinge2Param (joint[i],dParamHiStop,0); + // the following alternative method is no good as the wheels may get out + // of alignment: + // dJointSetHinge2Param (joint[i],dParamVel,0); + // dJointSetHinge2Param (joint[i],dParamFMax,dInfinity); + } + + // create car space and add it to the top level space + car_space = dSimpleSpaceCreate (space); + dSpaceSetCleanup (car_space,0); + dSpaceAdd (car_space,box[0]); + dSpaceAdd (car_space,sphere[0]); + dSpaceAdd (car_space,sphere[1]); + dSpaceAdd (car_space,sphere[2]); + + // environment + ground_box = dCreateBox (space,2,1.5,1); + dMatrix3 R; + dRFromAxisAndAngle (R,0,1,0,-0.15); + dGeomSetPosition (ground_box,2,0,-0.34); + dGeomSetRotation (ground_box,R); + + // run simulation + dsSimulationLoop (argc,argv,352,288,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + dGeomDestroy (box[0]); + dGeomDestroy (sphere[0]); + dGeomDestroy (sphere[1]); + dGeomDestroy (sphere[2]); + + return 0; +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp b/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp new file mode 100644 index 0000000..e550a32 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp @@ -0,0 +1,58 @@ +#include "stdafx.h" + +using namespace System::Reflection; +using namespace System::Runtime::CompilerServices; + +// +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +// +[assembly:AssemblyTitleAttribute("")]; +[assembly:AssemblyDescriptionAttribute("")]; +[assembly:AssemblyConfigurationAttribute("")]; +[assembly:AssemblyCompanyAttribute("")]; +[assembly:AssemblyProductAttribute("")]; +[assembly:AssemblyCopyrightAttribute("")]; +[assembly:AssemblyTrademarkAttribute("")]; +[assembly:AssemblyCultureAttribute("")]; + +// +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the value or you can default the Revision and Build Numbers +// by using the '*' as shown below: + +[assembly:AssemblyVersionAttribute("1.0.*")]; + +// +// In order to sign your assembly you must specify a key to use. Refer to the +// Microsoft .NET Framework documentation for more information on assembly signing. +// +// Use the attributes below to control which key is used for signing. +// +// Notes: +// (*) If no key is specified, the assembly is not signed. +// (*) KeyName refers to a key that has been installed in the Crypto Service +// Provider (CSP) on your machine. KeyFile refers to a file which contains +// a key. +// (*) If the KeyFile and the KeyName values are both specified, the +// following processing occurs: +// (1) If the KeyName can be found in the CSP, that key is used. +// (2) If the KeyName does not exist and the KeyFile does exist, the key +// in the KeyFile is installed into the CSP and used. +// (*) In order to create a KeyFile, you can use the sn.exe (Strong Name) utility. +// When specifying the KeyFile, the location of the KeyFile should be +// relative to the project directory. +// (*) Delay Signing is an advanced option - see the Microsoft .NET Framework +// documentation for more information on this. +// +[assembly:AssemblyDelaySignAttribute(false)]; +[assembly:AssemblyKeyFileAttribute("")]; +[assembly:AssemblyKeyNameAttribute("")]; + diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp new file mode 100644 index 0000000..c95ae57 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp @@ -0,0 +1,322 @@ +#include "StdAfx.h" + +#include +#include "Body.h" + +namespace ODEManaged +{ + + //Constructors + + Body::Body(void) + { + _id = 0; + } + + Body::Body(World &world) + { + _id = dBodyCreate(world.Id()); + } + + + //Destructor + + Body::~Body(void) + { + dBodyDestroy(this->_id); + } + + + //Methods + + //Id + dBodyID Body::Id() + { + return _id; + } + + + //SetData + void Body::SetData(void *data) + { + dBodySetData(this->_id, data); + } + + //GetData + void *Body::GetData(void) + { + return dBodyGetData(this->_id); + } + + + //SetPosition + void Body::SetPosition (double x, double y, double z) + { + dBodySetPosition(this->_id, x, y, z); + } + + + //Overloaded GetPosition + Vector3 Body::GetPosition(void) + { + Vector3 retVal; + const dReal *temp; + temp = dBodyGetPosition(this->_id); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + }; + + void Body::GetPosition(double position __gc[]) + { + const dReal *temp; + temp = dBodyGetPosition(this->_id); + position[0] = temp[0]; + position[1] = temp[1]; + position[2] = temp[2]; + } + + + //SetRotationIdentity + void Body::SetRotationIdentity(void) + { + dMatrix3 temp; + dRSetIdentity(temp); + dBodySetRotation(this->_id, temp); + } + + + //SetRotation (left handed system=>transpose) + void Body::SetRotation(Matrix3 rotation) + { + dMatrix3 temp; + temp[0] = rotation.m11; + temp[4] = rotation.m12; + temp[8] = rotation.m13; + temp[1] = rotation.m21; + temp[5] = rotation.m22; + temp[9] = rotation.m23; + temp[2] = rotation.m31; + temp[6] = rotation.m32; + temp[10] = rotation.m33; + dBodySetRotation(this->_id, temp); + } + + //GetRotation (left handed system=>transpose) + Matrix3 Body::GetRotation(void) + { + Matrix3 retVal; + //const dMatrix3 *m; + const dReal *temp; + temp = dBodyGetRotation(this->_id); + retVal.m11 = temp[0]; + retVal.m12 = temp[4]; + retVal.m13 = temp[8]; + retVal.m21 = temp[1]; + retVal.m22 = temp[5]; + retVal.m23 = temp[9]; + retVal.m31 = temp[2]; + retVal.m32 = temp[6]; + retVal.m33 = temp[10]; + return retVal; + } + + + //Overloaded SetMass + void Body::SetMass(double mass, Vector3 centerOfGravity, Matrix3 inertia) + { + dMass *temp = new dMass(); + dMassSetParameters(temp, mass, + centerOfGravity.x, + centerOfGravity.y, + centerOfGravity.z, + inertia.m11, inertia.m22, + inertia.m33, inertia.m12, + inertia.m13, inertia.m23); + + dBodySetMass(this->_id, temp); + } + + + //SetMassSphere + void Body::SetMassSphere(double density, double radius) + { + dMass *temp = new dMass(); + dMassSetSphere(temp, density, radius); + dBodySetMass(this->_id, temp); + } + + + //SetMassBox + void Body::SetMassBox(double density, double sideX, double sideY, double sideZ) + { + dMass *temp = new dMass(); + dMassSetBox(temp, density, sideX, sideY, sideZ); + dBodySetMass(this->_id, temp); + } + + + //SetMassCappedCylinder + void Body::SetMassCappedCylinder(double density, int axis, double cylinderRadius, double cylinderLength) + { + dMass *temp = new dMass(); + dMassSetCappedCylinder(temp, density, axis, + cylinderRadius, + cylinderLength); + + dBodySetMass(this->_id, temp); + } + + + //AddForce + void Body::AddForce(double fX, double fY, double fZ) + { + dBodyAddForce(this->_id, fX, fY, fZ); + } + + + //AddRelForce + void Body::AddRelForce(double fX, double fY, double fZ) + { + dBodyAddRelForce(this->_id, fX,fY,fZ); + } + + + //AddForceAtPos + void Body::AddForceAtPos(double fX, double fY, double fZ, double pX, double pY, double pZ) + { + dBodyAddForceAtPos(this->_id, fX, fY, fZ, pX, pY, pZ); + } + + + //AddRelForceAtPos + void Body::AddRelForceAtPos(double fX, double fY, double fZ, double pX, double pY, double pZ) + { + dBodyAddRelForceAtPos(this->_id, fX, fY, fZ, pX, pY, pZ); + } + + + //AddRelForceAtRelPos + void Body::AddRelForceAtRelPos(double fX, double fY, double fZ, double pX, double pY, double pZ) + { + dBodyAddRelForceAtRelPos(this->_id, fX, fY, fZ, pX, pY, pZ); + } + + + //ApplyLinearVelocityDrag + void Body::ApplyLinearVelocityDrag(double dragCoef) + { + const dReal *temp; + double fX; + double fY; + double fZ; + temp = dBodyGetLinearVel(this->_id); + fX = temp[0]*dragCoef*-1; + fY = temp[1]*dragCoef*-1; + fZ = temp[2]*dragCoef*-1; + dBodyAddForce(this->_id, fX, fY, fZ); + } + + + //ApplyAngularVelocityDrag + void Body::ApplyAngularVelocityDrag(double dragCoef) + { + const dReal *temp; + double fX; + double fY; + double fZ; + temp = dBodyGetAngularVel(this->_id); + fX = temp[0]*dragCoef*-1; + fY = temp[1]*dragCoef*-1; + fZ = temp[2]*dragCoef*-1; + dBodyAddTorque(this->_id, fX, fY, fZ); + } + + + //AddTorque + void Body::AddTorque(double fX, double fY, double fZ) + { + dBodyAddTorque(this->_id, fX, fY, fZ); + } + + + //AddRelTorque + void Body::AddRelTorque(double fX, double fY, double fZ) + { + dBodyAddRelTorque(this->_id, fX,fY,fZ); + } + + + //SetLinearVelocity + void Body::SetLinearVelocity(double x, double y, double z) + { + dBodySetLinearVel(this->_id, x, y, z); + } + + + //GetLinearVelocity + Vector3 Body::GetLinearVelocity(void) + { + Vector3 retVal; + const dReal *temp; + temp = dBodyGetLinearVel(this->_id); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //SetAngularVelocity + void Body::SetAngularVelocity(double x, double y, double z) + { + dBodySetAngularVel(this->_id, x, y, z); + } + + //GetAngularVelocity + Vector3 Body::GetAngularVelocity(void) + { + Vector3 retVal; + const dReal *temp; + temp = dBodyGetAngularVel(this->_id); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //GetRelPointPos + Vector3 Body::GetRelPointPos(double pX, double pY, double pZ) + { + Vector3 retVal; + dVector3 temp; + dBodyGetRelPointPos(this->_id, pX, pY, pZ, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //GetRelPointVel + Vector3 Body::GetRelPointVel(double pX, double pY, double pZ) + { + Vector3 retVal; + dVector3 temp; + dBodyGetRelPointVel(this->_id, pX, pY, pZ, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //ConnectedTo + int Body::ConnectedTo(const Body &b) + { + return dAreConnected(this->_id, b._id); + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Body.h b/libraries/ode-0.9/contrib/DotNetManaged/Body.h new file mode 100644 index 0000000..9347c17 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Body.h @@ -0,0 +1,76 @@ +#pragma once + +#include "World.h" +#include "CommonMgd.h" + +namespace ODEManaged +{ + __gc public class Body + { + public: + + //Constructors and Destructors + + Body(void); + Body(World &world); + + ~Body(void); + + + //Public Methods + + dBodyID Id(); + void SetData (void *data); + void *GetData (void); + + //POSITION + void SetPosition(double x, double y, double z); + Vector3 GetPosition(void); + void GetPosition(double position __gc[]); + + //ROTATION + void SetRotationIdentity(void); + void SetRotation(Matrix3 rotation); + Matrix3 GetRotation(void); + + //MASS + void SetMass(double mass, Vector3 centerOfGravity, Matrix3 inertia); + void SetMassSphere(double density, double radius); + void SetMassBox(double density, double sideX, double sideY, double sideZ); + void SetMassCappedCylinder(double density, int axis, double cylinderRadius, double cylinderLength); + + //FORCE AND TORQUE + void AddForce(double fX, double fY, double fZ); + void AddRelForce(double fX, double fY, double fZ); + void AddForceAtPos(double fX, double fY, double fZ,double pX, double pY, double pZ); + void AddRelForceAtPos(double fX, double fY, double fZ,double pX, double pY, double pZ); + void AddRelForceAtRelPos(double fX, double fY, double fZ,double pX, double pY, double pZ); + void ApplyLinearVelocityDrag(double dragCoef); + void ApplyAngularVelocityDrag(double dragCoef); + + + void AddTorque(double fX, double fY, double fZ); + void AddRelTorque(double fX, double fY, double fZ); + + //LINEAR VELOCITY + void SetLinearVelocity (double x, double y, double z); + Vector3 GetLinearVelocity(void); + + //ANGULAR VELOCITY + void SetAngularVelocity (double x, double y, double z); + Vector3 GetAngularVelocity(void); + + //POINT + Vector3 GetRelPointPos(double pX, double pY, double pZ); + Vector3 GetRelPointVel(double pX, double pY, double pZ); + + //CONNECTED TO + int ConnectedTo (const Body &b); + + private: + + dBodyID _id; + + }; +} + diff --git a/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h b/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h new file mode 100644 index 0000000..143397d --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h @@ -0,0 +1,43 @@ +#pragma once + +namespace ODEManaged +{ + + __value public struct Vector3 + { + double x; + double y; + double z; + }; + + + __value public struct Vector4 + { + double W; + double x; + double y; + double z; + }; + + + __value public struct Matrix3 + { + double m11; + double m12; + double m13; + double m21; + double m22; + double m23; + double m31; + double m32; + double m33; + }; + + //__value public struct NearCallback + //{ + // void *data; + // dGeomID o1; + // dGeomID o2; + //}; + +} \ 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 new file mode 100644 index 0000000..2694a26 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln @@ -0,0 +1,21 @@ +Microsoft Visual Studio Solution File, Format Version 7.00 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DotNetManaged", "DotNetManaged.vcproj", "{4B75AC19-971A-4CC6-A4F5-0695C9F8562F}" +EndProject +Global + GlobalSection(SolutionConfiguration) = preSolution + ConfigName.0 = Debug + ConfigName.1 = Release + EndGlobalSection + GlobalSection(ProjectDependencies) = postSolution + EndGlobalSection + GlobalSection(ProjectConfiguration) = postSolution + {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Debug.ActiveCfg = Debug|Win32 + {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Debug.Build.0 = Debug|Win32 + {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Release.ActiveCfg = Release|Win32 + {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Release.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + EndGlobalSection + GlobalSection(ExtensibilityAddIns) = postSolution + EndGlobalSection +EndGlobal diff --git a/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj new file mode 100644 index 0000000..2f5bb6c --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj @@ -0,0 +1,379 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp new file mode 100644 index 0000000..3655466 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp @@ -0,0 +1,219 @@ + +#include "StdAfx.h" + +#include +#include "Geom.h" + + +namespace ODEManaged +{ + + //Constructors + + Geom::Geom(void) + { + _id = 0; + } + + + //Destructor + + Geom::~Geom(void) + { + dGeomDestroy(this->_id); + } + + + //Methods + + //Id + dGeomID Geom::Id(void) + { + return _id; + } + + + //GetBody + dBodyID Geom::GetBody(void) + { + return dGeomGetBody(this->_id); + } + + + //Overloaded SetBody + void Geom::SetBody(Body &body) + { + dGeomSetBody(this->_id, body.Id()); + } + + //void Geom::SetBody(dBodyID b) + //{ + // dGeomSetBody(this->_id, b); + //} + + + //SetPosition + void Geom::SetPosition(double x, double y, double z) + { + dGeomSetPosition(this->_id, x, y, z); + } + + + //SetRotation + void Geom::SetRotation(Matrix3 rotation) + { + dMatrix3 temp; + temp[0] = rotation.m11; + temp[4] = rotation.m12; + temp[8] = rotation.m13; + temp[1] = rotation.m21; + temp[5] = rotation.m22; + temp[9] = rotation.m23; + temp[2] = rotation.m31; + temp[6] = rotation.m32; + temp[10] = rotation.m33; + dGeomSetRotation(_id, temp); + } + + + //Destroy + void Geom::Destroy() + { + if(this->_id) dGeomDestroy(this->_id); + _id = 0; + } + + + //SetData + void Geom::SetData(void *data) + { + dGeomSetData(this->_id, data); + } + + + //GetData + void *Geom::GetData(void) + { + return dGeomGetData(this->_id); + } + + + //GetPosition + Vector3 Geom::GetPosition(void) + { + Vector3 retVal; + const dReal *temp; + temp = dGeomGetPosition(this->_id); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //GetRotation (left handed system=>transpose) + Matrix3 Geom::GetRotation(void) + { + Matrix3 retVal; + const dReal *temp; + temp = dGeomGetRotation(this->_id); + retVal.m11 = temp[0]; + retVal.m12 = temp[4]; + retVal.m13 = temp[8]; + retVal.m21 = temp[1]; + retVal.m22 = temp[5]; + retVal.m23 = temp[9]; + retVal.m31 = temp[2]; + retVal.m32 = temp[6]; + retVal.m33 = temp[10]; + return retVal; + } + + + //CreateSphere + void Geom::CreateSphere(Space &space, double radius) + { + if(this->_id) dGeomDestroy(this->_id); + _id = dCreateSphere(space.Id(), radius); + } + + + //CreateBox + void Geom::CreateBox(Space &space, double lx, double ly, double lz) + { + if(this->_id) dGeomDestroy(this->_id); + _id = dCreateBox(space.Id(), lx, ly, lz); + } + + + //CreatePlane + void Geom::CreatePlane(Space &space, double a, double b, double c, double d) + { + if(this->_id) dGeomDestroy(this->_id); + _id = dCreatePlane(space.Id(), a, b, c, d); + } + + + //CreateCCylinder + void Geom::CreateCCylinder(Space &space, double radius, double length) + { + if(this->_id) dGeomDestroy(this->_id); + _id = dCreateCCylinder(space.Id(), radius, length); + } + + + //SphereGetRadius + double Geom::SphereGetRadius(void) + { + return dGeomSphereGetRadius(this->_id); + } + + + //BoxGetLengths + Vector3 Geom::BoxGetLengths(void) + { + Vector3 retVal; + dVector3 temp; + dGeomBoxGetLengths(this->_id, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //PlaneGetParams + Vector4 Geom::PlaneGetParams(void) + { + Vector4 retVal; + dVector4 temp; + dGeomPlaneGetParams(this->_id, temp); + retVal.W = temp[0]; + retVal.x = temp[1]; + retVal.y = temp[2]; + retVal.z = temp[3]; + return retVal; + } + + + //CCylinderGetParams + void Geom::CCylinderGetParams(double *radius, double *length) + { + dGeomCCylinderGetParams(this->_id, radius, length); + } + + + //GetClass + int Geom::GetClass(void) + { + return dGeomGetClass(this->_id); + } + +} + + + + + + + diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Geom.h b/libraries/ode-0.9/contrib/DotNetManaged/Geom.h new file mode 100644 index 0000000..83a6faf --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Geom.h @@ -0,0 +1,75 @@ +#pragma once + +#include "Body.h" +#include "Space.h" +#include "CommonMgd.h" + +namespace ODEManaged +{ + __gc public class Geom + { + public: + + + //Constructor + + Geom (void); + + + //Destructor + + ~Geom (void); + + + //Methods + + //Basic Stuff + + dGeomID Id (void); + dBodyID GetBody (void); + + //Overloaded SetBody + void SetBody (Body &body); + /*void SetBody (dBodyID b);*/ + + Vector3 GetPosition (void); + void SetPosition (double x, double y, double z); + + Matrix3 GetRotation (void); + void SetRotation (Matrix3 rotation); + + void SetData (void *data); + void *GetData (void); + + + //Create Objects + + void CreateSphere (Space &space, double radius); + void CreateBox (Space &space, double lx, double ly, double lz); + void CreatePlane (Space &space, double a, double b, double c, double d); + void CreateCCylinder (Space &space, double radius, double length); + + + //Destroy Objects + + void Destroy (void); + + + //Get Object's Parameters + + double SphereGetRadius (void); + Vector3 BoxGetLengths (void); + Vector4 PlaneGetParams (void); + void CCylinderGetParams (double *radius, double *length); + int GetClass (void); + + + //Properties + + private: + + dGeomID _id; + + }; + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp new file mode 100644 index 0000000..e2d8de6 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp @@ -0,0 +1,35 @@ +#include "StdAfx.h" + +#include +#include "joint.h" +#include "CommonMgd.h" +#include "world.h" + +namespace ODEManaged +{ + + //Constructor + + Joint::Joint(void) + { + _id=0; + } + + + //Destructor + + Joint::~Joint(void) + { + dJointDestroy(this->_id); + } + + + //Methods + + //Id + dJointID Joint::Id(void) + { + return _id; + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Joint.h b/libraries/ode-0.9/contrib/DotNetManaged/Joint.h new file mode 100644 index 0000000..d9ab254 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Joint.h @@ -0,0 +1,21 @@ +#pragma once + +#include "JointGroup.h" +#include "World.h" +#include "Body.h" + +namespace ODEManaged +{ + __gc public class Joint + { + protected: + //Constructor and Destructor Defenition + Joint(void); + ~Joint(void); + + //Public Methods + dJointID Id(void); + + dJointID _id; + }; +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp new file mode 100644 index 0000000..c5a4543 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp @@ -0,0 +1,162 @@ +#include "StdAfx.h" + +#include +#include "JointAMotor.h" + +namespace ODEManaged +{ + + //Constructors + + JointAMotor::JointAMotor(void) : Joint(){} + + + JointAMotor::JointAMotor(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateAMotor(world.Id(), 0); + } + + + JointAMotor::JointAMotor(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateAMotor(world.Id(), jointGroup.Id()); + } + + + //Destructor + + JointAMotor::~JointAMotor(void){} + + + //Methods + + //Overloaded Create + void JointAMotor::Create(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateAMotor(world.Id(), jointGroup.Id()); + } + + void JointAMotor::Create(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateAMotor(world.Id(), 0); + } + + + //Overloaded Attach + void JointAMotor::Attach(Body &body1, Body &body2) + { + dJointAttach(this->_id, body1.Id(), body2.Id()); + } + + void JointAMotor::Attach(Body &body1) + { + dJointAttach(this->_id, body1.Id(), 0); + } + + + //SetNumAxes + + void JointAMotor::SetNumAxes(int num) + { + dJointSetAMotorNumAxes(this->_id, num); + } + + + //GetNumAxes + + int JointAMotor::GetNumAxes(void) + { + return dJointGetAMotorNumAxes(this->_id); + } + + + //SetAxis + + void JointAMotor::SetAxis(int anum, int rel, double x, double y ,double z) + { + dJointSetAMotorAxis(this->_id, anum, rel, x, y, z); + } + + + //GetAxis + + Vector3 JointAMotor::GetAxis(int anum) + { + Vector3 retVal; + dVector3 temp; + dJointGetAMotorAxis(this->_id, anum, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //SetAngle + + void JointAMotor::SetAngle(int anum, double angle) + { + dJointSetAMotorAngle(this->_id, anum, angle); + } + + + //GetAngle + + double JointAMotor::GetAngle(int anum) + { + return dJointGetAMotorAngle(this->_id, anum); + } + + + //SetParam + + void JointAMotor::SetParam(int parameter, double value) + { + dJointSetAMotorParam(this->_id, parameter, value); + } + + + //GetParam + + double JointAMotor::GetParam(int parameter) + { + return dJointGetAMotorParam(this->_id, parameter); + } + + + //SetMode + + void JointAMotor::SetMode(int mode) + { + dJointSetAMotorMode(this->_id, mode); + } + + + //GetMode + + int JointAMotor::GetMode(void) + { + return dJointGetAMotorMode(this->_id); + } + + + //GetAxisRel + + int JointAMotor::GetAxisRel(int anum) + { + return dJointGetAMotorAxisRel(this->_id, anum); + } + + + //GetAngleRate + + double JointAMotor::GetAngleRate(int anum) + { + return dJointGetAMotorAngleRate(this->_id, anum); + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h new file mode 100644 index 0000000..aa3ca4b --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h @@ -0,0 +1,62 @@ +#pragma once + +#include "Joint.h" + +namespace ODEManaged +{ + __gc public class JointAMotor : public Joint + { + public: + + + //Constructors + + JointAMotor (void); + JointAMotor (World &world); + JointAMotor (World &world, JointGroup &jointGroup); + + + //Destructor + + virtual ~JointAMotor (void); + + + //Methods + + //Basic Stuff + + //Overloaded Create + void Create (World &world, JointGroup &jointGroup); + void Create (World &world); + + void SetNumAxes (int num); + int GetNumAxes (void); + + void SetAxis (int anum, int rel, double x, double y, double z); + Vector3 GetAxis (int anum); + + void SetAngle (int anum, double angle); + double GetAngle (int anum); + + void SetMode (int mode); + int GetMode (void); + + int GetAxisRel (int anum); + double GetAngleRate (int anum); + + //Overloaded Attach + void Attach (Body &body1, Body &body2); + void Attach (Body &body1); + + + //Movement Parameters + + void SetParam (int parameter, double value); + double GetParam (int parameter); + + + + + + }; +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp new file mode 100644 index 0000000..d9336c9 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp @@ -0,0 +1,79 @@ +#include "StdAfx.h" + +#include +#include "jointball.h" + +namespace ODEManaged +{ + + //Constructors + + JointBall::JointBall(void) : Joint(){} + + + JointBall::JointBall(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateBall(world.Id(), 0); + } + + + JointBall::JointBall(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateBall(world.Id(), jointGroup.Id()); + } + + + //Destructor + + JointBall::~JointBall(void){} + + + //Methods + + //Overloaded Create + void JointBall::Create(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateBall(world.Id(), jointGroup.Id()); + } + + void JointBall::Create(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateBall(world.Id(), 0); + } + + + //Overloaded Attach + void JointBall::Attach(Body &body1, Body &body2) + { + dJointAttach(this->_id, body1.Id(), body2.Id()); + } + + void JointBall::Attach(Body &body1) + { + dJointAttach(this->_id, body1.Id(), 0); + } + + + //SetAnchor + void JointBall::SetAnchor(double x, double y ,double z) + { + dJointSetBallAnchor(this->_id, x, y, z); + } + + //GetAnchor + Vector3 JointBall::GetAnchor(void) + { + Vector3 retVal; + dVector3 temp; + dJointGetBallAnchor(this->_id,temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h new file mode 100644 index 0000000..2355bdd --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h @@ -0,0 +1,38 @@ +#pragma once + +#include "Joint.h" + +namespace ODEManaged +{ + __gc public class JointBall : public Joint + { + public: + + //Constructors + + JointBall(void); + JointBall(World &world); + JointBall(World &world, JointGroup &jointGroup); + + + //Destructors + + virtual ~JointBall(void); + + + //Methods + + //Overloaded Create + void Create(World &world, JointGroup &jointGroup); + void Create(World &world); + + //Overloaded Attach + void Attach(Body &body1, Body &body2); + void Attach(Body &body1); + + void SetAnchor(double x, double y, double z); + Vector3 GetAnchor(void); + + }; + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp new file mode 100644 index 0000000..afe9222 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp @@ -0,0 +1,67 @@ +#include "StdAfx.h" + +#include +#include "jointfixed.h" + +namespace ODEManaged +{ + + //Constructors + + JointFixed::JointFixed(void) : Joint(){} + + + JointFixed::JointFixed(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateFixed(world.Id(),0); + } + + + JointFixed::JointFixed(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateFixed(world.Id(), jointGroup.Id()); + } + + + //Destructor + + JointFixed::~JointFixed(void){} + + + //Methods + + //Overloaded Create + void JointFixed::Create(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateFixed(world.Id(), jointGroup.Id()); + } + + void JointFixed::Create(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateFixed(world.Id(), 0); + } + + + //Overloaded Attach + void JointFixed::Attach(Body &body1, Body &body2) + { + dJointAttach(this->_id, body1.Id(), body2.Id()); + } + + void JointFixed::Attach(Body &body1) + { + dJointAttach(this->_id, body1.Id(), 0); + } + + + //Fixed + void JointFixed::SetFixed(void) + { + dJointSetFixed(this->_id); + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h new file mode 100644 index 0000000..5ca50dc --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h @@ -0,0 +1,37 @@ +#pragma once + +#include "Joint.h" + +namespace ODEManaged +{ + __gc public class JointFixed : public Joint + { + public: + + //Constructors + + JointFixed(void); + JointFixed(World &world); + JointFixed(World &world, JointGroup &jointGroup); + + + //Destructor + + virtual ~JointFixed(void); + + + //Methods + + //Overloaded Create + void Create(World &world, JointGroup &jointGroup); + void Create(World &world); + + //Overloaded Attach + void Attach(Body &body1, Body &body2); + void Attach(Body &body1); + + void SetFixed(void); + + }; + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp new file mode 100644 index 0000000..925751f --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp @@ -0,0 +1,53 @@ +#include "StdAfx.h" + +#include +#include "jointgroup.h" + +namespace ODEManaged +{ + + //Constructors + + JointGroup::JointGroup(void) + { + _id=0; + } + + JointGroup::JointGroup (int maxSize) + { + _id = dJointGroupCreate(maxSize); + } + + + //Destructor + + JointGroup::~JointGroup(void) + { + dJointGroupDestroy(this->_id); + } + + + //Methods + + //ID + dJointGroupID JointGroup::Id() + { + return _id; + } + + + //Create + void JointGroup::Create (int maxSize) + { + if(_id) dJointGroupDestroy(_id); + _id = dJointGroupCreate(maxSize); + } + + + //Empty + void JointGroup::Empty (void) + { + dJointGroupEmpty(this->_id); + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h new file mode 100644 index 0000000..b62ced0 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h @@ -0,0 +1,33 @@ +#pragma once + +namespace ODEManaged +{ + __gc public class JointGroup + { + public: + + //Constructors + + JointGroup(void); + JointGroup(int maxSize); + + + //Destructor + + ~JointGroup(void); + + + //Methods + + dJointGroupID Id(void); + void Create(int maxSize); + void Empty(void); + + + private: + + dJointGroupID _id; + + }; + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp new file mode 100644 index 0000000..85d420b --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp @@ -0,0 +1,121 @@ +#include "stdafx.h" + +#include +#include "jointhinge.h" + +namespace ODEManaged +{ + + //Constructors + + JointHinge::JointHinge(void) : Joint(){} + + + JointHinge::JointHinge(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge(world.Id(), 0); + } + + + JointHinge::JointHinge(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge(world.Id(), jointGroup.Id()); + } + + + //Destructor + + JointHinge::~JointHinge(void){} + + + //Methods + + //Overloaded Create + void JointHinge::Create(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge(world.Id(), jointGroup.Id()); + } + + void JointHinge::Create(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge(world.Id(), 0); + } + + + //Overloaded Attach + void JointHinge::Attach(Body &body1, Body &body2) + { + dJointAttach(this->_id, body1.Id(), body2.Id()); + } + + void JointHinge::Attach(Body &body1) + { + dJointAttach(this->_id, body1.Id(), 0); + } + + + //SetAxis + void JointHinge::SetAxis(double x, double y, double z) + { + dJointSetHingeAxis(this->_id, x, y, z); + } + + //GetAxis + Vector3 JointHinge::GetAxis(void) + { + Vector3 retVal; + dVector3 temp; + dJointGetHingeAxis(this->_id, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //SetAnchor + void JointHinge::SetAnchor(double x, double y, double z) + { + dJointSetHingeAnchor(this->_id, x, y, z); + } + + //GetAnchor + Vector3 JointHinge::GetAnchor(void) + { + Vector3 retVal; + dVector3 temp; + dJointGetHingeAnchor(this->_id, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //Movement Parameters + + //SetAllMovParams + void JointHinge::SetAllMovParams(double LoStop, double HiStop, + double Velocity, double MaxForce, + double FudgeFactor, double Bounce, + double StopERP, double StopCFM) + { + if (LoStop > -3.141592653 && LoStop <= 0) + dJointSetHingeParam(this->_id, dParamLoStop, LoStop); + + if (HiStop < 3.141592653 && HiStop >= 0) + dJointSetHingeParam(this->_id, dParamHiStop, HiStop); + + dJointSetHingeParam(this->_id, dParamVel, Velocity); + dJointSetHingeParam(this->_id, dParamFMax, MaxForce); + dJointSetHingeParam(this->_id, dParamFudgeFactor, FudgeFactor); + dJointSetHingeParam(this->_id, dParamBounce, Bounce); + dJointSetHingeParam(this->_id, dParamStopERP, StopERP); + dJointSetHingeParam(this->_id, dParamStopCFM, StopCFM); + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h new file mode 100644 index 0000000..3115845 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h @@ -0,0 +1,195 @@ +#pragma once + +#include "Joint.h" +#include "CommonMgd.h" + +namespace ODEManaged +{ + __gc public class JointHinge : public Joint + { + public: + + //Constructors + + JointHinge(void); + JointHinge(World &world); + JointHinge(World &world, JointGroup &jointGroup); + + + //Destructor + + virtual~JointHinge(void); + + + //Methods + + //Overloaded Create + void Create(World &world, JointGroup &jointGroup); + void Create(World &world); + + //Overloaded Attach + void Attach(Body &body1, Body &body2); + void Attach(Body &body1); + + void SetAnchor(double x, double y, double z); + Vector3 GetAnchor(void); + + void SetAxis(double x, double y, double z); + Vector3 GetAxis(void); + + void SetAllMovParams(double LoStop, double HiStop, + double Velocity, double MaxForce, + double FudgeFactor, double Bounce, + double StopERP, double StopCFM); + + + //Properties + + //LoStop + __property double get_LoStop(void) + { + return dJointGetHingeParam(this->_id, dParamLoStop); + } + + __property void set_LoStop(double value) + { + if (value > -3.141592653 && value <= 0) + dJointSetHingeParam(this->_id, dParamLoStop, value); + } + + + //HiStop + __property double get_HiStop(void) + { + return dJointGetHingeParam(this->_id, dParamHiStop); + } + + __property void set_HiStop(double value) + { + if (value < 3.141592653 && value >= 0) + dJointSetHingeParam(this->_id, dParamHiStop, value); + } + + + //Velocity + __property double get_Velocity(void) + { + return dJointGetHingeParam(this->_id, dParamVel); + } + + __property void set_Velocity(double value) + { + dJointSetHingeParam(this->_id, dParamVel, value); + } + + + //MaxForce + __property double get_MaxForce(void) + { + return dJointGetHingeParam(this->_id, dParamFMax); + } + + __property void set_MaxForce(double value) + { + dJointSetHingeParam(this->_id, dParamFMax, value); + } + + + //FudgeFactor + __property double get_FudgeFactor(void) + { + return dJointGetHingeParam(this->_id, dParamFudgeFactor); + } + + __property void set_FudgeFactor(double value) + { + dJointSetHingeParam(this->_id, dParamFudgeFactor, value); + } + + + //Bounce + __property double get_Bounce(void) + { + return dJointGetHingeParam(this->_id, dParamBounce); + } + + __property void set_Bounce(double value) + { + dJointSetHingeParam(this->_id, dParamBounce, value); + } + + + //StopERP + __property double get_StopERP(void) + { + return dJointGetHingeParam(this->_id, dParamStopERP); + } + + __property void set_StopERP(double value) + { + dJointSetHingeParam(this->_id, dParamStopERP, value); + } + + + //StopCFM + __property double get_StopCFM(void) + { + return dJointGetHingeParam(this->_id, dParamStopCFM); + } + + __property void set_StopCFM(double value) + { + dJointSetHingeParam(this->_id, dParamStopCFM, value); + } + + + //GetAngle + __property double get_Angle(void) + { + return dJointGetHingeAngle(this->_id); + } + + + //GetAngleRate + __property double get_AngleRate(void) + { + return dJointGetHingeAngleRate(this->_id); + } + + }; + +} + +// void SetSuspensionERP(double value); +// double GetSuspensionERP(void); + +// void SetSuspensionCFM(double value); +// double GetSuspensionCFM(void); + +/* + //SetSuspensionERP + void JointHinge::SetSuspensionERP(double value) + { + dJointSetHingeParam(this->_id, dParamSuspensionERP, value); + } + + //GetSuspensionERP + double JointHinge::GetSuspensionERP(void) + { + return dJointGetHingeParam(this->_id, dParamSuspensionERP); + } + + + //SetSuspensionCFM + void JointHinge::SetSuspensionCFM(double value) + { + dJointSetHingeParam(this->_id, dParamSuspensionCFM, value); + } + + //GetSuspensionCFM + double JointHinge::GetSuspensionCFM(void) + { + return dJointGetHingeParam(this->_id, dParamSuspensionCFM); + } + +*/ diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp new file mode 100644 index 0000000..94fd7a7 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp @@ -0,0 +1,133 @@ +#include "StdAfx.h" + +#include +#include "jointhinge2.h" + +namespace ODEManaged +{ + //Constructors + JointHinge2::JointHinge2(void) : Joint(){} + + JointHinge2::JointHinge2(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge2(world.Id(),0); + } + + JointHinge2::JointHinge2(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge2(world.Id(), jointGroup.Id()); + } + + //Destructor + JointHinge2::~JointHinge2(void){} + + //CreateHinge2 (overload 1) + void JointHinge2::Create(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge2(world.Id(), jointGroup.Id()); + } + + //CreateHinge2 (overload 2) + void JointHinge2::Create(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateHinge2(world.Id(),0); + } + + //SetAnchor1 + void JointHinge2::SetAnchor (double x, double y ,double z) + { + dJointSetHinge2Anchor(_id, x,y,z); + } + + //GetAnchor1 + Vector3 JointHinge2::GetAnchor() + { + Vector3 retVal; + dVector3 temp; + dJointGetHinge2Anchor(_id,temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + //SetAxis1 + void JointHinge2::SetAxis1 (double x, double y ,double z) + { + dJointSetHinge2Axis1(_id, x,y,z); + } + + //GetAxis1 + Vector3 JointHinge2::GetAxis1() + { + Vector3 retVal; + dVector3 temp; + dJointGetHinge2Axis1(_id,temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + //SetAxis2 + void JointHinge2::SetAxis2 (double x, double y ,double z) + { + dJointSetHinge2Axis2(_id, x,y,z); + } + + //GetAxis2 + Vector3 JointHinge2::GetAxis2() + { + Vector3 retVal; + dVector3 temp; + dJointGetHinge2Axis2(_id,temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + //GetAngle1 + double JointHinge2::GetAngle1 () + { + return dJointGetHinge2Angle1(this->_id); + } + + //GetAngle1Rate + double JointHinge2::GetAngle1Rate () + { + return dJointGetHinge2Angle1Rate(this->_id); + } + + ////GetAngle hmm, this doesn't exist + //double JointHinge2::GetAngle2 () + //{ + // return dJointGetHinge2Angle2(this->_id); + //} + + //GetAngle2Rate + double JointHinge2::GetAngle2Rate () + { + return dJointGetHinge2Angle2Rate(this->_id); + } + + + //Attach (overload 1) + void JointHinge2::Attach (Body &body1, Body &body2) + { + dJointAttach(_id, body1.Id(),body2.Id()); + } + + //Attach (overload 2) + //TODO: possibly add an overload that takes anchor as a param also. + void JointHinge2::Attach (Body &body1) + { + dJointAttach(_id, body1.Id(),0); + } + + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h new file mode 100644 index 0000000..e883ea8 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h @@ -0,0 +1,48 @@ +#pragma once + +#include "Joint.h" +#include "CommonMgd.h" + +namespace ODEManaged +{ + __gc public class JointHinge2 : public Joint + { + public: + + + //Constructors + + JointHinge2 (void); + JointHinge2 (World &world); + JointHinge2 (World &world, JointGroup &jointGroup); + + //Destructors + + virtual ~JointHinge2 (void); + + + //Methods + + //Overloaded Hinge.Create + void Create (World &world, JointGroup &jointGroup); + void Create (World &world); + + void SetAnchor (double x, double y, double z); + Vector3 GetAnchor (void); + + void SetAxis1 (double x, double y, double z); + Vector3 GetAxis1 (void); + + void SetAxis2 (double x, double y, double z); + Vector3 GetAxis2 (void); + + double GetAngle1 (void); + double GetAngle1Rate (void); + + //double GetAngle2 (void); + double GetAngle2Rate (void); + + void Attach (Body &body1, Body &body2); + void Attach( Body &body1); + }; +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp new file mode 100644 index 0000000..ab7ebd6 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp @@ -0,0 +1,102 @@ +#include "StdAfx.h" + +#include +#include "jointslider.h" + +namespace ODEManaged +{ + + //Constructors + + JointSlider::JointSlider(void) : Joint(){} + + + JointSlider::JointSlider(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateSlider(world.Id(), 0); + } + + + JointSlider::JointSlider(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateSlider(world.Id(), jointGroup.Id()); + } + + + //Destructor + + JointSlider::~JointSlider(void){} + + + //Methods + + //Overloaded Create + void JointSlider::Create(World &world, JointGroup &jointGroup) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateSlider(world.Id(), jointGroup.Id()); + } + + void JointSlider::Create(World &world) + { + if(this->_id) dJointDestroy(this->_id); + _id = dJointCreateSlider(world.Id(), 0); + } + + + //Overloaded Attach + void JointSlider::Attach(Body &body1, Body &body2) + { + dJointAttach(this->_id, body1.Id(), body2.Id()); + } + + void JointSlider::Attach(Body &body1) + { + dJointAttach(this->_id, body1.Id(), 0); + } + + + //SetAxis + void JointSlider::SetAxis(double x, double y, double z) + { + dJointSetSliderAxis(this->_id, x, y, z); + } + + //GetAxis + Vector3 JointSlider::GetAxis(void) + { + Vector3 retVal; + dVector3 temp; + dJointGetSliderAxis(this->_id, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + + //Movement Parameters + + //SetAllMovParams + void JointSlider::SetAllMovParams(double LoStop, double HiStop, + double Velocity, double MaxForce, + double FudgeFactor, double Bounce, + double StopERP, double StopCFM) + { + if (LoStop <= 0) + dJointSetHingeParam(this->_id, dParamLoStop, LoStop); + + if (HiStop >= 0) + dJointSetHingeParam(this->_id, dParamHiStop, HiStop); + + dJointSetSliderParam(this->_id, dParamVel, Velocity); + dJointSetSliderParam(this->_id, dParamFMax, MaxForce); + dJointSetSliderParam(this->_id, dParamFudgeFactor, FudgeFactor); + dJointSetSliderParam(this->_id, dParamBounce, Bounce); + dJointSetSliderParam(this->_id, dParamStopERP, StopERP); + dJointSetSliderParam(this->_id, dParamStopCFM, StopCFM); + } + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h new file mode 100644 index 0000000..7e96e59 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h @@ -0,0 +1,158 @@ +#pragma once + +#include "Joint.h" +#include "CommonMgd.h" + +namespace ODEManaged +{ + __gc public class JointSlider : public Joint + { + public: + + + //Constructors + + JointSlider(void); + JointSlider(World &world); + JointSlider(World &world, JointGroup &jointGroup); + + + //Destructors + + virtual ~JointSlider(void); + + + //Methods + + //Overloaded Create + void Create(World &world, JointGroup &jointGroup); + void Create(World &world); + + //Overloaded Attach + void Attach(Body &body1, Body &body2); + void Attach(Body &body1); + + void SetAxis(double x, double y, double z); + Vector3 GetAxis(void); + + void SetAllMovParams(double LoStop, double HiStop, + double Velocity, double MaxForce, + double FudgeFactor, double Bounce, + double StopERP, double StopCFM); + + + //Properties + + //LoStop + __property double get_LoStop(void) + { + return dJointGetSliderParam(this->_id, dParamLoStop); + } + + __property void set_LoStop(double value) + { + if (value <=0) + dJointSetSliderParam(this->_id, dParamLoStop, value); + } + + + //HiStop + __property double get_HiStop(void) + { + return dJointGetSliderParam(this->_id, dParamHiStop); + } + + __property void set_HiStop(double value) + { + if (value >= 0) + dJointSetSliderParam(this->_id, dParamHiStop, value); + } + + + //Velocity + __property double get_Velocity(void) + { + return dJointGetSliderParam(this->_id, dParamVel); + } + + __property void set_Velocity(double value) + { + dJointSetSliderParam(this->_id, dParamVel, value); + } + + + //MaxForce + __property double get_MaxForce(void) + { + return dJointGetSliderParam(this->_id, dParamFMax); + } + + __property void set_MaxForce(double value) + { + dJointSetSliderParam(this->_id, dParamFMax, value); + } + + + //FudgeFactor + __property double get_FudgeFactor(void) + { + return dJointGetSliderParam(this->_id, dParamFudgeFactor); + } + + __property void set_FudgeFactor(double value) + { + dJointSetSliderParam(this->_id, dParamFudgeFactor, value); + } + + + //Bounce + __property double get_Bounce(void) + { + return dJointGetSliderParam(this->_id, dParamBounce); + } + + __property void set_Bounce(double value) + { + dJointSetSliderParam(this->_id, dParamBounce, value); + } + + + //StopERP + __property double get_StopERP(void) + { + return dJointGetSliderParam(this->_id, dParamStopERP); + } + + __property void set_StopERP(double value) + { + dJointSetSliderParam(this->_id, dParamStopERP, value); + } + + + //StopCFM + __property double get_StopCFM(void) + { + return dJointGetSliderParam(this->_id, dParamStopCFM); + } + + __property void set_StopCFM(double value) + { + dJointSetSliderParam(this->_id, dParamStopCFM, value); + } + + + //GetAngle + __property double get_Position(void) + { + return dJointGetSliderPosition(this->_id); + } + + + //GetAngleRate + __property double get_PositionRate(void) + { + return dJointGetSliderPositionRate(this->_id); + } + + }; +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll b/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll new file mode 100755 index 0000000..ccf2a41 Binary files /dev/null and b/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll differ diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp new file mode 100644 index 0000000..c9a7e19 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp @@ -0,0 +1,53 @@ +#include "StdAfx.h" + +#include +#include "Space.h" +#include "TEST.h" + +namespace ODEManaged +{ + + //Constructor + + Space::Space(void) + { + _id = dSimpleSpaceCreate(); + } + + Space::Space(int minlevel, int maxlevel) + { + _id = dHashSpaceCreate(); + dHashSpaceSetLevels(this->_id, minlevel, maxlevel); + } + + + //Destructor + + Space::~Space(void) + { + dSpaceDestroy(this->_id); + } + + + //Methods + + //Id + dSpaceID Space::Id() + { + return _id; + } + + + //Collide + void Space::Collide(void *data, dNearCallback *callback) + { + dSpaceCollide(this->_id, data, callback); + } + + + + + + + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Space.h b/libraries/ode-0.9/contrib/DotNetManaged/Space.h new file mode 100644 index 0000000..78e81ad --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Space.h @@ -0,0 +1,33 @@ +#pragma once + +#include "CommonMgd.h" + +namespace ODEManaged +{ + __gc public class Space + { + public: + + //Constructor + + Space(void); + Space(int minlevel, int maxlevel); + + //Destructor + + ~Space(void); + + + //Methods + + dSpaceID Id(void); + void Collide(void *data, dNearCallback *callback); + + + private: + + dSpaceID _id; + + }; + +} diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp new file mode 100644 index 0000000..b6c9d98 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp @@ -0,0 +1,5 @@ +// stdafx.cpp : source file that includes just the standard includes +// ODEManaged.pch will be the pre-compiled header +// stdafx.obj will contain the pre-compiled type information + +#include "stdafx.h" diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h new file mode 100644 index 0000000..2222759 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h @@ -0,0 +1,12 @@ +// stdafx.h : include file for standard system include files, +// or project specific include files that are used frequently, +// but are changed infrequently + +#pragma once + +#using + + + + + diff --git a/libraries/ode-0.9/contrib/DotNetManaged/TEST.h b/libraries/ode-0.9/contrib/DotNetManaged/TEST.h new file mode 100644 index 0000000..e2cdbc3 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/TEST.h @@ -0,0 +1,17 @@ + +#pragma once + +#include "CommonMgd.h" + +namespace ODEManaged +{ + +void RnearCallback(void *data, dGeomID o1, dGeomID o2) + { + } + +} + + + + diff --git a/libraries/ode-0.9/contrib/DotNetManaged/World.cpp b/libraries/ode-0.9/contrib/DotNetManaged/World.cpp new file mode 100644 index 0000000..beab21a --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/World.cpp @@ -0,0 +1,74 @@ +#include "StdAfx.h" + +#include +#include "World.h" + +namespace ODEManaged +{ + + //Constructor + + World::World(void) + { + /*dWorldID _temp = dWorldCreate(); + _id = _temp;*/ + _id = dWorldCreate(); + } + + + //Destructor + + World::~World(void) + { + dWorldDestroy(this->_id); + } + + + //Methods + + //Id + dWorldID World::Id() + { + return _id; + } + + + //SetGravity + void World::SetGravity(double x, double y, double z) + { + dWorldSetGravity(this->_id, x, y, z); + } + + + //Overloaded GetGravity + Vector3 World::GetGravity(void) + { + Vector3 retVal; + dVector3 temp; + dWorldGetGravity(this->_id, temp); + retVal.x = temp[0]; + retVal.y = temp[1]; + retVal.z = temp[2]; + return retVal; + } + + void World::GetGravity(double gravity __gc[]) + { + dVector3 temp; + dWorldGetGravity(this->_id, temp); + gravity[0] = temp[0]; + gravity[1] = temp[1]; + gravity[2] = temp[2]; + } + + + //Step + void World::Step(double stepSize) + { + dWorldStep(this->_id, stepSize); + } + +} + + + diff --git a/libraries/ode-0.9/contrib/DotNetManaged/World.h b/libraries/ode-0.9/contrib/DotNetManaged/World.h new file mode 100644 index 0000000..c4c60e5 --- /dev/null +++ b/libraries/ode-0.9/contrib/DotNetManaged/World.h @@ -0,0 +1,67 @@ +#pragma once + +#include "CommonMgd.h" + +namespace ODEManaged +{ + __gc public class World + { + public: + + //Constructor + + World(void); + + + //Destructor + + ~World(void); + + + // Methods + + dWorldID Id(void); + + void SetGravity(double x, double y, double z); + + //Overloaded GetGravity + Vector3 GetGravity(void); + void GetGravity(double gravity __gc[]); + + void Step(double stepSize); + + + //Properties + + //Constraint Force Mixing + __property void set_CFM(double cfm) + { + dWorldSetCFM(this->_id,cfm); + } + + __property double get_CFM(void) + { + return dWorldGetCFM(this->_id); + } + + + //Error Reduction Parameter + __property void set_ERP(double erp) + { + dWorldSetERP(this->_id,erp); + } + + __property double get_ERP(void) + { + return dWorldGetERP(this->_id); + } + + + private: + + dWorldID _id; + + }; + +} + diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp new file mode 100644 index 0000000..26b77b0 --- /dev/null +++ b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp @@ -0,0 +1,218 @@ + +/* ************************************************************************ */ +/* + grouped and transformed geometry functions + author: Tim Schmidt tisch@uni-paderborn.de +*/ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "objects.h" +#include "array.h" +#include "geom_internal.h" + +// given a pointer `p' to a dContactGeom, return the dContactGeom at +// p + skip bytes. + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) + + +// ############################################################################ + +int dGeomTransformGroupClass = -1; +// ############################################################################ + +struct dxGeomTransformGroup { + dArray parts; // all the geoms that make up the group + dVector3 relativePosition; + dMatrix3 relativeRotation; +}; +// ############################################################################ + +void dGeomTransformGroupSetRelativePosition (dxGeom *g, dReal x, dReal y, dReal z) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + transformGroup->relativePosition[0] = x; + transformGroup->relativePosition[1] = y; + transformGroup->relativePosition[2] = z; +} +// ############################################################################ + +void dGeomTransformGroupSetRelativeRotation (dxGeom *g, const dMatrix3 R) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + memcpy (transformGroup->relativeRotation,R,sizeof(dMatrix3)); +} +// ############################################################################ + +const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + return transformGroup->relativePosition; +} +// ############################################################################ + +const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g) +{ + dAASSERT (g); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + return transformGroup->relativeRotation; +} +// ############################################################################ + +static void computeFinalTransformation (const dxGeom *tg, const dxGeom *part) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(tg); + dMULTIPLY0_331 (part->pos,tg->R,transformGroup->relativePosition); + part->pos[0] += tg->pos[0]; + part->pos[1] += tg->pos[1]; + part->pos[2] += tg->pos[2]; + dMULTIPLY0_333 (part->R,tg->R,transformGroup->relativeRotation); +} +// ############################################################################ + +int dCollideTransformGroup (const dxGeom *o1, const dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(o1); + if (transformGroup->parts.size() == 0) + { + return 0; + } + int numleft = flags & NUMC_MASK; + if (numleft == 0) numleft = 1; + flags &= ~NUMC_MASK; + int num=0, i=0; + while (i < transformGroup->parts.size() && numleft > 0) + { + dUASSERT (transformGroup->parts[i]->spaceid==0, + "GeomTransformGroup encapsulated object must not be in a space"); + dUASSERT (transformGroup->parts[i]->body==0, + "GeomTransformGroup encapsulated object must not be attached to a body"); + if (!o1->space_aabb) + { + computeFinalTransformation (o1, transformGroup->parts[i]); + } + dxBody *bodyBackup = transformGroup->parts[i]->body; + transformGroup->parts[i]->body = o1->body; + int n = dCollide (transformGroup->parts[i],const_cast(o2), + flags | numleft,contact,skip); + transformGroup->parts[i]->body = bodyBackup; + contact = CONTACT (contact,skip*n); + numleft -= n; + num += n; + i++; + } + return num; +} +// ############################################################################ + +static dColliderFn * dGeomTransformGroupColliderFn (int num) +{ + return (dColliderFn *) &dCollideTransformGroup; +} +// ############################################################################ + +static void dGeomTransformGroupAABB (dxGeom *geom, dReal aabb[6]) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom); + aabb[0] = dInfinity; + aabb[1] = -dInfinity; + aabb[2] = dInfinity; + aabb[3] = -dInfinity; + aabb[4] = dInfinity; + aabb[5] = -dInfinity; + int i,j; + for (i=0; i < transformGroup->parts.size(); i++) + { + computeFinalTransformation (geom, transformGroup->parts[i]); + dReal aabb2[6]; + transformGroup->parts[i]->_class->aabb (transformGroup->parts[i],aabb2); + for (j=0; j<6; j += 2) if (aabb2[j] < aabb[j]) aabb[j] = aabb2[j]; + for (j=1; j<6; j += 2) if (aabb2[j] > aabb[j]) aabb[j] = aabb2[j]; + } +} +// ############################################################################ + +static void dGeomTransformGroupDtor (dxGeom *geom) +{ + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom); + transformGroup->parts.~dArray(); +} +// ############################################################################ + +dxGeom *dCreateGeomTransformGroup (dSpaceID space) +{ + if (dGeomTransformGroupClass == -1) { + dGeomClass c; + c.bytes = sizeof (dxGeomTransformGroup); + c.collider = &dGeomTransformGroupColliderFn; + c.aabb = &dGeomTransformGroupAABB; + c.aabb_test = 0; + c.dtor = dGeomTransformGroupDtor; + dGeomTransformGroupClass = dCreateGeomClass (&c); + } + dxGeom *g = dCreateGeom (dGeomTransformGroupClass); + if (space) + { + dSpaceAdd (space,g); + } + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + transformGroup->parts.constructor(); + dSetZero (transformGroup->relativePosition,4); + dRSetIdentity (transformGroup->relativeRotation); + return g; +} +// ############################################################################ + +void dGeomTransformGroupAddGeom (dxGeom *g, dxGeom *obj) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + transformGroup->parts.push (obj); +} +// ############################################################################ + +void dGeomTransformGroupRemoveGeom (dxGeom *g, dxGeom *obj) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + for (int i=0; i < transformGroup->parts.size(); i++) { + if (transformGroup->parts[i] == obj) { + transformGroup->parts.remove (i); + return; + } + } +} +// ############################################################################ + +dxGeom * dGeomTransformGroupGetGeom (dxGeom *g, int i) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + dAASSERT (i >= 0 && i < transformGroup->parts.size()); + return transformGroup->parts[i]; +} +// ############################################################################ + +int dGeomTransformGroupGetNumGeoms (dxGeom *g) +{ + dUASSERT (g && g->_class->num == dGeomTransformGroupClass, + "argument not a geom TransformGroup"); + dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g); + return transformGroup->parts.size(); +} diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h new file mode 100644 index 0000000..705fdb9 --- /dev/null +++ b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h @@ -0,0 +1,29 @@ + +/* ************************************************************************ */ +/* + grouped and transformed geometry functions + author: Tim Schmidt tisch@uni-paderborn.de +*/ + + +#ifdef __cplusplus +extern "C" { +#endif + + +extern int dGeomTransformGroupClass; + +void dGeomTransformGroupSetRelativePosition (dGeomID g, dReal x, dReal y, dReal z); +void dGeomTransformGroupSetRelativeRotation (dGeomID g, const dMatrix3 R); +const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g); +const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g); +dGeomID dCreateGeomTransformGroup (dSpaceID space); +void dGeomTransformGroupAddGeom (dGeomID tg, dGeomID obj); +void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj); +dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i); +int dGeomTransformGroupGetNumGeoms (dGeomID tg); + + +#ifdef __cplusplus +} +#endif diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt b/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt new file mode 100644 index 0000000..bca0e66 --- /dev/null +++ b/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt @@ -0,0 +1,148 @@ +README for GeomTransformGroup by Tim Schmidt. +--------------------------------------------- + +This is a patch to add the dGeomTransformGroup object to the list of geometry +objects. + +It should work with the cvs version of the ode library from 07/24/2002. + +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + +comment by russ smith: this code is easy to use with the rest of ODE. +simply copy GeomTransformGroup.cpp to ode/src and copy GeomTransformGroup.h +to include/ode. then add GeomTransformGroup.cpp to the ODE_SRC variable +in the makefile. rebuild, and you're done! of course i could have done all +this for you, but i prefer to keep GeomTransformGroup separated from the +rest of ODE for now while other issues with the collision system are +resolved. + +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + +Description: + +The dGeomTransformGroup is an adaption of the TransformGroup known from +Java3D (and maybe other libraries with a similar scene graph representation). +It can be used to build an arbitrarily structured tree of objects that are +each positioned relative to the particular parent node. + +If you have a plane for example, there is one root node associated with the +plane's body and another three transformgroups placed 'under' this node. One +with the fuselage (cappedcylinder) under it and two with the underlying wings +(flat boxes). And if you want to add engines, simply put them 'next to' the +wings under another two transformgroups. + +bodyTG ---> associated with dBody + | + +--fuselageTG + | | + | +--fuselageCylinder + | + +--leftwingTG + | | + | +--wingBox + | | + | +--leftengineTG + | | + | +--leftengineCylinder + | + +--rightwingTG + | + +--wingBox + | + +--rightengineTG + | + +--rightengineCylinder + +This is a method to easily compose objects without the necessity of always +calculating global coordinates. But apart from this there is something else +that makes dGeomTransformGroups very valuable. + +Maybe you remember that some users reported the problem of acquiring the +correct bodies to be attached by a contactjoint in the nearCallback when +using dGeomGroups and dGeomTransforms at the same time. This results from the +fact that dGeomGroups are not associated with bodies while all other +geometries are. + +So, as you can see in the nearCallback of the the test_buggy demo you have to +attach the contactjoint with the bodies that you get from the geometries that +are stored in the contact struct (-> dGeomGetBody(contacts[i].geom.g1)). +Normally you would do this by asking o1 and o2 directly with dGeomGetBody(o1) +and dGeomGetBody(o2) respectively. + +As a first approach you can overcome that problem by testing o1 and o2 if +they are groups or not to find out how to get the corresponding bodies. + +However this will fail if you want grouped transforms that are constructed +out of dGeomTransforms encapsulated in a dGeomGroup. According to the test +you use contacts[i].geom.g1 to get the right body. Unfortunately g1 is +encapsulated in a transform and therefore not attached to any body. In this +case the dGeomTransform 'in the middle' would have been the right object to +be asked for the body. + +You may now conclude that it is a good idea to unwrap the group encapsulated +geoms at the beginning of the nearcallback and use dGeomGetBody(o1) +consistently. But keep in mind that this also means not to invoke +dCollide(..) on groups at all and therefore not to expoit the capability of +dGeomGroups to speed up collision detection by the creation of bounding boxes +around the encapsulated geometry. + +Everything becomes even worse if you create a dGeomTransform that contains a +dGeomGroup of geoms. The function that cares about the collision of +transforms with other objects uses the position and rotation of the +respective encapsulated object to compute its final position and orientation. +Unfortunately dGeomGroups do not have a position and rotation, so the result +will not be what you have expected. + +Here the dGeomTransformGroups comes into operation, because it combines the +advantages and capabilities of the dGeomGroup and the dGeomTransform. +And as an effect of synergy it is now even possible to set the position of a +group of geoms with one single command. +Even nested encapsulations of dGeomTransformGroups in dGeomTransformGroups +should be possible (to be honest, I have not tried that so far ;-) ). + +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + +API: + +dGeomID dCreateGeomTransformGroup (dSpaceID space); + - create a GeomTransformGroup + +void dGeomTransformGroupAddGeom (dGeomID tg, dGeomID obj); + - Comparable to dGeomTransformSetGeom or dGeomGroupAdd + - add objects to this group + +void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj); + - remove objects from this group + +void dGeomTransformGroupSetRelativePosition + (dGeomID g, dReal x, dReal y, dReal z); +void dGeomTransformGroupSetRelativeRotation + (dGeomID g, const dMatrix3 R); + - Comparable to setting the position and rotation of all the + dGeomTransform encapsulated geometry. The difference + is that it is global with respect to this group and therefore + affects all geoms in this group. + - The relative position and rotation are attributes of the + transformgroup, so the position and rotation of the individual + geoms are not changed + +const dReal * dGeomTransformGroupGetRelativePosition (dGeomID g); +const dReal * dGeomTransformGroupGetRelativeRotation (dGeomID g); + - get the relative position and rotation + +dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i); + - Comparable to dGeomGroupGetGeom + - get a specific geom of the group + +int dGeomTransformGroupGetNumGeoms (dGeomID tg); + - Comparable to dGeomGroupGetNumGeoms + - get the number of geoms in the group + + +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + +Tim Schmidt +student of computer science +University of Paderborn, Germany +tisch@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 new file mode 100644 index 0000000..c06768c Binary files /dev/null and b/libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin differ diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt b/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt new file mode 100644 index 0000000..7c1f8a4 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt @@ -0,0 +1,95 @@ +----------------------------- +ODE - Mac CFM Carbon Port +(contact Frank Condello with questions regarding this port) + +Although ODE contains a MacOSX makefile, and some individuals have implemented ODE in +Cocoa, I opted to use (and prefer) CodeWarrior. This also opens up ODE to MacOS8 & 9 +users, without scarfing functionality in MacOSX (same binaries run on both platforms). + +The 'ode_CW7.mcp' project contains release and debug targets to create static ODE and +DrawStuff libraries. + +'examples_CW7.mcp' contains targets for the entire ODE test suite, plus a couple other +test programs which were posted to the ODE mailing list. + + +----------------------------- +Compiling Notes: + +You'll need to extract the CodeWarrior projects from the 'CW7_projects.sit.bin' archive +(They're nearly a meg uncompressed so this was done to be bandwith friendly on the CVS). + +Projects require CodeWarrior 7 or above (recreating them with earlier versions shouldn't +be too difficult). The projects use relative paths and are meant to be compiled from +'contrib/Mac_CFMCarbon/'. Don't move them! + +All the libraries build into the 'lib/' directory, all test applications build into +'contrib/Mac_CFMCarbon/mac_testbin/' (and must be run from that directory since the +texture path is hard-coded). + +You'll need to compile the release ODE library, and the DrawStuff library before +compiling the examples. + +The ODE 'configurator' has not been ported, but a Mac-friendly 'config.h' header has been +manually hacked together (all PPC Macs should be fine with this header). Single or double +precision can be defined in the 'CommonPrefix.h' header found in +'contrib/Mac_CFMCarbon/mac_source/'. + +'contrib/Mac_CFMCarbon/mac_source/' also contains any mac specific additions to the main source. +The directory structure here matches the main source tree, and I would recommend that this +format is maintained when making additions, since the access paths are touchy (more below...) + +Some issues were encountered with duplicate header names. CodeWarrior tends to be +unforgiving about this sort of thing but fudging with the access paths eventually +cleared up the problem. If ODE fails to compile, make sure the and +"objects.h" or and are actually pointing to the correct header. + +You'll need Apple's OpenGL SDK (with GLUT) in your compiler path to build DrawStuff. I've +added redirection headers in 'contrib/Mac_CFMCarbon/mac_source/include/GL/' to properly +link with the Apple headers (since the projects are set to follow DOS paths). + +The examples link against a crapload of static libraries, but my initial builds using +ODE, MSL, GLUT, and DrawStuff shared/merged DLL's proved unstable (mostly problems with +SIOUX spawning multiple sessions, and crashes in Classic). Static libs just worked better +in the end, but the test apps are a little bloated as a result, and need to be re-linked +whenever a change to a library is made. + +IMPORTANT: You must use the same 'CommonPrefix.h' settings for libraries, and test apps +(i.e. double or single precision). + + +----------------------------- +Running the test apps: + +The test apps will show the SIOUX CLI prompt when run. Just hit OK to ignore it, or add any +DrawStuff arguments. You'll want to log output to a file for 'test_ode'. + +There are two extra test programs in the 'mac_source' directory. Both were posted to the ODE +mailing list by OSX users. 'test_stability1' visualizes some internal issues with ODE, and +'test_stacktest' is a standalone GLUT program (doesn't use DrawStuff) that can be useful +to stress test the library, and give you an idea of just how much stack memory you're +going to need for large systems. + +ISSUES: + +The carbon DrawStuff lib uses GLUT to make life easy, but GLUT isn't exactly bug-free +or stable on the Mac... Try moving the mouse around if a simulation is running slowly +on OS9 (it's not ODE's fault, but rather a poor carbon GLUT implementation - seems GLUT stalls +when it's not getting system events - I haven't seen this problem on OSX). + +The 3D view may not update if typing in the SIOUX console window. + +You cannot pass startup args to GLUT due to the way the DrawStuff library initializes. + +'Write Frames' doesn't actually do anything at the moment. + +The 'test_joints' app seems broken (though I don't know what the intended effect should be) + + +----------------------------- +TODO: + +- Re-add shared library targets (if stability issues are resolved). +- Implement 'Write Frames' in DrawStuff. +- Write a Carbon compatible configurator +- 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 new file mode 100644 index 0000000..5948b3e --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h @@ -0,0 +1,6 @@ +#define TARGET_API_MAC_CARBON 1 +#define finite isfinite +#define dNODEBUG 1 + +// Comment out for single precision +#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 new file mode 100644 index 0000000..6122528 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h @@ -0,0 +1,6 @@ +#ifndef prefix_h +#define prefix_h + +#include "CommonPrefix.h" + +#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 new file mode 100644 index 0000000..0e328a9 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h @@ -0,0 +1,10 @@ +#ifndef prefix_h +#define prefix_h + +#include "CommonPrefix.h" + +#ifdef dNODEBUG +#undef dNODEBUG +#endif + +#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 new file mode 100644 index 0000000..1dedfc9 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h @@ -0,0 +1,13 @@ +#ifndef prefix_h +#define prefix_h + +#include "CommonPrefix.h" + +// Hack to automatically call SIOUX's CLI interface for the test apps +#include +#include +int fmain (int argc, char **argv); +int main (int argc, char **argv) { argc = ccommand(&argv); return fmain(argc, argv); } +#define main(argc, argv) fmain(argc, argv) + +#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 new file mode 100644 index 0000000..f8f5022 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h @@ -0,0 +1,13 @@ +#ifndef prefix_h +#define prefix_h + +#include "CommonPrefix.h" + +// Hack to automatically call SIOUX's CLI interface for the test apps +#include +#include +int fmain (); +int main (int argc, char **argv) { argc = ccommand(&argv); return fmain(); } +#define main() fmain() + +#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 new file mode 100644 index 0000000..6122528 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h @@ -0,0 +1,6 @@ +#ifndef prefix_h +#define prefix_h + +#include "CommonPrefix.h" + +#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 new file mode 100644 index 0000000..eb0b144 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp @@ -0,0 +1,281 @@ +/************************************************************************* + * * + * DrawStuff Library, Copyright (C) 2001 Russell L. Smith. * + * Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library (see the file LICENSE.TXT); if not, * + * write to the Free Software Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA. * + * * + *************************************************************************/ + +// main window and event handling for Mac CFM Carbon + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "internal.h" + + +//*************************************************************************** +// error handling for unix (works just fine with SIOUX) + +static void printMessage (char *msg1, char *msg2, va_list ap) +{ + fflush (stderr); + fflush (stdout); + fprintf (stderr,"\n%s: ",msg1); + vfprintf (stderr,msg2,ap); + fprintf (stderr,"\n"); + fflush (stderr); +} + +extern "C" void dsError (char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + printMessage ("Error",msg,ap); + exit (1); +} + + +extern "C" void dsDebug (char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + printMessage ("INTERNAL ERROR",msg,ap); + // *((char *)0) = 0; ... commit SEGVicide ? + abort(); +} + +extern "C" void dsPrint (char *msg, ...) +{ + va_list ap; + va_start (ap,msg); + vprintf (msg,ap); +} + +//*************************************************************************** +// openGL window + +// window and openGL +static int width=0,height=0; // window size +static int last_key_pressed=0; // last key pressed in the window +static int pause=0; // 1 if in `pause' mode +static int singlestep=0; // 1 if single step key pressed +static int writeframes=0; // 1 if frame files to be written +static dsFunctions *gfn; +static int frame = 1; + +float getTime (void) +{ + UnsignedWide ms; + + Microseconds(&ms); + return ms.lo / 1000000.0; +} + + +static void captureFrame (int num) +{ +// TODO +} + +static void reshape(int w, int h) +{ + width = w; + height = h; +} + +static void draw(void) +{ + dsDrawFrame (width,height,gfn,pause && !singlestep); + singlestep = 0; + glutSwapBuffers(); + + if (pause==0 && writeframes) { + captureFrame (frame); + frame++; + } +} + +static void idle(void) +{ + static float lasttime=0; + float t; + + // Try to maintain a reasonable rate (good enough for testing anyway) + t = getTime(); + if (lasttime < t) { + lasttime = t+0.005; + draw(); + } +} + +static void key(unsigned char key, int x, int y) +{ + if (!glutGetModifiers()) { + + if (key >= ' ' && key <= 126 && gfn->command) gfn->command (key); + + // GLUT_ACTIVE_CTRL doesn't seem to be working, so we use Alt + } else if (glutGetModifiers()&GLUT_ACTIVE_ALT) { + + switch (key) { + case 't': case 'T': + dsSetTextures (dsGetTextures() ^ 1); + break; + case 's': case 'S': + dsSetShadows (dsGetShadows() ^ 1); + break; + case 'p': case 'P': + pause ^= 1; + singlestep = 0; + break; + case 'o': case 'O': + if (pause) singlestep = 1; + break; + case 'v': case 'V': { + float xyz[3],hpr[3]; + dsGetViewpoint (xyz,hpr); + printf ("Viewpoint = (%.4f,%.4f,%.4f,%.4f,%.4f,%.4f)\n", + xyz[0],xyz[1],xyz[2],hpr[0],hpr[1],hpr[2]); + break; + } + // No case 'X' - Quit works through the Mac system menu, or cmd-q + case 'w': case 'W': + writeframes ^= 1; + if (writeframes) printf ("Write frames not done yet!\n");// TODO + break; + } + } + + last_key_pressed = key; +} + +static int mx=0,my=0; // mouse position +static int mode = 0; // mouse button bits + +static void MouseDown(int button, int state, int x, int y) +{ + if(button == GLUT_LEFT_BUTTON) + { + if(state == GLUT_DOWN) + mode |= 1; + else if(state == GLUT_UP) + mode &= (~1); + } + else if (button == GLUT_MIDDLE_BUTTON) + { + if(state == GLUT_DOWN) + mode |= 3; + else if(state == GLUT_UP) + mode &= (~3); + } + else if (button == GLUT_RIGHT_BUTTON) + { + if(state == GLUT_DOWN) + mode |= 2; + else if(state == GLUT_UP) + mode &= (~2); + } + + mx = x; + my = y; +} + +static void MouseMove(int x, int y) +{ + dsMotion (mode, x - mx, y - my); + mx = x; + my = y; +} + +static void createMainWindow (int _width, int _height) +{ + // So GLUT doesn't complain + int argc = 0; + char **argv = NULL; + + // initialize variables + width = _width; + height = _height; + last_key_pressed = 0; + + if (width < 1 || height < 1) dsDebug (0,"bad window width or height"); + + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); + glutInitWindowSize(_width, _height); + glutInitWindowPosition(100, 100); + glutCreateWindow("ODE Simulation"); + + glutKeyboardFunc(key); + glutMotionFunc(MouseMove); + glutMouseFunc(MouseDown); + glutReshapeFunc(reshape); + glutDisplayFunc(idle); + glutIdleFunc(idle); +} + +void dsPlatformSimLoop (int window_width, int window_height, dsFunctions *fn, + int initial_pause) +{ + SIOUXSettings.initializeTB = false; + SIOUXSettings.standalone = false; + SIOUXSettings.setupmenus = false; + SIOUXSettings.autocloseonquit = true; + SIOUXSettings.asktosaveonclose = false; + + gfn = fn; + pause = initial_pause; + + printf ( + "\n" + "Simulation test environment v%d.%02d\n" + " Option-P : pause / unpause (or say `-pause' on command line).\n" + " Option-O : single step when paused.\n" + " Option-T : toggle textures (or say `-notex' on command line).\n" + " Option-S : toggle shadows (or say `-noshadow' on command line).\n" + " Option-V : print current viewpoint coordinates (x,y,z,h,p,r).\n" + " Option-W : write frames to ppm files: frame/frameNNN.ppm\n" + "\n" + "Change the camera position by clicking + dragging in the window.\n" + " Left button - pan and tilt.\n" + " Right button - forward and sideways.\n" + " Left + Right button (or middle button) - sideways and up.\n" + "\n",DS_VERSION >> 8,DS_VERSION & 0xff); + + createMainWindow (window_width, window_height); + dsStartGraphics (window_width,window_height,fn); + + if (fn->start) fn->start(); + + glutMainLoop(); + + if (fn->stop) fn->stop(); + dsStopGraphics(); +} + +extern "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 new file mode 100644 index 0000000..4acaeed --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h @@ -0,0 +1,2 @@ +// A little hackaround (Apple use / in the FILENAME, which doesn't work when following DOS paths) +#include \ 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 new file mode 100644 index 0000000..5b4a791 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h @@ -0,0 +1,2 @@ +// A little hackaround (Apple use / in the FILENAME, which doesn't work when following DOS paths) +#include \ 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 new file mode 100644 index 0000000..bb889f9 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h @@ -0,0 +1,52 @@ +/* This file has been manually hacked together for the Mac CFM Carbon build - Frank. */ + +#ifndef _ODE_CONFIG_H_ +#define _ODE_CONFIG_H_ + +/* standard system headers */ +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* #define PENTIUM 1 -- not a pentium */ + +/* integer types (we assume int >= 32 bits) */ +typedef char int8; +typedef unsigned char uint8; +typedef int int32; +typedef unsigned int uint32; + +/* an integer type that we can safely cast a pointer to and from without loss of bits. */ +typedef unsigned int intP; + +#ifdef PRECISION_DOUBLE + + /*select the base floating point type*/ + #define dDOUBLE 1 + + /* the floating point infinity */ + #define dInfinity DBL_MAX + +#else + + /* select the base floating point type */ + #define dSINGLE 1 + + /* the floating point infinity */ + #define dInfinity FLT_MAX + +#endif + +#ifdef __cplusplus +} +#endif +#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 new file mode 100644 index 0000000..79c066a --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp @@ -0,0 +1,289 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include +#include + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCappedCylinder dsDrawCappedCylinderD +#endif + + +// some constants + +#define DENSITY (5.0) // density of all objects + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom; // geometry representing this body +}; + +static dWorldID world; +static dSpaceID space; +static MyObject fallingObject; +static dGeomID box1, box2; +static dJointGroupID contactgroup; + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnected (b1,b2)) return; + + dContact contact[4]; // up to 3 contacts per box + for (i=0; i<4; i++) { + contact[i].surface.mode = dContactBounce; //dContactMu2; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.5; + contact[i].surface.bounce_vel = 0.1; + } + if (int numc = dCollide (o1,o2,4,&contact[0].geom,sizeof(dContact))) { + // dMatrix3 RI; + // dRSetIdentity (RI); + // const dReal ss[3] = {0.02,0.02,0.02}; + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + int i,k; + dReal sides[3]; + dMass m; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c') { + // Destroy the currently falling object and replace it by an instance of the requested type + if (fallingObject.body) { + dBodyDestroy (fallingObject.body); + dGeomDestroy (fallingObject.geom); + memset (&fallingObject, 0, sizeof(fallingObject)); + } + + fallingObject.body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + // Start out centered above the V-gap + dBodySetPosition (fallingObject.body, 0,0,5); + +#if 0 + dMatrix3 R; + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dBodySetRotation (fallingObject.body,R); + dBodySetData (fallingObject.body,(void*) i); +#endif + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + fallingObject.geom = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); + fallingObject.geom = dCreateCCylinder (space,sides[0],sides[1]); + } + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + fallingObject.geom = dCreateSphere (space,sides[0]); + } + + dGeomSetBody (fallingObject.geom,fallingObject.body); + + dBodySetMass (fallingObject.body,&m); + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R) +{ + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCCylinderClass) { + dReal radius,length; + dGeomCCylinderGetParams (g,&radius,&length); + dsDrawCappedCylinder (pos,R,length,radius); + } + /* + else if (type == dGeomTransformClass) { + dGeomID g2 = dGeomTransformGetGeom (g); + const dReal *pos2 = dGeomGetPosition (g2); + const dReal *R2 = dGeomGetRotation (g2); + dVector3 actual_pos; + dMatrix3 actual_R; + dMULTIPLY0_331 (actual_pos,R,pos2); + actual_pos[0] += pos[0]; + actual_pos[1] += pos[1]; + actual_pos[2] += pos[2]; + dMULTIPLY0_333 (actual_R,R,R2); + drawGeom (g2,actual_pos,actual_R); + } + */ +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + if (!pause) dWorldStep (world,0.0005); + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + + // draw the falling object + dsSetColor (1,0,0); + drawGeom (fallingObject.geom,0,0); + + // draw the constraining boxes + dsSetColor(0.8, 1, 0.8); + drawGeom (box1,0,0); + drawGeom (box2,0,0); +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = "../../drawstuff/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + + world = dWorldCreate(); + space = dHashSpaceCreate(); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + dWorldSetCFM (world,1e-5); + dCreatePlane (space,0,0,1,0); + memset (&fallingObject,0,sizeof(fallingObject)); + + // Create two flat boxes, just slightly off vertical and a bit apart for stuff to fall in between. + // Don't create bodies for these boxes -- they'll be immovable instead. + { + dReal sides[3]; + dMatrix3 R; + + sides[0] = 4; + sides[1] = 0.2; + sides[2] = 3; + + box1 = dCreateBox (space,sides[0],sides[1],sides[2]); + dGeomSetPosition (box1, 0, sides[1], sides[2]/2); + dRFromAxisAndAngle (R, 1, 0, 0, -0.1); + dGeomSetRotation (box1, R); + + box2 = dCreateBox (space,sides[0],sides[1],sides[2]); + dGeomSetPosition (box2, 0, -sides[1], sides[2]/2); + dRFromAxisAndAngle (R, 1, 0, 0, 0.1); + dGeomSetRotation (box2, R); + } + + // Pretend to drop a box to start + command('b'); + + // run simulation + dsSimulationLoop (argc,argv,640,480,&fn); + + dJointGroupDestroy (contactgroup); + dSpaceDestroy (space); + dWorldDestroy (world); + + return 0; +} 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 new file mode 100644 index 0000000..e49fd73 --- /dev/null +++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c @@ -0,0 +1,197 @@ +#include +#include +#include "ode.h" + +#define NUMBODIES 80 + +#define USE_SPHERE 0 +#define USE_HELIX 1 +#define USE_TORQUE 1 +#define USE_WEIRD_MATRIX_OPS 0 + +#define CONTACTS 1 + +dWorldID aWorld; +dSpaceID aSpace; +float cycle = 0, fade; +dJointGroupID aContactGroup; +dBodyID bodies[NUMBODIES]; +dGeomID geoms[NUMBODIES]; +GLfloat colors[NUMBODIES][4]; +unsigned int contactsThisFrame; + +void kglTransformByODEGeom(dGeomID geom) { + const dReal *p = dGeomGetPosition(geom); + const dReal *R = dGeomGetRotation(geom); + GLdouble glm[16]; + + glm[0] = R[0]; glm[1] = R[4]; glm[2] = R[8]; glm[3] = 0; + glm[4] = R[1]; glm[5] = R[5]; glm[6] = R[9]; glm[7] = 0; + glm[8] = R[2]; glm[9] = R[6]; glm[10] = R[10];glm[11] = 0; + glm[12] = p[0]; glm[13] = p[1]; glm[14] = p[2]; glm[15] = 1; + + glMultMatrixd(glm); +} + +static void odeNearCallback(void *data, dGeomID g1, dGeomID g2) { + dBodyID b1 = dGeomGetBody(g1), + b2 = dGeomGetBody(g2); + dContact contact[CONTACTS]; + int contactsUsed, i; + + if (b1 && b2 && dAreConnected(b1, b2)) return; + + contactsUsed = dCollide(g1, g2, CONTACTS, &contact[0].geom, + sizeof(dContact)); + if (contactsUsed > CONTACTS) contactsUsed = CONTACTS; + + for (i = 0; i < contactsUsed; i++) { + contact[i].surface.mode = 0; + contact[i].surface.mu = 20.0; + + dJointAttach(dJointCreateContact(aWorld, aContactGroup, + &(contact[i])), b1, b2); + contactsThisFrame++; + } +} + +void myGlutResize(int w, int h) { + glViewport(0, 0, w, h); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective(45.0, (GLfloat)w / h, 1.0, 120.0); + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + glTranslatef(0, -6, -20); +} + +void myGlutIdle(void) { + const float step = 1.0/120; + int i; + + cycle = fmod(cycle + step / 4, 1); + fade = fabs(cycle * 2 - 1); + + contactsThisFrame = 0; + dSpaceCollide(aSpace, NULL, &odeNearCallback); + //printf("%u\n", contactsThisFrame); + dWorldStep(aWorld, step); + dJointGroupEmpty(aContactGroup); + + for (i = 0; i < NUMBODIES; i++) { + const dReal *cvel = dBodyGetLinearVel(bodies[i]); + dBodyAddForce(bodies[i], + -cvel[0] * 0.5, + -cvel[1] * 0.5, + -cvel[2] * 0.5 + ); + } + + glutPostRedisplay(); +} + +void myGlutDisplay(void) { + int i; + + glClearColor(fade * 0.15, 0, 0, 1); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (USE_WEIRD_MATRIX_OPS) glPushMatrix(); + for (i = 0; i < NUMBODIES; i++) { + if (!USE_WEIRD_MATRIX_OPS) glPushMatrix(); + kglTransformByODEGeom(geoms[i]); + glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, colors[i]); + glColor3f(fade * 1.5, 0, 0); +#if USE_SPHERE + glRotatef(90, 1, 0, 0); + glutSolidSphere(0.5, 9, 6); + glDisable(GL_LIGHTING); + glutWireSphere(0.5, 9, 6); +#else + glutSolidCube(1); + glDisable(GL_LIGHTING); + glutWireCube(1); +#endif + glEnable(GL_LIGHTING); + if (!USE_WEIRD_MATRIX_OPS) glPopMatrix(); + } + if (USE_WEIRD_MATRIX_OPS) glPopMatrix(); + + glutSwapBuffers(); +} + +int main(int argc, char **argv) { + printf("Initializing GLUT\n"); + + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); + glutInitWindowSize(400, 300); + glutInitWindowPosition(100, 100); + glutCreateWindow("ODE Crash Test"); + + glutDisplayFunc(myGlutDisplay); + glutReshapeFunc(myGlutResize); + glutIdleFunc(myGlutIdle); + + glPolygonOffset(1, 1); + glDepthFunc(GL_LEQUAL); + glEnable(GL_POLYGON_OFFSET_FILL); + glEnable(GL_DEPTH_TEST); + glEnable(GL_CULL_FACE); + glEnable(GL_LIGHTING); + glEnable(GL_LIGHT0); + myGlutResize(400, 300); + + printf("Creating ODE world\n"); + aWorld = dWorldCreate(); + aSpace = dHashSpaceCreate(); + aContactGroup = dJointGroupCreate(0); + dCreatePlane(aSpace, 0, 1, 0, 0); + dWorldSetGravity(aWorld, 0, -9.81, 0); + dWorldSetERP(aWorld, 0.4); + dWorldSetCFM(aWorld, 1e-10); + + printf("Creating objects\n"); + { + int i; + dMass mass; + + dMassSetBox(&mass, 1.0, 1, 1, 1); + + for (i = 0; i < NUMBODIES; i++) { + float fraction = (float)i / NUMBODIES; + + bodies[i] = dBodyCreate(aWorld); + dBodySetMass(bodies[i], &mass); +#if USE_SPHERE + geoms[i] = dCreateSphere(aSpace, 0.5); +#else + geoms[i] = dCreateBox(aSpace, 1, 1, 1); +#endif + dGeomSetBody(geoms[i], bodies[i]); + + if (USE_HELIX) { + float r = (i % 3 - 1) * (1.5+4*(1 - fraction)), + theta = (float)i / 4; + dBodySetPosition(bodies[i], + sin(theta) * r, + (float)i + 1, + cos(theta) * r + ); + } else { + dBodySetPosition(bodies[i], 0, (float)i * 2 + 1, 0); + } + if (USE_TORQUE) dBodyAddTorque(bodies[i], fraction*10, fraction*20, fraction*30); + + colors[i][0] = fraction; + colors[i][1] = 1 - fraction; + colors[i][2] = 1 - fabs(fraction * 2 - 1); + colors[i][3] = 1; + } + } + + printf("Starting simulation\n"); + glutMainLoop(); + + return 0; +} \ 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 new file mode 100644 index 0000000..8d2b86a --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs @@ -0,0 +1,18 @@ +using System; +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +[assembly: AssemblyTitle("Drawstuff.NET")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("Ode.NET")] +[assembly: AssemblyCopyright("")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] +[assembly: ComVisible(false)] +[assembly: Guid("b2a39dd4-dd67-4e8a-af70-d3b412da8850")] +[assembly: AssemblyVersion("0.7.0.0")] +[assembly: AssemblyFileVersion("0.7.0.0")] +[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 new file mode 100644 index 0000000..aa84966 --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs @@ -0,0 +1,58 @@ +using System; +using System.Runtime.InteropServices; +using Ode.NET; + +namespace Drawstuff.NET +{ +#if dDOUBLE + using dReal = System.Double; +#else + using dReal = System.Single; +#endif + + public static class ds + { + public const int VERSION = 2; + + public enum Texture + { + None, + Wood + } + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void CallbackFunction(int arg); + + [StructLayout(LayoutKind.Sequential)] + public struct Functions + { + public int version; + public CallbackFunction start; + public CallbackFunction step; + public CallbackFunction command; + public CallbackFunction stop; + public string path_to_textures; + } + + [DllImport("drawstuff", EntryPoint="dsDrawBox")] + public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides); + + [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")] + public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius); + + [DllImport("drawstuff", EntryPoint = "dsDrawConvex")] + public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("drawstuff", EntryPoint="dsSetColor")] + public static extern void SetColor(float red, float green, float blue); + + [DllImport("drawstuff", EntryPoint="dsSetTexture")] + public static extern void SetTexture(Texture texture); + + [DllImport("drawstuff", EntryPoint="dsSetViewpoint")] + public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr); + + [DllImport("drawstuff", EntryPoint="dsSimulationLoop")] + public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn); + } +} diff --git a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua new file mode 100644 index 0000000..d777ffb --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua @@ -0,0 +1,19 @@ +package.name = "Drawstuff.NET" +package.kind = "dll" +package.language = "c#" + +if (options["with-doubles"]) then + package.defines = { "dDOUBLE" } +else + package.defines = { "dSINGLE " } +end + +package.links = { + "System", + "Ode.NET" +} + +package.files = { + "AssemblyInfo.cs", + "Drawstuff.cs" +} diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs b/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs new file mode 100644 index 0000000..6a8c2b6 --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs @@ -0,0 +1,18 @@ +using System; +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +[assembly: AssemblyTitle("Ode.NET")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("Ode.NET")] +[assembly: AssemblyCopyright("")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] +[assembly: ComVisible(false)] +[assembly: Guid("1347a35e-c32b-4ff6-8064-7d10b2cc113b")] +[assembly: AssemblyVersion("0.7.0.0")] +[assembly: AssemblyFileVersion("0.7.0.0")] +[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 new file mode 100644 index 0000000..0603810 --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs @@ -0,0 +1,1732 @@ +using System; +using System.Runtime.InteropServices; +using System.Security; + +namespace Ode.NET +{ +#if dDOUBLE + using dReal = System.Double; +#else + using dReal = System.Single; +#endif + + public static class d + { + public static dReal Infinity = dReal.MaxValue; + + #region Flags and Enumerations + + [Flags] + public enum ContactFlags : int + { + Mu2 = 0x001, + FDir1 = 0x002, + Bounce = 0x004, + SoftERP = 0x008, + SoftCFM = 0x010, + Motion1 = 0x020, + Motion2 = 0x040, + Slip1 = 0x080, + Slip2 = 0x100, + Approx0 = 0x0000, + Approx1_1 = 0x1000, + Approx1_2 = 0x2000, + Approx1 = 0x3000 + } + + public enum GeomClassID : int + { + SphereClass, + BoxClass, + CapsuleClass, + CylinderClass, + PlaneClass, + RayClass, + ConvexClass, + GeomTransformClass, + TriMeshClass, + HeightfieldClass, + FirstSpaceClass, + SimpleSpaceClass = FirstSpaceClass, + HashSpaceClass, + QuadTreeSpaceClass, + LastSpaceClass = QuadTreeSpaceClass, + FirstUserClass, + LastUserClass = FirstUserClass + MaxUserClasses - 1, + NumClasses, + MaxUserClasses = 4 + } + + public enum JointType : int + { + None, + Ball, + Hinge, + Slider, + Contact, + Universal, + Hinge2, + Fixed, + Null, + AMotor, + LMotor, + Plane2D + } + + public enum JointParam : int + { + LoStop, + HiStop, + Vel, + FMax, + FudgeFactor, + Bounce, + CFM, + StopERP, + StopCFM, + SuspensionERP, + SuspensionCFM, + LoStop2 = 256, + HiStop2, + Vel2, + FMax2, + FudgeFactor2, + Bounce2, + CFM2, + StopERP2, + StopCFM2, + SuspensionERP2, + SuspensionCFM2, + LoStop3 = 512, + HiStop3, + Vel3, + FMax3, + FudgeFactor3, + Bounce3, + CFM3, + StopERP3, + StopCFM3, + SuspensionERP3, + SuspensionCFM3 + } + + #endregion + + #region Callbacks + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GetAABBFn(IntPtr geom, out AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate ColliderFn GetColliderFnFn(int num); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GeomDtorFn(IntPtr o); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v); + + #endregion + + #region Structs + + [StructLayout(LayoutKind.Sequential)] + public struct AABB + { + public dReal MinX, MaxX; + public dReal MinY, MaxY; + public dReal MinZ, MaxZ; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Contact + { + public SurfaceParameters surface; + public ContactGeom geom; + public Vector3 fdir1; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct ContactGeom + { + public static readonly int SizeOf = Marshal.SizeOf(typeof(ContactGeom)); + + public Vector3 pos; + public Vector3 normal; + public dReal depth; + public IntPtr g1; + public IntPtr g2; + public int side1; + public int side2; + } + + [StructLayout(LayoutKind.Sequential)] + public struct GeomClass + { + public int bytes; + public GetColliderFnFn collider; + public GetAABBFn aabb; + public AABBTestFn aabb_test; + public GeomDtorFn dtor; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct JointFeedback + { + public Vector3 f1; + public Vector3 t1; + public Vector3 f2; + public Vector3 t2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Mass + { + public dReal mass; + public Vector4 c; + public Matrix3 I; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix3 + { + public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) + { + M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; + M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; + M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; + } + public dReal M00, M10, M20; + private dReal _m30; + public dReal M01, M11, M21; + private dReal _m31; + public dReal M02, M12, M22; + private dReal _m32; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4 + { + public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30, + dReal m01, dReal m11, dReal m21, dReal m31, + dReal m02, dReal m12, dReal m22, dReal m32, + dReal m03, dReal m13, dReal m23, dReal m33) + { + M00 = m00; M10 = m10; M20 = m20; M30 = m30; + M01 = m01; M11 = m11; M21 = m21; M31 = m31; + M02 = m02; M12 = m12; M22 = m22; M32 = m32; + M03 = m03; M13 = m13; M23 = m23; M33 = m33; + } + public dReal M00, M10, M20, M30; + public dReal M01, M11, M21, M31; + public dReal M02, M12, M22, M32; + public dReal M03, M13, M23, M33; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public dReal W, X, Y, Z; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct SurfaceParameters + { + public ContactFlags mode; + public dReal mu; + public dReal mu2; + public dReal bounce; + public dReal bounce_vel; + public dReal soft_erp; + public dReal soft_cfm; + public dReal motion1; + public dReal motion2; + public dReal slip1; + public dReal slip2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public Vector3(dReal x, dReal y, dReal z) + { + X = x; Y = y; Z = z; _w = 0.0f; + } + public dReal X, Y, Z; + private dReal _w; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector4 + { + public Vector4(dReal x, dReal y, dReal z, dReal w) + { + X = x; Y = y; Z = z; W = w; + } + public dReal X, Y, Z, W; + } + + #endregion + + [DllImport("ode", EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnected(IntPtr b1, IntPtr b2); + + [DllImport("ode", EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type); + + [DllImport("ode", EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos); + + [DllImport("ode", EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out dReal X); + + [DllImport("ode", EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat); + + [DllImport("ode", EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out dReal X); + + [DllImport("ode", EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R); + + [DllImport("ode", EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out dReal M00); + + [DllImport("ode", EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyCreate(IntPtr world); + + [DllImport("ode", EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void BodyDestroy(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyDisable(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyEnable(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetAutoDisableFlag(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetAutoDisableSteps(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableTime(IntPtr body); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); + public static Vector3 BodyGetAngularVel(IntPtr body) + { + unsafe { return *(BodyGetAngularVelUnsafe(body)); } + } +#endif + + [DllImport("ode", EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetData(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetFiniteRotationMode(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body); + public static Vector3 BodyGetForce(IntPtr body) + { + unsafe { return *(BodyGetForceUnsafe(body)); } + } +#endif + + [DllImport("ode", EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetGravityMode(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetJoint(IntPtr body, int index); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); + public static Vector3 BodyGetLinearVel(IntPtr body) + { + unsafe { return *(BodyGetLinearVelUnsafe(body)); } + } +#endif + + [DllImport("ode", EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetMass(IntPtr body, out Mass mass); + + [DllImport("ode", EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetNumJoints(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); + public static Vector3 BodyGetPosition(IntPtr body) + { + unsafe { return *(BodyGetPositionUnsafe(body)); } + } +#endif + + [DllImport("ode", EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); + public static Quaternion BodyGetQuaternion(IntPtr body) + { + unsafe { return *(BodyGetQuaternionUnsafe(body)); } + } +#endif + + [DllImport("ode", EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body); + public static Matrix3 BodyGetRotation(IntPtr body) + { + unsafe { return *(BodyGetRotationUnsafe(body)); } + } +#endif + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body); + public static Vector3 BodyGetTorque(IntPtr body) + { + unsafe { return *(BodyGetTorqueUnsafe(body)); } + } +#endif + + [DllImport("ode", EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyIsEnabled(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); + + [DllImport("ode", EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); + + [DllImport("ode", EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); + + [DllImport("ode", EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableSteps(IntPtr body, int steps); + + [DllImport("ode", EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableTime(IntPtr body, dReal time); + + [DllImport("ode", EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetData(IntPtr body, IntPtr data); + + [DllImport("ode", EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationMode(IntPtr body, int mode); + + [DllImport("ode", EntryPoint = "dBodySetFiniteRotationModeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationModeAxis(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetGravityMode(IntPtr body, bool mode); + + [DllImport("ode", EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetMass(IntPtr body, ref Mass mass); + + [DllImport("ode", EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); + + [DllImport("ode", EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref dReal w); + + [DllImport("ode", EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref Matrix3 R); + + [DllImport("ode", EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref dReal M00); + + [DllImport("ode", EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 p2, + ref Matrix3 R2, ref Vector3 side2, + ref Vector3 normal, out dReal depth, out int return_code, + int maxc, out ContactGeom contact, int skip); + + [DllImport("ode", EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 _p2, + ref Matrix3 R2, ref Vector3 side2); + + [DllImport("ode", EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity] + public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, + ref Vector3 b1, ref Vector3 b2, + ref Vector3 cp1, ref Vector3 cp2); + + [DllImport("ode", EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity] + public static extern void CloseODE(); + + [DllImport("ode", EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip); + + [DllImport("ode", EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); + + [DllImport("ode", EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable); + + [DllImport("ode", EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateGeom(int classnum); + + [DllImport("ode", EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] + public static extern int CreateGeomClass(ref GeomClass classptr); + + [DllImport("ode", EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateGeomTransform(IntPtr space); + + [DllImport("ode", EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateRay(IntPtr space, dReal length); + + [DllImport("ode", EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateSphere(IntPtr space, dReal radius); + + [DllImport("ode", EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); + + [DllImport("ode", EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] + public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); + + [DllImport("ode", EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] + public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q); + + [DllImport("ode", EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity] + public static extern int FactorCholesky(ref dReal A00, int n); + + [DllImport("ode", EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip); + + [DllImport("ode", EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len); + + [DllImport("ode", EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x); + + [DllImport("ode", EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity] + public static extern void GeomClearOffset(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos); + + [DllImport("ode", EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X); + + [DllImport("ode", EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos); + + [DllImport("ode", EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out dReal X); + + [DllImport("ode", EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R); + + [DllImport("ode", EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out dReal M00); + + [DllImport("ode", EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomDestroy(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomDisable(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomEnable(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out dReal minX); + + [DllImport("ode", EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetBody(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCategoryBits(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetClassData(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCollideBits(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] + public static extern GeomClassID GeomGetClass(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetData(IntPtr geom); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetOffsetPosition(IntPtr geom) + { + unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } + } +#endif + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetOffsetRotation(IntPtr geom) + { + unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } + } +#endif + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetPosition(IntPtr geom) + { + unsafe { return *(GeomGetPositionUnsafe(geom)); } + } +#endif + + [DllImport("ode", EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + + [DllImport("ode", EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetRotation(IntPtr geom) + { + unsafe { return *(GeomGetRotationUnsafe(geom)); } + } +#endif + + [DllImport("ode", EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetSpace(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldDataCreate(); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataDestroy(IntPtr d); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g); + + [DllImport("ode", EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsEnabled(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsOffset(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsSpace(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result); + + [DllImport("ode", EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A); + + [DllImport("ode", EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir); + + [DllImport("ode", EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX); + + [DllImport("ode", EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern int GeomRayGetClosestHit(IntPtr ray); + + [DllImport("ode", EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetLength(IntPtr ray); + + [DllImport("ode", EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); + + [DllImport("ode", EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); + + [DllImport("ode", EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); + + [DllImport("ode", EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetLength(IntPtr ray, dReal length); + + [DllImport("ode", EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); + + [DllImport("ode", EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetBody(IntPtr geom, IntPtr body); + + [DllImport("ode", EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCategoryBits(IntPtr geom, int bits); + + [DllImport("ode", EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCollideBits(IntPtr geom, int bits); + + [DllImport("ode", EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetData(IntPtr geom, IntPtr data); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat); + + [DllImport("ode", EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w); + + [DllImport("ode", EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSphereGetRadius(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius); + + [DllImport("ode", EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetCleanup(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTransformGetGeom(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetInfo(IntPtr geom); + + [DllImport("ode", EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetCleanup(IntPtr geom, int mode); + + [DllImport("ode", EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj); + + [DllImport("ode", EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetInfo(IntPtr geom, int info); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble1(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + double[] normals); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + dReal[] normals); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + float[] normals); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshClearTCCache(IntPtr g); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataCreate(); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataDestroy(IntPtr d); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataPreprocess(IntPtr d); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data); + + [DllImport("ode", EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataUpdate(IntPtr d); + + [DllImport("ode", EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable); + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g); + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriCallback GeomTriMeshGetCallback(IntPtr g); + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetData(IntPtr g); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom); + public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom) + { + unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); } + } +#endif + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec); + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g); + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2); + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity] + public extern static int GeomTriMeshGetTriangleCount(IntPtr g); + + [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g); + + [DllImport("ode", EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass); + + [DllImport("ode", EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback); + + [DllImport("ode", EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback); + + [DllImport("ode", EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data); + + [DllImport("ode", EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans); + + [DllImport("ode", EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00); + + [DllImport("ode", EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); + + [DllImport("ode", EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr HashSpaceCreate(IntPtr space); + + [DllImport("ode", EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel); + + [DllImport("ode", EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel); + + [DllImport("ode", EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity] + public static extern void InfiniteAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity] + public static extern void InitODE(); + + [DllImport("ode", EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity] + public static extern int IsPositiveDefinite(ref dReal A, int n); + + [DllImport("ode", EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity] + public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n); + + [DllImport("ode", EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3); + + [DllImport("ode", EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHingeTorque(IntPtr joint, dReal torque); + + [DllImport("ode", EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddPRTorque(IntPtr joint, dReal torque); + + [DllImport("ode", EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddSliderForce(IntPtr joint, dReal force); + + [DllImport("ode", EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity] + public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2); + + [DllImport("ode", EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact); + + [DllImport("ode", EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group); + + [DllImport("ode", EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointDestroy(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngle(IntPtr j, int anum); + + [DllImport("ode", EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum); + + [DllImport("ode", EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorAxisRel(IntPtr j, int anum); + + [DllImport("ode", EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorMode(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorNumAxes(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorParam(IntPtr j, int parameter); + + [DllImport("ode", EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetBody(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetData(IntPtr j); + +#if !dNO_UNSAFE_CODE + [CLSCompliant(false)] + [DllImport("ode", EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity] + public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j); + public static JointFeedback JointGetFeedback(IntPtr j) + { + unsafe { return *(JointGetFeedbackUnsafe(j)); } + } +#endif + + [DllImport("ode", EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngle(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngleRate(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeParam(IntPtr j, int parameter); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1Rate(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle2Rate(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Param(IntPtr j, int parameter); + + [DllImport("ode", EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetLMotorNumAxes(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetLMotorParam(IntPtr j, int parameter); + + [DllImport("ode", EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRParam(IntPtr j, int parameter); + + [DllImport("ode", EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPosition(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPositionRate(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderParam(IntPtr j, int parameter); + + [DllImport("ode", EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPosition(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPositionRate(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity] + public static extern JointType JointGetType(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1Rate(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2Rate(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalParam(IntPtr j, int parameter); + + [DllImport("ode", EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGroupCreate(int max_size); + + [DllImport("ode", EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupDestroy(IntPtr group); + + [DllImport("ode", EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupEmpty(IntPtr group); + + [DllImport("ode", EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle); + + [DllImport("ode", EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorMode(IntPtr j, int mode); + + [DllImport("ode", EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorNumAxes(IntPtr group, int num); + + [DllImport("ode", EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetData(IntPtr j, IntPtr data); + + [DllImport("ode", EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback); + + [DllImport("ode", EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFixed(IntPtr j); + + [DllImport("ode", EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorNumAxes(IntPtr j, int num); + + [DllImport("ode", EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity] + public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip); + + [DllImport("ode", EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdd(ref Mass a, ref Mass b); + + [DllImport("ode", EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdjust(ref Mass m, dReal newmass); + + [DllImport("ode", EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity] + public static extern bool MassCheck(ref Mass m); + + [DllImport("ode", EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(out Mass mass, ref Matrix3 R); + + [DllImport("ode", EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(out Mass mass, ref dReal M00); + + [DllImport("ode", EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetParameters(out Mass mass, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal i11, dReal i22, dReal i33, + dReal i12, dReal i13, dReal i23); + + [DllImport("ode", EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius); + + [DllImport("ode", EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity] + public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius); + + [DllImport("ode", EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g); + + [DllImport("ode", EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetZero(out Mass mass); + + [DllImport("ode", EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity] + public static extern void MassTranslate(out Mass mass, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle); + + [DllImport("ode", EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity] + public static extern void QfromR(out Quaternion q, ref Matrix3 R); + + [DllImport("ode", EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void QSetIdentity(out Quaternion q); + + [DllImport("ode", EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth); + + [DllImport("ode", EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth); + + [DllImport("ode", EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity] + public static extern dReal RandReal(); + + [DllImport("ode", EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity] + public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz); + + [DllImport("ode", EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle); + + [DllImport("ode", EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] + public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); + + [DllImport("ode", EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] + public static extern void RfromQ(out Matrix3 R, ref Quaternion q); + + [DllImport("ode", EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] + public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); + + [DllImport("ode", EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void RSetIdentity(out Matrix3 R); + + [DllImport("ode", EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity] + public static extern void SetValue(out dReal a, int n); + + [DllImport("ode", EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void SetZero(out dReal a, int n); + + [DllImport("ode", EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SimpleSpaceCreate(IntPtr space); + + [DllImport("ode", EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity] + public static extern void SolveCholesky(ref dReal L, out dReal b, int n); + + [DllImport("ode", EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip); + + [DllImport("ode", EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceAdd(IntPtr space, IntPtr geom); + + [DllImport("ode", EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceClean(IntPtr space); + + [DllImport("ode", EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback); + + [DllImport("ode", EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback); + + [DllImport("ode", EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceDestroy(IntPtr space); + + [DllImport("ode", EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceGetCleanup(IntPtr space); + + [DllImport("ode", EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetNumGeoms(IntPtr space); + + [DllImport("ode", EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SpaceGetGeom(IntPtr space, int i); + + [DllImport("ode", EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceQuery(IntPtr space, IntPtr geom); + + [DllImport("ode", EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceRemove(IntPtr space, IntPtr geom); + + [DllImport("ode", EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetCleanup(IntPtr space, bool mode); + + [DllImport("ode", EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity] + public static extern void VectorScale(out dReal a, ref dReal d, int n); + + [DllImport("ode", EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr WorldCreate(); + + [DllImport("ode", EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void WorldDestroy(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool WorldGetAutoDisableFlag(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableSteps(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableTime(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoEnableDepthSF1(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetCFM(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetERP(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity); + + [DllImport("ode", EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out dReal X); + + [DllImport("ode", EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactSurfaceLayer(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetQuickStepNumIterations(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetQuickStepW(IntPtr world); + + [DllImport("ode", EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force); + + [DllImport("ode", EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX); + + [DllImport("ode", EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldQuickStep(IntPtr world, dReal stepsize); + + [DllImport("ode", EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold); + + [DllImport("ode", EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable); + + [DllImport("ode", EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold); + + [DllImport("ode", EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps); + + [DllImport("ode", EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time); + + [DllImport("ode", EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth); + + [DllImport("ode", EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetCFM(IntPtr world, dReal cfm); + + [DllImport("ode", EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel); + + [DllImport("ode", EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth); + + [DllImport("ode", EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetERP(IntPtr world, dReal erp); + + [DllImport("ode", EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z); + + [DllImport("ode", EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num); + + [DllImport("ode", EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation); + + [DllImport("ode", EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStep(IntPtr world, dReal stepsize); + + [DllImport("ode", EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations); + } +} diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua new file mode 100644 index 0000000..b53d54c --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua @@ -0,0 +1,31 @@ +package.name = "Ode.NET" +package.kind = "dll" +package.language = "c#" + +-- Build options + + package.defines = { } + + if (options["with-doubles"]) then + table.insert(package.defines, "dDOUBLE") + else + table.insert(package.defines, "dSINGLE") + end + + if (options["no-unsafe"]) then + table.insert(package.defines, "dNO_UNSAFE_CODE") + else + package.buildflags = { "unsafe" } + end + + +-- Files & Libraries + + package.files = { + "AssemblyInfo.cs", + "Ode.cs" + } + + package.links = { + "System" + } diff --git a/libraries/ode-0.9/contrib/Ode.NET/README.TXT b/libraries/ode-0.9/contrib/Ode.NET/README.TXT new file mode 100644 index 0000000..e6f1262 --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/README.TXT @@ -0,0 +1,73 @@ +Ode.NET - .NET bindings for ODE +Jason Perkins (starkos@gmail.com) + + THIS IS A WORK IN PROGRESS! I'm not done yet! + + +--------------------------------------------------------------------- + INSTALLATION +--------------------------------------------------------------------- + + Note that this binding uses a C# 2.0 feature (the + UnmanagedFunctionPointer attribute). You will need to use + Visual Studio 2005 (C# Express is fine) or Mono's gmcs + compiler. + + Start by getting or building ODE as a shared library (DLL). + + The simplest way to build the bindings is probably to create a + new library assembly in your tool of choice and drop in the files + Ode/Ode.cs and Ode/AssemblyInfo.cs. Define the symbol`dDOUBLE` if + you used double-precision math in your ode.dll. Build, done. + + For testing purposes, I have also created bindings for the + Drawstuff library and a C# version of the BoxStack demo. You can + throw all of these files into a console executable and run it to + see the demo. + + If you happen to have Premake installed (http://premake.sf.net/), + you can generate build scripts for the library with: + + premake --target (toolset) # for single precision + premake --with-doubles --target (toolset) # for double precision + + To build the test application too, use: + + premake --with-tests --target (toolset) + + To build with Mono, you must add the --dotnet parameter to enable + support .NET 2.0: + + premake --dotnet mono2 --target gnu + + +--------------------------------------------------------------------- + USAGE +--------------------------------------------------------------------- + + I have tried to keep things as close to the original C API as I can, + rather than forcing a class structure on everyone. Everything is + contained within the `Ode.NET` namespace inside a static class + named `d`. All ODE IDs are replaced with IntPtrs. A quick example: + + using Ode.NET; + + IntPtr world = d.WorldCreate(); + IntPtr body = d.BodyCreate(world); + + Take a look at Tests/BoxStack.cs for a more complete example. + + +--------------------------------------------------------------------- + KNOWN ISSUES +--------------------------------------------------------------------- + + I'm not done yet, so many functions are still missing. + + It is not possible to implement dBodyGetPosition(), dBodyGetRotation(), + etc. without resorting to unsafe code, which I was trying to avoid. + This binding uses the .NET friendly dBodyCopyPosition(), + dBodyCopyRotation(), etc. instead. + + Collision response (contact joints) do not work when built under + 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 new file mode 100644 index 0000000..1077124 --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs @@ -0,0 +1,260 @@ +using System; +using System.Collections.Generic; +using Drawstuff.NET; + +namespace Ode.NET +{ +#if dDOUBLE + using dReal = System.Double; +#else + using dReal = System.Single; +#endif + + public class TestBoxStack + { + #region Description of convex shape + + static dReal[] planes = + { + 1.0f, 0.0f, 0.0f, 0.25f, + 0.0f, 1.0f, 0.0f, 0.25f, + 0.0f, 0.0f, 1.0f, 0.25f, + 0.0f, 0.0f, -1.0f, 0.25f, + 0.0f, -1.0f, 0.0f, 0.25f, + -1.0f, 0.0f , 0.0f, 0.25f + }; + + static dReal[] points = + { + 0.25f, 0.25f, 0.25f, + -0.25f, 0.25f, 0.25f, + 0.25f, -0.25f, 0.25f, + -0.25f, -0.25f, 0.25f, + 0.25f, 0.25f, -0.25f, + -0.25f,0.25f,-0.25f, + 0.25f,-0.25f,-0.25f, + -0.25f,-0.25f,-0.25f, + }; + + static int[] polygons = + { + 4, 0, 2, 6, 4, + 4, 1, 0, 4, 5, + 4, 0, 1, 3, 2, + 4, 3, 1, 5, 7, + 4, 2, 3, 7, 6, + 4, 5, 4, 6, 7, + }; + + #endregion + + const int NUM = 100; + const float DENSITY = 5.0f; + const int MAX_CONTACTS = 8; + + static IntPtr world; + static IntPtr space; + static IntPtr contactgroup; + + static Queue obj = new Queue(); + + static d.Vector3 xyz = new d.Vector3(2.1640f, -1.3079f, 1.7600f); + static d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f); + + static d.NearCallback nearCallback = near; + static d.ContactGeom[] contacts = new d.ContactGeom[MAX_CONTACTS]; + static d.Contact contact; + + + // Called when window is opened - sets up viewpoint and prints usage + static void start(int unused) + { + ds.SetViewpoint(ref xyz, ref hpr); + Console.WriteLine("To drop another object, press:"); + Console.WriteLine(" b for box."); + Console.WriteLine(" s for sphere."); + Console.WriteLine(" c for capsule."); + Console.WriteLine(" y for cylinder."); + Console.WriteLine(" v for a convex object."); + Console.WriteLine(" x for a composite object."); + Console.WriteLine("To select an object, press space."); + Console.WriteLine("To disable the selected object, press d."); + Console.WriteLine("To enable the selected object, press e."); + Console.WriteLine("To toggle showing the geom AABBs, press a."); + Console.WriteLine("To toggle showing the contact points, press t."); + Console.WriteLine("To toggle dropping from random position/orientation, press r."); + Console.WriteLine("To save the current state to 'state.dif', press 1."); + } + + + // Near callback - creates contact joints + static void near(IntPtr space, IntPtr g1, IntPtr g2) + { + IntPtr b1 = d.GeomGetBody(g1); + IntPtr b2 = d.GeomGetBody(g2); + if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) + return; + + int count = d.Collide(g1, g2, MAX_CONTACTS, contacts, d.ContactGeom.SizeOf); + for (int i = 0; i < count; ++i) + { + contact.geom = contacts[i]; + IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact); + d.JointAttach(joint, b1, b2); + } + } + + + // Adds a new object to the scene - attaches a body to the geom and + // sets the initial position and orientation + static void addObject(IntPtr geom, d.Mass mass) + { + // Create a body for this object + IntPtr body = d.BodyCreate(world); + d.GeomSetBody(geom, body); + d.BodySetMass(body, ref mass); + obj.Enqueue(geom); + + // Set the position of the new object + d.Matrix3 R; + d.BodySetPosition(body, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() + 2); + d.RFromAxisAndAngle(out R, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 10 - 5); + d.BodySetRotation(body, ref R); + + // Cap the total number of objects + if (obj.Count > NUM) + { + geom = obj.Dequeue(); + body = d.GeomGetBody(geom); + d.BodyDestroy(body); + d.GeomDestroy(geom); + } + } + + + // Keyboard callback + static void command(int cmd) + { + IntPtr geom; + d.Mass mass; + d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); + + Char ch = Char.ToLower((Char)cmd); + switch ((Char)ch) + { + case 'b': + d.MassSetBox(out mass, DENSITY, sides.X, sides.Y, sides.Z); + geom = d.CreateBox(space, sides.X, sides.Y, sides.Z); + addObject(geom, mass); + break; + + case 'c': + sides.X *= 0.5f; + d.MassSetCapsule(out mass, DENSITY, 3, sides.X, sides.Y); + geom = d.CreateCapsule(space, sides.X, sides.Y); + addObject(geom, mass); + break; + + case 'v': + d.MassSetBox(out mass, DENSITY, 0.25f, 0.25f, 0.25f); + geom = d.CreateConvex(space, planes, planes.Length / 4, points, points.Length / 3, polygons); + addObject(geom, mass); + break; + } + } + + + // Draw an object in the scene + static void drawGeom(IntPtr geom) + { + IntPtr body = d.GeomGetBody(geom); + + d.Vector3 pos; + d.BodyCopyPosition(body, out pos); + + d.Matrix3 R; + d.BodyCopyRotation(body, out R); + + d.GeomClassID type = d.GeomGetClass(geom); + switch (type) + { + case d.GeomClassID.BoxClass: + d.Vector3 sides; + d.GeomBoxGetLengths(geom, out sides); + ds.DrawBox(ref pos, ref R, ref sides); + break; + case d.GeomClassID.CapsuleClass: + dReal radius, length; + d.GeomCapsuleGetParams(geom, out radius, out length); + ds.DrawCapsule(ref pos, ref R, length, radius); + break; + case d.GeomClassID.ConvexClass: + ds.DrawConvex(ref pos, ref R, planes, planes.Length / 4, points, points.Length / 3, polygons); + break; + } + } + + + // Called once per frame; updates the scene + static void step(int pause) + { + d.SpaceCollide(space, IntPtr.Zero, nearCallback); + if (pause == 0) + d.WorldQuickStep(world, 0.02f); + d.JointGroupEmpty(contactgroup); + + ds.SetColor(1.0f, 1.0f, 0.0f); + ds.SetTexture(ds.Texture.Wood); + + foreach (IntPtr geom in obj) + { + drawGeom(geom); + } + } + + + static void Main(string[] args) + { + // Setup pointers to drawstuff callback functions + ds.Functions fn; + fn.version = ds.VERSION; + fn.start = new ds.CallbackFunction(start); + fn.step = new ds.CallbackFunction(step); + fn.command = new ds.CallbackFunction(command); + fn.stop = null; + fn.path_to_textures = "../../../../drawstuff/textures"; + if (args.Length > 0) + { + fn.path_to_textures = args[0]; + } + + // Set up contact response parameters + contact.surface.mode = d.ContactFlags.Bounce | d.ContactFlags.SoftCFM; + contact.surface.mu = d.Infinity; + contact.surface.mu2 = 0.0f; + contact.surface.bounce = 0.1f; + contact.surface.bounce_vel = 0.1f; + contact.surface.soft_cfm = 0.01f; + + // Initialize the scene + world = d.WorldCreate(); + space = d.HashSpaceCreate(IntPtr.Zero); + contactgroup = d.JointGroupCreate(0); + d.WorldSetGravity(world, 0.0f, 0.0f, -0.5f); + d.WorldSetCFM(world, 1e-5f); + d.WorldSetAutoDisableFlag(world, true); + d.WorldSetContactMaxCorrectingVel(world, 0.1f); + d.WorldSetContactSurfaceLayer(world, 0.001f); + d.CreatePlane(space, 0, 0, 1, 0); + + // Run the scene + ds.SimulationLoop(args.Length, args, 352, 288, ref fn); + + // Clean up + d.JointGroupDestroy(contactgroup); + d.SpaceDestroy(space); + d.WorldDestroy(world); + d.CloseODE(); + } + } +} diff --git a/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua new file mode 100644 index 0000000..5253ae1 --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua @@ -0,0 +1,27 @@ +-- This function creates the test packages +function maketest(name) + + package = newpackage() + package.name = name + package.kind = "exe" + package.language = "c#" + + if (options["with-doubles"]) then + package.defines = { "dDOUBLE" } + else + package.defines = { "dSINGLE " } + end + + package.links = { + "System", + "Ode.NET", + "Drawstuff.NET" + } + + package.files = { + name .. ".cs" + } + +end + +maketest("BoxStack") diff --git a/libraries/ode-0.9/contrib/Ode.NET/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/premake.lua new file mode 100644 index 0000000..c25a017 --- /dev/null +++ b/libraries/ode-0.9/contrib/Ode.NET/premake.lua @@ -0,0 +1,29 @@ +project.name = "Ode.NET" + +-- Target checking + + if (target and target ~= "vs2005" and target ~= "gnu") then + error("Ode.NET requires a .NET 2.0 compiler") + end + + +-- Project options + + addoption("with-doubles", "Use double instead of float as base numeric type") + addoption("with-tests", "Builds the test applications and DrawStuff library") + addoption("no-unsafe", "Exclude functions using unsafe code (dBodyGetPosition, etc.)") + + +-- Build settings + + project.config["Debug"].bindir = "bin/Debug" + project.config["Release"].bindir = "bin/Release" + + +-- Packages + + if (options["with-tests"]) then + dopackage("Tests") + dopackage("Drawstuff") + end + dopackage("Ode") diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT new file mode 100644 index 0000000..b2b1995 --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT @@ -0,0 +1,37 @@ + +This is the BSD-style license for The ODE Model Processor +---------------------------------------------------------- + +The ODE Model Processor +Copyright (c) 2007, Department Of Information Science, +University of Otago, Dunedin, New Zealand. +All rights reserved. + +Author: Richard Barrington + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +Neither the names of the copyright owner nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, 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 new file mode 100644 index 0000000..cfe59bc --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT @@ -0,0 +1,502 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That'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 new file mode 100644 index 0000000..6a3f521 --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln @@ -0,0 +1,20 @@ + +Microsoft Visual Studio Solution File, Format Version 9.00 +# Visual C# Express 2005 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "OdeModelProcessor", "OdeModelProcessor\OdeModelProcessor.csproj", "{246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Debug|Any CPU.Build.0 = Debug|Any CPU + {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Release|Any CPU.ActiveCfg = Release|Any CPU + {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs new file mode 100644 index 0000000..9c974eb --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs @@ -0,0 +1,354 @@ +/* + * The ODE Model Processor + * ----------------------- + * + * Copyright 2007, Department of Information Science, + * University of Otago, Dunedin, New Zealand. + * + * Author: Richard Barrington + * + * This is a Content Processor and Tag library written for use with + * Microsoft Visual C# 2005 Express Edition and Microsoft XNA Game + * Studio Express 1.0. + * + * It can be used to read .x model vertex and index data before + * insertion into the content pipeline. This is used to build ODE + * Triangle Meshes which are then used for collision detection that + * is more accurate than the default XNA bounding boxes or spheres. + * + * Usage is simple: + * Build the library and reference the DLL in your project. + * Add the DLL to the Content Pipeline + * Set the content processor for you .x models to OdeModelProcessor. + * + * Create triangle meshes as follows: + * 1) Create a space, but only one for all of models. + * 2) Create a triangle data. + * 3) Load the model. + * 4) Retreive the tag from the model. + * 6) Build the triangle mesh by calling d.GeomTriMeshDataBuildSimple. + * + * Eg: + * IntPtr space = d.SimpleSpaceCreate(IntPtr.Zero); + * IntPtr triangleData = d.GeomTriMeshDataCreate(); + * Model obj = content.Load("Content\\mycube"); + * OdeTag tag = (OdeTag)obj.Tag; + * IntPtr vertexArray = tag.getVertices(); + * IntPtr indexArray = tag.getIndices(); + * d.GeomTriMeshDataBuildSimple + * ( + * triangleData, + * vertexArray, tag.getVertexStride(), tag.getVertexCount(), + * indexArray, tag.getIndexCount(), tag.getIndexStride() + * ); + * IntPtr triangleMesh = d.CreateTriMesh(space, triangleData, null, null, null); + * + * You can load multiple models and test for collisions with something + * like this in the update method: + * + * d.GeomSetPosition(odeTri1, obj1Position.X, obj1Position.Y, obj1Position.Z); + * d.GeomSetPosition(odeTri2, obj2Position.X, obj2Position.Y, obj2Position.Z); + * int numberOfContacts = d.Collide(odeTri1, odeTri2, ODE_CONTACTS, + * contactGeom, d.ContactGeom.SizeOf); + * + * Where odeTri1 and odeTri2 are triangle meshes you've created, obj1Position + * and obj2Position are the positions of your rendered models in the scene, + * ODE_CONTACTS is a constant defining the maximum number of contacts + * to test for, contactGeom is a d.ContactGeom[] of length ODE_CONTACTS. + * + * If numberOfContacts is greater than 0, you have a collision. + * + * Other ODE functions such as d.SpaceCollide() also work; see ODE.NET BoxTest.cs. + * + * This library is free software; you can redistribute it and/or + * modify it under the same terms as the ODE and ODE.Net libraries. + * Specifically, the terms are one of EITHER: + * + * (1) The GNU Lesser General Public License as published by the Free + * Software Foundation; either version 2.1 of the License, or (at + * your option) any later version. The text of the GNU Lesser + * General Public License is included with this library in the + * file LICENSE.TXT. + * + * (2) The BSD-style license that is included with this library in + * the file LICENSE-BSD.TXT. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files + * LICENSE.TXT and LICENSE-BSD.TXT for more details. + * + */ + +using System; +using System.Collections.Generic; +using System.Text; +using Microsoft.Xna.Framework; +using Microsoft.Xna.Framework.Content; +using Microsoft.Xna.Framework.Graphics; +using Microsoft.Xna.Framework.Design; +using Microsoft.Xna.Framework.Content.Pipeline; +using Microsoft.Xna.Framework.Content.Pipeline.Graphics; +using Microsoft.Xna.Framework.Content.Pipeline.Processors; +using Microsoft.Xna.Framework.Content.Pipeline.Serialization.Compiler; +using Ode.NET; +using System.Runtime.InteropServices; + +namespace OdeModelProcessor +{ + /* + * Container for vertex and index data in a format + * that ODE.Net can use + */ + public class OdeTag + { + + private float[] vertexData; + private int[] indexData; + private const int indexStride = (sizeof(int)); + private const int vertexStride = (3 * sizeof(float)); + + /* Constructors */ + public OdeTag() + { + vertexData = new float[0]; + indexData = new int[0]; + } + + public OdeTag(float[] vertexData, int[] indexData) + { + this.vertexData = vertexData; + this.indexData = indexData; + } + + /* Data setter */ + public void setData(float[] vertexData, int[] indexData) + { + this.vertexData = vertexData; + this.indexData = indexData; + } + + /* Data appenders */ + public void appendVertexData(float[] vertexData) + { + int newVertexDataLength = vertexData.Length; + float[] tempVertexArray = new float[newVertexDataLength + this.vertexData.Length]; + this.vertexData.CopyTo(tempVertexArray, 0); + vertexData.CopyTo(tempVertexArray, this.vertexData.Length); + this.vertexData = tempVertexArray; + } + + public void appendIndexData(int[] indexData) + { + int newIndexDataLength = indexData.Length; + int[] tempIndexArray = new int[newIndexDataLength + this.indexData.Length]; + this.indexData.CopyTo(tempIndexArray, 0); + indexData.CopyTo(tempIndexArray, this.indexData.Length); + this.indexData = tempIndexArray; + } + + /* Data getters */ + public float[] getVertexData() + { + return this.vertexData; + } + + public int[] getIndexData() + { + return this.indexData; + } + + /* Native data getters */ + public IntPtr getVertices() + { + int count = getVertexData().Length; + int memsize = count * Marshal.SizeOf(getVertexData()[0].GetType()); + IntPtr pointer = Marshal.AllocCoTaskMem(memsize); + Marshal.Copy(getVertexData(), 0, pointer, count); + return pointer; + } + + public IntPtr getIndices() + { + int count = getIndexData().Length; + int memsize = count * Marshal.SizeOf(getIndexData()[0].GetType()); + IntPtr pointer = Marshal.AllocCoTaskMem(memsize); + Marshal.Copy(getIndexData(), 0, pointer, count); + return pointer; + } + + /* Count getters */ + public int getVertexCount() + { + return vertexData.Length/3; + } + + public int getIndexCount() + { + return indexData.Length; + } + + /* Stride getters */ + public int getVertexStride() + { + return vertexStride; + } + + public int getIndexStride() + { + return indexStride; + } + + /* + * Convienience method to build the mesh and return it. The triangleData + * is passed in to allow the calling application to delete it afterwards. + * + * Be sure to destroy the returned TriangleMesh in the client application. + * + * Can't destroy the index and vertex arrays here though, so best to handle + * this manually - only use this method if nothing else makes sense. + */ + public IntPtr getTriangleMesh(IntPtr space, IntPtr triangleData) + { + d.GeomTriMeshDataBuildSimple( + triangleData, + getVertices(), getVertexStride(), getVertexCount(), + getIndices(), getIndexCount(), getIndexStride() + ); + return d.CreateTriMesh(space, triangleData, null, null, null); + } + + } + + /* + * Subclass of the XNA .x model processor, which creates and appends a tag + * containing vertex and index data for ODE.Net to use. + */ + [ContentProcessor] + public class OdeModelProcessor : ModelProcessor + { + private OdeTag tag; + private int indexOffset = 0; + + public override ModelContent Process(NodeContent input, ContentProcessorContext context) + { + tag = new OdeTag(); + GenerateVerticesRecursive( input ); + ModelContent model = base.Process(input, context); + model.Tag = tag; + indexOffset = 0; + return model; + } + + public void GenerateVerticesRecursive(NodeContent input) + { + + MeshContent mesh = input as MeshContent; + + if (mesh != null) + { + GeometryContentCollection gc = mesh.Geometry; + foreach (GeometryContent g in gc) + { + VertexContent vc = g.Vertices; + IndirectPositionCollection ipc = vc.Positions; + IndexCollection ic = g.Indices; + + float[] vertexData = new float[ipc.Count * 3]; + for (int i = 0; i < ipc.Count; i++) + { + + Vector3 v0 = ipc[i]; + vertexData[(i * 3) + 0] = v0.X; + vertexData[(i * 3) + 1] = v0.Y; + vertexData[(i * 3) + 2] = v0.Z; + + } + + int[] indexData = new int[ic.Count]; + for (int j = 0; j < ic.Count; j ++) + { + + indexData[j] = ic[j] + indexOffset; + + } + + tag.appendVertexData(vertexData); + tag.appendIndexData(indexData); + indexOffset += ipc.Count; + } + + } + + foreach (NodeContent child in input.Children) + { + GenerateVerticesRecursive(child); + } + + } + + } + + /* Writer for the OdeTag class */ + [ContentTypeWriter] + public class OdeTagWriter : ContentTypeWriter + { + + protected override void Write(ContentWriter output, OdeTag value) + { + float[] vertexData = value.getVertexData(); + int[] indexData = value.getIndexData(); + output.Write(vertexData.Length); + output.Write(indexData.Length); + for (int j = 0; j < vertexData.Length; j++) + { + output.Write(vertexData[j]); + } + for (int i = 0; i < indexData.Length; i++) + { + output.Write(indexData[i]); + } + } + + public override string GetRuntimeType(TargetPlatform targetPlatform) + { + return typeof(OdeTag).AssemblyQualifiedName; + } + + public override string GetRuntimeReader(TargetPlatform targetPlatform) + { + return "OdeModelProcessor.OdeTagReader, OdeModelProcessor, Version=1.0.0.0, Culture=neutral"; + } + + } + + /* Reader for the OdeTag class */ + public class OdeTagReader : ContentTypeReader + { + protected override OdeTag Read(ContentReader input, OdeTag existingInstance) + { + float[] vertexData = new float[input.ReadInt32()]; + int[] indexData = new int[input.ReadInt32()]; + for (int j = 0; j < vertexData.Length; j++) + { + vertexData[j] = input.ReadSingle(); + } + for (int i = 0; i < indexData.Length; i++) + { + indexData[i] = input.ReadInt32(); + } + + OdeTag tag = null; + if (existingInstance == null) + { + tag = new OdeTag(vertexData, indexData); + } + else + { + tag = existingInstance; + tag.setData(vertexData, indexData); + } + return tag; + } + } +} diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj new file mode 100644 index 0000000..3a36a12 --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj @@ -0,0 +1,69 @@ + + + Debug + AnyCPU + 8.0.50727 + 2.0 + {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2} + Library + Properties + OdeModelProcessor + OdeModelProcessor + + + true + full + false + bin\Debug\ + DEBUG + prompt + 4 + false + false + + + pdbonly + true + bin\Release\ + + + prompt + 4 + false + false + + + + + + False + ..\..\ODE\Ode.NET-0.8\bin\Release\Ode.NET.dll + + + + + + + + + + True + True + Settings.settings + + + + + SettingsSingleFileGenerator + Settings.Designer.cs + + + + + \ 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 new file mode 100644 index 0000000..1ecad3e --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs @@ -0,0 +1,35 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("OdeModelProcessor")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("OdeModelProcessor")] +[assembly: AssemblyCopyright("Copyright © 2007")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("29c5609a-cd5f-480b-b4ef-5c11de022268")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Revision and Build Numbers +// by using the '*' as shown below: +[assembly: AssemblyVersion("1.0.0.0")] +[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 new file mode 100755 index 0000000..6ab60a8 --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs @@ -0,0 +1,26 @@ +//------------------------------------------------------------------------------ +// +// This code was generated by a tool. +// Runtime Version:2.0.50727.832 +// +// Changes to this file may cause incorrect behavior and will be lost if +// the code is regenerated. +// +//------------------------------------------------------------------------------ + +namespace OdeModelProcessor.Properties { + + + [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()] + [global::System.CodeDom.Compiler.GeneratedCodeAttribute("Microsoft.VisualStudio.Editors.SettingsDesigner.SettingsSingleFileGenerator", "8.0.0.0")] + internal sealed partial class Settings : global::System.Configuration.ApplicationSettingsBase { + + private static Settings defaultInstance = ((Settings)(global::System.Configuration.ApplicationSettingsBase.Synchronized(new Settings()))); + + public static Settings Default { + get { + return defaultInstance; + } + } + } +} diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings new file mode 100755 index 0000000..15034e7 --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings @@ -0,0 +1,6 @@ + + + + + + diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT new file mode 100644 index 0000000..97683f9 --- /dev/null +++ b/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT @@ -0,0 +1,78 @@ +The ODE Model Processor +----------------------- + +Copyright 2007, Department of Information Science, +University of Otago, Dunedin, New Zealand. + +Author: Richard Barrington + +This is a Content Processor and Tag library written for use with +Microsoft Visual C# 2005 Express Edition and Microsoft XNA Game +Studio Express 1.0. + +It can be used to read .x model vertex and index data before +insertion into the content pipeline. This is used to build ODE +Triangle Meshes which are then used for collision detection that +is more accurate than the default XNA bounding boxes or spheres. + +Usage is fairly simple: +Build the library and reference the DLL in your project. +Add the DLL to the Content Pipeline +Set the content processor for you .x models to OdeModelProcessor. + +Create triangle meshes as follows: +1) Create a space, but only one for all of models. +2) Create a triangle data. +3) Load the model. +4) Retreive the tag from the model. +6) Build the triangle mesh by calling d.GeomTriMeshDataBuildSimple. + +Eg: +IntPtr space = d.SimpleSpaceCreate(IntPtr.Zero); +IntPtr triangleData = d.GeomTriMeshDataCreate(); +Model obj = content.Load("Content\\mycube"); +OdeTag tag = (OdeTag)obj.Tag; +IntPtr vertexArray = tag.getVertices(); +IntPtr indexArray = tag.getIndices(); +d.GeomTriMeshDataBuildSimple +( + triangleData, + vertexArray, tag.getVertexStride(), tag.getVertexCount(), + indexArray, tag.getIndexCount(), tag.getIndexStride() +); +IntPtr triangleMesh = d.CreateTriMesh(space, triangleData, null, null, null); + +You can load multiple models and test for collisions with something +like this in the update method: + +d.GeomSetPosition(odeTri1, obj1Position.X, obj1Position.Y, obj1Position.Z); +d.GeomSetPosition(odeTri2, obj2Position.X, obj2Position.Y, obj2Position.Z); +int numberOfContacts = d.Collide(odeTri1, odeTri2, ODE_CONTACTS, + contactGeom, d.ContactGeom.SizeOf); + +Where odeTri1 and odeTri2 are triangle meshes you've created, obj1Position +and obj2Position are the positions of your rendered models in the scene, +ODE_CONTACTS is a constant defining the maximum number of contacts +to test for, contactGeom is a d.ContactGeom[] of length ODE_CONTACTS. + +If numberOfContacts is greater than 0, you have a collision. + +Other ODE functions such as d.SpaceCollide() also work; see ODE.NET BoxTest.cs. + +This library is free software; you can redistribute it and/or +modify it under the same terms as the ODE and ODE.Net libraries. +Specifically, the terms are one of EITHER: + + (1) The GNU Lesser General Public License as published by the Free + Software Foundation; either version 2.1 of the License, or (at + your option) any later version. The text of the GNU Lesser + General Public License is included with this library in the + file LICENSE.TXT. + + (2) The BSD-style license that is included with this library in + the file LICENSE-BSD.TXT. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files +LICENSE.TXT and LICENSE-BSD.TXT for more details. diff --git a/libraries/ode-0.9/contrib/README b/libraries/ode-0.9/contrib/README new file mode 100644 index 0000000..dbfaf7f --- /dev/null +++ b/libraries/ode-0.9/contrib/README @@ -0,0 +1,19 @@ +This directory contains ODE-related things that have been generously +contributed by ODE's users. Why is this stuff here and not integrated +into the main ODE source tree? There may be several reasons: + + * The author(s) and ODE maintainers(s) may not have had time to do + the job. + + * It may not be finished. + + * It may contribute functionality that is useful but not considered + to be part of ODE's core. + +No guarantees are made about the code in this directory - it may not +be documented, it may not have been tested, and it may not even +compile for you. + +Each package has its own subdirectory, with a README file in that +directory explaining what the package is. + diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h b/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h new file mode 100644 index 0000000..b445353 --- /dev/null +++ b/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h @@ -0,0 +1,100 @@ +//Benoit CHAPEROT 2003-2004 www.jstarlab.com +#ifndef _ODE_COLLISION_STD_INTERNAL_H_ +#define _ODE_COLLISION_STD_INTERNAL_H_ + +#include +#include "collision_kernel.h" + +struct dxSphere : public dxGeom { + dReal radius; // sphere radius + dxSphere (dSpaceID space, dReal _radius); + void computeAABB(); +}; + + +struct dxBox : public dxGeom { + dVector3 side; // side lengths (x,y,z) + dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz); + void computeAABB(); +}; + + +struct dxCCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + + +struct dxPlane : public dxGeom { + dReal p[4]; + dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d); + void computeAABB(); +}; + +struct dxCylinder : public dxGeom { + dReal radius,lz; // radius, length along z axis + dxCylinder (dSpaceID space, dReal _radius, dReal _length); + void computeAABB(); +}; + +struct dxCone : public dxGeom { + dReal radius,lz; + dxCone(dSpaceID space, dReal _radius,dReal _length); + ~dxCone(); + void computeAABB(); +}; + +struct dxRay : public dxGeom { + dReal length; + dxRay (dSpaceID space, dReal _length); + void computeAABB(); +}; + +struct dxTerrainY : public dxGeom { + dReal m_vLength; + dReal *m_pHeights; + dReal m_vMinHeight; + dReal m_vMaxHeight; + dReal m_vNodeLength; + int m_nNumNodesPerSide; + int m_nNumNodesPerSideShift; + int m_nNumNodesPerSideMask; + int m_bFinite; + dxTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable); + ~dxTerrainY(); + void computeAABB(); + dReal GetHeight(dReal x,dReal z); + dReal GetHeight(int x,int z); + int dCollideTerrainUnit(int x,int z,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip); + bool IsOnTerrain(int nx,int nz,int w,dReal *pos); +}; + +struct dxTerrainZ : public dxGeom { + dReal m_vLength; + dReal *m_pHeights; + dReal m_vMinHeight; + dReal m_vMaxHeight; + dReal m_vNodeLength; + int m_nNumNodesPerSide; + int m_nNumNodesPerSideShift; + int m_nNumNodesPerSideMask; + int m_bFinite; + dxTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable); + ~dxTerrainZ(); + void computeAABB(); + dReal GetHeight(dReal x,dReal y); + dReal GetHeight(int x,int y); + int dCollideTerrainUnit(int x,int y,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip); + bool IsOnTerrain(int nx,int ny,int w,dReal *pos); +}; + +#ifndef MIN +#define MIN(a,b) ((ab)?a:b) +#endif + +#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 new file mode 100644 index 0000000..8c5c3be --- /dev/null +++ b/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp @@ -0,0 +1,504 @@ +//Benoit CHAPEROT 2003-2004 www.jstarlab.com +//some code inspired by Magic Software +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_std_internal.h" +#include "collision_util.h" +#include +#include "windows.h" +#include "ode\ode.h" + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) +const dReal fEPSILON = 1e-9f; + +dxCone::dxCone (dSpaceID space, dReal _radius,dReal _length) : +dxGeom (space,1) +{ + dAASSERT(_radius > 0.f); + dAASSERT(_length > 0.f); + type = dConeClass; + radius = _radius; + lz = _length; +} + +dxCone::~dxCone() +{ +} + +void dxCone::computeAABB() +{ + const dMatrix3& R = final_posr->R; + const dVector3& pos = final_posr->pos; + + dReal xrange = dFabs(R[2] * lz) + radius; + dReal yrange = dFabs(R[6] * lz) + radius; + dReal zrange = dFabs(R[10] * lz) + radius; + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + +dGeomID dCreateCone(dSpaceID space, dReal _radius,dReal _length) +{ + return new dxCone(space,_radius,_length); +} + +void dGeomConeSetParams (dGeomID g, dReal _radius, dReal _length) +{ + dUASSERT (g && g->type == dConeClass,"argument not a cone"); + dAASSERT (_radius > 0.f); + dAASSERT (_length > 0.f); + g->recomputePosr(); + dxCone *c = (dxCone*) g; + c->radius = _radius; + c->lz = _length; + dGeomMoved (g); +} + + +void dGeomConeGetParams (dGeomID g, dReal *_radius, dReal *_length) +{ + dUASSERT (g && g->type == dConeClass,"argument not a cone"); + g->recomputePosr(); + dxCone *c = (dxCone*) g; + *_radius = c->radius; + *_length = c->lz; +} + +//positive inside +dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dConeClass,"argument not a cone"); + + g->recomputePosr(); + dxCone *cone = (dxCone*) g; + + dVector3 tmp,q; + tmp[0] = x - cone->final_posr->pos[0]; + tmp[1] = y - cone->final_posr->pos[1]; + tmp[2] = z - cone->final_posr->pos[2]; + dMULTIPLY1_331 (q,cone->final_posr->R,tmp); + + dReal r = cone->radius; + dReal h = cone->lz; + + dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]); + dReal d1 = q[2]; + dReal d2 = h-q[2]; + + if (d0 < d1) { + if (d0 < d2) return d0; else return d2; + } + else { + if (d1 < d2) return d1; else return d2; + } +} + +//plane plane +bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4], + dVector3 LinePos,dVector3 LineDir) +{ + // If Cross(N0,N1) is zero, then either planes are parallel and separated + // or the same plane. In both cases, 'false' is returned. Otherwise, + // the intersection line is + // + // L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1 + // + // for some coefficients c0 and c1 and for t any real number (the line + // parameter). Taking dot products with the normals, + // + // d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1) + // d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1) + // + // which are two equations in two unknowns. The solution is + // + // c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det + // c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det + // + // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2. +/* + Real fN00 = rkPlane0.Normal().SquaredLength(); + Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal()); + Real fN11 = rkPlane1.Normal().SquaredLength(); + Real fDet = fN00*fN11 - fN01*fN01; + + if ( Math::FAbs(fDet) < gs_fEpsilon ) + return false; + + Real fInvDet = 1.0f/fDet; + Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet; + Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet; + + rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal()); + rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal(); + return true; +*/ + dReal fN00 = dLENGTHSQUARED(Plane0); + dReal fN01 = dDOT(Plane0,Plane1); + dReal fN11 = dLENGTHSQUARED(Plane1); + dReal fDet = fN00*fN11 - fN01*fN01; + + if ( fabs(fDet) < fEPSILON) + return false; + + dReal fInvDet = 1.0f/fDet; + dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet; + dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet; + + dCROSS(LineDir,=,Plane0,Plane1); + dNormalize3(LineDir); + + dVector3 Temp0,Temp1; + dOPC(Temp0,*,Plane0,fC0); + dOPC(Temp1,*,Plane1,fC1); + dOP(LinePos,+,Temp0,Temp1); + + return true; +} + +//plane ray +bool FindIntersectionPlaneRay(const dReal Plane[4], + const dVector3 &LinePos,const dVector3 &LineDir, + dReal &u,dVector3 &Pos) +{ +/* + u = (A*X1 + B*Y1 + C*Z1 + D) / (A*(X1-X2) + B*(Y1-Y2)+C*(Z1-Z2)) +*/ + dReal fDet = -dDot(Plane,LineDir,3); + + if ( fabs(fDet) < fEPSILON) + return false; + + u = (dDot(Plane,LinePos,3) - Plane[3]) / fDet; + dOPC(Pos,*,LineDir,u); + dOPE(Pos,+=,LinePos); + + return true; +} + +int SolveQuadraticPolynomial(dReal a,dReal b,dReal c,dReal &x0,dReal &x1) +{ + dReal d = b*b - 4*a*c; + int NumRoots = 0; + dReal dr; + + if (d < 0.f) + return NumRoots; + + if (d == 0.f) + { + NumRoots = 1; + dr = 0.f; + } + else + { + NumRoots = 2; + dr = sqrtf(d); + } + + x0 = (-b -dr) / (2.f * a); + x1 = (-b +dr) / (2.f * a); + + return NumRoots; +} +/* +const int VALID_INTERSECTION = 1<<0; +const int POS_TEST_FAILEDT0 = 1<<0; +const int POS_TEST_FAILEDT1 = 1<<1; +*/ +int ProcessConeRayIntersectionPoint( dReal r,dReal h, + const dVector3 &q,const dVector3 &v,dReal t, + dVector3 &p, + dVector3 &n, + int &f) +{ + dOPC(p,*,v,t); + dOPE(p,+=,q); + n[0] = 2*p[0]; + n[1] = 2*p[1]; + n[2] = -2*p[2]*r*r/(h*h); + + f = 0; + if (p[2] > h) return 0; + if (p[2] < 0) return 0; + if (t > 1) return 0; + if (t < 0) return 0; + + return 1; +} + +//cone ray +//line in cone space (position,direction) +//distance from line position (direction normalized)(if any) +//return the number of intersection +int FindIntersectionConeRay(dReal r,dReal h, + const dVector3 &q,const dVector3 &v,dContactGeom *pContact) +{ + dVector3 qp,vp; + dOPE(qp,=,q); + dOPE(vp,=,v); + qp[2] = h-q[2]; + vp[2] = -v[2]; + dReal ts = (r/h); + ts *= ts; + dReal a = vp[0]*vp[0] + vp[1]*vp[1] - ts*vp[2]*vp[2]; + dReal b = 2.f*qp[0]*vp[0] + 2.f*qp[1]*vp[1] - 2.f*ts*qp[2]*vp[2]; + dReal c = qp[0]*qp[0] + qp[1]*qp[1] - ts*qp[2]*qp[2]; + +/* + dReal a = v[0]*v[0] + v[1]*v[1] - (v[2]*v[2]*r*r) / (h*h); + 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); + dReal c = q[0]*q[0] + q[1]*q[1] + 2*r*r*q[2]/h - r*r*q[2]/(h*h) - r*r; +*/ + int nNumRoots=SolveQuadraticPolynomial(a,b,c,pContact[0].depth,pContact[1].depth); + int flag = 0; + + dContactGeom ValidContact[2]; + + int nNumValidContacts = 0; + for (int i=0;i=0) && (d<=1)) + { + dOPC(vp,*,v,d); + dOP(qp,+,q,vp); + + if (qp[0]*qp[0]+qp[1]*qp[1] < r*r) + { + dOPE(ValidContact[nNumValidContacts].pos,=,qp); + ValidContact[nNumValidContacts].normal[0] = 0.f; + ValidContact[nNumValidContacts].normal[1] = 0.f; + ValidContact[nNumValidContacts].normal[2] = -1.f; + ValidContact[nNumValidContacts].depth = d; + nNumValidContacts++; + } + } + } + + if (nNumValidContacts == 2) + { + if (ValidContact[0].depth > ValidContact[1].depth) + { + pContact[0] = ValidContact[1]; + pContact[1] = ValidContact[0]; + } + else + { + pContact[0] = ValidContact[0]; + pContact[1] = ValidContact[1]; + } + } + else if (nNumValidContacts == 1) + { + pContact[0] = ValidContact[0]; + } + + return nNumValidContacts; +} + +int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConeClass); + dIASSERT (o2->type == dPlaneClass); + dxCone *cone = (dxCone*) o1; + dxPlane *plane = (dxPlane*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + dVector3 p0,p1,pp0,pp1; + dOPE(p0,=,cone->final_posr->pos); + p1[0] = cone->final_posr->R[0*4+2] * cone->lz + p0[0]; + p1[1] = cone->final_posr->R[1*4+2] * cone->lz + p0[1]; + p1[2] = cone->final_posr->R[2*4+2] * cone->lz + p0[2]; + + dReal u; + FindIntersectionPlaneRay(plane->p,p0,plane->p,u,pp0); + FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1); + + if (dDISTANCE(pp0,pp1) < fEPSILON) + { + p1[0] = cone->final_posr->R[0*4+0] * cone->lz + p0[0]; + p1[1] = cone->final_posr->R[1*4+0] * cone->lz + p0[1]; + p1[2] = cone->final_posr->R[2*4+0] * cone->lz + p0[2]; + FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1); + dIASSERT(dDISTANCE(pp0,pp1) >= fEPSILON); + } + dVector3 h,r0,r1; + h[0] = cone->final_posr->R[0*4+2]; + h[1] = cone->final_posr->R[1*4+2]; + h[2] = cone->final_posr->R[2*4+2]; + + dOP(r0,-,pp0,pp1); + dCROSS(r1,=,h,r0); + dCROSS(r0,=,r1,h); + dNormalize3(r0); + dOPEC(h,*=,cone->lz); + dOPEC(r0,*=,cone->radius); + + dVector3 p[3]; + dOP(p[0],+,cone->final_posr->pos,h); + dOP(p[1],+,cone->final_posr->pos,r0); + dOP(p[2],-,cone->final_posr->pos,r0); + + int numMaxContacts = flags & 0xffff; + if (numMaxContacts == 0) + numMaxContacts = 1; + + int n=0; + for (int i=0;i<3;i++) + { + dReal d = dGeomPlanePointDepth(o2, p[i][0], p[i][1], p[i][2]); + + if (d>0.f) + { + CONTACT(contact,n*skip)->g1 = o1; + CONTACT(contact,n*skip)->g2 = o2; + dOPE(CONTACT(contact,n*skip)->normal,=,plane->p); + dOPE(CONTACT(contact,n*skip)->pos,=,p[i]); + CONTACT(contact,n*skip)->depth = d; + n++; + + if (n == numMaxContacts) + return n; + } + } + + return n; +} + +int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dRayClass); + dIASSERT (o2->type == dConeClass); + dxRay *ray = (dxRay*) o1; + dxCone *cone = (dxCone*) o2; + + contact->g1 = o1; + contact->g2 = o2; + + dVector3 tmp,q,v; + tmp[0] = ray->final_posr->pos[0] - cone->final_posr->pos[0]; + tmp[1] = ray->final_posr->pos[1] - cone->final_posr->pos[1]; + tmp[2] = ray->final_posr->pos[2] - cone->final_posr->pos[2]; + dMULTIPLY1_331 (q,cone->final_posr->R,tmp); + tmp[0] = ray->final_posr->R[0*4+2] * ray->length; + tmp[1] = ray->final_posr->R[1*4+2] * ray->length; + tmp[2] = ray->final_posr->R[2*4+2] * ray->length; + dMULTIPLY1_331 (v,cone->final_posr->R,tmp); + + dReal r = cone->radius; + dReal h = cone->lz; + + dContactGeom Contact[2]; + + if (FindIntersectionConeRay(r,h,q,v,Contact)) + { + dMULTIPLY0_331(contact->normal,cone->final_posr->R,Contact[0].normal); + dMULTIPLY0_331(contact->pos,cone->final_posr->R,Contact[0].pos); + dOPE(contact->pos,+=,cone->final_posr->pos); + contact->depth = Contact[0].depth * dLENGTH(v); +/* + dMatrix3 RI; + dRSetIdentity (RI); + dVector3 ss; + ss[0] = 0.01f; + ss[1] = 0.01f; + ss[2] = 0.01f; + + dsSetColorAlpha (1,0,0,0.8f); + dsDrawBox(contact->pos,RI,ss); +*/ + return 1; + } + + return 0; +} + +int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConeClass); + dIASSERT (o2->type == dSphereClass); + dxCone *cone = (dxCone*) o1; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideSphereSphere(&ASphere, o2, flags, contact, skip); +} + +int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dConeClass); + dIASSERT (o2->type == dBoxClass); + dxCone *cone = (dxCone*) o1; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideSphereBox(&ASphere, o2, flags, contact, skip); +} + +int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dCCylinderClass); + dIASSERT (o2->type == dConeClass); + dxCone *cone = (dxCone*) o2; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideCCylinderSphere(o1, &ASphere, flags, contact, skip); +} + +extern int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTriMeshClass); + dIASSERT (o2->type == dConeClass); + dxCone *cone = (dxCone*) o2; + + dxSphere ASphere(0,cone->radius); + dGeomSetRotation(&ASphere,cone->final_posr->R); + dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]); + + return dCollideSTL(o1, &ASphere, flags, contact, skip); +} + + + + + diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp new file mode 100644 index 0000000..ff779ac --- /dev/null +++ b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp @@ -0,0 +1,662 @@ +//Benoit CHAPEROT 2003-2004 www.jstarlab.com +//some code inspired by Magic Software +#include +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_std_internal.h" +#include "collision_util.h" +//#include +#include "windows.h" +#include "ode\ode.h" + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) +#define MAXCONTACT 10 +#define TERRAINTOL 0.0f + +static bool IsAPowerOfTwo(int f) +{ + dAASSERT(f!=0); + while ((f&1) != 1) + f >>= 1; + + return (f == 1); +} + +static int GetPowerOfTwo(int f) +{ + dAASSERT(f!=0); + int n = 0; + while ((f&1) != 1) + { + n++; + f >>= 1; + } + + return n; +} + +dxTerrainY::dxTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) : +dxGeom (space,bPlaceable) +{ + dIASSERT(IsAPowerOfTwo(nNumNodesPerSide)); + dIASSERT(pHeights); + dIASSERT(vLength > 0.f); + dIASSERT(nNumNodesPerSide > 0); + type = dTerrainYClass; + m_vLength = vLength; + m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide]; + dIASSERT(m_pHeights); + m_nNumNodesPerSide = nNumNodesPerSide; + m_vNodeLength = m_vLength / m_nNumNodesPerSide; + m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide); + m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1; + m_vMinHeight = dInfinity; + m_vMaxHeight = -dInfinity; + m_bFinite = bFinite; + + for (int i=0;i m_vMaxHeight) m_vMaxHeight = m_pHeights[i]; + } +} + +dxTerrainY::~dxTerrainY() +{ + dIASSERT(m_pHeights); + delete [] m_pHeights; +} + +void dxTerrainY::computeAABB() +{ + if (m_bFinite) + { + if (gflags & GEOM_PLACEABLE) + { + dReal dx[6],dy[6],dz[6]; + dx[0] = 0; + dx[1] = final_posr->R[0] * m_vLength; + dx[2] = final_posr->R[1] * m_vMinHeight; + dx[3] = final_posr->R[1] * m_vMaxHeight; + dx[4] = 0; + dx[5] = final_posr->R[2] * m_vLength; + + dy[0] = 0; + dy[1] = final_posr->R[4] * m_vLength; + dy[2] = final_posr->R[5] * m_vMinHeight; + dy[3] = final_posr->R[5] * m_vMaxHeight; + dy[4] = 0; + dy[5] = final_posr->R[6] * m_vLength; + + dz[0] = 0; + dz[1] = final_posr->R[8] * m_vLength; + dz[2] = final_posr->R[9] * m_vMinHeight; + dz[3] = final_posr->R[9] * m_vMaxHeight; + dz[4] = 0; + dz[5] = final_posr->R[10] * m_vLength; + + aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]); + aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]); + aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]); + aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]); + aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]); + aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]); + } + else + { + aabb[0] = 0; + aabb[1] = m_vLength; + aabb[2] = m_vMinHeight; + aabb[3] = m_vMaxHeight; + aabb[4] = 0; + aabb[5] = m_vLength; + } + } + else + { + if (gflags & GEOM_PLACEABLE) + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + } + else + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = m_vMinHeight; + aabb[3] = m_vMaxHeight; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + } + } +} + +dReal dxTerrainY::GetHeight(int x,int z) +{ + return m_pHeights[ (((unsigned int)(z) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift) + + ((unsigned int)(x) & m_nNumNodesPerSideMask)]; +} + +dReal dxTerrainY::GetHeight(dReal x,dReal z) +{ + int nX = int(floor(x / m_vNodeLength)); + int nZ = int(floor(z / m_vNodeLength)); + dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength; + dReal dz = (z - (dReal(nZ) * m_vNodeLength)) / m_vNodeLength; + dIASSERT((dx >= 0.f) && (dx <= 1.f)); + dIASSERT((dz >= 0.f) && (dz <= 1.f)); + + dReal y,y0; + + if (dx + dz < 1.f) + { + y0 = GetHeight(nX,nZ); + y = y0 + + (GetHeight(nX+1,nZ) - y0) * dx + + (GetHeight(nX,nZ+1) - y0) * dz; + } + else + { + y0 = GetHeight(nX+1,nZ+1); + y = y0 + + (GetHeight(nX+1,nZ) - y0) * (1.f - dz) + + (GetHeight(nX,nZ+1) - y0) * (1.f - dx); + } + + return y; +} + +bool dxTerrainY::IsOnTerrain(int nx,int nz,int w,dReal *pos) +{ + dVector3 Min,Max; + Min[0] = nx * m_vNodeLength; + Min[2] = nz * m_vNodeLength; + Max[0] = (nx+1) * m_vNodeLength; + Max[2] = (nz+1) * m_vNodeLength; + dReal Tol = m_vNodeLength * TERRAINTOL; + + if ((pos[0]Max[0]+Tol)) + return false; + + if ((pos[2]Max[2]+Tol)) + return false; + + dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength; + dReal dz = (pos[2] - (dReal(nz) * m_vNodeLength)) / m_vNodeLength; + + if ((w == 0) && (dx + dz > 1.f+TERRAINTOL)) + return false; + + if ((w == 1) && (dx + dz < 1.f-TERRAINTOL)) + return false; + + return true; +} + +dGeomID dCreateTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) +{ + return new dxTerrainY(space, pHeights,vLength,nNumNodesPerSide,bFinite,bPlaceable); +} + +dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dTerrainYClass,"argument not a terrain"); + g->recomputePosr(); + dxTerrainY *t = (dxTerrainY*) g; + return t->GetHeight(x,z) - y; +} + +typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z); +#define RECOMPUTE_RAYNORMAL +//#define DO_RAYDEPTH + +#define DMESS(A) \ + dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \ + x,z,A, \ + pContact->depth, \ + dGeomSphereGetRadius(o2), \ + pContact->pos[0], \ + pContact->pos[1], \ + pContact->pos[2], \ + pContact->normal[0], \ + pContact->normal[1], \ + pContact->normal[2]); +/* +(y is up) + +A-B-E.x +|/| +C-D +| +F +. +z +*/ +int dxTerrainY::dCollideTerrainUnit( + int x,int z,dxGeom *o2,int numMaxContacts, + int flags,dContactGeom *contact, int skip) +{ + dColliderFn *CollideRayN; + dColliderFn *CollideNPlane; + dGetDepthFn *GetDepth; + int numContacts = 0; + int numPlaneContacts = 0; + int i; + + if (numContacts == numMaxContacts) + return numContacts; + + dContactGeom PlaneContact[MAXCONTACT]; + flags = (flags & 0xffff0000) | MAXCONTACT; + + switch (o2->type) + { + case dSphereClass: + CollideRayN = dCollideRaySphere; + CollideNPlane = dCollideSpherePlane; + GetDepth = dGeomSpherePointDepth; + break; + case dBoxClass: + CollideRayN = dCollideRayBox; + CollideNPlane = dCollideBoxPlane; + GetDepth = dGeomBoxPointDepth; + break; + case dCCylinderClass: + CollideRayN = dCollideRayCCylinder; + CollideNPlane = dCollideCCylinderPlane; + GetDepth = dGeomCCylinderPointDepth; + break; + case dRayClass: + CollideRayN = NULL; + CollideNPlane = dCollideRayPlane; + GetDepth = NULL; + break; + case dConeClass: + CollideRayN = dCollideRayCone; + CollideNPlane = dCollideConePlane; + GetDepth = dGeomConePointDepth; + break; + default: + dIASSERT(0); + } + + dReal Plane[4],lBD,lCD,lBC; + dVector3 A,B,C,D,BD,CD,BC,AB,AC; + A[0] = x * m_vNodeLength; + A[2] = z* m_vNodeLength; + A[1] = GetHeight(x,z); + B[0] = (x+1) * m_vNodeLength; + B[2] = z * m_vNodeLength; + B[1] = GetHeight(x+1,z); + C[0] = x * m_vNodeLength; + C[2] = (z+1) * m_vNodeLength; + C[1] = GetHeight(x,z+1); + D[0] = (x+1) * m_vNodeLength; + D[2] = (z+1) * m_vNodeLength; + D[1] = GetHeight(x+1,z+1); + + dOP(BC,-,C,B); + lBC = dLENGTH(BC); + dOPEC(BC,/=,lBC); + + dOP(BD,-,D,B); + lBD = dLENGTH(BD); + dOPEC(BD,/=,lBD); + + dOP(CD,-,D,C); + lCD = dLENGTH(CD); + dOPEC(CD,/=,lCD); + + dOP(AB,-,B,A); + dNormalize3(AB); + + dOP(AC,-,C,A); + dNormalize3(AC); + + if (CollideRayN) + { +#ifdef RECOMPUTE_RAYNORMAL + dVector3 E,F; + dVector3 CE,FB,AD; + dVector3 Normal[3]; + E[0] = (x+2) * m_vNodeLength; + E[2] = z * m_vNodeLength; + E[1] = GetHeight(x+2,z); + F[0] = x * m_vNodeLength; + F[2] = (z+2) * m_vNodeLength; + F[1] = GetHeight(x,z+2); + dOP(AD,-,D,A); + dNormalize3(AD); + dOP(CE,-,E,C); + dNormalize3(CE); + dOP(FB,-,B,F); + dNormalize3(FB); + + //BC + dCROSS(Normal[0],=,BC,AD); + dNormalize3(Normal[0]); + + //BD + dCROSS(Normal[1],=,BD,CE); + dNormalize3(Normal[1]); + + //CD + dCROSS(Normal[2],=,CD,FB); + dNormalize3(Normal[2]); +#endif + int nA[3],nB[3]; + dContactGeom ContactA[3],ContactB[3]; + dxRay rayBC(0,lBC); + dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]); + nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom)); + dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]); + nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom)); + + dxRay rayBD(0,lBD); + dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]); + nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom)); + dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]); + nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom)); + + dxRay rayCD(0,lCD); + dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]); + nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom)); + dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]); + nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom)); + + for (i=0;i<3;i++) + { + if (nA[i] & nB[i]) + { + dContactGeom *pContact = CONTACT(contact,numContacts*skip); + pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2; + pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2; + pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2; +#ifdef RECOMPUTE_RAYNORMAL + pContact->normal[0] = -Normal[i][0]; + pContact->normal[1] = -Normal[i][1]; + pContact->normal[2] = -Normal[i][2]; +#else + pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f; + pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f; + pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f; + dNormalize3(pContact->normal); +#endif +#ifdef DO_RAYDEPTH + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } +#else + + if (GetDepth == NULL) + { + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } + } + else + { + pContact->depth = GetDepth(o2, + pContact->pos[0], + pContact->pos[1], + pContact->pos[2]); + numContacts++; + } + +#endif + if (numContacts == numMaxContacts) + return numContacts; + + } + } + } + + dCROSS(Plane,=,AC,AB); + dNormalize3(Plane); + Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2]; + dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + + //DMESS(0); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + dCROSS(Plane,=,BD,CD); + dNormalize3(Plane); + Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2]; + dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + //DMESS(1); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + return numContacts; +} + +int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTerrainYClass); + int i,j; + + if ((flags & 0xffff) == 0) + flags = (flags & 0xffff0000) | 1; + + int numMaxTerrainContacts = (flags & 0xffff); + dxTerrainY *terrain = (dxTerrainY*) o1; + + dReal aabbbak[6]; + int gflagsbak; + + dVector3 pos0; + int numTerrainContacts = 0; + + dxPosR *bak; + dxPosR X1; + + if (terrain->gflags & GEOM_PLACEABLE) + { + dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos); + dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0); + dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R); + bak = o2->final_posr; + o2->final_posr = &X1; + memcpy(aabbbak,o2->aabb,sizeof(dReal)*6); + gflagsbak = o2->gflags; + o2->computeAABB(); + } + + int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength)); + int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1; + int nMinZ = int(floor(o2->aabb[4] / terrain->m_vNodeLength)); + int nMaxZ = int(floor(o2->aabb[5] / terrain->m_vNodeLength)) + 1; + + if (terrain->m_bFinite) + { + nMinX = MAX(nMinX,0); + nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide); + nMinZ = MAX(nMinZ,0); + nMaxZ = MIN(nMaxZ,terrain->m_nNumNodesPerSide); + + if ((nMinX >= nMaxX) || (nMinZ >= nMaxZ)) + goto dCollideTerrainYExit; + } + + dVector3 AabbTop; + AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2; + AabbTop[2] = (o2->aabb[4]+o2->aabb[5]) / 2; + AabbTop[1] = o2->aabb[3]; + if (o2->type != dRayClass) + { + dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[2]) - AabbTop[1]; + if (AabbTopDepth > 0.f) + { + contact->depth = AabbTopDepth; + dReal MaxDepth = (o2->aabb[3]-o2->aabb[2]) / 2; + if (contact->depth > MaxDepth) + contact->depth = MaxDepth; + contact->g1 = o1; + contact->g2 = o2; + dOPE(contact->pos,=,AabbTop); + contact->normal[0] = 0.f; + contact->normal[1] = -1.f; + contact->normal[2] = 0.f; + + numTerrainContacts = 1; + goto dCollideTerrainYExit; + } + } + + for (i=nMinX;idCollideTerrainUnit( + i,j,o2,numMaxTerrainContacts - numTerrainContacts, + flags,CONTACT(contact,numTerrainContacts*skip),skip ); + } + } + + dIASSERT(numTerrainContacts <= numMaxTerrainContacts); + + for (i=0; ig1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + +dCollideTerrainYExit: + + if (terrain->gflags & GEOM_PLACEABLE) + { + o2->final_posr = bak; + memcpy(o2->aabb,aabbbak,sizeof(dReal)*6); + o2->gflags = gflagsbak; + + for (i=0; ipos); + dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0); + dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos); + + dOPE(pos0,=,CONTACT(contact,i*skip)->normal); + dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0); + } + } + + return numTerrainContacts; +} +/* +void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos) +{ + float A[3],B[3],C[3],D[3]; + float R[12]; + float pos[3]; + if (pR) + memcpy(R,pR,sizeof(R)); + else + { + memset(R,0,sizeof(R)); + R[0] = 1.f; + R[5] = 1.f; + R[10] = 1.f; + } + + if (ppos) + memcpy(pos,ppos,sizeof(pos)); + else + memset(pos,0,sizeof(pos)); + + float vx,vz; + vx = vLength * x; + vz = vLength * z; + + int i; + for (i=0;i +#include +#include +#include +#include +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_std_internal.h" +#include "collision_util.h" +//#include +#include "windows.h" +#include "ode\ode.h" + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) +#define MAXCONTACT 10 +#define TERRAINTOL 0.0f + +static bool IsAPowerOfTwo(int f) +{ + dAASSERT(f!=0); + while ((f&1) != 1) + f >>= 1; + + return (f == 1); +} + +static int GetPowerOfTwo(int f) +{ + dAASSERT(f!=0); + int n = 0; + while ((f&1) != 1) + { + n++; + f >>= 1; + } + + return n; +} + +dxTerrainZ::dxTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) : +dxGeom (space,bPlaceable) +{ + dIASSERT(IsAPowerOfTwo(nNumNodesPerSide)); + dIASSERT(pHeights); + dIASSERT(vLength > 0.f); + dIASSERT(nNumNodesPerSide > 0); + type = dTerrainZClass; + m_vLength = vLength; + m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide]; + dIASSERT(m_pHeights); + m_nNumNodesPerSide = nNumNodesPerSide; + m_vNodeLength = m_vLength / m_nNumNodesPerSide; + m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide); + m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1; + m_vMinHeight = dInfinity; + m_vMaxHeight = -dInfinity; + m_bFinite = bFinite; + + for (int i=0;i m_vMaxHeight) m_vMaxHeight = m_pHeights[i]; + } +} + +dxTerrainZ::~dxTerrainZ() +{ + dIASSERT(m_pHeights); + delete [] m_pHeights; +} + +void dxTerrainZ::computeAABB() +{ + if (m_bFinite) + { + if (gflags & GEOM_PLACEABLE) + { + dReal dx[6],dy[6],dz[6]; + dx[0] = 0; + dx[1] = final_posr->R[0] * m_vLength; + dx[2] = 0; + dx[3] = final_posr->R[1] * m_vLength; + dx[4] = final_posr->R[2] * m_vMinHeight; + dx[5] = final_posr->R[2] * m_vMaxHeight; + + dy[0] = 0; + dy[1] = final_posr->R[4] * m_vLength; + dy[2] = 0; + dy[3] = final_posr->R[5] * m_vLength; + dy[4] = final_posr->R[6] * m_vMinHeight; + dy[5] = final_posr->R[6] * m_vMaxHeight; + + dz[0] = 0; + dz[1] = final_posr->R[8] * m_vLength; + dz[2] = 0; + dz[3] = final_posr->R[9] * m_vLength; + dz[4] = final_posr->R[10] * m_vMinHeight; + dz[5] = final_posr->R[10] * m_vMaxHeight; + + aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]); + aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]); + aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]); + aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]); + aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]); + aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]); + } + else + { + aabb[0] = 0; + aabb[1] = m_vLength; + aabb[2] = 0; + aabb[3] = m_vLength; + aabb[4] = m_vMinHeight; + aabb[5] = m_vMaxHeight; + } + } + else + { + if (gflags & GEOM_PLACEABLE) + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + } + else + { + aabb[0] = -dInfinity; + aabb[1] = dInfinity; + aabb[2] = -dInfinity; + aabb[3] = dInfinity; + aabb[4] = m_vMinHeight; + aabb[5] = m_vMaxHeight; + } + } +} + +dReal dxTerrainZ::GetHeight(int x,int y) +{ + return m_pHeights[ (((unsigned int)(y) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift) + + ((unsigned int)(x) & m_nNumNodesPerSideMask)]; +} + +dReal dxTerrainZ::GetHeight(dReal x,dReal y) +{ + int nX = int(floor(x / m_vNodeLength)); + int nY = int(floor(y / m_vNodeLength)); + dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength; + dReal dy = (y - (dReal(nY) * m_vNodeLength)) / m_vNodeLength; + dIASSERT((dx >= 0.f) && (dx <= 1.f)); + dIASSERT((dy >= 0.f) && (dy <= 1.f)); + + dReal z,z0; + + if (dx + dy < 1.f) + { + z0 = GetHeight(nX,nY); + z = z0 + + (GetHeight(nX+1,nY) - z0) * dx + + (GetHeight(nX,nY+1) - z0) * dy; + } + else + { + z0 = GetHeight(nX+1,nY+1); + z = z0 + + (GetHeight(nX+1,nY) - z0) * (1.f - dy) + + (GetHeight(nX,nY+1) - z0) * (1.f - dx); + } + + return z; +} + +bool dxTerrainZ::IsOnTerrain(int nx,int ny,int w,dReal *pos) +{ + dVector3 Min,Max; + Min[0] = nx * m_vNodeLength; + Min[1] = ny * m_vNodeLength; + Max[0] = (nx+1) * m_vNodeLength; + Max[1] = (ny+1) * m_vNodeLength; + dReal Tol = m_vNodeLength * TERRAINTOL; + + if ((pos[0]Max[0]+Tol)) + return false; + + if ((pos[1]Max[1]+Tol)) + return false; + + dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength; + dReal dy = (pos[1] - (dReal(ny) * m_vNodeLength)) / m_vNodeLength; + + if ((w == 0) && (dx + dy > 1.f+TERRAINTOL)) + return false; + + if ((w == 1) && (dx + dy < 1.f-TERRAINTOL)) + return false; + + return true; +} + +dGeomID dCreateTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) +{ + return new dxTerrainZ(space, pHeights,vLength,nNumNodesPerSide, bFinite, bPlaceable); +} + +dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z) +{ + dUASSERT (g && g->type == dTerrainZClass,"argument not a terrain"); + g->recomputePosr(); + dxTerrainZ *t = (dxTerrainZ*) g; + return t->GetHeight(x,y) - z; +} + +typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z); +#define RECOMPUTE_RAYNORMAL +//#define DO_RAYDEPTH + +#define DMESS(A) \ + dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \ + x,y,A, \ + pContact->depth, \ + dGeomSphereGetRadius(o2), \ + pContact->pos[0], \ + pContact->pos[1], \ + pContact->pos[2], \ + pContact->normal[0], \ + pContact->normal[1], \ + pContact->normal[2]); +/* +(z is up) + +y +. +F +| +C-D +|\| +A-B-E.x +*/ +int dxTerrainZ::dCollideTerrainUnit( + int x,int y,dxGeom *o2,int numMaxContacts, + int flags,dContactGeom *contact, int skip) +{ + dColliderFn *CollideRayN; + dColliderFn *CollideNPlane; + dGetDepthFn *GetDepth; + int numContacts = 0; + int numPlaneContacts = 0; + int i; + + if (numContacts == numMaxContacts) + return numContacts; + + dContactGeom PlaneContact[MAXCONTACT]; + flags = (flags & 0xffff0000) | MAXCONTACT; + + switch (o2->type) + { + case dSphereClass: + CollideRayN = dCollideRaySphere; + CollideNPlane = dCollideSpherePlane; + GetDepth = dGeomSpherePointDepth; + break; + case dBoxClass: + CollideRayN = dCollideRayBox; + CollideNPlane = dCollideBoxPlane; + GetDepth = dGeomBoxPointDepth; + break; + case dCCylinderClass: + CollideRayN = dCollideRayCCylinder; + CollideNPlane = dCollideCCylinderPlane; + GetDepth = dGeomCCylinderPointDepth; + break; + case dRayClass: + CollideRayN = NULL; + CollideNPlane = dCollideRayPlane; + GetDepth = NULL; + break; + case dConeClass: + CollideRayN = dCollideRayCone; + CollideNPlane = dCollideConePlane; + GetDepth = dGeomConePointDepth; + break; + default: + dIASSERT(0); + } + + dReal Plane[4],lBD,lCD,lBC; + dVector3 A,B,C,D,BD,CD,BC,AB,AC; + A[0] = x * m_vNodeLength; + A[1] = y * m_vNodeLength; + A[2] = GetHeight(x,y); + B[0] = (x+1) * m_vNodeLength; + B[1] = y * m_vNodeLength; + B[2] = GetHeight(x+1,y); + C[0] = x * m_vNodeLength; + C[1] = (y+1) * m_vNodeLength; + C[2] = GetHeight(x,y+1); + D[0] = (x+1) * m_vNodeLength; + D[1] = (y+1) * m_vNodeLength; + D[2] = GetHeight(x+1,y+1); + + dOP(BC,-,C,B); + lBC = dLENGTH(BC); + dOPEC(BC,/=,lBC); + + dOP(BD,-,D,B); + lBD = dLENGTH(BD); + dOPEC(BD,/=,lBD); + + dOP(CD,-,D,C); + lCD = dLENGTH(CD); + dOPEC(CD,/=,lCD); + + dOP(AB,-,B,A); + dNormalize3(AB); + + dOP(AC,-,C,A); + dNormalize3(AC); + + if (CollideRayN) + { +#ifdef RECOMPUTE_RAYNORMAL + dVector3 E,F; + dVector3 CE,FB,AD; + dVector3 Normal[3]; + E[0] = (x+2) * m_vNodeLength; + E[1] = y * m_vNodeLength; + E[2] = GetHeight(x+2,y); + F[0] = x * m_vNodeLength; + F[1] = (y+2) * m_vNodeLength; + F[2] = GetHeight(x,y+2); + dOP(AD,-,D,A); + dNormalize3(AD); + dOP(CE,-,E,C); + dNormalize3(CE); + dOP(FB,-,B,F); + dNormalize3(FB); + + //BC + dCROSS(Normal[0],=,AD,BC); + dNormalize3(Normal[0]); + + //BD + dCROSS(Normal[1],=,CE,BD); + dNormalize3(Normal[1]); + + //CD + dCROSS(Normal[2],=,FB,CD); + dNormalize3(Normal[2]); +#endif + int nA[3],nB[3]; + dContactGeom ContactA[3],ContactB[3]; + dxRay rayBC(0,lBC); + dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]); + nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom)); + dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]); + nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom)); + + dxRay rayBD(0,lBD); + dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]); + nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom)); + dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]); + nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom)); + + dxRay rayCD(0,lCD); + dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]); + nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom)); + dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]); + nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom)); + + for (i=0;i<3;i++) + { + if (nA[i] & nB[i]) + { + dContactGeom *pContact = CONTACT(contact,numContacts*skip); + pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2; + pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2; + pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2; +#ifdef RECOMPUTE_RAYNORMAL + pContact->normal[0] = -Normal[i][0]; + pContact->normal[1] = -Normal[i][1]; + pContact->normal[2] = -Normal[i][2]; +#else + pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f; + pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f; + pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f; + dNormalize3(pContact->normal); +#endif +#ifdef DO_RAYDEPTH + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } +#else + if (GetDepth == NULL) + { + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } + } + else + { + pContact->depth = GetDepth(o2, + pContact->pos[0], + pContact->pos[1], + pContact->pos[2]); + numContacts++; + } +#endif + if (numContacts == numMaxContacts) + return numContacts; + + } + } + } + + dCROSS(Plane,=,AB,AC); + dNormalize3(Plane); + Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2]; + dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + + //DMESS(0); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + dCROSS(Plane,=,CD,BD); + dNormalize3(Plane); + Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2]; + dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;ipos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + //DMESS(1); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + return numContacts; +} + +int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dTerrainZClass); + int i,j; + + if ((flags & 0xffff) == 0) + flags = (flags & 0xffff0000) | 1; + + int numMaxTerrainContacts = (flags & 0xffff); + dxTerrainZ *terrain = (dxTerrainZ*) o1; + + dReal aabbbak[6]; + int gflagsbak; + + dVector3 pos0; + int numTerrainContacts = 0; + + dxPosR *bak; + dxPosR X1; + + if (terrain->gflags & GEOM_PLACEABLE) + { + dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos); + dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0); + dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R); + bak = o2->final_posr; + o2->final_posr = &X1; + memcpy(aabbbak,o2->aabb,sizeof(dReal)*6); + gflagsbak = o2->gflags; + o2->computeAABB(); + } + + int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength)); + int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1; + int nMinY = int(floor(o2->aabb[2] / terrain->m_vNodeLength)); + int nMaxY = int(floor(o2->aabb[3] / terrain->m_vNodeLength)) + 1; + + if (terrain->m_bFinite) + { + nMinX = MAX(nMinX,0); + nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide); + nMinY = MAX(nMinY,0); + nMaxY = MIN(nMaxY,terrain->m_nNumNodesPerSide); + + if ((nMinX >= nMaxX) || (nMinY >= nMaxY)) + goto dCollideTerrainZExit; + } + + dVector3 AabbTop; + AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2; + AabbTop[1] = (o2->aabb[2]+o2->aabb[3]) / 2; + AabbTop[2] = o2->aabb[5]; + if (o2->type != dRayClass) + { + dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[1]) - AabbTop[2]; + if (AabbTopDepth > 0.f) + { + contact->depth = AabbTopDepth; + dReal MaxDepth = (o2->aabb[5]-o2->aabb[4]) / 2; + if (contact->depth > MaxDepth) + contact->depth = MaxDepth; + contact->g1 = o1; + contact->g2 = o2; + dOPE(contact->pos,=,AabbTop); + contact->normal[0] = 0.f; + contact->normal[1] = 0.f; + contact->normal[2] = -1.f; + + numTerrainContacts = 1; + goto dCollideTerrainZExit; + } + } + + for (i=nMinX;idCollideTerrainUnit( + i,j,o2,numMaxTerrainContacts - numTerrainContacts, + flags,CONTACT(contact,numTerrainContacts*skip),skip ); + } + } + + dIASSERT(numTerrainContacts <= numMaxTerrainContacts); + + for (i=0; ig1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + +dCollideTerrainZExit: + + if (terrain->gflags & GEOM_PLACEABLE) + { + o2->final_posr = bak; + memcpy(o2->aabb,aabbbak,sizeof(dReal)*6); + o2->gflags = gflagsbak; + + for (i=0; ipos); + dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0); + dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos); + + dOPE(pos0,=,CONTACT(contact,i*skip)->normal); + dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0); + } + } + + return numTerrainContacts; +} +/* +void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos) +{ + float A[3],B[3],C[3],D[3]; + float R[12]; + float pos[3]; + if (pR) + memcpy(R,pR,sizeof(R)); + else + { + memset(R,0,sizeof(R)); + R[0] = 1.f; + R[5] = 1.f; + R[10] = 1.f; + } + + if (ppos) + memcpy(pos,ppos,sizeof(pos)); + else + memset(pos,0,sizeof(pos)); + + float vx,vz; + vx = vLength * x; + vz = vLength * z; + + int i; + for (i=0;i edit each .cpp file and comment out #include "windows.h" & #include "ode\ode.h" + + +*** add to drawstuff\src\drawstuff.cpp: + +static void drawCone(float l, float r) +{ + int i; + float tmp,ny,nz,a,ca,sa; + const int n = 24; // number of sides to the cone (divisible by 4) + + a = float(M_PI*2.0)/float(n); + sa = (float) sin(a); + ca = (float) cos(a); + + // draw top + glShadeModel (GL_FLAT); + ny=1; nz=0; // normal vector = (0,ny,nz) + glBegin (GL_TRIANGLE_FAN); + glNormal3d (0,0,1); + glVertex3d (0,0,l); + for (i=0; i<=n; i++) { + if (i==1 || i==n/2+1) + setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]); + glNormal3d (ny*r,nz*r,0); + glVertex3d (ny*r,nz*r,0); + if (i==1 || i==n/2+1) + setColor (color[0],color[1],color[2],color[3]); + + // rotate ny,nz + tmp = ca*ny - sa*nz; + nz = sa*ny + ca*nz; + ny = tmp; + } + glEnd(); + + // draw bottom + ny=1; nz=0; // normal vector = (0,ny,nz) + glBegin (GL_TRIANGLE_FAN); + glNormal3d (0,0,-1); + glVertex3d (0,0,0); + for (i=0; i<=n; i++) { + if (i==1 || i==n/2+1) + setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]); + glNormal3d (0,0,-1); + glVertex3d (ny*r,nz*r,0); + if (i==1 || i==n/2+1) + setColor (color[0],color[1],color[2],color[3]); + + // rotate ny,nz + tmp = ca*ny + sa*nz; + nz = -sa*ny + ca*nz; + ny = tmp; + } + glEnd(); +} + +void dsDrawCone (const float pos[3], const float R[12], float length, float radius) +{ + if (current_state != 2) dsError ("drawing function called outside simulation loop"); + setupDrawingMode(); + glShadeModel (GL_SMOOTH); + setTransform (pos,R); + drawCone (length,radius); + glPopMatrix(); + + if (use_shadows) { + setShadowDrawingMode(); + setShadowTransform(); + setTransform (pos,R); + drawCone (length,radius); + glPopMatrix(); + glPopMatrix(); + glDepthRange (0,1); + } +} + +void dsDrawConeD (const double pos[3], const double R[12], float length, float radius) +{ + int i; + float pos2[3],R2[12]; + for (i=0; i<3; i++) pos2[i]=(float)pos[i]; + for (i=0; i<12; i++) R2[i]=(float)R[i]; + dsDrawCone(pos2,R2,length,radius); +} + +static float GetHeight(int x,int y,int nNumNodesPerSide,float *pHeights) +{ + int nNumNodesPerSideMask = nNumNodesPerSide - 1; + return pHeights[ (((unsigned int)(y) & nNumNodesPerSideMask) * nNumNodesPerSide) + + ((unsigned int)(x) & nNumNodesPerSideMask)]; +} + +void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos) +{ + float A[3],B[3],C[3],D[3]; + float R[12]; + float pos[3]; + if (pR) + memcpy(R,pR,sizeof(R)); + else + { + memset(R,0,sizeof(R)); + R[0] = 1.f; + R[5] = 1.f; + R[10] = 1.f; + } + + if (ppos) + memcpy(pos,ppos,sizeof(pos)); + else + memset(pos,0,sizeof(pos)); + + float vx,vz; + vx = vLength * x; + vz = vLength * z; + + int i; + for (i=0;i add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the libode_a_SOURCES variable in the ode/src/Makefile.am file. + +*** now you can now test using file test_boxstackb.cpp (to add in folder ode\test). + diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp new file mode 100644 index 0000000..f1fa592 --- /dev/null +++ b/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp @@ -0,0 +1,1375 @@ +/************************************************************************* + + +* * + + +* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + + +* All rights reserved. Email: russ@q12.org Web: www.q12.org * + + +* * + + +* This library is free software; you can redistribute it and/or * + + +* modify it under the terms of EITHER: * + + +* (1) The GNU Lesser General Public License as published by the Free * + + +* Software Foundation; either version 2.1 of the License, or (at * + + +* your option) any later version. The text of the GNU Lesser * + + +* General Public License is included with this library in the * + + +* file LICENSE.TXT. * + + +* (2) The BSD-style license that is included with this library in * + + +* the file LICENSE-BSD.TXT. * + + +* * + + +* This library is distributed in the hope that it will be useful, * + + +* but WITHOUT ANY WARRANTY; without even the implied warranty of * + + +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + + +* LICENSE.TXT and LICENSE-BSD.TXT for more details. * + + +* * + + +*************************************************************************/ + + + + + +#include + + +#include + + + + + +#ifdef _MSC_VER + + +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints + + +#endif + + + + + +// select correct drawing functions + + + + + +#ifdef dDOUBLE + + +#define dsDrawBox dsDrawBoxD + + +#define dsDrawSphere dsDrawSphereD + + +#define dsDrawCylinder dsDrawCylinderD + + +#define dsDrawCappedCylinder dsDrawCappedCylinderD + + +#endif + + + + + + + + +// some constants + + + + + +const dReal vTerrainLength = 4.f; + + +const dReal vTerrainHeight = 0.5f; + + +const int TERRAINNODES = 4; + + +dReal pTerrainHeights[TERRAINNODES*TERRAINNODES]; + + + + + +dGeomID terrainZ = NULL; + + +dGeomID terrainY = NULL; + + + + + +#define NUM 20 // max number of objects + + +#define DENSITY (5.0) // density of all objects + + +#define GPB 3 // maximum number of geometries per body + + +#define MAX_CONTACTS 4 // maximum number of contact points per body + + + + + + + + +// dynamics and collision objects + + + + + +struct MyObject { + + + dBodyID body; // the body + + + dGeomID geom[GPB]; // geometries representing this body + + +}; + + + + + +static int num=0; // number of objects in simulation + + +static int nextobj=0; // next object to recycle if num==NUM + + +static dWorldID world; + + +static dSpaceID space; + + +static MyObject obj[NUM]; + + +static dJointGroupID contactgroup; + + +static int selected = -1; // selected object + + +static int show_aabb = 0; // show geom AABBs? + + +static int show_contacts = 0; // show contact points? + + +static int random_pos = 1; // drop objects from random position? + + + + + + + + +// this is called by dSpaceCollide when two objects in space are + + +// potentially colliding. + + + + + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) + + +{ + + + int i; + + + // if (o1->body && o2->body) return; + + + + + + // exit without doing anything if the two bodies are connected by a joint + + + dBodyID b1 = dGeomGetBody(o1); + + + dBodyID b2 = dGeomGetBody(o2); + + + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + + + + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + + + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + + + else return c; + + +} + + + + + + + + +// called when a key pressed + + + + + +static void command (int cmd) + + +{ + + + int i,j,k; + + + dReal sides[3]; + + + dMass m; + + + + + + cmd = locase (cmd); + + + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' + + + /* || cmd == 'l' */) { + + + if (num < NUM) { + + + i = num; + + + num++; + + + } + + + else { + + + i = nextobj; + + + nextobj++; + + + if (nextobj >= num) nextobj = 0; + + + + + + // destroy the body and geoms for slot i + + + dBodyDestroy (obj[i].body); + + + for (k=0; k < GPB; k++) { + + + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + + + } + + + memset (&obj[i],0,sizeof(obj[i])); + + + } + + + + + + obj[i].body = dBodyCreate (world); + + + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + + + + + dMatrix3 R; + + + if (random_pos) { + + + dBodySetPosition (obj[i].body, + + + dRandReal()*2-1,dRandReal()*2+1,dRandReal()+3); + + + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + + + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + + + } + + + else { + + + dReal maxheight = 0; + + + for (k=0; k maxheight) maxheight = pos[2]; + + + } + + + dBodySetPosition (obj[i].body, 0,maxheight+1,maxheight+3); + + + dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); + + + } + + + dBodySetRotation (obj[i].body,R); + + + dBodySetData (obj[i].body,(void*) i); + + + + + + if (cmd == 'b') { + + + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + + + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + + + } + + + else if (cmd == 'c') { + + + sides[0] *= 0.5; + + + dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); + + + obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]); + + + } + + + /* + + + // cylinder option not yet implemented + + + else if (cmd == 'l') { + + + sides[1] *= 0.5; + + + dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); + + + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + + + } + + + */ + + + else if (cmd == 's') { + + + sides[0] *= 0.5; + + + dMassSetSphere (&m,DENSITY,sides[0]); + + + obj[i].geom[0] = dCreateSphere (space,sides[0]); + + + } + + + else if (cmd == 'x') { + + + dGeomID g2[GPB]; // encapsulated geometries + + + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + + + + + // start accumulating masses for the encapsulated geometries + + + dMass m2; + + + dMassSetZero (&m); + + + + + + // set random delta positions + + + for (j=0; j= num) selected = 0; + + + if (selected < 0) selected = 0; + + + } + + + else if (cmd == 'd' && selected >= 0 && selected < num) { + + + dBodyDisable (obj[selected].body); + + + } + + + else if (cmd == 'e' && selected >= 0 && selected < num) { + + + dBodyEnable (obj[selected].body); + + + } + + + else if (cmd == 'a') { + + + show_aabb ^= 1; + + + } + + + else if (cmd == 't') { + + + show_contacts ^= 1; + + + } + + + else if (cmd == 'r') { + + + random_pos ^= 1; + + + } + + +} + + + + + + + + +// draw a geom + + + + + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) + + +{ + + + int i; + + + + + + if (!g) return; + + + if (!pos) pos = dGeomGetPosition (g); + + + if (!R) R = dGeomGetRotation (g); + + + + + + int type = dGeomGetClass (g); + + + if (type == dBoxClass) { + + + dVector3 sides; + + + dGeomBoxGetLengths (g,sides); + + + dsDrawBox (pos,R,sides); + + + } + + + else if (type == dSphereClass) { + + + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + + + } + + + else if (type == dCCylinderClass) { + + + dReal radius,length; + + + dGeomCCylinderGetParams (g,&radius,&length); + + + dsDrawCappedCylinder (pos,R,length,radius); + + + } + + + /* + + + // cylinder option not yet implemented + + + else if (type == dCylinderClass) { + + + dReal radius,length; + + + dGeomCylinderGetParams (g,&radius,&length); + + + dsDrawCylinder (pos,R,length,radius); + + + } + + + */ + + + else if (type == dGeomTransformClass) { + + + dGeomID g2 = dGeomTransformGetGeom (g); + + + const dReal *pos2 = dGeomGetPosition (g2); + + + const dReal *R2 = dGeomGetRotation (g2); + + + dVector3 actual_pos; + + + dMatrix3 actual_R; + + + dMULTIPLY0_331 (actual_pos,R,pos2); + + + actual_pos[0] += pos[0]; + + + actual_pos[1] += pos[1]; + + + actual_pos[2] += pos[2]; + + + dMULTIPLY0_333 (actual_R,R,R2); + + + drawGeom (g2,actual_pos,actual_R,0); + + + } + + + + + + if (show_aabb) { + + + // draw the bounding box for this geom + + + dReal aabb[6]; + + + dGeomGetAABB (g,aabb); + + + dVector3 bbpos; + + + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + + + dVector3 bbsides; + + + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + + + dMatrix3 RI; + + + dRSetIdentity (RI); + + + dsSetColorAlpha (1,0,0,0.5); + + + dsDrawBox (bbpos,RI,bbsides); + + + } + + +} + + + + + + + + +// simulation loop + + + + + +static void simLoop (int pause) + + +{ + + + dsSetColor (0,0,2); + + + dSpaceCollide (space,0,&nearCallback); + + + if (!pause) dWorldStep (world,0.05); + + + + + + dAASSERT(terrainY); + + + dAASSERT(terrainZ); + + + dsSetColor (0,1,0); + + + dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY)); + + + dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ)); + + + + + + if (show_aabb) + + + { + + + dReal aabb[6]; + + + dGeomGetAABB (terrainY,aabb); + + + dVector3 bbpos; + + + int i; + + + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + + + dVector3 bbsides; + + + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + + + dMatrix3 RI; + + + dRSetIdentity (RI); + + + dsSetColorAlpha (1,0,0,0.5); + + + dsDrawBox (bbpos,RI,bbsides); + + + + + + dGeomGetAABB (terrainZ,aabb); + + + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + + + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + + + dsDrawBox (bbpos,RI,bbsides); + + + } + + + + + + dsSetColor (1,1,0); + + + + + + // remove all contact joints + + + dJointGroupEmpty (contactgroup); + + + + + + dsSetColor (1,1,0); + + + dsSetTexture (DS_WOOD); + + + for (int i=0; i +#include "dCylinder.h" +// given a pointer `p' to a dContactGeom, return the dContactGeom at +// p + skip bytes. + +struct dxCylinder { // cylinder + dReal radius,lz; // radius, length along y axis // +}; + +int dCylinderClassUser = -1; + +#define NUMC_MASK (0xffff) + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) + +///////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////circleIntersection////////////////////////////////////////////////// +//this does following: +//takes two circles as normals to planes n1,n2, center points cp1,cp2,and radiuses r1,r2 +//finds line on which circles' planes intersect +//finds four points O1,O2 - intersection between the line and sphere with center cp1 radius r1 +// O3,O4 - intersection between the line and sphere with center cp2 radius r2 +//returns false if there is no intersection +//computes distances O1-O3, O1-O4, O2-O3, O2-O4 +//in "point" returns mean point between intersection points with smallest distance +///////////////////////////////////////////////////////////////////////////////////////////////// +inline bool circleIntersection(const dReal* n1,const dReal* cp1,dReal r1,const dReal* n2,const dReal* cp2,dReal r2,dVector3 point){ +dReal c1=dDOT14(cp1,n1); +dReal c2=dDOT14(cp2,n2); +dReal cos=dDOT44(n1,n2); +dReal cos_2=cos*cos; +dReal sin_2=1-cos_2; +dReal p1=(c1-c2*cos)/sin_2; +dReal p2=(c2-c1*cos)/sin_2; +dVector3 lp={p1*n1[0]+p2*n2[0],p1*n1[4]+p2*n2[4],p1*n1[8]+p2*n2[8]}; +dVector3 n; +dCROSS144(n,=,n1,n2); +dVector3 LC1={lp[0]-cp1[0],lp[1]-cp1[1],lp[2]-cp1[2]}; +dVector3 LC2={lp[0]-cp2[0],lp[1]-cp2[1],lp[2]-cp2[2]}; +dReal A,B,C,B_A,B_A_2,D; +dReal t1,t2,t3,t4; +A=dDOT(n,n); +B=dDOT(LC1,n); +C=dDOT(LC1,LC1)-r1*r1; +B_A=B/A; +B_A_2=B_A*B_A; +D=B_A_2-C; +if(D<0.f){ //somewhat strange solution + //- it is needed to set some + //axis to sepparate cylinders + //when their edges approach + t1=-B_A+sqrtf(-D); + t2=-B_A-sqrtf(-D); +// return false; + } +else{ +t1=-B_A-sqrtf(D); +t2=-B_A+sqrtf(D); +} +B=dDOT(LC2,n); +C=dDOT(LC2,LC2)-r2*r2; +B_A=B/A; +B_A_2=B_A*B_A; +D=B_A_2-C; + +if(D<0.f) { + t3=-B_A+sqrtf(-D); + t4=-B_A-sqrtf(-D); +// return false; + } +else{ +t3=-B_A-sqrtf(D); +t4=-B_A+sqrtf(D); +} +dVector3 O1={lp[0]+n[0]*t1,lp[1]+n[1]*t1,lp[2]+n[2]*t1}; +dVector3 O2={lp[0]+n[0]*t2,lp[1]+n[1]*t2,lp[2]+n[2]*t2}; + +dVector3 O3={lp[0]+n[0]*t3,lp[1]+n[1]*t3,lp[2]+n[2]*t3}; +dVector3 O4={lp[0]+n[0]*t4,lp[1]+n[1]*t4,lp[2]+n[2]*t4}; + +dVector3 L1_3={O3[0]-O1[0],O3[1]-O1[1],O3[2]-O1[2]}; +dVector3 L1_4={O4[0]-O1[0],O4[1]-O1[1],O4[2]-O1[2]}; + +dVector3 L2_3={O3[0]-O2[0],O3[1]-O2[1],O3[2]-O2[2]}; +dVector3 L2_4={O4[0]-O2[0],O4[1]-O2[1],O4[2]-O2[2]}; + + +dReal l1_3=dDOT(L1_3,L1_3); +dReal l1_4=dDOT(L1_4,L1_4); + +dReal l2_3=dDOT(L2_3,L2_3); +dReal l2_4=dDOT(L2_4,L2_4); + + +if (l1_3 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + *code = 0; + + // separating axis = cylinder ax u2 + //used when a box vertex touches a flat face of the cylinder + TEST (pp[1],(hlz + B1*Q21 + B2*Q22 + B3*Q23),R1+1,0); + + + // separating axis = box axis v1,v2,v3 + //used when cylinder edge touches box face + //there is two ways to compute sQ: sQ21=sqrtf(1.f-Q21*Q21); or sQ21=sqrtf(Q23*Q23+Q22*Q22); + //if we did not need Q23 and Q22 the first way might be used to quiken the routine but then it need to + //check if Q21<=1.f, becouse it may slightly exeed 1.f. + + + sQ21=sqrtf(Q23*Q23+Q22*Q22); + TEST (dDOT41(R2+0,p),(radius*sQ21 + hlz*Q21 + B1),R2+0,1); + + sQ22=sqrtf(Q23*Q23+Q21*Q21); + TEST (dDOT41(R2+1,p),(radius*sQ22 + hlz*Q22 + B2),R2+1,2); + + sQ23=sqrtf(Q22*Q22+Q21*Q21); + TEST (dDOT41(R2+2,p),(radius*sQ23 + hlz*Q23 + B3),R2+2,3); + + +#undef TEST +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + + +// separating axis is a normal to the cylinder axis passing across the nearest box vertex +//used when a box vertex touches the lateral surface of the cylinder + +dReal proj,boxProj,cos,sin,cos1,cos3; +dVector3 tAx,Ax,pb; +{ +//making Ax which is perpendicular to cyl ax to box position// +proj=dDOT14(p2,R1+1)-dDOT14(p1,R1+1); + +Ax[0]=p2[0]-p1[0]-R1[1]*proj; +Ax[1]=p2[1]-p1[1]-R1[5]*proj; +Ax[2]=p2[2]-p1[2]-R1[9]*proj; +dNormalize3(Ax); +//using Ax find box vertex which is nearest to the cylinder axis + dReal sign; + + for (i=0; i<3; i++) pb[i] = p2[i]; + sign = (dDOT14(Ax,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4]; + sign = (dDOT14(Ax,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1]; + sign = (dDOT14(Ax,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2]; + +//building axis which is normal to cylinder ax to the nearest box vertex +proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1); + +Ax[0]=pb[0]-p1[0]-R1[1]*proj; +Ax[1]=pb[1]-p1[1]-R1[5]*proj; +Ax[2]=pb[2]-p1[2]-R1[9]*proj; +dNormalize3(Ax); +} + +boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+ + dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+2)*B3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(radius+boxProj),Ax[0],Ax[1],Ax[2],4); + + +//next three test used to handle collisions between cylinder circles and box ages +proj=dDOT14(p1,R2+0)-dDOT14(p2,R2+0); + +tAx[0]=-p1[0]+p2[0]+R2[0]*proj; +tAx[1]=-p1[1]+p2[1]+R2[4]*proj; +tAx[2]=-p1[2]+p2[2]+R2[8]*proj; +dNormalize3(tAx); + +//now tAx is normal to first ax of the box to cylinder center +//making perpendicular to tAx lying in the plane which is normal to the cylinder axis +//it is tangent in the point where projection of tAx on cylinder's ring intersect edge circle + +cos=dDOT14(tAx,R1+0); +sin=dDOT14(tAx,R1+2); +tAx[0]=R1[2]*cos-R1[0]*sin; +tAx[1]=R1[6]*cos-R1[4]*sin; +tAx[2]=R1[10]*cos-R1[8]*sin; + + +//use cross between tAx and first ax of the box as separating axix + +dCROSS114(Ax,=,tAx,R2+0); +dNormalize3(Ax); + +boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+0)*B1)+ + dFabs(dDOT14(Ax,R2+2)*B3); + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],5); + + +//same thing with the second axis of the box +proj=dDOT14(p1,R2+1)-dDOT14(p2,R2+1); + +tAx[0]=-p1[0]+p2[0]+R2[1]*proj; +tAx[1]=-p1[1]+p2[1]+R2[5]*proj; +tAx[2]=-p1[2]+p2[2]+R2[9]*proj; +dNormalize3(tAx); + + +cos=dDOT14(tAx,R1+0); +sin=dDOT14(tAx,R1+2); +tAx[0]=R1[2]*cos-R1[0]*sin; +tAx[1]=R1[6]*cos-R1[4]*sin; +tAx[2]=R1[10]*cos-R1[8]*sin; + +dCROSS114(Ax,=,tAx,R2+1); +dNormalize3(Ax); + +boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+ + dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+2)*B3); + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],6); + +//same thing with the third axis of the box +proj=dDOT14(p1,R2+2)-dDOT14(p2,R2+2); + +Ax[0]=-p1[0]+p2[0]+R2[2]*proj; +Ax[1]=-p1[1]+p2[1]+R2[6]*proj; +Ax[2]=-p1[2]+p2[2]+R2[10]*proj; +dNormalize3(tAx); + +cos=dDOT14(tAx,R1+0); +sin=dDOT14(tAx,R1+2); +tAx[0]=R1[2]*cos-R1[0]*sin; +tAx[1]=R1[6]*cos-R1[4]*sin; +tAx[2]=R1[10]*cos-R1[8]*sin; + +dCROSS114(Ax,=,tAx,R2+2); +dNormalize3(Ax); +boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+2)*B3)+ + dFabs(dDOT14(Ax,R2+0)*B1); + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],7); + + +#undef TEST + +// note: cross product axes need to be scaled when s is computed. +// normal (n1,n2,n3) is relative to box 1. + +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } \ + } + +//crosses between cylinder axis and box axes + // separating axis = u2 x (v1,v2,v3) + TEST(pp[0]*R31-pp[2]*R11,(radius+B2*Q23+B3*Q22),R31,0,-R11,8); + TEST(pp[0]*R32-pp[2]*R12,(radius+B1*Q23+B3*Q21),R32,0,-R12,9); + TEST(pp[0]*R33-pp[2]*R13,(radius+B1*Q22+B2*Q21),R33,0,-R13,10); + + +#undef TEST + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + if(*code>7) dMULTIPLY0_331 (normal,R1,normalC); + else {normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];} + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (*code > 7) { + //find point on the cylinder pa deepest along normal + dVector3 pa; + dReal sign, cos1,cos3,factor; + + + for (i=0; i<3; i++) pa[i] = p1[i]; + + cos1 = dDOT14(normal,R1+0); + cos3 = dDOT14(normal,R1+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + + for (i=0; i<3; i++) pa[i] += cos1 * radius * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hlz * R1[i*4+1]; + + + for (i=0; i<3; i++) pa[i] += cos3 * radius * R1[i*4+2]; + + // find vertex of the box deepest along normal + dVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4]; + sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1]; + sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2]; + + + dReal alpha,beta; + dVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[1 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[*code-8 + i*4]; + + lineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); + contact[0].depth = *depth; + return 1; + } + + + if(*code==4){ + for (i=0; i<3; i++) contact[0].pos[i] = pb[i]; + contact[0].depth = *depth; + return 1; + } + + + dVector3 vertex; + if (*code == 0) { + + dReal sign; + for (i=0; i<3; i++) vertex[i] = p2[i]; + sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4]; + sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1]; + sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2]; + } + else { + + dReal sign,cos1,cos3,factor; + for (i=0; i<3; i++) vertex[i] = p1[i]; + cos1 = dDOT14(normal,R1+0) ; + cos3 = dDOT14(normal,R1+2); + factor=sqrtf(cos1*cos1+cos3*cos3); + factor= factor ? factor : 1.f; + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) vertex[i] += cos1 * radius * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) vertex[i] += sign * hlz * R1[i*4+1]; + + for (i=0; i<3; i++) vertex[i] += cos3 * radius * R1[i*4+2]; + } + for (i=0; i<3; i++) contact[0].pos[i] = vertex[i]; + contact[0].depth = *depth; + return 1; +} + +//**************************************************************************** + +extern "C" int dCylCyl (const dVector3 p1, const dMatrix3 R1, + const dReal radius1,const dReal lz1, const dVector3 p2, + const dMatrix3 R2, const dReal radius2,const dReal lz2, + dVector3 normal, dReal *depth, int *code, + int maxc, dContactGeom *contact, int skip) +{ + dVector3 p,pp1,pp2,normalC; + const dReal *normalR = 0; + dReal hlz1,hlz2,s,s2; + int i,invert_normal; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (pp1,R1,p); // get pp1 = p relative to body 1 + dMULTIPLY1_331 (pp2,R2,p); + // get side lengths / 2 + hlz1 = lz1*REAL(0.5); + hlz2 = lz2*REAL(0.5); + + dReal proj,cos,sin,cos1,cos3; + + + +#define TEST(expr1,expr2,norm,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + *code = 0; + + cos=dFabs(dDOT44(R1+1,R2+1)); + sin=sqrtf(1.f-(cos>1.f ? 1.f : cos)); + + TEST (pp1[1],(hlz1 + radius2*sin + hlz2*cos ),R1+1,0);//pp + + TEST (pp2[1],(radius1*sin + hlz1*cos + hlz2),R2+1,1); + + + + // note: cross product axes need to be scaled when s is computed. + +#undef TEST +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + +dVector3 tAx,Ax,pa,pb; + +//cross between cylinders' axes +dCROSS144(Ax,=,R1+1,R2+1); +dNormalize3(Ax); +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+radius2,Ax[0],Ax[1],Ax[2],6); + + +{ + + dReal sign, factor; + + //making ax which is perpendicular to cyl1 ax passing across cyl2 position// + //(project p on cyl1 flat surface ) + for (i=0; i<3; i++) pb[i] = p2[i]; + //cos1 = dDOT14(p,R1+0); + //cos3 = dDOT14(p,R1+2) ; + tAx[0]=pp1[0]*R1[0]+pp1[2]*R1[2]; + tAx[1]=pp1[0]*R1[4]+pp1[2]*R1[6]; + tAx[2]=pp1[0]*R1[8]+pp1[2]*R1[10]; + dNormalize3(tAx); + +//find deepest point pb of cyl2 on opposite direction of tAx + cos1 = dDOT14(tAx,R2+0); + cos3 = dDOT14(tAx,R2+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4]; + + sign = (dDOT14(tAx,R2+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1]; + + for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2]; + +//making perpendicular to cyl1 ax passing across pb + proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1); + + Ax[0]=pb[0]-p1[0]-R1[1]*proj; + Ax[1]=pb[1]-p1[1]-R1[5]*proj; + Ax[2]=pb[2]-p1[2]-R1[9]*proj; + +} + +dNormalize3(Ax); + + + cos=dFabs(dDOT14(Ax,R2+1)); + cos1=dDOT14(Ax,R2+0); + cos3=dDOT14(Ax,R2+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+cos*hlz2+sin*radius2,Ax[0],Ax[1],Ax[2],3); + + + +{ + + dReal sign, factor; + + for (i=0; i<3; i++) pa[i] = p1[i]; + + //making ax which is perpendicular to cyl2 ax passing across cyl1 position// + //(project p on cyl2 flat surface ) + //cos1 = dDOT14(p,R2+0); + //cos3 = dDOT14(p,R2+2) ; + tAx[0]=pp2[0]*R2[0]+pp2[2]*R2[2]; + tAx[1]=pp2[0]*R2[4]+pp2[2]*R2[6]; + tAx[2]=pp2[0]*R2[8]+pp2[2]*R2[10]; + dNormalize3(tAx); + + cos1 = dDOT14(tAx,R1+0); + cos3 = dDOT14(tAx,R1+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + cos1/=factor; + cos3/=factor; + +//find deepest point pa of cyl2 on direction of tAx + for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4]; + + sign = (dDOT14(tAx,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1]; + + + for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2]; + + proj=dDOT14(pa,R2+1)-dDOT14(p2,R2+1); + + Ax[0]=pa[0]-p2[0]-R2[1]*proj; + Ax[1]=pa[1]-p2[1]-R2[5]*proj; + Ax[2]=pa[2]-p2[2]-R2[9]*proj; + +} +dNormalize3(Ax); + + + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius2+cos*hlz1+sin*radius1,Ax[0],Ax[1],Ax[2],4); + + +////test circl + +//@ this needed to set right normal when cylinders edges intersect +//@ the most precise axis for this test may be found as a line between nearest points of two +//@ circles. But it needs comparatively a lot of computation. +//@ I use a trick which lets not to solve quadric equation. +//@ In the case when cylinder eidges touches the test below rather accurate. +//@ I still not sure about problems with sepparation but they have not been revealed during testing. +dVector3 point; +{ + dVector3 ca,cb; + dReal sign; + for (i=0; i<3; i++) ca[i] = p1[i]; + for (i=0; i<3; i++) cb[i] = p2[i]; +//find two nearest flat rings + sign = (pp1[1] > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) ca[i] += sign * hlz1 * R1[i*4+1]; + + sign = (pp2[1] > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) cb[i] -= sign * hlz2 * R2[i*4+1]; + + dVector3 tAx,tAx1; + circleIntersection(R1+1,ca,radius1,R2+1,cb,radius2,point); + + Ax[0]=point[0]-ca[0]; + Ax[1]=point[1]-ca[1]; + Ax[2]=point[2]-ca[2]; + + cos1 = dDOT14(Ax,R1+0); + cos3 = dDOT14(Ax,R1+2) ; + + tAx[0]=cos3*R1[0]-cos1*R1[2]; + tAx[1]=cos3*R1[4]-cos1*R1[6]; + tAx[2]=cos3*R1[8]-cos1*R1[10]; + + Ax[0]=point[0]-cb[0]; + Ax[1]=point[1]-cb[1]; + Ax[2]=point[2]-cb[2]; + + + cos1 = dDOT14(Ax,R2+0); + cos3 = dDOT14(Ax,R2+2) ; + + tAx1[0]=cos3*R2[0]-cos1*R2[2]; + tAx1[1]=cos3*R2[4]-cos1*R2[6]; + tAx1[2]=cos3*R2[8]-cos1*R2[10]; + dCROSS(Ax,=,tAx,tAx1); + + + + +dNormalize3(Ax); +dReal cyl1Pr,cyl2Pr; + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + cyl1Pr=cos*hlz1+sin*radius1; + + cos=dFabs(dDOT14(Ax,R2+1)); + cos1=dDOT14(Ax,R2+0); + cos3=dDOT14(Ax,R2+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + cyl2Pr=cos*hlz2+sin*radius2; +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],cyl1Pr+cyl2Pr,Ax[0],Ax[1],Ax[2],5); + + +} + + +#undef TEST + + + + // if we get to this point, the cylinders interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2]; + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + + *depth = -s; + + // compute contact point(s) + + if(*code==3){ + for (i=0; i<3; i++) contact[0].pos[i] = pb[i]; + contact[0].depth = *depth; + return 1; + } + + if(*code==4){ + for (i=0; i<3; i++) contact[0].pos[i] = pa[i]; + contact[0].depth = *depth; + return 1; + } + + if(*code==5){ + for (i=0; i<3; i++) contact[0].pos[i] = point[i]; + contact[0].depth = *depth; + return 1; + } + +if (*code == 6) { + dVector3 pa; + dReal sign, cos1,cos3,factor; + + + for (i=0; i<3; i++) pa[i] = p1[i]; + + cos1 = dDOT14(normal,R1+0); + cos3 = dDOT14(normal,R1+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + + for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1]; + + + for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2]; + + // find a point pb on the intersecting edge of cylinder 2 + dVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + cos1 = dDOT14(normal,R2+0); + cos3 = dDOT14(normal,R2+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + + for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4]; + + sign = (dDOT14(normal,R2+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1]; + + + for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2]; + + + dReal alpha,beta; + dVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[1 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[1 + i*4]; + lineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); + contact[0].depth = *depth; + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). + + // @@@ temporary: make deepest point on the "other" cylinder the contact point. + // @@@ this kind of works, but we need multiple contact points for stability, + // @@@ especially for face-face contact. + + dVector3 vertex; + if (*code == 0) { + // flat face from cylinder 1 touches a edge/face from cylinder 2. + dReal sign,cos1,cos3,factor; + for (i=0; i<3; i++) vertex[i] = p2[i]; + cos1 = dDOT14(normal,R2+0) ; + cos3 = dDOT14(normal,R2+2); + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) vertex[i] -= cos1 * radius2 * R2[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) vertex[i] -= sign * hlz2 * R2[i*4+1]; + + for (i=0; i<3; i++) vertex[i] -= cos3 * radius2 * R2[i*4+2]; + } + else { + // flat face from cylinder 2 touches a edge/face from cylinder 1. + dReal sign,cos1,cos3,factor; + for (i=0; i<3; i++) vertex[i] = p1[i]; + cos1 = dDOT14(normal,R1+0) ; + cos3 = dDOT14(normal,R1+2); + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) vertex[i] += cos1 * radius1 * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) vertex[i] += sign * hlz1 * R1[i*4+1]; + + for (i=0; i<3; i++) vertex[i] += cos3 * radius1 * R1[i*4+2]; + } + for (i=0; i<3; i++) contact[0].pos[i] = vertex[i]; + contact[0].depth = *depth; + return 1; +} + +//**************************************************************************** + + +int dCollideCylS (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + + + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (dGeomGetClass(o2) == dSphereClass); + dIASSERT (dGeomGetClass(o1) == dCylinderClassUser); + const dReal* p1=dGeomGetPosition(o1); + const dReal* p2=dGeomGetPosition(o2); + const dReal* R=dGeomGetRotation(o1); + dVector3 p,normalC,normal; + const dReal *normalR = 0; + dReal cylRadius; + dReal hl; + dGeomCylinderGetParams(o1,&cylRadius,&hl); + dReal sphereRadius; + sphereRadius=dGeomSphereGetRadius(o2); + + int i,invert_normal; + + // get vector from centers of cyl to shere + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + +dReal s,s2; +unsigned char code; +#define TEST(expr1,expr2,norm,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis cyl ax + + TEST (dDOT14(p,R+1),sphereRadius+hl,R+1,2); + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to +#undef TEST +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + +//making ax which is perpendicular to cyl1 ax to sphere center// + +dReal proj,cos,sin,cos1,cos3; +dVector3 Ax; + proj=dDOT14(p2,R+1)-dDOT14(p1,R+1); + + Ax[0]=p2[0]-p1[0]-R[1]*proj; + Ax[1]=p2[1]-p1[1]-R[5]*proj; + Ax[2]=p2[2]-p1[2]-R[9]*proj; +dNormalize3(Ax); +TEST(dDOT(p,Ax),sphereRadius+cylRadius,Ax[0],Ax[1],Ax[2],9); + + +Ax[0]=p[0]; +Ax[1]=p[1]; +Ax[2]=p[2]; +dNormalize3(Ax); + + dVector3 pa; + dReal sign, factor; + for (i=0; i<3; i++) pa[i] = p1[i]; + + cos1 = dDOT14(Ax,R+0); + cos3 = dDOT14(Ax,R+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) pa[i] += cos1 * cylRadius * R[i*4]; + sign = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hl * R[i*4+1]; + for (i=0; i<3; i++) pa[i] += cos3 * cylRadius * R[i*4+2]; + +Ax[0]=p2[0]-pa[0]; +Ax[1]=p2[1]-pa[1]; +Ax[2]=p2[2]-pa[2]; +dNormalize3(Ax); + + cos=dFabs(dDOT14(Ax,R+1)); + cos1=dDOT14(Ax,R+0); + cos3=dDOT14(Ax,R+2); + sin=sqrtf(cos1*cos1+cos3*cos3); +TEST(dDOT(p,Ax),sphereRadius+cylRadius*sin+hl*cos,Ax[0],Ax[1],Ax[2],14); + + +#undef TEST + + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + + normal[0] = normalC[0]; + normal[1] = normalC[1]; + normal[2] = normalC[2]; + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + // compute contact point(s) +contact->depth=-s; +contact->normal[0]=-normal[0]; +contact->normal[1]=-normal[1]; +contact->normal[2]=-normal[2]; +contact->g1=const_cast (o1); +contact->g2=const_cast (o2); +contact->pos[0]=p2[0]-normal[0]*sphereRadius; +contact->pos[1]=p2[1]-normal[1]*sphereRadius; +contact->pos[2]=p2[2]-normal[2]*sphereRadius; +return 1; +} + + + +int dCollideCylB (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dVector3 normal; + dReal depth; + int code; + dReal cylRadius,cylLength; + dVector3 boxSides; + dGeomCylinderGetParams(o1,&cylRadius,&cylLength); + dGeomBoxGetLengths(o2,boxSides); + int num = dCylBox(dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius,cylLength, + dGeomGetPosition(o2),dGeomGetRotation(o2),boxSides, + normal,&depth,&code,flags & NUMC_MASK,contact,skip); + for (int i=0; inormal[0] = -normal[0]; + CONTACT(contact,i*skip)->normal[1] = -normal[1]; + CONTACT(contact,i*skip)->normal[2] = -normal[2]; + CONTACT(contact,i*skip)->g1 = const_cast (o1); + CONTACT(contact,i*skip)->g2 = const_cast (o2); + } + return num; +} + +int dCollideCylCyl (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dVector3 normal; + dReal depth; + int code; +dReal cylRadius1,cylRadius2; +dReal cylLength1,cylLength2; +dGeomCylinderGetParams(o1,&cylRadius1,&cylLength1); +dGeomCylinderGetParams(o2,&cylRadius2,&cylLength2); +int num = dCylCyl (dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius1,cylLength1, + dGeomGetPosition(o2),dGeomGetRotation(o2),cylRadius2,cylLength2, + normal,&depth,&code,flags & NUMC_MASK,contact,skip); + + for (int i=0; inormal[0] = -normal[0]; + CONTACT(contact,i*skip)->normal[1] = -normal[1]; + CONTACT(contact,i*skip)->normal[2] = -normal[2]; + CONTACT(contact,i*skip)->g1 = const_cast (o1); + CONTACT(contact,i*skip)->g2 = const_cast (o2); + } + return num; +} + +struct dxPlane { + dReal p[4]; +}; + + +int dCollideCylPlane + ( + dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip){ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (dGeomGetClass(o1) == dCylinderClassUser); + dIASSERT (dGeomGetClass(o2) == dPlaneClass); + contact->g1 = const_cast (o1); + contact->g2 = const_cast (o2); + + unsigned int ret = 0; + + dReal radius; + dReal hlz; + dGeomCylinderGetParams(o1,&radius,&hlz); + hlz /= 2; + + const dReal *R = dGeomGetRotation(o1);// rotation of cylinder + const dReal* p = dGeomGetPosition(o1); + dVector4 n; // normal vector + dReal pp; + dGeomPlaneGetParams (o2, n); + pp=n[3]; + dReal cos1,sin1; + cos1=dFabs(dDOT14(n,R+1)); + +cos1=cos10 ? hlz*R[1]:-hlz*R[1]; + pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5]; + pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9]; + + + + contact->pos[0] = pos[0]; + contact->pos[1] = pos[1]; + contact->pos[2] = pos[2]; + contact->depth = outDepth; + ret=1; + +if(dFabs(Q2)>M_SQRT1_2){ + + CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0]; + CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4]; + CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8]; + CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1); + + if(CONTACT(contact,ret*skip)->depth>0.f) + ret++; + + + CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2]; + CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6]; + CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10]; + CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3); + + if(CONTACT(contact,ret*skip)->depth>0.f) ret++; +} else { + + CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]); + CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]); + CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]); + CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2); + + if(CONTACT(contact,ret*skip)->depth>0.f) ret++; +} + + + + for (unsigned int i=0; ig1 = const_cast (o1); + CONTACT(contact,i*skip)->g2 = const_cast (o2); + CONTACT(contact,i*skip)->normal[0] =n[0]; + CONTACT(contact,i*skip)->normal[1] =n[1]; + CONTACT(contact,i*skip)->normal[2] =n[2]; + } + return ret; +} + +int dCollideCylRay(dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) { + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (dGeomGetClass(o1) == dCylinderClassUser); + dIASSERT (dGeomGetClass(o2) == dRayClass); + contact->g1 = const_cast (o1); + contact->g2 = const_cast (o2); + dReal radius; + dReal lz; + dGeomCylinderGetParams(o1,&radius,&lz); + dReal lz2=lz*REAL(0.5); + const dReal *R = dGeomGetRotation(o1); // rotation of the cylinder + const dReal *p = dGeomGetPosition(o1); // position of the cylinder + dVector3 start,dir; + dGeomRayGet(o2,start,dir); // position and orientation of the ray + dReal length = dGeomRayGetLength(o2); + + // compute some useful info + dVector3 cs,q,r; + dReal C,k; + cs[0] = start[0] - p[0]; + cs[1] = start[1] - p[1]; + cs[2] = start[2] - p[2]; + k = dDOT41(R+1,cs); // position of ray start along cyl axis (Y) + q[0] = k*R[0*4+1] - cs[0]; + q[1] = k*R[1*4+1] - cs[1]; + q[2] = k*R[2*4+1] - cs[2]; + C = dDOT(q,q) - radius*radius; + // if C < 0 then ray start position within infinite extension of cylinder + // if ray start position is inside the cylinder + int inside_cyl=0; + if (C<0 && !(k<-lz2 || k>lz2)) inside_cyl=1; + // compute ray collision with infinite cylinder, except for the case where + // the ray is outside the cylinder but within the infinite cylinder + // (it that case the ray can only hit endcaps) + if (!inside_cyl && C < 0) { + // set k to cap position to check + if (k < 0) k = -lz2; else k = lz2; + } + else { + dReal uv = dDOT41(R+1,dir); + r[0] = uv*R[0*4+1] - dir[0]; + r[1] = uv*R[1*4+1] - dir[1]; + r[2] = uv*R[2*4+1] - dir[2]; + dReal A = dDOT(r,r); + dReal B = 2*dDOT(q,r); + k = B*B-4*A*C; + if (k < 0) { + // the ray does not intersect the infinite cylinder, but if the ray is + // inside and parallel to the cylinder axis it may intersect the end + // caps. set k to cap position to check. + if (!inside_cyl) return 0; + if (uv < 0) k = -lz2; else k = lz2; + } + else { + k = dSqrt(k); + A = dRecip (2*A); + dReal alpha = (-B-k)*A; + if (alpha < 0) { + alpha = (-B+k)*A; + if (alpha<0) return 0; + } + if (alpha>length) return 0; + // the ray intersects the infinite cylinder. check to see if the + // intersection point is between the caps + contact->pos[0] = start[0] + alpha*dir[0]; + contact->pos[1] = start[1] + alpha*dir[1]; + contact->pos[2] = start[2] + alpha*dir[2]; + q[0] = contact->pos[0] - p[0]; + q[1] = contact->pos[1] - p[1]; + q[2] = contact->pos[2] - p[2]; + k = dDOT14(q,R+1); + dReal nsign = inside_cyl ? -1 : 1; + if (k >= -lz2 && k <= lz2) { + contact->normal[0] = nsign * (contact->pos[0] - + (p[0] + k*R[0*4+1])); + contact->normal[1] = nsign * (contact->pos[1] - + (p[1] + k*R[1*4+1])); + contact->normal[2] = nsign * (contact->pos[2] - + (p[2] + k*R[2*4+1])); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; + } + // the infinite cylinder intersection point is not between the caps. + // set k to cap position to check. + if (k < 0) k = -lz2; else k = lz2; + } + } + // check for ray intersection with the caps. k must indicate the cap + // position to check + // perform a ray plan interesection + // R+1 is the plan normal + q[0] = start[0] - (p[0] + k*R[0*4+1]); + q[1] = start[1] - (p[1] + k*R[1*4+1]); + q[2] = start[2] - (p[2] + k*R[2*4+1]); + dReal alpha = -dDOT14(q,R+1); + dReal k2 = dDOT14(dir,R+1); + if (k2==0) return 0; // ray parallel to the plane + alpha/=k2; + if (alpha<0 || alpha>length) return 0; // too short + contact->pos[0]=start[0]+alpha*dir[0]; + contact->pos[1]=start[1]+alpha*dir[1]; + contact->pos[2]=start[2]+alpha*dir[2]; + dReal nsign = (k<0)?-1:1; + contact->normal[0]=nsign*R[0*4+1]; + contact->normal[1]=nsign*R[1*4+1]; + contact->normal[2]=nsign*R[2*4+1]; + contact->depth=alpha; + return 1; +} + +static dColliderFn * dCylinderColliderFn (int num) +{ + if (num == dBoxClass) return (dColliderFn *) &dCollideCylB; + else if (num == dSphereClass) return (dColliderFn *) &dCollideCylS; + else if (num == dCylinderClassUser) return (dColliderFn *) &dCollideCylCyl; + else if (num == dPlaneClass) return (dColliderFn *) &dCollideCylPlane; + else if (num == dRayClass) return (dColliderFn *) &dCollideCylRay; + return 0; +} + + +static void dCylinderAABB (dxGeom *geom, dReal aabb[6]) +{ + dReal radius,lz; + dGeomCylinderGetParams(geom,&radius,&lz); +const dReal* R= dGeomGetRotation(geom); +const dReal* pos= dGeomGetPosition(geom); + dReal xrange = dFabs (R[0] *radius) + + REAL(0.5) *dFabs (R[1] * lz) + dFabs (R[2] * radius); + + dReal yrange = dFabs (R[4] *radius) + + REAL(0.5) * dFabs (R[5] * lz) + dFabs (R[6] * radius); + + dReal zrange = dFabs (R[8] * radius) + + REAL(0.5) *dFabs (R[9] * lz) + dFabs (R[10] * radius); + + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + +dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz) +{ + dAASSERT (r > 0 && lz > 0); + if (dCylinderClassUser == -1) + { + dGeomClass c; + c.bytes = sizeof (dxCylinder); + c.collider = &dCylinderColliderFn; + c.aabb = &dCylinderAABB; + c.aabb_test = 0; + c.dtor = 0; + dCylinderClassUser=dCreateGeomClass (&c); + + } + + dGeomID g = dCreateGeom (dCylinderClassUser); + if (space) dSpaceAdd (space,g); + dxCylinder *c = (dxCylinder*) dGeomGetClassData(g); + + c->radius = r; + c->lz = lz; + return g; +} + + + +void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length) +{ + dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser,"argument not a cylinder"); + dAASSERT (radius > 0 && length > 0); + dxCylinder *c = (dxCylinder*) dGeomGetClassData(g); + c->radius = radius; + c->lz = length; +} + + + +void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length) +{ + dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser ,"argument not a cylinder"); + dxCylinder *c = (dxCylinder*) dGeomGetClassData(g); + *radius = c->radius; + *length = c->lz; +} + +/* +void dMassSetCylinder (dMass *m, dReal density, + dReal radius, dReal length) +{ + dAASSERT (m); + dMassSetZero (m); + dReal M = length*M_PI*radius*radius*density; + m->mass = M; + m->_I(0,0) = M/REAL(4.0) * (ly*ly + lz*lz); + m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz); + m->_I(2,2) = M/REAL(4.0) * (lx*lx + ly*ly); + +# ifndef dNODEBUG + checkMass (m); +# endif +} +*/ diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.h b/libraries/ode-0.9/contrib/dCylinder/dCylinder.h new file mode 100644 index 0000000..06e3a0b --- /dev/null +++ b/libraries/ode-0.9/contrib/dCylinder/dCylinder.h @@ -0,0 +1,14 @@ + +#ifndef dCylinder_h +#define dCylinder_h + +struct dxCylinder; +extern int dCylinderClassUser; + + +dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz); +void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length); + +void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length); +#endif //dCylinder_h + diff --git a/libraries/ode-0.9/contrib/dCylinder/readme.txt b/libraries/ode-0.9/contrib/dCylinder/readme.txt new file mode 100644 index 0000000..facd13e --- /dev/null +++ b/libraries/ode-0.9/contrib/dCylinder/readme.txt @@ -0,0 +1,62 @@ +readme.txt + +WARNING: THIS IS NOT VERY RELIABLE CODE. IT HAS BUGS. YOUR + SUCCESS MAY VARY. CONTRIBUTIONS OF FIXES/REWRITES ARE + WELCOME. + +/////////////////////////////////////////////////////////////////////// + +Cylinder geometry class. + +New in this version: + +Cylinder class implemented as User Geometry Class so it now can be +used with old and new ODE collision detection. + +Cylinder - Ray has been contributed by Olivier Michel. + +THE IDENTIFIER dCylinderClass HAS BEEN REPLACED BY dCylinderClassUser + +to avoid conflict with dCylinderClass in the enum definite in collision.h + +/////////////////////////////////////////////////////////////////////// +The dCylinder class includes the following collisions: + +Cylinder - Box +Cylinder - Cylinder +Cylinder - Sphere +Cylinder - Plane +Cylinder - Ray (contributed by Olivier Michel) + +Cylinder aligned along axis - Y when created. (Not like Capped +Cylinder which aligned along axis - Z). + +Interface is just the same as Capped Cylinder has. + +Use functions which have one "C" instead of double "C". + +to create: +dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length); + +to set params: +void dGeomCylinderSetParams (dGeomID cylinder, + dReal radius, dReal length); + + +to get params: +void dGeomCylinderGetParams (dGeomID cylinder, + dReal *radius, dReal *length); + +Return in radius and length the parameters of the given cylinder. + +Identification number of the class: + dCylinderClassUser + + I do not include a function that sets inertia tensor for cylinder. + One may use existing ODE functions dMassSetCappedCylinder or dMassSetBox. + To set exact tensor for cylinder use dMassSetParameters. + Remember cylinder aligned along axis - Y. + + /////////////////////////////////////////////////////////////////////////// + Konstantin Slipchenko + 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 new file mode 100644 index 0000000..f6caea8 --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/Include/dRay.h @@ -0,0 +1,15 @@ +#include "ode\ode.h" + +/* Class ID */ +extern int dRayClass; + +/* Creates a ray */ +dxGeom* dGeomCreateRay(dSpaceID space, dReal Length); + +/* Set/Get length */ +void dGeomRaySetLength(dxGeom* g, dReal Length); +dReal dGeomRayGetLength(dxGeom* g); + +/* Utility function to override the ray's pos + rot */ +void dGeomRaySet(dxGeom* g, dVector3 Origin, dVector3 Direction); +void 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 new file mode 100644 index 0000000..8997208 --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/README.txt @@ -0,0 +1,16 @@ +From: "Erwin de Vries" +To: +Subject: [ODE] dRay class +Date: Thu, 25 Jul 2002 13:05:28 +0200 + +Yesterday and today i've written a dRay class. It interacts with dPlane, +dSphere, dBox and dCCylinder. It does not generate full contact information. +It only generates the pos member. I dont think its useful to anyone to go +through hoops and find a reasonable normal and penetration depth, as i dont +think anyone will want to use it for dynamics. Just for CD. + +It should compile in single and double precision mode, and should be +platform independant. I hope. + +The next Tri-Collider release using Opcode 1.1 will also implement a ray +collision 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 new file mode 100644 index 0000000..faa8b14 --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp @@ -0,0 +1,1372 @@ +/************************************************************************* + + + * * + + + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + + + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + + + * * + + + * This library is free software; you can redistribute it and/or * + + + * modify it under the terms of EITHER: * + + + * (1) The GNU Lesser General Public License as published by the Free * + + + * Software Foundation; either version 2.1 of the License, or (at * + + + * your option) any later version. The text of the GNU Lesser * + + + * General Public License is included with this library in the * + + + * file LICENSE.TXT. * + + + * (2) The BSD-style license that is included with this library in * + + + * the file LICENSE-BSD.TXT. * + + + * * + + + * This library is distributed in the hope that it will be useful, * + + + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + + + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + + + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + + + * * + + + *************************************************************************/ + + + + + +#include + + +#include + + +#include + + + + + +#ifdef _MSC_VER + + +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints + + +#endif + + + + + +// select correct drawing functions + + + + + +#ifdef dDOUBLE + + +#define dsDrawBox dsDrawBoxD + + +#define dsDrawSphere dsDrawSphereD + + +#define dsDrawCylinder dsDrawCylinderD + + +#define dsDrawCappedCylinder dsDrawCappedCylinderD + + +#endif + + + + + + + + +// some constants + + + + + +#define NUM 20 // max number of objects + + +#define DENSITY (5.0) // density of all objects + + +#define GPB 3 // maximum number of geometries per body + + + + + + + + +// dynamics and collision objects + + + + + +struct MyObject { + + + dBodyID body; // the body + + + dGeomID geom[GPB]; // geometries representing this body + + +}; + + + + + +static int num=0; // number of objects in simulation + + +static int nextobj=0; // next object to recycle if num==NUM + + +static dWorldID world; + + +static dSpaceID space; + + +static MyObject obj[NUM]; + + +static dJointGroupID contactgroup; + + +static int selected = -1; // selected object + + + + + +static dGeomID* Rays; + + +static int RayCount; + + + + + +// this is called by dSpaceCollide when two objects in space are + + +// potentially colliding. + + + + + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) + + +{ + + + int i; + + + // if (o1->body && o2->body) return; + + + + + + // exit without doing anything if the two bodies are connected by a joint + + + dBodyID b1 = dGeomGetBody(o1); + + + dBodyID b2 = dGeomGetBody(o2); + + + if (b1 && b2 && dAreConnected (b1,b2)) return; + + + + + + dContact contact[32]; // up to 3 contacts per box + + + for (i=0; i<32; i++) { + + + contact[i].surface.mode = dContactBounce; //dContactMu2; + + + contact[i].surface.mu = dInfinity; + + + contact[i].surface.mu2 = 0; + + + contact[i].surface.bounce = 0.5; + + + contact[i].surface.bounce_vel = 0.1; + + + } + + + if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) { + + + dMatrix3 RI; + + + dRSetIdentity (RI); + + + const dReal ss[3] = {0.02,0.02,0.02}; + + + for (i=0; i= 'A' && c <= 'Z') return c - ('a'-'A'); + + + else return c; + + +} + + + + + + + + +// called when a key pressed + + + + + +static void command (int cmd) + + +{ + + + int i,j,k; + + + dReal sides[3]; + + + dMass m; + + + + + + cmd = locase (cmd); + + + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x') { + + + if (num < NUM) { + + + i = num; + + + num++; + + + } + + + else { + + + i = nextobj; + + + nextobj++; + + + if (nextobj >= num) nextobj = 0; + + + + + + // destroy the body and geoms for slot i + + + dBodyDestroy (obj[i].body); + + + for (k=0; k < GPB; k++) { + + + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + + + } + + + memset (&obj[i],0,sizeof(obj[i])); + + + } + + + + + + obj[i].body = dBodyCreate (world); + + + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + + + + + dBodySetPosition (obj[i].body, + + + dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1); + + + dMatrix3 R; + + + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + + + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + + + dBodySetRotation (obj[i].body,R); + + + dBodySetData (obj[i].body,(void*) i); + + + + + + if (cmd == 'b') { + + + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + + + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + + + } + + + else if (cmd == 'c') { + + + sides[0] *= 0.5; + + + dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); + + + obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]); + + + } + + + else if (cmd == 's') { + + + sides[0] *= 0.5; + + + dMassSetSphere (&m,DENSITY,sides[0]); + + + obj[i].geom[0] = dCreateSphere (space,sides[0]); + + + } + + + else if (cmd == 'x') { + + + dGeomID g2[GPB]; // encapsulated geometries + + + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + + + + + // start accumulating masses for the encapsulated geometries + + + dMass m2; + + + dMassSetZero (&m); + + + + + + // set random delta positions + + + for (j=0; j= num) selected = 0; + + + if (selected < 0) selected = 0; + + + } + + + else if (cmd == 'd' && selected >= 0 && selected < num) { + + + dBodyDisable (obj[selected].body); + + + } + + + else if (cmd == 'e' && selected >= 0 && selected < num) { + + + dBodyEnable (obj[selected].body); + + + } + + +} + + + + + + + + +// draw a geom + + + + + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R) + + +{ + + + if (!g) return; + + + if (!pos) pos = dGeomGetPosition (g); + + + if (!R) R = dGeomGetRotation (g); + + + + + + int type = dGeomGetClass (g); + + + if (type == dBoxClass) { + + + dVector3 sides; + + + dGeomBoxGetLengths (g,sides); + + + dsDrawBox (pos,R,sides); + + + } + + + else if (type == dSphereClass) { + + + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + + + } + + + else if (type == dCCylinderClass) { + + + dReal radius,length; + + + dGeomCCylinderGetParams (g,&radius,&length); + + + dsDrawCappedCylinder (pos,R,length,radius); + + + } + + + else if (type == dGeomTransformClass) { + + + dGeomID g2 = dGeomTransformGetGeom (g); + + + const dReal *pos2 = dGeomGetPosition (g2); + + + const dReal *R2 = dGeomGetRotation (g2); + + + dVector3 actual_pos; + + + dMatrix3 actual_R; + + + dMULTIPLY0_331 (actual_pos,R,pos2); + + + actual_pos[0] += pos[0]; + + + actual_pos[1] += pos[1]; + + + actual_pos[2] += pos[2]; + + + dMULTIPLY0_333 (actual_R,R,R2); + + + drawGeom (g2,actual_pos,actual_R); + + + } + + +} + + + + + + + + +// simulation loop + + + + + +static void simLoop (int pause) + + +{ + + + dsSetColor (0,0,2); + + + dSpaceCollide (space,0,&nearCallback); + + + if (!pause) dWorldStep (world,0.05); + + + + + + // remove all contact joints + + + dJointGroupEmpty (contactgroup); + + + + + + dsSetColor (1,1,0); + + + dsSetTexture (DS_WOOD); + + + for (int i=0; iLength = Length; +} + +dReal dGeomRayGetLength(dxGeom* g){ + return ((dxRay*)dGeomGetClassData(g))->Length; +} + +void dGeomRaySet(dxGeom* g, dVector3 Origin, dVector3 Direction){ + dGeomSetPosition(g, Origin[0], Origin[1], Origin[2]); + + dVector3 Up, Right; + dPlaneSpace(Direction, Up, Right); + + Origin[3] = Up[3] = Right[3] = REAL(0.0); + + dMatrix3 Rotation; + Rotation[0 * 4 + 0] = Right[0]; + Rotation[1 * 4 + 0] = Right[1]; + Rotation[2 * 4 + 0] = Right[2]; + Rotation[3 * 4 + 0] = Right[3]; + + Rotation[0 * 4 + 1] = Up[0]; + Rotation[1 * 4 + 1] = Up[1]; + Rotation[2 * 4 + 1] = Up[2]; + Rotation[3 * 4 + 1] = Up[3]; + + Rotation[0 * 4 + 2] = Direction[0]; + Rotation[1 * 4 + 2] = Direction[1]; + Rotation[2 * 4 + 2] = Direction[2]; + Rotation[3 * 4 + 2] = Direction[3]; + + dGeomSetRotation(g, Rotation); +} + +void dGeomRayGet(dxGeom* g, dVector3 Origin, dVector3 Direction){ + const dReal* Position = dGeomGetPosition(g); + Origin[0] = Position[0]; + Origin[1] = Position[1]; + Origin[2] = Position[2]; + Origin[3] = Position[3]; + + const dReal* Rotation = dGeomGetRotation(g); + Direction[0] = Rotation[0 * 4 + 2]; + Direction[1] = Rotation[1 * 4 + 2]; + Direction[2] = Rotation[2 * 4 + 2]; + Direction[3] = Rotation[3 * 4 + 2]; +} \ 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 new file mode 100644 index 0000000..d2a0d9c --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/dRay_Box.cpp @@ -0,0 +1,134 @@ +// Ripped from Magic Software + +#include "Include\dRay.h" +#include "dxRay.h" + +bool Clip(dReal Denom, dReal Numer, dReal& T0, dReal& T1){ + // Return value is 'true' if line segment intersects the current test + // plane. Otherwise 'false' is returned in which case the line segment + // is entirely clipped. + + if (Denom > REAL(0.0)){ + if (Numer > Denom * T1){ + return false; + } + + if (Numer > Denom * T0){ + T0 = Numer / Denom; + } + return true; + } + else if (Denom < REAL(0.0)){ + if (Numer > Denom * T0){ + return false; + } + + if (Numer > Denom * T1){ + T1 = Numer / Denom; + } + return true; + } + else return Numer <= REAL(0.0); +} + +bool FindIntersection(const dVector3 Origin, const dVector3 Direction, const dVector3 Extents, dReal& T0, dReal& T1){ + dReal SaveT0 = T0; + dReal SaveT1 = T1; + + bool NotEntirelyClipped = + Clip(+Direction[0], -Origin[0] - Extents[0], T0, T1) && + Clip(-Direction[0], +Origin[0] - Extents[0], T0, T1) && + Clip(+Direction[1], -Origin[1] - Extents[1], T0, T1) && + Clip(-Direction[1], +Origin[1] - Extents[1], T0, T1) && + Clip(+Direction[2], -Origin[2] - Extents[2], T0, T1) && + Clip(-Direction[2], +Origin[2] - Extents[2], T0, T1); + + return NotEntirelyClipped && (T0 != SaveT0 || T1 != SaveT1); +} + +int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){ + const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom); + dVector3 Extents; + dGeomBoxGetLengths(BoxGeom, Extents); + Extents[0] /= 2; + Extents[1] /= 2; + Extents[2] /= 2; + Extents[3] /= 2; + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + dReal Length = dGeomRayGetLength(RayGeom); + + dVector3 Diff; + Diff[0] = Origin[0] - Position[0]; + Diff[1] = Origin[1] - Position[1]; + Diff[2] = Origin[2] - Position[2]; + Diff[3] = Origin[3] - Position[3]; + + Direction[0] *= Length; + Direction[1] *= Length; + Direction[2] *= Length; + Direction[3] *= Length; + + dVector3 Rot[3]; + Decompose(Rotation, Rot); + + dVector3 TransOrigin; + TransOrigin[0] = dDOT(Diff, Rot[0]); + TransOrigin[1] = dDOT(Diff, Rot[1]); + TransOrigin[2] = dDOT(Diff, Rot[2]); + TransOrigin[3] = REAL(0.0); + + dVector3 TransDirection; + TransDirection[0] = dDOT(Direction, Rot[0]); + TransDirection[1] = dDOT(Direction, Rot[1]); + TransDirection[2] = dDOT(Direction, Rot[2]); + TransDirection[3] = REAL(0.0); + + dReal T[2]; + T[0] = 0.0f; + T[1] = dInfinity; + + bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]); + + if (Intersect){ + if (T[0] > REAL(0.0)){ + dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride); + Contact0->pos[0] = Origin[0] + T[0] * Direction[0]; + Contact0->pos[1] = Origin[1] + T[0] * Direction[1]; + Contact0->pos[2] = Origin[2] + T[0] * Direction[2]; + Contact0->pos[3] = Origin[3] + T[0] * Direction[3]; + //Contact0->normal = 0; + Contact0->depth = 0.0f; + Contact0->g1 = RayGeom; + Contact0->g2 = BoxGeom; + + dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride); + Contact1->pos[0] = Origin[0] + T[1] * Direction[0]; + Contact1->pos[1] = Origin[1] + T[1] * Direction[1]; + Contact1->pos[2] = Origin[2] + T[1] * Direction[2]; + Contact1->pos[3] = Origin[3] + T[1] * Direction[3]; + //Contact1->normal = 0; + Contact1->depth = 0.0f; + Contact1->g1 = RayGeom; + Contact1->g2 = BoxGeom; + + return 2; + } + else{ + dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride); + Contact->pos[0] = Origin[0] + T[1] * Direction[0]; + Contact->pos[1] = Origin[1] + T[1] * Direction[1]; + Contact->pos[2] = Origin[2] + T[1] * Direction[2]; + Contact->pos[3] = Origin[3] + T[1] * Direction[3]; + //Contact->normal = 0; + Contact->depth = 0.0f; + Contact->g1 = RayGeom; + Contact->g2 = BoxGeom; + + return 1; + } + } + else return 0; +} \ 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 new file mode 100644 index 0000000..b9ea0c0 --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp @@ -0,0 +1,199 @@ +// Ripped from Magic Software + +#include "Include\dRay.h" +#include "dxRay.h" + +int Find(const dVector3 Origin, dVector3 Direction, dReal Length, const dVector3 CCPos, const dMatrix3 CCRot, dReal CCRadius, dReal CCLength, dReal T[2]){ + dVector3 U, V, W; + Decompose(CCRot, U, V, W); + + dVector3 CCOrigin; + CCOrigin[0] = CCPos[0] - (W[0] * CCLength / 2); + CCOrigin[1] = CCPos[1] - (W[1] * CCLength / 2); + CCOrigin[2] = CCPos[2] - (W[2] * CCLength / 2); + CCOrigin[3] = CCPos[3] - (W[3] * CCLength / 2); + + dVector3 D; + D[0] = dDOT(U, Direction); + D[1] = dDOT(V, Direction); + D[2] = dDOT(W, Direction); + + dReal DMag = Length; + dReal InvDMag = REAL(1.0) / DMag; + + dVector3 Diff; + Diff[0] = Origin[0] - CCOrigin[0]; + Diff[1] = Origin[1] - CCOrigin[1]; + Diff[2] = Origin[2] - CCOrigin[2]; + Diff[3] = Origin[3] - CCOrigin[3]; + + dVector3 P; + P[0] = dDOT(U, Diff); + P[1] = dDOT(V, Diff); + P[2] = dDOT(W, Diff); + + dReal CCRadiusSq = CCRadius * CCRadius; + + dReal Epsilon = 1e-12f; + + if (dFabs(D[2]) >= REAL(1.0) - Epsilon){ // line is parallel to capsule axis + dReal Discr = CCRadiusSq - P[0] * P[0] - P[1] * P[1]; + + if (Discr >= REAL(0.0)){ + dReal Root = dSqrt(Discr); + T[0] = (-P[2] + Root) * InvDMag; + T[1] = (CCLength - P[2] + Root) * InvDMag; + return 2; + } + else return 0; + } + + // test intersection with infinite cylinder + dReal A = D[0] * D[0] + D[1] * D[1]; + dReal B = P[0] * D[0] + P[1] * D[1]; + dReal C = P[0] * P[0] + P[1] * P[1] - CCRadiusSq; + dReal Discr = B * B - A * C; + if (Discr < REAL(0.0)){ // line does not intersect infinite cylinder + return 0; + } + + int Count = 0; + + if (Discr > REAL(0.0)){ // line intersects infinite cylinder in two places + dReal Root = dSqrt(Discr); + dReal Inv = REAL(1.0) / A; + + dReal TTemp = (-B - Root) * Inv; + + dReal Tmp = P[2] + TTemp * D[2]; + if (REAL(0.0) <= Tmp && Tmp <= CCLength){ + T[Count++] = TTemp * InvDMag; + } + + + TTemp = (-B + Root) * Inv; + Tmp = P[2] + TTemp * D[2]; + if (REAL(0.0) <= Tmp && Tmp <= CCLength){ + T[Count++] = TTemp * InvDMag; + } + + if (Count == 2){ // line intersects capsule wall in two places + return 2; + } + } + else{ // line is tangent to infinite cylinder + dReal TTemp = -B / A; + dReal Tmp = P[2] + TTemp * D[2]; + if (REAL(0.0) <= Tmp && Tmp <= CCLength){ + T[0] = TTemp * InvDMag; + return 1; + } + } + + // test intersection with bottom hemisphere + // fA = 1 + B += P[2] * D[2]; + C += P[2] * P[2]; + Discr = B * B - C; + if (Discr > REAL(0.0)){ + dReal Root = dSqrt(Discr); + dReal TTemp = -B - Root; + dReal Tmp = P[2] + TTemp * D[2]; + if (Tmp <= REAL(0.0)){ + T[Count++] = TTemp * InvDMag; + if (Count == 2){ + return 2; + } + } + + TTemp = -B + Root; + Tmp = P[2] + TTemp * D[2]; + if (Tmp <= REAL(0.0)){ + T[Count++] = TTemp * InvDMag; + if (Count == 2){ + return 2; + } + } + } + else if (Discr == REAL(0.0)){ + dReal TTemp = -B; + dReal Tmp = P[2] + TTemp * D[2]; + if (Tmp <= REAL(0.0)){ + T[Count++] = TTemp * InvDMag; + if (Count == 2){ + return 2; + } + } + } + + // test intersection with top hemisphere + // fA = 1 + B -= D[2] * CCLength; + C += CCLength * (CCLength - REAL(2.0) * P[2]); + + Discr = B * B - C; + if (Discr > REAL(0.0)){ + dReal Root = dSqrt(Discr); + dReal TTemp = -B - Root; + dReal Tmp = P[2] + TTemp * D[2]; + if (Tmp >= CCLength){ + + T[Count++] = TTemp * InvDMag; + if (Count == 2){ + return 2; + } + } + + TTemp = -B + Root; + Tmp = P[2] + TTemp * D[2]; + if (Tmp >= CCLength){ + T[Count++] = TTemp * InvDMag; + if (Count == 2){ + return 2; + } + } + } + else if (Discr == REAL(0.0)){ + dReal TTemp = -B; + dReal Tmp = P[2] + TTemp * D[2]; + if (Tmp >= CCLength){ + T[Count++] = TTemp * InvDMag; + if (Count == 2){ + return 2; + } + } + } + return Count; +} + +int dCollideCCR(dxGeom* RayGeom, dxGeom* CCGeom, int Flags, dContactGeom* Contacts, int Stride){ + const dVector3& CCPos = *(const dVector3*)dGeomGetPosition(CCGeom); + const dMatrix3& CCRot = *(const dMatrix3*)dGeomGetRotation(CCGeom); + + dReal CCRadius, CCLength; + dGeomCCylinderGetParams(CCGeom, &CCRadius, &CCLength); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + dReal Length = dGeomRayGetLength(RayGeom); + + dReal T[2]; + int Count = Find(Origin, Direction, Length, CCPos, CCRot, CCRadius, CCLength, T); + int ContactCount = 0; + for (int i = 0; i < Count; i++){ + if (T[i] >= 0.0){ + dContactGeom* Contact = CONTACT(Flags, Contacts, ContactCount, Stride); + Contact->pos[0] = Origin[0] + T[i] * Direction[0] * Length; + Contact->pos[1] = Origin[1] + T[i] * Direction[1] * Length; + Contact->pos[2] = Origin[2] + T[i] * Direction[2] * Length; + Contact->pos[3] = Origin[3] + T[i] * Direction[3] * Length; + //Contact->normal = 0; + Contact->depth = 0.0f; + Contact->g1 = RayGeom; + Contact->g2 = CCGeom; + + ContactCount++; + } + } + return ContactCount; +} \ 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 new file mode 100644 index 0000000..cf03c5b --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp @@ -0,0 +1,35 @@ +// Ripped from Paul Bourke + +#include "Include\dRay.h" +#include "dxRay.h" + +int dCollidePR(dxGeom* RayGeom, dxGeom* PlaneGeom, int Flags, dContactGeom* Contact, int Stride){ + dVector3 Plane; + dGeomPlaneGetParams(PlaneGeom, Plane); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + dReal Length = dGeomRayGetLength(RayGeom); + + dReal Denom = Plane[0] * Direction[0] + Plane[1] * Direction[1] + Plane[2] * Direction[2]; + if (dFabs(Denom) < 0.00001f){ + return 0; // Ray never hits + } + + float T = -(Plane[3] + Plane[0] * Origin[0] + Plane[1] * Origin[1] + Plane[2] * Origin[2]) / Denom; + + if (T < 0 || T > Length){ + return 0; // Ray hits but not within boundaries + } + + Contact->pos[0] = Origin[0] + T * Direction[0]; + Contact->pos[1] = Origin[1] + T * Direction[1]; + Contact->pos[2] = Origin[2] + T * Direction[2]; + Contact->pos[3] = REAL(0.0); + //Contact->normal = 0; + Contact->depth = 0.0f; + Contact->g1 = RayGeom; + Contact->g2 = PlaneGeom; + return 1; +} \ 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 new file mode 100644 index 0000000..8e1ac39 --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp @@ -0,0 +1,95 @@ +// Ripped from Magic Software + +#include "Include\dRay.h" +#include "dxRay.h" + +int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){ + const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom); + dReal Radius = dGeomSphereGetRadius(SphereGeom); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + dReal Length = dGeomRayGetLength(RayGeom); + + dVector3 Diff; + Diff[0] = Origin[0] - Position[0]; + Diff[1] = Origin[1] - Position[1]; + Diff[2] = Origin[2] - Position[2]; + Diff[3] = Origin[3] - Position[3]; + + Direction[0] *= Length; + Direction[1] *= Length; + Direction[2] *= Length; + Direction[3] *= Length; + + dReal A = Length * Length; + dReal B = dDOT(Diff, Direction); + dReal C = dDOT(Diff, Diff) - (Radius * Radius); + + dReal Discr = B * B - A * C; + if (Discr < REAL(0.0)){ + return 0; + } + else if (Discr > REAL(0.0)){ + dReal T[2]; + dReal Root = dSqrt(Discr); + dReal InvA = REAL(1.0) / A; + T[0] = (-B - Root) * InvA; + T[1] = (-B + Root) * InvA; + + if (T[0] >= REAL(0.0)){ + dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride); + Contact0->pos[0] = Origin[0] + T[0] * Direction[0]; + Contact0->pos[1] = Origin[1] + T[0] * Direction[1]; + Contact0->pos[2] = Origin[2] + T[0] * Direction[2]; + Contact0->pos[3] = Origin[3] + T[0] * Direction[3]; + //Contact0->normal = 0; + Contact0->depth = 0.0f; + Contact0->g1 = RayGeom; + Contact0->g2 = SphereGeom; + + dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride); + Contact1->pos[0] = Origin[0] + T[1] * Direction[0]; + Contact1->pos[1] = Origin[1] + T[1] * Direction[1]; + Contact1->pos[2] = Origin[2] + T[1] * Direction[2]; + Contact1->pos[3] = Origin[3] + T[1] * Direction[3]; + //Contact1->normal = 0; + Contact1->depth = 0.0f; + Contact1->g1 = RayGeom; + Contact1->g2 = SphereGeom; + + return 2; + } + else if (T[1] >= REAL(0.0)){ + dContactGeom* Contact = CONTACT(Flags, Contacts, 1, Stride); + Contact->pos[0] = Origin[0] + T[1] * Direction[0]; + Contact->pos[1] = Origin[1] + T[1] * Direction[1]; + Contact->pos[2] = Origin[2] + T[1] * Direction[2]; + Contact->pos[3] = Origin[3] + T[1] * Direction[3]; + //Contact->normal = 0; + Contact->depth = 0.0f; + Contact->g1 = RayGeom; + Contact->g2 = SphereGeom; + + return 1; + } + else return 0; + } + else{ + dReal T; + T = -B / A; + if (T >= REAL(0.0)){ + dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride); + Contact->pos[0] = Origin[0] + T * Direction[0]; + Contact->pos[1] = Origin[1] + T * Direction[1]; + Contact->pos[2] = Origin[2] + T * Direction[2]; + Contact->pos[3] = Origin[3] + T * Direction[3]; + //Contact->normal = 0; + Contact->depth = 0.0f; + Contact->g1 = RayGeom; + Contact->g2 = SphereGeom; + return 1; + } + else return 0; + } +} \ 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 new file mode 100644 index 0000000..0fd1d2d --- /dev/null +++ b/libraries/ode-0.9/contrib/dRay/dxRay.h @@ -0,0 +1,32 @@ +struct dxRay{ + dReal Length; +}; + +inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){ + Right[0] = Matrix[0 * 4 + 0]; + Right[1] = Matrix[1 * 4 + 0]; + Right[2] = Matrix[2 * 4 + 0]; + Right[3] = Matrix[3 * 4 + 0]; + Up[0] = Matrix[0 * 4 + 1]; + Up[1] = Matrix[1 * 4 + 1]; + Up[2] = Matrix[2 * 4 + 1]; + Up[3] = Matrix[3 * 4 + 1]; + Direction[0] = Matrix[0 * 4 + 2]; + Direction[1] = Matrix[1 * 4 + 2]; + Direction[2] = Matrix[2 * 4 + 2]; + Direction[3] = Matrix[3 * 4 + 2]; +} + +inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){ + Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]); +} + +inline dContactGeom* CONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ + dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff)); + return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); +} + +int dCollidePR(dxGeom* RayGeom, dxGeom* PlaneGeom, int Flags, dContactGeom* Contacts, int Stride); +int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride); +int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride); +int dCollideCCR(dxGeom* RayGeom, dxGeom* CCylinderGeom, int Flags, dContactGeom* Contacts, int Stride); \ No newline at end of file -- cgit v1.1