Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

merged most of the changes from the branch into trunk, except for COLLADA, libxml and glut glitches.

Browse Source 
Still need to verify to make sure no unwanted renaming is introduced.
This commit is contained in:
ejcoumans
2006-09-27 20:43:51 +00:00
parent d1e9a885f3
commit eb23bb5c0c
263 changed files with 7528 additions and 6714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
Demos/BspDemo/BspConverter.cpp
View File

@@ -16,12 +16,12 @@ subject to the following restrictions:
#include "BspConverter.h" #include "BspConverter.h"
#include "BspLoader.h" #include "BspLoader.h"
#include "CcdPhysicsEnvironment.h" #include "CcdPhysicsEnvironment.h"
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
void BspConverter::convertBsp(BspLoader& bspLoader,float scaling) void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
{ {
{ {
SimdVector3 playerStart (0.f, 0.f, 100.f); btVector3 playerStart (0.f, 0.f, 100.f);
if (bspLoader.findVectorByName(&playerStart[0],"info_player_start")) if (bspLoader.findVectorByName(&playerStart[0],"info_player_start"))
{ {
@@ -53,7 +53,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
for (int b=0;b<leaf.numLeafBrushes;b++) for (int b=0;b<leaf.numLeafBrushes;b++)
{ {
std::vector<SimdVector3> planeEquations; std::vector<btVector3> planeEquations;
int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush+b]; int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush+b];
@@ -70,7 +70,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid]; BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum; int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid]; BSPPlane& plane = bspLoader.m_dplanes[planeid];
SimdVector3 planeEq; btVector3 planeEq;
planeEq.setValue( planeEq.setValue(
plane.normal[0], plane.normal[0],
plane.normal[1], plane.normal[1],
@@ -83,13 +83,13 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
if (isValidBrush) if (isValidBrush)
{ {
std::vector<SimdVector3> vertices; std::vector<btVector3> vertices;
getVerticesFromPlaneEquations(planeEquations,vertices); getVerticesFromPlaneEquations(planeEquations,vertices);
printf("getVerticesFromPlaneEquations returned %i\n",(int)vertices.size()); printf("getVerticesFromPlaneEquations returned %i\n",(int)vertices.size());
bool isEntity = false; bool isEntity = false;
SimdVector3 entityTarget(0.f,0.f,0.f); btVector3 entityTarget(0.f,0.f,0.f);
AddConvexVerticesCollider(vertices,isEntity,entityTarget); AddConvexVerticesCollider(vertices,isEntity,entityTarget);
} }
@@ -109,7 +109,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
const BSPEntity& entity = bspLoader.m_entities[i]; const BSPEntity& entity = bspLoader.m_entities[i];
const char* cl = bspLoader.ValueForKey(&entity,"classname"); const char* cl = bspLoader.ValueForKey(&entity,"classname");
if ( !strcmp( cl, "trigger_push" ) ) { if ( !strcmp( cl, "trigger_push" ) ) {
SimdVector3 targetLocation(0.f,0.f,0.f); btVector3 targetLocation(0.f,0.f,0.f);
cl = bspLoader.ValueForKey(&entity,"target"); cl = bspLoader.ValueForKey(&entity,"target");
if ( strcmp( cl, "" ) ) { if ( strcmp( cl, "" ) ) {
@@ -136,7 +136,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
const BSPModel& model = bspLoader.m_dmodels[modelnr]; const BSPModel& model = bspLoader.m_dmodels[modelnr];
for (int n=0;n<model.numBrushes;n++) for (int n=0;n<model.numBrushes;n++)
{ {
std::vector<SimdVector3> planeEquations; std::vector<btVector3> planeEquations;
bool isValidBrush = false; bool isValidBrush = false;
//convert brush //convert brush
@@ -148,7 +148,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid]; BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum; int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid]; BSPPlane& plane = bspLoader.m_dplanes[planeid];
SimdVector3 planeEq; btVector3 planeEq;
planeEq.setValue( planeEq.setValue(
plane.normal[0], plane.normal[0],
plane.normal[1], plane.normal[1],
@@ -160,7 +160,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
if (isValidBrush) if (isValidBrush)
{ {
std::vector<SimdVector3> vertices; std::vector<btVector3> vertices;
getVerticesFromPlaneEquations(planeEquations,vertices); getVerticesFromPlaneEquations(planeEquations,vertices);
bool isEntity=true; bool isEntity=true;
@@ -197,27 +197,27 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
void BspConverter::getVerticesFromPlaneEquations(const std::vector<SimdVector3>& planeEquations , std::vector<SimdVector3>& verticesOut ) void BspConverter::getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut )
{ {
const int numbrushes = planeEquations.size(); const int numbrushes = planeEquations.size();
// brute force: // brute force:
for (int i=0;i<numbrushes;i++) for (int i=0;i<numbrushes;i++)
{ {
const SimdVector3& N1 = planeEquations[i]; const btVector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++) for (int j=i+1;j<numbrushes;j++)
{ {
const SimdVector3& N2 = planeEquations[j]; const btVector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++) for (int k=j+1;k<numbrushes;k++)
{ {
const SimdVector3& N3 = planeEquations[k]; const btVector3& N3 = planeEquations[k];
SimdVector3 n2n3; n2n3 = N2.cross(N3); btVector3 n2n3; n2n3 = N2.cross(N3);
SimdVector3 n3n1; n3n1 = N3.cross(N1); btVector3 n3n1; n3n1 = N3.cross(N1);
SimdVector3 n1n2; n1n2 = N1.cross(N2); btVector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > 0.0001f ) && if ( ( n2n3.length2() > 0.0001f ) &&
( n3n1.length2() > 0.0001f ) && ( n3n1.length2() > 0.0001f ) &&
@@ -231,13 +231,13 @@ void BspConverter::getVerticesFromPlaneEquations(const std::vector<SimdVector3>&
float quotient = (N1.dot(n2n3)); float quotient = (N1.dot(n2n3));
if (SimdFabs(quotient) > 0.000001f) if (btFabs(quotient) > 0.000001f)
{ {
quotient = -1.f / quotient; quotient = -1.f / quotient;
n2n3 *= N1[3]; n2n3 *= N1[3];
n3n1 *= N2[3]; n3n1 *= N2[3];
n1n2 *= N3[3]; n1n2 *= N3[3];
SimdVector3 potentialVertex = n2n3; btVector3 potentialVertex = n2n3;
potentialVertex += n3n1; potentialVertex += n3n1;
potentialVertex += n1n2; potentialVertex += n1n2;
potentialVertex *= quotient; potentialVertex *= quotient;
@@ -257,12 +257,12 @@ void BspConverter::getVerticesFromPlaneEquations(const std::vector<SimdVector3>&
bool BspConverter::isInside(const std::vector<SimdVector3>& planeEquations, const SimdVector3& point, float margin) bool BspConverter::isInside(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin)
{ {
int numbrushes = planeEquations.size(); int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++) for (int i=0;i<numbrushes;i++)
{ {
const SimdVector3& N1 = planeEquations[i]; const btVector3& N1 = planeEquations[i];
float dist = float(N1.dot(point))+float(N1[3])-margin; float dist = float(N1.dot(point))+float(N1[3])-margin;
if (dist>0.f) if (dist>0.f)
{ {

8
Demos/BspDemo/BspConverter.h
View File

@@ -18,7 +18,7 @@ subject to the following restrictions:
class BspLoader; class BspLoader;
#include <vector> #include <vector>
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
///BspConverter turns a loaded bsp level into convex parts (vertices) ///BspConverter turns a loaded bsp level into convex parts (vertices)
class BspConverter class BspConverter
@@ -31,11 +31,11 @@ class BspConverter
} }
///Utility function to create vertices from a Quake Brush. Brute force but it works. ///Utility function to create vertices from a Quake Brush. Brute force but it works.
///Bit overkill to use QHull package ///Bit overkill to use QHull package
void getVerticesFromPlaneEquations(const std::vector<SimdVector3>& planeEquations , std::vector<SimdVector3>& verticesOut ); void getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut );
bool isInside(const std::vector<SimdVector3>& planeEquations, const SimdVector3& point, float margin); bool isInside(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin);
///this callback is called for each brush that succesfully converted into vertices ///this callback is called for each brush that succesfully converted into vertices
virtual void AddConvexVerticesCollider(std::vector<SimdVector3>& vertices, bool isEntity, const SimdVector3& entityTargetLocation) = 0; virtual void AddConvexVerticesCollider(std::vector<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation) = 0;
}; };

24
Demos/BspDemo/BspDemo.cpp
View File

@@ -18,8 +18,8 @@ subject to the following restrictions:
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/GenQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
@@ -60,7 +60,7 @@ public:
{ {
} }
virtual void AddConvexVerticesCollider(std::vector<SimdVector3>& vertices, bool isEntity, const SimdVector3& entityTargetLocation) virtual void AddConvexVerticesCollider(std::vector<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation)
{ {
///perhaps we can do something special with entities (isEntity) ///perhaps we can do something special with entities (isEntity)
///like adding a collision Triggering (as example) ///like adding a collision Triggering (as example)
@@ -69,12 +69,12 @@ public:
{ {
bool isDynamic = false; bool isDynamic = false;
float mass = 0.f; float mass = 0.f;
SimdTransform startTransform; btTransform startTransform;
//can use a shift //can use a shift
startTransform.setIdentity(); startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,0,-10.f)); startTransform.setOrigin(btVector3(0,0,-10.f));
//this create an internal copy of the vertices //this create an internal copy of the vertices
CollisionShape* shape = new ConvexHullShape(&vertices[0],vertices.size()); btCollisionShape* shape = new btConvexHullShape(&vertices[0],vertices.size());
m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape); m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape);
} }
@@ -128,15 +128,15 @@ void BspDemo::initPhysics(char* bspfilename)
{ {
m_cameraUp = SimdVector3(0,0,1); m_cameraUp = btVector3(0,0,1);
m_forwardAxis = 1; m_forwardAxis = 1;
///Setup a Physics Simulation Environment ///Setup a Physics Simulation Environment
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000); btVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000); btVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//BroadphaseInterface* broadphase = new SimpleBroadphase(); //BroadphaseInterface* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
m_physicsEnvironmentPtr->setDeactivationTime(2.f); m_physicsEnvironmentPtr->setDeactivationTime(2.f);
m_physicsEnvironmentPtr->setGravity(0,0,-10); m_physicsEnvironmentPtr->setGravity(0,0,-10);

6
Demos/BspDemo/BspLoader.cpp
View File

@@ -5,16 +5,16 @@ Copyright (C) 1999-2005 Id Software, Inc.
This file is part of Quake III Arena source code. This file is part of Quake III Arena source code.
Quake III Arena source code is free software; you can redistribute it Quake III Arena source code is free software; you can redistribute it
and/or modify it under the terms of the GNU General Public License as and/or modify it under the terms of the GNU bteral Public License as
published by the Free Software Foundation; either version 2 of the License, published by the Free Software Foundation; either version 2 of the License,
or (at your option) any later version. or (at your option) any later version.
Quake III Arena source code is distributed in the hope that it will be Quake III Arena source code is distributed in the hope that it will be
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU bteral Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU bteral Public License
along with Foobar; if not, write to the Free Software along with Foobar; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
=========================================================================== ===========================================================================

6
Demos/BspDemo/BspLoader.h
View File

@@ -5,16 +5,16 @@ Copyright (C) 1999-2005 Id Software, Inc.
This file is part of Quake III Arena source code. This file is part of Quake III Arena source code.
Quake III Arena source code is free software; you can redistribute it Quake III Arena source code is free software; you can redistribute it
and/or modify it under the terms of the GNU General Public License as and/or modify it under the terms of the GNU bteral Public License as
published by the Free Software Foundation; either version 2 of the License, published by the Free Software Foundation; either version 2 of the License,
or (at your option) any later version. or (at your option) any later version.
Quake III Arena source code is distributed in the hope that it will be Quake III Arena source code is distributed in the hope that it will be
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU bteral Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU bteral Public License
along with Foobar; if not, write to the Free Software along with Foobar; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
=========================================================================== ===========================================================================

122
Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
View File

@@ -13,28 +13,17 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
//#define USER_DEFINED_FRICTION_MODEL 1 //#define USER_DEFINED_FRICTION_MODEL 1
//#define PRINT_CONTACT_STATISTICS 1 //#define PRINT_CONTACT_STATISTICS 1
#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1 #define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
#include "CcdPhysicsEnvironment.h"
#include "ParallelPhysicsEnvironment.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "CcdPhysicsController.h" #include "LinearMath/btQuickprof.h"
#include "MyMotionState.h" #include "LinearMath/btIDebugDraw.h"
#include "ParallelIslandDispatcher.h"
#include "LinearMath/GenQuickprof.h"
#include "LinearMath/GenIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
#include "PHY_Pro.h" #include "PHY_Pro.h"
#include "BMF_Api.h" #include "BMF_Api.h"
#include <stdio.h> //printf debugging #include <stdio.h> //printf debugging
@@ -68,40 +57,35 @@ const int maxNumObjects = 32760;
int shapeIndex[maxNumObjects]; int shapeIndex[maxNumObjects];
#ifdef USE_PARALLEL_DISPATCHER
ParallelPhysicsEnvironment* m_physicsEnvironmentPtr = 0;
#else
CcdPhysicsEnvironment* m_physicsEnvironmentPtr = 0;
#endif
#define CUBE_HALF_EXTENTS 1 #define CUBE_HALF_EXTENTS 1
#define EXTRA_HEIGHT -20.f #define EXTRA_HEIGHT -20.f
//GL_LineSegmentShape shapeE(SimdPoint3(-50,0,0), //GL_LineSegmentShape shapeE(btPoint3(-50,0,0),
// SimdPoint3(50,0,0)); // btPoint3(50,0,0));
static const int numShapes = 4; static const int numShapes = 4;
CollisionShape* shapePtr[numShapes] = btCollisionShape* shapePtr[numShapes] =
{ {
///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate. ///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate.
///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346 ///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346
//#define USE_GROUND_PLANE 1 //#define USE_GROUND_PLANE 1
#ifdef USE_GROUND_PLANE #ifdef USE_GROUND_PLANE
new StaticPlaneShape(SimdVector3(0,1,0),10), new btStaticPlaneShape(btVector3(0,1,0),10),
#else #else
new BoxShape (SimdVector3(50,10,50)), new btBoxShape (btVector3(50,10,50)),
#endif #endif
new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)), new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
new SphereShape (CUBE_HALF_EXTENTS- 0.05f), new btSphereShape (CUBE_HALF_EXTENTS- 0.05f),
//new ConeShape(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS), //new btConeShape(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS),
//new BU_Simplex1to4(SimdPoint3(-1,-1,-1),SimdPoint3(1,-1,-1),SimdPoint3(-1,1,-1),SimdPoint3(0,0,1)), //new btBU_Simplex1to4(btPoint3(-1,-1,-1),btPoint3(1,-1,-1),btPoint3(-1,1,-1),btPoint3(0,0,1)),
//new EmptyShape(), //new btEmptyShape(),
new BoxShape (SimdVector3(0.4,1,0.8)) new btBoxShape (btVector3(0.4,1,0.8))
}; };
@@ -138,19 +122,17 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
{ {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
if (m_dynamicsWorld)
m_dynamicsWorld->stepSimulation(deltaTime);
if (m_physicsEnvironmentPtr)
m_physicsEnvironmentPtr->proceedDeltaTime(0.f,deltaTime);
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("render"); btProfiler::beginBlock("render");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
renderme(); renderme();
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("render"); btProfiler::endBlock("render");
#endif #endif
glFlush(); glFlush();
//some additional debugging info //some additional debugging info
@@ -171,12 +153,15 @@ void CcdPhysicsDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
/*
if (m_physicsEnvironmentPtr) if (m_physicsEnvironmentPtr)
{ {
m_physicsEnvironmentPtr->UpdateAabbs(deltaTime); m_physicsEnvironmentPtr->UpdateAabbs(deltaTime);
//draw contactpoints //draw contactpoints
m_physicsEnvironmentPtr->CallbackTriggers(); m_physicsEnvironmentPtr->CallbackTriggers();
} }
*/
renderme(); renderme();
@@ -192,7 +177,7 @@ void CcdPhysicsDemo::displayCallback(void) {
void CcdPhysicsDemo::clientResetScene() void CcdPhysicsDemo::clientResetScene()
{ {
/*
int i; int i;
int numObjects = m_physicsEnvironmentPtr->GetNumControllers(); int numObjects = m_physicsEnvironmentPtr->GetNumControllers();
@@ -203,7 +188,7 @@ void CcdPhysicsDemo::clientResetScene()
{ {
CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i); CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i);
if ((getDebugMode() & IDebugDraw::DBG_NoHelpText)) if ((getDebugMode() & btIDebugDraw::DBG_NoHelpText))
{ {
if (ctrl->GetRigidBody()->GetCollisionShape()->GetShapeType() != SPHERE_SHAPE_PROXYTYPE) if (ctrl->GetRigidBody()->GetCollisionShape()->GetShapeType() != SPHERE_SHAPE_PROXYTYPE)
{ {
@@ -213,7 +198,7 @@ void CcdPhysicsDemo::clientResetScene()
ctrl->GetRigidBody()->SetCollisionShape(shapePtr[1]); ctrl->GetRigidBody()->SetCollisionShape(shapePtr[1]);
} }
BroadphaseProxy* bpproxy = ctrl->GetRigidBody()->m_broadphaseHandle; btBroadphaseProxy* bpproxy = ctrl->GetRigidBody()->m_broadphaseHandle;
m_physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy); m_physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy);
} }
@@ -236,11 +221,13 @@ void CcdPhysicsDemo::clientResetScene()
ctrl->SetAngularVelocity(0,0,0,false); ctrl->SetAngularVelocity(0,0,0,false);
} }
} }
*/
} }
///User-defined friction model, the most simple friction model available: no friction ///User-defined friction model, the most simple friction model available: no friction
float myFrictionModel( RigidBody& body1, RigidBody& body2, ManifoldPoint& contactPoint, const ContactSolverInfo& solverInfo ) float myFrictionModel( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo )
{ {
//don't do any friction //don't do any friction
return 0.f; return 0.f;
@@ -249,32 +236,25 @@ float myFrictionModel( RigidBody& body1, RigidBody& body2, ManifoldPoint& contac
void CcdPhysicsDemo::initPhysics() void CcdPhysicsDemo::initPhysics()
{ {
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000); btVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000); btVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies); btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
// OverlappingPairCache* broadphase = new SimpleBroadphase; // btOverlappingPairCache* broadphase = new btSimpleBroadphase;
#ifdef REGISTER_CUSTOM_COLLISION_ALGORITHM #ifdef REGISTER_CUSTOM_COLLISION_ALGORITHM
dispatcher->RegisterCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new SphereSphereCollisionAlgorithm::CreateFunc); dispatcher->RegisterCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
#endif //REGISTER_CUSTOM_COLLISION_ALGORITHM #endif //REGISTER_CUSTOM_COLLISION_ALGORITHM
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
#ifdef USE_PARALLEL_DISPATCHER m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase);
#else
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
#endif
m_physicsEnvironmentPtr->setDeactivationTime(2.f);
m_physicsEnvironmentPtr->setGravity(0,-10,0);
m_physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
#ifdef USER_DEFINED_FRICTION_MODEL #ifdef USER_DEFINED_FRICTION_MODEL
SequentialImpulseConstraintSolver* solver = (SequentialImpulseConstraintSolver*) m_physicsEnvironmentPtr->GetConstraintSolver(); btSequentialImpulseConstraintSolver* solver = (btSequentialImpulseConstraintSolver*) m_physicsEnvironmentPtr->GetConstraintSolver();
//solver->SetContactSolverFunc(ContactSolverFunc func,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE); //solver->SetContactSolverFunc(ContactSolverFunc func,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE); solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1); solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1);
@@ -285,7 +265,7 @@ void CcdPhysicsDemo::initPhysics()
int i; int i;
SimdTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
@@ -301,26 +281,26 @@ void CcdPhysicsDemo::initPhysics()
if (useCompound) if (useCompound)
{ {
CompoundShape* compoundShape = new CompoundShape(); btCompoundShape* compoundShape = new btCompoundShape();
CollisionShape* oldShape = shapePtr[1]; btCollisionShape* oldShape = shapePtr[1];
shapePtr[1] = compoundShape; shapePtr[1] = compoundShape;
SimdTransform ident; btTransform ident;
ident.setIdentity(); ident.setIdentity();
ident.setOrigin(SimdVector3(0,0,0)); ident.setOrigin(btVector3(0,0,0));
compoundShape->AddChildShape(ident,oldShape);// compoundShape->AddChildShape(ident,oldShape);//
ident.setOrigin(SimdVector3(0,0,2)); ident.setOrigin(btVector3(0,0,2));
compoundShape->AddChildShape(ident,new SphereShape(0.9));// compoundShape->AddChildShape(ident,new btSphereShape(0.9));//
} }
for (i=0;i<gNumObjects;i++) for (i=0;i<gNumObjects;i++)
{ {
CollisionShape* shape = shapePtr[shapeIndex[i]]; btCollisionShape* shape = shapePtr[shapeIndex[i]];
shape->SetMargin(0.05f); shape->SetMargin(0.05f);
bool isDyna = i>0; bool isDyna = i>0;
SimdTransform trans; btTransform trans;
trans.setIdentity(); trans.setIdentity();
if (i>0) if (i>0)
@@ -338,13 +318,13 @@ void CcdPhysicsDemo::initPhysics()
row2 |=1; row2 |=1;
} }
SimdVector3 pos(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS, btVector3 pos(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS,
row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0); row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0);
trans.setOrigin(pos); trans.setOrigin(pos);
} else } else
{ {
trans.setOrigin(SimdVector3(0,-30,0)); trans.setOrigin(btVector3(0,-30,0));
} }
float mass = 1.f; float mass = 1.f;
@@ -352,13 +332,16 @@ void CcdPhysicsDemo::initPhysics()
if (!isDyna) if (!isDyna)
mass = 0.f; mass = 0.f;
CcdPhysicsController* ctrl = LocalCreatePhysicsObject(isDyna,mass,trans,shape); btRigidBody* body = LocalCreateRigidBody(isDyna,mass,trans,shape);
m_dynamicsWorld->AddCollisionObject(body);
// Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS // Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
ctrl->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS; body->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
//Experimental: better estimation of CCD Time of Impact: //Experimental: better estimation of CCD Time of Impact:
ctrl->GetRigidBody()->m_ccdSweptShereRadius = 0.2*CUBE_HALF_EXTENTS; body->m_ccdSweptShereRadius = 0.2*CUBE_HALF_EXTENTS;
#ifdef USER_DEFINED_FRICTION_MODEL #ifdef USER_DEFINED_FRICTION_MODEL
///Advanced use: override the friction solver ///Advanced use: override the friction solver
ctrl->GetRigidBody()->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1; ctrl->GetRigidBody()->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1;
@@ -369,7 +352,6 @@ void CcdPhysicsDemo::initPhysics()
clientResetScene(); clientResetScene();
m_physicsEnvironmentPtr->SyncMotionStates(0.f);
} }

6
Demos/CcdPhysicsDemo/MyMotionState.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/ */
#include "MyMotionState.h" #include "MyMotionState.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
MyMotionState::MyMotionState() MyMotionState::MyMotionState()
{ {
@@ -51,13 +51,13 @@ void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& q
void MyMotionState::setWorldPosition(float posX,float posY,float posZ) void MyMotionState::setWorldPosition(float posX,float posY,float posZ)
{ {
SimdPoint3 pos(posX,posY,posZ); btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos ); m_worldTransform.setOrigin( pos );
} }
void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
{ {
SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
m_worldTransform.setRotation( orn ); m_worldTransform.setRotation( orn );
} }

4
Demos/CcdPhysicsDemo/MyMotionState.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#define MY_MOTIONSTATE_H #define MY_MOTIONSTATE_H
#include "PHY_IMotionState.h" #include "PHY_IMotionState.h"
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
class MyMotionState : public PHY_IMotionState class MyMotionState : public PHY_IMotionState
@@ -37,7 +37,7 @@ class MyMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations(); virtual void calculateWorldTransformations();
SimdTransform m_worldTransform; btTransform m_worldTransform;
}; };

134
Demos/ColladaDemo/ColladaConverter.cpp
View File

@@ -37,7 +37,7 @@ char* getLastFileName();
char* fixFileName(const char* lpCmdLine); char* fixFileName(const char* lpCmdLine);
//todo: sort out this domInstance_rigid_bodyRef forward definition, put it in the headerfile and make it virtual (make code more re-usable) //todo: sort out this domInstance_rigid_bodyRef forward definition, put it in the headerfile and make it virtual (make code more re-usable)
struct RigidBodyInput struct btRigidBodyInput
{ {
domInstance_rigid_bodyRef m_instanceRigidBodyRef; domInstance_rigid_bodyRef m_instanceRigidBodyRef;
domRigid_bodyRef m_rigidBodyRef2; domRigid_bodyRef m_rigidBodyRef2;
@@ -52,12 +52,12 @@ struct ConstraintInput
}; };
struct RigidBodyOutput struct btRigidBodyOutput
{ {
float m_mass; float m_mass;
bool m_isDynamics; bool m_isDynamics;
CollisionShape* m_colShape; btCollisionShape* m_colShape;
CompoundShape* m_compoundShape; btCompoundShape* m_compoundShape;
}; };
@@ -66,13 +66,13 @@ domMatrix_Array emptyMatrixArray;
domNodeRef m_colladadomNodes[COLLADA_CONVERTER_MAX_NUM_OBJECTS]; domNodeRef m_colladadomNodes[COLLADA_CONVERTER_MAX_NUM_OBJECTS];
SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray, btTransform GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
domRotate_Array& rotateArray, domRotate_Array& rotateArray,
domTranslate_Array& translateArray domTranslate_Array& translateArray
) )
{ {
SimdTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
unsigned int i; unsigned int i;
@@ -81,9 +81,9 @@ SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
{ {
domMatrixRef matrixRef = matrixArray[i]; domMatrixRef matrixRef = matrixArray[i];
domFloat4x4 fl16 = matrixRef->getValue(); domFloat4x4 fl16 = matrixRef->getValue();
SimdVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11)); btVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11));
startTransform.setOrigin(origin); startTransform.setOrigin(origin);
SimdMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2), btMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2),
fl16.get(4),fl16.get(5),fl16.get(6), fl16.get(4),fl16.get(5),fl16.get(6),
fl16.get(8),fl16.get(9),fl16.get(10)); fl16.get(8),fl16.get(9),fl16.get(10));
startTransform.setBasis(basis); startTransform.setBasis(basis);
@@ -94,15 +94,15 @@ SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
domRotateRef rotateRef = rotateArray[i]; domRotateRef rotateRef = rotateArray[i];
domFloat4 fl4 = rotateRef->getValue(); domFloat4 fl4 = rotateRef->getValue();
float angleRad = SIMD_RADS_PER_DEG*fl4.get(3); float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad); btQuaternion rotQuat(btVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat); startTransform.getBasis() = startTransform.getBasis() * btMatrix3x3(rotQuat);
} }
for (i=0;i<translateArray.getCount();i++) for (i=0;i<translateArray.getCount();i++)
{ {
domTranslateRef translateRef = translateArray[i]; domTranslateRef translateRef = translateArray[i];
domFloat3 fl3 = translateRef->getValue(); domFloat3 fl3 = translateRef->getValue();
startTransform.getOrigin() += SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2)); startTransform.getOrigin() += btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
} }
return startTransform; return startTransform;
} }
@@ -185,20 +185,20 @@ bool ColladaConverter::convert()
printf(" X is Up Data and Hiearchies must be converted!\n" ); printf(" X is Up Data and Hiearchies must be converted!\n" );
printf(" Conversion to X axis Up isn't currently supported!\n" ); printf(" Conversion to X axis Up isn't currently supported!\n" );
printf(" COLLADA_RT defaulting to Y Up \n" ); printf(" COLLADA_RT defaulting to Y Up \n" );
SetGravity(SimdVector3(-10,0,0)); SetGravity(btVector3(-10,0,0));
SetCameraInfo(SimdVector3(1,0,0),1); SetCameraInfo(btVector3(1,0,0),1);
break; break;
case UPAXISTYPE_Y_UP: case UPAXISTYPE_Y_UP:
printf(" Y Axis is Up for this file \n" ); printf(" Y Axis is Up for this file \n" );
printf(" COLLADA_RT set to Y Up \n" ); printf(" COLLADA_RT set to Y Up \n" );
SetGravity(SimdVector3(0,-10,0)); SetGravity(btVector3(0,-10,0));
SetCameraInfo(SimdVector3(0,1,0),0); SetCameraInfo(btVector3(0,1,0),0);
break; break;
case UPAXISTYPE_Z_UP: case UPAXISTYPE_Z_UP:
printf(" Z Axis is Up for this file \n" ); printf(" Z Axis is Up for this file \n" );
printf(" All Geometry and Hiearchies must be converted!\n" ); printf(" All Geometry and Hiearchies must be converted!\n" );
SetGravity(SimdVector3(0,0,-10)); SetGravity(btVector3(0,0,-10));
break; break;
default: default:
@@ -294,7 +294,7 @@ bool ColladaConverter::convert()
const domFloat3 grav = physicsSceneRef->getTechnique_common()->getGravity()->getValue(); const domFloat3 grav = physicsSceneRef->getTechnique_common()->getGravity()->getValue();
printf("gravity set to %f,%f,%f\n",grav.get(0),grav.get(1),grav.get(2)); printf("gravity set to %f,%f,%f\n",grav.get(0),grav.get(1),grav.get(2));
SetGravity(SimdVector3(grav.get(0),grav.get(1),grav.get(2))); SetGravity(btVector3(grav.get(0),grav.get(1),grav.get(2)));
} }
} }
@@ -324,8 +324,8 @@ bool ColladaConverter::convert()
float mass = 1.f; float mass = 1.f;
bool isDynamics = true; bool isDynamics = true;
CollisionShape* colShape = 0; btCollisionShape* colShape = 0;
CompoundShape* compoundShape = 0; btCompoundShape* compoundShape = 0;
xsNCName bodyName = instRigidbodyRef->getBody(); xsNCName bodyName = instRigidbodyRef->getBody();
@@ -355,11 +355,11 @@ bool ColladaConverter::convert()
{ {
RigidBodyOutput output; btRigidBodyOutput output;
output.m_colShape = colShape; output.m_colShape = colShape;
output.m_compoundShape = compoundShape; output.m_compoundShape = compoundShape;
RigidBodyInput rbInput; btRigidBodyInput rbInput;
rbInput.m_rigidBodyRef2 = rigidBodyRef; rbInput.m_rigidBodyRef2 = rigidBodyRef;
rbInput.m_instanceRigidBodyRef = instRigidbodyRef; rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
ConvertRigidBodyRef( rbInput , output ); ConvertRigidBodyRef( rbInput , output );
@@ -380,7 +380,7 @@ bool ColladaConverter::convert()
if (colShape) if (colShape)
{ {
RigidBodyInput input; btRigidBodyInput input;
input.m_instanceRigidBodyRef = instRigidbodyRef; input.m_instanceRigidBodyRef = instRigidbodyRef;
input.m_rigidBodyRef2 = 0; input.m_rigidBodyRef2 = 0;
input.m_bodyName = (char*)bodyName; input.m_bodyName = (char*)bodyName;
@@ -399,8 +399,8 @@ bool ColladaConverter::convert()
float mass = 1.f; float mass = 1.f;
bool isDynamics = true; bool isDynamics = true;
CollisionShape* colShape = 0; btCollisionShape* colShape = 0;
CompoundShape* compoundShape = 0; btCompoundShape* compoundShape = 0;
xsNCName bodyName = instRigidbodyRef->getBody(); xsNCName bodyName = instRigidbodyRef->getBody();
@@ -430,11 +430,11 @@ bool ColladaConverter::convert()
{ {
RigidBodyOutput output; btRigidBodyOutput output;
output.m_colShape = colShape; output.m_colShape = colShape;
output.m_compoundShape = compoundShape; output.m_compoundShape = compoundShape;
RigidBodyInput rbInput; btRigidBodyInput rbInput;
rbInput.m_rigidBodyRef2 = rigidBodyRef; rbInput.m_rigidBodyRef2 = rigidBodyRef;
rbInput.m_instanceRigidBodyRef = instRigidbodyRef; rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
ConvertRigidBodyRef( rbInput , output ); ConvertRigidBodyRef( rbInput , output );
@@ -455,7 +455,7 @@ bool ColladaConverter::convert()
if (colShape) if (colShape)
{ {
RigidBodyInput input; btRigidBodyInput input;
input.m_instanceRigidBodyRef = instRigidbodyRef; input.m_instanceRigidBodyRef = instRigidbodyRef;
input.m_rigidBodyRef2 = 0; input.m_rigidBodyRef2 = 0;
input.m_bodyName = (char*)bodyName; input.m_bodyName = (char*)bodyName;
@@ -560,31 +560,31 @@ void ColladaConverter::PrepareConstraints(ConstraintInput& input)
const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common(); const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common();
domFloat3 flMin = commonRef->getLimits()->getLinear()->getMin()->getValue(); domFloat3 flMin = commonRef->getLimits()->getLinear()->getMin()->getValue();
SimdVector3 minLinearLimit(flMin.get(0),flMin.get(1),flMin.get(2)); btVector3 minLinearLimit(flMin.get(0),flMin.get(1),flMin.get(2));
domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue(); domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue();
SimdVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2)); btVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2));
domFloat3 coneMinLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMin()->getValue(); domFloat3 coneMinLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMin()->getValue();
SimdVector3 angularMin(coneMinLimit.get(0),coneMinLimit.get(1),coneMinLimit.get(2)); btVector3 angularMin(coneMinLimit.get(0),coneMinLimit.get(1),coneMinLimit.get(2));
domFloat3 coneMaxLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMax()->getValue(); domFloat3 coneMaxLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMax()->getValue();
SimdVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2)); btVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2));
{ {
int constraintId; int constraintId;
SimdTransform attachFrameRef0; btTransform attachFrameRef0;
attachFrameRef0 = attachFrameRef0 =
GetSimdTransformFromCOLLADA_DOM GetbtTransformFromCOLLADA_DOM
( (
emptyMatrixArray, emptyMatrixArray,
attachRefBody->getRotate_array(), attachRefBody->getRotate_array(),
attachRefBody->getTranslate_array()); attachRefBody->getTranslate_array());
SimdTransform attachFrameOther; btTransform attachFrameOther;
attachFrameOther = attachFrameOther =
GetSimdTransformFromCOLLADA_DOM GetbtTransformFromCOLLADA_DOM
( (
emptyMatrixArray, emptyMatrixArray,
attachBody1->getRotate_array(), attachBody1->getRotate_array(),
@@ -609,10 +609,10 @@ void ColladaConverter::PrepareConstraints(ConstraintInput& input)
//limited means upper > lower //limited means upper > lower
//limitIndex: first 3 are linear, next 3 are angular //limitIndex: first 3 are linear, next 3 are angular
SimdVector3 linearLowerLimits = minLinearLimit; btVector3 linearLowerLimits = minLinearLimit;
SimdVector3 linearUpperLimits = maxLinearLimit; btVector3 linearUpperLimits = maxLinearLimit;
SimdVector3 angularLowerLimits = angularMin; btVector3 angularLowerLimits = angularMin;
SimdVector3 angularUpperLimits = angularMax; btVector3 angularUpperLimits = angularMax;
{ {
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
@@ -665,11 +665,11 @@ void ColladaConverter::PrepareConstraints(ConstraintInput& input)
} }
void ColladaConverter::PreparePhysicsObject(struct RigidBodyInput& input, bool isDynamics, float mass,CollisionShape* colShape) void ColladaConverter::PreparePhysicsObject(struct btRigidBodyInput& input, bool isDynamics, float mass,btCollisionShape* colShape)
{ {
SimdTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
SimdVector3 startScale(1.f,1.f,1.f); btVector3 startScale(1.f,1.f,1.f);
//The 'target' points to a graphics element/node, which contains the start (world) transform //The 'target' points to a graphics element/node, which contains the start (world) transform
daeElementRef elem = input.m_instanceRigidBodyRef->getTarget().getElement(); daeElementRef elem = input.m_instanceRigidBodyRef->getTarget().getElement();
@@ -681,7 +681,7 @@ void ColladaConverter::PreparePhysicsObject(struct RigidBodyInput& input, bool i
//find transform of the node that this rigidbody maps to //find transform of the node that this rigidbody maps to
startTransform = GetSimdTransformFromCOLLADA_DOM( startTransform = GetbtTransformFromCOLLADA_DOM(
node->getMatrix_array(), node->getMatrix_array(),
node->getRotate_array(), node->getRotate_array(),
node->getTranslate_array() node->getTranslate_array()
@@ -692,7 +692,7 @@ void ColladaConverter::PreparePhysicsObject(struct RigidBodyInput& input, bool i
{ {
domScaleRef scaleRef = node->getScale_array()[i]; domScaleRef scaleRef = node->getScale_array()[i];
domFloat3 fl3 = scaleRef->getValue(); domFloat3 fl3 = scaleRef->getValue();
startScale = SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2)); startScale = btVector3(fl3.get(0),fl3.get(1),fl3.get(2));
} }
} }
@@ -764,15 +764,15 @@ bool ColladaConverter::saveAs(const char* filename)
} }
{ {
SimdQuaternion quat = m_physObjects[i]->GetRigidBody()->getCenterOfMassTransform().getRotation(); btQuaternion quat = m_physObjects[i]->GetRigidBody()->getCenterOfMassTransform().getRotation();
SimdVector3 axis(quat.getX(),quat.getY(),quat.getZ()); btVector3 axis(quat.getX(),quat.getY(),quat.getZ());
axis[3] = 0.f; axis[3] = 0.f;
//check for axis length //check for axis length
SimdScalar len = axis.length2(); btScalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON) if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = SimdVector3(1.f,0.f,0.f); axis = btVector3(1.f,0.f,0.f);
else else
axis /= SimdSqrt(len); axis /= btSqrt(len);
m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(0,axis[0]); m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(0,axis[0]);
m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(1,axis[1]); m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(1,axis[1]);
m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(2,axis[2]); m_colladadomNodes[i]->getRotate_array().get(0)->getValue().set(2,axis[2]);
@@ -886,7 +886,7 @@ char* fixFileName(const char* lpCmdLine)
} }
void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOutput& rbOutput) void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBodyOutput& rbOutput)
{ {
const domRigid_body::domTechnique_commonRef techniqueRef = rbInput.m_rigidBodyRef2->getTechnique_common(); const domRigid_body::domTechnique_commonRef techniqueRef = rbInput.m_rigidBodyRef2->getTechnique_common();
@@ -913,9 +913,9 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
if (planeRef->getEquation()) if (planeRef->getEquation())
{ {
const domFloat4 planeEq = planeRef->getEquation()->getValue(); const domFloat4 planeEq = planeRef->getEquation()->getValue();
SimdVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2)); btVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2));
SimdScalar planeConstant = planeEq.get(3); btScalar planeConstant = planeEq.get(3);
rbOutput.m_colShape = new StaticPlaneShape(planeNormal,planeConstant); rbOutput.m_colShape = new btStaticPlaneShape(planeNormal,planeConstant);
} }
} }
@@ -928,14 +928,14 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
float x = halfExtents.get(0); float x = halfExtents.get(0);
float y = halfExtents.get(1); float y = halfExtents.get(1);
float z = halfExtents.get(2); float z = halfExtents.get(2);
rbOutput.m_colShape = new BoxShape(SimdVector3(x,y,z)); rbOutput.m_colShape = new btBoxShape(btVector3(x,y,z));
} }
if (shapeRef->getSphere()) if (shapeRef->getSphere())
{ {
domSphereRef sphereRef = shapeRef->getSphere(); domSphereRef sphereRef = shapeRef->getSphere();
domSphere::domRadiusRef radiusRef = sphereRef->getRadius(); domSphere::domRadiusRef radiusRef = sphereRef->getRadius();
domFloat radius = radiusRef->getValue(); domFloat radius = radiusRef->getValue();
rbOutput.m_colShape = new SphereShape(radius); rbOutput.m_colShape = new btSphereShape(radius);
} }
if (shapeRef->getCylinder()) if (shapeRef->getCylinder())
@@ -946,7 +946,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
domFloat radius0 = radius2.get(0); domFloat radius0 = radius2.get(0);
//Cylinder around the local Y axis //Cylinder around the local Y axis
rbOutput.m_colShape = new CylinderShape(SimdVector3(radius0,height,radius0)); rbOutput.m_colShape = new btCylinderShape(btVector3(radius0,height,radius0));
} }
@@ -960,7 +960,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
{ {
const domMeshRef meshRef = geom->getMesh(); const domMeshRef meshRef = geom->getMesh();
TriangleMesh* trimesh = new TriangleMesh(); btTriangleMesh* trimesh = new btTriangleMesh();
for (unsigned int tg = 0;tg<meshRef->getTriangles_array().getCount();tg++) for (unsigned int tg = 0;tg<meshRef->getTriangles_array().getCount();tg++)
@@ -969,7 +969,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
domTrianglesRef triRef = meshRef->getTriangles_array()[tg]; domTrianglesRef triRef = meshRef->getTriangles_array()[tg];
const domPRef pRef = triRef->getP(); const domPRef pRef = triRef->getP();
IndexedMesh meshPart; btIndexedMesh meshPart;
meshPart.m_triangleIndexStride=0; meshPart.m_triangleIndexStride=0;
@@ -1024,7 +1024,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
for (;t<meshPart.m_numTriangles;t++) for (;t<meshPart.m_numTriangles;t++)
{ {
SimdVector3 verts[3]; btVector3 verts[3];
int index0; int index0;
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
@@ -1055,11 +1055,11 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
if (rbOutput.m_isDynamics) if (rbOutput.m_isDynamics)
{ {
printf("moving concave <mesh> not supported, transformed into convex\n"); printf("moving concave <mesh> not supported, transformed into convex\n");
rbOutput.m_colShape = new ConvexTriangleMeshShape(trimesh); rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
} else } else
{ {
printf("static concave triangle <mesh> added\n"); printf("static concave triangle <mesh> added\n");
rbOutput.m_colShape = new TriangleMeshShape(trimesh); rbOutput.m_colShape = new btTriangleMeshShape(trimesh);
} }
} }
@@ -1086,7 +1086,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
ConvexHullShape* convexHullShape = new ConvexHullShape(0,0); btConvexHullShape* convexHullShape = new btConvexHullShape(0,0);
//it is quite a trick to get to the vertices, using Collada. //it is quite a trick to get to the vertices, using Collada.
//we are not there yet... //we are not there yet...
@@ -1144,7 +1144,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
domFloat fl2 = listFloats.get(k+2); domFloat fl2 = listFloats.get(k+2);
//printf("float %f %f %f\n",fl0,fl1,fl2); //printf("float %f %f %f\n",fl0,fl1,fl2);
convexHullShape->AddPoint(SimdPoint3(fl0,fl1,fl2)); convexHullShape->AddPoint(btPoint3(fl0,fl1,fl2));
} }
} }
@@ -1192,7 +1192,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
domFloat fl2 = listFloats.get(k+2); domFloat fl2 = listFloats.get(k+2);
//printf("float %f %f %f\n",fl0,fl1,fl2); //printf("float %f %f %f\n",fl0,fl1,fl2);
convexHullShape->AddPoint(SimdPoint3(fl0,fl1,fl2)); convexHullShape->AddPoint(btPoint3(fl0,fl1,fl2));
} }
} }
@@ -1237,14 +1237,14 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
if (!rbOutput.m_compoundShape) if (!rbOutput.m_compoundShape)
{ {
rbOutput.m_compoundShape = new CompoundShape(); rbOutput.m_compoundShape = new btCompoundShape();
} }
SimdTransform localTransform; btTransform localTransform;
localTransform.setIdentity(); localTransform.setIdentity();
if (hasShapeLocalTransform) if (hasShapeLocalTransform)
{ {
localTransform = GetSimdTransformFromCOLLADA_DOM( localTransform = GetbtTransformFromCOLLADA_DOM(
emptyMatrixArray, emptyMatrixArray,
shapeRef->getRotate_array(), shapeRef->getRotate_array(),
shapeRef->getTranslate_array() shapeRef->getTranslate_array()

30
Demos/ColladaDemo/ColladaConverter.h
View File

@@ -18,10 +18,10 @@ subject to the following restrictions:
#ifndef COLLADA_CONVERTER_H #ifndef COLLADA_CONVERTER_H
#define COLLADA_CONVERTER_H #define COLLADA_CONVERTER_H
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
class CollisionShape; class btCollisionShape;
class PHY_IPhysicsController; class PHY_IPhysicsController;
class CcdPhysicsController; class CcdPhysicsController;
class ConstraintInput; class ConstraintInput;
@@ -44,11 +44,11 @@ protected:
int m_numObjects; int m_numObjects;
CcdPhysicsController* m_physObjects[COLLADA_CONVERTER_MAX_NUM_OBJECTS]; CcdPhysicsController* m_physObjects[COLLADA_CONVERTER_MAX_NUM_OBJECTS];
void PreparePhysicsObject(struct RigidBodyInput& input, bool isDynamics, float mass,CollisionShape* colShape); void PreparePhysicsObject(struct btRigidBodyInput& input, bool isDynamics, float mass,btCollisionShape* colShape);
void PrepareConstraints(ConstraintInput& input); void PrepareConstraints(ConstraintInput& input);
void ConvertRigidBodyRef( struct RigidBodyInput& , struct RigidBodyOutput& output ); void ConvertRigidBodyRef( struct btRigidBodyInput& , struct btRigidBodyOutput& output );
public: public:
@@ -68,22 +68,22 @@ public:
///those 2 virtuals are called for each constraint/physics object ///those 2 virtuals are called for each constraint/physics object
virtual int createUniversalD6Constraint( virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef, btTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther, btTransform& localAttachmentOther,
const SimdVector3& linearMinLimits, const btVector3& linearMinLimits,
const SimdVector3& linearMaxLimits, const btVector3& linearMaxLimits,
const SimdVector3& angularMinLimits, const btVector3& angularMinLimits,
const SimdVector3& angularMaxLimits const btVector3& angularMaxLimits
) = 0; ) = 0;
virtual CcdPhysicsController* CreatePhysicsObject(bool isDynamic, virtual CcdPhysicsController* CreatePhysicsObject(bool isDynamic,
float mass, float mass,
const SimdTransform& startTransform, const btTransform& startTransform,
CollisionShape* shape) = 0; btCollisionShape* shape) = 0;
virtual void SetGravity(const SimdVector3& gravity) = 0; virtual void SetGravity(const btVector3& gravity) = 0;
virtual void SetCameraInfo(const SimdVector3& up, int forwardAxis) = 0; virtual void SetCameraInfo(const btVector3& up, int forwardAxis) = 0;
}; };

36
Demos/ColladaDemo/ColladaDemo.cpp
View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#include "CcdPhysicsEnvironment.h" #include "CcdPhysicsEnvironment.h"
#include "CcdPhysicsController.h" #include "CcdPhysicsController.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/GenQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
@@ -50,12 +50,12 @@ class MyColladaConverter : public ColladaConverter
///those 2 virtuals are called for each constraint/physics object ///those 2 virtuals are called for each constraint/physics object
virtual int createUniversalD6Constraint( virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef, btTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther, btTransform& localAttachmentOther,
const SimdVector3& linearMinLimits, const btVector3& linearMinLimits,
const SimdVector3& linearMaxLimits, const btVector3& linearMaxLimits,
const SimdVector3& angularMinLimits, const btVector3& angularMinLimits,
const SimdVector3& angularMaxLimits const btVector3& angularMaxLimits
) )
{ {
return m_demoApp->GetPhysicsEnvironment()->createUniversalD6Constraint( return m_demoApp->GetPhysicsEnvironment()->createUniversalD6Constraint(
@@ -71,19 +71,19 @@ class MyColladaConverter : public ColladaConverter
virtual CcdPhysicsController* CreatePhysicsObject(bool isDynamic, virtual CcdPhysicsController* CreatePhysicsObject(bool isDynamic,
float mass, float mass,
const SimdTransform& startTransform, const btTransform& startTransform,
CollisionShape* shape) btCollisionShape* shape)
{ {
CcdPhysicsController* ctrl = m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape); CcdPhysicsController* ctrl = m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape);
return ctrl; return ctrl;
} }
virtual void SetGravity(const SimdVector3& grav) virtual void SetGravity(const btVector3& grav)
{ {
m_demoApp->GetPhysicsEnvironment()->setGravity(grav.getX(),grav.getY(),grav.getZ()); m_demoApp->GetPhysicsEnvironment()->setGravity(grav.getX(),grav.getY(),grav.getZ());
} }
virtual void SetCameraInfo(const SimdVector3& camUp,int forwardAxis) virtual void SetCameraInfo(const btVector3& camUp,int forwardAxis)
{ {
m_demoApp->setCameraUp(camUp); m_demoApp->setCameraUp(camUp);
m_demoApp->setCameraForwardAxis(forwardAxis); m_demoApp->setCameraForwardAxis(forwardAxis);
@@ -138,15 +138,15 @@ int main(int argc,char** argv)
void ColladaDemo::initPhysics(const char* filename) void ColladaDemo::initPhysics(const char* filename)
{ {
m_cameraUp = SimdVector3(0,0,1); m_cameraUp = btVector3(0,0,1);
m_forwardAxis = 1; m_forwardAxis = 1;
///Setup a Physics Simulation Environment ///Setup a Physics Simulation Environment
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000); btVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000); btVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//BroadphaseInterface* broadphase = new SimpleBroadphase(); //BroadphaseInterface* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
m_physicsEnvironmentPtr->setDeactivationTime(2.f); m_physicsEnvironmentPtr->setDeactivationTime(2.f);
m_physicsEnvironmentPtr->setGravity(0,0,-10); m_physicsEnvironmentPtr->setGravity(0,0,-10);

58
Demos/CollisionDemo/CollisionDemo.cpp
View File

@@ -16,15 +16,15 @@ subject to the following restrictions:
/// ///
/// Collision Demo shows a degenerate case, where the Simplex solver has to deal with near-affine dependent cases /// Collision Demo shows a degenerate case, where the Simplex solver has to deal with near-affine dependent cases
/// See the define CATCH_DEGENERATE_TETRAHEDRON in Bullet's VoronoiSimplexSolver.cpp /// See the define CATCH_DEGENERATE_TETRAHEDRON in Bullet's btVoronoiSimplexSolver.cpp
/// ///
///This low-level internal demo does intentionally NOT use the btBulletCollisionCommon.h header ///This low-level internal demo does intentionally NOT use the btBulletCollisionCommon.h header
///It needs internal access ///It needs internal access
#include "GL_Simplex1to4.h" #include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
@@ -35,7 +35,7 @@ subject to the following restrictions:
#include "CollisionDemo.h" #include "CollisionDemo.h"
#include "GL_ShapeDrawer.h" #include "GL_ShapeDrawer.h"
#include "GlutStuff.h" #include "GlutStuff.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
float yaw=0.f,pitch=0.f,roll=0.f; float yaw=0.f,pitch=0.f,roll=0.f;
@@ -44,9 +44,9 @@ const int numObjects = 2;
GL_Simplex1to4 simplex; GL_Simplex1to4 simplex;
PolyhedralConvexShape* shapePtr[maxNumObjects]; btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform tr[numObjects]; btTransform tr[numObjects];
int screenWidth = 640; int screenWidth = 640;
int screenHeight = 480; int screenHeight = 480;
@@ -70,22 +70,22 @@ int main(int argc,char** argv)
void CollisionDemo::initPhysics() void CollisionDemo::initPhysics()
{ {
m_debugMode |= IDebugDraw::DBG_DrawWireframe; m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
m_azi = 250.f; m_azi = 250.f;
m_ele = 25.f; m_ele = 25.f;
tr[0].setOrigin(SimdVector3(0.0013328250f,8.1363249f,7.0390840f)); tr[0].setOrigin(btVector3(0.0013328250f,8.1363249f,7.0390840f));
tr[1].setOrigin(SimdVector3(0.00000000f,9.1262732f,2.0343180f)); tr[1].setOrigin(btVector3(0.00000000f,9.1262732f,2.0343180f));
//tr[0].setOrigin(SimdVector3(0,0,0)); //tr[0].setOrigin(btVector3(0,0,0));
//tr[1].setOrigin(SimdVector3(0,10,0)); //tr[1].setOrigin(btVector3(0,10,0));
SimdMatrix3x3 basisA; btMatrix3x3 basisA;
basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f, basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
-0.00029313788f,0.99753088f,0.070228584f, -0.00029313788f,0.99753088f,0.070228584f,
-0.00089153741f,-0.070228823f,0.99753052f); -0.00089153741f,-0.070228823f,0.99753052f);
SimdMatrix3x3 basisB; btMatrix3x3 basisB;
basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f, basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f,
4.4865495e-018f,0.97979438f,0.20000751f, 4.4865495e-018f,0.97979438f,0.20000751f,
4.4410586e-017f,-0.20000751f,0.97979438f); 4.4410586e-017f,-0.20000751f,0.97979438f);
@@ -95,11 +95,11 @@ void CollisionDemo::initPhysics()
SimdVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f); btVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
SimdVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f); btVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);
BoxShape* boxA = new BoxShape(boxHalfExtentsA); btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
BoxShape* boxB = new BoxShape(boxHalfExtentsB); btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
shapePtr[0] = boxA; shapePtr[0] = boxA;
shapePtr[1] = boxB; shapePtr[1] = boxB;
@@ -112,8 +112,8 @@ void CollisionDemo::clientMoveAndDisplay()
} }
static VoronoiSimplexSolver sGjkSimplexSolver; static btVoronoiSimplexSolver sGjkSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver; btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
@@ -127,13 +127,13 @@ void CollisionDemo::displayCallback(void) {
float m[16]; float m[16];
int i; int i;
GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f); btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.SetCachedSeperatingAxis(seperatingAxis); convexConvex.SetCachedSeperatingAxis(seperatingAxis);
PointCollector gjkOutput; btPointCollector gjkOutput;
GjkPairDetector::ClosestPointInput input; btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0]; input.m_transformA = tr[0];
input.m_transformB = tr[1]; input.m_transformB = tr[1];
@@ -141,7 +141,7 @@ void CollisionDemo::displayCallback(void) {
if (gjkOutput.m_hasResult) if (gjkOutput.m_hasResult)
{ {
SimdVector3 endPt = gjkOutput.m_pointInWorld + btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance; gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES); glBegin(GL_LINES);
@@ -159,26 +159,26 @@ void CollisionDemo::displayCallback(void) {
tr[i].getOpenGLMatrix( m ); tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode());
} }
simplex.SetSimplexSolver(&sGjkSimplexSolver); simplex.SetSimplexSolver(&sGjkSimplexSolver);
SimdPoint3 ybuf[4],pbuf[4],qbuf[4]; btPoint3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf); int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.Reset(); simplex.Reset();
for (i=0;i<numpoints;i++) for (i=0;i<numpoints;i++)
simplex.AddVertex(ybuf[i]); simplex.AddVertex(ybuf[i]);
SimdTransform ident; btTransform ident;
ident.setIdentity(); ident.setIdentity();
ident.getOpenGLMatrix(m); ident.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,&simplex,SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode());
SimdQuaternion orn; btQuaternion orn;
orn.setEuler(yaw,pitch,roll); orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn); tr[0].setRotation(orn);

62
Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp
View File

@@ -23,7 +23,7 @@ subject to the following restrictions:
//include common Bullet Collision Detection headerfiles //include common Bullet Collision Detection headerfiles
#include "btBulletCollisionCommon.h" #include "btBulletCollisionCommon.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GL_ShapeDrawer.h" #include "GL_ShapeDrawer.h"
#include "CollisionInterfaceDemo.h" #include "CollisionInterfaceDemo.h"
#include "GlutStuff.h" #include "GlutStuff.h"
@@ -36,8 +36,8 @@ const int numObjects = 2;
GL_Simplex1to4 simplex; GL_Simplex1to4 simplex;
CollisionObject objects[maxNumObjects]; btCollisionObject objects[maxNumObjects];
CollisionWorld* collisionWorld = 0; btCollisionWorld* collisionWorld = 0;
int screenWidth = 640; int screenWidth = 640;
int screenHeight = 480; int screenHeight = 480;
@@ -57,40 +57,40 @@ int main(int argc,char** argv)
void CollisionInterfaceDemo::initPhysics() void CollisionInterfaceDemo::initPhysics()
{ {
m_debugMode |= IDebugDraw::DBG_DrawWireframe; m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
SimdMatrix3x3 basisA; btMatrix3x3 basisA;
basisA.setIdentity(); basisA.setIdentity();
SimdMatrix3x3 basisB; btMatrix3x3 basisB;
basisB.setIdentity(); basisB.setIdentity();
objects[0].m_worldTransform.setBasis(basisA); objects[0].m_worldTransform.setBasis(basisA);
objects[1].m_worldTransform.setBasis(basisB); objects[1].m_worldTransform.setBasis(basisB);
//SimdPoint3 points0[3]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1)}; //btPoint3 points0[3]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1)};
//SimdPoint3 points1[5]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1),SimdPoint3(0,0,-1),SimdPoint3(-1,-1,0)}; //btPoint3 points1[5]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1),btPoint3(0,0,-1),btPoint3(-1,-1,0)};
BoxShape* boxA = new BoxShape(SimdVector3(1,1,1)); btBoxShape* boxA = new btBoxShape(btVector3(1,1,1));
BoxShape* boxB = new BoxShape(SimdVector3(0.5,0.5,0.5)); btBoxShape* boxB = new btBoxShape(btVector3(0.5,0.5,0.5));
//ConvexHullShape hullA(points0,3); //ConvexHullShape hullA(points0,3);
//hullA.setLocalScaling(SimdVector3(3,3,3)); //hullA.setLocalScaling(btVector3(3,3,3));
//ConvexHullShape hullB(points1,4); //ConvexHullShape hullB(points1,4);
//hullB.setLocalScaling(SimdVector3(4,4,4)); //hullB.setLocalScaling(btVector3(4,4,4));
objects[0].m_collisionShape = boxA;//&hullA; objects[0].m_collisionShape = boxA;//&hullA;
objects[1].m_collisionShape = boxB;//&hullB; objects[1].m_collisionShape = boxB;//&hullB;
CollisionDispatcher* dispatcher = new CollisionDispatcher; btCollisionDispatcher* dispatcher = new btCollisionDispatcher;
SimdVector3 worldAabbMin(-1000,-1000,-1000); btVector3 worldAabbMin(-1000,-1000,-1000);
SimdVector3 worldAabbMax(1000,1000,1000); btVector3 worldAabbMax(1000,1000,1000);
AxisSweep3* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); btAxisSweep3* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//SimpleBroadphase is a brute force alternative, performing N^2 aabb overlap tests //SimpleBroadphase is a brute force alternative, performing N^2 aabb overlap tests
//SimpleBroadphase* broadphase = new SimpleBroadphase; //SimpleBroadphase* broadphase = new btSimpleBroadphase;
collisionWorld = new CollisionWorld(dispatcher,broadphase); collisionWorld = new btCollisionWorld(dispatcher,broadphase);
collisionWorld->AddCollisionObject(&objects[0]); collisionWorld->AddCollisionObject(&objects[0]);
collisionWorld->AddCollisionObject(&objects[1]); collisionWorld->AddCollisionObject(&objects[1]);
@@ -107,8 +107,8 @@ void CollisionInterfaceDemo::clientMoveAndDisplay()
} }
static VoronoiSimplexSolver sGjkSimplexSolver; static btVoronoiSimplexSolver sGjkSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver; btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
@@ -127,21 +127,21 @@ void CollisionInterfaceDemo::displayCallback(void) {
int numManifolds = collisionWorld->GetDispatcher()->GetNumManifolds(); int numManifolds = collisionWorld->GetDispatcher()->GetNumManifolds();
for (i=0;i<numManifolds;i++) for (i=0;i<numManifolds;i++)
{ {
PersistentManifold* contactManifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i); btPersistentManifold* contactManifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
CollisionObject* obA = static_cast<CollisionObject*>(contactManifold->GetBody0()); btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->GetBody0());
CollisionObject* obB = static_cast<CollisionObject*>(contactManifold->GetBody1()); btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->GetBody1());
contactManifold->RefreshContactPoints(obA->m_worldTransform,obB->m_worldTransform); contactManifold->RefreshContactPoints(obA->m_worldTransform,obB->m_worldTransform);
int numContacts = contactManifold->GetNumContacts(); int numContacts = contactManifold->GetNumContacts();
for (int j=0;j<numContacts;j++) for (int j=0;j<numContacts;j++)
{ {
ManifoldPoint& pt = contactManifold->GetContactPoint(j); btManifoldPoint& pt = contactManifold->GetContactPoint(j);
glBegin(GL_LINES); glBegin(GL_LINES);
glColor3f(1, 0, 1); glColor3f(1, 0, 1);
SimdVector3 ptA = pt.GetPositionWorldOnA(); btVector3 ptA = pt.GetPositionWorldOnA();
SimdVector3 ptB = pt.GetPositionWorldOnB(); btVector3 ptB = pt.GetPositionWorldOnB();
glVertex3d(ptA.x(),ptA.y(),ptA.z()); glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z()); glVertex3d(ptB.x(),ptB.y(),ptB.z());
@@ -162,14 +162,14 @@ void CollisionInterfaceDemo::displayCallback(void) {
{ {
objects[i].m_worldTransform.getOpenGLMatrix( m ); objects[i].m_worldTransform.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,objects[i].m_collisionShape,SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,objects[i].m_collisionShape,btVector3(1,1,1),getDebugMode());
} }
SimdQuaternion orn; btQuaternion orn;
orn.setEuler(yaw,pitch,roll); orn.setEuler(yaw,pitch,roll);
objects[1].m_worldTransform.setOrigin(objects[1].m_worldTransform.getOrigin()+SimdVector3(0,-0.01,0)); objects[1].m_worldTransform.setOrigin(objects[1].m_worldTransform.getOrigin()+btVector3(0,-0.01,0));
objects[0].m_worldTransform.setRotation(orn); objects[0].m_worldTransform.setRotation(orn);
@@ -182,8 +182,8 @@ void CollisionInterfaceDemo::displayCallback(void) {
void CollisionInterfaceDemo::clientResetScene() void CollisionInterfaceDemo::clientResetScene()
{ {
objects[0].m_worldTransform.setOrigin(SimdVector3(0.0f,3.f,0.f)); objects[0].m_worldTransform.setOrigin(btVector3(0.0f,3.f,0.f));
objects[1].m_worldTransform.setOrigin(SimdVector3(0.0f,9.f,0.f)); objects[1].m_worldTransform.setOrigin(btVector3(0.0f,9.f,0.f));
} }

2
Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#include "DemoApplication.h" #include "DemoApplication.h"
///CollisionInterfaceDemo shows how to use the collision detection without dynamics (CollisionWorld/CollisionObject) ///CollisionInterfaceDemo shows how to use the collision detection without dynamics (btCollisionWorld/CollisionObject)
class CollisionInterfaceDemo : public DemoApplication class CollisionInterfaceDemo : public DemoApplication
{ {
public: public:

44
Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#include "CcdPhysicsController.h" #include "CcdPhysicsController.h"
#include "MyMotionState.h" #include "MyMotionState.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
#include "PHY_Pro.h" #include "PHY_Pro.h"
#include "ConcaveDemo.h" #include "ConcaveDemo.h"
@@ -30,17 +30,17 @@ GLDebugDrawer debugDrawer;
static const int NUM_VERTICES = 5; static const int NUM_VERTICES = 5;
static const int NUM_TRIANGLES=4; static const int NUM_TRIANGLES=4;
SimdVector3 gVertices[NUM_VERTICES]; btVector3 gVertices[NUM_VERTICES];
int gIndices[NUM_TRIANGLES*3]; int gIndices[NUM_TRIANGLES*3];
const float TRIANGLE_SIZE=80.f; const float TRIANGLE_SIZE=80.f;
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= CollisionObject::customMaterialCallback; ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
inline SimdScalar calculateCombinedFriction(float friction0,float friction1) inline btScalar calculateCombinedFriction(float friction0,float friction1)
{ {
SimdScalar friction = friction0 * friction1; btScalar friction = friction0 * friction1;
const SimdScalar MAX_FRICTION = 10.f; const btScalar MAX_FRICTION = 10.f;
if (friction < -MAX_FRICTION) if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION; friction = -MAX_FRICTION;
if (friction > MAX_FRICTION) if (friction > MAX_FRICTION)
@@ -49,14 +49,14 @@ inline SimdScalar calculateCombinedFriction(float friction0,float friction1)
} }
inline SimdScalar calculateCombinedRestitution(float restitution0,float restitution1) inline btScalar calculateCombinedRestitution(float restitution0,float restitution1)
{ {
return restitution0 * restitution1; return restitution0 * restitution1;
} }
bool CustomMaterialCombinerCallback(ManifoldPoint& cp, const CollisionObject* colObj0,int partId0,int index0,const CollisionObject* colObj1,int partId1,int index1) bool CustomMaterialCombinerCallback(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
{ {
float friction0 = colObj0->getFriction(); float friction0 = colObj0->getFriction();
@@ -64,12 +64,12 @@ bool CustomMaterialCombinerCallback(ManifoldPoint& cp, const CollisionObject* co
float restitution0 = colObj0->getRestitution(); float restitution0 = colObj0->getRestitution();
float restitution1 = colObj1->getRestitution(); float restitution1 = colObj1->getRestitution();
if (colObj0->m_collisionFlags & CollisionObject::customMaterialCallback) if (colObj0->m_collisionFlags & btCollisionObject::customMaterialCallback)
{ {
friction0 = 1.0;//partId0,index0 friction0 = 1.0;//partId0,index0
restitution0 = 0.f; restitution0 = 0.f;
} }
if (colObj1->m_collisionFlags & CollisionObject::customMaterialCallback) if (colObj1->m_collisionFlags & btCollisionObject::customMaterialCallback)
{ {
if (index1&1) if (index1&1)
{ {
@@ -107,7 +107,7 @@ void ConcaveDemo::initPhysics()
{ {
#define TRISIZE 10.f #define TRISIZE 10.f
int vertStride = sizeof(SimdVector3); int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int); int indexStride = 3*sizeof(int);
const int NUM_VERTS_X = 50; const int NUM_VERTS_X = 50;
@@ -116,7 +116,7 @@ void ConcaveDemo::initPhysics()
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
SimdVector3* gVertices = new SimdVector3[totalVerts]; btVector3* gVertices = new btVector3[totalVerts];
int* gIndices = new int[totalTriangles*3]; int* gIndices = new int[totalTriangles*3];
int i; int i;
@@ -144,39 +144,39 @@ void ConcaveDemo::initPhysics()
} }
} }
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices, gIndices,
indexStride, indexStride,
totalVerts,(float*) &gVertices[0].x(),vertStride); totalVerts,(float*) &gVertices[0].x(),vertStride);
CollisionShape* trimeshShape = new BvhTriangleMeshShape(indexVertexArrays); btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(indexVertexArrays);
// ConstraintSolver* solver = new SequentialImpulseConstraintSolver; // btConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
OverlappingPairCache* broadphase = new SimpleBroadphase(); btOverlappingPairCache* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
bool isDynamic = false; bool isDynamic = false;
float mass = 0.f; float mass = 0.f;
SimdTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,-2,0)); startTransform.setOrigin(btVector3(0,-2,0));
CcdPhysicsController* staticTrimesh = LocalCreatePhysicsObject(isDynamic, mass, startTransform,trimeshShape); CcdPhysicsController* staticTrimesh = LocalCreatePhysicsObject(isDynamic, mass, startTransform,trimeshShape);
//enable custom material callback //enable custom material callback
staticTrimesh->GetRigidBody()->m_collisionFlags |= CollisionObject::customMaterialCallback; staticTrimesh->GetRigidBody()->m_collisionFlags |= btCollisionObject::customMaterialCallback;
{ {
for (int i=0;i<10;i++) for (int i=0;i<10;i++)
{ {
CollisionShape* boxShape = new BoxShape(SimdVector3(1,1,1)); btCollisionShape* boxShape = new btBoxShape(btVector3(1,1,1));
startTransform.setOrigin(SimdVector3(2*i,1,1)); startTransform.setOrigin(btVector3(2*i,1,1));
LocalCreatePhysicsObject(true, 1, startTransform,boxShape); LocalCreatePhysicsObject(true, 1, startTransform,boxShape);
} }
} }

6
Demos/ConcaveDemo/MyMotionState.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/ */
#include "MyMotionState.h" #include "MyMotionState.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
MyMotionState::MyMotionState() MyMotionState::MyMotionState()
{ {
@@ -51,13 +51,13 @@ void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& q
void MyMotionState::setWorldPosition(float posX,float posY,float posZ) void MyMotionState::setWorldPosition(float posX,float posY,float posZ)
{ {
SimdPoint3 pos(posX,posY,posZ); btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos ); m_worldTransform.setOrigin( pos );
} }
void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
{ {
SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
m_worldTransform.setRotation( orn ); m_worldTransform.setRotation( orn );
} }

4
Demos/ConcaveDemo/MyMotionState.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#define MY_MOTIONSTATE_H #define MY_MOTIONSTATE_H
#include "PHY_IMotionState.h" #include "PHY_IMotionState.h"
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
class MyMotionState : public PHY_IMotionState class MyMotionState : public PHY_IMotionState
@@ -37,7 +37,7 @@ class MyMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations(); virtual void calculateWorldTransformations();
SimdTransform m_worldTransform; btTransform m_worldTransform;
}; };

16
Demos/ConstraintDemo/ConstraintDemo.cpp
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#include "CcdPhysicsController.h" #include "CcdPhysicsController.h"
#include "MyMotionState.h" #include "MyMotionState.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
@@ -53,12 +53,12 @@ int main(int argc,char** argv)
void ConstraintDemo::initPhysics() void ConstraintDemo::initPhysics()
{ {
//ConstraintSolver* solver = new SequentialImpulseConstraintSolver; //ConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
//ConstraintSolver* solver = new OdeConstraintSolver; //ConstraintSolver* solver = new OdeConstraintSolver;
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
OverlappingPairCache* broadphase = new SimpleBroadphase(); btOverlappingPairCache* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
@@ -66,16 +66,16 @@ void ConstraintDemo::initPhysics()
m_physicsEnvironmentPtr->setGravity(0,-10,0); m_physicsEnvironmentPtr->setGravity(0,-10,0);
CollisionShape* shape = new BoxShape(SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)); btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
SimdTransform trans; btTransform trans;
trans.setIdentity(); trans.setIdentity();
trans.setOrigin(SimdVector3(0,20,0)); trans.setOrigin(btVector3(0,20,0));
bool isDynamic = false; bool isDynamic = false;
float mass = 1.f; float mass = 1.f;
CcdPhysicsController* ctrl0 = LocalCreatePhysicsObject( isDynamic,mass,trans,shape); CcdPhysicsController* ctrl0 = LocalCreatePhysicsObject( isDynamic,mass,trans,shape);
trans.setOrigin(SimdVector3(2*CUBE_HALF_EXTENTS,20,0)); trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0));
isDynamic = true; isDynamic = true;
CcdPhysicsController* ctrl1 = LocalCreatePhysicsObject( isDynamic,mass,trans,shape); CcdPhysicsController* ctrl1 = LocalCreatePhysicsObject( isDynamic,mass,trans,shape);

2
Demos/ConstraintDemo/ConstraintDemo.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#include "DemoApplication.h" #include "DemoApplication.h"
///ConstraintDemo shows how to create a constraint, like Hinge or GenericD6constraint ///ConstraintDemo shows how to create a constraint, like Hinge or btGenericD6constraint
class ConstraintDemo : public DemoApplication class ConstraintDemo : public DemoApplication
{ {
public: public:

6
Demos/ConstraintDemo/MyMotionState.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/ */
#include "MyMotionState.h" #include "MyMotionState.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
MyMotionState::MyMotionState() MyMotionState::MyMotionState()
{ {
@@ -51,13 +51,13 @@ void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& q
void MyMotionState::setWorldPosition(float posX,float posY,float posZ) void MyMotionState::setWorldPosition(float posX,float posY,float posZ)
{ {
SimdPoint3 pos(posX,posY,posZ); btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos ); m_worldTransform.setOrigin( pos );
} }
void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
{ {
SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
m_worldTransform.setRotation( orn ); m_worldTransform.setRotation( orn );
} }

4
Demos/ConstraintDemo/MyMotionState.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#define MY_MOTIONSTATE_H #define MY_MOTIONSTATE_H
#include "PHY_IMotionState.h" #include "PHY_IMotionState.h"
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
class MyMotionState : public PHY_IMotionState class MyMotionState : public PHY_IMotionState
@@ -37,7 +37,7 @@ class MyMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations(); virtual void calculateWorldTransformations();
SimdTransform m_worldTransform; btTransform m_worldTransform;
}; };

2
Demos/ContinuousConvexCollision/ContinuousConvexCollision.h
View File

@@ -18,7 +18,7 @@ subject to the following restrictions:
#include "DemoApplication.h" #include "DemoApplication.h"
///ContinuousConvexCollisionDemo shows the working of the continuous collision detection, including linear and angular motion ///ContinuousConvexCollisionDemo shows the working of the continuous collision detection, including linear and angular motion
class ContinuousConvexCollisionDemo : public DemoApplication class btContinuousConvexCollisionDemo : public DemoApplication
{ {
public: public:

110
Demos/ContinuousConvexCollision/ContinuousConvexCollisionDemo.cpp
View File

@@ -20,8 +20,8 @@
///This low level demo need internal access, and intentionally doesn't include the btBulletCollisionCommon.h headerfile ///This low level demo need internal access, and intentionally doesn't include the btBulletCollisionCommon.h headerfile
#include "LinearMath/SimdQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
@@ -31,7 +31,7 @@
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
#include "LinearMath/SimdTransformUtil.h" #include "LinearMath/btTransformUtil.h"
#include "DebugCastResult.h" #include "DebugCastResult.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
@@ -50,14 +50,14 @@ float yaw=0.f,pitch=0.f,roll=0.f;
const int maxNumObjects = 4; const int maxNumObjects = 4;
const int numObjects = 2; const int numObjects = 2;
SimdVector3 angVels[numObjects]; btVector3 angVels[numObjects];
SimdVector3 linVels[numObjects]; btVector3 linVels[numObjects];
PolyhedralConvexShape* shapePtr[maxNumObjects]; btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform fromTrans[maxNumObjects]; btTransform fromTrans[maxNumObjects];
SimdTransform toTrans[maxNumObjects]; btTransform toTrans[maxNumObjects];
int screenWidth = 640; int screenWidth = 640;
@@ -66,7 +66,7 @@ int screenHeight = 480;
int main(int argc,char** argv) int main(int argc,char** argv)
{ {
ContinuousConvexCollisionDemo* ccdDemo = new ContinuousConvexCollisionDemo(); btContinuousConvexCollisionDemo* ccdDemo = new btContinuousConvexCollisionDemo();
ccdDemo->setCameraDistance(40.f); ccdDemo->setCameraDistance(40.f);
@@ -75,17 +75,17 @@ int main(int argc,char** argv)
return glutmain(argc, argv,screenWidth,screenHeight,"Continuous Convex Collision Demo",ccdDemo); return glutmain(argc, argv,screenWidth,screenHeight,"Continuous Convex Collision Demo",ccdDemo);
} }
void ContinuousConvexCollisionDemo::initPhysics() void btContinuousConvexCollisionDemo::initPhysics()
{ {
fromTrans[0].setOrigin(SimdVector3(0,10,20)); fromTrans[0].setOrigin(btVector3(0,10,20));
toTrans[0].setOrigin(SimdVector3(0,10,-20)); toTrans[0].setOrigin(btVector3(0,10,-20));
fromTrans[1].setOrigin(SimdVector3(-2,7,0)); fromTrans[1].setOrigin(btVector3(-2,7,0));
toTrans[1].setOrigin(SimdVector3(-2,10,0)); toTrans[1].setOrigin(btVector3(-2,10,0));
SimdMatrix3x3 identBasis; btMatrix3x3 identBasis;
identBasis.setIdentity(); identBasis.setIdentity();
SimdMatrix3x3 basisA; btMatrix3x3 basisA;
basisA.setIdentity(); basisA.setIdentity();
basisA.setEulerZYX(0.f,-SIMD_HALF_PI,0.f); basisA.setEulerZYX(0.f,-SIMD_HALF_PI,0.f);
@@ -96,15 +96,15 @@ void ContinuousConvexCollisionDemo::initPhysics()
toTrans[1].setBasis(identBasis); toTrans[1].setBasis(identBasis);
toTrans[1].setBasis(identBasis); toTrans[1].setBasis(identBasis);
SimdVector3 boxHalfExtentsA(10,1,1); btVector3 boxHalfExtentsA(10,1,1);
SimdVector3 boxHalfExtentsB(1.1f,1.1f,1.1f); btVector3 boxHalfExtentsB(1.1f,1.1f,1.1f);
BoxShape* boxA = new BoxShape(boxHalfExtentsA); btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
// BU_Simplex1to4* boxA = new BU_Simplex1to4(SimdPoint3(-2,0,-2),SimdPoint3(2,0,-2),SimdPoint3(0,0,2),SimdPoint3(0,2,0)); // btBU_Simplex1to4* boxA = new btBU_Simplex1to4(btPoint3(-2,0,-2),btPoint3(2,0,-2),btPoint3(0,0,2),btPoint3(0,2,0));
// BU_Simplex1to4* boxA = new BU_Simplex1to4(SimdPoint3(-12,0,0),SimdPoint3(12,0,0)); // btBU_Simplex1to4* boxA = new btBU_Simplex1to4(btPoint3(-12,0,0),btPoint3(12,0,0));
BoxShape* boxB = new BoxShape(boxHalfExtentsB); btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
// BU_Simplex1to4 boxB(SimdPoint3(0,10,0),SimdPoint3(0,-10,0)); // btBU_Simplex1to4 boxB(btPoint3(0,10,0),btPoint3(0,-10,0));
shapePtr[0] = boxA; shapePtr[0] = boxA;
@@ -115,22 +115,22 @@ void ContinuousConvexCollisionDemo::initPhysics()
for (int i=0;i<numObjects;i++) for (int i=0;i<numObjects;i++)
{ {
SimdTransformUtil::CalculateVelocity(fromTrans[i],toTrans[i],1.f,linVels[i],angVels[i]); btTransformUtil::CalculateVelocity(fromTrans[i],toTrans[i],1.f,linVels[i],angVels[i]);
} }
} }
//to be implemented by the demo //to be implemented by the demo
void ContinuousConvexCollisionDemo::clientMoveAndDisplay() void btContinuousConvexCollisionDemo::clientMoveAndDisplay()
{ {
displayCallback(); displayCallback();
} }
static VoronoiSimplexSolver sVoronoiSimplexSolver; static btVoronoiSimplexSolver sVoronoiSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver; btSimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver;
bool drawLine= false; bool drawLine= false;
@@ -139,7 +139,7 @@ int minlines = 0;
int maxlines = 512; int maxlines = 512;
void ContinuousConvexCollisionDemo::displayCallback(void) { void btContinuousConvexCollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING); glDisable(GL_LIGHTING);
@@ -156,7 +156,7 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
} }
*/ */
if (getDebugMode()==IDebugDraw::DBG_DrawAabb) if (getDebugMode()==btIDebugDraw::DBG_DrawAabb)
{ {
i=0;//for (i=1;i<numObjects;i++) i=0;//for (i=1;i<numObjects;i++)
{ {
@@ -165,10 +165,10 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
int numSubSteps = 10; int numSubSteps = 10;
for (int s=0;s<10;s++) for (int s=0;s<10;s++)
{ {
SimdScalar subStep = s * 1.f/(float)numSubSteps; btScalar subStep = s * 1.f/(float)numSubSteps;
SimdTransform interpolatedTrans; btTransform interpolatedTrans;
SimdTransformUtil::IntegrateTransform(fromTrans[i],linVels[i],angVels[i],subStep,interpolatedTrans); btTransformUtil::IntegrateTransform(fromTrans[i],linVels[i],angVels[i],subStep,interpolatedTrans);
//fromTrans[i].getOpenGLMatrix(m); //fromTrans[i].getOpenGLMatrix(m);
//GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]); //GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]);
@@ -177,15 +177,15 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
//GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]); //GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]);
interpolatedTrans.getOpenGLMatrix( m ); interpolatedTrans.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,0,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(1,0,1),getDebugMode());
} }
} }
} }
SimdMatrix3x3 mat; btMatrix3x3 mat;
mat.setEulerZYX(yaw,pitch,roll); mat.setEulerZYX(yaw,pitch,roll);
SimdQuaternion orn; btQuaternion orn;
mat.getRotation(orn); mat.getRotation(orn);
orn.setEuler(yaw,pitch,roll); orn.setEuler(yaw,pitch,roll);
fromTrans[1].setRotation(orn); fromTrans[1].setRotation(orn);
@@ -198,17 +198,17 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
pitch += 0.005f; pitch += 0.005f;
// yaw += 0.01f; // yaw += 0.01f;
} }
// SimdVector3 fromA(-25,11,0); // btVector3 fromA(-25,11,0);
// SimdVector3 toA(-15,11,0); // btVector3 toA(-15,11,0);
// SimdQuaternion ornFromA(0.f,0.f,0.f,1.f); // btQuaternion ornFromA(0.f,0.f,0.f,1.f);
// SimdQuaternion ornToA(0.f,0.f,0.f,1.f); // btQuaternion ornToA(0.f,0.f,0.f,1.f);
// SimdTransform rayFromWorld(ornFromA,fromA); // btTransform rayFromWorld(ornFromA,fromA);
// SimdTransform rayToWorld(ornToA,toA); // btTransform rayToWorld(ornToA,toA);
SimdTransform rayFromWorld = fromTrans[0]; btTransform rayFromWorld = fromTrans[0];
SimdTransform rayToWorld = toTrans[0]; btTransform rayToWorld = toTrans[0];
if (drawLine) if (drawLine)
@@ -229,17 +229,17 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
for (i=0;i<numObjects;i++) for (i=0;i<numObjects;i++)
{ {
fromTrans[i].getOpenGLMatrix(m); fromTrans[i].getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode());
} }
DebugCastResult rayResult1(fromTrans[0],shapePtr[0],linVels[0],angVels[0]); btDebugCastResult rayResult1(fromTrans[0],shapePtr[0],linVels[0],angVels[0]);
for (i=1;i<numObjects;i++) for (i=1;i<numObjects;i++)
{ {
ConvexCast::CastResult rayResult2; btConvexCast::CastResult rayResult2;
ConvexCast::CastResult* rayResultPtr; btConvexCast::CastResult* rayResultPtr;
if (IDebugDraw::DBG_DrawAabb) if (btIDebugDraw::DBG_DrawAabb)
{ {
rayResultPtr = &rayResult1; rayResultPtr = &rayResult1;
} else } else
@@ -251,8 +251,8 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
//SubsimplexConvexCast convexCaster(&gGjkSimplexSolver); //SubsimplexConvexCast convexCaster(&gGjkSimplexSolver);
//optional //optional
ConvexPenetrationDepthSolver* penetrationDepthSolver = 0; btConvexPenetrationDepthSolver* penetrationDepthSolver = 0;
ContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver ); btContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver );
gGjkSimplexSolver.reset(); gGjkSimplexSolver.reset();
@@ -263,16 +263,16 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
glDisable(GL_DEPTH_TEST); glDisable(GL_DEPTH_TEST);
SimdTransform hitTrans; btTransform hitTrans;
SimdTransformUtil::IntegrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans); btTransformUtil::IntegrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans);
hitTrans.getOpenGLMatrix(m); hitTrans.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,0),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],btVector3(0,1,0),getDebugMode());
SimdTransformUtil::IntegrateTransform(fromTrans[i],linVels[i],angVels[i],rayResultPtr->m_fraction,hitTrans); btTransformUtil::IntegrateTransform(fromTrans[i],linVels[i],angVels[i],rayResultPtr->m_fraction,hitTrans);
hitTrans.getOpenGLMatrix(m); hitTrans.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(0,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(0,1,1),getDebugMode());
} }

60
Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp
View File

@@ -21,8 +21,8 @@ subject to the following restrictions:
#include "CcdPhysicsController.h" #include "CcdPhysicsController.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/GenQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
@@ -42,11 +42,11 @@ const int maxNumObjects = 450;
int shapeIndex[maxNumObjects]; int shapeIndex[maxNumObjects];
SimdVector3 centroid; btVector3 centroid;
#define CUBE_HALF_EXTENTS 4 #define CUBE_HALF_EXTENTS 4
CollisionShape* shapePtr[maxNumObjects]; btCollisionShape* shapePtr[maxNumObjects];
@@ -81,25 +81,25 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
tcount = wo.loadObj(filename); tcount = wo.loadObj(filename);
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000); btVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000); btVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//OverlappingPairCache* broadphase = new SimpleBroadphase(); //OverlappingPairCache* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
m_physicsEnvironmentPtr->setDeactivationTime(2.f); m_physicsEnvironmentPtr->setDeactivationTime(2.f);
m_physicsEnvironmentPtr->setGravity(0,-10,0); m_physicsEnvironmentPtr->setGravity(0,-10,0);
SimdTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,-4,0)); startTransform.setOrigin(btVector3(0,-4,0));
LocalCreatePhysicsObject(false,0,startTransform,new BoxShape(SimdVector3(30,2,30))); LocalCreatePhysicsObject(false,0,startTransform,new btBoxShape(btVector3(30,2,30)));
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{ {
@@ -119,9 +119,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result) virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{ {
TriangleMesh* trimesh = new TriangleMesh(); btTriangleMesh* trimesh = new btTriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f); btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj //export data to .obj
printf("ConvexResult\n"); printf("ConvexResult\n");
@@ -148,9 +148,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
unsigned int index0 = *src++; unsigned int index0 = *src++;
unsigned int index1 = *src++; unsigned int index1 = *src++;
unsigned int index2 = *src++; unsigned int index2 = *src++;
SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling; vertex0 *= localScaling;
vertex1 *= localScaling; vertex1 *= localScaling;
vertex2 *= localScaling; vertex2 *= localScaling;
@@ -173,9 +173,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
unsigned int index2 = *src++; unsigned int index2 = *src++;
SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling; vertex0 *= localScaling;
vertex1 *= localScaling; vertex1 *= localScaling;
vertex2 *= localScaling; vertex2 *= localScaling;
@@ -196,8 +196,8 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
bool isDynamic = true; bool isDynamic = true;
float mass = 1.f; float mass = 1.f;
CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh); btCollisionShape* convexShape = new btConvexTriangleMeshShape(trimesh);
SimdTransform trans; btTransform trans;
trans.setIdentity(); trans.setIdentity();
trans.setOrigin(centroid); trans.setOrigin(centroid);
m_convexDemo->LocalCreatePhysicsObject(isDynamic, mass, trans,convexShape); m_convexDemo->LocalCreatePhysicsObject(isDynamic, mass, trans,convexShape);
@@ -216,9 +216,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
if (tcount) if (tcount)
{ {
TriangleMesh* trimesh = new TriangleMesh(); btTriangleMesh* trimesh = new btTriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f); btVector3 localScaling(6.f,6.f,6.f);
for (int i=0;i<wo.mTriCount;i++) for (int i=0;i<wo.mTriCount;i++)
{ {
@@ -226,9 +226,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
int index1 = wo.mIndices[i*3+1]; int index1 = wo.mIndices[i*3+1];
int index2 = wo.mIndices[i*3+2]; int index2 = wo.mIndices[i*3+2];
SimdVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]); btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
SimdVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]); btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
SimdVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]); btVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
vertex0 *= localScaling; vertex0 *= localScaling;
vertex1 *= localScaling; vertex1 *= localScaling;
@@ -237,13 +237,13 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
trimesh->AddTriangle(vertex0,vertex1,vertex2); trimesh->AddTriangle(vertex0,vertex1,vertex2);
} }
CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh); btCollisionShape* convexShape = new btConvexTriangleMeshShape(trimesh);
bool isDynamic = true; bool isDynamic = true;
float mass = 1.f; float mass = 1.f;
SimdTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(20,2,0)); startTransform.setOrigin(btVector3(20,2,0));
LocalCreatePhysicsObject(isDynamic, mass, startTransform,convexShape); LocalCreatePhysicsObject(isDynamic, mass, startTransform,convexShape);

52
Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp
View File

@@ -16,12 +16,12 @@ subject to the following restrictions:
/// ///
/// Convex Hull Distance Demo shows distance calculation between two convex hulls of points. /// Convex Hull Distance Demo shows distance calculation between two convex hulls of points.
/// GJK with the VoronoiSimplexSolver is used. /// GJK with the btVoronoiSimplexSolver is used.
/// ///
#include "GL_Simplex1to4.h" #include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
@@ -51,9 +51,9 @@ const int numObjects = 2;
GL_Simplex1to4 simplex; GL_Simplex1to4 simplex;
PolyhedralConvexShape* shapePtr[maxNumObjects]; btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform tr[numObjects]; btTransform tr[numObjects];
int screenWidth = 640.f; int screenWidth = 640.f;
int screenHeight = 480.f; int screenHeight = 480.f;
@@ -62,26 +62,26 @@ int main(int argc,char** argv)
{ {
clientResetScene(); clientResetScene();
SimdMatrix3x3 basisA; btMatrix3x3 basisA;
basisA.setIdentity(); basisA.setIdentity();
SimdMatrix3x3 basisB; btMatrix3x3 basisB;
basisB.setIdentity(); basisB.setIdentity();
tr[0].setBasis(basisA); tr[0].setBasis(basisA);
tr[1].setBasis(basisB); tr[1].setBasis(basisB);
SimdPoint3 points0[3]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1)}; btPoint3 points0[3]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1)};
SimdPoint3 points1[5]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1),SimdPoint3(0,0,-1),SimdPoint3(-1,-1,0)}; btPoint3 points1[5]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1),btPoint3(0,0,-1),btPoint3(-1,-1,0)};
ConvexHullShape hullA(points0,3); btConvexHullShape hullA(points0,3);
ConvexHullShape hullB(points1,5); btConvexHullShape hullB(points1,5);
shapePtr[0] = &hullA; shapePtr[0] = &hullA;
shapePtr[1] = &hullB; shapePtr[1] = &hullB;
SimdTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
@@ -97,8 +97,8 @@ void clientMoveAndDisplay()
} }
static VoronoiSimplexSolver sGjkSimplexSolver; static btVoronoiSimplexSolver sGjkSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver; btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
@@ -112,13 +112,13 @@ void clientDisplay(void) {
float m[16]; float m[16];
int i; int i;
GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f); btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.SetCachedSeperatingAxis(seperatingAxis); convexConvex.SetCachedSeperatingAxis(seperatingAxis);
PointCollector gjkOutput; btPointCollector gjkOutput;
GjkPairDetector::ClosestPointInput input; btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0]; input.m_transformA = tr[0];
input.m_transformB = tr[1]; input.m_transformB = tr[1];
@@ -126,7 +126,7 @@ void clientDisplay(void) {
if (gjkOutput.m_hasResult) if (gjkOutput.m_hasResult)
{ {
SimdVector3 endPt = gjkOutput.m_pointInWorld + btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance; gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES); glBegin(GL_LINES);
@@ -142,26 +142,26 @@ void clientDisplay(void) {
tr[i].getOpenGLMatrix( m ); tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode());
} }
simplex.SetSimplexSolver(&sGjkSimplexSolver); simplex.SetSimplexSolver(&sGjkSimplexSolver);
SimdPoint3 ybuf[4],pbuf[4],qbuf[4]; btPoint3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf); int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.Reset(); simplex.Reset();
for (i=0;i<numpoints;i++) for (i=0;i<numpoints;i++)
simplex.AddVertex(ybuf[i]); simplex.AddVertex(ybuf[i]);
SimdTransform ident; btTransform ident;
ident.setIdentity(); ident.setIdentity();
ident.getOpenGLMatrix(m); ident.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,&simplex,SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode());
SimdQuaternion orn; btQuaternion orn;
orn.setEuler(yaw,pitch,roll); orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn); tr[0].setRotation(orn);
tr[1].setRotation(orn); tr[1].setRotation(orn);
@@ -175,8 +175,8 @@ void clientDisplay(void) {
void clientResetScene() void clientResetScene()
{ {
tr[0].setOrigin(SimdVector3(0.0f,3.f,7.f)); tr[0].setOrigin(btVector3(0.0f,3.f,7.f));
tr[1].setOrigin(SimdVector3(0.0f,9.f,2.f)); tr[1].setOrigin(btVector3(0.0f,9.f,2.f));
} }
void clientKeyboard(unsigned char key, int x, int y) void clientKeyboard(unsigned char key, int x, int y)

96
Demos/EPAPenDepthDemo/EpaPenDepthDemo.cpp
View File

@@ -19,13 +19,13 @@ subject to the following restrictions:
/** /**
* Sample that shows the Expanding Polytope Algorithm ( EPA ) * Sample that shows the Expanding Polytope Algorithm ( EPA )
* Generates two convex shapes and calculates the penetration depth * bterates two convex shapes and calculates the penetration depth
* between them in case they are penetrating * between them in case they are penetrating
*/ */
#include "GL_Simplex1to4.h" #include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "GL_ShapeDrawer.h" #include "GL_ShapeDrawer.h"
#include <GL/glut.h> #include <GL/glut.h>
#include "GlutStuff.h" #include "GlutStuff.h"
@@ -51,34 +51,34 @@ subject to the following restrictions:
EpaPenetrationDepthSolver epaPenDepthSolver; EpaPenetrationDepthSolver epaPenDepthSolver;
SimplexSolverInterface simplexSolver; btSimplexSolverInterface simplexSolver;
int screenWidth = 640.f; int screenWidth = 640.f;
int screenHeight = 480.f; int screenHeight = 480.f;
// Scene stuff // Scene stuff
SimdPoint3 g_sceneVolumeMin( -1, -1, -1 ); btPoint3 g_sceneVolumeMin( -1, -1, -1 );
SimdPoint3 g_sceneVolumeMax( 1, 1, 1 ); btPoint3 g_sceneVolumeMax( 1, 1, 1 );
bool g_shapesPenetrate = false; bool g_shapesPenetrate = false;
SimdVector3 g_wWitnesses[ 2 ]; btVector3 g_wWitnesses[ 2 ];
// Shapes stuff // Shapes stuff
ConvexShape* g_pConvexShapes[ 2 ] = { 0 }; btConvexShape* g_pConvexShapes[ 2 ] = { 0 };
SimdTransform g_convexShapesTransform[ 2 ]; btTransform g_convexShapesTransform[ 2 ];
SimdScalar g_animAngle = SIMD_RADS_PER_DEG; btScalar g_animAngle = SIMD_RADS_PER_DEG;
bool g_pauseAnim = true; bool g_pauseAnim = true;
// 0 - Box ; 1 - Sphere // 0 - Box ; 1 - Sphere
int g_shapesType[ 2 ] = { 0 }; int g_shapesType[ 2 ] = { 0 };
// Box config // Box config
SimdVector3 g_boxExtents( 1, 1, 1 ); btVector3 g_boxExtents( 1, 1, 1 );
// Sphere config // Sphere config
SimdScalar g_sphereRadius = 1; btScalar g_sphereRadius = 1;
float randomFloat( float rangeMin, float rangeMax ) float randomFloat( float rangeMin, float rangeMax )
{ {
@@ -90,9 +90,9 @@ int randomShapeType( int minShapeType, int maxShapeType )
return ( ( ( ( maxShapeType - minShapeType ) + 1 ) * rand() ) / ( ( RAND_MAX + 1 ) + minShapeType ) ); return ( ( ( ( maxShapeType - minShapeType ) + 1 ) * rand() ) / ( ( RAND_MAX + 1 ) + minShapeType ) );
} }
SimdVector3 randomPosition( const SimdPoint3& minPoint, const SimdPoint3& maxPoint ) btVector3 randomPosition( const btPoint3& minPoint, const btPoint3& maxPoint )
{ {
return SimdVector3( randomFloat( minPoint.getX(), maxPoint.getX() ), return btVector3( randomFloat( minPoint.getX(), maxPoint.getX() ),
randomFloat( minPoint.getY(), maxPoint.getY() ), randomFloat( minPoint.getY(), maxPoint.getY() ),
randomFloat( minPoint.getZ(), maxPoint.getZ() ) ); randomFloat( minPoint.getZ(), maxPoint.getZ() ) );
} }
@@ -104,39 +104,39 @@ bool createBoxShape( int shapeIndex )
// //
//if ( b ) //if ( b )
//{ //{
// g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 1, 1, 1 ) ); // g_pConvexShapes[ shapeIndex ] = new btBoxShape( btVector3( 1, 1, 1 ) );
// g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 ); // g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 );
// g_convexShapesTransform[ shapeIndex ].setIdentity(); // g_convexShapesTransform[ shapeIndex ].setIdentity();
// SimdMatrix3x3 basis( 0.99365157, 0.024418538, -0.10981932, // btMatrix3x3 basis( 0.99365157, 0.024418538, -0.10981932,
// -0.025452739, 0.99964380, -0.0080251107, // -0.025452739, 0.99964380, -0.0080251107,
// 0.10958424, 0.010769366, 0.99391919 ); // 0.10958424, 0.010769366, 0.99391919 );
// g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 4.4916530, -19.059078, -0.22695254 ) ); // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 4.4916530, -19.059078, -0.22695254 ) );
// g_convexShapesTransform[ shapeIndex ].setBasis( basis ); // g_convexShapesTransform[ shapeIndex ].setBasis( basis );
// b = false; // b = false;
//} //}
//else //else
//{ //{
// g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 25, 10, 25 ) ); // g_pConvexShapes[ shapeIndex ] = new btBoxShape( btVector3( 25, 10, 25 ) );
// g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 ); // g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 );
// //SimdMatrix3x3 basis( 0.658257, 0.675022, -0.333709, // //btMatrix3x3 basis( 0.658257, 0.675022, -0.333709,
// // -0.333120, 0.658556, 0.675023, // // -0.333120, 0.658556, 0.675023,
// // 0.675314, -0.333120, 0.658256 ); // // 0.675314, -0.333120, 0.658256 );
// g_convexShapesTransform[ shapeIndex ].setIdentity(); // g_convexShapesTransform[ shapeIndex ].setIdentity();
// g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0, -30, 0/*0.326090, -0.667531, 0.214331*/ ) ); // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0, -30, 0/*0.326090, -0.667531, 0.214331*/ ) );
// //g_convexShapesTransform[ shapeIndex ].setBasis( basis ); // //g_convexShapesTransform[ shapeIndex ].setBasis( basis );
//} //}
//#endif //#endif
g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 1, 1, 1 ) ); g_pConvexShapes[ shapeIndex ] = new btBoxShape( btVector3( 1, 1, 1 ) );
g_pConvexShapes[ shapeIndex ]->SetMargin( 1e-1 ); g_pConvexShapes[ shapeIndex ]->SetMargin( 1e-1 );
@@ -149,7 +149,7 @@ bool createBoxShape( int shapeIndex )
bool createSphereShape( int shapeIndex ) bool createSphereShape( int shapeIndex )
{ {
g_pConvexShapes[ shapeIndex ] = new SphereShape( g_sphereRadius ); g_pConvexShapes[ shapeIndex ] = new btSphereShape( g_sphereRadius );
g_pConvexShapes[ shapeIndex ]->SetMargin( 1e-1 ); g_pConvexShapes[ shapeIndex ]->SetMargin( 1e-1 );
@@ -160,12 +160,12 @@ bool createSphereShape( int shapeIndex )
//static bool b = true; //static bool b = true;
//if ( b ) //if ( b )
//{ //{
// g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0.001, 0, 0 ) ); // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0.001, 0, 0 ) );
// b = false; // b = false;
//} //}
//else //else
//{ //{
// g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0, 0, 0 ) ); // g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0, 0, 0 ) );
//} //}
//#endif //#endif
@@ -191,15 +191,15 @@ bool calcPenDepth()
{ {
// Ryn Hybrid Pen Depth and EPA if necessary // Ryn Hybrid Pen Depth and EPA if necessary
SimdVector3 v( 1, 0, 0 ); btVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY; btScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f; btScalar delta = 0.f;
const SimdScalar margin = g_pConvexShapes[ 0 ]->GetMargin() + g_pConvexShapes[ 1 ]->GetMargin(); const btScalar margin = g_pConvexShapes[ 0 ]->GetMargin() + g_pConvexShapes[ 1 ]->GetMargin();
const SimdScalar marginSqrd = margin * margin; const btScalar marginSqrd = margin * margin;
SimdScalar maxRelErrorSqrd = 1e-3 * 1e-3; btScalar maxRelErrorSqrd = 1e-3 * 1e-3;
simplexSolver.reset(); simplexSolver.reset();
@@ -207,16 +207,16 @@ bool calcPenDepth()
{ {
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" ); assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis(); btVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis();
SimdVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis(); btVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis();
SimdVector3 pInA = g_pConvexShapes[ 0 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA ); btVector3 pInA = g_pConvexShapes[ 0 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = g_pConvexShapes[ 1 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB ); btVector3 qInB = g_pConvexShapes[ 1 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA ); btPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA );
SimdPoint3 qWorld = g_convexShapesTransform[ 1 ]( qInB ); btPoint3 qWorld = g_convexShapesTransform[ 1 ]( qInB );
SimdVector3 w = pWorld - qWorld; btVector3 w = pWorld - qWorld;
delta = v.dot( w ); delta = v.dot( w );
// potential exit, they don't overlap // potential exit, they don't overlap
@@ -232,7 +232,7 @@ bool calcPenDepth()
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance ); btScalar vLength = sqrt( squaredDistance );
g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength );
g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
@@ -249,7 +249,7 @@ bool calcPenDepth()
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance ); btScalar vLength = sqrt( squaredDistance );
g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength );
g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
@@ -257,7 +257,7 @@ bool calcPenDepth()
return true; return true;
} }
SimdScalar previousSquaredDistance = squaredDistance; btScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2(); squaredDistance = v.length2();
//are we getting any closer ? //are we getting any closer ?
@@ -269,7 +269,7 @@ bool calcPenDepth()
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance ); btScalar vLength = sqrt( squaredDistance );
g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength );
g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
@@ -313,7 +313,7 @@ void drawShape( int shapeIndex )
if ( g_pConvexShapes[ shapeIndex ]->GetShapeType() == BOX_SHAPE_PROXYTYPE ) if ( g_pConvexShapes[ shapeIndex ]->GetShapeType() == BOX_SHAPE_PROXYTYPE )
{ {
glutWireCube( ( ( BoxShape* ) g_pConvexShapes[ shapeIndex ] )->GetHalfExtents().x() * 2 ); glutWireCube( ( ( btBoxShape* ) g_pConvexShapes[ shapeIndex ] )->GetHalfExtents().x() * 2 );
} }
else if ( g_pConvexShapes[ shapeIndex ]->GetShapeType() == SPHERE_SHAPE_PROXYTYPE ) else if ( g_pConvexShapes[ shapeIndex ]->GetShapeType() == SPHERE_SHAPE_PROXYTYPE )
{ {
@@ -349,10 +349,10 @@ void clientMoveAndDisplay()
{ {
if ( !g_pauseAnim ) if ( !g_pauseAnim )
{ {
SimdMatrix3x3 rot; btMatrix3x3 rot;
rot.setEulerZYX( g_animAngle * 0.05, g_animAngle * 0.05, g_animAngle * 0.05 ); rot.setEulerZYX( g_animAngle * 0.05, g_animAngle * 0.05, g_animAngle * 0.05 );
SimdTransform t; btTransform t;
t.setIdentity(); t.setIdentity();
t.setBasis( rot ); t.setBasis( rot );
@@ -419,11 +419,11 @@ void clientKeyboard(unsigned char key, int x, int y)
else if ( key == 'T' || key == 't' ) else if ( key == 'T' || key == 't' )
{ {
#ifdef DEBUG_ME #ifdef DEBUG_ME
SimdVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin(); btVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin();
SimdVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin(); btVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin();
SimdMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis(); btMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis();
SimdMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis(); btMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis();
FILE* fp = 0; FILE* fp = 0;

62
Demos/ForkLiftDemo/ForkLiftDemo.cpp
View File

@@ -27,8 +27,8 @@ subject to the following restrictions:
#include "PHY_IVehicle.h" #include "PHY_IVehicle.h"
#include "ParallelIslandDispatcher.h" #include "ParallelIslandDispatcher.h"
#include "LinearMath/GenQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
@@ -50,7 +50,7 @@ const int maxOverlap = 65535;
DefaultMotionState wheelMotionState[4]; DefaultMotionState wheelMotionState[4];
///PHY_IVehicle is the interface behind the constraint that implements the raycast vehicle (WrapperVehicle which holds a RaycastVehicle) ///PHY_IVehicle is the interface behind the constraint that implements the raycast vehicle (WrapperVehicle which holds a btRaycastVehicle)
///notice that for higher-quality slow-moving vehicles, another approach might be better ///notice that for higher-quality slow-moving vehicles, another approach might be better
///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts ///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
PHY_IVehicle* gVehicleConstraint=0; PHY_IVehicle* gVehicleConstraint=0;
@@ -66,9 +66,9 @@ float suspensionStiffness = 10.f;
float suspensionDamping = 1.3f; float suspensionDamping = 1.3f;
float suspensionCompression = 2.4f; float suspensionCompression = 2.4f;
float rollInfluence = 0.1f; float rollInfluence = 0.1f;
SimdVector3 wheelDirectionCS0(0,-1,0); btVector3 wheelDirectionCS0(0,-1,0);
SimdVector3 wheelAxleCS(1,0,0); btVector3 wheelAxleCS(1,0,0);
SimdScalar suspensionRestLength(0.6); btScalar suspensionRestLength(0.6);
#define CUBE_HALF_EXTENTS 1 #define CUBE_HALF_EXTENTS 1
@@ -97,20 +97,20 @@ m_cameraHeight(4.f),
m_minCameraDistance(3.f), m_minCameraDistance(3.f),
m_maxCameraDistance(10.f) m_maxCameraDistance(10.f)
{ {
m_cameraPosition = SimdVector3(30,30,30); m_cameraPosition = btVector3(30,30,30);
} }
void ForkLiftDemo::setupPhysics() void ForkLiftDemo::setupPhysics()
{ {
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
//ParallelIslandDispatcher* dispatcher2 = new ParallelIslandDispatcher(); //ParallelIslandDispatcher* dispatcher2 = new ParallelIslandDispatcher();
SimdVector3 worldAabbMin(-30000,-30000,-30000); btVector3 worldAabbMin(-30000,-30000,-30000);
SimdVector3 worldAabbMax(30000,30000,30000); btVector3 worldAabbMax(30000,30000,30000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies); btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
//OverlappingPairCache* broadphase = new SimpleBroadphase(maxProxies,maxOverlap); //OverlappingPairCache* broadphase = new btSimpleBroadphase(maxProxies,maxOverlap);
#ifdef USE_PARALLEL_DISPATCHER #ifdef USE_PARALLEL_DISPATCHER
m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase); m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase);
@@ -124,7 +124,7 @@ void ForkLiftDemo::setupPhysics()
m_physicsEnvironmentPtr->setGravity(0,-10,0);//0,0);//-10,0); m_physicsEnvironmentPtr->setGravity(0,-10,0);//0,0);//-10,0);
int i; int i;
CollisionShape* groundShape = new BoxShape(SimdVector3(50,3,50)); btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
#define USE_TRIMESH_GROUND 1 #define USE_TRIMESH_GROUND 1
#ifdef USE_TRIMESH_GROUND #ifdef USE_TRIMESH_GROUND
@@ -133,7 +133,7 @@ void ForkLiftDemo::setupPhysics()
const float TRIANGLE_SIZE=20.f; const float TRIANGLE_SIZE=20.f;
//create a triangle-mesh ground //create a triangle-mesh ground
int vertStride = sizeof(SimdVector3); int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int); int indexStride = 3*sizeof(int);
const int NUM_VERTS_X = 50; const int NUM_VERTS_X = 50;
@@ -142,7 +142,7 @@ const float TRIANGLE_SIZE=20.f;
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
SimdVector3* gVertices = new SimdVector3[totalVerts]; btVector3* gVertices = new btVector3[totalVerts];
int* gIndices = new int[totalTriangles*3]; int* gIndices = new int[totalTriangles*3];
@@ -170,25 +170,25 @@ const float TRIANGLE_SIZE=20.f;
} }
} }
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices, gIndices,
indexStride, indexStride,
totalVerts,(float*) &gVertices[0].x(),vertStride); totalVerts,(float*) &gVertices[0].x(),vertStride);
groundShape = new BvhTriangleMeshShape(indexVertexArrays); groundShape = new btBvhTriangleMeshShape(indexVertexArrays);
#endif // #endif //
SimdTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
tr.setOrigin(SimdVector3(0,-20.f,0)); tr.setOrigin(btVector3(0,-20.f,0));
//create ground object //create ground object
LocalCreatePhysicsObject(false,0,tr,groundShape); LocalCreatePhysicsObject(false,0,tr,groundShape);
CollisionShape* chassisShape = new BoxShape(SimdVector3(1.f,0.5f,2.f)); btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
tr.setOrigin(SimdVector3(0,0.f,0)); tr.setOrigin(btVector3(0,0.f,0));
m_carChassis = LocalCreatePhysicsObject(true,800,tr,chassisShape); m_carChassis = LocalCreatePhysicsObject(true,800,tr,chassisShape);
@@ -211,8 +211,8 @@ const float TRIANGLE_SIZE=20.f;
gVehicleConstraint = m_physicsEnvironmentPtr->getVehicleConstraint(constraintId); gVehicleConstraint = m_physicsEnvironmentPtr->getVehicleConstraint(constraintId);
SimdVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius); btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
RaycastVehicle::VehicleTuning tuning; btRaycastVehicle::btVehicleTuning tuning;
bool isFrontWheel=true; bool isFrontWheel=true;
int rightIndex = 0; int rightIndex = 0;
int upIndex = 1; int upIndex = 1;
@@ -224,18 +224,18 @@ const float TRIANGLE_SIZE=20.f;
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
connectionPointCS0 = SimdVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
gVehicleConstraint->AddWheel(&wheelMotionState[1], gVehicleConstraint->AddWheel(&wheelMotionState[1],
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
connectionPointCS0 = SimdVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius);
isFrontWheel = false; isFrontWheel = false;
gVehicleConstraint->AddWheel(&wheelMotionState[2], gVehicleConstraint->AddWheel(&wheelMotionState[2],
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
connectionPointCS0 = SimdVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius); connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius);
gVehicleConstraint->AddWheel(&wheelMotionState[3], gVehicleConstraint->AddWheel(&wheelMotionState[3],
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
@@ -280,8 +280,8 @@ void ForkLiftDemo::renderme()
float m[16]; float m[16];
int i; int i;
CylinderShapeX wheelShape(SimdVector3(wheelWidth,wheelRadius,wheelRadius)); btCylinderShapeX wheelShape(btVector3(wheelWidth,wheelRadius,wheelRadius));
SimdVector3 wheelColor(1,0,0); btVector3 wheelColor(1,0,0);
for (i=0;i<4;i++) for (i=0;i<4;i++)
{ {
@@ -318,14 +318,14 @@ void ForkLiftDemo::clientMoveAndDisplay()
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("render"); btProfiler::beginBlock("render");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
renderme(); renderme();
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("render"); btProfiler::endBlock("render");
#endif #endif
glFlush(); glFlush();
glutSwapBuffers(); glutSwapBuffers();
@@ -419,7 +419,7 @@ void ForkLiftDemo::updateCamera()
//interpolate the camera height //interpolate the camera height
m_cameraPosition[1] = (15.0*m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0; m_cameraPosition[1] = (15.0*m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0;
SimdVector3 camToObject = m_cameraTargetPosition - m_cameraPosition; btVector3 camToObject = m_cameraTargetPosition - m_cameraPosition;
//keep distance between min and max distance //keep distance between min and max distance
float cameraDistance = camToObject.length(); float cameraDistance = camToObject.length();

70
Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp
View File

@@ -21,8 +21,8 @@
///Low level demo, doesn't include btBulletCollisionCommon.h ///Low level demo, doesn't include btBulletCollisionCommon.h
#include "LinearMath/SimdQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
@@ -49,9 +49,9 @@ const int maxNumObjects = 4;
const int numObjects = 2; const int numObjects = 2;
PolyhedralConvexShape* shapePtr[maxNumObjects]; btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform tr[numObjects]; btTransform tr[numObjects];
int screenWidth = 640; int screenWidth = 640;
int screenHeight = 480; int screenHeight = 480;
void DrawRasterizerLine(float const* , float const*, int) void DrawRasterizerLine(float const* , float const*, int)
@@ -73,15 +73,15 @@ int main(int argc,char** argv)
void LinearConvexCastDemo::initPhysics() void LinearConvexCastDemo::initPhysics()
{ {
setCameraDistance(30.f); setCameraDistance(30.f);
tr[0].setOrigin(SimdVector3(0,0,0)); tr[0].setOrigin(btVector3(0,0,0));
tr[1].setOrigin(SimdVector3(0,10,0)); tr[1].setOrigin(btVector3(0,10,0));
SimdMatrix3x3 basisA; btMatrix3x3 basisA;
basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f, basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
-0.00029313788f,0.99753088f,0.070228584f, -0.00029313788f,0.99753088f,0.070228584f,
-0.00089153741f,-0.070228823f,0.99753052f); -0.00089153741f,-0.070228823f,0.99753052f);
SimdMatrix3x3 basisB; btMatrix3x3 basisB;
basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f, basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f,
4.4865495e-018f,0.97979438f,0.20000751f, 4.4865495e-018f,0.97979438f,0.20000751f,
4.4410586e-017f,-0.20000751f,0.97979438f); 4.4410586e-017f,-0.20000751f,0.97979438f);
@@ -91,25 +91,25 @@ void LinearConvexCastDemo::initPhysics()
SimdVector3 boxHalfExtentsA(0.2,4,4); btVector3 boxHalfExtentsA(0.2,4,4);
SimdVector3 boxHalfExtentsB(6,6,6); btVector3 boxHalfExtentsB(6,6,6);
BoxShape* boxA = new BoxShape(boxHalfExtentsA); btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
/* BU_Simplex1to4 boxB; /* btBU_Simplex1to4 boxB;
boxB.AddVertex(SimdPoint3(-5,0,-5)); boxB.AddVertex(btPoint3(-5,0,-5));
boxB.AddVertex(SimdPoint3(5,0,-5)); boxB.AddVertex(btPoint3(5,0,-5));
boxB.AddVertex(SimdPoint3(0,0,5)); boxB.AddVertex(btPoint3(0,0,5));
boxB.AddVertex(SimdPoint3(0,5,0)); boxB.AddVertex(btPoint3(0,5,0));
*/ */
BoxShape* boxB = new BoxShape(boxHalfExtentsB); btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
shapePtr[0] = boxA; shapePtr[0] = boxA;
shapePtr[1] = boxB; shapePtr[1] = boxB;
shapePtr[0]->SetMargin(0.01f); shapePtr[0]->SetMargin(0.01f);
shapePtr[1]->SetMargin(0.01f); shapePtr[1]->SetMargin(0.01f);
SimdTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
} }
@@ -124,9 +124,9 @@ void LinearConvexCastDemo::clientMoveAndDisplay()
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
static VoronoiSimplexSolver sVoronoiSimplexSolver; static btVoronoiSimplexSolver sVoronoiSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver; btSimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver;
bool drawLine= false; bool drawLine= false;
@@ -145,13 +145,13 @@ void LinearConvexCastDemo::displayCallback(void)
for (i=0;i<numObjects;i++) for (i=0;i<numObjects;i++)
{ {
tr[i].getOpenGLMatrix( m ); tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode());
} }
int shapeIndex = 1; int shapeIndex = 1;
SimdQuaternion orn; btQuaternion orn;
orn.setEuler(yaw,pitch,roll); orn.setEuler(yaw,pitch,roll);
tr[shapeIndex].setRotation(orn); tr[shapeIndex].setRotation(orn);
@@ -163,14 +163,14 @@ void LinearConvexCastDemo::displayCallback(void)
yaw += 0.01f; yaw += 0.01f;
} }
SimdVector3 fromA(-25,11,0); btVector3 fromA(-25,11,0);
SimdVector3 toA(15,11,0); btVector3 toA(15,11,0);
SimdQuaternion ornFromA(0.f,0.f,0.f,1.f); btQuaternion ornFromA(0.f,0.f,0.f,1.f);
SimdQuaternion ornToA(0.f,0.f,0.f,1.f); btQuaternion ornToA(0.f,0.f,0.f,1.f);
SimdTransform rayFromWorld(ornFromA,fromA); btTransform rayFromWorld(ornFromA,fromA);
SimdTransform rayToWorld(ornToA,toA); btTransform rayToWorld(ornToA,toA);
tr[0] = rayFromWorld; tr[0] = rayFromWorld;
@@ -191,16 +191,16 @@ void LinearConvexCastDemo::displayCallback(void)
for (i=1;i<numObjects;i++) for (i=1;i<numObjects;i++)
{ {
ContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0); btContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0);
GjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver); btGjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
//BU_CollisionPair (algebraic version) is currently broken, will look into this //BU_CollisionPair (algebraic version) is currently broken, will look into this
//BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]); //BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]);
SubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver); btSubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
gGjkSimplexSolver.reset(); gGjkSimplexSolver.reset();
ConvexCast::CastResult rayResult; btConvexCast::CastResult rayResult;
@@ -208,7 +208,7 @@ void LinearConvexCastDemo::displayCallback(void)
{ {
glDisable(GL_DEPTH_TEST); glDisable(GL_DEPTH_TEST);
SimdVector3 hitPoint; btVector3 hitPoint;
hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction); hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction);
//draw the raycast result //draw the raycast result
@@ -219,12 +219,12 @@ void LinearConvexCastDemo::displayCallback(void)
glEnd(); glEnd();
glEnable(GL_DEPTH_TEST); glEnable(GL_DEPTH_TEST);
SimdTransform toTransWorld; btTransform toTransWorld;
toTransWorld = tr[0]; toTransWorld = tr[0];
toTransWorld.setOrigin(hitPoint); toTransWorld.setOrigin(hitPoint);
toTransWorld.getOpenGLMatrix( m ); toTransWorld.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],btVector3(0,1,1),getDebugMode());
} }

6
Demos/OpenGL/BMF_Api.cpp
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

6
Demos/OpenGL/BMF_Api.h
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

6
Demos/OpenGL/BMF_BitmapFont.cpp
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

6
Demos/OpenGL/BMF_BitmapFont.h
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

6
Demos/OpenGL/BMF_FontData.h
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

6
Demos/OpenGL/BMF_Fonts.h
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

6
Demos/OpenGL/BMF_Settings.h
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

6
Demos/OpenGL/BMF_font_helv10.cpp
View File

@@ -2,7 +2,7 @@
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
* *
* This program is free software; you can redistribute it and/or * This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License * modify it under the terms of the GNU bteral Public License
* as published by the Free Software Foundation; either version 2 * as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender * of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under * Foundation also sells licenses for use in proprietary software under
@@ -12,9 +12,9 @@
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU bteral Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU bteral Public License
* along with this program; if not, write to the Free Software Foundation, * along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
* *

26
Demos/OpenGL/DebugCastResult.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#define DEBUG_CAST_RESULT_H #define DEBUG_CAST_RESULT_H
#include "BulletCollision/NarrowPhaseCollision/btConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btConvexCast.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "GL_ShapeDrawer.h" #include "GL_ShapeDrawer.h"
#ifdef WIN32 #ifdef WIN32
#include <windows.h> #include <windows.h>
@@ -29,16 +29,16 @@ subject to the following restrictions:
#else #else
#include <GL/gl.h> #include <GL/gl.h>
#endif #endif
struct DebugCastResult : public ConvexCast::CastResult struct btDebugCastResult : public btConvexCast::CastResult
{ {
SimdTransform m_fromTrans; btTransform m_fromTrans;
const PolyhedralConvexShape* m_shape; const btPolyhedralConvexShape* m_shape;
SimdVector3 m_linVel; btVector3 m_linVel;
SimdVector3 m_angVel; btVector3 m_angVel;
DebugCastResult(const SimdTransform& fromTrans,const PolyhedralConvexShape* shape, btDebugCastResult(const btTransform& fromTrans,const btPolyhedralConvexShape* shape,
const SimdVector3& linVel,const SimdVector3& angVel) const btVector3& linVel,const btVector3& angVel)
:m_fromTrans(fromTrans), :m_fromTrans(fromTrans),
m_shape(shape), m_shape(shape),
m_linVel(linVel), m_linVel(linVel),
@@ -46,7 +46,7 @@ struct DebugCastResult : public ConvexCast::CastResult
{ {
} }
virtual void DrawCoordSystem(const SimdTransform& tr) virtual void DrawCoordSystem(const btTransform& tr)
{ {
float m[16]; float m[16];
tr.getOpenGLMatrix(m); tr.getOpenGLMatrix(m);
@@ -66,14 +66,14 @@ struct DebugCastResult : public ConvexCast::CastResult
glPopMatrix(); glPopMatrix();
} }
virtual void DebugDraw(SimdScalar fraction) virtual void DebugDraw(btScalar fraction)
{ {
float m[16]; float m[16];
SimdTransform hitTrans; btTransform hitTrans;
SimdTransformUtil::IntegrateTransform(m_fromTrans,m_linVel,m_angVel,fraction,hitTrans); btTransformUtil::IntegrateTransform(m_fromTrans,m_linVel,m_angVel,fraction,hitTrans);
hitTrans.getOpenGLMatrix(m); hitTrans.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,m_shape,SimdVector3(1,0,0),IDebugDraw::DBG_NoDebug); GL_ShapeDrawer::DrawOpenGL(m,m_shape,btVector3(1,0,0),btIDebugDraw::DBG_NoDebug);
} }
}; };

404
Demos/OpenGL/DemoApplication.cpp
View File

@@ -14,7 +14,8 @@ subject to the following restrictions:
*/ */
#include "DemoApplication.h" #include "DemoApplication.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
#include "CcdPhysicsEnvironment.h" #include "CcdPhysicsEnvironment.h"
#include "CcdPhysicsController.h" #include "CcdPhysicsController.h"
@@ -23,21 +24,24 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "GL_ShapeDrawer.h" #include "GL_ShapeDrawer.h"
#include "LinearMath/GenQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "BMF_Api.h" #include "BMF_Api.h"
#include "BulletDynamics/Dynamics/btMassProps.h"
int numObjects = 0; int numObjects = 0;
const int maxNumObjects = 16384; const int maxNumObjects = 16384;
DefaultMotionState ms[maxNumObjects]; DefaultMotionState ms[maxNumObjects];
CcdPhysicsController* physObjects[maxNumObjects]; CcdPhysicsController* physObjects[maxNumObjects];
SimdTransform startTransforms[maxNumObjects]; btTransform startTransforms[maxNumObjects];
CollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use of shapes) btCollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use of shapes)
DemoApplication::DemoApplication() DemoApplication::DemoApplication()
//see IDebugDraw.h for modes //see btIDebugDraw.h for modes
: :
m_physicsEnvironmentPtr(0), m_physicsEnvironmentPtr(0),
m_dynamicsWorld(0),
m_pickConstraint(0),
m_cameraDistance(15.0), m_cameraDistance(15.0),
m_debugMode(0), m_debugMode(0),
m_ele(0.f), m_ele(0.f),
@@ -133,21 +137,21 @@ void DemoApplication::updateCamera() {
float razi = m_azi * 0.01745329251994329547;// rads per deg float razi = m_azi * 0.01745329251994329547;// rads per deg
SimdQuaternion rot(m_cameraUp,razi); btQuaternion rot(m_cameraUp,razi);
SimdVector3 eyePos(0,0,0); btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance; eyePos[m_forwardAxis] = -m_cameraDistance;
SimdVector3 forward(eyePos[0],eyePos[1],eyePos[2]); btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON) if (forward.length2() < SIMD_EPSILON)
{ {
forward.setValue(1.f,0.f,0.f); forward.setValue(1.f,0.f,0.f);
} }
SimdVector3 right = m_cameraUp.cross(forward); btVector3 right = m_cameraUp.cross(forward);
SimdQuaternion roll(right,-rele); btQuaternion roll(right,-rele);
eyePos = SimdMatrix3x3(rot) * SimdMatrix3x3(roll) * eyePos; eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX(); m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY(); m_cameraPosition[1] = eyePos.getY();
@@ -226,70 +230,70 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
case 'x' : zoomOut(); break; case 'x' : zoomOut(); break;
case 'i' : toggleIdle(); break; case 'i' : toggleIdle(); break;
case 'h': case 'h':
if (m_debugMode & IDebugDraw::DBG_NoHelpText) if (m_debugMode & btIDebugDraw::DBG_NoHelpText)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_NoHelpText); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoHelpText);
else else
m_debugMode |= IDebugDraw::DBG_NoHelpText; m_debugMode |= btIDebugDraw::DBG_NoHelpText;
break; break;
case 'w': case 'w':
if (m_debugMode & IDebugDraw::DBG_DrawWireframe) if (m_debugMode & btIDebugDraw::DBG_DrawWireframe)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawWireframe); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawWireframe);
else else
m_debugMode |= IDebugDraw::DBG_DrawWireframe; m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
break; break;
case 'p': case 'p':
if (m_debugMode & IDebugDraw::DBG_ProfileTimings) if (m_debugMode & btIDebugDraw::DBG_ProfileTimings)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_ProfileTimings); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_ProfileTimings);
else else
m_debugMode |= IDebugDraw::DBG_ProfileTimings; m_debugMode |= btIDebugDraw::DBG_ProfileTimings;
break; break;
case 'm': case 'm':
if (m_debugMode & IDebugDraw::DBG_EnableSatComparison) if (m_debugMode & btIDebugDraw::DBG_EnableSatComparison)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_EnableSatComparison); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_EnableSatComparison);
else else
m_debugMode |= IDebugDraw::DBG_EnableSatComparison; m_debugMode |= btIDebugDraw::DBG_EnableSatComparison;
break; break;
case 'n': case 'n':
if (m_debugMode & IDebugDraw::DBG_DisableBulletLCP) if (m_debugMode & btIDebugDraw::DBG_DisableBulletLCP)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DisableBulletLCP); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DisableBulletLCP);
else else
m_debugMode |= IDebugDraw::DBG_DisableBulletLCP; m_debugMode |= btIDebugDraw::DBG_DisableBulletLCP;
break; break;
case 't' : case 't' :
if (m_debugMode & IDebugDraw::DBG_DrawText) if (m_debugMode & btIDebugDraw::DBG_DrawText)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawText); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawText);
else else
m_debugMode |= IDebugDraw::DBG_DrawText; m_debugMode |= btIDebugDraw::DBG_DrawText;
break; break;
case 'y': case 'y':
if (m_debugMode & IDebugDraw::DBG_DrawFeaturesText) if (m_debugMode & btIDebugDraw::DBG_DrawFeaturesText)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawFeaturesText); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawFeaturesText);
else else
m_debugMode |= IDebugDraw::DBG_DrawFeaturesText; m_debugMode |= btIDebugDraw::DBG_DrawFeaturesText;
break; break;
case 'a': case 'a':
if (m_debugMode & IDebugDraw::DBG_DrawAabb) if (m_debugMode & btIDebugDraw::DBG_DrawAabb)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawAabb); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawAabb);
else else
m_debugMode |= IDebugDraw::DBG_DrawAabb; m_debugMode |= btIDebugDraw::DBG_DrawAabb;
break; break;
case 'c' : case 'c' :
if (m_debugMode & IDebugDraw::DBG_DrawContactPoints) if (m_debugMode & btIDebugDraw::DBG_DrawContactPoints)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawContactPoints); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawContactPoints);
else else
m_debugMode |= IDebugDraw::DBG_DrawContactPoints; m_debugMode |= btIDebugDraw::DBG_DrawContactPoints;
break; break;
case 'd' : case 'd' :
if (m_debugMode & IDebugDraw::DBG_NoDeactivation) if (m_debugMode & btIDebugDraw::DBG_NoDeactivation)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_NoDeactivation); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoDeactivation);
else else
m_debugMode |= IDebugDraw::DBG_NoDeactivation; m_debugMode |= btIDebugDraw::DBG_NoDeactivation;
break; break;
@@ -306,10 +310,10 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
break; break;
case '1': case '1':
{ {
if (m_debugMode & IDebugDraw::DBG_EnableCCD) if (m_debugMode & btIDebugDraw::DBG_EnableCCD)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_EnableCCD); m_debugMode = m_debugMode & (~btIDebugDraw::DBG_EnableCCD);
else else
m_debugMode |= IDebugDraw::DBG_EnableCCD; m_debugMode |= btIDebugDraw::DBG_EnableCCD;
break; break;
} }
@@ -380,21 +384,45 @@ void DemoApplication::displayCallback()
void DemoApplication::shootBox(const SimdVector3& destination) void DemoApplication::shootBox(const btVector3& destination)
{ {
if (m_dynamicsWorld)
{
bool isDynamic = true;
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
btVector3 camPos = getCameraPosition();
startTransform.setOrigin(camPos);
btCollisionShape* boxShape = new btBoxShape(btVector3(1.f,1.f,1.f));
btRigidBody* body = this->LocalCreateRigidBody(isDynamic, mass, startTransform,boxShape);
m_dynamicsWorld->AddCollisionObject(body);
btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]);
linVel.normalize();
linVel*=m_ShootBoxInitialSpeed;
body->m_worldTransform.setOrigin(camPos);
body->m_worldTransform.setRotation(btQuaternion(0,0,0,1));
body->setLinearVelocity(linVel);
body->setAngularVelocity(btVector3(0,0,0));
}
if (m_physicsEnvironmentPtr) if (m_physicsEnvironmentPtr)
{ {
bool isDynamic = true; bool isDynamic = true;
float mass = 1.f; float mass = 1.f;
SimdTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
SimdVector3 camPos = getCameraPosition(); btVector3 camPos = getCameraPosition();
startTransform.setOrigin(camPos); startTransform.setOrigin(camPos);
CollisionShape* boxShape = new BoxShape(SimdVector3(1.f,1.f,1.f)); btCollisionShape* boxShape = new btBoxShape(btVector3(1.f,1.f,1.f));
CcdPhysicsController* newBox = LocalCreatePhysicsObject(isDynamic, mass, startTransform,boxShape); CcdPhysicsController* newBox = LocalCreatePhysicsObject(isDynamic, mass, startTransform,boxShape);
SimdVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]); btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]);
linVel.normalize(); linVel.normalize();
linVel*=m_ShootBoxInitialSpeed; linVel*=m_ShootBoxInitialSpeed;
@@ -407,12 +435,12 @@ void DemoApplication::shootBox(const SimdVector3& destination)
int gPickingConstraintId = 0; int gPickingConstraintId = 0;
SimdVector3 gOldPickingPos; btVector3 gOldPickingPos;
float gOldPickingDist = 0.f; float gOldPickingDist = 0.f;
RigidBody* pickedBody = 0;//for deactivation state btRigidBody* pickedBody = 0;//for deactivation state
SimdVector3 DemoApplication::GetRayTo(int x,int y) btVector3 DemoApplication::GetRayTo(int x,int y)
{ {
float top = 1.f; float top = 1.f;
@@ -421,16 +449,16 @@ SimdVector3 DemoApplication::GetRayTo(int x,int y)
float tanFov = (top-bottom)*0.5f / nearPlane; float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov); float fov = 2.0 * atanf (tanFov);
SimdVector3 rayFrom = getCameraPosition(); btVector3 rayFrom = getCameraPosition();
SimdVector3 rayForward = (getCameraTargetPosition()-getCameraPosition()); btVector3 rayForward = (getCameraTargetPosition()-getCameraPosition());
rayForward.normalize(); rayForward.normalize();
float farPlane = 600.f; float farPlane = 600.f;
rayForward*= farPlane; rayForward*= farPlane;
SimdVector3 rightOffset; btVector3 rightOffset;
SimdVector3 vertical = m_cameraUp; btVector3 vertical = m_cameraUp;
SimdVector3 hor; btVector3 hor;
hor = rayForward.cross(vertical); hor = rayForward.cross(vertical);
hor.normalize(); hor.normalize();
vertical = hor.cross(rayForward); vertical = hor.cross(rayForward);
@@ -439,10 +467,10 @@ SimdVector3 DemoApplication::GetRayTo(int x,int y)
float tanfov = tanf(0.5f*fov); float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov; hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov; vertical *= 2.f * farPlane * tanfov;
SimdVector3 rayToCenter = rayFrom + rayForward; btVector3 rayToCenter = rayFrom + rayForward;
SimdVector3 dHor = hor * 1.f/float(m_glutScreenWidth); btVector3 dHor = hor * 1.f/float(m_glutScreenWidth);
SimdVector3 dVert = vertical * 1.f/float(m_glutScreenHeight); btVector3 dVert = vertical * 1.f/float(m_glutScreenHeight);
SimdVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor; rayTo += x * dHor;
rayTo -= y * dVert; rayTo -= y * dVert;
return rayTo; return rayTo;
@@ -454,7 +482,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
//printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y); //printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y);
//button 0, state 0 means left mouse down //button 0, state 0 means left mouse down
SimdVector3 rayTo = GetRayTo(x,y); btVector3 rayTo = GetRayTo(x,y);
switch (button) switch (button)
{ {
@@ -468,8 +496,40 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
}; };
case 1: case 1:
{ {
if (state==0) if (state==0)
{ {
//apply an impulse
if (m_dynamicsWorld)
{
float hit[3];
float normal[3];
btCollisionWorld::ClosestRayResultCallback rayCallback(m_cameraPosition,rayTo);
m_dynamicsWorld->RayTest(m_cameraPosition,rayTo,rayCallback);
if (rayCallback.HasHit())
{
if (rayCallback.m_collisionObject->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)rayCallback.m_collisionObject->m_internalOwner;
if (body)
{
body->SetActivationState(ACTIVE_TAG);
btVector3 impulse = rayTo;
impulse.normalize();
float impulseStrength = 10.f;
impulse *= impulseStrength;
btVector3 relPos = rayCallback.m_hitPointWorld - body->getCenterOfMassPosition();
body->applyImpulse(impulse,relPos);
}
}
}
}
//apply an impulse //apply an impulse
if (m_physicsEnvironmentPtr) if (m_physicsEnvironmentPtr)
{ {
@@ -479,15 +539,15 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
if (hitObj) if (hitObj)
{ {
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj); CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody(); btRigidBody* body = physCtrl->GetRigidBody();
if (body) if (body)
{ {
body->SetActivationState(ACTIVE_TAG); body->SetActivationState(ACTIVE_TAG);
SimdVector3 impulse = rayTo; btVector3 impulse = rayTo;
impulse.normalize(); impulse.normalize();
float impulseStrength = 10.f; float impulseStrength = 10.f;
impulse *= impulseStrength; impulse *= impulseStrength;
SimdVector3 relPos( btVector3 relPos(
hit[0] - body->getCenterOfMassPosition().getX(), hit[0] - body->getCenterOfMassPosition().getX(),
hit[1] - body->getCenterOfMassPosition().getY(), hit[1] - body->getCenterOfMassPosition().getY(),
hit[2] - body->getCenterOfMassPosition().getZ()); hit[2] - body->getCenterOfMassPosition().getZ());
@@ -509,6 +569,48 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
{ {
if (state==0) if (state==0)
{ {
//add a point to point constraint for picking
if (m_dynamicsWorld)
{
float hit[3];
float normal[3];
btCollisionWorld::ClosestRayResultCallback rayCallback(m_cameraPosition,rayTo);
m_dynamicsWorld->RayTest(m_cameraPosition,rayTo,rayCallback);
if (rayCallback.HasHit())
{
if (rayCallback.m_collisionObject->m_internalOwner)
{
btRigidBody* body = (btRigidBody*)rayCallback.m_collisionObject->m_internalOwner;
if (body && !body->IsStatic())
{
pickedBody = body;
pickedBody->SetActivationState(DISABLE_DEACTIVATION);
btVector3 pickPos = rayCallback.m_hitPointWorld;
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
m_dynamicsWorld->addConstraint(p2p);
m_pickConstraint = p2p;
//save mouse position for dragging
gOldPickingPos = rayTo;
btVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]);
gOldPickingDist = (pickPos-eyePos).length();
//very weak constraint for picking
p2p->m_setting.m_tau = 0.1f;
}
}
}
}
//add a point to point constraint for picking //add a point to point constraint for picking
if (m_physicsEnvironmentPtr) if (m_physicsEnvironmentPtr)
{ {
@@ -519,16 +621,16 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
{ {
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj); CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody(); btRigidBody* body = physCtrl->GetRigidBody();
if (body && !body->IsStatic()) if (body && !body->IsStatic())
{ {
pickedBody = body; pickedBody = body;
pickedBody->SetActivationState(DISABLE_DEACTIVATION); pickedBody->SetActivationState(DISABLE_DEACTIVATION);
SimdVector3 pickPos(hit[0],hit[1],hit[2]); btVector3 pickPos(hit[0],hit[1],hit[2]);
SimdVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
gPickingConstraintId = m_physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT, gPickingConstraintId = m_physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT,
localPivot.getX(), localPivot.getX(),
@@ -541,11 +643,11 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
gOldPickingPos = rayTo; gOldPickingPos = rayTo;
SimdVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]); btVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]);
gOldPickingDist = (pickPos-eyePos).length(); gOldPickingDist = (pickPos-eyePos).length();
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId)); btPoint2PointConstraint* p2p = static_cast<btPoint2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p) if (p2p)
{ {
//very weak constraint for picking //very weak constraint for picking
@@ -556,6 +658,18 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
} }
} else } else
{ {
if (m_pickConstraint && m_dynamicsWorld)
{
m_dynamicsWorld->removeConstraint(m_pickConstraint);
delete m_pickConstraint;
//printf("removed constraint %i",gPickingConstraintId);
m_pickConstraint = 0;
pickedBody->ForceActivationState(ACTIVE_TAG);
pickedBody->m_deactivationTime = 0.f;
pickedBody = 0;
}
if (gPickingConstraintId && m_physicsEnvironmentPtr) if (gPickingConstraintId && m_physicsEnvironmentPtr)
{ {
m_physicsEnvironmentPtr->removeConstraint(gPickingConstraintId); m_physicsEnvironmentPtr->removeConstraint(gPickingConstraintId);
@@ -582,23 +696,43 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
void DemoApplication::mouseMotionFunc(int x,int y) void DemoApplication::mouseMotionFunc(int x,int y)
{ {
if (m_pickConstraint)
{
//move the constraint pivot
btPoint2PointConstraint* p2p = static_cast<btPoint2PointConstraint*>(m_pickConstraint);
if (p2p)
{
//keep it at the same picking distance
btVector3 newRayTo = GetRayTo(x,y);
btVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]);
btVector3 dir = newRayTo-eyePos;
dir.normalize();
dir *= gOldPickingDist;
btVector3 newPos = eyePos + dir;
p2p->SetPivotB(newPos);
}
}
if (gPickingConstraintId && m_physicsEnvironmentPtr) if (gPickingConstraintId && m_physicsEnvironmentPtr)
{ {
//move the constraint pivot //move the constraint pivot
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId)); btPoint2PointConstraint* p2p = static_cast<btPoint2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p) if (p2p)
{ {
//keep it at the same picking distance //keep it at the same picking distance
SimdVector3 newRayTo = GetRayTo(x,y); btVector3 newRayTo = GetRayTo(x,y);
SimdVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]); btVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]);
SimdVector3 dir = newRayTo-eyePos; btVector3 dir = newRayTo-eyePos;
dir.normalize(); dir.normalize();
dir *= gOldPickingDist; dir *= gOldPickingDist;
SimdVector3 newPos = eyePos + dir; btVector3 newPos = eyePos + dir;
p2p->SetPivotB(newPos); p2p->SetPivotB(newPos);
} }
@@ -606,16 +740,40 @@ void DemoApplication::mouseMotionFunc(int x,int y)
} }
btRigidBody* DemoApplication::LocalCreateRigidBody(bool isDynamic, float mass, const btTransform& startTransform,btCollisionShape* shape)
{
btVector3 localInertia(0,0,0);
if (isDynamic)
shape->CalculateLocalInertia(mass,localInertia);
btMassProps massProps(0.f,localInertia);
btRigidBody* body = new btRigidBody(massProps);
body->m_collisionShape = shape;
body->m_worldTransform = startTransform;
body->m_internalOwner = body;
body->setMassProps( mass, localInertia);
body->setGravity(btVector3(0,-9.8f,0));
if (!isDynamic)
{
body->m_collisionFlags = btCollisionObject::isStatic;//??
}
return body;
}
///Very basic import ///Very basic import
CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape) CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic, float mass, const btTransform& startTransform,btCollisionShape* shape)
{ {
startTransforms[numObjects] = startTransform; startTransforms[numObjects] = startTransform;
CcdConstructionInfo ccdObjectCi; btCcdConstructionInfo ccdObjectCi;
ccdObjectCi.m_friction = 0.5f; ccdObjectCi.m_friction = 0.5f;
SimdTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
int i = numObjects; int i = numObjects;
@@ -623,20 +781,20 @@ CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic,
gShapePtr[i] = shape; gShapePtr[i] = shape;
gShapePtr[i]->SetMargin(0.05f); gShapePtr[i]->SetMargin(0.05f);
SimdQuaternion orn = startTransform.getRotation(); btQuaternion orn = startTransform.getRotation();
ms[i].setWorldOrientation(orn[0],orn[1],orn[2],orn[3]); ms[i].setWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
ms[i].setWorldPosition(startTransform.getOrigin().getX(),startTransform.getOrigin().getY(),startTransform.getOrigin().getZ()); ms[i].setWorldPosition(startTransform.getOrigin().getX(),startTransform.getOrigin().getY(),startTransform.getOrigin().getZ());
ccdObjectCi.m_MotionState = &ms[i]; ccdObjectCi.m_MotionState = &ms[i];
ccdObjectCi.m_gravity = SimdVector3(0,-9.8,0); ccdObjectCi.m_gravity = btVector3(0,-9.8,0);
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); ccdObjectCi.m_localInertiaTensor =btVector3(0,0,0);
if (!isDynamic) if (!isDynamic)
{ {
ccdObjectCi.m_mass = 0.f; ccdObjectCi.m_mass = 0.f;
ccdObjectCi.m_collisionFlags = CollisionObject::isStatic; ccdObjectCi.m_collisionFlags = btCollisionObject::isStatic;
ccdObjectCi.m_collisionFilterGroup = CcdConstructionInfo::StaticFilter; ccdObjectCi.m_collisionFilterGroup = btCcdConstructionInfo::StaticFilter;
ccdObjectCi.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::StaticFilter; ccdObjectCi.m_collisionFilterMask = btCcdConstructionInfo::AllFilter ^ btCcdConstructionInfo::StaticFilter;
} }
else else
{ {
@@ -644,7 +802,7 @@ CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic,
ccdObjectCi.m_collisionFlags = 0; ccdObjectCi.m_collisionFlags = 0;
} }
SimdVector3 localInertia(0.f,0.f,0.f); btVector3 localInertia(0.f,0.f,0.f);
if (isDynamic) if (isDynamic)
{ {
@@ -677,10 +835,50 @@ void DemoApplication::renderme()
float m[16]; float m[16];
if (m_dynamicsWorld)
{
int numObjects = m_dynamicsWorld->GetNumCollisionObjects();
btVector3 wireColor(1,0,0);
for (int i=0;i<numObjects;i++)
{
btCollisionObject* colObj = m_dynamicsWorld->GetCollisionObjectArray()[i];
colObj->m_worldTransform.getOpenGLMatrix(m);
btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if (i & 1)
{
wireColor = btVector3(0.f,0.0f,1.f);
}
///color differently for active, sleeping, wantsdeactivation states
if (colObj->GetActivationState() == 1) //active
{
if (i & 1)
{
wireColor += btVector3 (1.f,0.f,0.f);
} else
{
wireColor += btVector3 (.5f,0.f,0.f);
}
}
if (colObj->GetActivationState() == 2) //ISLAND_SLEEPING
{
if (i & 1)
{
wireColor += btVector3 (0.f,1.f, 0.f);
} else
{
wireColor += btVector3 (0.f,0.5f,0.f);
}
}
GL_ShapeDrawer::DrawOpenGL(m,colObj->m_collisionShape,wireColor,getDebugMode());
}
}
if (m_physicsEnvironmentPtr) if (m_physicsEnvironmentPtr)
{ {
if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP) if (getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP)
{ {
//don't use Bullet, use quickstep //don't use Bullet, use quickstep
m_physicsEnvironmentPtr->setSolverType(0); m_physicsEnvironmentPtr->setSolverType(0);
@@ -690,7 +888,7 @@ void DemoApplication::renderme()
m_physicsEnvironmentPtr->setSolverType(1); m_physicsEnvironmentPtr->setSolverType(1);
} }
if (getDebugMode() & IDebugDraw::DBG_EnableCCD) if (getDebugMode() & btIDebugDraw::DBG_EnableCCD)
{ {
m_physicsEnvironmentPtr->setCcdMode(3); m_physicsEnvironmentPtr->setCcdMode(3);
} else } else
@@ -699,7 +897,7 @@ void DemoApplication::renderme()
} }
bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison); bool isSatEnabled = (getDebugMode() & btIDebugDraw::DBG_EnableSatComparison);
m_physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled); m_physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);
@@ -711,34 +909,34 @@ void DemoApplication::renderme()
{ {
CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i); CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i);
RigidBody* body = ctrl->GetRigidBody(); btRigidBody* body = ctrl->GetRigidBody();
body->m_worldTransform.getOpenGLMatrix( m ); body->m_worldTransform.getOpenGLMatrix( m );
SimdVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if (i & 1) if (i & 1)
{ {
wireColor = SimdVector3(0.f,0.0f,1.f); wireColor = btVector3(0.f,0.0f,1.f);
} }
///color differently for active, sleeping, wantsdeactivation states ///color differently for active, sleeping, wantsdeactivation states
if (ctrl->GetRigidBody()->GetActivationState() == 1) //active if (ctrl->GetRigidBody()->GetActivationState() == 1) //active
{ {
if (i & 1) if (i & 1)
{ {
wireColor += SimdVector3 (1.f,0.f,0.f); wireColor += btVector3 (1.f,0.f,0.f);
} else } else
{ {
wireColor += SimdVector3 (.5f,0.f,0.f); wireColor += btVector3 (.5f,0.f,0.f);
} }
} }
if (ctrl->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING if (ctrl->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{ {
if (i & 1) if (i & 1)
{ {
wireColor += SimdVector3 (0.f,1.f, 0.f); wireColor += btVector3 (0.f,1.f, 0.f);
} else } else
{ {
wireColor += SimdVector3 (0.f,0.5f,0.f); wireColor += btVector3 (0.f,0.5f,0.f);
} }
} }
@@ -747,7 +945,7 @@ void DemoApplication::renderme()
ctrl->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug); ctrl->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug);
float vec[16]; float vec[16];
SimdTransform ident; btTransform ident;
ident.setIdentity(); ident.setIdentity();
ident.getOpenGLMatrix(vec); ident.getOpenGLMatrix(vec);
@@ -759,7 +957,7 @@ void DemoApplication::renderme()
} }
if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText)) if (!(getDebugMode() & btIDebugDraw::DBG_NoHelpText))
{ {
float xOffset = 10.f; float xOffset = 10.f;
@@ -774,15 +972,15 @@ void DemoApplication::renderme()
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings) if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
{ {
static int counter = 0; static int counter = 0;
counter++; counter++;
std::map<std::string, hidden::ProfileBlock*>::iterator iter; std::map<std::string, hidden::ProfileBlock*>::iterator iter;
for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter) for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter)
{ {
char blockTime[128]; char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT)); sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0); glRasterPos3f(xOffset,yStart,0);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr; yStart += yIncr;
@@ -790,7 +988,7 @@ void DemoApplication::renderme()
} }
} }
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
//profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT); //profiling << btProfiler::createStatsString(btProfiler::BLOCK_TOTAL_PERCENT);
//<< std::endl; //<< std::endl;
@@ -836,9 +1034,9 @@ void DemoApplication::renderme()
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr; yStart += yIncr;
bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP); bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD); bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD);
glRasterPos3f(xOffset,yStart,0); glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"m Bullet GJK = %i",!isSatEnabled); sprintf(buf,"m Bullet GJK = %i",!isSatEnabled);

39
Demos/OpenGL/DemoApplication.h
View File

@@ -35,14 +35,16 @@ subject to the following restrictions:
#include <math.h> #include <math.h>
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/SimdMatrix3x3.h" #include "LinearMath/btMatrix3x3.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
class CcdPhysicsEnvironment;
class CcdPhysicsController;
class CollisionShape;
class CcdPhysicsEnvironment;
class CcdPhysicsController;
class btCollisionShape;
class btDynamicsWorld;
class btRigidBody;
class btTypedConstraint;
class DemoApplication class DemoApplication
{ {
@@ -53,18 +55,21 @@ class DemoApplication
///this is the most important class ///this is the most important class
CcdPhysicsEnvironment* m_physicsEnvironmentPtr; CcdPhysicsEnvironment* m_physicsEnvironmentPtr;
btDynamicsWorld* m_dynamicsWorld;
btTypedConstraint* m_pickConstraint;
float m_cameraDistance; float m_cameraDistance;
int m_debugMode; int m_debugMode;
float m_ele; float m_ele;
float m_azi; float m_azi;
SimdVector3 m_cameraPosition; btVector3 m_cameraPosition;
SimdVector3 m_cameraTargetPosition;//look at btVector3 m_cameraTargetPosition;//look at
float m_scaleBottom; float m_scaleBottom;
float m_scaleFactor; float m_scaleFactor;
SimdVector3 m_cameraUp; btVector3 m_cameraUp;
int m_forwardAxis; int m_forwardAxis;
int m_glutScreenWidth; int m_glutScreenWidth;
@@ -99,7 +104,7 @@ class DemoApplication
return m_physicsEnvironmentPtr; return m_physicsEnvironmentPtr;
} }
void setCameraUp(const SimdVector3& camUp) void setCameraUp(const btVector3& camUp)
{ {
m_cameraUp = camUp; m_cameraUp = camUp;
} }
@@ -114,11 +119,11 @@ class DemoApplication
virtual void updateCamera(); virtual void updateCamera();
SimdVector3 getCameraPosition() btVector3 getCameraPosition()
{ {
return m_cameraPosition; return m_cameraPosition;
} }
SimdVector3 getCameraTargetPosition() btVector3 getCameraTargetPosition()
{ {
return m_cameraTargetPosition; return m_cameraTargetPosition;
} }
@@ -135,11 +140,13 @@ class DemoApplication
virtual void clientResetScene() =0 ; virtual void clientResetScene() =0 ;
///Demo functions ///Demo functions
void shootBox(const SimdVector3& destination); void shootBox(const btVector3& destination);
SimdVector3 GetRayTo(int x,int y); btVector3 GetRayTo(int x,int y);
CcdPhysicsController* LocalCreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape); CcdPhysicsController* LocalCreatePhysicsObject(bool isDynamic, float mass, const btTransform& startTransform,btCollisionShape* shape);
btRigidBody* LocalCreateRigidBody(bool isDynamic, float mass, const btTransform& startTransform,btCollisionShape* shape);
///callback methods by glut ///callback methods by glut

12
Demos/OpenGL/GLDebugDrawer.cpp
View File

@@ -1,6 +1,6 @@
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
#ifdef WIN32 //needed for glut.h #ifdef WIN32 //needed for glut.h
#include <windows.h> #include <windows.h>
@@ -22,7 +22,7 @@ GLDebugDrawer::GLDebugDrawer()
{ {
} }
void GLDebugDrawer::DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color) void GLDebugDrawer::DrawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{ {
if (m_debugMode > 0) if (m_debugMode > 0)
{ {
@@ -40,12 +40,12 @@ void GLDebugDrawer::SetDebugMode(int debugMode)
} }
void GLDebugDrawer::DrawContactPoint(const SimdVector3& pointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color) void GLDebugDrawer::DrawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,float distance,int lifeTime,const btVector3& color)
{ {
if (m_debugMode & IDebugDraw::DBG_DrawContactPoints) if (m_debugMode & btIDebugDraw::DBG_DrawContactPoints)
{ {
SimdVector3 to=pointOnB+normalOnB*distance; btVector3 to=pointOnB+normalOnB*distance;
const SimdVector3&from = pointOnB; const btVector3&from = pointOnB;
glBegin(GL_LINES); glBegin(GL_LINES);
glColor3f(color.getX(), color.getY(), color.getZ()); glColor3f(color.getX(), color.getY(), color.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ()); glVertex3d(from.getX(), from.getY(), from.getZ());

8
Demos/OpenGL/GLDebugDrawer.h
View File

@@ -1,11 +1,11 @@
#ifndef GL_DEBUG_DRAWER_H #ifndef GL_DEBUG_DRAWER_H
#define GL_DEBUG_DRAWER_H #define GL_DEBUG_DRAWER_H
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
class GLDebugDrawer : public IDebugDraw class GLDebugDrawer : public btIDebugDraw
{ {
int m_debugMode; int m_debugMode;
@@ -13,9 +13,9 @@ public:
GLDebugDrawer(); GLDebugDrawer();
virtual void DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color); virtual void DrawLine(const btVector3& from,const btVector3& to,const btVector3& color);
virtual void DrawContactPoint(const SimdVector3& PointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color); virtual void DrawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,float distance,int lifeTime,const btVector3& color);
virtual void SetDebugMode(int debugMode); virtual void SetDebugMode(int debugMode);

62
Demos/OpenGL/GL_ShapeDrawer.cpp
View File

@@ -39,7 +39,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
//for debugmodes //for debugmodes
#include "BMF_Api.h" #include "BMF_Api.h"
#include <stdio.h> //printf debugging #include <stdio.h> //printf debugging
@@ -63,11 +63,11 @@ void GL_ShapeDrawer::DrawCoordSystem() {
class GlDrawcallback : public TriangleCallback class GlDrawcallback : public btTriangleCallback
{ {
public: public:
virtual void ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex) virtual void ProcessTriangle(btVector3* triangle,int partId, int triangleIndex)
{ {
glBegin(GL_LINES); glBegin(GL_LINES);
glColor3f(1, 0, 0); glColor3f(1, 0, 0);
@@ -84,10 +84,10 @@ public:
} }
}; };
class TriangleGlDrawcallback : public InternalTriangleIndexCallback class TriangleGlDrawcallback : public btInternalTriangleIndexCallback
{ {
public: public:
virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex) virtual void InternalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{ {
glBegin(GL_TRIANGLES);//LINES); glBegin(GL_TRIANGLES);//LINES);
glColor3f(1, 0, 0); glColor3f(1, 0, 0);
@@ -104,7 +104,7 @@ public:
}; };
void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const SimdVector3& color,int debugMode) void GL_ShapeDrawer::DrawOpenGL(float* m, const btCollisionShape* shape, const btVector3& color,int debugMode)
{ {
@@ -113,11 +113,11 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
if (shape->GetShapeType() == COMPOUND_SHAPE_PROXYTYPE) if (shape->GetShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{ {
const CompoundShape* compoundShape = static_cast<const CompoundShape*>(shape); const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->GetNumChildShapes()-1;i>=0;i--) for (int i=compoundShape->GetNumChildShapes()-1;i>=0;i--)
{ {
SimdTransform childTrans = compoundShape->GetChildTransform(i); btTransform childTrans = compoundShape->GetChildTransform(i);
const CollisionShape* colShape = compoundShape->GetChildShape(i); const btCollisionShape* colShape = compoundShape->GetChildShape(i);
float childMat[16]; float childMat[16];
childTrans.getOpenGLMatrix(childMat); childTrans.getOpenGLMatrix(childMat);
DrawOpenGL(childMat,colShape,color,debugMode); DrawOpenGL(childMat,colShape,color,debugMode);
@@ -135,14 +135,14 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
bool useWireframeFallback = true; bool useWireframeFallback = true;
if (!(debugMode & IDebugDraw::DBG_DrawWireframe)) if (!(debugMode & btIDebugDraw::DBG_DrawWireframe))
{ {
switch (shape->GetShapeType()) switch (shape->GetShapeType())
{ {
case BOX_SHAPE_PROXYTYPE: case BOX_SHAPE_PROXYTYPE:
{ {
const BoxShape* boxShape = static_cast<const BoxShape*>(shape); const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
SimdVector3 halfExtent = boxShape->GetHalfExtents(); btVector3 halfExtent = boxShape->GetHalfExtents();
glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]); glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]);
glutSolidCube(1.0); glutSolidCube(1.0);
useWireframeFallback = false; useWireframeFallback = false;
@@ -159,7 +159,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
break; break;
case SPHERE_SHAPE_PROXYTYPE: case SPHERE_SHAPE_PROXYTYPE:
{ {
const SphereShape* sphereShape = static_cast<const SphereShape*>(shape); const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
float radius = sphereShape->GetMargin();//radius doesn't include the margin, so draw with margin float radius = sphereShape->GetMargin();//radius doesn't include the margin, so draw with margin
glutSolidSphere(radius,10,10); glutSolidSphere(radius,10,10);
useWireframeFallback = false; useWireframeFallback = false;
@@ -168,7 +168,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
case MULTI_SPHERE_SHAPE_PROXYTYPE: case MULTI_SPHERE_SHAPE_PROXYTYPE:
case CONE_SHAPE_PROXYTYPE: case CONE_SHAPE_PROXYTYPE:
{ {
const ConeShape* coneShape = static_cast<const ConeShape*>(shape); const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
float radius = coneShape->GetRadius();//+coneShape->GetMargin(); float radius = coneShape->GetRadius();//+coneShape->GetMargin();
float height = coneShape->GetHeight();//+coneShape->GetMargin(); float height = coneShape->GetHeight();//+coneShape->GetMargin();
//glRotatef(-90.0, 1.0, 0.0, 0.0); //glRotatef(-90.0, 1.0, 0.0, 0.0);
@@ -187,7 +187,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
case CONVEX_SHAPE_PROXYTYPE: case CONVEX_SHAPE_PROXYTYPE:
case CYLINDER_SHAPE_PROXYTYPE: case CYLINDER_SHAPE_PROXYTYPE:
{ {
const CylinderShape* cylinder = static_cast<const CylinderShape*>(shape); const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
int upAxis = cylinder->GetUpAxis(); int upAxis = cylinder->GetUpAxis();
GLUquadricObj *quadObj = gluNewQuadric(); GLUquadricObj *quadObj = gluNewQuadric();
@@ -246,7 +246,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
/// for polyhedral shapes /// for polyhedral shapes
if (shape->IsPolyhedral()) if (shape->IsPolyhedral())
{ {
PolyhedralConvexShape* polyshape = (PolyhedralConvexShape*) shape; btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
glBegin(GL_LINES); glBegin(GL_LINES);
@@ -255,7 +255,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
int i; int i;
for (i=0;i<polyshape->GetNumEdges();i++) for (i=0;i<polyshape->GetNumEdges();i++)
{ {
SimdPoint3 a,b; btPoint3 a,b;
polyshape->GetEdge(i,a,b); polyshape->GetEdge(i,a,b);
glVertex3f(a.getX(),a.getY(),a.getZ()); glVertex3f(a.getX(),a.getY(),a.getZ());
@@ -266,7 +266,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
glEnd(); glEnd();
if (debugMode==IDebugDraw::DBG_DrawFeaturesText) if (debugMode==btIDebugDraw::DBG_DrawFeaturesText)
{ {
glRasterPos3f(0.0, 0.0, 0.0); glRasterPos3f(0.0, 0.0, 0.0);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo()); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo());
@@ -274,7 +274,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
glColor3f(1.f, 1.f, 1.f); glColor3f(1.f, 1.f, 1.f);
for (i=0;i<polyshape->GetNumVertices();i++) for (i=0;i<polyshape->GetNumVertices();i++)
{ {
SimdPoint3 vtx; btPoint3 vtx;
polyshape->GetVertex(i,vtx); polyshape->GetVertex(i,vtx);
glRasterPos3f(vtx.x(), vtx.y(), vtx.z()); glRasterPos3f(vtx.x(), vtx.y(), vtx.z());
char buf[12]; char buf[12];
@@ -284,10 +284,10 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
for (i=0;i<polyshape->GetNumPlanes();i++) for (i=0;i<polyshape->GetNumPlanes();i++)
{ {
SimdVector3 normal; btVector3 normal;
SimdPoint3 vtx; btPoint3 vtx;
polyshape->GetPlane(normal,vtx,i); polyshape->GetPlane(normal,vtx,i);
SimdScalar d = vtx.dot(normal); btScalar d = vtx.dot(normal);
glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d); glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d);
char buf[12]; char buf[12];
@@ -303,13 +303,13 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
if (shape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) if (shape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
{ {
TriangleMeshShape* concaveMesh = (TriangleMeshShape*) shape; btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape;
//SimdVector3 aabbMax(1e30f,1e30f,1e30f); //btVector3 aabbMax(1e30f,1e30f,1e30f);
//SimdVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
//todo pass camera, for some culling //todo pass camera, for some culling
SimdVector3 aabbMax(1e30f,1e30f,1e30f); btVector3 aabbMax(1e30f,1e30f,1e30f);
SimdVector3 aabbMin(-1e30f,-1e30f,-1e30f); btVector3 aabbMin(-1e30f,-1e30f,-1e30f);
GlDrawcallback drawCallback; GlDrawcallback drawCallback;
@@ -320,16 +320,17 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
if (shape->GetShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) if (shape->GetShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{ {
ConvexTriangleMeshShape* convexMesh = (ConvexTriangleMeshShape*) shape; btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
//todo: pass camera for some culling //todo: pass camera for some culling
SimdVector3 aabbMax(1e30f,1e30f,1e30f); btVector3 aabbMax(1e30f,1e30f,1e30f);
SimdVector3 aabbMin(-1e30f,-1e30f,-1e30f); btVector3 aabbMin(-1e30f,-1e30f,-1e30f);
TriangleGlDrawcallback drawCallback; TriangleGlDrawcallback drawCallback;
convexMesh->GetStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); convexMesh->GetStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
} }
glDisable(GL_DEPTH_BUFFER_BIT); glDisable(GL_DEPTH_BUFFER_BIT);
glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z()); glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z());
if (debugMode&IDebugDraw::DBG_DrawText) if (debugMode&IDebugDraw::DBG_DrawText)
@@ -343,7 +344,6 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim
} }
glEnable(GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_BUFFER_BIT);
// glPopMatrix(); // glPopMatrix();
} }
glPopMatrix(); glPopMatrix();

6
Demos/OpenGL/GL_ShapeDrawer.h
View File

@@ -15,15 +15,15 @@ subject to the following restrictions:
#ifndef GL_SHAPE_DRAWER_H #ifndef GL_SHAPE_DRAWER_H
#define GL_SHAPE_DRAWER_H #define GL_SHAPE_DRAWER_H
class CollisionShape; class btCollisionShape;
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
/// OpenGL shape drawing /// OpenGL shape drawing
class GL_ShapeDrawer class GL_ShapeDrawer
{ {
public: public:
static void DrawOpenGL(float* m, const CollisionShape* shape, const SimdVector3& color,int debugMode); static void DrawOpenGL(float* m, const btCollisionShape* shape, const btVector3& color,int debugMode);
static void DrawCoordSystem(); static void DrawCoordSystem();
}; };

8
Demos/OpenGL/GL_Simplex1to4.cpp
View File

@@ -26,7 +26,7 @@ subject to the following restrictions:
#else #else
#include <GL/gl.h> #include <GL/gl.h>
#endif #endif
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
GL_Simplex1to4::GL_Simplex1to4() GL_Simplex1to4::GL_Simplex1to4()
:m_simplexSolver(0) :m_simplexSolver(0)
@@ -38,7 +38,7 @@ GL_Simplex1to4::GL_Simplex1to4()
/// ///
void GL_Simplex1to4::CalcClosest(float* m) void GL_Simplex1to4::CalcClosest(float* m)
{ {
SimdTransform tr; btTransform tr;
tr.setFromOpenGLMatrix(m); tr.setFromOpenGLMatrix(m);
@@ -50,12 +50,12 @@ void GL_Simplex1to4::CalcClosest(float* m)
m_simplexSolver->reset(); m_simplexSolver->reset();
bool res; bool res;
SimdVector3 v; btVector3 v;
for (int i=0;i<m_numVertices;i++) for (int i=0;i<m_numVertices;i++)
{ {
v = tr(m_vertices[i]); v = tr(m_vertices[i]);
m_simplexSolver->addVertex(v,v,SimdPoint3(0.f,0.f,0.f)); m_simplexSolver->addVertex(v,v,btPoint3(0.f,0.f,0.f));
res = m_simplexSolver->closest(v); res = m_simplexSolver->closest(v);
} }

6
Demos/OpenGL/GL_Simplex1to4.h
View File

@@ -21,9 +21,9 @@ subject to the following restrictions:
///GL_Simplex1to4 is a class to debug a Simplex Solver with 1 to 4 points. ///GL_Simplex1to4 is a class to debug a Simplex Solver with 1 to 4 points.
///Can be used by GJK. ///Can be used by GJK.
class GL_Simplex1to4 : public BU_Simplex1to4 class GL_Simplex1to4 : public btBU_Simplex1to4
{ {
SimplexSolverInterface* m_simplexSolver; btSimplexSolverInterface* m_simplexSolver;
public: public:
@@ -31,7 +31,7 @@ class GL_Simplex1to4 : public BU_Simplex1to4
void CalcClosest(float* m); void CalcClosest(float* m);
void SetSimplexSolver(SimplexSolverInterface* simplexSolver) { void SetSimplexSolver(btSimplexSolverInterface* simplexSolver) {
m_simplexSolver = simplexSolver; m_simplexSolver = simplexSolver;
} }

4
Demos/OpenGL/RenderTexture.cpp
View File

@@ -30,7 +30,7 @@ RenderTexture::RenderTexture(int width,int height)
{ {
for (int y=0;y<m_height;y++) for (int y=0;y<m_height;y++)
{ {
SetPixel(x,y,SimdVector4(float(x),float(y),0.f,1.f)); SetPixel(x,y,btVector4(float(x),float(y),0.f,1.f));
} }
} }
@@ -54,7 +54,7 @@ void RenderTexture::Printf(char* str, BMF_FontData* fontData, int startx,int sta
{ {
char packedColor = bitmap[y]; char packedColor = bitmap[y];
float colorf = packedColor & bit ? 1.f : 0.f; float colorf = packedColor & bit ? 1.f : 0.f;
SimdVector4 rgba(colorf,colorf,colorf,1.f); btVector4 rgba(colorf,colorf,colorf,1.f);
SetPixel(rasterposx+x,rasterposy+8-y-1,rgba); SetPixel(rasterposx+x,rasterposy+8-y-1,rgba);
bit >>=1; bit >>=1;
} }

4
Demos/OpenGL/RenderTexture.h
View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#ifndef RENDER_TEXTURE_H #ifndef RENDER_TEXTURE_H
#define RENDER_TEXTURE_H #define RENDER_TEXTURE_H
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "BMF_FontData.h" #include "BMF_FontData.h"
/// ///
@@ -33,7 +33,7 @@ public:
RenderTexture(int width,int height); RenderTexture(int width,int height);
~RenderTexture(); ~RenderTexture();
inline void SetPixel(int x,int y,const SimdVector4& rgba) inline void SetPixel(int x,int y,const btVector4& rgba)
{ {
unsigned char* pixel = &m_buffer[ (x+y*m_width) * 4]; unsigned char* pixel = &m_buffer[ (x+y*m_width) * 4];

100
Demos/Raytracer/Raytracer.cpp
View File

@@ -19,8 +19,8 @@ Very basic raytracer, rendering into a texture.
///Low level demo, doesn't include btBulletCollisionCommon.h ///Low level demo, doesn't include btBulletCollisionCommon.h
#include "GL_Simplex1to4.h" #include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "GL_ShapeDrawer.h" #include "GL_ShapeDrawer.h"
#include "Raytracer.h" #include "Raytracer.h"
@@ -48,7 +48,7 @@ Very basic raytracer, rendering into a texture.
#include "RenderTexture.h" #include "RenderTexture.h"
VoronoiSimplexSolver simplexSolver; btVoronoiSimplexSolver simplexSolver;
float yaw=0.f,pitch=0.f,roll=0.f; float yaw=0.f,pitch=0.f,roll=0.f;
const int maxNumObjects = 4; const int maxNumObjects = 4;
@@ -57,8 +57,8 @@ const int numObjects = 4;
/// simplex contains the vertices, and some extra code to draw and debug /// simplex contains the vertices, and some extra code to draw and debug
GL_Simplex1to4 simplex; GL_Simplex1to4 simplex;
ConvexShape* shapePtr[maxNumObjects]; btConvexShape* shapePtr[maxNumObjects];
SimdTransform transforms[maxNumObjects]; btTransform transforms[maxNumObjects];
RenderTexture* raytracePicture = 0; RenderTexture* raytracePicture = 0;
@@ -66,12 +66,12 @@ int screenWidth = 128;
int screenHeight = 128; int screenHeight = 128;
GLuint glTextureId; GLuint glTextureId;
SphereShape mySphere(1); btSphereShape mySphere(1);
BoxShape myBox(SimdVector3(0.4f,0.4f,0.4f)); btBoxShape myBox(btVector3(0.4f,0.4f,0.4f));
CylinderShape myCylinder(SimdVector3(0.3f,0.3f,0.3f)); btCylinderShape myCylinder(btVector3(0.3f,0.3f,0.3f));
ConeShape myCone(1,1); btConeShape myCone(1,1);
MinkowskiSumShape myMink(&myCylinder,&myBox); btMinkowskiSumShape myMink(&myCylinder,&myBox);
/// ///
@@ -96,25 +96,25 @@ void Raytracer::initPhysics()
myCone.SetMargin(0.2f); myCone.SetMargin(0.2f);
simplex.SetSimplexSolver(&simplexSolver); simplex.SetSimplexSolver(&simplexSolver);
simplex.AddVertex(SimdPoint3(-1,0,-1)); simplex.AddVertex(btPoint3(-1,0,-1));
simplex.AddVertex(SimdPoint3(1,0,-1)); simplex.AddVertex(btPoint3(1,0,-1));
simplex.AddVertex(SimdPoint3(0,0,1)); simplex.AddVertex(btPoint3(0,0,1));
simplex.AddVertex(SimdPoint3(0,1,0)); simplex.AddVertex(btPoint3(0,1,0));
/// convex hull of 5 spheres /// convex hull of 5 spheres
#define NUM_SPHERES 5 #define NUM_SPHERES 5
SimdVector3 inertiaHalfExtents(10.f,10.f,10.f); btVector3 inertiaHalfExtents(10.f,10.f,10.f);
SimdVector3 positions[NUM_SPHERES] = { btVector3 positions[NUM_SPHERES] = {
SimdVector3(-1.2f, -0.3f, 0.f), btVector3(-1.2f, -0.3f, 0.f),
SimdVector3(0.8f, -0.3f, 0.f), btVector3(0.8f, -0.3f, 0.f),
SimdVector3(0.5f, 0.6f, 0.f), btVector3(0.5f, 0.6f, 0.f),
SimdVector3(-0.5f, 0.6f, 0.f), btVector3(-0.5f, 0.6f, 0.f),
SimdVector3(0.f, 0.f, 0.f) btVector3(0.f, 0.f, 0.f)
}; };
// MultiSphereShape* multiSphereShape = new MultiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); // btMultiSphereShape* multiSphereShape = new btMultiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES);
ConvexHullShape* convexHullShape = new ConvexHullShape(positions,3); btConvexHullShape* convexHullShape = new btConvexHullShape(positions,3);
//choose shape //choose shape
@@ -146,16 +146,16 @@ void Raytracer::displayCallback()
for (int i=0;i<numObjects;i++) for (int i=0;i<numObjects;i++)
{ {
transforms[i].setIdentity(); transforms[i].setIdentity();
SimdVector3 pos(-3.5f+i*2.5f,0.f,0.f); btVector3 pos(-3.5f+i*2.5f,0.f,0.f);
transforms[i].setOrigin( pos ); transforms[i].setOrigin( pos );
SimdQuaternion orn; btQuaternion orn;
if (i < 2) if (i < 2)
{ {
orn.setEuler(yaw,pitch,roll); orn.setEuler(yaw,pitch,roll);
transforms[i].setRotation(orn); transforms[i].setRotation(orn);
} }
} }
myMink.SetTransformA(SimdTransform(transforms[0].getRotation())); myMink.SetTransformA(btTransform(transforms[0].getRotation()));
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING); glDisable(GL_LIGHTING);
@@ -184,7 +184,7 @@ void Raytracer::displayCallback()
SimdVector4 rgba(1.f,0.f,0.f,0.5f); btVector4 rgba(1.f,0.f,0.f,0.5f);
float top = 1.f; float top = 1.f;
float bottom = -1.f; float bottom = -1.f;
@@ -195,15 +195,15 @@ void Raytracer::displayCallback()
float fov = 2.0 * atanf (tanFov); float fov = 2.0 * atanf (tanFov);
SimdVector3 rayFrom = getCameraPosition(); btVector3 rayFrom = getCameraPosition();
SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition(); btVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
rayForward.normalize(); rayForward.normalize();
float farPlane = 600.f; float farPlane = 600.f;
rayForward*= farPlane; rayForward*= farPlane;
SimdVector3 rightOffset; btVector3 rightOffset;
SimdVector3 vertical(0.f,1.f,0.f); btVector3 vertical(0.f,1.f,0.f);
SimdVector3 hor; btVector3 hor;
hor = rayForward.cross(vertical); hor = rayForward.cross(vertical);
hor.normalize(); hor.normalize();
vertical = hor.cross(rayForward); vertical = hor.cross(rayForward);
@@ -214,20 +214,20 @@ void Raytracer::displayCallback()
hor *= 2.f * farPlane * tanfov; hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov; vertical *= 2.f * farPlane * tanfov;
SimdVector3 rayToCenter = rayFrom + rayForward; btVector3 rayToCenter = rayFrom + rayForward;
SimdVector3 dHor = hor * 1.f/float(screenWidth); btVector3 dHor = hor * 1.f/float(screenWidth);
SimdVector3 dVert = vertical * 1.f/float(screenHeight); btVector3 dVert = vertical * 1.f/float(screenHeight);
SimdTransform rayFromTrans; btTransform rayFromTrans;
rayFromTrans.setIdentity(); rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom); rayFromTrans.setOrigin(rayFrom);
SimdTransform rayFromLocal; btTransform rayFromLocal;
SimdTransform rayToLocal; btTransform rayToLocal;
SphereShape pointShape(0.0f); btSphereShape pointShape(0.0f);
///clear texture ///clear texture
@@ -235,16 +235,16 @@ void Raytracer::displayCallback()
{ {
for (int y=0;y<screenHeight;y++) for (int y=0;y<screenHeight;y++)
{ {
SimdVector4 rgba(0.f,0.f,0.f,0.f); btVector4 rgba(0.f,0.f,0.f,0.f);
raytracePicture->SetPixel(x,y,rgba); raytracePicture->SetPixel(x,y,rgba);
} }
} }
ConvexCast::CastResult rayResult; btConvexCast::CastResult rayResult;
SimdTransform rayToTrans; btTransform rayToTrans;
rayToTrans.setIdentity(); rayToTrans.setIdentity();
SimdVector3 rayTo; btVector3 rayTo;
for (int x=0;x<screenWidth;x++) for (int x=0;x<screenWidth;x++)
{ {
for (int y=0;y<screenHeight;y++) for (int y=0;y<screenHeight;y++)
@@ -259,11 +259,11 @@ void Raytracer::displayCallback()
// rayToLocal = transforms[s].inverse()* rayToTrans; // rayToLocal = transforms[s].inverse()* rayToTrans;
//choose the continuous collision detection method //choose the continuous collision detection method
SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver); //GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0); //ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0);
// BU_Simplex1to4 ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex' // btBU_Simplex1to4 ptShape(btVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex'
// BU_CollisionPair convexCaster(&ptShape,shapePtr[0]); // BU_CollisionPair convexCaster(&ptShape,shapePtr[0]);
@@ -276,21 +276,21 @@ void Raytracer::displayCallback()
//float fog = 1.f - 0.1f * rayResult.m_fraction; //float fog = 1.f - 0.1f * rayResult.m_fraction;
rayResult.m_normal.normalize(); rayResult.m_normal.normalize();
SimdVector3 worldNormal; btVector3 worldNormal;
worldNormal = transforms[s].getBasis() *rayResult.m_normal; worldNormal = transforms[s].getBasis() *rayResult.m_normal;
float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f)); float light = worldNormal.dot(btVector3(0.4f,-1.f,-0.4f));
if (light < 0.2f) if (light < 0.2f)
light = 0.2f; light = 0.2f;
if (light > 1.f) if (light > 1.f)
light = 1.f; light = 1.f;
rgba = SimdVector4(light,light,light,1.f); rgba = btVector4(light,light,light,1.f);
raytracePicture->SetPixel(x,y,rgba); raytracePicture->SetPixel(x,y,rgba);
} else } else
{ {
//clear is already done //clear is already done
//rgba = SimdVector4(0.f,0.f,0.f,0.f); //rgba = btVector4(0.f,0.f,0.f,0.f);
//raytracePicture->SetPixel(x,y,rgba); //raytracePicture->SetPixel(x,y,rgba);
} }
@@ -386,7 +386,7 @@ void Raytracer::displayCallback()
transA.getOpenGLMatrix( m ); transA.getOpenGLMatrix( m );
/// draw the simplex /// draw the simplex
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1)); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(1,1,1));
/// calculate closest point from simplex to the origin, and draw this vector /// calculate closest point from simplex to the origin, and draw this vector
simplex.CalcClosest(m); simplex.CalcClosest(m);

26
Demos/SimplexDemo/SimplexDemo.cpp
View File

@@ -19,15 +19,15 @@ subject to the following restrictions:
*/ */
#include "GL_Simplex1to4.h" #include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "GL_ShapeDrawer.h" #include "GL_ShapeDrawer.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "SimplexDemo.h" #include "SimplexDemo.h"
#include "GlutStuff.h" #include "GlutStuff.h"
VoronoiSimplexSolver simplexSolver; btVoronoiSimplexSolver simplexSolver;
@@ -40,7 +40,7 @@ int screenHeight = 480;
GL_Simplex1to4 simplex; GL_Simplex1to4 simplex;
PolyhedralConvexShape* shapePtr[maxNumObjects]; btPolyhedralConvexShape* shapePtr[maxNumObjects];
/// ///
@@ -78,17 +78,17 @@ void SimplexDemo::displayCallback()
for (i=0;i<numObjects;i++) for (i=0;i<numObjects;i++)
{ {
SimdTransform transA; btTransform transA;
transA.setIdentity(); transA.setIdentity();
SimdVector3 dpos(0.f,5.f,0.f); btVector3 dpos(0.f,5.f,0.f);
transA.setOrigin( dpos ); transA.setOrigin( dpos );
SimdQuaternion orn; btQuaternion orn;
orn.setEuler(yaw,pitch,roll); orn.setEuler(yaw,pitch,roll);
transA.setRotation(orn); transA.setRotation(orn);
transA.getOpenGLMatrix( m ); transA.getOpenGLMatrix( m );
/// draw the simplex /// draw the simplex
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode()); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode());
/// calculate closest point from simplex to the origin, and draw this vector /// calculate closest point from simplex to the origin, and draw this vector
simplex.CalcClosest(m); simplex.CalcClosest(m);
@@ -106,14 +106,14 @@ void SimplexDemo::initPhysics()
simplex.SetSimplexSolver(&simplexSolver); simplex.SetSimplexSolver(&simplexSolver);
simplex.AddVertex(SimdPoint3(-2,0,-2)); simplex.AddVertex(btPoint3(-2,0,-2));
simplex.AddVertex(SimdPoint3(2,0,-2)); simplex.AddVertex(btPoint3(2,0,-2));
simplex.AddVertex(SimdPoint3(0,0,2)); simplex.AddVertex(btPoint3(0,0,2));
simplex.AddVertex(SimdPoint3(0,2,0)); simplex.AddVertex(btPoint3(0,2,0));
shapePtr[0] = &simplex; shapePtr[0] = &simplex;
SimdTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
} }

42
Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#include "CcdPhysicsEnvironment.h" #include "CcdPhysicsEnvironment.h"
#include "CcdPhysicsController.h" #include "CcdPhysicsController.h"
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
#include "PHY_Pro.h" #include "PHY_Pro.h"
#include "UserCollisionAlgorithm.h" #include "UserCollisionAlgorithm.h"
@@ -31,17 +31,17 @@ GLDebugDrawer debugDrawer;
static const int NUM_VERTICES = 5; static const int NUM_VERTICES = 5;
static const int NUM_TRIANGLES=4; static const int NUM_TRIANGLES=4;
SimdVector3 gVertices[NUM_VERTICES]; btVector3 gVertices[NUM_VERTICES];
int gIndices[NUM_TRIANGLES*3]; int gIndices[NUM_TRIANGLES*3];
const float TRIANGLE_SIZE=80.f; const float TRIANGLE_SIZE=80.f;
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= CollisionObject::customMaterialCallback; ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
inline SimdScalar calculateCombinedFriction(float friction0,float friction1) inline btScalar calculateCombinedFriction(float friction0,float friction1)
{ {
SimdScalar friction = friction0 * friction1; btScalar friction = friction0 * friction1;
const SimdScalar MAX_FRICTION = 10.f; const btScalar MAX_FRICTION = 10.f;
if (friction < -MAX_FRICTION) if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION; friction = -MAX_FRICTION;
if (friction > MAX_FRICTION) if (friction > MAX_FRICTION)
@@ -50,7 +50,7 @@ inline SimdScalar calculateCombinedFriction(float friction0,float friction1)
} }
inline SimdScalar calculateCombinedRestitution(float restitution0,float restitution1) inline btScalar calculateCombinedRestitution(float restitution0,float restitution1)
{ {
return restitution0 * restitution1; return restitution0 * restitution1;
} }
@@ -74,7 +74,7 @@ void UserCollisionAlgorithm::initPhysics()
{ {
#define TRISIZE 10.f #define TRISIZE 10.f
int vertStride = sizeof(SimdVector3); int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int); int indexStride = 3*sizeof(int);
const int NUM_VERTS_X = 50; const int NUM_VERTS_X = 50;
@@ -83,7 +83,7 @@ void UserCollisionAlgorithm::initPhysics()
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
SimdVector3* gVertices = new SimdVector3[totalVerts]; btVector3* gVertices = new btVector3[totalVerts];
int* gIndices = new int[totalTriangles*3]; int* gIndices = new int[totalTriangles*3];
int i; int i;
@@ -111,41 +111,41 @@ void UserCollisionAlgorithm::initPhysics()
} }
} }
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices, gIndices,
indexStride, indexStride,
totalVerts,(float*) &gVertices[0].x(),vertStride); totalVerts,(float*) &gVertices[0].x(),vertStride);
CollisionShape* trimeshShape = new BvhTriangleMeshShape(indexVertexArrays); btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(indexVertexArrays);
//ConstraintSolver* solver = new SequentialImpulseConstraintSolver; //ConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
SimdVector3 maxAabb(10000,10000,10000); btVector3 maxAabb(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase(); btOverlappingPairCache* broadphase = new btAxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase();
dispatcher->RegisterCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new SphereSphereCollisionAlgorithm::CreateFunc); dispatcher->RegisterCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
bool isDynamic = false; bool isDynamic = false;
float mass = 0.f; float mass = 0.f;
SimdTransform startTransform; btTransform startTransform;
startTransform.setIdentity(); startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,-2,0)); startTransform.setOrigin(btVector3(0,-2,0));
CcdPhysicsController* staticTrimesh = LocalCreatePhysicsObject(isDynamic, mass, startTransform,trimeshShape); CcdPhysicsController* staticTrimesh = LocalCreatePhysicsObject(isDynamic, mass, startTransform,trimeshShape);
//enable custom material callback //enable custom material callback
staticTrimesh->GetRigidBody()->m_collisionFlags |= CollisionObject::customMaterialCallback; staticTrimesh->GetRigidBody()->m_collisionFlags |= btCollisionObject::customMaterialCallback;
{ {
for (int i=0;i<10;i++) for (int i=0;i<10;i++)
{ {
CollisionShape* sphereShape = new SphereShape(1); btCollisionShape* sphereShape = new btSphereShape(1);
startTransform.setOrigin(SimdVector3(1,2*i,1)); startTransform.setOrigin(btVector3(1,2*i,1));
LocalCreatePhysicsObject(true, 1, startTransform,sphereShape); LocalCreatePhysicsObject(true, 1, startTransform,sphereShape);
} }
} }

62
Demos/VehicleDemo/VehicleDemo.cpp
View File

@@ -28,8 +28,8 @@ subject to the following restrictions:
#include "ParallelIslandDispatcher.h" #include "ParallelIslandDispatcher.h"
#include "LinearMath/GenQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
@@ -51,7 +51,7 @@ const int maxOverlap = 65535;
DefaultMotionState wheelMotionState[4]; DefaultMotionState wheelMotionState[4];
///PHY_IVehicle is the interface behind the constraint that implements the raycast vehicle (WrapperVehicle which holds a RaycastVehicle) ///PHY_IVehicle is the interface behind the constraint that implements the raycast vehicle (WrapperVehicle which holds a btRaycastVehicle)
///notice that for higher-quality slow-moving vehicles, another approach might be better ///notice that for higher-quality slow-moving vehicles, another approach might be better
///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts ///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
PHY_IVehicle* gVehicleConstraint=0; PHY_IVehicle* gVehicleConstraint=0;
@@ -67,9 +67,9 @@ float suspensionStiffness = 10.f;
float suspensionDamping = 1.3f; float suspensionDamping = 1.3f;
float suspensionCompression = 2.4f; float suspensionCompression = 2.4f;
float rollInfluence = 0.1f; float rollInfluence = 0.1f;
SimdVector3 wheelDirectionCS0(0,-1,0); btVector3 wheelDirectionCS0(0,-1,0);
SimdVector3 wheelAxleCS(1,0,0); btVector3 wheelAxleCS(1,0,0);
SimdScalar suspensionRestLength(0.6); btScalar suspensionRestLength(0.6);
#define CUBE_HALF_EXTENTS 1 #define CUBE_HALF_EXTENTS 1
@@ -98,20 +98,20 @@ m_cameraHeight(4.f),
m_minCameraDistance(3.f), m_minCameraDistance(3.f),
m_maxCameraDistance(10.f) m_maxCameraDistance(10.f)
{ {
m_cameraPosition = SimdVector3(30,30,30); m_cameraPosition = btVector3(30,30,30);
} }
void VehicleDemo::setupPhysics() void VehicleDemo::setupPhysics()
{ {
CollisionDispatcher* dispatcher = new CollisionDispatcher(); btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
ParallelIslandDispatcher* dispatcher2 = new ParallelIslandDispatcher(); ParallelIslandDispatcher* dispatcher2 = new ParallelIslandDispatcher();
SimdVector3 worldAabbMin(-30000,-30000,-30000); btVector3 worldAabbMin(-30000,-30000,-30000);
SimdVector3 worldAabbMax(30000,30000,30000); btVector3 worldAabbMax(30000,30000,30000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies); btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
//OverlappingPairCache* broadphase = new SimpleBroadphase(maxProxies,maxOverlap); //OverlappingPairCache* broadphase = new btSimpleBroadphase(maxProxies,maxOverlap);
#ifdef USE_PARALLEL_DISPATCHER #ifdef USE_PARALLEL_DISPATCHER
m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase); m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase);
@@ -125,7 +125,7 @@ void VehicleDemo::setupPhysics()
m_physicsEnvironmentPtr->setGravity(0,-10,0);//0,0);//-10,0); m_physicsEnvironmentPtr->setGravity(0,-10,0);//0,0);//-10,0);
int i; int i;
CollisionShape* groundShape = new BoxShape(SimdVector3(50,3,50)); btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
#define USE_TRIMESH_GROUND 1 #define USE_TRIMESH_GROUND 1
#ifdef USE_TRIMESH_GROUND #ifdef USE_TRIMESH_GROUND
@@ -134,7 +134,7 @@ void VehicleDemo::setupPhysics()
const float TRIANGLE_SIZE=20.f; const float TRIANGLE_SIZE=20.f;
//create a triangle-mesh ground //create a triangle-mesh ground
int vertStride = sizeof(SimdVector3); int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int); int indexStride = 3*sizeof(int);
const int NUM_VERTS_X = 50; const int NUM_VERTS_X = 50;
@@ -143,7 +143,7 @@ const float TRIANGLE_SIZE=20.f;
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
SimdVector3* gVertices = new SimdVector3[totalVerts]; btVector3* gVertices = new btVector3[totalVerts];
int* gIndices = new int[totalTriangles*3]; int* gIndices = new int[totalTriangles*3];
@@ -171,25 +171,25 @@ const float TRIANGLE_SIZE=20.f;
} }
} }
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices, gIndices,
indexStride, indexStride,
totalVerts,(float*) &gVertices[0].x(),vertStride); totalVerts,(float*) &gVertices[0].x(),vertStride);
groundShape = new BvhTriangleMeshShape(indexVertexArrays); groundShape = new btBvhTriangleMeshShape(indexVertexArrays);
#endif // #endif //
SimdTransform tr; btTransform tr;
tr.setIdentity(); tr.setIdentity();
tr.setOrigin(SimdVector3(0,-20.f,0)); tr.setOrigin(btVector3(0,-20.f,0));
//create ground object //create ground object
LocalCreatePhysicsObject(false,0,tr,groundShape); LocalCreatePhysicsObject(false,0,tr,groundShape);
CollisionShape* chassisShape = new BoxShape(SimdVector3(1.f,0.5f,2.f)); btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
tr.setOrigin(SimdVector3(0,0.f,0)); tr.setOrigin(btVector3(0,0.f,0));
m_carChassis = LocalCreatePhysicsObject(true,800,tr,chassisShape); m_carChassis = LocalCreatePhysicsObject(true,800,tr,chassisShape);
@@ -212,8 +212,8 @@ const float TRIANGLE_SIZE=20.f;
gVehicleConstraint = m_physicsEnvironmentPtr->getVehicleConstraint(constraintId); gVehicleConstraint = m_physicsEnvironmentPtr->getVehicleConstraint(constraintId);
SimdVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius); btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
RaycastVehicle::VehicleTuning tuning; btRaycastVehicle::btVehicleTuning tuning;
bool isFrontWheel=true; bool isFrontWheel=true;
int rightIndex = 0; int rightIndex = 0;
int upIndex = 1; int upIndex = 1;
@@ -225,18 +225,18 @@ const float TRIANGLE_SIZE=20.f;
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
connectionPointCS0 = SimdVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
gVehicleConstraint->AddWheel(&wheelMotionState[1], gVehicleConstraint->AddWheel(&wheelMotionState[1],
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
connectionPointCS0 = SimdVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius); connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius);
isFrontWheel = false; isFrontWheel = false;
gVehicleConstraint->AddWheel(&wheelMotionState[2], gVehicleConstraint->AddWheel(&wheelMotionState[2],
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
connectionPointCS0 = SimdVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius); connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,-2*CUBE_HALF_EXTENTS+wheelRadius);
gVehicleConstraint->AddWheel(&wheelMotionState[3], gVehicleConstraint->AddWheel(&wheelMotionState[3],
(PHY__Vector3&)connectionPointCS0, (PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel); (PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
@@ -281,8 +281,8 @@ void VehicleDemo::renderme()
float m[16]; float m[16];
int i; int i;
CylinderShapeX wheelShape(SimdVector3(wheelWidth,wheelRadius,wheelRadius)); btCylinderShapeX wheelShape(btVector3(wheelWidth,wheelRadius,wheelRadius));
SimdVector3 wheelColor(1,0,0); btVector3 wheelColor(1,0,0);
for (i=0;i<4;i++) for (i=0;i<4;i++)
{ {
@@ -319,14 +319,14 @@ void VehicleDemo::clientMoveAndDisplay()
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("render"); btProfiler::beginBlock("render");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
renderme(); renderme();
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("render"); btProfiler::endBlock("render");
#endif #endif
glFlush(); glFlush();
glutSwapBuffers(); glutSwapBuffers();
@@ -420,7 +420,7 @@ void VehicleDemo::updateCamera()
//interpolate the camera height //interpolate the camera height
m_cameraPosition[1] = (15.0*m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0; m_cameraPosition[1] = (15.0*m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0;
SimdVector3 camToObject = m_cameraTargetPosition - m_cameraPosition; btVector3 camToObject = m_cameraTargetPosition - m_cameraPosition;
//keep distance between min and max distance //keep distance between min and max distance
float cameraDistance = camToObject.length(); float cameraDistance = camToObject.length();

98
Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.cpp
View File

@@ -16,23 +16,23 @@ subject to the following restrictions:
#include "BU_AlgebraicPolynomialSolver.h" #include "BU_AlgebraicPolynomialSolver.h"
#include <math.h> #include <math.h>
#include <SimdMinMax.h> #include <btSimdMinMax.h>
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q) int BU_AlgebraicPolynomialSolver::Solve2Quadratic(btScalar p, btScalar q)
{ {
SimdScalar basic_h_local; btScalar basic_h_local;
SimdScalar basic_h_local_delta; btScalar basic_h_local_delta;
basic_h_local = p * 0.5f; basic_h_local = p * 0.5f;
basic_h_local_delta = basic_h_local * basic_h_local - q; basic_h_local_delta = basic_h_local * basic_h_local - q;
if (basic_h_local_delta > 0.0f) { if (basic_h_local_delta > 0.0f) {
basic_h_local_delta = SimdSqrt(basic_h_local_delta); basic_h_local_delta = btSqrt(basic_h_local_delta);
m_roots[0] = - basic_h_local + basic_h_local_delta; m_roots[0] = - basic_h_local + basic_h_local_delta;
m_roots[1] = - basic_h_local - basic_h_local_delta; m_roots[1] = - basic_h_local - basic_h_local_delta;
return 2; return 2;
} }
else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) { else if (btGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) {
m_roots[0] = - basic_h_local; m_roots[0] = - basic_h_local;
return 1; return 1;
} }
@@ -42,13 +42,13 @@ int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q)
} }
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(btScalar a,btScalar b, btScalar c)
{ {
SimdScalar radical = b * b - 4.0f * a * c; btScalar radical = b * b - 4.0f * a * c;
if(radical >= 0.f) if(radical >= 0.f)
{ {
SimdScalar sqrtRadical = SimdSqrt(radical); btScalar sqrtRadical = btSqrt(radical);
SimdScalar idenom = 1.0f/(2.0f * a); btScalar idenom = 1.0f/(2.0f * a);
m_roots[0]=(-b + sqrtRadical) * idenom; m_roots[0]=(-b + sqrtRadical) * idenom;
m_roots[1]=(-b - sqrtRadical) * idenom; m_roots[1]=(-b - sqrtRadical) * idenom;
return 2; return 2;
@@ -58,8 +58,8 @@ int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b,
#define cubic_rt(x) \ #define cubic_rt(x) \
((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \ ((x) > 0.0f ? btPow((btScalar)(x), 0.333333333333333333333333f) : \
((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f)) ((x) < 0.0f ? -btPow((btScalar)-(x), 0.333333333333333333333333f) : 0.0f))
@@ -72,26 +72,26 @@ int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b,
/* it returns the number of different roots found, and stores the roots in */ /* it returns the number of different roots found, and stores the roots in */
/* roots[0,2]. it returns -1 for a degenerate equation 0 = 0. */ /* roots[0,2]. it returns -1 for a degenerate equation 0 = 0. */
/* */ /* */
int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) int BU_AlgebraicPolynomialSolver::Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c)
{ {
SimdScalar p, q, r; btScalar p, q, r;
SimdScalar delta, u, phi; btScalar delta, u, phi;
SimdScalar dummy; btScalar dummy;
if (lead != 1.0) { if (lead != 1.0) {
/* */ /* */
/* transform into normal form: x^3 + a x^2 + b x + c = 0 */ /* transform into normal form: x^3 + a x^2 + b x + c = 0 */
/* */ /* */
if (SimdEqual(lead, SIMD_EPSILON)) { if (btEqual(lead, SIMD_EPSILON)) {
/* */ /* */
/* we have a x^2 + b x + c = 0 */ /* we have a x^2 + b x + c = 0 */
/* */ /* */
if (SimdEqual(a, SIMD_EPSILON)) { if (btEqual(a, SIMD_EPSILON)) {
/* */ /* */
/* we have b x + c = 0 */ /* we have b x + c = 0 */
/* */ /* */
if (SimdEqual(b, SIMD_EPSILON)) { if (btEqual(b, SIMD_EPSILON)) {
if (SimdEqual(c, SIMD_EPSILON)) { if (btEqual(c, SIMD_EPSILON)) {
return -1; return -1;
} }
else { else {
@@ -128,8 +128,8 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */ /* */
/* now use Cardano's formula */ /* now use Cardano's formula */
/* */ /* */
if (SimdEqual(p, SIMD_EPSILON)) { if (btEqual(p, SIMD_EPSILON)) {
if (SimdEqual(q, SIMD_EPSILON)) { if (btEqual(q, SIMD_EPSILON)) {
/* */ /* */
/* one triple root */ /* one triple root */
/* */ /* */
@@ -151,7 +151,7 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */ /* */
/* one real and two complex roots. note that v = -p / u. */ /* one real and two complex roots. note that v = -p / u. */
/* */ /* */
u = -q + SimdSqrt(delta); u = -q + btSqrt(delta);
u = cubic_rt(u); u = cubic_rt(u);
m_roots[0] = u - p / u - a; m_roots[0] = u - p / u - a;
return 1; return 1;
@@ -160,19 +160,19 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */ /* */
/* Casus irreducibilis: we have three real roots */ /* Casus irreducibilis: we have three real roots */
/* */ /* */
r = SimdSqrt(-p); r = btSqrt(-p);
p *= -r; p *= -r;
r *= 2.0; r *= 2.0;
phi = SimdAcos(-q / p) / 3.0f; phi = btAcos(-q / p) / 3.0f;
dummy = SIMD_2_PI / 3.0f; dummy = SIMD_2_PI / 3.0f;
m_roots[0] = r * SimdCos(phi) - a; m_roots[0] = r * btCos(phi) - a;
m_roots[1] = r * SimdCos(phi + dummy) - a; m_roots[1] = r * btCos(phi + dummy) - a;
m_roots[2] = r * SimdCos(phi - dummy) - a; m_roots[2] = r * btCos(phi - dummy) - a;
return 3; return 3;
} }
else { else {
/* */ /* */
/* one single and one SimdScalar root */ /* one single and one btScalar root */
/* */ /* */
r = cubic_rt(-q); r = cubic_rt(-q);
m_roots[0] = 2.0f * r - a; m_roots[0] = 2.0f * r - a;
@@ -191,31 +191,31 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* it returns the number of different roots found, and stores the roots in */ /* it returns the number of different roots found, and stores the roots in */
/* roots[0,3]. it returns -1 for a degenerate equation 0 = 0. */ /* roots[0,3]. it returns -1 for a degenerate equation 0 = 0. */
/* */ /* */
int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) int BU_AlgebraicPolynomialSolver::Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d)
{ {
SimdScalar p, q ,r; btScalar p, q ,r;
SimdScalar u, v, w; btScalar u, v, w;
int i, num_roots, num_tmp; int i, num_roots, num_tmp;
//SimdScalar tmp[2]; //btScalar tmp[2];
if (lead != 1.0) { if (lead != 1.0) {
/* */ /* */
/* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */ /* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */
/* */ /* */
if (SimdEqual(lead, SIMD_EPSILON)) { if (btEqual(lead, SIMD_EPSILON)) {
/* */ /* */
/* we have a x^3 + b x^2 + c x + d = 0 */ /* we have a x^3 + b x^2 + c x + d = 0 */
/* */ /* */
if (SimdEqual(a, SIMD_EPSILON)) { if (btEqual(a, SIMD_EPSILON)) {
/* */ /* */
/* we have b x^2 + c x + d = 0 */ /* we have b x^2 + c x + d = 0 */
/* */ /* */
if (SimdEqual(b, SIMD_EPSILON)) { if (btEqual(b, SIMD_EPSILON)) {
/* */ /* */
/* we have c x + d = 0 */ /* we have c x + d = 0 */
/* */ /* */
if (SimdEqual(c, SIMD_EPSILON)) { if (btEqual(c, SIMD_EPSILON)) {
if (SimdEqual(d, SIMD_EPSILON)) { if (btEqual(d, SIMD_EPSILON)) {
return -1; return -1;
} }
else { else {
@@ -254,7 +254,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
p = b - 6.0f * a * a; p = b - 6.0f * a * a;
q = a * (8.0f * a * a - 2.0f * b) + c; q = a * (8.0f * a * a - 2.0f * b) + c;
r = a * (a * (b - 3.f * a * a) - c) + d; r = a * (a * (b - 3.f * a * a) - c) + d;
if (SimdEqual(q, SIMD_EPSILON)) { if (btEqual(q, SIMD_EPSILON)) {
/* */ /* */
/* biquadratic equation: y^4 + p y^2 + r = 0. */ /* biquadratic equation: y^4 + p y^2 + r = 0. */
/* */ /* */
@@ -263,23 +263,23 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
if (m_roots[0] > 0.0f) { if (m_roots[0] > 0.0f) {
if (num_roots > 1) { if (num_roots > 1) {
if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) { if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) {
u = SimdSqrt(m_roots[1]); u = btSqrt(m_roots[1]);
m_roots[2] = u - a; m_roots[2] = u - a;
m_roots[3] = -u - a; m_roots[3] = -u - a;
u = SimdSqrt(m_roots[0]); u = btSqrt(m_roots[0]);
m_roots[0] = u - a; m_roots[0] = u - a;
m_roots[1] = -u - a; m_roots[1] = -u - a;
return 4; return 4;
} }
else { else {
u = SimdSqrt(m_roots[0]); u = btSqrt(m_roots[0]);
m_roots[0] = u - a; m_roots[0] = u - a;
m_roots[1] = -u - a; m_roots[1] = -u - a;
return 2; return 2;
} }
} }
else { else {
u = SimdSqrt(m_roots[0]); u = btSqrt(m_roots[0]);
m_roots[0] = u - a; m_roots[0] = u - a;
m_roots[1] = -u - a; m_roots[1] = -u - a;
return 2; return 2;
@@ -288,7 +288,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
} }
return 0; return 0;
} }
else if (SimdEqual(r, SIMD_EPSILON)) { else if (btEqual(r, SIMD_EPSILON)) {
/* */ /* */
/* no absolute term: y (y^3 + p y + q) = 0. */ /* no absolute term: y (y^3 + p y + q) = 0. */
/* */ /* */
@@ -321,17 +321,17 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
u = w * w - r; u = w * w - r;
v = 2.0f * w - p; v = 2.0f * w - p;
if (SimdEqual(u, SIMD_EPSILON)) if (btEqual(u, SIMD_EPSILON))
u = 0.0; u = 0.0;
else if (u > 0.0f) else if (u > 0.0f)
u = SimdSqrt(u); u = btSqrt(u);
else else
return 0; return 0;
if (SimdEqual(v, SIMD_EPSILON)) if (btEqual(v, SIMD_EPSILON))
v = 0.0; v = 0.0;
else if (v > 0.0f) else if (v > 0.0f)
v = SimdSqrt(v); v = btSqrt(v);
else else
return 0; return 0;
@@ -343,7 +343,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
m_roots[i] -= a; m_roots[i] -= a;
} }
w += 2.0f *u; w += 2.0f *u;
SimdScalar tmp[2]; btScalar tmp[2];
tmp[0] = m_roots[0]; tmp[0] = m_roots[0];
tmp[1] = m_roots[1]; tmp[1] = m_roots[1];

12
Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.h
View File

@@ -26,19 +26,19 @@ class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface
public: public:
BU_AlgebraicPolynomialSolver() {}; BU_AlgebraicPolynomialSolver() {};
int Solve2Quadratic(SimdScalar p, SimdScalar q); int Solve2Quadratic(btScalar p, btScalar q);
int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c); int Solve2QuadraticFull(btScalar a,btScalar b, btScalar c);
int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c); int Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c);
int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d); int Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d);
SimdScalar GetRoot(int i) const btScalar GetRoot(int i) const
{ {
return m_roots[i]; return m_roots[i];
} }
private: private:
SimdScalar m_roots[4]; btScalar m_roots[4];
}; };

4
Extras/AlgebraicCCD/BU_Collidable.cpp
View File

@@ -16,10 +16,10 @@ subject to the following restrictions:
#include "BU_Collidable.h" #include "BU_Collidable.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
#include "BU_MotionStateInterface.h" #include "BU_MotionStateInterface.h"
BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer ) BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,btPolyhedralConvexShape& shape,void* userPointer )
:m_motionState(motion),m_shape(shape),m_userPointer(userPointer) :m_motionState(motion),m_shape(shape),m_userPointer(userPointer)
{ {
} }

10
Extras/AlgebraicCCD/BU_Collidable.h
View File

@@ -18,14 +18,14 @@ subject to the following restrictions:
#define BU_COLLIDABLE #define BU_COLLIDABLE
class PolyhedralConvexShape; class btPolyhedralConvexShape;
class BU_MotionStateInterface; class BU_MotionStateInterface;
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
class BU_Collidable class BU_Collidable
{ {
public: public:
BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape, void* userPointer); BU_Collidable(BU_MotionStateInterface& motion,btPolyhedralConvexShape& shape, void* userPointer);
void* GetUserPointer() const void* GetUserPointer() const
{ {
@@ -41,7 +41,7 @@ public:
return m_motionState; return m_motionState;
} }
inline const PolyhedralConvexShape& GetShape() const inline const btPolyhedralConvexShape& GetShape() const
{ {
return m_shape; return m_shape;
}; };
@@ -49,7 +49,7 @@ public:
private: private:
BU_MotionStateInterface& m_motionState; BU_MotionStateInterface& m_motionState;
PolyhedralConvexShape& m_shape; btPolyhedralConvexShape& m_shape;
void* m_userPointer; void* m_userPointer;
}; };

160
Extras/AlgebraicCCD/BU_CollisionPair.cpp
View File

@@ -23,13 +23,13 @@ subject to the following restrictions:
#include "BU_MotionStateInterface.h" #include "BU_MotionStateInterface.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
#include <SimdMinMax.h> #include <btSimdMinMax.h>
#include "LinearMath/SimdTransformUtil.h" #include "LinearMath/btTransformUtil.h"
BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance) BU_CollisionPair::BU_CollisionPair(const btPolyhedralConvexShape* convexA,const btPolyhedralConvexShape* convexB,btScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)), : m_convexA(convexA),m_convexB(convexB),m_screwing(btVector3(0,0,0),btVector3(0,0,0)),
m_tolerance(tolerance) m_tolerance(tolerance)
{ {
@@ -40,55 +40,55 @@ m_tolerance(tolerance)
/* /*
bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB) bool BU_CollisionPair::GetTimeOfImpact(const btVector3& linearMotionA,const btQuaternion& angularMotionA,const btVector3& linearMotionB,const btQuaternion& angularMotionB, btScalar& toi,btTransform& impactTransA,btTransform& impactTransB)
*/ */
bool BU_CollisionPair::calcTimeOfImpact( bool BU_CollisionPair::calcTimeOfImpact(
const SimdTransform& fromA, const btTransform& fromA,
const SimdTransform& toA, const btTransform& toA,
const SimdTransform& fromB, const btTransform& fromB,
const SimdTransform& toB, const btTransform& toB,
CastResult& result) CastResult& result)
{ {
SimdVector3 linvelA,angvelA; btVector3 linvelA,angvelA;
SimdVector3 linvelB,angvelB; btVector3 linvelB,angvelB;
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA); btTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB); btTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin(); btVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
SimdQuaternion angularMotionA(0,0,0,1.f); btQuaternion angularMotionA(0,0,0,1.f);
SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin(); btVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
SimdQuaternion angularMotionB(0,0,0,1); btQuaternion angularMotionB(0,0,0,1);
result.m_fraction = 1.f; result.m_fraction = 1.f;
SimdTransform impactTransA; btTransform impactTransA;
SimdTransform impactTransB; btTransform impactTransB;
int index=0; int index=0;
SimdScalar toiUnscaled=result.m_fraction; btScalar toiUnscaled=result.m_fraction;
const SimdScalar toiUnscaledLimit = result.m_fraction; const btScalar toiUnscaledLimit = result.m_fraction;
SimdTransform a2w; btTransform a2w;
a2w = fromA; a2w = fromA;
SimdTransform b2w = fromB; btTransform b2w = fromB;
/* debugging code /* debugging code
{ {
const int numvertsB = m_convexB->GetNumVertices(); const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++) for (int v=0;v<numvertsB;v++)
{ {
SimdPoint3 pt; btPoint3 pt;
m_convexB->GetVertex(v,pt); m_convexB->GetVertex(v,pt);
pt = b2w * pt; pt = b2w * pt;
char buf[1000]; char buf[1000];
@@ -110,52 +110,52 @@ bool BU_CollisionPair::calcTimeOfImpact(
*/ */
SimdTransform b2wp = b2w; btTransform b2wp = b2w;
b2wp.setOrigin(b2w.getOrigin() + linearMotionB); b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
b2wp.setRotation( b2w.getRotation() + angularMotionB); b2wp.setRotation( b2w.getRotation() + angularMotionB);
impactTransB = b2wp; impactTransB = b2wp;
SimdTransform a2wp; btTransform a2wp;
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA); a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
a2wp.setRotation(a2w.getRotation()+angularMotionA); a2wp.setRotation(a2w.getRotation()+angularMotionA);
impactTransA = a2wp; impactTransA = a2wp;
SimdTransform a2winv; btTransform a2winv;
a2winv = a2w.inverse(); a2winv = a2w.inverse();
SimdTransform b2wpinv; btTransform b2wpinv;
b2wpinv = b2wp.inverse(); b2wpinv = b2wp.inverse();
SimdTransform b2winv; btTransform b2winv;
b2winv = b2w.inverse(); b2winv = b2w.inverse();
SimdTransform a2wpinv; btTransform a2wpinv;
a2wpinv = a2wp.inverse(); a2wpinv = a2wp.inverse();
//Redon's version with concatenated transforms //Redon's version with concatenated transforms
SimdTransform relative; btTransform relative;
relative = b2w * b2wpinv * a2wp * a2winv; relative = b2w * b2wpinv * a2wp * a2winv;
//relative = a2winv * a2wp * b2wpinv * b2w; //relative = a2winv * a2wp * b2wpinv * b2w;
SimdQuaternion qrel; btQuaternion qrel;
relative.getBasis().getRotation(qrel); relative.getBasis().getRotation(qrel);
SimdVector3 linvel = relative.getOrigin(); btVector3 linvel = relative.getOrigin();
if (linvel.length() < SCREWEPSILON) if (linvel.length() < SCREWEPSILON)
{ {
linvel.setValue(0.,0.,0.); linvel.setValue(0.,0.,0.);
} }
SimdVector3 angvel; btVector3 angvel;
angvel[0] = 2.f * SimdAsin (qrel[0]); angvel[0] = 2.f * btAsin (qrel[0]);
angvel[1] = 2.f * SimdAsin (qrel[1]); angvel[1] = 2.f * btAsin (qrel[1]);
angvel[2] = 2.f * SimdAsin (qrel[2]); angvel[2] = 2.f * btAsin (qrel[2]);
if (angvel.length() < SCREWEPSILON) if (angvel.length() < SCREWEPSILON)
{ {
@@ -165,10 +165,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//Redon's version with concatenated transforms //Redon's version with concatenated transforms
m_screwing = BU_Screwing(linvel,angvel); m_screwing = BU_Screwing(linvel,angvel);
SimdTransform w2s; btTransform w2s;
m_screwing.LocalMatrix(w2s); m_screwing.LocalMatrix(w2s);
SimdTransform s2w; btTransform s2w;
s2w = w2s.inverse(); s2w = w2s.inverse();
//impactTransA = a2w; //impactTransA = a2w;
@@ -176,7 +176,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
bool hit = false; bool hit = false;
if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW())) if (btFuzzyZero(m_screwing.GetS()) && btFuzzyZero(m_screwing.GetW()))
{ {
//W = 0 , S = 0 , no collision //W = 0 , S = 0 , no collision
//toi = 0; //toi = 0;
@@ -185,7 +185,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
const int numvertsB = m_convexB->GetNumVertices(); const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++) for (int v=0;v<numvertsB;v++)
{ {
SimdPoint3 pt; btPoint3 pt;
m_convexB->GetVertex(v,pt); m_convexB->GetVertex(v,pt);
pt = impactTransB * pt; pt = impactTransB * pt;
char buf[1000]; char buf[1000];
@@ -217,7 +217,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//for all edged in A check agains all edges in B //for all edged in A check agains all edges in B
for (int ea = 0;ea < m_convexA->GetNumEdges();ea++) for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
{ {
SimdPoint3 pA0,pA1; btPoint3 pA0,pA1;
m_convexA->GetEdge(ea,pA0,pA1); m_convexA->GetEdge(ea,pA0,pA1);
@@ -231,7 +231,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
for (int eb = 0; eb < numedgesB;eb++) for (int eb = 0; eb < numedgesB;eb++)
{ {
{ {
SimdPoint3 pB0,pB1; btPoint3 pB0,pB1;
m_convexB->GetEdge(eb,pB0,pB1); m_convexB->GetEdge(eb,pB0,pB1);
pB0= b2w * pB0;//in world space pB0= b2w * pB0;//in world space
@@ -241,12 +241,12 @@ bool BU_CollisionPair::calcTimeOfImpact(
pB1 = w2s * pB1;//in screwing space pB1 = w2s * pB1;//in screwing space
SimdScalar lambda,mu; btScalar lambda,mu;
toiUnscaled = 1.; toiUnscaled = 1.;
SimdVector3 edgeDirA(pA1-pA0); btVector3 edgeDirA(pA1-pA0);
SimdVector3 edgeDirB(pB1-pB0); btVector3 edgeDirB(pB1-pB0);
if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu)) if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
{ {
@@ -258,10 +258,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//inside check is already done by checking the mu and gamma ! //inside check is already done by checking the mu and gamma !
SimdPoint3 vtx = pA0+lambda * (pA1-pA0); btPoint3 vtx = pA0+lambda * (pA1-pA0);
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdPoint3 hitptWorld = s2w * hitpt; btPoint3 hitptWorld = s2w * hitpt;
{ {
if (toiUnscaled < result.m_fraction) if (toiUnscaled < result.m_fraction)
@@ -269,7 +269,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
hit = true; hit = true;
SimdVector3 hitNormal = edgeDirB.cross(edgeDirA); btVector3 hitNormal = edgeDirB.cross(edgeDirA);
hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled); hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
@@ -279,9 +279,9 @@ bool BU_CollisionPair::calcTimeOfImpact(
//an approximated normal can be calculated by taking the cross product of both edges //an approximated normal can be calculated by taking the cross product of both edges
//take care of the sign ! //take care of the sign !
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld); btScalar dist = m_screwing.GetU().dot(hitNormalWorld);
if (dist > 0) if (dist > 0)
hitNormalWorld *= -1; hitNormalWorld *= -1;
@@ -312,7 +312,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//int v=3; //int v=3;
{ {
SimdPoint3 vtx; btPoint3 vtx;
m_convexA->GetVertex(v,vtx); m_convexA->GetVertex(v,vtx);
vtx = a2w * vtx;//in world space vtx = a2w * vtx;//in world space
@@ -326,23 +326,23 @@ bool BU_CollisionPair::calcTimeOfImpact(
{ {
SimdVector3 planeNorm; btVector3 planeNorm;
SimdPoint3 planeSupport; btPoint3 planeSupport;
m_convexB->GetPlane(planeNorm,planeSupport,p); m_convexB->GetPlane(planeNorm,planeSupport,p);
planeSupport = b2w * planeSupport;//transform to world space planeSupport = b2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm; btVector3 planeNormWorld = b2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld; planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize(); planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm); btScalar d = planeSupport.dot(planeNorm);
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); btVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxApolyB; BU_VertexPoly vtxApolyB;
@@ -368,11 +368,11 @@ bool BU_CollisionPair::calcTimeOfImpact(
{ {
// printf("toiUnscaled %f\n",toiUnscaled ); // printf("toiUnscaled %f\n",toiUnscaled );
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled); btVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt; btPoint3 hitptWorld = s2w * hitpt;
hitpt = b2winv * hitptWorld; hitpt = b2winv * hitptWorld;
@@ -408,7 +408,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//int v=0; //int v=0;
{ {
SimdPoint3 vtx; btPoint3 vtx;
m_convexB->GetVertex(v,vtx); m_convexB->GetVertex(v,vtx);
vtx = b2w * vtx;//in world space vtx = b2w * vtx;//in world space
@@ -436,23 +436,23 @@ bool BU_CollisionPair::calcTimeOfImpact(
{ {
{ {
SimdVector3 planeNorm; btVector3 planeNorm;
SimdPoint3 planeSupport; btPoint3 planeSupport;
m_convexA->GetPlane(planeNorm,planeSupport,p); m_convexA->GetPlane(planeNorm,planeSupport,p);
planeSupport = a2w * planeSupport;//transform to world space planeSupport = a2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm; btVector3 planeNormWorld = a2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld; planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize(); planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm); btScalar d = planeSupport.dot(planeNorm);
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); btVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxBpolyA; BU_VertexPoly vtxBpolyA;
@@ -464,15 +464,15 @@ bool BU_CollisionPair::calcTimeOfImpact(
{ {
if (toiUnscaled < toiUnscaledLimit) if (toiUnscaled < toiUnscaledLimit)
{ {
SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled); btPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled); btVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//SimdScalar len = hitNormal.length()-1; //btScalar len = hitNormal.length()-1;
//assert( SimdFuzzyZero(len) ); //assert( btFuzzyZero(len) );
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt; btPoint3 hitptWorld = s2w * hitpt;
hitpt = a2winv * hitptWorld; hitpt = a2winv * hitptWorld;
@@ -522,19 +522,19 @@ bool BU_CollisionPair::calcTimeOfImpact(
} else } else
{ {
//SimdScalar vel = linearMotionB.length(); //btScalar vel = linearMotionB.length();
//todo: check this margin //todo: check this margin
result.m_fraction *= 0.99f; result.m_fraction *= 0.99f;
//move B to new position //move B to new position
impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB); impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction; btQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
ornB.normalize(); ornB.normalize();
impactTransB.setRotation(ornB); impactTransB.setRotation(ornB);
//now transform A //now transform A
SimdTransform a2s,a2b; btTransform a2s,a2b;
a2s.mult( w2s , a2w); a2s.mult( w2s , a2w);
a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction); a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
a2s.multInverseLeft(w2s,a2s); a2s.multInverseLeft(w2s,a2s);
@@ -543,10 +543,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//transform by motion B //transform by motion B
impactTransA.mult(impactTransB, a2b); impactTransA.mult(impactTransB, a2b);
//normalize rotation //normalize rotation
SimdQuaternion orn; btQuaternion orn;
impactTransA.getBasis().getRotation(orn); impactTransA.getBasis().getRotation(orn);
orn.normalize(); orn.normalize();
impactTransA.setBasis(SimdMatrix3x3(orn)); impactTransA.setBasis(btMatrix3x3(orn));
} }
} }
@@ -555,7 +555,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
const int numvertsB = m_convexB->GetNumVertices(); const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++) for (int v=0;v<numvertsB;v++)
{ {
SimdPoint3 pt; btPoint3 pt;
m_convexB->GetVertex(v,pt); m_convexB->GetVertex(v,pt);
pt = impactTransB * pt; pt = impactTransB * pt;
char buf[1000]; char buf[1000];

22
Extras/AlgebraicCCD/BU_CollisionPair.h
View File

@@ -21,34 +21,34 @@ subject to the following restrictions:
#include <NarrowPhaseCollision/ConvexCast.h> #include <NarrowPhaseCollision/ConvexCast.h>
#include <SimdQuaternion.h> #include <btQuaternion.h>
class PolyhedralConvexShape; class btPolyhedralConvexShape;
///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes. ///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes.
class BU_CollisionPair : public ConvexCast class BU_CollisionPair : public btConvexCast
{ {
public: public:
BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance=0.2f); BU_CollisionPair(const btPolyhedralConvexShape* convexA,const btPolyhedralConvexShape* convexB,btScalar tolerance=0.2f);
//toi //toi
virtual bool calcTimeOfImpact( virtual bool calcTimeOfImpact(
const SimdTransform& fromA, const btTransform& fromA,
const SimdTransform& toA, const btTransform& toA,
const SimdTransform& fromB, const btTransform& fromB,
const SimdTransform& toB, const btTransform& toB,
CastResult& result); CastResult& result);
private: private:
const PolyhedralConvexShape* m_convexA; const btPolyhedralConvexShape* m_convexA;
const PolyhedralConvexShape* m_convexB; const btPolyhedralConvexShape* m_convexB;
BU_Screwing m_screwing; BU_Screwing m_screwing;
SimdScalar m_tolerance; btScalar m_tolerance;
}; };
#endif //BU_COLLISIONPAIR #endif //BU_COLLISIONPAIR

238
Extras/AlgebraicCCD/BU_EdgeEdge.cpp
View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#include "BU_EdgeEdge.h" #include "BU_EdgeEdge.h"
#include "BU_Screwing.h" #include "BU_Screwing.h"
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
//#include "BU_IntervalArithmeticPolynomialSolver.h" //#include "BU_IntervalArithmeticPolynomialSolver.h"
#include "BU_AlgebraicPolynomialSolver.h" #include "BU_AlgebraicPolynomialSolver.h"
@@ -36,37 +36,37 @@ BU_EdgeEdge::BU_EdgeEdge()
bool BU_EdgeEdge::GetTimeOfImpact( bool BU_EdgeEdge::GetTimeOfImpact(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A const btPoint3& a,//edge in object A
const SimdVector3& u, const btVector3& u,
const SimdPoint3& c,//edge in object B const btPoint3& c,//edge in object B
const SimdVector3& v, const btVector3& v,
SimdScalar &minTime, btScalar &minTime,
SimdScalar &lambda1, btScalar &lambda1,
SimdScalar& mu1 btScalar& mu1
) )
{ {
bool hit=false; bool hit=false;
SimdScalar lambda; btScalar lambda;
SimdScalar mu; btScalar mu;
const SimdScalar w=screwAB.GetW(); const btScalar w=screwAB.GetW();
const SimdScalar s=screwAB.GetS(); const btScalar s=screwAB.GetS();
if (SimdFuzzyZero(s) && if (btFuzzyZero(s) &&
SimdFuzzyZero(w)) btFuzzyZero(w))
{ {
//no motion, no collision //no motion, no collision
return false; return false;
} }
if (SimdFuzzyZero(w) ) if (btFuzzyZero(w) )
{ {
//pure translation W=0, S <> 0 //pure translation W=0, S <> 0
//no trig, f(t)=t //no trig, f(t)=t
SimdScalar det = u.y()*v.x()-u.x()*v.y(); btScalar det = u.y()*v.x()-u.x()*v.y();
if (!SimdFuzzyZero(det)) if (!btFuzzyZero(det))
{ {
lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det; lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det;
mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det; mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det;
@@ -74,7 +74,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1) if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
{ {
// single potential collision is // single potential collision is
SimdScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s; btScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
//if this is on the edge, and time t within [0..1] report hit //if this is on the edge, and time t within [0..1] report hit
if (t>=0 && t <= minTime) if (t>=0 && t <= minTime)
{ {
@@ -91,14 +91,14 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} }
} else } else
{ {
if (SimdFuzzyZero(s) ) if (btFuzzyZero(s) )
{ {
if (SimdFuzzyZero(u.z()) ) if (btFuzzyZero(u.z()) )
{ {
if (SimdFuzzyZero(v.z()) ) if (btFuzzyZero(v.z()) )
{ {
//u.z()=0,v.z()=0 //u.z()=0,v.z()=0
if (SimdFuzzyZero(a.z()-c.z())) if (btFuzzyZero(a.z()-c.z()))
{ {
//printf("NOT YET planar problem, 4 vertex=edge cases\n"); //printf("NOT YET planar problem, 4 vertex=edge cases\n");
@@ -110,7 +110,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else } else
{ {
SimdScalar mu = (a.z() - c.z())/v.z(); btScalar mu = (a.z() - c.z())/v.z();
if (0<=mu && mu <= 1) if (0<=mu && mu <= 1)
{ {
// printf("NOT YET//u.z()=0,v.z()<>0\n"); // printf("NOT YET//u.z()=0,v.z()<>0\n");
@@ -124,22 +124,22 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{ {
//u.z()<>0 //u.z()<>0
if (SimdFuzzyZero(v.z()) ) if (btFuzzyZero(v.z()) )
{ {
//printf("u.z()<>0,v.z()=0\n"); //printf("u.z()<>0,v.z()=0\n");
lambda = (c.z() - a.z())/u.z(); lambda = (c.z() - a.z())/u.z();
if (0<=lambda && lambda <= 1) if (0<=lambda && lambda <= 1)
{ {
//printf("u.z()<>0,v.z()=0\n"); //printf("u.z()<>0,v.z()=0\n");
SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f); btPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
SimdScalar r2 = rotPt.length2();//px*px + py*py; btScalar r2 = rotPt.length2();//px*px + py*py;
//either y=a*x+b, or x = a*x+b... //either y=a*x+b, or x = a*x+b...
//depends on whether value v.x() is zero or not //depends on whether value v.x() is zero or not
SimdScalar aa; btScalar aa;
SimdScalar bb; btScalar bb;
if (SimdFuzzyZero(v.x())) if (btFuzzyZero(v.x()))
{ {
aa = v.x()/v.y(); aa = v.x()/v.y();
bb= c.x()+ (-c.y() /v.y()) *v.x(); bb= c.x()+ (-c.y() /v.y()) *v.x();
@@ -155,21 +155,21 @@ bool BU_EdgeEdge::GetTimeOfImpact(
bb= c.y()+ (-c.x() /v.x()) *v.y(); bb= c.y()+ (-c.x() /v.x()) *v.y();
} }
SimdScalar disc = aa*aa*r2 + r2 - bb*bb; btScalar disc = aa*aa*r2 + r2 - bb*bb;
if (disc <0) if (disc <0)
{ {
//edge doesn't intersect the circle (motion of the vertex) //edge doesn't intersect the circle (motion of the vertex)
return false; return false;
} }
SimdScalar rad = SimdSqrt(r2); btScalar rad = btSqrt(r2);
if (SimdFuzzyZero(disc)) if (btFuzzyZero(disc))
{ {
SimdPoint3 intersectPt; btPoint3 intersectPt;
SimdScalar mu; btScalar mu;
//intersectionPoint edge with circle; //intersectionPoint edge with circle;
if (SimdFuzzyZero(v.x())) if (btFuzzyZero(v.x()))
{ {
intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1))); intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
intersectPt.setX( aa*intersectPt.y()+bb ); intersectPt.setX( aa*intersectPt.y()+bb );
@@ -191,20 +191,20 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{ {
//two points... //two points...
//intersectionPoint edge with circle; //intersectionPoint edge with circle;
SimdPoint3 intersectPt; btPoint3 intersectPt;
//intersectionPoint edge with circle; //intersectionPoint edge with circle;
if (SimdFuzzyZero(v.x())) if (btFuzzyZero(v.x()))
{ {
SimdScalar mu; btScalar mu;
intersectPt.setY((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f))); intersectPt.setY((-2.f*aa*bb+2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setX(aa*intersectPt.y()+bb); intersectPt.setX(aa*intersectPt.y()+bb);
mu = ((intersectPt.getY()-c.getY())/v.getY()); mu = ((intersectPt.getY()-c.getY())/v.getY());
if (0.f <= mu && mu <= 1.f) if (0.f <= mu && mu <= 1.f)
{ {
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime); hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
} }
intersectPt.setY((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f))); intersectPt.setY((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setX(aa*intersectPt.y()+bb); intersectPt.setX(aa*intersectPt.y()+bb);
mu = ((intersectPt.getY()-c.getY())/v.getY()); mu = ((intersectPt.getY()-c.getY())/v.getY());
if (0 <= mu && mu <= 1) if (0 <= mu && mu <= 1)
@@ -214,16 +214,16 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else } else
{ {
SimdScalar mu; btScalar mu;
intersectPt.setX((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2*(aa*aa+1.f))); intersectPt.setX((-2.f*aa*bb+2.f*btSqrt(disc))/(2*(aa*aa+1.f)));
intersectPt.setY(aa*intersectPt.x()+bb); intersectPt.setY(aa*intersectPt.x()+bb);
mu = ((intersectPt.getX()-c.getX())/v.getX()); mu = ((intersectPt.getX()-c.getX())/v.getX());
if (0 <= mu && mu <= 1) if (0 <= mu && mu <= 1)
{ {
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime); hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
} }
intersectPt.setX((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f))); intersectPt.setX((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setY(aa*intersectPt.x()+bb); intersectPt.setY(aa*intersectPt.x()+bb);
mu = ((intersectPt.getX()-c.getX())/v.getX()); mu = ((intersectPt.getX()-c.getX())/v.getX());
if (0.f <= mu && mu <= 1.f) if (0.f <= mu && mu <= 1.f)
@@ -247,7 +247,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{ {
//u.z()<>0,v.z()<>0 //u.z()<>0,v.z()<>0
//printf("general case with s=0\n"); //printf("general case with s=0\n");
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu); hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit) if (hit)
{ {
lambda1 = lambda; lambda1 = lambda;
@@ -260,7 +260,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else } else
{ {
//printf("general case, W<>0,S<>0\n"); //printf("general case, W<>0,S<>0\n");
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu); hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit) if (hit)
{ {
lambda1 = lambda; lambda1 = lambda;
@@ -277,60 +277,60 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} }
bool BU_EdgeEdge::GetTimeOfImpactGeneralCase( bool BU_EdgeEdge::GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A const btPoint3& a,//edge in object A
const SimdVector3& u, const btVector3& u,
const SimdPoint3& c,//edge in object B const btPoint3& c,//edge in object B
const SimdVector3& v, const btVector3& v,
SimdScalar &minTime, btScalar &minTime,
SimdScalar &lambda, btScalar &lambda,
SimdScalar& mu btScalar& mu
) )
{ {
bool hit = false; bool hit = false;
SimdScalar coefs[4]={0.f,0.f,0.f,0.f}; btScalar coefs[4]={0.f,0.f,0.f,0.f};
BU_Polynomial polynomialSolver; BU_Polynomial polynomialSolver;
int numroots = 0; int numroots = 0;
//SimdScalar eps=1e-15f; //btScalar eps=1e-15f;
//SimdScalar eps2=1e-20f; //btScalar eps2=1e-20f;
SimdScalar s=screwAB.GetS(); btScalar s=screwAB.GetS();
SimdScalar w = screwAB.GetW(); btScalar w = screwAB.GetW();
SimdScalar ax = a.x(); btScalar ax = a.x();
SimdScalar ay = a.y(); btScalar ay = a.y();
SimdScalar az = a.z(); btScalar az = a.z();
SimdScalar cx = c.x(); btScalar cx = c.x();
SimdScalar cy = c.y(); btScalar cy = c.y();
SimdScalar cz = c.z(); btScalar cz = c.z();
SimdScalar vx = v.x(); btScalar vx = v.x();
SimdScalar vy = v.y(); btScalar vy = v.y();
SimdScalar vz = v.z(); btScalar vz = v.z();
SimdScalar ux = u.x(); btScalar ux = u.x();
SimdScalar uy = u.y(); btScalar uy = u.y();
SimdScalar uz = u.z(); btScalar uz = u.z();
if (!SimdFuzzyZero(v.z())) if (!btFuzzyZero(v.z()))
{ {
//Maple Autogenerated C code //Maple Autogenerated C code
SimdScalar t1,t2,t3,t4,t7,t8,t10; btScalar t1,t2,t3,t4,t7,t8,t10;
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20; btScalar t13,t14,t15,t16,t17,t18,t19,t20;
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30; btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
SimdScalar t31,t32,t33,t34,t35,t36,t39,t40; btScalar t31,t32,t33,t34,t35,t36,t39,t40;
SimdScalar t41,t43,t48; btScalar t41,t43,t48;
SimdScalar t63; btScalar t63;
SimdScalar aa,bb,cc,dd;//the coefficients btScalar aa,bb,cc,dd;//the coefficients
t1 = v.y()*s; t2 = t1*u.x(); t1 = v.y()*s; t2 = t1*u.x();
t3 = v.x()*s; t3 = v.x()*s;
t4 = t3*u.y(); t4 = t3*u.y();
t7 = SimdTan(w/2.0f); t7 = btTan(w/2.0f);
t8 = 1.0f/t7; t8 = 1.0f/t7;
t10 = 1.0f/v.z(); t10 = 1.0f/v.z();
aa = (t2-t4)*t8*t10; aa = (t2-t4)*t8*t10;
@@ -377,17 +377,17 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
} else } else
{ {
SimdScalar t1,t2,t3,t4,t7,t8,t10; btScalar t1,t2,t3,t4,t7,t8,t10;
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20; btScalar t13,t14,t15,t16,t17,t18,t19,t20;
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30; btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57; btScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
SimdScalar p1,p2,p3,p4; btScalar p1,p2,p3,p4;
t1 = uy*s; t1 = uy*s;
t2 = t1*vx; t2 = t1*vx;
t3 = ux*s; t3 = ux*s;
t4 = t3*vy; t4 = t3*vy;
t7 = SimdTan(w/2.0f); t7 = btTan(w/2.0f);
t8 = 1/t7; t8 = 1/t7;
t10 = 1/uz; t10 = 1/uz;
t13 = ux*az; t13 = ux*az;
@@ -435,38 +435,38 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
for (int i=0;i<numroots;i++) for (int i=0;i<numroots;i++)
{ {
//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i); //btScalar tau = roots[i];//polynomialSolver.GetRoot(i);
SimdScalar tau = polynomialSolver.GetRoot(i); btScalar tau = polynomialSolver.GetRoot(i);
//check whether mu and lambda are in range [0..1] //check whether mu and lambda are in range [0..1]
if (!SimdFuzzyZero(v.z())) if (!btFuzzyZero(v.z()))
{ {
SimdScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz); btScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
SimdScalar B1=((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+ btScalar B1=((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+
vx*az*SimdTan(1.f/2.f*w)-vx*cz*SimdTan(1.f/2.f*w)+ vx*az*btTan(1.f/2.f*w)-vx*cz*btTan(1.f/2.f*w)+
vx*s*tau)/SimdTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay); vx*s*tau)/btTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
lambda = B1/A1; lambda = B1/A1;
mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(SimdTan(w/2.f)))/v.z(); mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(btTan(w/2.f)))/v.z();
//double check in original equation //double check in original equation
SimdScalar lhs = (a.x()+lambda*u.x()) btScalar lhs = (a.x()+lambda*u.x())
*((1.f-tau*tau)/(1.f+tau*tau))- *((1.f-tau*tau)/(1.f+tau*tau))-
(a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau)); (a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau));
lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz)); lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz));
SimdScalar rhs = c.x()+mu*v.x(); btScalar rhs = c.x()+mu*v.x();
rhs = ((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+vx*az*SimdTan(1.f/2.f*w)- rhs = ((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+vx*az*btTan(1.f/2.f*w)-
vx*cz*SimdTan(1.f/2.f*w)+vx*s*tau)/(SimdTan(1.f/2.f*w)*vz))- vx*cz*btTan(1.f/2.f*w)+vx*s*tau)/(btTan(1.f/2.f*w)*vz))-
(ax-ax*tau*tau-2.f*tau*ay); (ax-ax*tau*tau-2.f*tau*ay);
/*SimdScalar res = coefs[0]*tau*tau*tau+ /*btScalar res = coefs[0]*tau*tau*tau+
coefs[1]*tau*tau+ coefs[1]*tau*tau+
coefs[2]*tau+ coefs[2]*tau+
coefs[3];*/ coefs[3];*/
@@ -484,7 +484,7 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
} }
SimdScalar t = 2.f*SimdAtan(tau)/screwAB.GetW(); btScalar t = 2.f*btAtan(tau)/screwAB.GetW();
//tau = tan (wt/2) so 2*atan (tau)/w //tau = tan (wt/2) so 2*atan (tau)/w
if (t>=0.f && t<minTime) if (t>=0.f && t<minTime)
{ {
@@ -517,24 +517,24 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
//C -S //C -S
//S C //S C
bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime) bool BU_EdgeEdge::Calc2DRotationPointPoint(const btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime)
{ {
bool hit = false; bool hit = false;
// now calculate the planeEquation for the vertex motion, // now calculate the planeEquation for the vertex motion,
// and check if the intersectionpoint is at the positive side // and check if the intersectionpoint is at the positive side
SimdPoint3 rotPt1(SimdCos(rotW)*rotPt.x()-SimdSin(rotW)*rotPt.y(), btPoint3 rotPt1(btCos(rotW)*rotPt.x()-btSin(rotW)*rotPt.y(),
SimdSin(rotW)*rotPt.x()+SimdCos(rotW)*rotPt.y(), btSin(rotW)*rotPt.x()+btCos(rotW)*rotPt.y(),
0.f); 0.f);
SimdVector3 rotVec = rotPt1-rotPt; btVector3 rotVec = rotPt1-rotPt;
SimdVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f); btVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
//SimdPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly //btPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
SimdScalar planeD = planeNormal.dot(rotPt1); btScalar planeD = planeNormal.dot(rotPt1);
SimdScalar dist = (planeNormal.dot(intersectPt)-planeD); btScalar dist = (planeNormal.dot(intersectPt)-planeD);
hit = (dist >= -0.001); hit = (dist >= -0.001);
//if (hit) //if (hit)
@@ -545,10 +545,10 @@ bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar r
// cos (alpha) = adjacent/hypothenuse; // cos (alpha) = adjacent/hypothenuse;
//adjacent = dotproduct(ipedge,point); //adjacent = dotproduct(ipedge,point);
//hypothenuse = sqrt(r2); //hypothenuse = sqrt(r2);
SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius; btScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
SimdScalar hypo = rotRadius; btScalar hypo = rotRadius;
SimdScalar alpha = SimdAcos(adjacent/hypo); btScalar alpha = btAcos(adjacent/hypo);
SimdScalar t = alpha / rotW; btScalar t = alpha / rotW;
if (t >= 0 && t < minTime) if (t >= 0 && t < minTime)
{ {
hit = true; hit = true;
@@ -564,13 +564,13 @@ bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar r
bool BU_EdgeEdge::GetTimeOfImpactVertexEdge( bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A const btPoint3& a,//edge in object A
const SimdVector3& u, const btVector3& u,
const SimdPoint3& c,//edge in object B const btPoint3& c,//edge in object B
const SimdVector3& v, const btVector3& v,
SimdScalar &minTime, btScalar &minTime,
SimdScalar &lamda, btScalar &lamda,
SimdScalar& mu btScalar& mu
) )
{ {

54
Extras/AlgebraicCCD/BU_EdgeEdge.h
View File

@@ -18,13 +18,13 @@ subject to the following restrictions:
#define BU_EDGEEDGE #define BU_EDGEEDGE
class BU_Screwing; class BU_Screwing;
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
#include <LinearMath/SimdVector3.h> #include <LinearMath/btVector3.h>
//class BUM_Point2; //class BUM_Point2;
#include <LinearMath/SimdScalar.h> #include <LinearMath/btScalar.h>
///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges. ///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges.
class BU_EdgeEdge class BU_EdgeEdge
@@ -35,39 +35,39 @@ public:
BU_EdgeEdge(); BU_EdgeEdge();
bool GetTimeOfImpact( bool GetTimeOfImpact(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A const btPoint3& a,//edge in object A
const SimdVector3& u, const btVector3& u,
const SimdPoint3& c,//edge in object B const btPoint3& c,//edge in object B
const SimdVector3& v, const btVector3& v,
SimdScalar &minTime, btScalar &minTime,
SimdScalar &lamda, btScalar &lamda,
SimdScalar& mu btScalar& mu
); );
private: private:
bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime); bool Calc2DRotationPointPoint(const btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime);
bool GetTimeOfImpactGeneralCase( bool GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A const btPoint3& a,//edge in object A
const SimdVector3& u, const btVector3& u,
const SimdPoint3& c,//edge in object B const btPoint3& c,//edge in object B
const SimdVector3& v, const btVector3& v,
SimdScalar &minTime, btScalar &minTime,
SimdScalar &lamda, btScalar &lamda,
SimdScalar& mu btScalar& mu
); );
bool GetTimeOfImpactVertexEdge( bool GetTimeOfImpactVertexEdge(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A const btPoint3& a,//edge in object A
const SimdVector3& u, const btVector3& u,
const SimdPoint3& c,//edge in object B const btPoint3& c,//edge in object B
const SimdVector3& v, const btVector3& v,
SimdScalar &minTime, btScalar &minTime,
SimdScalar &lamda, btScalar &lamda,
SimdScalar& mu btScalar& mu
); );

30
Extras/AlgebraicCCD/BU_MotionStateInterface.h
View File

@@ -18,32 +18,32 @@ subject to the following restrictions:
#define BU_MOTIONSTATE #define BU_MOTIONSTATE
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
#include <SimdQuaternion.h> #include <btQuaternion.h>
class BU_MotionStateInterface class BU_MotionStateInterface
{ {
public: public:
virtual ~BU_MotionStateInterface(){}; virtual ~BU_MotionStateInterface(){};
virtual void SetTransform(const SimdTransform& trans) = 0; virtual void SetTransform(const btTransform& trans) = 0;
virtual void GetTransform(SimdTransform& trans) const = 0; virtual void GetTransform(btTransform& trans) const = 0;
virtual void SetPosition(const SimdPoint3& position) = 0; virtual void SetPosition(const btPoint3& position) = 0;
virtual void GetPosition(SimdPoint3& position) const = 0; virtual void GetPosition(btPoint3& position) const = 0;
virtual void SetOrientation(const SimdQuaternion& orientation) = 0; virtual void SetOrientation(const btQuaternion& orientation) = 0;
virtual void GetOrientation(SimdQuaternion& orientation) const = 0; virtual void GetOrientation(btQuaternion& orientation) const = 0;
virtual void SetBasis(const SimdMatrix3x3& basis) = 0; virtual void SetBasis(const btMatrix3x3& basis) = 0;
virtual void GetBasis(SimdMatrix3x3& basis) const = 0; virtual void GetBasis(btMatrix3x3& basis) const = 0;
virtual void SetLinearVelocity(const SimdVector3& linvel) = 0; virtual void SetLinearVelocity(const btVector3& linvel) = 0;
virtual void GetLinearVelocity(SimdVector3& linvel) const = 0; virtual void GetLinearVelocity(btVector3& linvel) const = 0;
virtual void GetAngularVelocity(SimdVector3& angvel) const = 0; virtual void GetAngularVelocity(btVector3& angvel) const = 0;
virtual void SetAngularVelocity(const SimdVector3& angvel) = 0; virtual void SetAngularVelocity(const btVector3& angvel) = 0;
}; };

10
Extras/AlgebraicCCD/BU_PolynomialSolverInterface.h
View File

@@ -15,7 +15,7 @@ subject to the following restrictions:
#ifndef BUM_POLYNOMIAL_SOLVER_INTERFACE #ifndef BUM_POLYNOMIAL_SOLVER_INTERFACE
#define BUM_POLYNOMIAL_SOLVER_INTERFACE #define BUM_POLYNOMIAL_SOLVER_INTERFACE
#include <LinearMath/SimdScalar.h> #include <LinearMath/btScalar.h>
// //
//BUM_PolynomialSolverInterface is interface class for polynomial root finding. //BUM_PolynomialSolverInterface is interface class for polynomial root finding.
//The number of roots is returned as a result, query GetRoot to get the actual solution. //The number of roots is returned as a result, query GetRoot to get the actual solution.
@@ -26,13 +26,13 @@ public:
virtual ~BUM_PolynomialSolverInterface() {}; virtual ~BUM_PolynomialSolverInterface() {};
// virtual int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) = 0; // virtual int Solve2QuadraticFull(btScalar a,btScalar b, btScalar c) = 0;
virtual int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) = 0; virtual int Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c) = 0;
virtual int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) = 0; virtual int Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d) = 0;
virtual SimdScalar GetRoot(int i) const = 0; virtual btScalar GetRoot(int i) const = 0;
}; };

90
Extras/AlgebraicCCD/BU_Screwing.cpp
View File

@@ -18,15 +18,15 @@ subject to the following restrictions:
#include "BU_Screwing.h" #include "BU_Screwing.h"
BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel) { BU_Screwing::BU_Screwing(const btVector3& relLinVel,const btVector3& relAngVel) {
const SimdScalar dx=relLinVel[0]; const btScalar dx=relLinVel[0];
const SimdScalar dy=relLinVel[1]; const btScalar dy=relLinVel[1];
const SimdScalar dz=relLinVel[2]; const btScalar dz=relLinVel[2];
const SimdScalar wx=relAngVel[0]; const btScalar wx=relAngVel[0];
const SimdScalar wy=relAngVel[1]; const btScalar wy=relAngVel[1];
const SimdScalar wz=relAngVel[2]; const btScalar wz=relAngVel[2];
// Compute the screwing parameters : // Compute the screwing parameters :
// w : total amount of rotation // w : total amount of rotation
@@ -34,25 +34,25 @@ BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngV
// u : vector along the screwing axis (||u||=1) // u : vector along the screwing axis (||u||=1)
// o : point on the screwing axis // o : point on the screwing axis
m_w=SimdSqrt(wx*wx+wy*wy+wz*wz); m_w=btSqrt(wx*wx+wy*wy+wz*wz);
//if (!w) { //if (!w) {
if (fabs(m_w)<SCREWEPSILON ) { if (fabs(m_w)<SCREWEPSILON ) {
assert(m_w == 0.f); assert(m_w == 0.f);
m_w=0.; m_w=0.;
m_s=SimdSqrt(dx*dx+dy*dy+dz*dz); m_s=btSqrt(dx*dx+dy*dy+dz*dz);
if (fabs(m_s)<SCREWEPSILON ) { if (fabs(m_s)<SCREWEPSILON ) {
assert(m_s == 0.); assert(m_s == 0.);
m_s=0.; m_s=0.;
m_u=SimdPoint3(0.,0.,1.); m_u=btPoint3(0.,0.,1.);
m_o=SimdPoint3(0.,0.,0.); m_o=btPoint3(0.,0.,0.);
} }
else { else {
float t=1.f/m_s; float t=1.f/m_s;
m_u=SimdPoint3(dx*t,dy*t,dz*t); m_u=btPoint3(dx*t,dy*t,dz*t);
m_o=SimdPoint3(0.f,0.f,0.f); m_o=btPoint3(0.f,0.f,0.f);
} }
} }
else { // there is some rotation else { // there is some rotation
@@ -60,35 +60,35 @@ BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngV
// we compute u // we compute u
float v(1.f/m_w); float v(1.f/m_w);
m_u=SimdPoint3(wx*v,wy*v,wz*v); // normalization m_u=btPoint3(wx*v,wy*v,wz*v); // normalization
// decomposition of the translation along u and one orthogonal vector // decomposition of the translation along u and one orthogonal vector
SimdPoint3 t(dx,dy,dz); btPoint3 t(dx,dy,dz);
m_s=t.dot(m_u); // component along u m_s=t.dot(m_u); // component along u
if (fabs(m_s)<SCREWEPSILON) if (fabs(m_s)<SCREWEPSILON)
{ {
//printf("m_s component along u < SCREWEPSILION\n"); //printf("m_s component along u < SCREWEPSILION\n");
m_s=0.f; m_s=0.f;
} }
SimdPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u) btPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u)
// now we have to compute o // now we have to compute o
//SimdScalar len = n1.length2(); //btScalar len = n1.length2();
//(len >= BUM_EPSILON2) { //(len >= BUM_EPSILON2) {
if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector
n1.normalize(); n1.normalize();
SimdVector3 n1orth=m_u.cross(n1); btVector3 n1orth=m_u.cross(n1);
float n2x=SimdCos(0.5f*m_w); float n2x=btCos(0.5f*m_w);
float n2y=SimdSin(0.5f*m_w); float n2y=btSin(0.5f*m_w);
m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth); m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth);
} }
else else
{ {
m_o=SimdPoint3(0.f,0.f,0.f); m_o=btPoint3(0.f,0.f,0.f);
} }
} }
@@ -99,7 +99,7 @@ BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngV
//the screwing frame : //the screwing frame :
void BU_Screwing::LocalMatrix(SimdTransform &t) const { void BU_Screwing::LocalMatrix(btTransform &t) const {
//So the whole computations do this : align the Oz axis along the //So the whole computations do this : align the Oz axis along the
// screwing axis (thanks to u), and then find two others orthogonal axes to // screwing axis (thanks to u), and then find two others orthogonal axes to
// complete the basis. // complete the basis.
@@ -107,9 +107,9 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON)) if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON))
{ {
// to avoid numerical problems // to avoid numerical problems
float n=SimdSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]); float n=btSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
float invn=1.0f/n; float invn=1.0f/n;
SimdMatrix3x3 mat; btMatrix3x3 mat;
mat[0][0]=-m_u[1]*invn; mat[0][0]=-m_u[1]*invn;
mat[0][1]=m_u[0]*invn; mat[0][1]=m_u[0]*invn;
@@ -123,7 +123,7 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
mat[2][1]=m_u[1]; mat[2][1]=m_u[1];
mat[2][2]=m_u[2]; mat[2][2]=m_u[2];
t.setOrigin(SimdPoint3( t.setOrigin(btPoint3(
m_o[0]*m_u[1]*invn-m_o[1]*m_u[0]*invn, m_o[0]*m_u[1]*invn-m_o[1]*m_u[0]*invn,
-(m_o[0]*mat[1][0]+m_o[1]*mat[1][1]+m_o[2]*n), -(m_o[0]*mat[1][0]+m_o[1]*mat[1][1]+m_o[2]*n),
-(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2]))); -(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2])));
@@ -133,24 +133,24 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
} }
else { else {
SimdMatrix3x3 m; btMatrix3x3 m;
m[0][0]=1.; m[0][0]=1.;
m[1][0]=0.; m[1][0]=0.;
m[2][0]=0.; m[2][0]=0.;
m[0][1]=0.f; m[0][1]=0.f;
m[1][1]=float(SimdSign(m_u[2])); m[1][1]=float(btSign(m_u[2]));
m[2][1]=0.f; m[2][1]=0.f;
m[0][2]=0.f; m[0][2]=0.f;
m[1][2]=0.f; m[1][2]=0.f;
m[2][2]=float(SimdSign(m_u[2])); m[2][2]=float(btSign(m_u[2]));
t.setOrigin(SimdPoint3( t.setOrigin(btPoint3(
-m_o[0], -m_o[0],
-SimdSign(m_u[2])*m_o[1], -btSign(m_u[2])*m_o[1],
-SimdSign(m_u[2])*m_o[2] -btSign(m_u[2])*m_o[2]
)); ));
t.setBasis(m); t.setBasis(m);
@@ -158,42 +158,42 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
} }
//gives interpolated transform for time in [0..1] in screwing frame //gives interpolated transform for time in [0..1] in screwing frame
SimdTransform BU_Screwing::InBetweenTransform(const SimdTransform& tr,SimdScalar t) const btTransform BU_Screwing::InBetweenTransform(const btTransform& tr,btScalar t) const
{ {
SimdPoint3 org = tr.getOrigin(); btPoint3 org = tr.getOrigin();
SimdPoint3 neworg ( btPoint3 neworg (
org.x()*SimdCos(m_w*t)-org.y()*SimdSin(m_w*t), org.x()*btCos(m_w*t)-org.y()*btSin(m_w*t),
org.x()*SimdSin(m_w*t)+org.y()*SimdCos(m_w*t), org.x()*btSin(m_w*t)+org.y()*btCos(m_w*t),
org.z()+m_s*CalculateF(t)); org.z()+m_s*CalculateF(t));
SimdTransform newtr; btTransform newtr;
newtr.setOrigin(neworg); newtr.setOrigin(neworg);
SimdMatrix3x3 basis = tr.getBasis(); btMatrix3x3 basis = tr.getBasis();
SimdMatrix3x3 basisorg = tr.getBasis(); btMatrix3x3 basisorg = tr.getBasis();
SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t); btQuaternion rot(btVector3(0.,0.,1.),m_w*t);
SimdQuaternion tmpOrn; btQuaternion tmpOrn;
tr.getBasis().getRotation(tmpOrn); tr.getBasis().getRotation(tmpOrn);
rot = rot * tmpOrn; rot = rot * tmpOrn;
//to avoid numerical drift, normalize quaternion //to avoid numerical drift, normalize quaternion
rot.normalize(); rot.normalize();
newtr.setBasis(SimdMatrix3x3(rot)); newtr.setBasis(btMatrix3x3(rot));
return newtr; return newtr;
} }
SimdScalar BU_Screwing::CalculateF(SimdScalar t) const btScalar BU_Screwing::CalculateF(btScalar t) const
{ {
SimdScalar result; btScalar result;
if (!m_w) if (!m_w)
{ {
result = t; result = t;
} else } else
{ {
result = ( SimdTan((m_w*t)/2.f) / SimdTan(m_w/2.f)); result = ( btTan((m_w*t)/2.f) / btTan(m_w/2.f));
} }
return result; return result;
} }

42
Extras/AlgebraicCCD/BU_Screwing.h
View File

@@ -18,9 +18,9 @@ subject to the following restrictions:
#define B_SCREWING_H #define B_SCREWING_H
#include <LinearMath/SimdVector3.h> #include <LinearMath/btVector3.h>
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
#define SCREWEPSILON 0.00001f #define SCREWEPSILON 0.00001f
@@ -31,47 +31,47 @@ class BU_Screwing
public: public:
BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel); BU_Screwing(const btVector3& relLinVel,const btVector3& relAngVel);
~BU_Screwing() { ~BU_Screwing() {
}; };
SimdScalar CalculateF(SimdScalar t) const; btScalar CalculateF(btScalar t) const;
//gives interpolated position for time in [0..1] in screwing frame //gives interpolated position for time in [0..1] in screwing frame
inline SimdPoint3 InBetweenPosition(const SimdPoint3& pt,SimdScalar t) const inline btPoint3 InBetweenPosition(const btPoint3& pt,btScalar t) const
{ {
return SimdPoint3( return btPoint3(
pt.x()*SimdCos(m_w*t)-pt.y()*SimdSin(m_w*t), pt.x()*btCos(m_w*t)-pt.y()*btSin(m_w*t),
pt.x()*SimdSin(m_w*t)+pt.y()*SimdCos(m_w*t), pt.x()*btSin(m_w*t)+pt.y()*btCos(m_w*t),
pt.z()+m_s*CalculateF(t)); pt.z()+m_s*CalculateF(t));
} }
inline SimdVector3 InBetweenVector(const SimdVector3& vec,SimdScalar t) const inline btVector3 InBetweenVector(const btVector3& vec,btScalar t) const
{ {
return SimdVector3( return btVector3(
vec.x()*SimdCos(m_w*t)-vec.y()*SimdSin(m_w*t), vec.x()*btCos(m_w*t)-vec.y()*btSin(m_w*t),
vec.x()*SimdSin(m_w*t)+vec.y()*SimdCos(m_w*t), vec.x()*btSin(m_w*t)+vec.y()*btCos(m_w*t),
vec.z()); vec.z());
} }
//gives interpolated transform for time in [0..1] in screwing frame //gives interpolated transform for time in [0..1] in screwing frame
SimdTransform InBetweenTransform(const SimdTransform& tr,SimdScalar t) const; btTransform InBetweenTransform(const btTransform& tr,btScalar t) const;
//gives matrix from global frame into screwing frame //gives matrix from global frame into screwing frame
void LocalMatrix(SimdTransform &t) const; void LocalMatrix(btTransform &t) const;
inline const SimdVector3& GetU() const { return m_u;} inline const btVector3& GetU() const { return m_u;}
inline const SimdVector3& GetO() const {return m_o;} inline const btVector3& GetO() const {return m_o;}
inline const SimdScalar GetS() const{ return m_s;} inline const btScalar GetS() const{ return m_s;}
inline const SimdScalar GetW() const { return m_w;} inline const btScalar GetW() const { return m_w;}
private: private:
float m_w; float m_w;
float m_s; float m_s;
SimdVector3 m_u; btVector3 m_u;
SimdVector3 m_o; btVector3 m_o;
}; };
#endif //B_SCREWING_H #endif //B_SCREWING_H

32
Extras/AlgebraicCCD/BU_StaticMotionState.h
View File

@@ -18,62 +18,62 @@ subject to the following restrictions:
#define BU_STATIC_MOTIONSTATE #define BU_STATIC_MOTIONSTATE
#include <CollisionShapes/BU_MotionStateInterface.h> #include <btCollisionShapes/BU_MotionStateInterface.h>
class BU_StaticMotionState :public BU_MotionStateInterface class BU_StaticMotionState :public BU_MotionStateInterface
{ {
public: public:
virtual ~BU_StaticMotionState(){}; virtual ~BU_StaticMotionState(){};
virtual void SetTransform(const SimdTransform& trans) virtual void SetTransform(const btTransform& trans)
{ {
m_trans = trans; m_trans = trans;
} }
virtual void GetTransform(SimdTransform& trans) const virtual void GetTransform(btTransform& trans) const
{ {
trans = m_trans; trans = m_trans;
} }
virtual void SetPosition(const SimdPoint3& position) virtual void SetPosition(const btPoint3& position)
{ {
m_trans.setOrigin( position ); m_trans.setOrigin( position );
} }
virtual void GetPosition(SimdPoint3& position) const virtual void GetPosition(btPoint3& position) const
{ {
position = m_trans.getOrigin(); position = m_trans.getOrigin();
} }
virtual void SetOrientation(const SimdQuaternion& orientation) virtual void SetOrientation(const btQuaternion& orientation)
{ {
m_trans.setRotation( orientation); m_trans.setRotation( orientation);
} }
virtual void GetOrientation(SimdQuaternion& orientation) const virtual void GetOrientation(btQuaternion& orientation) const
{ {
orientation = m_trans.getRotation(); orientation = m_trans.getRotation();
} }
virtual void SetBasis(const SimdMatrix3x3& basis) virtual void SetBasis(const btMatrix3x3& basis)
{ {
m_trans.setBasis( basis); m_trans.setBasis( basis);
} }
virtual void GetBasis(SimdMatrix3x3& basis) const virtual void GetBasis(btMatrix3x3& basis) const
{ {
basis = m_trans.getBasis(); basis = m_trans.getBasis();
} }
virtual void SetLinearVelocity(const SimdVector3& linvel) virtual void SetLinearVelocity(const btVector3& linvel)
{ {
m_linearVelocity = linvel; m_linearVelocity = linvel;
} }
virtual void GetLinearVelocity(SimdVector3& linvel) const virtual void GetLinearVelocity(btVector3& linvel) const
{ {
linvel = m_linearVelocity; linvel = m_linearVelocity;
} }
virtual void SetAngularVelocity(const SimdVector3& angvel) virtual void SetAngularVelocity(const btVector3& angvel)
{ {
m_angularVelocity = angvel; m_angularVelocity = angvel;
} }
virtual void GetAngularVelocity(SimdVector3& angvel) const virtual void GetAngularVelocity(btVector3& angvel) const
{ {
angvel = m_angularVelocity; angvel = m_angularVelocity;
} }
@@ -82,9 +82,9 @@ public:
protected: protected:
SimdTransform m_trans; btTransform m_trans;
SimdVector3 m_angularVelocity; btVector3 m_angularVelocity;
SimdVector3 m_linearVelocity; btVector3 m_linearVelocity;
}; };

44
Extras/AlgebraicCCD/BU_VertexPoly.cpp
View File

@@ -17,9 +17,9 @@ subject to the following restrictions:
#include "BU_VertexPoly.h" #include "BU_VertexPoly.h"
#include "BU_Screwing.h" #include "BU_Screwing.h"
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
#include <LinearMath/SimdVector3.h> #include <LinearMath/btVector3.h>
#define USE_ALGEBRAIC #define USE_ALGEBRAIC
#ifdef USE_ALGEBRAIC #ifdef USE_ALGEBRAIC
@@ -30,7 +30,7 @@ subject to the following restrictions:
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver #define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
#endif #endif
inline bool TestFuzzyZero(SimdScalar x) { return SimdFabs(x) < 0.0001f; } inline bool TestFuzzyZero(btScalar x) { return btFabs(x) < 0.0001f; }
BU_VertexPoly::BU_VertexPoly() BU_VertexPoly::BU_VertexPoly()
@@ -41,9 +41,9 @@ BU_VertexPoly::BU_VertexPoly()
//false otherwise. If true, minTime contains the time of impact //false otherwise. If true, minTime contains the time of impact
bool BU_VertexPoly::GetTimeOfImpact( bool BU_VertexPoly::GetTimeOfImpact(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& a, const btPoint3& a,
const SimdVector4& planeEq, const btVector4& planeEq,
SimdScalar &minTime,bool swapAB) btScalar &minTime,bool swapAB)
{ {
bool hit = false; bool hit = false;
@@ -57,21 +57,21 @@ bool BU_VertexPoly::GetTimeOfImpact(
//case w<>0 and s<> 0 //case w<>0 and s<> 0
const SimdScalar w=screwAB.GetW(); const btScalar w=screwAB.GetW();
const SimdScalar s=screwAB.GetS(); const btScalar s=screwAB.GetS();
SimdScalar coefs[4]; btScalar coefs[4];
const SimdScalar p=planeEq[0]; const btScalar p=planeEq[0];
const SimdScalar q=planeEq[1]; const btScalar q=planeEq[1];
const SimdScalar r=planeEq[2]; const btScalar r=planeEq[2];
const SimdScalar d=planeEq[3]; const btScalar d=planeEq[3];
const SimdVector3 norm(p,q,r); const btVector3 norm(p,q,r);
BU_Polynomial polynomialSolver; BU_Polynomial polynomialSolver;
int numroots = 0; int numroots = 0;
//SimdScalar eps=1e-80f; //btScalar eps=1e-80f;
//SimdScalar eps2=1e-100f; //btScalar eps2=1e-100f;
if (TestFuzzyZero(screwAB.GetS()) ) if (TestFuzzyZero(screwAB.GetS()) )
{ {
@@ -93,7 +93,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
// W = 0 , S <> 0 // W = 0 , S <> 0
//pax+qay+r(az+st)=d //pax+qay+r(az+st)=d
SimdScalar dist = (d - a.dot(norm)); btScalar dist = (d - a.dot(norm));
if (TestFuzzyZero(r)) if (TestFuzzyZero(r))
{ {
@@ -108,7 +108,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
} else } else
{ {
SimdScalar etoi = (dist)/(r*screwAB.GetS()); btScalar etoi = (dist)/(r*screwAB.GetS());
if (swapAB) if (swapAB)
etoi *= -1; etoi *= -1;
@@ -124,7 +124,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
//ax^3+bx^2+cx+d=0 //ax^3+bx^2+cx+d=0
//degenerate coefficients mess things up :( //degenerate coefficients mess things up :(
SimdScalar ietsje = (r*s)/SimdTan(w/2.f); btScalar ietsje = (r*s)/btTan(w/2.f);
if (ietsje*ietsje < 0.01f) if (ietsje*ietsje < 0.01f)
ietsje = 0.f; ietsje = 0.f;
@@ -140,9 +140,9 @@ bool BU_VertexPoly::GetTimeOfImpact(
for (int i=0;i<numroots;i++) for (int i=0;i<numroots;i++)
{ {
SimdScalar tau = polynomialSolver.GetRoot(i); btScalar tau = polynomialSolver.GetRoot(i);
SimdScalar t = 2.f*SimdAtan(tau)/w; btScalar t = 2.f*btAtan(tau)/w;
//tau = tan (wt/2) so 2*atan (tau)/w //tau = tan (wt/2) so 2*atan (tau)/w
if (swapAB) if (swapAB)
{ {

12
Extras/AlgebraicCCD/BU_VertexPoly.h
View File

@@ -19,9 +19,9 @@ subject to the following restrictions:
class BU_Screwing; class BU_Screwing;
#include <LinearMath/SimdTransform.h> #include <LinearMath/btTransform.h>
#include <LinearMath/SimdPoint3.h> #include <LinearMath/btPoint3.h>
#include <LinearMath/SimdScalar.h> #include <LinearMath/btScalar.h>
///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane. ///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane.
class BU_VertexPoly class BU_VertexPoly
@@ -30,9 +30,9 @@ public:
BU_VertexPoly(); BU_VertexPoly();
bool GetTimeOfImpact( bool GetTimeOfImpact(
const BU_Screwing& screwAB, const BU_Screwing& screwAB,
const SimdPoint3& vtx, const btPoint3& vtx,
const SimdVector4& planeEq, const btVector4& planeEq,
SimdScalar &minTime, btScalar &minTime,
bool swapAB); bool swapAB);
private: private:

36
Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.cpp
View File

@@ -13,13 +13,13 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SphereSphereCollisionAlgorithm.h" #include "btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
SphereSphereCollisionAlgorithm::SphereSphereCollisionAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
: CollisionAlgorithm(ci), : btCollisionAlgorithm(ci),
m_ownManifold(false), m_ownManifold(false),
m_manifoldPtr(mf) m_manifoldPtr(mf)
{ {
@@ -30,7 +30,7 @@ m_manifoldPtr(mf)
} }
} }
SphereSphereCollisionAlgorithm::~SphereSphereCollisionAlgorithm() btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
{ {
if (m_ownManifold) if (m_ownManifold)
{ {
@@ -39,42 +39,42 @@ SphereSphereCollisionAlgorithm::~SphereSphereCollisionAlgorithm()
} }
} }
void SphereSphereCollisionAlgorithm::ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) void btSphereSphereCollisionAlgorithm::ProcessCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
{ {
if (!m_manifoldPtr) if (!m_manifoldPtr)
return; return;
CollisionObject* col0 = static_cast<CollisionObject*>(proxy0->m_clientObject); btCollisionObject* col0 = static_cast<btCollisionObject*>(proxy0->m_clientObject);
CollisionObject* col1 = static_cast<CollisionObject*>(proxy1->m_clientObject); btCollisionObject* col1 = static_cast<btCollisionObject*>(proxy1->m_clientObject);
SphereShape* sphere0 = (SphereShape*)col0->m_collisionShape; btSphereShape* sphere0 = (btSphereShape*)col0->m_collisionShape;
SphereShape* sphere1 = (SphereShape*)col1->m_collisionShape; btSphereShape* sphere1 = (btSphereShape*)col1->m_collisionShape;
SimdVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin(); btVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin();
float len = diff.length(); float len = diff.length();
SimdScalar radius0 = sphere0->GetRadius(); btScalar radius0 = sphere0->GetRadius();
SimdScalar radius1 = sphere1->GetRadius(); btScalar radius1 = sphere1->GetRadius();
///iff distance positive, don't generate a new contact ///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1)) if ( len > (radius0+radius1))
return; return;
///distance (negative means penetration) ///distance (negative means penetration)
SimdScalar dist = len - (radius0+radius1); btScalar dist = len - (radius0+radius1);
SimdVector3 normalOnSurfaceB = diff / len; btVector3 normalOnSurfaceB = diff / len;
///point on A (worldspace) ///point on A (worldspace)
SimdVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB; btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace) ///point on B (worldspace)
SimdVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB; btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr); btManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
resultOut->AddContactPoint(normalOnSurfaceB,pos1,dist); resultOut->AddContactPoint(normalOnSurfaceB,pos1,dist);
m_dispatcher->ReleaseManifoldResult(resultOut); m_dispatcher->ReleaseManifoldResult(resultOut);
} }
float SphereSphereCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) float btSphereSphereCollisionAlgorithm::CalculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
{ {
//not yet //not yet
return 1.f; return 1.f;

28
Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.h
View File

@@ -19,33 +19,33 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class PersistentManifold; class btPersistentManifold;
/// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response. /// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user CollisionAlgorithm /// Also provides the most basic sample for custom/user btCollisionAlgorithm
class SphereSphereCollisionAlgorithm : public CollisionAlgorithm class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
{ {
bool m_ownManifold; bool m_ownManifold;
PersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;
public: public:
SphereSphereCollisionAlgorithm(const CollisionAlgorithmConstructionInfo& ci) btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: CollisionAlgorithm(ci) {} : btCollisionAlgorithm(ci) {}
virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); virtual void ProcessCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
virtual float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); virtual float CalculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
SphereSphereCollisionAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
virtual ~SphereSphereCollisionAlgorithm(); virtual ~btSphereSphereCollisionAlgorithm();
struct CreateFunc :public CollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual CollisionAlgorithm* CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo& ci, BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
{ {
return new SphereSphereCollisionAlgorithm(0,ci,proxy0,proxy1); return new btSphereSphereCollisionAlgorithm(0,ci,proxy0,proxy1);
} }
}; };

136
Extras/EPA/Epa.cpp
View File

@@ -15,11 +15,11 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/SimdScalar.h" #include "LinearMath/btScalar.h"
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "LinearMath/SimdMinMax.h" #include "LinearMath/btSimdMinMax.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
@@ -37,11 +37,11 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/EpaPolyhedron.h" #include "NarrowPhaseCollision/EpaPolyhedron.h"
#include "NarrowPhaseCollision/Epa.h" #include "NarrowPhaseCollision/Epa.h"
const SimdScalar EPA_MAX_RELATIVE_ERROR = 1e-2f; const btScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR; const btScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB, Epa::Epa( btConvexShape* pConvexShapeA, btConvexShape* pConvexShapeB,
const SimdTransform& transformA, const SimdTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ), const btTransform& transformA, const btTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
m_pConvexShapeB( pConvexShapeB ), m_pConvexShapeB( pConvexShapeB ),
m_transformA( transformA ), m_transformA( transformA ),
m_transformB( transformB ) m_transformB( transformB )
@@ -53,14 +53,14 @@ Epa::~Epa()
{ {
} }
bool Epa::Initialize( SimplexSolverInterface& simplexSolver ) bool Epa::Initialize( btSimplexSolverInterface& simplexSolver )
{ {
// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO // Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
SimdVector3 v( 1, 0, 0 ); btVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY; btScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f; btScalar delta = 0.f;
simplexSolver.reset(); simplexSolver.reset();
@@ -70,16 +70,16 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
{ {
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" ); EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis(); btVector3 seperatingAxisInA = -v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis(); btVector3 seperatingAxisInB = v * m_transformB.getBasis();
SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); btVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); btVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( pInA ); btPoint3 pWorld = m_transformA( pInA );
SimdPoint3 qWorld = m_transformB( qInB ); btPoint3 qWorld = m_transformB( qInB );
SimdVector3 w = pWorld - qWorld; btVector3 w = pWorld - qWorld;
delta = v.dot( w ); delta = v.dot( w );
EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" ); EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" );
@@ -98,7 +98,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
if (!closestOk) if (!closestOk)
return false; return false;
SimdScalar prevVSqrd = squaredDistance; btScalar prevVSqrd = squaredDistance;
squaredDistance = v.length2(); squaredDistance = v.length2();
// Is v converging to v(A-B) ? // Is v converging to v(A-B) ?
@@ -115,9 +115,9 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
++nbIterations; ++nbIterations;
} }
SimdPoint3 simplexPoints[ 5 ]; btPoint3 simplexPoints[ 5 ];
SimdPoint3 wSupportPointsOnA[ 5 ]; btPoint3 wSupportPointsOnA[ 5 ];
SimdPoint3 wSupportPointsOnB[ 5 ]; btPoint3 wSupportPointsOnB[ 5 ];
int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints ); int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
@@ -142,18 +142,18 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
// We have a line segment inside the CSO that contains the origin // We have a line segment inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points // Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ]; btVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
d.normalize(); d.normalize();
SimdVector3 v1; btVector3 v1;
SimdVector3 v2; btVector3 v2;
SimdVector3 v3; btVector3 v3;
SimdVector3 e1; btVector3 e1;
SimdScalar absx = abs( d.getX() ); btScalar absx = abs( d.getX() );
SimdScalar absy = abs( d.getY() ); btScalar absy = abs( d.getY() );
SimdScalar absz = abs( d.getZ() ); btScalar absz = abs( d.getZ() );
if ( absx < absy ) if ( absx < absy )
{ {
@@ -186,14 +186,14 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
nbPolyhedronPoints = 5; nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis(); btVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis(); btVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p ); btPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q ); btPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 2 ] = pWorld; wSupportPointsOnA[ 2 ] = pWorld;
wSupportPointsOnB[ 2 ] = qWorld; wSupportPointsOnB[ 2 ] = qWorld;
@@ -255,21 +255,21 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
// We have a triangle inside the CSO that contains the origin // We have a triangle inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points // Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ]; btVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ]; btVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
SimdVector3 triangleNormal = v0.cross( v1 ); btVector3 triangleNormal = v0.cross( v1 );
triangleNormal.normalize(); triangleNormal.normalize();
nbPolyhedronPoints = 5; nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis(); btVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis(); btVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p ); btPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q ); btPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld; wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld; wSupportPointsOnB[ 3 ] = qWorld;
@@ -316,17 +316,17 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
#endif #endif
#ifndef EPA_POLYHEDRON_USE_PLANES #ifndef EPA_POLYHEDRON_USE_PLANES
SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ], btPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
simplexPoints[ initTetraIndices[ 1 ] ], simplexPoints[ initTetraIndices[ 1 ] ],
simplexPoints[ initTetraIndices[ 2 ] ], simplexPoints[ initTetraIndices[ 2 ] ],
simplexPoints[ initTetraIndices[ 3 ] ] }; simplexPoints[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ], btPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
wSupportPointsOnA[ initTetraIndices[ 1 ] ], wSupportPointsOnA[ initTetraIndices[ 1 ] ],
wSupportPointsOnA[ initTetraIndices[ 2 ] ], wSupportPointsOnA[ initTetraIndices[ 2 ] ],
wSupportPointsOnA[ initTetraIndices[ 3 ] ] }; wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ], btPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
wSupportPointsOnB[ initTetraIndices[ 1 ] ], wSupportPointsOnB[ initTetraIndices[ 1 ] ],
wSupportPointsOnB[ initTetraIndices[ 2 ] ], wSupportPointsOnB[ initTetraIndices[ 2 ] ],
wSupportPointsOnB[ initTetraIndices[ 3 ] ] }; wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
@@ -398,16 +398,16 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
return true; return true;
} }
SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ) btScalar Epa::CalcPenDepth( btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
{ {
SimdVector3 v; btVector3 v;
SimdScalar upperBoundSqrd = SIMD_INFINITY; btScalar upperBoundSqrd = SIMD_INFINITY;
SimdScalar vSqrd = 0; btScalar vSqrd = 0;
#ifdef _DEBUG #ifdef _DEBUG
SimdScalar prevVSqrd; btScalar prevVSqrd;
#endif #endif
SimdScalar delta; btScalar delta;
bool isCloseEnough = false; bool isCloseEnough = false;
@@ -445,20 +445,20 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
#endif //_DEBUG #endif //_DEBUG
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" ); EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" );
SimdVector3 seperatingAxisInA = v * m_transformA.getBasis(); btVector3 seperatingAxisInA = v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis(); btVector3 seperatingAxisInB = -v * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p ); btPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q ); btPoint3 qWorld = m_transformB( q );
SimdPoint3 w = pWorld - qWorld; btPoint3 w = pWorld - qWorld;
delta = v.dot( w ); delta = v.dot( w );
// Keep tighest upper bound // Keep tighest upper bound
upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd ); upperBoundSqrd = btMin( upperBoundSqrd, delta * delta / vSqrd );
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" ); //assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd ); isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
@@ -544,10 +544,10 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
return v.length(); return v.length();
} }
bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1, bool Epa::TetrahedronContainsOrigin( const btPoint3& point0, const btPoint3& point1,
const SimdPoint3& point2, const SimdPoint3& point3 ) const btPoint3& point2, const btPoint3& point3 )
{ {
SimdVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ), btVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ),
( point2 - point1 ).cross( point3 - point1 ), ( point2 - point1 ).cross( point3 - point1 ),
( point3 - point2 ).cross( point0 - point2 ), ( point3 - point2 ).cross( point0 - point2 ),
( point0 - point3 ).cross( point1 - point3 ) }; ( point0 - point3 ).cross( point1 - point3 ) };
@@ -558,7 +558,7 @@ bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3&
( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) ); ( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) );
} }
bool Epa::TetrahedronContainsOrigin( SimdPoint3* pPoints ) bool Epa::TetrahedronContainsOrigin( btPoint3* pPoints )
{ {
return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] ); return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] );
} }

26
Extras/EPA/Epa.h
View File

@@ -21,8 +21,8 @@ subject to the following restrictions:
extern const SimdScalar EPA_MAX_RELATIVE_ERROR; extern const btScalar EPA_MAX_RELATIVE_ERROR;
extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD; extern const btScalar EPA_MAX_RELATIVE_ERROR_SQRD;
class Epa class Epa
{ {
@@ -34,30 +34,30 @@ class Epa
public : public :
Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB, Epa( btConvexShape* pConvexShapeA, btConvexShape* pConvexShapeB,
const SimdTransform& transformA, const SimdTransform& transformB ); const btTransform& transformA, const btTransform& transformB );
~Epa(); ~Epa();
bool Initialize( SimplexSolverInterface& simplexSolver ); bool Initialize( btSimplexSolverInterface& simplexSolver );
SimdScalar CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ); btScalar CalcPenDepth( btPoint3& wWitnessOnA, btPoint3& wWitnessOnB );
private : private :
bool TetrahedronContainsOrigin( SimdPoint3* pPoints ); bool TetrahedronContainsOrigin( btPoint3* pPoints );
bool TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1, bool TetrahedronContainsOrigin( const btPoint3& point0, const btPoint3& point1,
const SimdPoint3& point2, const SimdPoint3& point3 ); const btPoint3& point2, const btPoint3& point3 );
private : private :
//! Priority queue //! Priority queue
std::vector< EpaFace* > m_faceEntries; std::vector< EpaFace* > m_faceEntries;
ConvexShape* m_pConvexShapeA; btConvexShape* m_pConvexShapeA;
ConvexShape* m_pConvexShapeB; btConvexShape* m_pConvexShapeB;
SimdTransform m_transformA; btTransform m_transformA;
SimdTransform m_transformB; btTransform m_transformB;
EpaPolyhedron m_polyhedron; EpaPolyhedron m_polyhedron;
}; };

52
Extras/EPA/EpaFace.cpp
View File

@@ -14,9 +14,9 @@ subject to the following restrictions:
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/SimdScalar.h" #include "LinearMath/btScalar.h"
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
#include "NarrowPhaseCollision/EpaCommon.h" #include "NarrowPhaseCollision/EpaCommon.h"
@@ -25,7 +25,7 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/EpaFace.h" #include "NarrowPhaseCollision/EpaFace.h"
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar PLANE_THICKNESS = 1e-5f; btScalar PLANE_THICKNESS = 1e-5f;
#endif #endif
EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false ) EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false )
@@ -43,17 +43,17 @@ bool EpaFace::Initialize()
CollectVertices( m_pVertices ); CollectVertices( m_pVertices );
const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdScalar e0Sqrd = e0.length2(); const btScalar e0Sqrd = e0.length2();
const SimdScalar e1Sqrd = e1.length2(); const btScalar e1Sqrd = e1.length2();
const SimdScalar e0e1 = e0.dot( e1 ); const btScalar e0e1 = e0.dot( e1 );
m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1; m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
const SimdScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point ); const btScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
const SimdScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point ); const btScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0; m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0; m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
@@ -83,8 +83,8 @@ bool EpaFace::CalculatePlane()
// Traditional method // Traditional method
const SimdVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; const btVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; const btVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
m_planeNormal = v2.cross( v1 ); m_planeNormal = v2.cross( v1 );
@@ -99,10 +99,10 @@ bool EpaFace::CalculatePlane()
// Robust method // Robust method
//SimdVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; //btVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
//SimdVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; //btVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
//SimdVector3 n; //btVector3 n;
//n = _v2.cross( _v1 ); //n = _v2.cross( _v1 );
@@ -119,8 +119,8 @@ bool EpaFace::CalculatePlane()
//n /= 3; //n /= 3;
//n.normalize(); //n.normalize();
//SimdVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3; //btVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
//SimdScalar d = c.dot( -n ); //btScalar d = c.dot( -n );
//m_robustPlaneNormal = n; //m_robustPlaneNormal = n;
//m_robustPlaneDistance = d; //m_robustPlaneDistance = d;
@@ -142,8 +142,8 @@ bool EpaFace::CalculatePlane()
void EpaFace::CalcClosestPoint() void EpaFace::CalcClosestPoint()
{ {
const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
m_v = m_pVertices[ 0 ]->m_point + m_v = m_pVertices[ 0 ]->m_point +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant; ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant;
@@ -151,20 +151,20 @@ void EpaFace::CalcClosestPoint()
m_vSqrd = m_v.length2(); m_vSqrd = m_v.length2();
} }
void EpaFace::CalcClosestPointOnA( SimdVector3& closestPointOnA ) void EpaFace::CalcClosestPointOnA( btVector3& closestPointOnA )
{ {
const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA; const btVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA; const btVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
closestPointOnA = m_pVertices[ 0 ]->m_wSupportPointOnA + closestPointOnA = m_pVertices[ 0 ]->m_wSupportPointOnA +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /
m_determinant; m_determinant;
} }
void EpaFace::CalcClosestPointOnB( SimdVector3& closestPointOnB ) void EpaFace::CalcClosestPointOnB( btVector3& closestPointOnB )
{ {
const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB; const btVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB; const btVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
closestPointOnB = m_pVertices[ 0 ]->m_wSupportPointOnB + closestPointOnB = m_pVertices[ 0 ]->m_wSupportPointOnB +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /

22
Extras/EPA/EpaFace.h
View File

@@ -21,7 +21,7 @@ class EpaVertex;
class EpaHalfEdge; class EpaHalfEdge;
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
extern SimdScalar PLANE_THICKNESS; extern btScalar PLANE_THICKNESS;
#endif #endif
//! Note : This class is not supposed to be a base class //! Note : This class is not supposed to be a base class
@@ -44,8 +44,8 @@ class EpaFace
bool CalculatePlane(); bool CalculatePlane();
#endif #endif
void CalcClosestPoint(); void CalcClosestPoint();
void CalcClosestPointOnA( SimdVector3& closestPointOnA ); void CalcClosestPointOnA( btVector3& closestPointOnA );
void CalcClosestPointOnB( SimdVector3& closestPointOnB ); void CalcClosestPointOnB( btVector3& closestPointOnB );
bool IsAffinelyDependent() const; bool IsAffinelyDependent() const;
bool IsClosestPointInternal() const; bool IsClosestPointInternal() const;
@@ -63,18 +63,18 @@ class EpaFace
EpaVertex* m_pVertices[ 3 ]; EpaVertex* m_pVertices[ 3 ];
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 m_planeNormal; btVector3 m_planeNormal;
SimdScalar m_planeDistance; btScalar m_planeDistance;
//SimdVector3 m_robustPlaneNormal; //btVector3 m_robustPlaneNormal;
//SimdScalar m_robustPlaneDistance; //btScalar m_robustPlaneDistance;
#endif #endif
SimdVector3 m_v; btVector3 m_v;
SimdScalar m_vSqrd; btScalar m_vSqrd;
SimdScalar m_determinant; btScalar m_determinant;
SimdScalar m_lambdas[ 2 ]; btScalar m_lambdas[ 2 ];
bool m_deleted; bool m_deleted;
}; };

74
Extras/EPA/EpaPenetrationDepthSolver.cpp
View File

@@ -14,11 +14,11 @@ subject to the following restrictions:
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/SimdScalar.h" #include "LinearMath/btScalar.h"
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "LinearMath/SimdMinMax.h" #include "LinearMath/btSimdMinMax.h"
#include <list> #include <list>
@@ -36,19 +36,19 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" #include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
SimdScalar g_GJKMaxRelError = 1e-3f; btScalar g_GJKMaxRelError = 1e-3f;
SimdScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError; btScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver, bool EpaPenetrationDepthSolver::CalcPenDepth( btSimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB, btConvexShape* pConvexA, btConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB, const btTransform& transformA, const btTransform& transformB,
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class IDebugDraw* debugDraw ) class btIDebugDraw* debugDraw )
{ {
EPA_DEBUG_ASSERT( pConvexA ,"Convex shape A is invalid!" ); EPA_DEBUG_ASSERT( pConvexA ,"Convex shape A is invalid!" );
EPA_DEBUG_ASSERT( pConvexB ,"Convex shape B is invalid!" ); EPA_DEBUG_ASSERT( pConvexB ,"Convex shape B is invalid!" );
SimdScalar penDepth; btScalar penDepth;
#ifdef EPA_USE_HYBRID #ifdef EPA_USE_HYBRID
bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB, bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
@@ -70,17 +70,17 @@ bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSol
} }
#ifdef EPA_USE_HYBRID #ifdef EPA_USE_HYBRID
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver, bool EpaPenetrationDepthSolver::HybridPenDepth( btSimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB, btConvexShape* pConvexA, btConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB, const btTransform& transformA, const btTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v ) btScalar& penDepth, btVector3& v )
{ {
SimdScalar squaredDistance = SIMD_INFINITY; btScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f; btScalar delta = 0.f;
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin(); const btScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const SimdScalar marginSqrd = margin * margin; const btScalar marginSqrd = margin * margin;
simplexSolver.reset(); simplexSolver.reset();
@@ -90,16 +90,16 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
{ {
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" ); assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * transformA.getBasis(); btVector3 seperatingAxisInA = -v * transformA.getBasis();
SimdVector3 seperatingAxisInB = v * transformB.getBasis(); btVector3 seperatingAxisInB = v * transformB.getBasis();
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA ); btVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB ); btVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = transformA( pInA ); btPoint3 pWorld = transformA( pInA );
SimdPoint3 qWorld = transformB( qInB ); btPoint3 qWorld = transformB( qInB );
SimdVector3 w = pWorld - qWorld; btVector3 w = pWorld - qWorld;
delta = v.dot( w ); delta = v.dot( w );
// potential exit, they don't overlap // potential exit, they don't overlap
@@ -117,7 +117,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance ); btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@@ -137,7 +137,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance ); btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@@ -148,7 +148,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
return true; return true;
} }
SimdScalar previousSquaredDistance = squaredDistance; btScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2(); squaredDistance = v.length2();
//are we getting any closer ? //are we getting any closer ?
@@ -160,7 +160,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance ); btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@@ -184,10 +184,10 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
} }
#endif #endif
SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver, btScalar EpaPenetrationDepthSolver::EpaPenDepth( btSimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB, btConvexShape* pConvexA, btConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB, const btTransform& transformA, const btTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ) btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
{ {
Epa epa( pConvexA, pConvexB, transformA, transformB ); Epa epa( pConvexA, pConvexB, transformA, transformB );

34
Extras/EPA/EpaPenetrationDepthSolver.h
View File

@@ -22,34 +22,34 @@ subject to the following restrictions:
* calculate the penetration depth between two convex shapes. * calculate the penetration depth between two convex shapes.
*/ */
extern SimdScalar g_GJKMaxRelError; extern btScalar g_GJKMaxRelError;
extern SimdScalar g_GJKMaxRelErrorSqrd; extern btScalar g_GJKMaxRelErrorSqrd;
//! Note : This class is not supposed to be a base class //! Note : This class is not supposed to be a base class
class EpaPenetrationDepthSolver : public ConvexPenetrationDepthSolver class EpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{ {
public : public :
bool CalcPenDepth( SimplexSolverInterface& simplexSolver, bool CalcPenDepth( btSimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB, btConvexShape* pConvexA, btConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB, const btTransform& transformA, const btTransform& transformB,
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class IDebugDraw* debugDraw ); class btIDebugDraw* debugDraw );
private : private :
#ifdef EPA_USE_HYBRID #ifdef EPA_USE_HYBRID
bool HybridPenDepth( SimplexSolverInterface& simplexSolver, bool HybridPenDepth( btSimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB, btConvexShape* pConvexA, btConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB, const btTransform& transformA, const btTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v ); btScalar& penDepth, btVector3& v );
#endif #endif
SimdScalar EpaPenDepth( SimplexSolverInterface& simplexSolver, btScalar EpaPenDepth( btSimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB, btConvexShape* pConvexA, btConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB, const btTransform& transformA, const btTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ); btPoint3& wWitnessOnA, btPoint3& wWitnessOnB );
}; };
#endif // EPA_PENETRATION_DEPTH_H #endif // EPA_PENETRATION_DEPTH_H

70
Extras/EPA/EpaPolyhedron.cpp
View File

@@ -14,9 +14,9 @@ subject to the following restrictions:
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/SimdScalar.h" #include "LinearMath/btScalar.h"
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/SimdPoint3.h" #include "LinearMath/btPoint3.h"
#include "Memory2.h" #include "Memory2.h"
#include <list> #include <list>
@@ -42,8 +42,8 @@ EpaPolyhedron::~EpaPolyhedron()
Destroy(); Destroy();
} }
bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints, bool EpaPolyhedron::Create( btPoint3* pInitialPoints,
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB, btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints ) const int nbInitialPoints )
{ {
#ifndef EPA_POLYHEDRON_USE_PLANES #ifndef EPA_POLYHEDRON_USE_PLANES
@@ -134,8 +134,8 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{ {
int axisIndex = axisOrderIndices[ axis ]; int axisIndex = axisOrderIndices[ axis ];
SimdScalar axisMin = SIMD_INFINITY; btScalar axisMin = SIMD_INFINITY;
SimdScalar axisMax = -SIMD_INFINITY; btScalar axisMax = -SIMD_INFINITY;
for ( int i = 0; i < 4; ++i ) for ( int i = 0; i < 4; ++i )
{ {
@@ -194,36 +194,36 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; btVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; btVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else #else
SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ]; btVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ]; btVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif #endif
SimdVector3 planeNormal = v1.cross( v0 ); btVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize(); planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal ); btScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else #else
SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal ); btScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif #endif
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
bool pointOnPlane0 = SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ); bool pointOnPlane0 = btEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane0) if (!pointOnPlane0)
{ {
EPA_DEBUG_ASSERT(0,"Point0 should be on plane!"); EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
return false; return false;
} }
bool pointOnPlane1 = SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ); bool pointOnPlane1 = btEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane1) if (!pointOnPlane1)
{ {
EPA_DEBUG_ASSERT(0,"Point1 should be on plane!"); EPA_DEBUG_ASSERT(0,"Point1 should be on plane!");
return false; return false;
} }
bool pointOnPlane2 = SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ); bool pointOnPlane2 = btEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane2) if (!pointOnPlane2)
{ {
EPA_DEBUG_ASSERT(0,"Point2 should be on plane!"); EPA_DEBUG_ASSERT(0,"Point2 should be on plane!");
@@ -235,7 +235,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{ {
if ( planeDistance > 0 ) if ( planeDistance > 0 )
{ {
SimdVector3 tmp = pInitialPoints[ 1 ]; btVector3 tmp = pInitialPoints[ 1 ];
pInitialPoints[ 1 ] = pInitialPoints[ 2 ]; pInitialPoints[ 1 ] = pInitialPoints[ 2 ];
pInitialPoints[ 2 ] = tmp; pInitialPoints[ 2 ] = tmp;
@@ -256,16 +256,16 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
#else #else
finalPointsIndices[ 3 ] = -1; finalPointsIndices[ 3 ] = -1;
SimdScalar absMaxDist = -SIMD_INFINITY; btScalar absMaxDist = -SIMD_INFINITY;
SimdScalar maxDist; btScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex ) for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{ {
SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance; btScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
SimdScalar absDist = abs( dist ); btScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) && if ( ( absDist > absMaxDist ) &&
!SimdEqual( dist, PLANE_THICKNESS ) ) !btEqual( dist, PLANE_THICKNESS ) )
{ {
absMaxDist = absDist; absMaxDist = absDist;
maxDist = dist; maxDist = dist;
@@ -283,7 +283,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{ {
// Can swap indices only // Can swap indices only
SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ]; btPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ]; pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ];
pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp; pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp;
@@ -425,7 +425,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{ {
EpaFace* pFace = *facesItr; EpaFace* pFace = *facesItr;
SimdScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance; btScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance;
if ( dist > PLANE_THICKNESS ) if ( dist > PLANE_THICKNESS )
{ {
@@ -486,9 +486,9 @@ EpaHalfEdge* EpaPolyhedron::CreateHalfEdge()
return pNewHalfEdge; return pNewHalfEdge;
} }
EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint, EpaVertex* EpaPolyhedron::CreateVertex( const btPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA, const btPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB ) const btPoint3& wSupportPointOnB )
{ {
EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB );
EPA_DEBUG_ASSERT( pNewVertex ,"Failed to allocate memory for a new EpaVertex!" ); EPA_DEBUG_ASSERT( pNewVertex ,"Failed to allocate memory for a new EpaVertex!" );
@@ -542,9 +542,9 @@ void EpaPolyhedron::DestroyAllVertices()
} }
} }
bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint, bool EpaPolyhedron::Expand( const btPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA, const btPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB, const btPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces ) EpaFace* pFace, std::list< EpaFace* >& newFaces )
{ {
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" ); EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
@@ -555,7 +555,7 @@ bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint,
// wSupportPoint must be front of face's plane used to do the expansion // wSupportPoint must be front of face's plane used to do the expansion
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance; btScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance;
if ( dist <= PLANE_THICKNESS ) if ( dist <= PLANE_THICKNESS )
{ {
return false; return false;
@@ -601,7 +601,7 @@ int EpaPolyhedron::GetNbFaces() const
return m_faces.size(); return m_faces.size();
} }
void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace, void EpaPolyhedron::DeleteVisibleFaces( const btPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges ) std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges )
{ {
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" ); EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
@@ -622,12 +622,12 @@ void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
EPA_DEBUG_ASSERT( ( pAdjacentFace->m_planeNormal.length2() > 0 ) ,"Invalid plane!" ); EPA_DEBUG_ASSERT( ( pAdjacentFace->m_planeNormal.length2() > 0 ) ,"Invalid plane!" );
SimdScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) + btScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) +
pAdjacentFace->m_planeDistance; pAdjacentFace->m_planeDistance;
if ( pointDist > PLANE_THICKNESS ) if ( pointDist > PLANE_THICKNESS )
#else #else
SimdScalar dot = pAdjacentFace->m_v.dot( point ); btScalar dot = pAdjacentFace->m_v.dot( point );
if ( dot >= pAdjacentFace->m_vSqrd ) if ( dot >= pAdjacentFace->m_vSqrd )
#endif #endif
{ {

18
Extras/EPA/EpaPolyhedron.h
View File

@@ -34,16 +34,16 @@ class EpaPolyhedron
EpaPolyhedron(); EpaPolyhedron();
~EpaPolyhedron(); ~EpaPolyhedron();
bool Create( SimdPoint3* pInitialPoints, bool Create( btPoint3* pInitialPoints,
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB, btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints ); const int nbInitialPoints );
void Destroy(); void Destroy();
EpaFace* CreateFace(); EpaFace* CreateFace();
EpaHalfEdge* CreateHalfEdge(); EpaHalfEdge* CreateHalfEdge();
EpaVertex* CreateVertex( const SimdPoint3& wSupportPoint, EpaVertex* CreateVertex( const btPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA, const btPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB ); const btPoint3& wSupportPointOnB );
void DeleteFace( EpaFace* pFace ); void DeleteFace( EpaFace* pFace );
@@ -51,9 +51,9 @@ class EpaPolyhedron
void DestroyAllHalfEdges(); void DestroyAllHalfEdges();
void DestroyAllVertices(); void DestroyAllVertices();
bool Expand( const SimdPoint3& wSupportPoint, bool Expand( const btPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA, const btPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB, const btPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces ); EpaFace* pFace, std::list< EpaFace* >& newFaces );
std::list< EpaFace* >& GetFaces(); std::list< EpaFace* >& GetFaces();
@@ -61,7 +61,7 @@ class EpaPolyhedron
private : private :
void DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace, void DeleteVisibleFaces( const btPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges ); std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges );
void CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges, void CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges,

14
Extras/EPA/EpaVertex.h
View File

@@ -30,13 +30,13 @@ class EpaVertex
public : public :
EpaVertex( const SimdPoint3& point ) : /*m_pHalfEdge( 0 ),*/ m_point( point ) EpaVertex( const btPoint3& point ) : /*m_pHalfEdge( 0 ),*/ m_point( point )
{ {
} }
EpaVertex( const SimdPoint3& point, EpaVertex( const btPoint3& point,
const SimdPoint3& wSupportPointOnA, const btPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ), const btPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ),
m_wSupportPointOnA( wSupportPointOnA ), m_wSupportPointOnA( wSupportPointOnA ),
m_wSupportPointOnB( wSupportPointOnB ) m_wSupportPointOnB( wSupportPointOnB )
{ {
@@ -51,10 +51,10 @@ class EpaVertex
//! This is not necessary //! This is not necessary
//EpaHalfEdge* m_pHalfEdge; //EpaHalfEdge* m_pHalfEdge;
SimdPoint3 m_point; btPoint3 m_point;
SimdPoint3 m_wSupportPointOnA; btPoint3 m_wSupportPointOnA;
SimdPoint3 m_wSupportPointOnB; btPoint3 m_wSupportPointOnB;
}; };
#endif #endif

42
Extras/ExtraSolid35/CombinedSimplexSolver.cpp
View File

@@ -30,7 +30,7 @@ void CombinedSimplexSolver::reset()
} }
void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q) void CombinedSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{ {
printf("addVertex (%f %f %f)\n",w[0],w[1],w[2]); printf("addVertex (%f %f %f)\n",w[0],w[1],w[2]);
m_voronoiSolver.addVertex(w,p,q); m_voronoiSolver.addVertex(w,p,q);
@@ -38,8 +38,8 @@ void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
int i; int i;
i=0; i=0;
SimdPoint3 vp1,vp2; btPoint3 vp1,vp2;
SimdPoint3 jp1,jp2; btPoint3 jp1,jp2;
/* /*
bool isClosest0 = m_voronoiSolver.closest(vp1); bool isClosest0 = m_voronoiSolver.closest(vp1);
bool isClosest1 = m_johnsonSolver.closest(vp1); bool isClosest1 = m_johnsonSolver.closest(vp1);
@@ -52,12 +52,12 @@ void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
bool CombinedSimplexSolver::closest(SimdVector3& v) bool CombinedSimplexSolver::closest(btVector3& v)
{ {
bool result0 = 0; bool result0 = 0;
bool result1 = 0; bool result1 = 0;
SimdVector3 v0,v1; btVector3 v0,v1;
result0 = m_voronoiSolver.closest(v0); result0 = m_voronoiSolver.closest(v0);
result1 = m_johnsonSolver.closest(v1); result1 = m_johnsonSolver.closest(v1);
@@ -79,10 +79,10 @@ bool CombinedSimplexSolver::closest(SimdVector3& v)
return result1; return result1;
} }
SimdScalar CombinedSimplexSolver::maxVertex() btScalar CombinedSimplexSolver::maxVertex()
{ {
SimdScalar maxv0 = m_voronoiSolver.maxVertex(); btScalar maxv0 = m_voronoiSolver.maxVertex();
SimdScalar maxv1 = m_johnsonSolver.maxVertex(); btScalar maxv1 = m_johnsonSolver.maxVertex();
MY_ASSERT(maxv0 = maxv1); MY_ASSERT(maxv0 = maxv1);
if (m_useVoronoiSolver) if (m_useVoronoiSolver)
return maxv0; return maxv0;
@@ -102,7 +102,7 @@ bool CombinedSimplexSolver::fullSimplex() const
return fullSimplex1; return fullSimplex1;
} }
int CombinedSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const int CombinedSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{ {
@@ -119,12 +119,12 @@ int CombinedSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVe
void CombinedSimplexSolver::debugPrint() void CombinedSimplexSolver::debugPrint()
{ {
SimdPoint3 pBuf0[4]; btPoint3 pBuf0[4];
SimdPoint3 qBuf0[4]; btPoint3 qBuf0[4];
SimdPoint3 yBuf0[4]; btPoint3 yBuf0[4];
SimdPoint3 pBuf1[4]; btPoint3 pBuf1[4];
SimdPoint3 qBuf1[4]; btPoint3 qBuf1[4];
SimdPoint3 yBuf1[4]; btPoint3 yBuf1[4];
int verts0,verts1; int verts0,verts1;
verts0 = m_voronoiSolver.getSimplex(&pBuf0[0], &qBuf0[0], &yBuf0[0]); verts0 = m_voronoiSolver.getSimplex(&pBuf0[0], &qBuf0[0], &yBuf0[0]);
@@ -146,7 +146,7 @@ void CombinedSimplexSolver::debugPrint()
} }
bool CombinedSimplexSolver::inSimplex(const SimdVector3& w) bool CombinedSimplexSolver::inSimplex(const btVector3& w)
{ {
bool insimplex0 = m_voronoiSolver.inSimplex(w); bool insimplex0 = m_voronoiSolver.inSimplex(w);
bool insimplex1 = m_johnsonSolver.inSimplex(w); bool insimplex1 = m_johnsonSolver.inSimplex(w);
@@ -163,9 +163,9 @@ bool CombinedSimplexSolver::inSimplex(const SimdVector3& w)
return insimplex1; return insimplex1;
} }
void CombinedSimplexSolver::backup_closest(SimdVector3& v) void CombinedSimplexSolver::backup_closest(btVector3& v)
{ {
SimdVector3 v0,v1; btVector3 v0,v1;
m_voronoiSolver.backup_closest(v0); m_voronoiSolver.backup_closest(v0);
m_johnsonSolver.backup_closest(v1); m_johnsonSolver.backup_closest(v1);
@@ -189,10 +189,10 @@ bool CombinedSimplexSolver::emptySimplex() const
return empty1; return empty1;
} }
void CombinedSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2) void CombinedSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{ {
SimdPoint3 tmpP1,tmpP2; btPoint3 tmpP1,tmpP2;
SimdPoint3 tmpJP1,tmpJP2; btPoint3 tmpJP1,tmpJP2;
m_voronoiSolver.compute_points(tmpP1,tmpP2); m_voronoiSolver.compute_points(tmpP1,tmpP2);
m_johnsonSolver.compute_points(tmpJP1,tmpJP2); m_johnsonSolver.compute_points(tmpJP1,tmpJP2);

20
Extras/ExtraSolid35/CombinedSimplexSolver.h
View File

@@ -17,10 +17,10 @@
#include "Solid3JohnsonSimplexSolver.h" #include "Solid3JohnsonSimplexSolver.h"
/// CombinedSimplexSolver runs both Solid and Voronoi Simplex Solver for comparison /// CombinedSimplexSolver runs both Solid and Voronoi Simplex Solver for comparison
class CombinedSimplexSolver: public SimplexSolverInterface class CombinedSimplexSolver: public btSimplexSolverInterface
{ {
VoronoiSimplexSolver m_voronoiSolver; btVoronoiSimplexSolver m_voronoiSolver;
// VoronoiSimplexSolver m_johnsonSolver; // btVoronoiSimplexSolver m_johnsonSolver;
Solid3JohnsonSimplexSolver m_johnsonSolver; Solid3JohnsonSimplexSolver m_johnsonSolver;
@@ -35,23 +35,23 @@ class CombinedSimplexSolver: public SimplexSolverInterface
virtual void reset(); virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q); virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
virtual bool closest(SimdVector3& v); virtual bool closest(btVector3& v);
virtual SimdScalar maxVertex(); virtual btScalar maxVertex();
virtual bool fullSimplex() const; virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const; virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w); virtual bool inSimplex(const btVector3& w);
virtual void backup_closest(SimdVector3& v) ; virtual void backup_closest(btVector3& v) ;
virtual bool emptySimplex() const; virtual bool emptySimplex() const;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2); virtual void compute_points(btPoint3& p1, btPoint3& p2);
virtual int numVertices() const; virtual int numVertices() const;

154
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.cpp
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file. * LICENSE.QPL included in the packaging of this file.
* *
* This library may be distributed and/or modified under the terms of the * This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software * GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the * Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file. * packaging of this file.
* *
@@ -26,34 +26,34 @@
#include <vector> #include <vector>
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "LinearMath/GenMinMax.h" #include "LinearMath/btMinMax.h"
#define ASSERT_MESSAGE #define ASSERT_MESSAGE
class ReplaceMeAccuracy { class ReplaceMeAccuracy {
public: public:
static SimdScalar rel_error2; // squared relative error in the computed distance static btScalar rel_error2; // squared relative error in the computed distance
static SimdScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2 static btScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
static SimdScalar tol_error; // error tolerance if the distance is almost zero static btScalar tol_error; // error tolerance if the distance is almost zero
static void setAccuracy(SimdScalar rel_error) static void setAccuracy(btScalar rel_error)
{ {
rel_error2 = rel_error * rel_error; rel_error2 = rel_error * rel_error;
depth_tolerance = SimdScalar(1.0f) + SimdScalar(2.0f) * rel_error; depth_tolerance = btScalar(1.0f) + btScalar(2.0f) * rel_error;
} }
static void setTolerance(SimdScalar epsilon) static void setTolerance(btScalar epsilon)
{ {
tol_error = epsilon; tol_error = epsilon;
} }
}; };
static const SimdScalar rel_error = SimdScalar(1.0e-3); static const btScalar rel_error = btScalar(1.0e-3);
SimdScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error; btScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error;
SimdScalar ReplaceMeAccuracy::depth_tolerance = SimdScalar(1.0) + SimdScalar(2.0) * rel_error; btScalar ReplaceMeAccuracy::depth_tolerance = btScalar(1.0) + btScalar(2.0) * rel_error;
SimdScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON; btScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
@@ -101,28 +101,28 @@ public:
bool isObsolete() const { return m_obsolete; } bool isObsolete() const { return m_obsolete; }
bool computeClosest(const SimdVector3 *verts); bool computeClosest(const btVector3 *verts);
const SimdVector3& getClosest() const { return m_closest; } const btVector3& getClosest() const { return m_closest; }
bool isClosestInternal() const bool isClosestInternal() const
{ {
return m_lambda1 >= SimdScalar(0.0) && return m_lambda1 >= btScalar(0.0) &&
m_lambda2 >= SimdScalar(0.0) && m_lambda2 >= btScalar(0.0) &&
m_lambda1 + m_lambda2 <= m_det; m_lambda1 + m_lambda2 <= m_det;
} }
SimdScalar getDist2() const { return m_dist2; } btScalar getDist2() const { return m_dist2; }
SimdPoint3 getClosestPoint(const SimdPoint3 *points) const btPoint3 getClosestPoint(const btPoint3 *points) const
{ {
const SimdPoint3& p0 = points[m_indices[0]]; const btPoint3& p0 = points[m_indices[0]];
return p0 + (m_lambda1 * (points[m_indices[1]] - p0) + return p0 + (m_lambda1 * (points[m_indices[1]] - p0) +
m_lambda2 * (points[m_indices[2]] - p0)) / m_det; m_lambda2 * (points[m_indices[2]] - p0)) / m_det;
} }
void silhouette(const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer) void silhouette(const btVector3& w, ReplaceMeEdgeBuffer& edgeBuffer)
{ {
edgeBuffer.clear(); edgeBuffer.clear();
m_obsolete = true; m_obsolete = true;
@@ -132,18 +132,18 @@ public:
} }
private: private:
void silhouette(int index, const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer); void silhouette(int index, const btVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
int m_indices[3]; int m_indices[3];
bool m_obsolete; bool m_obsolete;
ReplaceMeFacet *m_adjFacets[3]; ReplaceMeFacet *m_adjFacets[3];
int m_adjEdges[3]; int m_adjEdges[3];
SimdScalar m_det; btScalar m_det;
SimdScalar m_lambda1; btScalar m_lambda1;
SimdScalar m_lambda2; btScalar m_lambda2;
SimdVector3 m_closest; btVector3 m_closest;
SimdScalar m_dist2; btScalar m_dist2;
}; };
@@ -163,17 +163,17 @@ bool ReplaceMeFacet::link(int edge0, ReplaceMeFacet *facet, int edge1)
} }
bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts) bool ReplaceMeFacet::computeClosest(const btVector3 *verts)
{ {
const SimdVector3& p0 = verts[m_indices[0]]; const btVector3& p0 = verts[m_indices[0]];
SimdVector3 v1 = verts[m_indices[1]] - p0; btVector3 v1 = verts[m_indices[1]] - p0;
SimdVector3 v2 = verts[m_indices[2]] - p0; btVector3 v2 = verts[m_indices[2]] - p0;
SimdScalar v1dv1 = v1.length2(); btScalar v1dv1 = v1.length2();
SimdScalar v1dv2 = v1.dot(v2); btScalar v1dv2 = v1.dot(v2);
SimdScalar v2dv2 = v2.length2(); btScalar v2dv2 = v2.length2();
SimdScalar p0dv1 = p0.dot(v1); btScalar p0dv1 = p0.dot(v1);
SimdScalar p0dv2 = p0.dot(v2); btScalar p0dv2 = p0.dot(v2);
m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative
//printf("m_det = %f\n",m_det); //printf("m_det = %f\n",m_det);
@@ -192,7 +192,7 @@ bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
return false; return false;
} }
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w, void ReplaceMeFacet::silhouette(int index, const btVector3& w,
ReplaceMeEdgeBuffer& edgeBuffer) ReplaceMeEdgeBuffer& edgeBuffer)
{ {
if (!m_obsolete) { if (!m_obsolete) {
@@ -227,9 +227,9 @@ inline int ReplaceMeEdge::getTarget() const
const int MaxSupportPoints = 100;//1000; const int MaxSupportPoints = 100;//1000;
const int MaxFacets = 200;//b2000; const int MaxFacets = 200;//b2000;
static SimdPoint3 pBuf[MaxSupportPoints]; static btPoint3 pBuf[MaxSupportPoints];
static SimdPoint3 qBuf[MaxSupportPoints]; static btPoint3 qBuf[MaxSupportPoints];
static SimdVector3 yBuf[MaxSupportPoints]; static btVector3 yBuf[MaxSupportPoints];
static ReplaceMeFacet facetBuf[MaxFacets]; static ReplaceMeFacet facetBuf[MaxFacets];
static int freeFacet = 0; static int freeFacet = 0;
@@ -249,7 +249,7 @@ public:
ReplaceMeFacetComp myFacetComp; ReplaceMeFacetComp myFacetComp;
inline ReplaceMeFacet *addFacet(int i0, int i1, int i2, inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
SimdScalar lower2, SimdScalar upper2) btScalar lower2, btScalar upper2)
{ {
assert(i0 != i1 && i0 != i2 && i1 != i2); assert(i0 != i1 && i0 != i2 && i1 != i2);
if (freeFacet < MaxFacets) if (freeFacet < MaxFacets)
@@ -300,27 +300,27 @@ inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
inline bool originInTetrahedron(const SimdVector3& p1, const SimdVector3& p2, inline bool originInTetrahedron(const btVector3& p1, const btVector3& p2,
const SimdVector3& p3, const SimdVector3& p4) const btVector3& p3, const btVector3& p4)
{ {
SimdVector3 normal1 = (p2 - p1).cross(p3 - p1); btVector3 normal1 = (p2 - p1).cross(p3 - p1);
SimdVector3 normal2 = (p3 - p2).cross(p4 - p2); btVector3 normal2 = (p3 - p2).cross(p4 - p2);
SimdVector3 normal3 = (p4 - p3).cross(p1 - p3); btVector3 normal3 = (p4 - p3).cross(p1 - p3);
SimdVector3 normal4 = (p1 - p4).cross(p2 - p4); btVector3 normal4 = (p1 - p4).cross(p2 - p4);
return return
(normal1.dot(p1) > SimdScalar(0.0)) != (normal1.dot(p4) > SimdScalar(0.0)) && (normal1.dot(p1) > btScalar(0.0)) != (normal1.dot(p4) > btScalar(0.0)) &&
(normal2.dot(p2) > SimdScalar(0.0)) != (normal2.dot(p1) > SimdScalar(0.0)) && (normal2.dot(p2) > btScalar(0.0)) != (normal2.dot(p1) > btScalar(0.0)) &&
(normal3.dot(p3) > SimdScalar(0.0)) != (normal3.dot(p2) > SimdScalar(0.0)) && (normal3.dot(p3) > btScalar(0.0)) != (normal3.dot(p2) > btScalar(0.0)) &&
(normal4.dot(p4) > SimdScalar(0.0)) != (normal4.dot(p3) > SimdScalar(0.0)); (normal4.dot(p4) > btScalar(0.0)) != (normal4.dot(p3) > btScalar(0.0));
} }
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver, bool Solid3EpaPenetrationDepth::CalcPenDepth( btSimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB, btConvexShape* convexA,btConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB, const btTransform& transformA,const btTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb) btVector3& v, btPoint3& pa, btPoint3& pb)
{ {
int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf); int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);
@@ -336,17 +336,17 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
// We have a line segment inside the Minkowski sum containing the // We have a line segment inside the Minkowski sum containing the
// origin. Blow it up by adding three additional support points. // origin. Blow it up by adding three additional support points.
SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized(); btVector3 dir = (yBuf[1] - yBuf[0]).normalized();
int axis = dir.furthestAxis(); int axis = dir.furthestAxis();
static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;// static btScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5)); btQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, btScalar(0.5));
SimdMatrix3x3 rot_mat(rot); btMatrix3x3 rot_mat(rot);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2)); btVector3 aux1 = dir.cross(btVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1; btVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2; btVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis())); pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis())); qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
@@ -387,9 +387,9 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
// We have a triangle inside the Minkowski sum containing // We have a triangle inside the Minkowski sum containing
// the origin. First blow it up. // the origin. First blow it up.
SimdVector3 v1 = yBuf[1] - yBuf[0]; btVector3 v1 = yBuf[1] - yBuf[0];
SimdVector3 v2 = yBuf[2] - yBuf[0]; btVector3 v2 = yBuf[2] - yBuf[0];
SimdVector3 vv = v1.cross(v2); btVector3 vv = v1.cross(v2);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis())); pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis())); qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
@@ -430,15 +430,15 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
num_facets = 0; num_facets = 0;
freeFacet = 0; freeFacet = 0;
ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f0 = addFacet(0, 1, 2, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f1 = addFacet(0, 3, 1, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f2 = addFacet(0, 2, 3, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f3 = addFacet(1, 3, 2, btScalar(0.0), SIMD_INFINITY);
if (!f0 || f0->getDist2() == SimdScalar(0.0) || if (!f0 || f0->getDist2() == btScalar(0.0) ||
!f1 || f1->getDist2() == SimdScalar(0.0) || !f1 || f1->getDist2() == btScalar(0.0) ||
!f2 || f2->getDist2() == SimdScalar(0.0) || !f2 || f2->getDist2() == btScalar(0.0) ||
!f3 || f3->getDist2() == SimdScalar(0.0)) !f3 || f3->getDist2() == btScalar(0.0))
{ {
return false; return false;
} }
@@ -461,7 +461,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
ReplaceMeFacet *facet = 0; ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY; btScalar upper_bound2 = SIMD_INFINITY;
do { do {
facet = facetHeap[0]; facet = facetHeap[0];
@@ -470,7 +470,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
if (!facet->isObsolete()) if (!facet->isObsolete())
{ {
assert(facet->getDist2() > SimdScalar(0.0)); assert(facet->getDist2() > btScalar(0.0));
if (num_verts == MaxSupportPoints) if (num_verts == MaxSupportPoints)
{ {
@@ -487,16 +487,16 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
int index = num_verts++; int index = num_verts++;
SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest()); btScalar far_dist2 = yBuf[index].dot(facet->getClosest());
// Make sure the support mapping is OK. // Make sure the support mapping is OK.
//assert(far_dist2 > SimdScalar(0.0)); //assert(far_dist2 > btScalar(0.0));
// //
// this is to avoid problems with implicit-sphere-touching contact // this is to avoid problems with implicit-sphere-touching contact
// //
if (far_dist2 < SimdScalar(0.0)) if (far_dist2 < btScalar(0.0))
{ {
return false; return false;
} }

12
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file. * LICENSE.QPL included in the packaging of this file.
* *
* This library may be distributed and/or modified under the terms of the * This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software * GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the * Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file. * packaging of this file.
* *
@@ -28,14 +28,14 @@
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
/// Solid3EpaPenetrationDepth contains the 'Expanding Polytope Algorithm' from Solid 3.5 /// Solid3EpaPenetrationDepth contains the 'Expanding Polytope Algorithm' from Solid 3.5
class Solid3EpaPenetrationDepth : public ConvexPenetrationDepthSolver class Solid3EpaPenetrationDepth : public btConvexPenetrationDepthSolver
{ {
public: public:
virtual bool CalcPenDepth(SimplexSolverInterface& simplexSolver, virtual bool CalcPenDepth(btSimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB, btConvexShape* convexA,btConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB, const btTransform& transformA,const btTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb); btVector3& v, btPoint3& pa, btPoint3& pb);
}; };

54
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.cpp
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file. * LICENSE.QPL included in the packaging of this file.
* *
* This library may be distributed and/or modified under the terms of the * This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software * GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the * Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file. * packaging of this file.
* *
@@ -22,7 +22,7 @@
*/ */
#include "Solid3JohnsonSimplexSolver.h" #include "Solid3JohnsonSimplexSolver.h"
#include "LinearMath/GenMinMax.h" #include "LinearMath/btMinMax.h"
//#define USE_BACKUP_PROCEDURE //#define USE_BACKUP_PROCEDURE
//#define FAST_CLOSEST //#define FAST_CLOSEST
@@ -48,7 +48,7 @@ void Solid3JohnsonSimplexSolver::reset()
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w) void Solid3JohnsonSimplexSolver::addVertex(const btVector3& w)
{ {
assert(!fullSimplex()); assert(!fullSimplex());
m_last = 0; m_last = 0;
@@ -66,7 +66,7 @@ void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
compute_det(); compute_det();
} }
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q) void Solid3JohnsonSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{ {
addVertex(w); addVertex(w);
m_p[m_last] = p; m_p[m_last] = p;
@@ -83,12 +83,12 @@ bool Solid3JohnsonSimplexSolver::fullSimplex() const
return m_bits1 == 0xf; return m_bits1 == 0xf;
} }
SimdScalar Solid3JohnsonSimplexSolver::maxVertex() btScalar Solid3JohnsonSimplexSolver::maxVertex()
{ {
return m_maxlen2; return m_maxlen2;
} }
bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v) bool Solid3JohnsonSimplexSolver::closest(btVector3& v)
{ {
#ifdef FAST_CLOSEST #ifdef FAST_CLOSEST
T_Bits s; T_Bits s;
@@ -125,14 +125,14 @@ bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
// Original GJK calls the backup procedure at this point. // Original GJK calls the backup procedure at this point.
#ifdef USE_BACKUP_PROCEDURE #ifdef USE_BACKUP_PROCEDURE
backup_closest(SimdVector3& v); backup_closest(btVector3& v);
#endif #endif
return false; return false;
} }
int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const int Solid3JohnsonSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{ {
int num_verts = 0; int num_verts = 0;
int i; int i;
@@ -155,7 +155,7 @@ int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, S
return num_verts; return num_verts;
} }
bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w) bool Solid3JohnsonSimplexSolver::inSimplex(const btVector3& w)
{ {
int i; int i;
T_Bits bit; T_Bits bit;
@@ -171,18 +171,18 @@ bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w)
void Solid3JohnsonSimplexSolver::backup_closest(SimdVector3& v) void Solid3JohnsonSimplexSolver::backup_closest(btVector3& v)
{ {
SimdScalar min_dist2 = SIMD_INFINITY; btScalar min_dist2 = SIMD_INFINITY;
T_Bits s; T_Bits s;
for (s = m_all_bits; s != 0x0; --s) for (s = m_all_bits; s != 0x0; --s)
{ {
if (subseteq(s, m_all_bits) && proper(s)) if (subseteq(s, m_all_bits) && proper(s))
{ {
SimdVector3 u; btVector3 u;
compute_vector(s, u); compute_vector(s, u);
SimdScalar dist2 = u.length2(); btScalar dist2 = u.length2();
if (dist2 < min_dist2) if (dist2 < min_dist2)
{ {
min_dist2 = dist2; min_dist2 = dist2;
@@ -195,11 +195,11 @@ void Solid3JohnsonSimplexSolver::backup_closest(SimdVector3& v)
} }
void Solid3JohnsonSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2) void Solid3JohnsonSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{ {
SimdScalar sum = SimdScalar(0.0); btScalar sum = btScalar(0.0);
p1.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0)); p1.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
p2.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0)); p2.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i; int i;
T_Bits bit; T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
@@ -212,8 +212,8 @@ void Solid3JohnsonSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
} }
} }
assert(sum > SimdScalar(0.0)); assert(sum > btScalar(0.0));
SimdScalar s = SimdScalar(1.0) / sum; btScalar s = btScalar(1.0) / sum;
p1 *= s; p1 *= s;
p2 *= s; p2 *= s;
} }
@@ -272,12 +272,12 @@ bool Solid3JohnsonSimplexSolver::valid(T_Bits s)
{ {
if (contains(s, bit)) if (contains(s, bit))
{ {
if (m_det[s][i] <= SimdScalar(0.0)) if (m_det[s][i] <= btScalar(0.0))
{ {
return false; return false;
} }
} }
else if (m_det[s | bit][i] > SimdScalar(0.0)) else if (m_det[s | bit][i] > btScalar(0.0))
{ {
return false; return false;
} }
@@ -292,7 +292,7 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
T_Bits bit; T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{ {
if (contains(s, bit) && m_det[s][i] <= SimdScalar(0.0)) if (contains(s, bit) && m_det[s][i] <= btScalar(0.0))
{ {
return false; return false;
} }
@@ -300,11 +300,11 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
return true; return true;
} }
void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, SimdVector3& v) void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, btVector3& v)
{ {
m_maxlen2 = SimdScalar(0.0); m_maxlen2 = btScalar(0.0);
SimdScalar sum = SimdScalar(0.0); btScalar sum = btScalar(0.0);
v .setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0)); v .setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i; int i;
T_Bits bit; T_Bits bit;
@@ -318,7 +318,7 @@ void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, SimdVector3& v)
} }
} }
assert(sum > SimdScalar(0.0)); assert(sum > btScalar(0.0));
v /= sum; v /= sum;
} }

38
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file. * LICENSE.QPL included in the packaging of this file.
* *
* This library may be distributed and/or modified under the terms of the * This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software * GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the * Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file. * packaging of this file.
* *
@@ -30,7 +30,7 @@
/// Solid3JohnsonSimplexSolver contains Johnson subdistance algorithm from Solid 3.5 library /// Solid3JohnsonSimplexSolver contains Johnson subdistance algorithm from Solid 3.5 library
class Solid3JohnsonSimplexSolver : public SimplexSolverInterface class Solid3JohnsonSimplexSolver : public btSimplexSolverInterface
{ {
private: private:
@@ -42,22 +42,22 @@ private:
void compute_det(); void compute_det();
bool valid(T_Bits s); bool valid(T_Bits s);
bool proper(T_Bits s); bool proper(T_Bits s);
void compute_vector(T_Bits s, SimdVector3& v); void compute_vector(T_Bits s, btVector3& v);
SimdScalar m_det[16][4]; // cached sub-determinants btScalar m_det[16][4]; // cached sub-determinants
SimdVector3 m_edge[4][4]; btVector3 m_edge[4][4];
#ifdef JOHNSON_ROBUST #ifdef JOHNSON_ROBUST
SimdScalar m_norm[4][4]; btScalar m_norm[4][4];
#endif #endif
SimdPoint3 m_p[4]; // support points of object A in local coordinates btPoint3 m_p[4]; // support points of object A in local coordinates
SimdPoint3 m_q[4]; // support points of object B in local coordinates btPoint3 m_q[4]; // support points of object B in local coordinates
SimdVector3 m_y[4]; // support points of A - B in world coordinates btVector3 m_y[4]; // support points of A - B in world coordinates
SimdScalar m_ylen2[4]; // Squared lengths support points y btScalar m_ylen2[4]; // Squared lengths support points y
SimdScalar m_maxlen2; // Maximum squared length to a vertex of the current btScalar m_maxlen2; // Maximum squared length to a vertex of the current
// simplex // simplex
T_Bits m_bits1; // identifies current simplex T_Bits m_bits1; // identifies current simplex
T_Bits m_last; // identifies last found support point T_Bits m_last; // identifies last found support point
@@ -69,7 +69,7 @@ private:
private: private:
void addVertex(const SimdVector3& w); void addVertex(const btVector3& w);
public: public:
Solid3JohnsonSimplexSolver(); Solid3JohnsonSimplexSolver();
@@ -78,23 +78,23 @@ public:
virtual void reset(); virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q); virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
virtual bool closest(SimdVector3& v); virtual bool closest(btVector3& v);
virtual SimdScalar maxVertex(); virtual btScalar maxVertex();
virtual bool fullSimplex() const; virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const; virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w); virtual bool inSimplex(const btVector3& w);
virtual void backup_closest(SimdVector3& v) ; virtual void backup_closest(btVector3& v) ;
virtual bool emptySimplex() const ; virtual bool emptySimplex() const ;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) ; virtual void compute_points(btPoint3& p1, btPoint3& p2) ;
virtual int numVertices() const ; virtual int numVertices() const ;

120
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
View File

@@ -21,14 +21,14 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "CcdPhysicsEnvironment.h" #include "CcdPhysicsEnvironment.h"
#include "LinearMath/SimdTransformUtil.h" #include "LinearMath/btTransformUtil.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btConeShape.h" #include "BulletCollision/CollisionShapes/btConeShape.h"
class BP_Proxy; class BP_Proxy;
///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class ///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
//'temporarily' global variables //'temporarily' global variables
float gDeactivationTime = 2.f; float gDeactivationTime = 2.f;
@@ -39,8 +39,8 @@ float gAngularSleepingTreshold = 1.0f;
#include "BulletDynamics/Dynamics/btMassProps.h" #include "BulletDynamics/Dynamics/btMassProps.h"
SimdVector3 startVel(0,0,0);//-10000); btVector3 startVel(0,0,0);//-10000);
CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci) CcdPhysicsController::CcdPhysicsController (const btCcdConstructionInfo& ci)
:m_cci(ci) :m_cci(ci)
{ {
m_collisionDelay = 0; m_collisionDelay = 0;
@@ -61,14 +61,14 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
} }
SimdTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState) btTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
{ {
SimdTransform trans; btTransform trans;
float tmp[3]; float tmp[3];
motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]); motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2])); trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2]));
SimdQuaternion orn; btQuaternion orn;
motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]); motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
trans.setRotation(orn); trans.setRotation(orn);
return trans; return trans;
@@ -78,11 +78,11 @@ SimdTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState
void CcdPhysicsController::CreateRigidbody() void CcdPhysicsController::CreateRigidbody()
{ {
SimdTransform trans = GetTransformFromMotionState(m_cci.m_MotionState); btTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
MassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor); btMassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);
m_body = new RigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution); m_body = new btRigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
m_body->m_collisionShape = m_cci.m_collisionShape; m_body->m_collisionShape = m_cci.m_collisionShape;
@@ -120,27 +120,27 @@ bool CcdPhysicsController::SynchronizeMotionStates(float time)
if (!m_body->IsStatic()) if (!m_body->IsStatic())
{ {
const SimdVector3& worldPos = m_body->getCenterOfMassPosition(); const btVector3& worldPos = m_body->getCenterOfMassPosition();
m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]); m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
const SimdQuaternion& worldquat = m_body->getOrientation(); const btQuaternion& worldquat = m_body->getOrientation();
m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]); m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
m_MotionState->calculateWorldTransformations(); m_MotionState->calculateWorldTransformations();
float scale[3]; float scale[3];
m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]); m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
SimdVector3 scaling(scale[0],scale[1],scale[2]); btVector3 scaling(scale[0],scale[1],scale[2]);
GetCollisionShape()->setLocalScaling(scaling); GetCollisionShape()->setLocalScaling(scaling);
} else } else
{ {
SimdVector3 worldPos; btVector3 worldPos;
SimdQuaternion worldquat; btQuaternion worldquat;
m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]); m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]); m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
SimdTransform oldTrans = m_body->getCenterOfMassTransform(); btTransform oldTrans = m_body->getCenterOfMassTransform();
SimdTransform newTrans(worldquat,worldPos); btTransform newTrans(worldquat,worldPos);
m_body->setCenterOfMassTransform(newTrans); m_body->setCenterOfMassTransform(newTrans);
//need to keep track of previous position for friction effects... //need to keep track of previous position for friction effects...
@@ -149,7 +149,7 @@ bool CcdPhysicsController::SynchronizeMotionStates(float time)
float scale[3]; float scale[3];
m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]); m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
SimdVector3 scaling(scale[0],scale[1],scale[2]); btVector3 scaling(scale[0],scale[1],scale[2]);
GetCollisionShape()->setLocalScaling(scaling); GetCollisionShape()->setLocalScaling(scaling);
} }
return true; return true;
@@ -219,8 +219,8 @@ void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dloc
{ {
m_body->activate(); m_body->activate();
SimdVector3 dloc(dlocX,dlocY,dlocZ); btVector3 dloc(dlocX,dlocY,dlocZ);
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
if (local) if (local)
{ {
@@ -239,15 +239,15 @@ void CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
{ {
m_body->activate(); m_body->activate();
SimdMatrix3x3 drotmat( rotval[0],rotval[1],rotval[2], btMatrix3x3 drotmat( rotval[0],rotval[1],rotval[2],
rotval[4],rotval[5],rotval[6], rotval[4],rotval[5],rotval[6],
rotval[8],rotval[9],rotval[10]); rotval[8],rotval[9],rotval[10]);
SimdMatrix3x3 currentOrn; btMatrix3x3 currentOrn;
GetWorldOrientation(currentOrn); GetWorldOrientation(currentOrn);
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
xform.setBasis(xform.getBasis()*(local ? xform.setBasis(xform.getBasis()*(local ?
drotmat : (currentOrn.inverse() * drotmat * currentOrn))); drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
@@ -257,17 +257,17 @@ void CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
} }
void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat) void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
{ {
float orn[4]; float orn[4];
m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]); m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]); btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
mat.setRotation(quat); mat.setRotation(quat);
} }
void CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal) void CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
{ {
SimdQuaternion q = m_body->getCenterOfMassTransform().getRotation(); btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
quatImag0 = q[0]; quatImag0 = q[0];
quatImag1 = q[1]; quatImag1 = q[1];
quatImag2 = q[2]; quatImag2 = q[2];
@@ -277,8 +277,8 @@ void CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float
{ {
m_body->activate(); m_body->activate();
m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal); m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal)); xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
m_body->setCenterOfMassTransform(xform); m_body->setCenterOfMassTransform(xform);
} }
@@ -287,8 +287,8 @@ void CcdPhysicsController::setPosition(float posX,float posY,float posZ)
{ {
m_body->activate(); m_body->activate();
m_MotionState->setWorldPosition(posX,posY,posZ); m_MotionState->setWorldPosition(posX,posY,posZ);
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
xform.setOrigin(SimdVector3(posX,posY,posZ)); xform.setOrigin(btVector3(posX,posY,posZ));
m_body->setCenterOfMassTransform(xform); m_body->setCenterOfMassTransform(xform);
} }
@@ -298,7 +298,7 @@ void CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvel
void CcdPhysicsController::getPosition(PHY__Vector3& pos) const void CcdPhysicsController::getPosition(PHY__Vector3& pos) const
{ {
const SimdTransform& xform = m_body->getCenterOfMassTransform(); const btTransform& xform = m_body->getCenterOfMassTransform();
pos[0] = xform.getOrigin().x(); pos[0] = xform.getOrigin().x();
pos[1] = xform.getOrigin().y(); pos[1] = xform.getOrigin().y();
pos[2] = xform.getOrigin().z(); pos[2] = xform.getOrigin().z();
@@ -306,11 +306,11 @@ void CcdPhysicsController::getPosition(PHY__Vector3& pos) const
void CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ) void CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
{ {
if (!SimdFuzzyZero(m_cci.m_scaling.x()-scaleX) || if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
!SimdFuzzyZero(m_cci.m_scaling.y()-scaleY) || !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
!SimdFuzzyZero(m_cci.m_scaling.z()-scaleZ)) !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
{ {
m_cci.m_scaling = SimdVector3(scaleX,scaleY,scaleZ); m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
if (m_body && m_body->GetCollisionShape()) if (m_body && m_body->GetCollisionShape())
{ {
@@ -324,8 +324,8 @@ void CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
// physics methods // physics methods
void CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local) void CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
{ {
SimdVector3 torque(torqueX,torqueY,torqueZ); btVector3 torque(torqueX,torqueY,torqueZ);
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
if (local) if (local)
{ {
torque = xform.getBasis()*torque; torque = xform.getBasis()*torque;
@@ -335,8 +335,8 @@ void CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torque
void CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local) void CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
{ {
SimdVector3 force(forceX,forceY,forceZ); btVector3 force(forceX,forceY,forceZ);
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
if (local) if (local)
{ {
force = xform.getBasis()*force; force = xform.getBasis()*force;
@@ -345,14 +345,14 @@ void CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bo
} }
void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local) void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
{ {
SimdVector3 angvel(ang_velX,ang_velY,ang_velZ); btVector3 angvel(ang_velX,ang_velY,ang_velZ);
if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON)) if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{ {
m_body->activate(); m_body->activate();
} }
{ {
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
if (local) if (local)
{ {
angvel = xform.getBasis()*angvel; angvel = xform.getBasis()*angvel;
@@ -365,14 +365,14 @@ void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,flo
void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local) void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
{ {
SimdVector3 linVel(lin_velX,lin_velY,lin_velZ); btVector3 linVel(lin_velX,lin_velY,lin_velZ);
if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON)) if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{ {
m_body->activate(); m_body->activate();
} }
{ {
SimdTransform xform = m_body->getCenterOfMassTransform(); btTransform xform = m_body->getCenterOfMassTransform();
if (local) if (local)
{ {
linVel = xform.getBasis()*linVel; linVel = xform.getBasis()*linVel;
@@ -382,13 +382,13 @@ void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,floa
} }
void CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ) void CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
{ {
SimdVector3 impulse(impulseX,impulseY,impulseZ); btVector3 impulse(impulseX,impulseY,impulseZ);
if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON)) if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{ {
m_body->activate(); m_body->activate();
SimdVector3 pos(attachX,attachY,attachZ); btVector3 pos(attachX,attachY,attachZ);
m_body->activate(); m_body->activate();
@@ -402,7 +402,7 @@ void CcdPhysicsController::SetActive(bool active)
// reading out information from physics // reading out information from physics
void CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ) void CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
{ {
const SimdVector3& linvel = this->m_body->getLinearVelocity(); const btVector3& linvel = this->m_body->getLinearVelocity();
linvX = linvel.x(); linvX = linvel.x();
linvY = linvel.y(); linvY = linvel.y();
linvZ = linvel.z(); linvZ = linvel.z();
@@ -411,7 +411,7 @@ void CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& l
void CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ) void CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
{ {
const SimdVector3& angvel= m_body->getAngularVelocity(); const btVector3& angvel= m_body->getAngularVelocity();
angVelX = angvel.x(); angVelX = angvel.x();
angVelY = angvel.y(); angVelY = angvel.y();
angVelZ = angvel.z(); angVelZ = angvel.z();
@@ -419,9 +419,9 @@ void CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,flo
void CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ) void CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
{ {
SimdVector3 pos(posX,posY,posZ); btVector3 pos(posX,posY,posZ);
SimdVector3 rel_pos = pos-m_body->getCenterOfMassPosition(); btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
SimdVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos); btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
linvX = linvel.x(); linvX = linvel.x();
linvY = linvel.y(); linvY = linvel.y();
linvZ = linvel.z(); linvZ = linvel.z();
@@ -436,7 +436,7 @@ void CcdPhysicsController::setRigidBody(bool rigid)
if (!rigid) if (!rigid)
{ {
//fake it for now //fake it for now
SimdVector3 inertia = m_body->getInvInertiaDiagLocal(); btVector3 inertia = m_body->getInvInertiaDiagLocal();
inertia[1] = 0.f; inertia[1] = 0.f;
m_body->setInvInertiaDiagLocal(inertia); m_body->setInvInertiaDiagLocal(inertia);
m_body->updateInertiaTensor(); m_body->updateInertiaTensor();
@@ -494,24 +494,24 @@ bool CcdPhysicsController::wantsSleeping()
PHY_IPhysicsController* CcdPhysicsController::GetReplica() PHY_IPhysicsController* CcdPhysicsController::GetReplica()
{ {
//very experimental, shape sharing is not implemented yet. //very experimental, shape sharing is not implemented yet.
//just support SphereShape/ConeShape for now //just support btSphereShape/ConeShape for now
CcdConstructionInfo cinfo = m_cci; btCcdConstructionInfo cinfo = m_cci;
if (cinfo.m_collisionShape) if (cinfo.m_collisionShape)
{ {
switch (cinfo.m_collisionShape->GetShapeType()) switch (cinfo.m_collisionShape->GetShapeType())
{ {
case SPHERE_SHAPE_PROXYTYPE: case SPHERE_SHAPE_PROXYTYPE:
{ {
SphereShape* orgShape = (SphereShape*)cinfo.m_collisionShape; btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new SphereShape(*orgShape); cinfo.m_collisionShape = new btSphereShape(*orgShape);
break; break;
} }
case CONE_SHAPE_PROXYTYPE: case CONE_SHAPE_PROXYTYPE:
{ {
ConeShape* orgShape = (ConeShape*)cinfo.m_collisionShape; btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new ConeShape(*orgShape); cinfo.m_collisionShape = new btConeShape(*orgShape);
break; break;
} }
@@ -570,13 +570,13 @@ void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,flo
void DefaultMotionState::setWorldPosition(float posX,float posY,float posZ) void DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
{ {
SimdPoint3 pos(posX,posY,posZ); btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos ); m_worldTransform.setOrigin( pos );
} }
void DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) void DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
{ {
SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
m_worldTransform.setRotation( orn ); m_worldTransform.setRotation( orn );
} }

62
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
View File

@@ -21,16 +21,16 @@ subject to the following restrictions:
/// PHY_IPhysicsController is the abstract simplified Interface to a physical object. /// PHY_IPhysicsController is the abstract simplified Interface to a physical object.
/// It contains the IMotionState and IDeformableMesh Interfaces. /// It contains the IMotionState and IDeformableMesh Interfaces.
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/SimdScalar.h" #include "LinearMath/btScalar.h"
#include "LinearMath/SimdMatrix3x3.h" #include "LinearMath/btMatrix3x3.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "PHY_IMotionState.h" #include "PHY_IMotionState.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for CollisionShape access #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for btCollisionShape access
class CollisionShape; class btCollisionShape;
extern float gDeactivationTime; extern float gDeactivationTime;
extern float gLinearSleepingTreshold; extern float gLinearSleepingTreshold;
@@ -41,12 +41,12 @@ class CcdPhysicsEnvironment;
struct CcdConstructionInfo struct btCcdConstructionInfo
{ {
///CollisionFilterGroups provides some optional usage of basic collision filtering ///CollisionFilterGroups provides some optional usage of basic collision filtering
///this is done during broadphase, so very early in the pipeline ///this is done during broadphase, so very early in the pipeline
///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision ///more advanced collision filtering should be done in btCollisionDispatcher::NeedsCollision
enum CollisionFilterGroups enum CollisionFilterGroups
{ {
DefaultFilter = 1, DefaultFilter = 1,
@@ -57,7 +57,7 @@ struct CcdConstructionInfo
}; };
CcdConstructionInfo() btCcdConstructionInfo()
: m_gravity(0,0,0), : m_gravity(0,0,0),
m_scaling(1.f,1.f,1.f), m_scaling(1.f,1.f,1.f),
m_mass(0.f), m_mass(0.f),
@@ -74,26 +74,26 @@ struct CcdConstructionInfo
{ {
} }
SimdVector3 m_localInertiaTensor; btVector3 m_localInertiaTensor;
SimdVector3 m_gravity; btVector3 m_gravity;
SimdVector3 m_scaling; btVector3 m_scaling;
SimdScalar m_mass; btScalar m_mass;
SimdScalar m_restitution; btScalar m_restitution;
SimdScalar m_friction; btScalar m_friction;
SimdScalar m_linearDamping; btScalar m_linearDamping;
SimdScalar m_angularDamping; btScalar m_angularDamping;
int m_collisionFlags; int m_collisionFlags;
///optional use of collision group/mask: ///optional use of collision group/mask:
///only collision with object goups that match the collision mask. ///only collision with object goups that match the collision mask.
///this is very basic early out. advanced collision filtering should be ///this is very basic early out. advanced collision filtering should be
///done in the CollisionDispatcher::NeedsCollision and NeedsResponse ///done in the btCollisionDispatcher::NeedsCollision and NeedsResponse
///both values default to 1 ///both values default to 1
short int m_collisionFilterGroup; short int m_collisionFilterGroup;
short int m_collisionFilterMask; short int m_collisionFilterMask;
CollisionShape* m_collisionShape; btCollisionShape* m_collisionShape;
class PHY_IMotionState* m_MotionState; class PHY_IMotionState* m_MotionState;
CcdPhysicsEnvironment* m_physicsEnv; //needed for self-replication CcdPhysicsEnvironment* m_physicsEnv; //needed for self-replication
@@ -101,19 +101,19 @@ struct CcdConstructionInfo
}; };
class RigidBody; class btRigidBody;
///CcdPhysicsController is a physics object that supports continuous collision detection and time of impact based physics resolution. ///CcdPhysicsController is a physics object that supports continuous collision detection and time of impact based physics resolution.
class CcdPhysicsController : public PHY_IPhysicsController class CcdPhysicsController : public PHY_IPhysicsController
{ {
RigidBody* m_body; btRigidBody* m_body;
class PHY_IMotionState* m_MotionState; class PHY_IMotionState* m_MotionState;
void* m_newClientInfo; void* m_newClientInfo;
CcdConstructionInfo m_cci;//needed for replication btCcdConstructionInfo m_cci;//needed for replication
void GetWorldOrientation(SimdMatrix3x3& mat); void GetWorldOrientation(btMatrix3x3& mat);
void CreateRigidbody(); void CreateRigidbody();
@@ -122,14 +122,14 @@ class CcdPhysicsController : public PHY_IPhysicsController
int m_collisionDelay; int m_collisionDelay;
CcdPhysicsController (const CcdConstructionInfo& ci); CcdPhysicsController (const btCcdConstructionInfo& ci);
virtual ~CcdPhysicsController(); virtual ~CcdPhysicsController();
RigidBody* GetRigidBody() { return m_body;} btRigidBody* GetRigidBody() { return m_body;}
CollisionShape* GetCollisionShape() { btCollisionShape* GetCollisionShape() {
return m_body->GetCollisionShape(); return m_body->GetCollisionShape();
} }
//////////////////////////////////// ////////////////////////////////////
@@ -206,9 +206,9 @@ class CcdPhysicsController : public PHY_IPhysicsController
void UpdateDeactivation(float timeStep); void UpdateDeactivation(float timeStep);
static SimdTransform GetTransformFromMotionState(PHY_IMotionState* motionState); static btTransform GetTransformFromMotionState(PHY_IMotionState* motionState);
void SetAabb(const SimdVector3& aabbMin,const SimdVector3& aabbMax); void SetAabb(const btVector3& aabbMin,const btVector3& aabbMax);
class PHY_IMotionState* GetMotionState() class PHY_IMotionState* GetMotionState()
@@ -227,7 +227,7 @@ class CcdPhysicsController : public PHY_IPhysicsController
///DefaultMotionState implements standard motionstate, using SimdTransform ///DefaultMotionState implements standard motionstate, using btTransform
class DefaultMotionState : public PHY_IMotionState class DefaultMotionState : public PHY_IMotionState
{ {
@@ -245,8 +245,8 @@ class DefaultMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations(); virtual void calculateWorldTransformations();
SimdTransform m_worldTransform; btTransform m_worldTransform;
SimdVector3 m_localScaling; btVector3 m_localScaling;
}; };

362
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
View File

File diff suppressed because it is too large Load Diff

66
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
View File

@@ -19,16 +19,16 @@ subject to the following restrictions:
#include "PHY_IPhysicsEnvironment.h" #include "PHY_IPhysicsEnvironment.h"
#include <vector> #include <vector>
class CcdPhysicsController; class CcdPhysicsController;
#include "LinearMath/SimdVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
class TypedConstraint; class btTypedConstraint;
class SimulationIslandManager; class btSimulationIslandManager;
class CollisionDispatcher; class btCollisionDispatcher;
class Dispatcher; class btDispatcher;
//#include "btBroadphaseInterface.h" //#include "btBroadphaseInterface.h"
//switch on/off new vehicle support //switch on/off new vehicle support
@@ -37,10 +37,10 @@ class Dispatcher;
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class WrapperVehicle; class WrapperVehicle;
class PersistentManifold; class btPersistentManifold;
class BroadphaseInterface; class btBroadphaseInterface;
class OverlappingPairCache; class btOverlappingPairCache;
class IDebugDraw; class btIDebugDraw;
class PHY_IVehicle; class PHY_IVehicle;
/// CcdPhysicsEnvironment is an experimental mainloop for physics simulation using optional continuous collision detection. /// CcdPhysicsEnvironment is an experimental mainloop for physics simulation using optional continuous collision detection.
@@ -49,12 +49,12 @@ class PHY_IVehicle;
/// A derived class may be able to 'construct' entities by loading and/or converting /// A derived class may be able to 'construct' entities by loading and/or converting
class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
{ {
SimdVector3 m_gravity; btVector3 m_gravity;
protected: protected:
IDebugDraw* m_debugDrawer; btIDebugDraw* m_debugDrawer;
//solver iterations //solver iterations
int m_numIterations; int m_numIterations;
@@ -67,12 +67,12 @@ protected:
int m_profileTimings; int m_profileTimings;
bool m_enableSatCollisionDetection; bool m_enableSatCollisionDetection;
ContactSolverInfo m_solverInfo; btContactSolverInfo m_solverInfo;
SimulationIslandManager* m_islandManager; btSimulationIslandManager* m_islandManager;
public: public:
CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0); CcdPhysicsEnvironment(btDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0);
virtual ~CcdPhysicsEnvironment(); virtual ~CcdPhysicsEnvironment();
@@ -82,7 +82,7 @@ protected:
/// Perform an integration step of duration 'timeStep'. /// Perform an integration step of duration 'timeStep'.
virtual void setDebugDrawer(IDebugDraw* debugDrawer) virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
{ {
m_debugDrawer = debugDrawer; m_debugDrawer = debugDrawer;
} }
@@ -127,12 +127,12 @@ protected:
//Following the COLLADA physics specification for constraints //Following the COLLADA physics specification for constraints
virtual int createUniversalD6Constraint( virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef, btTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther, btTransform& localAttachmentOther,
const SimdVector3& linearMinLimits, const btVector3& linearMinLimits,
const SimdVector3& linearMaxLimits, const btVector3& linearMaxLimits,
const SimdVector3& angularMinLimits, const btVector3& angularMinLimits,
const SimdVector3& angularMaxLimits const btVector3& angularMaxLimits
); );
@@ -154,7 +154,7 @@ protected:
} }
#endif //NEW_BULLET_VEHICLE_SUPPORT #endif //NEW_BULLET_VEHICLE_SUPPORT
TypedConstraint* getConstraintById(int constraintId); btTypedConstraint* getConstraintById(int constraintId);
virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ,
float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ); float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ);
@@ -183,7 +183,7 @@ protected:
void removeCcdPhysicsController(CcdPhysicsController* ctrl); void removeCcdPhysicsController(CcdPhysicsController* ctrl);
BroadphaseInterface* GetBroadphase(); btBroadphaseInterface* GetBroadphase();
@@ -207,34 +207,34 @@ protected:
const PersistentManifold* GetManifold(int index) const; const btPersistentManifold* GetManifold(int index) const;
std::vector<TypedConstraint*> m_constraints; std::vector<btTypedConstraint*> m_constraints;
void SyncMotionStates(float timeStep); void SyncMotionStates(float timeStep);
class CollisionWorld* GetCollisionWorld() class btCollisionWorld* GetCollisionWorld()
{ {
return m_collisionWorld; return m_collisionWorld;
} }
const class CollisionWorld* GetCollisionWorld() const const class btCollisionWorld* GetCollisionWorld() const
{ {
return m_collisionWorld; return m_collisionWorld;
} }
SimulationIslandManager* GetSimulationIslandManager() btSimulationIslandManager* GetSimulationIslandManager()
{ {
return m_islandManager; return m_islandManager;
} }
const SimulationIslandManager* GetSimulationIslandManager() const const btSimulationIslandManager* GetSimulationIslandManager() const
{ {
return m_islandManager; return m_islandManager;
} }
class ConstraintSolver* GetConstraintSolver() class btConstraintSolver* GetConstraintSolver()
{ {
return m_solver; return m_solver;
} }
@@ -253,9 +253,9 @@ protected:
std::vector<WrapperVehicle*> m_wrapperVehicles; std::vector<WrapperVehicle*> m_wrapperVehicles;
class CollisionWorld* m_collisionWorld; class btCollisionWorld* m_collisionWorld;
class ConstraintSolver* m_solver; class btConstraintSolver* m_solver;
bool m_scalingPropagated; bool m_scalingPropagated;

82
Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp
View File

@@ -51,29 +51,29 @@ ParallelIslandDispatcher::ParallelIslandDispatcher ():
}; };
PersistentManifold* ParallelIslandDispatcher::GetNewManifold(void* b0,void* b1) btPersistentManifold* ParallelIslandDispatcher::GetNewManifold(void* b0,void* b1)
{ {
gNumManifold2++; gNumManifold2++;
//ASSERT(gNumManifold < 65535); //ASSERT(gNumManifold < 65535);
CollisionObject* body0 = (CollisionObject*)b0; btCollisionObject* body0 = (btCollisionObject*)b0;
CollisionObject* body1 = (CollisionObject*)b1; btCollisionObject* body1 = (btCollisionObject*)b1;
PersistentManifold* manifold = new PersistentManifold (body0,body1); btPersistentManifold* manifold = new btPersistentManifold (body0,body1);
m_manifoldsPtr.push_back(manifold); m_manifoldsPtr.push_back(manifold);
return manifold; return manifold;
} }
void ParallelIslandDispatcher::ClearManifold(PersistentManifold* manifold) void ParallelIslandDispatcher::ClearManifold(btPersistentManifold* manifold)
{ {
manifold->ClearManifold(); manifold->ClearManifold();
} }
void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold) void ParallelIslandDispatcher::ReleaseManifold(btPersistentManifold* manifold)
{ {
gNumManifold2--; gNumManifold2--;
@@ -82,7 +82,7 @@ void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
ClearManifold(manifold); ClearManifold(manifold);
std::vector<PersistentManifold*>::iterator i = std::vector<btPersistentManifold*>::iterator i =
std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold); std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
if (!(i == m_manifoldsPtr.end())) if (!(i == m_manifoldsPtr.end()))
{ {
@@ -99,14 +99,14 @@ void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
// //
// todo: this is random access, it can be walked 'cache friendly'! // todo: this is random access, it can be walked 'cache friendly'!
// //
void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback) void ParallelIslandDispatcher::BuildAndProcessIslands(btCollisionObjectArray& collisionObjects, IslandCallback* callback)
{ {
int numBodies = collisionObjects.size(); int numBodies = collisionObjects.size();
for (int islandId=0;islandId<numBodies;islandId++) for (int islandId=0;islandId<numBodies;islandId++)
{ {
std::vector<PersistentManifold*> islandmanifold; std::vector<btPersistentManifold*> islandmanifold;
//int numSleeping = 0; //int numSleeping = 0;
@@ -115,7 +115,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i; int i;
for (i=0;i<numBodies;i++) for (i=0;i<numBodies;i++)
{ {
CollisionObject* colObj0 = collisionObjects[i]; btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId) if (colObj0->m_islandTag1 == islandId)
{ {
if (colObj0->GetActivationState()== ACTIVE_TAG) if (colObj0->GetActivationState()== ACTIVE_TAG)
@@ -132,12 +132,12 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
for (i=0;i<GetNumManifolds();i++) for (i=0;i<GetNumManifolds();i++)
{ {
PersistentManifold* manifold = this->GetManifoldByIndexInternal(i); btPersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
//filtering for response //filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0()); btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1()); btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->GetBody1());
{ {
if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) || if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
((colObj1) && (colObj1)->m_islandTag1 == (islandId))) ((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
@@ -153,7 +153,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i; int i;
for (i=0;i<numBodies;i++) for (i=0;i<numBodies;i++)
{ {
CollisionObject* colObj0 = collisionObjects[i]; btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId) if (colObj0->m_islandTag1 == islandId)
{ {
colObj0->SetActivationState( ISLAND_SLEEPING ); colObj0->SetActivationState( ISLAND_SLEEPING );
@@ -167,7 +167,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i; int i;
for (i=0;i<numBodies;i++) for (i=0;i<numBodies;i++)
{ {
CollisionObject* colObj0 = collisionObjects[i]; btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId) if (colObj0->m_islandTag1 == islandId)
{ {
if ( colObj0->GetActivationState() == ISLAND_SLEEPING) if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
@@ -189,73 +189,73 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
CollisionAlgorithm* ParallelIslandDispatcher::InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) btCollisionAlgorithm* ParallelIslandDispatcher::InternalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
{ {
m_count++; m_count++;
CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject; btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject; btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
CollisionAlgorithmConstructionInfo ci; btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher = this; ci.m_dispatcher = this;
if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConvex() ) if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConvex() )
{ {
return new ConvexConvexAlgorithm(0,ci,&proxy0,&proxy1); return new btConvexConvexAlgorithm(0,ci,&proxy0,&proxy1);
} }
if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConcave()) if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConcave())
{ {
return new ConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1); return new btConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
} }
if (body1->m_collisionShape->IsConvex() && body0->m_collisionShape->IsConcave()) if (body1->m_collisionShape->IsConvex() && body0->m_collisionShape->IsConcave())
{ {
return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0); return new btConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
} }
if (body0->m_collisionShape->IsCompound()) if (body0->m_collisionShape->IsCompound())
{ {
return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1); return new btCompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
} else } else
{ {
if (body1->m_collisionShape->IsCompound()) if (body1->m_collisionShape->IsCompound())
{ {
return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0); return new btCompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
} }
} }
//failed to find an algorithm //failed to find an algorithm
return new EmptyAlgorithm(ci); return new btEmptyAlgorithm(ci);
} }
bool ParallelIslandDispatcher::NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1) bool ParallelIslandDispatcher::NeedsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1)
{ {
//here you can do filtering //here you can do filtering
bool hasResponse = bool hasResponse =
(!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) && (!(colObj0.m_collisionFlags & btCollisionObject::noContactResponse)) &&
(!(colObj1.m_collisionFlags & CollisionObject::noContactResponse)); (!(colObj1.m_collisionFlags & btCollisionObject::noContactResponse));
hasResponse = hasResponse && hasResponse = hasResponse &&
(colObj0.IsActive() || colObj1.IsActive()); (colObj0.IsActive() || colObj1.IsActive());
return hasResponse; return hasResponse;
} }
bool ParallelIslandDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) bool ParallelIslandDispatcher::NeedsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
{ {
CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject; btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject; btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
assert(body0); assert(body0);
assert(body1); assert(body1);
bool needsCollision = true; bool needsCollision = true;
if ((body0->m_collisionFlags & CollisionObject::isStatic) && if ((body0->m_collisionFlags & btCollisionObject::isStatic) &&
(body1->m_collisionFlags & CollisionObject::isStatic)) (body1->m_collisionFlags & btCollisionObject::isStatic))
needsCollision = false; needsCollision = false;
if ((!body0->IsActive()) && (!body1->IsActive())) if ((!body0->IsActive()) && (!body1->IsActive()))
@@ -266,23 +266,23 @@ bool ParallelIslandDispatcher::NeedsCollision(BroadphaseProxy& proxy0,Broadphase
} }
///allows the user to get contact point callbacks ///allows the user to get contact point callbacks
ManifoldResult* ParallelIslandDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) btManifoldResult* ParallelIslandDispatcher::GetNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold)
{ {
//in-place, this prevents parallel dispatching, but just adding a list would fix that. //in-place, this prevents parallel dispatching, but just adding a list would fix that.
ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold); btManifoldResult* manifoldResult = new (&m_defaultManifoldResult) btManifoldResult(obj0,obj1,manifold);
return manifoldResult; return manifoldResult;
} }
///allows the user to get contact point callbacks ///allows the user to get contact point callbacks
void ParallelIslandDispatcher::ReleaseManifoldResult(ManifoldResult*) void ParallelIslandDispatcher::ReleaseManifoldResult(btManifoldResult*)
{ {
} }
void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo) void ParallelIslandDispatcher::DispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)
{ {
//m_blockedForChanges = true; //m_blockedForChanges = true;
@@ -296,7 +296,7 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
for (i=0;i<numPairs;i++) for (i=0;i<numPairs;i++)
{ {
BroadphasePair& pair = pairs[i]; btBroadphasePair& pair = pairs[i];
if (dispatcherId>= 0) if (dispatcherId>= 0)
{ {
@@ -310,7 +310,7 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
if (pair.m_algorithms[dispatcherId]) if (pair.m_algorithms[dispatcherId])
{ {
if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
{ {
pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
} else } else
@@ -324,13 +324,13 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
} else } else
{ {
//non-persistent algorithm dispatcher //non-persistent algorithm dispatcher
CollisionAlgorithm* algo = FindAlgorithm( btCollisionAlgorithm* algo = FindAlgorithm(
*pair.m_pProxy0, *pair.m_pProxy0,
*pair.m_pProxy1); *pair.m_pProxy1);
if (algo) if (algo)
{ {
if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
{ {
algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
} else } else

44
Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h
View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include <vector> #include <vector>
class IDebugDraw; class btIDebugDraw;
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
@@ -35,28 +35,28 @@ class IDebugDraw;
///ParallelIslandDispatcher dispatches entire simulation islands in parallel. ///ParallelIslandDispatcher dispatches entire simulation islands in parallel.
///For both collision detection and constraint solving. ///For both collision detection and constraint solving.
///This development heads toward multi-core, CELL SPU and GPU approach ///This development heads toward multi-core, CELL SPU and GPU approach
class ParallelIslandDispatcher : public Dispatcher class ParallelIslandDispatcher : public btDispatcher
{ {
std::vector<PersistentManifold*> m_manifoldsPtr; std::vector<btPersistentManifold*> m_manifoldsPtr;
UnionFind m_unionFind; btUnionFind m_unionFind;
bool m_useIslands; bool m_useIslands;
ManifoldResult m_defaultManifoldResult; btManifoldResult m_defaultManifoldResult;
CollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
public: public:
UnionFind& GetUnionFind() { return m_unionFind;} btUnionFind& GetUnionFind() { return m_unionFind;}
struct IslandCallback struct IslandCallback
{ {
virtual ~IslandCallback() {}; virtual ~IslandCallback() {};
virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) = 0; virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds) = 0;
}; };
@@ -65,12 +65,12 @@ public:
return m_manifoldsPtr.size(); return m_manifoldsPtr.size();
} }
PersistentManifold* GetManifoldByIndexInternal(int index) btPersistentManifold* GetManifoldByIndexInternal(int index)
{ {
return m_manifoldsPtr[index]; return m_manifoldsPtr[index];
} }
const PersistentManifold* GetManifoldByIndexInternal(int index) const const btPersistentManifold* GetManifoldByIndexInternal(int index) const
{ {
return m_manifoldsPtr[index]; return m_manifoldsPtr[index];
} }
@@ -88,37 +88,37 @@ public:
ParallelIslandDispatcher (); ParallelIslandDispatcher ();
virtual ~ParallelIslandDispatcher() {}; virtual ~ParallelIslandDispatcher() {};
virtual PersistentManifold* GetNewManifold(void* b0,void* b1); virtual btPersistentManifold* GetNewManifold(void* b0,void* b1);
virtual void ReleaseManifold(PersistentManifold* manifold); virtual void ReleaseManifold(btPersistentManifold* manifold);
virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback); virtual void BuildAndProcessIslands(btCollisionObjectArray& collisionObjects, IslandCallback* callback);
///allows the user to get contact point callbacks ///allows the user to get contact point callbacks
virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold); virtual btManifoldResult* GetNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold);
///allows the user to get contact point callbacks ///allows the user to get contact point callbacks
virtual void ReleaseManifoldResult(ManifoldResult*); virtual void ReleaseManifoldResult(btManifoldResult*);
virtual void ClearManifold(PersistentManifold* manifold); virtual void ClearManifold(btPersistentManifold* manifold);
CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) btCollisionAlgorithm* FindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
{ {
CollisionAlgorithm* algo = InternalFindAlgorithm(proxy0,proxy1); btCollisionAlgorithm* algo = InternalFindAlgorithm(proxy0,proxy1);
return algo; return algo;
} }
CollisionAlgorithm* InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1); btCollisionAlgorithm* InternalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1);
virtual bool NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1); virtual bool NeedsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1);
virtual bool NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1); virtual bool NeedsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1);
virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;} virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
virtual void DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo); virtual void DispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo);

40
Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp
View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
#include "SimulationIsland.h" #include "SimulationIsland.h"
ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, OverlappingPairCache* pairCache): ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, btOverlappingPairCache* pairCache):
CcdPhysicsEnvironment(dispatcher,pairCache) CcdPhysicsEnvironment(dispatcher,pairCache)
{ {
@@ -41,14 +41,14 @@ ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment()
bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{ {
// Make sure the broadphase / overlapping AABB paircache is up-to-date // Make sure the broadphase / overlapping AABB paircache is up-to-date
OverlappingPairCache* scene = m_collisionWorld->GetPairCache(); btOverlappingPairCache* scene = m_collisionWorld->GetPairCache();
scene->RefreshOverlappingPairs(); scene->RefreshOverlappingPairs();
// Find the connected sets that can be simulated in parallel // Find the connected sets that can be simulated in parallel
// Using union find // Using union find
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("IslandUnionFind"); btProfiler::beginBlock("IslandUnionFind");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher()); GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
@@ -58,10 +58,10 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
int numConstraints = m_constraints.size(); int numConstraints = m_constraints.size();
for (i=0;i< numConstraints ; i++ ) for (i=0;i< numConstraints ; i++ )
{ {
TypedConstraint* constraint = m_constraints[i]; btTypedConstraint* constraint = m_constraints[i];
const RigidBody* colObj0 = &constraint->GetRigidBodyA(); const btRigidBody* colObj0 = &constraint->GetRigidBodyA();
const RigidBody* colObj1 = &constraint->GetRigidBodyB(); const btRigidBody* colObj1 = &constraint->GetRigidBodyB();
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands()))) ((colObj1) && ((colObj1)->mergesSimulationIslands())))
@@ -80,7 +80,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld()); GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld());
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("IslandUnionFind"); btProfiler::endBlock("IslandUnionFind");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
@@ -88,7 +88,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
///build simulation islands ///build simulation islands
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("BuildIslands"); btProfiler::beginBlock("BuildIslands");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
std::vector<SimulationIsland> simulationIslands; std::vector<SimulationIsland> simulationIslands;
@@ -105,7 +105,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
} }
} }
Dispatcher* dispatcher = GetCollisionWorld()->GetDispatcher(); btDispatcher* dispatcher = GetCollisionWorld()->GetDispatcher();
//this is a brute force approach, will rethink later about more subtle ways //this is a brute force approach, will rethink later about more subtle ways
@@ -117,10 +117,10 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{ {
BroadphasePair* pair = &scene->GetOverlappingPair(i); btBroadphasePair* pair = &scene->GetOverlappingPair(i);
CollisionObject* col0 = static_cast<CollisionObject*>(pair->m_pProxy0->m_clientObject); btCollisionObject* col0 = static_cast<btCollisionObject*>(pair->m_pProxy0->m_clientObject);
CollisionObject* col1 = static_cast<CollisionObject*>(pair->m_pProxy1->m_clientObject); btCollisionObject* col1 = static_cast<btCollisionObject*>(pair->m_pProxy1->m_clientObject);
if (col0->m_islandTag1 > col1->m_islandTag1) if (col0->m_islandTag1 > col1->m_islandTag1)
{ {
@@ -136,7 +136,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
//store constraint indices for each island //store constraint indices for each island
for (unsigned int ui=0;ui<m_constraints.size();ui++) for (unsigned int ui=0;ui<m_constraints.size();ui++)
{ {
TypedConstraint& constraint = *m_constraints[ui]; btTypedConstraint& constraint = *m_constraints[ui];
if (constraint.GetRigidBodyA().m_islandTag1 > constraint.GetRigidBodyB().m_islandTag1) if (constraint.GetRigidBodyA().m_islandTag1 > constraint.GetRigidBodyB().m_islandTag1)
{ {
simulationIslands[constraint.GetRigidBodyA().m_islandTag1].m_constraintIndices.push_back(ui); simulationIslands[constraint.GetRigidBodyA().m_islandTag1].m_constraintIndices.push_back(ui);
@@ -151,12 +151,12 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
for (i=0;i<dispatcher->GetNumManifolds();i++) for (i=0;i<dispatcher->GetNumManifolds();i++)
{ {
PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i); btPersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
//filtering for response //filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0()); btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1()); btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->GetBody1());
{ {
int islandTag = colObj0->m_islandTag1; int islandTag = colObj0->m_islandTag1;
if (colObj1->m_islandTag1 > islandTag) if (colObj1->m_islandTag1 > islandTag)
@@ -169,15 +169,15 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
} }
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("BuildIslands"); btProfiler::endBlock("BuildIslands");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("SimulateIsland"); btProfiler::beginBlock("SimulateIsland");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
TypedConstraint** constraintBase = 0; btTypedConstraint** constraintBase = 0;
if (m_constraints.size()) if (m_constraints.size())
constraintBase = &m_constraints[0]; constraintBase = &m_constraints[0];
@@ -198,7 +198,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("SimulateIsland"); btProfiler::endBlock("SimulateIsland");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
return true; return true;

2
Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h
View File

@@ -29,7 +29,7 @@ class ParallelPhysicsEnvironment : public CcdPhysicsEnvironment
public: public:
ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher=0, OverlappingPairCache* pairCache=0); ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0);
virtual ~ParallelPhysicsEnvironment(); virtual ~ParallelPhysicsEnvironment();

74
Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/ */
#include "SimulationIsland.h" #include "SimulationIsland.h"
#include "LinearMath/SimdTransform.h" #include "LinearMath/btTransform.h"
#include "CcdPhysicsController.h" #include "CcdPhysicsController.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h"
@@ -22,17 +22,17 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
extern float gContactBreakingTreshold; extern float gContactBreakingTreshold;
bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,TypedConstraint** constraintsBaseAddress,BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase,class ConstraintSolver* solver,float timeStep) bool SimulationIsland::Simulate(btIDebugDraw* debugDrawer,int numSolverIterations,btTypedConstraint** constraintsBaseAddress,btBroadphasePair* overlappingPairBaseAddress, btDispatcher* dispatcher,btBroadphaseInterface* broadphase,class btConstraintSolver* solver,float timeStep)
{ {
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("predictIntegratedTransform"); btProfiler::beginBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
{ {
@@ -44,8 +44,8 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
for (k=0;k<GetNumControllers();k++) for (k=0;k<GetNumControllers();k++)
{ {
CcdPhysicsController* ctrl = m_controllers[k]; CcdPhysicsController* ctrl = m_controllers[k];
// SimdTransform predictedTrans; // btTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody(); btRigidBody* body = ctrl->GetRigidBody();
//todo: only do this when necessary, it's used for contact points //todo: only do this when necessary, it's used for contact points
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
@@ -63,7 +63,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
} }
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("predictIntegratedTransform"); btProfiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
//BroadphaseInterface* scene = GetBroadphase(); //BroadphaseInterface* scene = GetBroadphase();
@@ -75,20 +75,20 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("DispatchAllCollisionPairs"); btProfiler::beginBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
// int numsubstep = m_numIterations; // int numsubstep = m_numIterations;
DispatcherInfo dispatchInfo; btDispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0; dispatchInfo.m_stepCount = 0;
dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection; dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection;
dispatchInfo.m_debugDraw = debugDrawer; dispatchInfo.m_debugDraw = debugDrawer;
std::vector<BroadphasePair> overlappingPairs; std::vector<btBroadphasePair> overlappingPairs;
overlappingPairs.resize(this->m_overlappingPairIndices.size()); overlappingPairs.resize(this->m_overlappingPairIndices.size());
//gather overlapping pair info //gather overlapping pair info
@@ -115,13 +115,13 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("DispatchAllCollisionPairs"); btProfiler::endBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
//contacts //contacts
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("SolveConstraint"); btProfiler::beginBlock("SolveConstraint");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
@@ -139,7 +139,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
//point to point constraints //point to point constraints
for (i=0;i< numConstraints ; i++ ) for (i=0;i< numConstraints ; i++ )
{ {
TypedConstraint* constraint = constraintsBaseAddress[m_constraintIndices[i]]; btTypedConstraint* constraint = constraintsBaseAddress[m_constraintIndices[i]];
constraint->BuildJacobian(); constraint->BuildJacobian();
constraint->SolveConstraint( timeStep ); constraint->SolveConstraint( timeStep );
@@ -149,7 +149,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
} }
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("SolveConstraint"); btProfiler::endBlock("SolveConstraint");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
/* /*
@@ -162,7 +162,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
for (int i=0;i<numVehicles;i++) for (int i=0;i<numVehicles;i++)
{ {
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
RaycastVehicle* vehicle = wrapperVehicle->GetVehicle(); btRaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
vehicle->UpdateVehicle( timeStep); vehicle->UpdateVehicle( timeStep);
} }
#endif //NEW_BULLET_VEHICLE_SUPPORT #endif //NEW_BULLET_VEHICLE_SUPPORT
@@ -170,20 +170,20 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
/* /*
Profiler::beginBlock("CallbackTriggers"); btProfiler::beginBlock("CallbackTriggers");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
CallbackTriggers(); CallbackTriggers();
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("CallbackTriggers"); btProfiler::endBlock("CallbackTriggers");
} }
*/ */
//OverlappingPairCache* scene = GetCollisionWorld()->GetPairCache(); //OverlappingPairCache* scene = GetCollisionWorld()->GetPairCache();
ContactSolverInfo solverInfo; btContactSolverInfo solverInfo;
solverInfo.m_friction = 0.9f; solverInfo.m_friction = 0.9f;
solverInfo.m_numIterations = numSolverIterations; solverInfo.m_numIterations = numSolverIterations;
@@ -198,7 +198,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::beginBlock("proceedToTransform"); btProfiler::beginBlock("proceedToTransform");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
{ {
@@ -216,10 +216,10 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
/* if (m_ccdMode == 3) /* if (m_ccdMode == 3)
{ {
DispatcherInfo dispatchInfo; btDispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0; dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
// GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo); // GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo);
toi = dispatchInfo.m_timeOfImpact; toi = dispatchInfo.m_timeOfImpact;
@@ -243,8 +243,8 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
CcdPhysicsController* ctrl = *i; CcdPhysicsController* ctrl = *i;
SimdTransform predictedTrans; btTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody(); btRigidBody* body = ctrl->GetRigidBody();
if (body->IsActive()) if (body->IsActive())
{ {
@@ -276,7 +276,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
!(i==m_controllers.end()); i++) !(i==m_controllers.end()); i++)
{ {
CcdPhysicsController* ctrl = (*i); CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody(); btRigidBody* body = ctrl->GetRigidBody();
ctrl->UpdateDeactivation(timeStep); ctrl->UpdateDeactivation(timeStep);
@@ -313,15 +313,15 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("proceedToTransform"); btProfiler::endBlock("proceedToTransform");
Profiler::beginBlock("SyncMotionStates"); btProfiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
SyncMotionStates(timeStep); SyncMotionStates(timeStep);
#ifdef USE_QUICKPROF #ifdef USE_QUICKPROF
Profiler::endBlock("SyncMotionStates"); btProfiler::endBlock("SyncMotionStates");
#endif //USE_QUICKPROF #endif //USE_QUICKPROF
@@ -363,7 +363,7 @@ void SimulationIsland::SyncMotionStates(float timeStep)
void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* scene,float timeStep) void SimulationIsland::UpdateAabbs(btIDebugDraw* debugDrawer,btBroadphaseInterface* scene,float timeStep)
{ {
std::vector<CcdPhysicsController*>::iterator i; std::vector<CcdPhysicsController*>::iterator i;
@@ -375,33 +375,33 @@ void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface*
!(i==m_controllers.end()); i++) !(i==m_controllers.end()); i++)
{ {
CcdPhysicsController* ctrl = (*i); CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody(); btRigidBody* body = ctrl->GetRigidBody();
SimdPoint3 minAabb,maxAabb; btPoint3 minAabb,maxAabb;
CollisionShape* shapeinterface = ctrl->GetCollisionShape(); btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(), shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
body->getLinearVelocity(), body->getLinearVelocity(),
//body->getAngularVelocity(), //body->getAngularVelocity(),
SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(), btVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
timeStep,minAabb,maxAabb); timeStep,minAabb,maxAabb);
SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold); btVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
minAabb -= manifoldExtraExtents; minAabb -= manifoldExtraExtents;
maxAabb += manifoldExtraExtents; maxAabb += manifoldExtraExtents;
BroadphaseProxy* bp = body->m_broadphaseHandle; btBroadphaseProxy* bp = body->m_broadphaseHandle;
if (bp) if (bp)
{ {
SimdVector3 color (1,1,0); btVector3 color (1,1,0);
/* /*
class IDebugDraw* m_debugDrawer = 0; class btIDebugDraw* m_debugDrawer = 0;
if (m_debugDrawer) if (m_debugDrawer)
{ {
//draw aabb //draw aabb
@@ -428,7 +428,7 @@ void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface*
}; };
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) if (m_debugDrawer->GetDebugMode() & btIDebugDraw::DBG_DrawAabb)
{ {
DrawAabb(m_debugDrawer,minAabb,maxAabb,color); DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
} }

12
Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h
View File

@@ -17,9 +17,9 @@ subject to the following restrictions:
#define SIMULATION_ISLAND_H #define SIMULATION_ISLAND_H
#include <vector> #include <vector>
class BroadphaseInterface; class btBroadphaseInterface;
class Dispatcher; class btDispatcher;
class IDebugDraw; class btIDebugDraw;
///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's ///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's
///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's ///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's
@@ -30,12 +30,12 @@ class SimulationIsland
public: public:
std::vector<class CcdPhysicsController*> m_controllers; std::vector<class CcdPhysicsController*> m_controllers;
std::vector<class PersistentManifold*> m_manifolds; std::vector<class btPersistentManifold*> m_manifolds;
std::vector<int> m_overlappingPairIndices; std::vector<int> m_overlappingPairIndices;
std::vector<int> m_constraintIndices; std::vector<int> m_constraintIndices;
bool Simulate(IDebugDraw* debugDrawer,int numSolverIterations,class TypedConstraint** constraintsBaseAddress,struct BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase, class ConstraintSolver* solver, float timeStep); bool Simulate(btIDebugDraw* debugDrawer,int numSolverIterations,class btTypedConstraint** constraintsBaseAddress,struct btBroadphasePair* overlappingPairBaseAddress, btDispatcher* dispatcher,btBroadphaseInterface* broadphase, class btConstraintSolver* solver, float timeStep);
int GetNumControllers() int GetNumControllers()
@@ -47,7 +47,7 @@ class SimulationIsland
void SyncMotionStates(float timeStep); void SyncMotionStates(float timeStep);
void UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* broadphase,float timeStep); void UpdateAabbs(btIDebugDraw* debugDrawer,btBroadphaseInterface* broadphase,float timeStep);
}; };
#endif //SIMULATION_ISLAND_H #endif //SIMULATION_ISLAND_H

4
Extras/PhysicsInterface/Common/PHY_Pro.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#define PHY_PROPSH #define PHY_PROPSH
class CollisionShape; class btCollisionShape;
// Properties of dynamic objects // Properties of dynamic objects
struct PHY_ShapeProps { struct PHY_ShapeProps {
@@ -29,7 +29,7 @@ struct PHY_ShapeProps {
bool m_do_anisotropic; // Should I do anisotropic friction? bool m_do_anisotropic; // Should I do anisotropic friction?
bool m_do_fh; // Should the object have a linear Fh spring? bool m_do_fh; // Should the object have a linear Fh spring?
bool m_do_rot_fh; // Should the object have an angular Fh spring? bool m_do_rot_fh; // Should the object have an angular Fh spring?
CollisionShape* m_shape; btCollisionShape* m_shape;
}; };

22
Extras/SATConvexCollision/Hull.cpp
View File

@@ -540,7 +540,7 @@ Hull* Hull::MakeHullFromTemp()
void Hull::ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB,HullContactCollector* collector) void Hull::ProcessHullHull(btSeparation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB,HullContactCollector* collector)
{ {
Point3 vertsA[Hull::kMaxVerts]; Point3 vertsA[Hull::kMaxVerts];
Point3 vertsB[Hull::kMaxVerts]; Point3 vertsB[Hull::kMaxVerts];
@@ -563,7 +563,7 @@ void Hull::ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB
#ifdef SHAPE_COLLIDER_USE_CACHING #ifdef SHAPE_COLLIDER_USE_CACHING
// update cached pair // update cached pair
if (sep.m_separator != Separation::kFeatureNone) if (sep.m_separator != btSeparation::kFeatureNone)
{ {
// re-use the separation // re-use the separation
if (UpdateSeparationHullHull(sep, pVertsA, pVertsB, trA, trB) == true) if (UpdateSeparationHullHull(sep, pVertsA, pVertsB, trA, trB) == true)
@@ -756,7 +756,7 @@ int Hull::ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plan
int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, int Hull::AddContactsHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB, const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB,
HullContactCollector* hullContactCollector) HullContactCollector* hullContactCollector)
{ {
@@ -765,7 +765,7 @@ int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Poin
Vector3 normalWorld = sep.m_axis; Vector3 normalWorld = sep.m_axis;
// edge->edge contact is always a single point // edge->edge contact is always a single point
if (sep.m_separator == Separation::kFeatureBoth) if (sep.m_separator == btSeparation::kFeatureBoth)
{ {
const Hull::Edge& edgeA = hullA.GetEdge(sep.m_featureA); const Hull::Edge& edgeA = hullA.GetEdge(sep.m_featureA);
const Hull::Edge& edgeB = hullB.GetEdge(sep.m_featureB); const Hull::Edge& edgeB = hullB.GetEdge(sep.m_featureB);
@@ -799,7 +799,7 @@ int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Poin
// find face of hull A that is most opposite contact axis // find face of hull A that is most opposite contact axis
// TODO: avoid having to transform planes here // TODO: avoid having to transform planes here
if (sep.m_separator == Separation::kFeatureB) if (sep.m_separator == btSeparation::kFeatureB)
{ {
const Hull::Edge& edgeB = hullB.GetEdge(hullB.GetFaceFirstEdge(faceB)); const Hull::Edge& edgeB = hullB.GetEdge(hullB.GetFaceFirstEdge(faceB));
tangent = Normalize(pVertsB[edgeB.m_verts[1]] - pVertsB[edgeB.m_verts[0]]); tangent = Normalize(pVertsB[edgeB.m_verts[1]] - pVertsB[edgeB.m_verts[0]]);
@@ -921,7 +921,7 @@ int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Poin
// if no separating axis was found then details of least penetrating axis are returned // if no separating axis was found then details of least penetrating axis are returned
// resulting axis always points away from hullB // resulting axis always points away from hullB
// either transform can be null in which case it is treated as identity (this avoids a bunch of work) // either transform can be null in which case it is treated as identity (this avoids a bunch of work)
bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, bool Hull::GetSeparationHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB, const Transform& trA, const Transform& trB,
const Hull& hullA, const Hull& hullA,
const Hull& hullB const Hull& hullB
@@ -930,7 +930,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
//const Hull& hullA((const Hull&)*sep.m_pShapeA->GetShape()); //const Hull& hullA((const Hull&)*sep.m_pShapeA->GetShape());
//const Hull& hullB((const Hull&)*sep.m_pShapeB->GetShape()); //const Hull& hullB((const Hull&)*sep.m_pShapeB->GetShape());
sep.m_separator = Separation::kFeatureNone; sep.m_separator = btSeparation::kFeatureNone;
sep.m_dist = MinValueF; sep.m_dist = MinValueF;
sep.m_featureA = sep.m_featureB = -1; sep.m_featureA = sep.m_featureB = -1;
sep.m_contact = -1; sep.m_contact = -1;
@@ -952,7 +952,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
sep.m_featureB = p; sep.m_featureB = p;
sep.m_dist = minDist; sep.m_dist = minDist;
sep.m_axis = planeWorld.GetNormal(); sep.m_axis = planeWorld.GetNormal();
sep.m_separator = Separation::kFeatureB; sep.m_separator = btSeparation::kFeatureB;
} }
// got a separating plane? // got a separating plane?
@@ -982,7 +982,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
{ {
sep.m_dist = (float)minDist; sep.m_dist = (float)minDist;
sep.m_axis = -planeWorld.GetNormal(); sep.m_axis = -planeWorld.GetNormal();
sep.m_separator = Separation::kFeatureA; sep.m_separator = btSeparation::kFeatureA;
} }
} }
@@ -1048,7 +1048,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
sep.m_featureB = eb; sep.m_featureB = eb;
sep.m_dist = dminA; sep.m_dist = dminA;
sep.m_axis = axis; sep.m_axis = axis;
sep.m_separator = Separation::kFeatureBoth; sep.m_separator = btSeparation::kFeatureBoth;
return true; return true;
} }
@@ -1065,7 +1065,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
// sep.m_featureB = eb; // sep.m_featureB = eb;
sep.m_dist = dminA; sep.m_dist = dminA;
sep.m_axis = axis; sep.m_axis = axis;
// sep.m_separator = Separation::kFeatureBoth; // sep.m_separator = btSeparation::kFeatureBoth;
} }
} }

8
Extras/SATConvexCollision/Hull.h
View File

@@ -138,8 +138,8 @@ public:
Point3 GetFaceCentroid(short face) const; Point3 GetFaceCentroid(short face) const;
//static void ProcessHullHull(Separation& sep); //static void ProcessHullHull(btSeparation& sep);
static void ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB, HullContactCollector* collector); static void ProcessHullHull(btSeparation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB, HullContactCollector* collector);
virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const; virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const;
virtual Bounds3 ComputeBounds(const Transform& transform) const; virtual Bounds3 ComputeBounds(const Transform& transform) const;
@@ -153,13 +153,13 @@ public:
/// Clips a face to the back of a plane /// Clips a face to the back of a plane
static int ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plane& plane); static int ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plane& plane);
static bool GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, static bool GetSeparationHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB, const Transform& trA, const Transform& trB,
const Hull& hullA, const Hull& hullA,
const Hull& hullB const Hull& hullB
); );
static int AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, static int AddContactsHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB, const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB,
HullContactCollector* hullContactCollector); HullContactCollector* hullContactCollector);

4
Extras/SATConvexCollision/HullContactCollector.h
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
class Vector3; class Vector3;
class Point3; class Point3;
class Scalar; class Scalar;
struct Separation; struct btSeparation;
///HullContactCollector collects the Hull computation to the contact point results ///HullContactCollector collects the Hull computation to the contact point results
class HullContactCollector class HullContactCollector
@@ -29,7 +29,7 @@ public:
virtual ~HullContactCollector() {}; virtual ~HullContactCollector() {};
virtual int BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)=0; virtual int BatchAddContactGroup(const btSeparation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)=0;
virtual int GetMaxNumContacts() const = 0; virtual int GetMaxNumContacts() const = 0;

2
Extras/SATConvexCollision/Shape.h
View File

@@ -26,7 +26,7 @@
struct Separation struct btSeparation
{ {
short m_featureA; short m_featureA;

46
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -20,12 +20,12 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btContactConstraint.h" #include "btContactConstraint.h"
#include "Solve2LinearConstraint.h" #include "btSolve2LinearConstraint.h"
#include "btContactSolverInfo.h" #include "btContactSolverInfo.h"
#include "Dynamics/BU_Joint.h" #include "Dynamics/BU_Joint.h"
#include "Dynamics/ContactJoint.h" #include "Dynamics/ContactJoint.h"
#include "LinearMath/GenIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#define USE_SOR_SOLVER #define USE_SOR_SOLVER
@@ -69,13 +69,13 @@ m_erp(0.4f)
//iterative lcp and penalty method //iterative lcp and penalty method
float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) float OdeConstraintSolver::SolveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{ {
m_CurBody = 0; m_CurBody = 0;
m_CurJoint = 0; m_CurJoint = 0;
RigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES]; btRigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
int numBodies = 0; int numBodies = 0;
BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4]; BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4];
@@ -86,11 +86,11 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
int body0=-1,body1=-1; int body0=-1,body1=-1;
PersistentManifold* manifold = manifoldPtr[j]; btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->GetNumContacts() > 0) if (manifold->GetNumContacts() > 0)
{ {
body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies); body0 = ConvertBody((btRigidBody*)manifold->GetBody0(),bodies,numBodies);
body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies); body1 = ConvertBody((btRigidBody*)manifold->GetBody1(),bodies,numBodies);
ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer); ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
} }
} }
@@ -104,15 +104,15 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
///////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////
typedef SimdScalar dQuaternion[4]; typedef btScalar dQuaternion[4];
#define _R(i,j) R[(i)*4+(j)] #define _R(i,j) R[(i)*4+(j)]
void dRfromQ1 (dMatrix3 R, const dQuaternion q) void dRfromQ1 (dMatrix3 R, const dQuaternion q)
{ {
// q = (s,vx,vy,vz) // q = (s,vx,vy,vz)
SimdScalar qq1 = 2.f*q[1]*q[1]; btScalar qq1 = 2.f*q[1]*q[1];
SimdScalar qq2 = 2.f*q[2]*q[2]; btScalar qq2 = 2.f*q[2]*q[2];
SimdScalar qq3 = 2.f*q[3]*q[3]; btScalar qq3 = 2.f*q[3]*q[3];
_R(0,0) = 1.f - qq2 - qq3; _R(0,0) = 1.f - qq2 - qq3;
_R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
_R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
@@ -132,7 +132,7 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q)
int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) int OdeConstraintSolver::ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies)
{ {
if (!body || (body->getInvMass() == 0.f) ) if (!body || (body->getInvMass() == 0.f) )
{ {
@@ -194,13 +194,13 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num
static ContactJoint gJointArray[MAX_JOINTS_1]; static ContactJoint gJointArray[MAX_JOINTS_1];
void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer) btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{ {
manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(), manifold->RefreshContactPoints(((btRigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform()); ((btRigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
int bodyId0 = _bodyId0,bodyId1 = _bodyId1; int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
@@ -209,31 +209,31 @@ void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Join
bool swapBodies = (bodyId0 < 0); bool swapBodies = (bodyId0 < 0);
RigidBody* body0,*body1; btRigidBody* body0,*body1;
if (swapBodies) if (swapBodies)
{ {
bodyId0 = _bodyId1; bodyId0 = _bodyId1;
bodyId1 = _bodyId0; bodyId1 = _bodyId0;
body0 = (RigidBody*)manifold->GetBody1(); body0 = (btRigidBody*)manifold->GetBody1();
body1 = (RigidBody*)manifold->GetBody0(); body1 = (btRigidBody*)manifold->GetBody0();
} else } else
{ {
body0 = (RigidBody*)manifold->GetBody0(); body0 = (btRigidBody*)manifold->GetBody0();
body1 = (RigidBody*)manifold->GetBody1(); body1 = (btRigidBody*)manifold->GetBody1();
} }
assert(bodyId0 >= 0); assert(bodyId0 >= 0);
SimdVector3 color(0,1,0); btVector3 color(0,1,0);
for (i=0;i<numContacts;i++) for (i=0;i<numContacts;i++)
{ {
if (debugDrawer) if (debugDrawer)
{ {
const ManifoldPoint& cp = manifold->GetContactPoint(i); const btManifoldPoint& cp = manifold->GetContactPoint(i);
debugDrawer->DrawContactPoint( debugDrawer->DrawContactPoint(
cp.m_positionWorldOnB, cp.m_positionWorldOnB,

14
Extras/quickstep/OdeConstraintSolver.h
View File

@@ -16,14 +16,14 @@ subject to the following restrictions:
#ifndef ODE_CONSTRAINT_SOLVER_H #ifndef ODE_CONSTRAINT_SOLVER_H
#define ODE_CONSTRAINT_SOLVER_H #define ODE_CONSTRAINT_SOLVER_H
#include "ConstraintSolver.h" #include "btConstraintSolver.h"
class RigidBody; class btRigidBody;
class BU_Joint; class BU_Joint;
/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework /// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework
/// It uses the the unmodified version of quickstep solver from the open dynamics project /// It uses the the unmodified version of quickstep solver from the open dynamics project
class OdeConstraintSolver : public ConstraintSolver class OdeConstraintSolver : public btConstraintSolver
{ {
private: private:
@@ -34,9 +34,9 @@ private:
float m_erp; float m_erp;
int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); int ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies);
void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, void ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer); btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
public: public:
@@ -44,7 +44,7 @@ public:
virtual ~OdeConstraintSolver() {} virtual ~OdeConstraintSolver() {}
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0); virtual float SolveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0);
///setConstraintForceMixing, the cfm adds some positive value to the main diagonal ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'

Some files were not shown because too many files have changed in this diff Show More