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