AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues 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
Show Stats 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 "BspLoader.h"
#include "CcdPhysicsEnvironment.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
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"))
{
@ -53,7 +53,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
for (int b=0;b<leaf.numLeafBrushes;b++)
{
std::vector<SimdVector3> planeEquations;
std::vector<btVector3> planeEquations;
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];
int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid];
SimdVector3 planeEq;
btVector3 planeEq;
planeEq.setValue(
plane.normal[0],
plane.normal[1],
@ -83,13 +83,13 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
if (isValidBrush)
{
std::vector<SimdVector3> vertices;
std::vector<btVector3> vertices;
getVerticesFromPlaneEquations(planeEquations,vertices);
printf("getVerticesFromPlaneEquations returned %i\n",(int)vertices.size());
bool isEntity = false;
SimdVector3 entityTarget(0.f,0.f,0.f);
btVector3 entityTarget(0.f,0.f,0.f);
AddConvexVerticesCollider(vertices,isEntity,entityTarget);
}
@ -109,7 +109,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
const BSPEntity& entity = bspLoader.m_entities[i];
const char* cl = bspLoader.ValueForKey(&entity,"classname");
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");
if ( strcmp( cl, "" ) ) {
@ -136,7 +136,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
const BSPModel& model = bspLoader.m_dmodels[modelnr];
for (int n=0;n<model.numBrushes;n++)
{
std::vector<SimdVector3> planeEquations;
std::vector<btVector3> planeEquations;
bool isValidBrush = false;
//convert brush
@ -148,7 +148,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
int planeid = brushside.planeNum;
BSPPlane& plane = bspLoader.m_dplanes[planeid];
SimdVector3 planeEq;
btVector3 planeEq;
planeEq.setValue(
plane.normal[0],
plane.normal[1],
@ -160,7 +160,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling)
if (isValidBrush)
{
std::vector<SimdVector3> vertices;
std::vector<btVector3> vertices;
getVerticesFromPlaneEquations(planeEquations,vertices);
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();
// brute force:
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++)
{
const SimdVector3& N2 = planeEquations[j];
const btVector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
const SimdVector3& N3 = planeEquations[k];
const btVector3& N3 = planeEquations[k];
SimdVector3 n2n3; n2n3 = N2.cross(N3);
SimdVector3 n3n1; n3n1 = N3.cross(N1);
SimdVector3 n1n2; n1n2 = N1.cross(N2);
btVector3 n2n3; n2n3 = N2.cross(N3);
btVector3 n3n1; n3n1 = N3.cross(N1);
btVector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > 0.0001f ) &&
( n3n1.length2() > 0.0001f ) &&
@ -231,13 +231,13 @@ void BspConverter::getVerticesFromPlaneEquations(const std::vector<SimdVector3>&
float quotient = (N1.dot(n2n3));
if (SimdFabs(quotient) > 0.000001f)
if (btFabs(quotient) > 0.000001f)
{
quotient = -1.f / quotient;
n2n3 *= N1[3];
n3n1 *= N2[3];
n1n2 *= N3[3];
SimdVector3 potentialVertex = n2n3;
btVector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
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();
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;
if (dist>0.f)
{

8
Demos/BspDemo/BspConverter.h
View File

@ -18,7 +18,7 @@ subject to the following restrictions:
class BspLoader;
#include <vector>
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
///BspConverter turns a loaded bsp level into convex parts (vertices)
class BspConverter
@ -31,11 +31,11 @@ class BspConverter
}
///Utility function to create vertices from a Quake Brush. Brute force but it works.
///Bit overkill to use QHull package
void getVerticesFromPlaneEquations(const std::vector<SimdVector3>& planeEquations , std::vector<SimdVector3>& verticesOut );
bool isInside(const std::vector<SimdVector3>& planeEquations, const SimdVector3& point, float margin);
void getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut );
bool isInside(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin);
///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 "LinearMath/GenQuickprof.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.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)
///like adding a collision Triggering (as example)
@ -69,12 +69,12 @@ public:
{
bool isDynamic = false;
float mass = 0.f;
SimdTransform startTransform;
btTransform startTransform;
//can use a shift
startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,0,-10.f));
startTransform.setOrigin(btVector3(0,0,-10.f));
//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);
}
@ -128,15 +128,15 @@ void BspDemo::initPhysics(char* bspfilename)
{
m_cameraUp = SimdVector3(0,0,1);
m_cameraUp = btVector3(0,0,1);
m_forwardAxis = 1;
///Setup a Physics Simulation Environment
CollisionDispatcher* dispatcher = new CollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
//BroadphaseInterface* broadphase = new SimpleBroadphase();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btVector3 worldAabbMin(-10000,-10000,-10000);
btVector3 worldAabbMax(10000,10000,10000);
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//BroadphaseInterface* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
m_physicsEnvironmentPtr->setDeactivationTime(2.f);
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.
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,
or (at your option) any later version.
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
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
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.
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,
or (at your option) any later version.
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
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
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
===========================================================================

126
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.
*/
//#define USER_DEFINED_FRICTION_MODEL 1
//#define PRINT_CONTACT_STATISTICS 1
#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1
#include "CcdPhysicsEnvironment.h"
#include "ParallelPhysicsEnvironment.h"
#include "btBulletDynamicsCommon.h"
#include "CcdPhysicsController.h"
#include "MyMotionState.h"
#include "ParallelIslandDispatcher.h"
#include "LinearMath/GenQuickprof.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
#include "PHY_Pro.h"
#include "BMF_Api.h"
#include <stdio.h> //printf debugging
@ -68,40 +57,35 @@ const int maxNumObjects = 32760;
int shapeIndex[maxNumObjects];
#ifdef USE_PARALLEL_DISPATCHER
ParallelPhysicsEnvironment* m_physicsEnvironmentPtr = 0;
#else
CcdPhysicsEnvironment* m_physicsEnvironmentPtr = 0;
#endif
#define CUBE_HALF_EXTENTS 1
#define EXTRA_HEIGHT -20.f
//GL_LineSegmentShape shapeE(SimdPoint3(-50,0,0),
// SimdPoint3(50,0,0));
//GL_LineSegmentShape shapeE(btPoint3(-50,0,0),
// btPoint3(50,0,0));
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.
///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346
//#define USE_GROUND_PLANE 1
#ifdef USE_GROUND_PLANE
new StaticPlaneShape(SimdVector3(0,1,0),10),
new btStaticPlaneShape(btVector3(0,1,0),10),
#else
new BoxShape (SimdVector3(50,10,50)),
new btBoxShape (btVector3(50,10,50)),
#endif
new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
new SphereShape (CUBE_HALF_EXTENTS- 0.05f),
new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)),
new btSphereShape (CUBE_HALF_EXTENTS- 0.05f),
//new ConeShape(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 btConeShape(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS),
//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);
if (m_physicsEnvironmentPtr)
m_physicsEnvironmentPtr->proceedDeltaTime(0.f,deltaTime);
if (m_dynamicsWorld)
m_dynamicsWorld->stepSimulation(deltaTime);
#ifdef USE_QUICKPROF
Profiler::beginBlock("render");
btProfiler::beginBlock("render");
#endif //USE_QUICKPROF
renderme();
#ifdef USE_QUICKPROF
Profiler::endBlock("render");
btProfiler::endBlock("render");
#endif
glFlush();
//some additional debugging info
@ -171,12 +153,15 @@ void CcdPhysicsDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
/*
if (m_physicsEnvironmentPtr)
{
m_physicsEnvironmentPtr->UpdateAabbs(deltaTime);
//draw contactpoints
m_physicsEnvironmentPtr->CallbackTriggers();
}
*/
renderme();
@ -192,7 +177,7 @@ void CcdPhysicsDemo::displayCallback(void) {
void CcdPhysicsDemo::clientResetScene()
{
/*
int i;
int numObjects = m_physicsEnvironmentPtr->GetNumControllers();
@ -203,7 +188,7 @@ void CcdPhysicsDemo::clientResetScene()
{
CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i);
if ((getDebugMode() & IDebugDraw::DBG_NoHelpText))
if ((getDebugMode() & btIDebugDraw::DBG_NoHelpText))
{
if (ctrl->GetRigidBody()->GetCollisionShape()->GetShapeType() != SPHERE_SHAPE_PROXYTYPE)
{
@ -213,7 +198,7 @@ void CcdPhysicsDemo::clientResetScene()
ctrl->GetRigidBody()->SetCollisionShape(shapePtr[1]);
}
BroadphaseProxy* bpproxy = ctrl->GetRigidBody()->m_broadphaseHandle;
btBroadphaseProxy* bpproxy = ctrl->GetRigidBody()->m_broadphaseHandle;
m_physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy);
}
@ -236,11 +221,13 @@ void CcdPhysicsDemo::clientResetScene()
ctrl->SetAngularVelocity(0,0,0,false);
}
}
*/
}
///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
return 0.f;
@ -249,32 +236,25 @@ float myFrictionModel( RigidBody& body1, RigidBody& body2, ManifoldPoint& contac
void CcdPhysicsDemo::initPhysics()
{
CollisionDispatcher* dispatcher = new CollisionDispatcher();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
btVector3 worldAabbMin(-10000,-10000,-10000);
btVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
// OverlappingPairCache* broadphase = new SimpleBroadphase;
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
// btOverlappingPairCache* broadphase = new btSimpleBroadphase;
#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
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
#ifdef USE_PARALLEL_DISPATCHER
m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase);
#else
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
#endif
m_physicsEnvironmentPtr->setDeactivationTime(2.f);
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
m_physicsEnvironmentPtr->setGravity(0,-10,0);
m_physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
#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->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1);
@ -285,7 +265,7 @@ void CcdPhysicsDemo::initPhysics()
int i;
SimdTransform tr;
btTransform tr;
tr.setIdentity();
@ -301,26 +281,26 @@ void CcdPhysicsDemo::initPhysics()
if (useCompound)
{
CompoundShape* compoundShape = new CompoundShape();
CollisionShape* oldShape = shapePtr[1];
btCompoundShape* compoundShape = new btCompoundShape();
btCollisionShape* oldShape = shapePtr[1];
shapePtr[1] = compoundShape;
SimdTransform ident;
btTransform ident;
ident.setIdentity();
ident.setOrigin(SimdVector3(0,0,0));
ident.setOrigin(btVector3(0,0,0));
compoundShape->AddChildShape(ident,oldShape);//
ident.setOrigin(SimdVector3(0,0,2));
compoundShape->AddChildShape(ident,new SphereShape(0.9));//
ident.setOrigin(btVector3(0,0,2));
compoundShape->AddChildShape(ident,new btSphereShape(0.9));//
}
for (i=0;i<gNumObjects;i++)
{
CollisionShape* shape = shapePtr[shapeIndex[i]];
btCollisionShape* shape = shapePtr[shapeIndex[i]];
shape->SetMargin(0.05f);
bool isDyna = i>0;
SimdTransform trans;
btTransform trans;
trans.setIdentity();
if (i>0)
@ -338,13 +318,13 @@ void CcdPhysicsDemo::initPhysics()
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);
trans.setOrigin(pos);
} else
{
trans.setOrigin(SimdVector3(0,-30,0));
trans.setOrigin(btVector3(0,-30,0));
}
float mass = 1.f;
@ -352,13 +332,16 @@ void CcdPhysicsDemo::initPhysics()
if (!isDyna)
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
ctrl->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
body->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
//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
///Advanced use: override the friction solver
ctrl->GetRigidBody()->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1;
@ -369,7 +352,6 @@ void CcdPhysicsDemo::initPhysics()
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 "LinearMath/SimdPoint3.h"
#include "LinearMath/btPoint3.h"
MyMotionState::MyMotionState()
{
@ -51,13 +51,13 @@ void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& q
void MyMotionState::setWorldPosition(float posX,float posY,float posZ)
{
SimdPoint3 pos(posX,posY,posZ);
btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos );
}
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 );
}

4
Demos/CcdPhysicsDemo/MyMotionState.h
View File

@ -17,7 +17,7 @@ subject to the following restrictions:
#define MY_MOTIONSTATE_H
#include "PHY_IMotionState.h"
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btTransform.h>
class MyMotionState : public PHY_IMotionState
@ -37,7 +37,7 @@ class MyMotionState : public PHY_IMotionState
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);
//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;
domRigid_bodyRef m_rigidBodyRef2;
@ -52,12 +52,12 @@ struct ConstraintInput
};
struct RigidBodyOutput
struct btRigidBodyOutput
{
float m_mass;
bool m_isDynamics;
CollisionShape* m_colShape;
CompoundShape* m_compoundShape;
btCollisionShape* m_colShape;
btCompoundShape* m_compoundShape;
};
@ -66,13 +66,13 @@ domMatrix_Array emptyMatrixArray;
domNodeRef m_colladadomNodes[COLLADA_CONVERTER_MAX_NUM_OBJECTS];
SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
btTransform GetbtTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
domRotate_Array& rotateArray,
domTranslate_Array& translateArray
)
{
SimdTransform startTransform;
btTransform startTransform;
startTransform.setIdentity();
unsigned int i;
@ -81,9 +81,9 @@ SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
{
domMatrixRef matrixRef = matrixArray[i];
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);
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(8),fl16.get(9),fl16.get(10));
startTransform.setBasis(basis);
@ -94,15 +94,15 @@ SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray,
domRotateRef rotateRef = rotateArray[i];
domFloat4 fl4 = rotateRef->getValue();
float angleRad = SIMD_RADS_PER_DEG*fl4.get(3);
SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat);
btQuaternion rotQuat(btVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad);
startTransform.getBasis() = startTransform.getBasis() * btMatrix3x3(rotQuat);
}
for (i=0;i<translateArray.getCount();i++)
{
domTranslateRef translateRef = translateArray[i];
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;
}
@ -185,20 +185,20 @@ bool ColladaConverter::convert()
printf(" X is Up Data and Hiearchies must be converted!\n" );
printf(" Conversion to X axis Up isn't currently supported!\n" );
printf(" COLLADA_RT defaulting to Y Up \n" );
SetGravity(SimdVector3(-10,0,0));
SetCameraInfo(SimdVector3(1,0,0),1);
SetGravity(btVector3(-10,0,0));
SetCameraInfo(btVector3(1,0,0),1);
break;
case UPAXISTYPE_Y_UP:
printf(" Y Axis is Up for this file \n" );
printf(" COLLADA_RT set to Y Up \n" );
SetGravity(SimdVector3(0,-10,0));
SetCameraInfo(SimdVector3(0,1,0),0);
SetGravity(btVector3(0,-10,0));
SetCameraInfo(btVector3(0,1,0),0);
break;
case UPAXISTYPE_Z_UP:
printf(" Z Axis is Up for this file \n" );
printf(" All Geometry and Hiearchies must be converted!\n" );
SetGravity(SimdVector3(0,0,-10));
SetGravity(btVector3(0,0,-10));
break;
default:
@ -294,7 +294,7 @@ bool ColladaConverter::convert()
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));
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;
bool isDynamics = true;
CollisionShape* colShape = 0;
CompoundShape* compoundShape = 0;
btCollisionShape* colShape = 0;
btCompoundShape* compoundShape = 0;
xsNCName bodyName = instRigidbodyRef->getBody();
@ -355,11 +355,11 @@ bool ColladaConverter::convert()
{
RigidBodyOutput output;
btRigidBodyOutput output;
output.m_colShape = colShape;
output.m_compoundShape = compoundShape;
RigidBodyInput rbInput;
btRigidBodyInput rbInput;
rbInput.m_rigidBodyRef2 = rigidBodyRef;
rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
ConvertRigidBodyRef( rbInput , output );
@ -380,7 +380,7 @@ bool ColladaConverter::convert()
if (colShape)
{
RigidBodyInput input;
btRigidBodyInput input;
input.m_instanceRigidBodyRef = instRigidbodyRef;
input.m_rigidBodyRef2 = 0;
input.m_bodyName = (char*)bodyName;
@ -399,8 +399,8 @@ bool ColladaConverter::convert()
float mass = 1.f;
bool isDynamics = true;
CollisionShape* colShape = 0;
CompoundShape* compoundShape = 0;
btCollisionShape* colShape = 0;
btCompoundShape* compoundShape = 0;
xsNCName bodyName = instRigidbodyRef->getBody();
@ -430,11 +430,11 @@ bool ColladaConverter::convert()
{
RigidBodyOutput output;
btRigidBodyOutput output;
output.m_colShape = colShape;
output.m_compoundShape = compoundShape;
RigidBodyInput rbInput;
btRigidBodyInput rbInput;
rbInput.m_rigidBodyRef2 = rigidBodyRef;
rbInput.m_instanceRigidBodyRef = instRigidbodyRef;
ConvertRigidBodyRef( rbInput , output );
@ -455,7 +455,7 @@ bool ColladaConverter::convert()
if (colShape)
{
RigidBodyInput input;
btRigidBodyInput input;
input.m_instanceRigidBodyRef = instRigidbodyRef;
input.m_rigidBodyRef2 = 0;
input.m_bodyName = (char*)bodyName;
@ -560,31 +560,31 @@ void ColladaConverter::PrepareConstraints(ConstraintInput& input)
const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common();
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();
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();
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();
SimdVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2));
btVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2));
{
int constraintId;
SimdTransform attachFrameRef0;
btTransform attachFrameRef0;
attachFrameRef0 =
GetSimdTransformFromCOLLADA_DOM
GetbtTransformFromCOLLADA_DOM
(
emptyMatrixArray,
attachRefBody->getRotate_array(),
attachRefBody->getTranslate_array());
SimdTransform attachFrameOther;
btTransform attachFrameOther;
attachFrameOther =
GetSimdTransformFromCOLLADA_DOM
GetbtTransformFromCOLLADA_DOM
(
emptyMatrixArray,
attachBody1->getRotate_array(),
@ -609,10 +609,10 @@ void ColladaConverter::PrepareConstraints(ConstraintInput& input)
//limited means upper > lower
//limitIndex: first 3 are linear, next 3 are angular
SimdVector3 linearLowerLimits = minLinearLimit;
SimdVector3 linearUpperLimits = maxLinearLimit;
SimdVector3 angularLowerLimits = angularMin;
SimdVector3 angularUpperLimits = angularMax;
btVector3 linearLowerLimits = minLinearLimit;
btVector3 linearUpperLimits = maxLinearLimit;
btVector3 angularLowerLimits = angularMin;
btVector3 angularUpperLimits = angularMax;
{
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();
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
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
startTransform = GetSimdTransformFromCOLLADA_DOM(
startTransform = GetbtTransformFromCOLLADA_DOM(
node->getMatrix_array(),
node->getRotate_array(),
node->getTranslate_array()
@ -692,7 +692,7 @@ void ColladaConverter::PreparePhysicsObject(struct RigidBodyInput& input, bool i
{
domScaleRef scaleRef = node->getScale_array()[i];
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();
SimdVector3 axis(quat.getX(),quat.getY(),quat.getZ());
btQuaternion quat = m_physObjects[i]->GetRigidBody()->getCenterOfMassTransform().getRotation();
btVector3 axis(quat.getX(),quat.getY(),quat.getZ());
axis[3] = 0.f;
//check for axis length
SimdScalar len = axis.length2();
btScalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = SimdVector3(1.f,0.f,0.f);
axis = btVector3(1.f,0.f,0.f);
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(1,axis[1]);
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();
@ -913,9 +913,9 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
if (planeRef->getEquation())
{
const domFloat4 planeEq = planeRef->getEquation()->getValue();
SimdVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2));
SimdScalar planeConstant = planeEq.get(3);
rbOutput.m_colShape = new StaticPlaneShape(planeNormal,planeConstant);
btVector3 planeNormal(planeEq.get(0),planeEq.get(1),planeEq.get(2));
btScalar planeConstant = planeEq.get(3);
rbOutput.m_colShape = new btStaticPlaneShape(planeNormal,planeConstant);
}
}
@ -928,14 +928,14 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
float x = halfExtents.get(0);
float y = halfExtents.get(1);
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())
{
domSphereRef sphereRef = shapeRef->getSphere();
domSphere::domRadiusRef radiusRef = sphereRef->getRadius();
domFloat radius = radiusRef->getValue();
rbOutput.m_colShape = new SphereShape(radius);
rbOutput.m_colShape = new btSphereShape(radius);
}
if (shapeRef->getCylinder())
@ -946,7 +946,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
domFloat radius0 = radius2.get(0);
//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();
TriangleMesh* trimesh = new TriangleMesh();
btTriangleMesh* trimesh = new btTriangleMesh();
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];
const domPRef pRef = triRef->getP();
IndexedMesh meshPart;
btIndexedMesh meshPart;
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'
for (;t<meshPart.m_numTriangles;t++)
{
SimdVector3 verts[3];
btVector3 verts[3];
int index0;
for (int i=0;i<3;i++)
{
@ -1055,11 +1055,11 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
if (rbOutput.m_isDynamics)
{
printf("moving concave <mesh> not supported, transformed into convex\n");
rbOutput.m_colShape = new ConvexTriangleMeshShape(trimesh);
rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
} else
{
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.
//we are not there yet...
@ -1144,7 +1144,7 @@ void ColladaConverter::ConvertRigidBodyRef( RigidBodyInput& rbInput,RigidBodyOut
domFloat fl2 = listFloats.get(k+2);
//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);
//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)
{
rbOutput.m_compoundShape = new CompoundShape();
rbOutput.m_compoundShape = new btCompoundShape();
}
SimdTransform localTransform;
btTransform localTransform;
localTransform.setIdentity();
if (hasShapeLocalTransform)
{
localTransform = GetSimdTransformFromCOLLADA_DOM(
localTransform = GetbtTransformFromCOLLADA_DOM(
emptyMatrixArray,
shapeRef->getRotate_array(),
shapeRef->getTranslate_array()

30
Demos/ColladaDemo/ColladaConverter.h
View File

@ -18,10 +18,10 @@ subject to the following restrictions:
#ifndef COLLADA_CONVERTER_H
#define COLLADA_CONVERTER_H
#include "LinearMath/SimdTransform.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
class CollisionShape;
class btCollisionShape;
class PHY_IPhysicsController;
class CcdPhysicsController;
class ConstraintInput;
@ -44,11 +44,11 @@ protected:
int m_numObjects;
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 ConvertRigidBodyRef( struct RigidBodyInput& , struct RigidBodyOutput& output );
void ConvertRigidBodyRef( struct btRigidBodyInput& , struct btRigidBodyOutput& output );
public:
@ -68,22 +68,22 @@ public:
///those 2 virtuals are called for each constraint/physics object
virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
btTransform& localAttachmentFrameRef,
btTransform& localAttachmentOther,
const btVector3& linearMinLimits,
const btVector3& linearMaxLimits,
const btVector3& angularMinLimits,
const btVector3& angularMaxLimits
) = 0;
virtual CcdPhysicsController* CreatePhysicsObject(bool isDynamic,
float mass,
const SimdTransform& startTransform,
CollisionShape* shape) = 0;
const btTransform& startTransform,
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 "CcdPhysicsController.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/GenQuickprof.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
@ -50,12 +50,12 @@ class MyColladaConverter : public ColladaConverter
///those 2 virtuals are called for each constraint/physics object
virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
btTransform& localAttachmentFrameRef,
btTransform& localAttachmentOther,
const btVector3& linearMinLimits,
const btVector3& linearMaxLimits,
const btVector3& angularMinLimits,
const btVector3& angularMaxLimits
)
{
return m_demoApp->GetPhysicsEnvironment()->createUniversalD6Constraint(
@ -71,19 +71,19 @@ class MyColladaConverter : public ColladaConverter
virtual CcdPhysicsController* CreatePhysicsObject(bool isDynamic,
float mass,
const SimdTransform& startTransform,
CollisionShape* shape)
const btTransform& startTransform,
btCollisionShape* shape)
{
CcdPhysicsController* ctrl = m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape);
return ctrl;
}
virtual void SetGravity(const SimdVector3& grav)
virtual void SetGravity(const btVector3& grav)
{
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->setCameraForwardAxis(forwardAxis);
@ -138,15 +138,15 @@ int main(int argc,char** argv)
void ColladaDemo::initPhysics(const char* filename)
{
m_cameraUp = SimdVector3(0,0,1);
m_cameraUp = btVector3(0,0,1);
m_forwardAxis = 1;
///Setup a Physics Simulation Environment
CollisionDispatcher* dispatcher = new CollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
//BroadphaseInterface* broadphase = new SimpleBroadphase();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btVector3 worldAabbMin(-10000,-10000,-10000);
btVector3 worldAabbMax(10000,10000,10000);
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//BroadphaseInterface* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
m_physicsEnvironmentPtr->setDeactivationTime(2.f);
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
/// 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
///It needs internal access
#include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
@ -35,7 +35,7 @@ subject to the following restrictions:
#include "CollisionDemo.h"
#include "GL_ShapeDrawer.h"
#include "GlutStuff.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
float yaw=0.f,pitch=0.f,roll=0.f;
@ -44,9 +44,9 @@ const int numObjects = 2;
GL_Simplex1to4 simplex;
PolyhedralConvexShape* shapePtr[maxNumObjects];
btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform tr[numObjects];
btTransform tr[numObjects];
int screenWidth = 640;
int screenHeight = 480;
@ -70,22 +70,22 @@ int main(int argc,char** argv)
void CollisionDemo::initPhysics()
{
m_debugMode |= IDebugDraw::DBG_DrawWireframe;
m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
m_azi = 250.f;
m_ele = 25.f;
tr[0].setOrigin(SimdVector3(0.0013328250f,8.1363249f,7.0390840f));
tr[1].setOrigin(SimdVector3(0.00000000f,9.1262732f,2.0343180f));
tr[0].setOrigin(btVector3(0.0013328250f,8.1363249f,7.0390840f));
tr[1].setOrigin(btVector3(0.00000000f,9.1262732f,2.0343180f));
//tr[0].setOrigin(SimdVector3(0,0,0));
//tr[1].setOrigin(SimdVector3(0,10,0));
//tr[0].setOrigin(btVector3(0,0,0));
//tr[1].setOrigin(btVector3(0,10,0));
SimdMatrix3x3 basisA;
btMatrix3x3 basisA;
basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
-0.00029313788f,0.99753088f,0.070228584f,
-0.00089153741f,-0.070228823f,0.99753052f);
SimdMatrix3x3 basisB;
btMatrix3x3 basisB;
basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f,
4.4865495e-018f,0.97979438f,0.20000751f,
4.4410586e-017f,-0.20000751f,0.97979438f);
@ -95,11 +95,11 @@ void CollisionDemo::initPhysics()
SimdVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
SimdVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);
btVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
btVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);
BoxShape* boxA = new BoxShape(boxHalfExtentsA);
BoxShape* boxB = new BoxShape(boxHalfExtentsB);
btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
shapePtr[0] = boxA;
shapePtr[1] = boxB;
@ -112,8 +112,8 @@ void CollisionDemo::clientMoveAndDisplay()
}
static VoronoiSimplexSolver sGjkSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
static btVoronoiSimplexSolver sGjkSimplexSolver;
btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
@ -127,13 +127,13 @@ void CollisionDemo::displayCallback(void) {
float m[16];
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);
PointCollector gjkOutput;
GjkPairDetector::ClosestPointInput input;
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
@ -141,7 +141,7 @@ void CollisionDemo::displayCallback(void) {
if (gjkOutput.m_hasResult)
{
SimdVector3 endPt = gjkOutput.m_pointInWorld +
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
@ -159,26 +159,26 @@ void CollisionDemo::displayCallback(void) {
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);
SimdPoint3 ybuf[4],pbuf[4],qbuf[4];
btPoint3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.Reset();
for (i=0;i<numpoints;i++)
simplex.AddVertex(ybuf[i]);
SimdTransform ident;
btTransform ident;
ident.setIdentity();
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);
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 "btBulletCollisionCommon.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#include "GL_ShapeDrawer.h"
#include "CollisionInterfaceDemo.h"
#include "GlutStuff.h"
@ -36,8 +36,8 @@ const int numObjects = 2;
GL_Simplex1to4 simplex;
CollisionObject objects[maxNumObjects];
CollisionWorld* collisionWorld = 0;
btCollisionObject objects[maxNumObjects];
btCollisionWorld* collisionWorld = 0;
int screenWidth = 640;
int screenHeight = 480;
@ -57,40 +57,40 @@ int main(int argc,char** argv)
void CollisionInterfaceDemo::initPhysics()
{
m_debugMode |= IDebugDraw::DBG_DrawWireframe;
m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
SimdMatrix3x3 basisA;
btMatrix3x3 basisA;
basisA.setIdentity();
SimdMatrix3x3 basisB;
btMatrix3x3 basisB;
basisB.setIdentity();
objects[0].m_worldTransform.setBasis(basisA);
objects[1].m_worldTransform.setBasis(basisB);
//SimdPoint3 points0[3]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(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 points0[3]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1)};
//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));
BoxShape* boxB = new BoxShape(SimdVector3(0.5,0.5,0.5));
btBoxShape* boxA = new btBoxShape(btVector3(1,1,1));
btBoxShape* boxB = new btBoxShape(btVector3(0.5,0.5,0.5));
//ConvexHullShape hullA(points0,3);
//hullA.setLocalScaling(SimdVector3(3,3,3));
//hullA.setLocalScaling(btVector3(3,3,3));
//ConvexHullShape hullB(points1,4);
//hullB.setLocalScaling(SimdVector3(4,4,4));
//hullB.setLocalScaling(btVector3(4,4,4));
objects[0].m_collisionShape = boxA;//&hullA;
objects[1].m_collisionShape = boxB;//&hullB;
CollisionDispatcher* dispatcher = new CollisionDispatcher;
SimdVector3 worldAabbMin(-1000,-1000,-1000);
SimdVector3 worldAabbMax(1000,1000,1000);
btCollisionDispatcher* dispatcher = new btCollisionDispatcher;
btVector3 worldAabbMin(-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* broadphase = new SimpleBroadphase;
//SimpleBroadphase* broadphase = new btSimpleBroadphase;
collisionWorld = new CollisionWorld(dispatcher,broadphase);
collisionWorld = new btCollisionWorld(dispatcher,broadphase);
collisionWorld->AddCollisionObject(&objects[0]);
collisionWorld->AddCollisionObject(&objects[1]);
@ -107,8 +107,8 @@ void CollisionInterfaceDemo::clientMoveAndDisplay()
}
static VoronoiSimplexSolver sGjkSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
static btVoronoiSimplexSolver sGjkSimplexSolver;
btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
@ -127,21 +127,21 @@ void CollisionInterfaceDemo::displayCallback(void) {
int numManifolds = collisionWorld->GetDispatcher()->GetNumManifolds();
for (i=0;i<numManifolds;i++)
{
PersistentManifold* contactManifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
CollisionObject* obA = static_cast<CollisionObject*>(contactManifold->GetBody0());
CollisionObject* obB = static_cast<CollisionObject*>(contactManifold->GetBody1());
btPersistentManifold* contactManifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->GetBody0());
btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->GetBody1());
contactManifold->RefreshContactPoints(obA->m_worldTransform,obB->m_worldTransform);
int numContacts = contactManifold->GetNumContacts();
for (int j=0;j<numContacts;j++)
{
ManifoldPoint& pt = contactManifold->GetContactPoint(j);
btManifoldPoint& pt = contactManifold->GetContactPoint(j);
glBegin(GL_LINES);
glColor3f(1, 0, 1);
SimdVector3 ptA = pt.GetPositionWorldOnA();
SimdVector3 ptB = pt.GetPositionWorldOnB();
btVector3 ptA = pt.GetPositionWorldOnA();
btVector3 ptB = pt.GetPositionWorldOnB();
glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z());
@ -162,14 +162,14 @@ void CollisionInterfaceDemo::displayCallback(void) {
{
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);
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);
@ -182,8 +182,8 @@ void CollisionInterfaceDemo::displayCallback(void) {
void CollisionInterfaceDemo::clientResetScene()
{
objects[0].m_worldTransform.setOrigin(SimdVector3(0.0f,3.f,0.f));
objects[1].m_worldTransform.setOrigin(SimdVector3(0.0f,9.f,0.f));
objects[0].m_worldTransform.setOrigin(btVector3(0.0f,3.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"
///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
{
public:

44
Demos/ConcaveDemo/ConcavePhysicsDemo.cpp
View File

@ -17,7 +17,7 @@ subject to the following restrictions:
#include "CcdPhysicsController.h"
#include "MyMotionState.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
#include "PHY_Pro.h"
#include "ConcaveDemo.h"
@ -30,17 +30,17 @@ GLDebugDrawer debugDrawer;
static const int NUM_VERTICES = 5;
static const int NUM_TRIANGLES=4;
SimdVector3 gVertices[NUM_VERTICES];
btVector3 gVertices[NUM_VERTICES];
int gIndices[NUM_TRIANGLES*3];
const float TRIANGLE_SIZE=80.f;
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= CollisionObject::customMaterialCallback;
inline SimdScalar calculateCombinedFriction(float friction0,float friction1)
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
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)
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;
}
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();
@ -64,12 +64,12 @@ bool CustomMaterialCombinerCallback(ManifoldPoint& cp, const CollisionObject* co
float restitution0 = colObj0->getRestitution();
float restitution1 = colObj1->getRestitution();
if (colObj0->m_collisionFlags & CollisionObject::customMaterialCallback)
if (colObj0->m_collisionFlags & btCollisionObject::customMaterialCallback)
{
friction0 = 1.0;//partId0,index0
restitution0 = 0.f;
}
if (colObj1->m_collisionFlags & CollisionObject::customMaterialCallback)
if (colObj1->m_collisionFlags & btCollisionObject::customMaterialCallback)
{
if (index1&1)
{
@ -107,7 +107,7 @@ void ConcaveDemo::initPhysics()
{
#define TRISIZE 10.f
int vertStride = sizeof(SimdVector3);
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
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);
SimdVector3* gVertices = new SimdVector3[totalVerts];
btVector3* gVertices = new btVector3[totalVerts];
int* gIndices = new int[totalTriangles*3];
int i;
@ -144,39 +144,39 @@ void ConcaveDemo::initPhysics()
}
}
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
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);
bool isDynamic = false;
float mass = 0.f;
SimdTransform startTransform;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,-2,0));
startTransform.setOrigin(btVector3(0,-2,0));
CcdPhysicsController* staticTrimesh = LocalCreatePhysicsObject(isDynamic, mass, startTransform,trimeshShape);
//enable custom material callback
staticTrimesh->GetRigidBody()->m_collisionFlags |= CollisionObject::customMaterialCallback;
staticTrimesh->GetRigidBody()->m_collisionFlags |= btCollisionObject::customMaterialCallback;
{
for (int i=0;i<10;i++)
{
CollisionShape* boxShape = new BoxShape(SimdVector3(1,1,1));
startTransform.setOrigin(SimdVector3(2*i,1,1));
btCollisionShape* boxShape = new btBoxShape(btVector3(1,1,1));
startTransform.setOrigin(btVector3(2*i,1,1));
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 "LinearMath/SimdPoint3.h"
#include "LinearMath/btPoint3.h"
MyMotionState::MyMotionState()
{
@ -51,13 +51,13 @@ void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& q
void MyMotionState::setWorldPosition(float posX,float posY,float posZ)
{
SimdPoint3 pos(posX,posY,posZ);
btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos );
}
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 );
}

4
Demos/ConcaveDemo/MyMotionState.h
View File

@ -17,7 +17,7 @@ subject to the following restrictions:
#define MY_MOTIONSTATE_H
#include "PHY_IMotionState.h"
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btTransform.h>
class MyMotionState : public PHY_IMotionState
@ -37,7 +37,7 @@ class MyMotionState : public PHY_IMotionState
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 "MyMotionState.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
@ -53,12 +53,12 @@ int main(int argc,char** argv)
void ConstraintDemo::initPhysics()
{
//ConstraintSolver* solver = new SequentialImpulseConstraintSolver;
//ConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
//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);
@ -66,16 +66,16 @@ void ConstraintDemo::initPhysics()
m_physicsEnvironmentPtr->setGravity(0,-10,0);
CollisionShape* shape = new BoxShape(SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
SimdTransform trans;
btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
btTransform trans;
trans.setIdentity();
trans.setOrigin(SimdVector3(0,20,0));
trans.setOrigin(btVector3(0,20,0));
bool isDynamic = false;
float mass = 1.f;
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;
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"
///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
{
public:

6
Demos/ConstraintDemo/MyMotionState.cpp
View File

@ -14,7 +14,7 @@ subject to the following restrictions:
*/
#include "MyMotionState.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btPoint3.h"
MyMotionState::MyMotionState()
{
@ -51,13 +51,13 @@ void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& q
void MyMotionState::setWorldPosition(float posX,float posY,float posZ)
{
SimdPoint3 pos(posX,posY,posZ);
btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos );
}
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 );
}

4
Demos/ConstraintDemo/MyMotionState.h
View File

@ -17,7 +17,7 @@ subject to the following restrictions:
#define MY_MOTIONSTATE_H
#include "PHY_IMotionState.h"
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btTransform.h>
class MyMotionState : public PHY_IMotionState
@ -37,7 +37,7 @@ class MyMotionState : public PHY_IMotionState
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"
///ContinuousConvexCollisionDemo shows the working of the continuous collision detection, including linear and angular motion
class ContinuousConvexCollisionDemo : public DemoApplication
class btContinuousConvexCollisionDemo : public DemoApplication
{
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
#include "LinearMath/SimdQuaternion.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
@ -31,7 +31,7 @@
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
#include "LinearMath/SimdTransformUtil.h"
#include "LinearMath/btTransformUtil.h"
#include "DebugCastResult.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 numObjects = 2;
SimdVector3 angVels[numObjects];
SimdVector3 linVels[numObjects];
btVector3 angVels[numObjects];
btVector3 linVels[numObjects];
PolyhedralConvexShape* shapePtr[maxNumObjects];
btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform fromTrans[maxNumObjects];
SimdTransform toTrans[maxNumObjects];
btTransform fromTrans[maxNumObjects];
btTransform toTrans[maxNumObjects];
int screenWidth = 640;
@ -66,7 +66,7 @@ int screenHeight = 480;
int main(int argc,char** argv)
{
ContinuousConvexCollisionDemo* ccdDemo = new ContinuousConvexCollisionDemo();
btContinuousConvexCollisionDemo* ccdDemo = new btContinuousConvexCollisionDemo();
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);
}
void ContinuousConvexCollisionDemo::initPhysics()
void btContinuousConvexCollisionDemo::initPhysics()
{
fromTrans[0].setOrigin(SimdVector3(0,10,20));
toTrans[0].setOrigin(SimdVector3(0,10,-20));
fromTrans[1].setOrigin(SimdVector3(-2,7,0));
toTrans[1].setOrigin(SimdVector3(-2,10,0));
fromTrans[0].setOrigin(btVector3(0,10,20));
toTrans[0].setOrigin(btVector3(0,10,-20));
fromTrans[1].setOrigin(btVector3(-2,7,0));
toTrans[1].setOrigin(btVector3(-2,10,0));
SimdMatrix3x3 identBasis;
btMatrix3x3 identBasis;
identBasis.setIdentity();
SimdMatrix3x3 basisA;
btMatrix3x3 basisA;
basisA.setIdentity();
basisA.setEulerZYX(0.f,-SIMD_HALF_PI,0.f);
@ -96,15 +96,15 @@ void ContinuousConvexCollisionDemo::initPhysics()
toTrans[1].setBasis(identBasis);
toTrans[1].setBasis(identBasis);
SimdVector3 boxHalfExtentsA(10,1,1);
SimdVector3 boxHalfExtentsB(1.1f,1.1f,1.1f);
BoxShape* boxA = new BoxShape(boxHalfExtentsA);
// BU_Simplex1to4* boxA = new BU_Simplex1to4(SimdPoint3(-2,0,-2),SimdPoint3(2,0,-2),SimdPoint3(0,0,2),SimdPoint3(0,2,0));
// BU_Simplex1to4* boxA = new BU_Simplex1to4(SimdPoint3(-12,0,0),SimdPoint3(12,0,0));
btVector3 boxHalfExtentsA(10,1,1);
btVector3 boxHalfExtentsB(1.1f,1.1f,1.1f);
btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
// btBU_Simplex1to4* boxA = new btBU_Simplex1to4(btPoint3(-2,0,-2),btPoint3(2,0,-2),btPoint3(0,0,2),btPoint3(0,2,0));
// btBU_Simplex1to4* boxA = new btBU_Simplex1to4(btPoint3(-12,0,0),btPoint3(12,0,0));
BoxShape* boxB = new BoxShape(boxHalfExtentsB);
// BU_Simplex1to4 boxB(SimdPoint3(0,10,0),SimdPoint3(0,-10,0));
btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
// btBU_Simplex1to4 boxB(btPoint3(0,10,0),btPoint3(0,-10,0));
shapePtr[0] = boxA;
@ -115,22 +115,22 @@ void ContinuousConvexCollisionDemo::initPhysics()
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
void ContinuousConvexCollisionDemo::clientMoveAndDisplay()
void btContinuousConvexCollisionDemo::clientMoveAndDisplay()
{
displayCallback();
}
static VoronoiSimplexSolver sVoronoiSimplexSolver;
static btVoronoiSimplexSolver sVoronoiSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver;
btSimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver;
bool drawLine= false;
@ -139,7 +139,7 @@ int minlines = 0;
int maxlines = 512;
void ContinuousConvexCollisionDemo::displayCallback(void) {
void btContinuousConvexCollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
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++)
{
@ -165,10 +165,10 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
int numSubSteps = 10;
for (int s=0;s<10;s++)
{
SimdScalar subStep = s * 1.f/(float)numSubSteps;
SimdTransform interpolatedTrans;
btScalar subStep = s * 1.f/(float)numSubSteps;
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);
//GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]);
@ -177,15 +177,15 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
//GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]);
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);
SimdQuaternion orn;
btQuaternion orn;
mat.getRotation(orn);
orn.setEuler(yaw,pitch,roll);
fromTrans[1].setRotation(orn);
@ -198,17 +198,17 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
pitch += 0.005f;
// yaw += 0.01f;
}
// SimdVector3 fromA(-25,11,0);
// SimdVector3 toA(-15,11,0);
// btVector3 fromA(-25,11,0);
// btVector3 toA(-15,11,0);
// SimdQuaternion ornFromA(0.f,0.f,0.f,1.f);
// SimdQuaternion ornToA(0.f,0.f,0.f,1.f);
// btQuaternion ornFromA(0.f,0.f,0.f,1.f);
// btQuaternion ornToA(0.f,0.f,0.f,1.f);
// SimdTransform rayFromWorld(ornFromA,fromA);
// SimdTransform rayToWorld(ornToA,toA);
// btTransform rayFromWorld(ornFromA,fromA);
// btTransform rayToWorld(ornToA,toA);
SimdTransform rayFromWorld = fromTrans[0];
SimdTransform rayToWorld = toTrans[0];
btTransform rayFromWorld = fromTrans[0];
btTransform rayToWorld = toTrans[0];
if (drawLine)
@ -229,17 +229,17 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
for (i=0;i<numObjects;i++)
{
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++)
{
ConvexCast::CastResult rayResult2;
ConvexCast::CastResult* rayResultPtr;
if (IDebugDraw::DBG_DrawAabb)
btConvexCast::CastResult rayResult2;
btConvexCast::CastResult* rayResultPtr;
if (btIDebugDraw::DBG_DrawAabb)
{
rayResultPtr = &rayResult1;
} else
@ -251,8 +251,8 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
//SubsimplexConvexCast convexCaster(&gGjkSimplexSolver);
//optional
ConvexPenetrationDepthSolver* penetrationDepthSolver = 0;
ContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver );
btConvexPenetrationDepthSolver* penetrationDepthSolver = 0;
btContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver );
gGjkSimplexSolver.reset();
@ -263,16 +263,16 @@ void ContinuousConvexCollisionDemo::displayCallback(void) {
glDisable(GL_DEPTH_TEST);
SimdTransform hitTrans;
SimdTransformUtil::IntegrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans);
btTransform hitTrans;
btTransformUtil::IntegrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans);
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);
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 "btBulletDynamicsCommon.h"
#include "LinearMath/GenQuickprof.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
@ -42,11 +42,11 @@ const int maxNumObjects = 450;
int shapeIndex[maxNumObjects];
SimdVector3 centroid;
btVector3 centroid;
#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);
CollisionDispatcher* dispatcher = new CollisionDispatcher();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
btVector3 worldAabbMin(-10000,-10000,-10000);
btVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
//OverlappingPairCache* broadphase = new SimpleBroadphase();
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//OverlappingPairCache* broadphase = new btSimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
m_physicsEnvironmentPtr->setDeactivationTime(2.f);
m_physicsEnvironmentPtr->setGravity(0,-10,0);
SimdTransform startTransform;
btTransform startTransform;
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
{
@ -119,9 +119,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
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
printf("ConvexResult\n");
@ -148,9 +148,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
SimdVector3 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]);
SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
@ -173,9 +173,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
unsigned int index2 = *src++;
SimdVector3 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]);
SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
@ -196,8 +196,8 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
bool isDynamic = true;
float mass = 1.f;
CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh);
SimdTransform trans;
btCollisionShape* convexShape = new btConvexTriangleMeshShape(trimesh);
btTransform trans;
trans.setIdentity();
trans.setOrigin(centroid);
m_convexDemo->LocalCreatePhysicsObject(isDynamic, mass, trans,convexShape);
@ -216,9 +216,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
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++)
{
@ -226,9 +226,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
int index1 = wo.mIndices[i*3+1];
int index2 = wo.mIndices[i*3+2];
SimdVector3 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]);
SimdVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
btVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
@ -237,13 +237,13 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
trimesh->AddTriangle(vertex0,vertex1,vertex2);
}
CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh);
btCollisionShape* convexShape = new btConvexTriangleMeshShape(trimesh);
bool isDynamic = true;
float mass = 1.f;
SimdTransform startTransform;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(20,2,0));
startTransform.setOrigin(btVector3(20,2,0));
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.
/// GJK with the VoronoiSimplexSolver is used.
/// GJK with the btVoronoiSimplexSolver is used.
///
#include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
@ -51,9 +51,9 @@ const int numObjects = 2;
GL_Simplex1to4 simplex;
PolyhedralConvexShape* shapePtr[maxNumObjects];
btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform tr[numObjects];
btTransform tr[numObjects];
int screenWidth = 640.f;
int screenHeight = 480.f;
@ -62,26 +62,26 @@ int main(int argc,char** argv)
{
clientResetScene();
SimdMatrix3x3 basisA;
btMatrix3x3 basisA;
basisA.setIdentity();
SimdMatrix3x3 basisB;
btMatrix3x3 basisB;
basisB.setIdentity();
tr[0].setBasis(basisA);
tr[1].setBasis(basisB);
SimdPoint3 points0[3]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(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 points0[3]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1)};
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);
ConvexHullShape hullB(points1,5);
btConvexHullShape hullA(points0,3);
btConvexHullShape hullB(points1,5);
shapePtr[0] = &hullA;
shapePtr[1] = &hullB;
SimdTransform tr;
btTransform tr;
tr.setIdentity();
@ -97,8 +97,8 @@ void clientMoveAndDisplay()
}
static VoronoiSimplexSolver sGjkSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
static btVoronoiSimplexSolver sGjkSimplexSolver;
btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
@ -112,13 +112,13 @@ void clientDisplay(void) {
float m[16];
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);
PointCollector gjkOutput;
GjkPairDetector::ClosestPointInput input;
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
@ -126,7 +126,7 @@ void clientDisplay(void) {
if (gjkOutput.m_hasResult)
{
SimdVector3 endPt = gjkOutput.m_pointInWorld +
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
@ -142,26 +142,26 @@ void clientDisplay(void) {
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);
SimdPoint3 ybuf[4],pbuf[4],qbuf[4];
btPoint3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.Reset();
for (i=0;i<numpoints;i++)
simplex.AddVertex(ybuf[i]);
SimdTransform ident;
btTransform ident;
ident.setIdentity();
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);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
@ -175,8 +175,8 @@ void clientDisplay(void) {
void clientResetScene()
{
tr[0].setOrigin(SimdVector3(0.0f,3.f,7.f));
tr[1].setOrigin(SimdVector3(0.0f,9.f,2.f));
tr[0].setOrigin(btVector3(0.0f,3.f,7.f));
tr[1].setOrigin(btVector3(0.0f,9.f,2.f));
}
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 )
* 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
*/
#include "GL_Simplex1to4.h"
#include "LinearMath/SimdQuaternion.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btTransform.h"
#include "GL_ShapeDrawer.h"
#include <GL/glut.h>
#include "GlutStuff.h"
@ -51,34 +51,34 @@ subject to the following restrictions:
EpaPenetrationDepthSolver epaPenDepthSolver;
SimplexSolverInterface simplexSolver;
btSimplexSolverInterface simplexSolver;
int screenWidth = 640.f;
int screenHeight = 480.f;
// Scene stuff
SimdPoint3 g_sceneVolumeMin( -1, -1, -1 );
SimdPoint3 g_sceneVolumeMax( 1, 1, 1 );
btPoint3 g_sceneVolumeMin( -1, -1, -1 );
btPoint3 g_sceneVolumeMax( 1, 1, 1 );
bool g_shapesPenetrate = false;
SimdVector3 g_wWitnesses[ 2 ];
btVector3 g_wWitnesses[ 2 ];
// Shapes stuff
ConvexShape* g_pConvexShapes[ 2 ] = { 0 };
SimdTransform g_convexShapesTransform[ 2 ];
btConvexShape* g_pConvexShapes[ 2 ] = { 0 };
btTransform g_convexShapesTransform[ 2 ];
SimdScalar g_animAngle = SIMD_RADS_PER_DEG;
btScalar g_animAngle = SIMD_RADS_PER_DEG;
bool g_pauseAnim = true;
// 0 - Box ; 1 - Sphere
int g_shapesType[ 2 ] = { 0 };
// Box config
SimdVector3 g_boxExtents( 1, 1, 1 );
btVector3 g_boxExtents( 1, 1, 1 );
// Sphere config
SimdScalar g_sphereRadius = 1;
btScalar g_sphereRadius = 1;
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 ) );
}
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.getZ(), maxPoint.getZ() ) );
}
@ -104,39 +104,39 @@ bool createBoxShape( int shapeIndex )
//
//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_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.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 );
// b = false;
//}
//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 );
// //SimdMatrix3x3 basis( 0.658257, 0.675022, -0.333709,
// //btMatrix3x3 basis( 0.658257, 0.675022, -0.333709,
// // -0.333120, 0.658556, 0.675023,
// // 0.675314, -0.333120, 0.658256 );
// 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 );
//}
//#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 );
@ -149,7 +149,7 @@ bool createBoxShape( 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 );
@ -160,12 +160,12 @@ bool createSphereShape( int shapeIndex )
//static bool b = true;
//if ( b )
//{
// g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0.001, 0, 0 ) );
// g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0.001, 0, 0 ) );
// b = false;
//}
//else
//{
// g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0, 0, 0 ) );
// g_convexShapesTransform[ shapeIndex ].setOrigin( btVector3( 0, 0, 0 ) );
//}
//#endif
@ -191,15 +191,15 @@ bool calcPenDepth()
{
// Ryn Hybrid Pen Depth and EPA if necessary
SimdVector3 v( 1, 0, 0 );
btVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
btScalar squaredDistance = SIMD_INFINITY;
btScalar delta = 0.f;
const SimdScalar margin = g_pConvexShapes[ 0 ]->GetMargin() + g_pConvexShapes[ 1 ]->GetMargin();
const SimdScalar marginSqrd = margin * margin;
const btScalar margin = g_pConvexShapes[ 0 ]->GetMargin() + g_pConvexShapes[ 1 ]->GetMargin();
const btScalar marginSqrd = margin * margin;
SimdScalar maxRelErrorSqrd = 1e-3 * 1e-3;
btScalar maxRelErrorSqrd = 1e-3 * 1e-3;
simplexSolver.reset();
@ -207,16 +207,16 @@ bool calcPenDepth()
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis();
SimdVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis();
btVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis();
btVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis();
SimdVector3 pInA = g_pConvexShapes[ 0 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = g_pConvexShapes[ 1 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
btVector3 pInA = g_pConvexShapes[ 0 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
btVector3 qInB = g_pConvexShapes[ 1 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA );
SimdPoint3 qWorld = g_convexShapesTransform[ 1 ]( qInB );
btPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA );
btPoint3 qWorld = g_convexShapesTransform[ 1 ]( qInB );
SimdVector3 w = pWorld - qWorld;
btVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
@ -232,7 +232,7 @@ bool calcPenDepth()
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
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[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
@ -249,7 +249,7 @@ bool calcPenDepth()
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
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[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
@ -257,7 +257,7 @@ bool calcPenDepth()
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
btScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
@ -269,7 +269,7 @@ bool calcPenDepth()
simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] );
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[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength );
@ -313,7 +313,7 @@ void drawShape( int shapeIndex )
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 )
{
@ -349,10 +349,10 @@ void clientMoveAndDisplay()
{
if ( !g_pauseAnim )
{
SimdMatrix3x3 rot;
btMatrix3x3 rot;
rot.setEulerZYX( g_animAngle * 0.05, g_animAngle * 0.05, g_animAngle * 0.05 );
SimdTransform t;
btTransform t;
t.setIdentity();
t.setBasis( rot );
@ -419,11 +419,11 @@ void clientKeyboard(unsigned char key, int x, int y)
else if ( key == 'T' || key == 't' )
{
#ifdef DEBUG_ME
SimdVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin();
SimdVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin();
btVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin();
btVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin();
SimdMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis();
SimdMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis();
btMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis();
btMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis();
FILE* fp = 0;

62
Demos/ForkLiftDemo/ForkLiftDemo.cpp
View File

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

70
Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp
View File

@ -21,8 +21,8 @@
///Low level demo, doesn't include btBulletCollisionCommon.h
#include "LinearMath/SimdQuaternion.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
@ -49,9 +49,9 @@ const int maxNumObjects = 4;
const int numObjects = 2;
PolyhedralConvexShape* shapePtr[maxNumObjects];
btPolyhedralConvexShape* shapePtr[maxNumObjects];
SimdTransform tr[numObjects];
btTransform tr[numObjects];
int screenWidth = 640;
int screenHeight = 480;
void DrawRasterizerLine(float const* , float const*, int)
@ -73,15 +73,15 @@ int main(int argc,char** argv)
void LinearConvexCastDemo::initPhysics()
{
setCameraDistance(30.f);
tr[0].setOrigin(SimdVector3(0,0,0));
tr[1].setOrigin(SimdVector3(0,10,0));
tr[0].setOrigin(btVector3(0,0,0));
tr[1].setOrigin(btVector3(0,10,0));
SimdMatrix3x3 basisA;
btMatrix3x3 basisA;
basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
-0.00029313788f,0.99753088f,0.070228584f,
-0.00089153741f,-0.070228823f,0.99753052f);
SimdMatrix3x3 basisB;
btMatrix3x3 basisB;
basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f,
4.4865495e-018f,0.97979438f,0.20000751f,
4.4410586e-017f,-0.20000751f,0.97979438f);
@ -91,25 +91,25 @@ void LinearConvexCastDemo::initPhysics()
SimdVector3 boxHalfExtentsA(0.2,4,4);
SimdVector3 boxHalfExtentsB(6,6,6);
btVector3 boxHalfExtentsA(0.2,4,4);
btVector3 boxHalfExtentsB(6,6,6);
BoxShape* boxA = new BoxShape(boxHalfExtentsA);
/* BU_Simplex1to4 boxB;
boxB.AddVertex(SimdPoint3(-5,0,-5));
boxB.AddVertex(SimdPoint3(5,0,-5));
boxB.AddVertex(SimdPoint3(0,0,5));
boxB.AddVertex(SimdPoint3(0,5,0));
btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
/* btBU_Simplex1to4 boxB;
boxB.AddVertex(btPoint3(-5,0,-5));
boxB.AddVertex(btPoint3(5,0,-5));
boxB.AddVertex(btPoint3(0,0,5));
boxB.AddVertex(btPoint3(0,5,0));
*/
BoxShape* boxB = new BoxShape(boxHalfExtentsB);
btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
shapePtr[0] = boxA;
shapePtr[1] = boxB;
shapePtr[0]->SetMargin(0.01f);
shapePtr[1]->SetMargin(0.01f);
SimdTransform tr;
btTransform tr;
tr.setIdentity();
}
@ -124,9 +124,9 @@ void LinearConvexCastDemo::clientMoveAndDisplay()
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
static VoronoiSimplexSolver sVoronoiSimplexSolver;
static btVoronoiSimplexSolver sVoronoiSimplexSolver;
SimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver;
btSimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver;
bool drawLine= false;
@ -145,13 +145,13 @@ void LinearConvexCastDemo::displayCallback(void)
for (i=0;i<numObjects;i++)
{
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;
SimdQuaternion orn;
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[shapeIndex].setRotation(orn);
@ -163,14 +163,14 @@ void LinearConvexCastDemo::displayCallback(void)
yaw += 0.01f;
}
SimdVector3 fromA(-25,11,0);
SimdVector3 toA(15,11,0);
btVector3 fromA(-25,11,0);
btVector3 toA(15,11,0);
SimdQuaternion ornFromA(0.f,0.f,0.f,1.f);
SimdQuaternion ornToA(0.f,0.f,0.f,1.f);
btQuaternion ornFromA(0.f,0.f,0.f,1.f);
btQuaternion ornToA(0.f,0.f,0.f,1.f);
SimdTransform rayFromWorld(ornFromA,fromA);
SimdTransform rayToWorld(ornToA,toA);
btTransform rayFromWorld(ornFromA,fromA);
btTransform rayToWorld(ornToA,toA);
tr[0] = rayFromWorld;
@ -191,16 +191,16 @@ void LinearConvexCastDemo::displayCallback(void)
for (i=1;i<numObjects;i++)
{
ContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0);
GjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
btContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0);
btGjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
//BU_CollisionPair (algebraic version) is currently broken, will look into this
//BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]);
SubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
btSubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
gGjkSimplexSolver.reset();
ConvexCast::CastResult rayResult;
btConvexCast::CastResult rayResult;
@ -208,7 +208,7 @@ void LinearConvexCastDemo::displayCallback(void)
{
glDisable(GL_DEPTH_TEST);
SimdVector3 hitPoint;
btVector3 hitPoint;
hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction);
//draw the raycast result
@ -219,12 +219,12 @@ void LinearConvexCastDemo::displayCallback(void)
glEnd();
glEnable(GL_DEPTH_TEST);
SimdTransform toTransWorld;
btTransform toTransWorld;
toTransWorld = tr[0];
toTransWorld.setOrigin(hitPoint);
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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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 *****
*
* 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
* of the License, or (at your option) any later version. The Blender
* 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,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* 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,
* 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
#include "BulletCollision/NarrowPhaseCollision/btConvexCast.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btTransform.h"
#include "GL_ShapeDrawer.h"
#ifdef WIN32
#include <windows.h>
@ -29,16 +29,16 @@ subject to the following restrictions:
#else
#include <GL/gl.h>
#endif
struct DebugCastResult : public ConvexCast::CastResult
struct btDebugCastResult : public btConvexCast::CastResult
{
SimdTransform m_fromTrans;
const PolyhedralConvexShape* m_shape;
SimdVector3 m_linVel;
SimdVector3 m_angVel;
btTransform m_fromTrans;
const btPolyhedralConvexShape* m_shape;
btVector3 m_linVel;
btVector3 m_angVel;
DebugCastResult(const SimdTransform& fromTrans,const PolyhedralConvexShape* shape,
const SimdVector3& linVel,const SimdVector3& angVel)
btDebugCastResult(const btTransform& fromTrans,const btPolyhedralConvexShape* shape,
const btVector3& linVel,const btVector3& angVel)
:m_fromTrans(fromTrans),
m_shape(shape),
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];
tr.getOpenGLMatrix(m);
@ -66,14 +66,14 @@ struct DebugCastResult : public ConvexCast::CastResult
glPopMatrix();
}
virtual void DebugDraw(SimdScalar fraction)
virtual void DebugDraw(btScalar fraction)
{
float m[16];
SimdTransform hitTrans;
SimdTransformUtil::IntegrateTransform(m_fromTrans,m_linVel,m_angVel,fraction,hitTrans);
btTransform hitTrans;
btTransformUtil::IntegrateTransform(m_fromTrans,m_linVel,m_angVel,fraction,hitTrans);
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 "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
#include "CcdPhysicsEnvironment.h"
#include "CcdPhysicsController.h"
@ -23,21 +24,24 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "GL_ShapeDrawer.h"
#include "LinearMath/GenQuickprof.h"
#include "LinearMath/btQuickprof.h"
#include "BMF_Api.h"
#include "BulletDynamics/Dynamics/btMassProps.h"
int numObjects = 0;
const int maxNumObjects = 16384;
DefaultMotionState ms[maxNumObjects];
CcdPhysicsController* physObjects[maxNumObjects];
SimdTransform startTransforms[maxNumObjects];
CollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use of shapes)
btTransform startTransforms[maxNumObjects];
btCollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use of shapes)
DemoApplication::DemoApplication()
//see IDebugDraw.h for modes
//see btIDebugDraw.h for modes
:
m_physicsEnvironmentPtr(0),
m_dynamicsWorld(0),
m_pickConstraint(0),
m_cameraDistance(15.0),
m_debugMode(0),
m_ele(0.f),
@ -133,21 +137,21 @@ void DemoApplication::updateCamera() {
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;
SimdVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
SimdVector3 right = m_cameraUp.cross(forward);
SimdQuaternion roll(right,-rele);
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
eyePos = SimdMatrix3x3(rot) * SimdMatrix3x3(roll) * eyePos;
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
@ -226,70 +230,70 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
case 'x' : zoomOut(); break;
case 'i' : toggleIdle(); break;
case 'h':
if (m_debugMode & IDebugDraw::DBG_NoHelpText)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_NoHelpText);
if (m_debugMode & btIDebugDraw::DBG_NoHelpText)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoHelpText);
else
m_debugMode |= IDebugDraw::DBG_NoHelpText;
m_debugMode |= btIDebugDraw::DBG_NoHelpText;
break;
case 'w':
if (m_debugMode & IDebugDraw::DBG_DrawWireframe)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawWireframe);
if (m_debugMode & btIDebugDraw::DBG_DrawWireframe)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawWireframe);
else
m_debugMode |= IDebugDraw::DBG_DrawWireframe;
m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
break;
case 'p':
if (m_debugMode & IDebugDraw::DBG_ProfileTimings)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_ProfileTimings);
if (m_debugMode & btIDebugDraw::DBG_ProfileTimings)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_ProfileTimings);
else
m_debugMode |= IDebugDraw::DBG_ProfileTimings;
m_debugMode |= btIDebugDraw::DBG_ProfileTimings;
break;
case 'm':
if (m_debugMode & IDebugDraw::DBG_EnableSatComparison)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_EnableSatComparison);
if (m_debugMode & btIDebugDraw::DBG_EnableSatComparison)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_EnableSatComparison);
else
m_debugMode |= IDebugDraw::DBG_EnableSatComparison;
m_debugMode |= btIDebugDraw::DBG_EnableSatComparison;
break;
case 'n':
if (m_debugMode & IDebugDraw::DBG_DisableBulletLCP)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DisableBulletLCP);
if (m_debugMode & btIDebugDraw::DBG_DisableBulletLCP)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DisableBulletLCP);
else
m_debugMode |= IDebugDraw::DBG_DisableBulletLCP;
m_debugMode |= btIDebugDraw::DBG_DisableBulletLCP;
break;
case 't' :
if (m_debugMode & IDebugDraw::DBG_DrawText)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawText);
if (m_debugMode & btIDebugDraw::DBG_DrawText)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawText);
else
m_debugMode |= IDebugDraw::DBG_DrawText;
m_debugMode |= btIDebugDraw::DBG_DrawText;
break;
case 'y':
if (m_debugMode & IDebugDraw::DBG_DrawFeaturesText)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawFeaturesText);
if (m_debugMode & btIDebugDraw::DBG_DrawFeaturesText)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawFeaturesText);
else
m_debugMode |= IDebugDraw::DBG_DrawFeaturesText;
m_debugMode |= btIDebugDraw::DBG_DrawFeaturesText;
break;
case 'a':
if (m_debugMode & IDebugDraw::DBG_DrawAabb)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawAabb);
if (m_debugMode & btIDebugDraw::DBG_DrawAabb)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawAabb);
else
m_debugMode |= IDebugDraw::DBG_DrawAabb;
m_debugMode |= btIDebugDraw::DBG_DrawAabb;
break;
case 'c' :
if (m_debugMode & IDebugDraw::DBG_DrawContactPoints)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_DrawContactPoints);
if (m_debugMode & btIDebugDraw::DBG_DrawContactPoints)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_DrawContactPoints);
else
m_debugMode |= IDebugDraw::DBG_DrawContactPoints;
m_debugMode |= btIDebugDraw::DBG_DrawContactPoints;
break;
case 'd' :
if (m_debugMode & IDebugDraw::DBG_NoDeactivation)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_NoDeactivation);
if (m_debugMode & btIDebugDraw::DBG_NoDeactivation)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_NoDeactivation);
else
m_debugMode |= IDebugDraw::DBG_NoDeactivation;
m_debugMode |= btIDebugDraw::DBG_NoDeactivation;
break;
@ -306,10 +310,10 @@ void DemoApplication::keyboardCallback(unsigned char key, int x, int y)
break;
case '1':
{
if (m_debugMode & IDebugDraw::DBG_EnableCCD)
m_debugMode = m_debugMode & (~IDebugDraw::DBG_EnableCCD);
if (m_debugMode & btIDebugDraw::DBG_EnableCCD)
m_debugMode = m_debugMode & (~btIDebugDraw::DBG_EnableCCD);
else
m_debugMode |= IDebugDraw::DBG_EnableCCD;
m_debugMode |= btIDebugDraw::DBG_EnableCCD;
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)
{
bool isDynamic = true;
float mass = 1.f;
SimdTransform startTransform;
btTransform startTransform;
startTransform.setIdentity();
SimdVector3 camPos = getCameraPosition();
btVector3 camPos = getCameraPosition();
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);
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*=m_ShootBoxInitialSpeed;
@ -407,12 +435,12 @@ void DemoApplication::shootBox(const SimdVector3& destination)
int gPickingConstraintId = 0;
SimdVector3 gOldPickingPos;
btVector3 gOldPickingPos;
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;
@ -421,16 +449,16 @@ SimdVector3 DemoApplication::GetRayTo(int x,int y)
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
SimdVector3 rayFrom = getCameraPosition();
SimdVector3 rayForward = (getCameraTargetPosition()-getCameraPosition());
btVector3 rayFrom = getCameraPosition();
btVector3 rayForward = (getCameraTargetPosition()-getCameraPosition());
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
SimdVector3 rightOffset;
SimdVector3 vertical = m_cameraUp;
btVector3 rightOffset;
btVector3 vertical = m_cameraUp;
SimdVector3 hor;
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
@ -439,10 +467,10 @@ SimdVector3 DemoApplication::GetRayTo(int x,int y)
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
SimdVector3 rayToCenter = rayFrom + rayForward;
SimdVector3 dHor = hor * 1.f/float(m_glutScreenWidth);
SimdVector3 dVert = vertical * 1.f/float(m_glutScreenHeight);
SimdVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/float(m_glutScreenWidth);
btVector3 dVert = vertical * 1.f/float(m_glutScreenHeight);
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
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);
//button 0, state 0 means left mouse down
SimdVector3 rayTo = GetRayTo(x,y);
btVector3 rayTo = GetRayTo(x,y);
switch (button)
{
@ -468,8 +496,40 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
};
case 1:
{
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
if (m_physicsEnvironmentPtr)
{
@ -479,15 +539,15 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
if (hitObj)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
btRigidBody* body = physCtrl->GetRigidBody();
if (body)
{
body->SetActivationState(ACTIVE_TAG);
SimdVector3 impulse = rayTo;
btVector3 impulse = rayTo;
impulse.normalize();
float impulseStrength = 10.f;
impulse *= impulseStrength;
SimdVector3 relPos(
btVector3 relPos(
hit[0] - body->getCenterOfMassPosition().getX(),
hit[1] - body->getCenterOfMassPosition().getY(),
hit[2] - body->getCenterOfMassPosition().getZ());
@ -509,6 +569,48 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
{
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
if (m_physicsEnvironmentPtr)
{
@ -519,16 +621,16 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
btRigidBody* body = physCtrl->GetRigidBody();
if (body && !body->IsStatic())
{
pickedBody = body;
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,
localPivot.getX(),
@ -541,11 +643,11 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
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();
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
btPoint2PointConstraint* p2p = static_cast<btPoint2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p)
{
//very weak constraint for picking
@ -556,6 +658,18 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
}
} 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)
{
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)
{
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)
{
//move the constraint pivot
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
btPoint2PointConstraint* p2p = static_cast<btPoint2PointConstraint*>(m_physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p)
{
//keep it at the same picking distance
SimdVector3 newRayTo = GetRayTo(x,y);
SimdVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]);
SimdVector3 dir = newRayTo-eyePos;
btVector3 newRayTo = GetRayTo(x,y);
btVector3 eyePos(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2]);
btVector3 dir = newRayTo-eyePos;
dir.normalize();
dir *= gOldPickingDist;
SimdVector3 newPos = eyePos + dir;
btVector3 newPos = eyePos + dir;
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
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;
CcdConstructionInfo ccdObjectCi;
btCcdConstructionInfo ccdObjectCi;
ccdObjectCi.m_friction = 0.5f;
SimdTransform tr;
btTransform tr;
tr.setIdentity();
int i = numObjects;
@ -623,20 +781,20 @@ CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic,
gShapePtr[i] = shape;
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].setWorldPosition(startTransform.getOrigin().getX(),startTransform.getOrigin().getY(),startTransform.getOrigin().getZ());
ccdObjectCi.m_MotionState = &ms[i];
ccdObjectCi.m_gravity = SimdVector3(0,-9.8,0);
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
ccdObjectCi.m_gravity = btVector3(0,-9.8,0);
ccdObjectCi.m_localInertiaTensor =btVector3(0,0,0);
if (!isDynamic)
{
ccdObjectCi.m_mass = 0.f;
ccdObjectCi.m_collisionFlags = CollisionObject::isStatic;
ccdObjectCi.m_collisionFilterGroup = CcdConstructionInfo::StaticFilter;
ccdObjectCi.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::StaticFilter;
ccdObjectCi.m_collisionFlags = btCollisionObject::isStatic;
ccdObjectCi.m_collisionFilterGroup = btCcdConstructionInfo::StaticFilter;
ccdObjectCi.m_collisionFilterMask = btCcdConstructionInfo::AllFilter ^ btCcdConstructionInfo::StaticFilter;
}
else
{
@ -644,7 +802,7 @@ CcdPhysicsController* DemoApplication::LocalCreatePhysicsObject(bool isDynamic,
ccdObjectCi.m_collisionFlags = 0;
}
SimdVector3 localInertia(0.f,0.f,0.f);
btVector3 localInertia(0.f,0.f,0.f);
if (isDynamic)
{
@ -677,10 +835,50 @@ void DemoApplication::renderme()
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 (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP)
if (getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP)
{
//don't use Bullet, use quickstep
m_physicsEnvironmentPtr->setSolverType(0);
@ -690,7 +888,7 @@ void DemoApplication::renderme()
m_physicsEnvironmentPtr->setSolverType(1);
}
if (getDebugMode() & IDebugDraw::DBG_EnableCCD)
if (getDebugMode() & btIDebugDraw::DBG_EnableCCD)
{
m_physicsEnvironmentPtr->setCcdMode(3);
} else
@ -699,7 +897,7 @@ void DemoApplication::renderme()
}
bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison);
bool isSatEnabled = (getDebugMode() & btIDebugDraw::DBG_EnableSatComparison);
m_physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);
@ -711,34 +909,34 @@ void DemoApplication::renderme()
{
CcdPhysicsController* ctrl = m_physicsEnvironmentPtr->GetPhysicsController(i);
RigidBody* body = ctrl->GetRigidBody();
btRigidBody* body = ctrl->GetRigidBody();
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)
{
wireColor = SimdVector3(0.f,0.0f,1.f);
wireColor = btVector3(0.f,0.0f,1.f);
}
///color differently for active, sleeping, wantsdeactivation states
if (ctrl->GetRigidBody()->GetActivationState() == 1) //active
{
if (i & 1)
{
wireColor += SimdVector3 (1.f,0.f,0.f);
wireColor += btVector3 (1.f,0.f,0.f);
} else
{
wireColor += SimdVector3 (.5f,0.f,0.f);
wireColor += btVector3 (.5f,0.f,0.f);
}
}
if (ctrl->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
if (i & 1)
{
wireColor += SimdVector3 (0.f,1.f, 0.f);
wireColor += btVector3 (0.f,1.f, 0.f);
} 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);
float vec[16];
SimdTransform ident;
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(vec);
@ -759,7 +957,7 @@ void DemoApplication::renderme()
}
if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText))
if (!(getDebugMode() & btIDebugDraw::DBG_NoHelpText))
{
float xOffset = 10.f;
@ -774,15 +972,15 @@ void DemoApplication::renderme()
#ifdef USE_QUICKPROF
if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings)
if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
{
static int counter = 0;
counter++;
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];
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);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
@ -790,7 +988,7 @@ void DemoApplication::renderme()
}
}
#endif //USE_QUICKPROF
//profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT);
//profiling << btProfiler::createStatsString(btProfiler::BLOCK_TOTAL_PERCENT);
//<< std::endl;
@ -836,9 +1034,9 @@ void DemoApplication::renderme()
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
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);
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 "LinearMath/SimdVector3.h"
#include "LinearMath/SimdMatrix3x3.h"
#include "LinearMath/SimdTransform.h"
class CcdPhysicsEnvironment;
class CcdPhysicsController;
class CollisionShape;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
class CcdPhysicsEnvironment;
class CcdPhysicsController;
class btCollisionShape;
class btDynamicsWorld;
class btRigidBody;
class btTypedConstraint;
class DemoApplication
{
@ -53,18 +55,21 @@ class DemoApplication
///this is the most important class
CcdPhysicsEnvironment* m_physicsEnvironmentPtr;
btDynamicsWorld* m_dynamicsWorld;
btTypedConstraint* m_pickConstraint;
float m_cameraDistance;
int m_debugMode;
float m_ele;
float m_azi;
SimdVector3 m_cameraPosition;
SimdVector3 m_cameraTargetPosition;//look at
btVector3 m_cameraPosition;
btVector3 m_cameraTargetPosition;//look at
float m_scaleBottom;
float m_scaleFactor;
SimdVector3 m_cameraUp;
btVector3 m_cameraUp;
int m_forwardAxis;
int m_glutScreenWidth;
@ -99,7 +104,7 @@ class DemoApplication
return m_physicsEnvironmentPtr;
}
void setCameraUp(const SimdVector3& camUp)
void setCameraUp(const btVector3& camUp)
{
m_cameraUp = camUp;
}
@ -114,11 +119,11 @@ class DemoApplication
virtual void updateCamera();
SimdVector3 getCameraPosition()
btVector3 getCameraPosition()
{
return m_cameraPosition;
}
SimdVector3 getCameraTargetPosition()
btVector3 getCameraTargetPosition()
{
return m_cameraTargetPosition;
}
@ -135,11 +140,13 @@ class DemoApplication
virtual void clientResetScene() =0 ;
///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

12
Demos/OpenGL/GLDebugDrawer.cpp
View File

@ -1,6 +1,6 @@
#include "GLDebugDrawer.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btPoint3.h"
#ifdef WIN32 //needed for glut.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)
{
@ -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;
const SimdVector3&from = pointOnB;
btVector3 to=pointOnB+normalOnB*distance;
const btVector3&from = pointOnB;
glBegin(GL_LINES);
glColor3f(color.getX(), color.getY(), color.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());

8
Demos/OpenGL/GLDebugDrawer.h
View File

@ -1,11 +1,11 @@
#ifndef 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;
@ -13,9 +13,9 @@ public:
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);

62
Demos/OpenGL/GL_ShapeDrawer.cpp
View File

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

6
Demos/OpenGL/GL_ShapeDrawer.h
View File

@ -15,15 +15,15 @@ subject to the following restrictions:
#ifndef GL_SHAPE_DRAWER_H
#define GL_SHAPE_DRAWER_H
class CollisionShape;
#include "LinearMath/SimdVector3.h"
class btCollisionShape;
#include "LinearMath/btVector3.h"
/// OpenGL shape drawing
class GL_ShapeDrawer
{
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();
};

8
Demos/OpenGL/GL_Simplex1to4.cpp
View File

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

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.
///Can be used by GJK.
class GL_Simplex1to4 : public BU_Simplex1to4
class GL_Simplex1to4 : public btBU_Simplex1to4
{
SimplexSolverInterface* m_simplexSolver;
btSimplexSolverInterface* m_simplexSolver;
public:
@ -31,7 +31,7 @@ class GL_Simplex1to4 : public BU_Simplex1to4
void CalcClosest(float* m);
void SetSimplexSolver(SimplexSolverInterface* simplexSolver) {
void SetSimplexSolver(btSimplexSolverInterface* 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++)
{
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];
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);
bit >>=1;
}

4
Demos/OpenGL/RenderTexture.h
View File

@ -16,7 +16,7 @@ subject to the following restrictions:
#ifndef RENDER_TEXTURE_H
#define RENDER_TEXTURE_H
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "BMF_FontData.h"
///
@ -33,7 +33,7 @@ public:
RenderTexture(int width,int height);
~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];

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

26
Demos/SimplexDemo/SimplexDemo.cpp
View File

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

42
Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp
View File

@ -16,7 +16,7 @@ subject to the following restrictions:
#include "CcdPhysicsEnvironment.h"
#include "CcdPhysicsController.h"
#include "btBulletDynamicsCommon.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
#include "PHY_Pro.h"
#include "UserCollisionAlgorithm.h"
@ -31,17 +31,17 @@ GLDebugDrawer debugDrawer;
static const int NUM_VERTICES = 5;
static const int NUM_TRIANGLES=4;
SimdVector3 gVertices[NUM_VERTICES];
btVector3 gVertices[NUM_VERTICES];
int gIndices[NUM_TRIANGLES*3];
const float TRIANGLE_SIZE=80.f;
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= CollisionObject::customMaterialCallback;
inline SimdScalar calculateCombinedFriction(float friction0,float friction1)
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
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)
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;
}
@ -74,7 +74,7 @@ void UserCollisionAlgorithm::initPhysics()
{
#define TRISIZE 10.f
int vertStride = sizeof(SimdVector3);
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
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);
SimdVector3* gVertices = new SimdVector3[totalVerts];
btVector3* gVertices = new btVector3[totalVerts];
int* gIndices = new int[totalTriangles*3];
int i;
@ -111,41 +111,41 @@ void UserCollisionAlgorithm::initPhysics()
}
}
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
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);
OverlappingPairCache* broadphase = new AxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase();
dispatcher->RegisterCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new SphereSphereCollisionAlgorithm::CreateFunc);
btVector3 maxAabb(10000,10000,10000);
btOverlappingPairCache* broadphase = new btAxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase();
dispatcher->RegisterCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
bool isDynamic = false;
float mass = 0.f;
SimdTransform startTransform;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,-2,0));
startTransform.setOrigin(btVector3(0,-2,0));
CcdPhysicsController* staticTrimesh = LocalCreatePhysicsObject(isDynamic, mass, startTransform,trimeshShape);
//enable custom material callback
staticTrimesh->GetRigidBody()->m_collisionFlags |= CollisionObject::customMaterialCallback;
staticTrimesh->GetRigidBody()->m_collisionFlags |= btCollisionObject::customMaterialCallback;
{
for (int i=0;i<10;i++)
{
CollisionShape* sphereShape = new SphereShape(1);
startTransform.setOrigin(SimdVector3(1,2*i,1));
btCollisionShape* sphereShape = new btSphereShape(1);
startTransform.setOrigin(btVector3(1,2*i,1));
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 "LinearMath/GenQuickprof.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"
@ -51,7 +51,7 @@ const int maxOverlap = 65535;
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
///implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
PHY_IVehicle* gVehicleConstraint=0;
@ -67,9 +67,9 @@ float suspensionStiffness = 10.f;
float suspensionDamping = 1.3f;
float suspensionCompression = 2.4f;
float rollInfluence = 0.1f;
SimdVector3 wheelDirectionCS0(0,-1,0);
SimdVector3 wheelAxleCS(1,0,0);
SimdScalar suspensionRestLength(0.6);
btVector3 wheelDirectionCS0(0,-1,0);
btVector3 wheelAxleCS(1,0,0);
btScalar suspensionRestLength(0.6);
#define CUBE_HALF_EXTENTS 1
@ -98,20 +98,20 @@ m_cameraHeight(4.f),
m_minCameraDistance(3.f),
m_maxCameraDistance(10.f)
{
m_cameraPosition = SimdVector3(30,30,30);
m_cameraPosition = btVector3(30,30,30);
}
void VehicleDemo::setupPhysics()
{
CollisionDispatcher* dispatcher = new CollisionDispatcher();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
ParallelIslandDispatcher* dispatcher2 = new ParallelIslandDispatcher();
SimdVector3 worldAabbMin(-30000,-30000,-30000);
SimdVector3 worldAabbMax(30000,30000,30000);
btVector3 worldAabbMin(-30000,-30000,-30000);
btVector3 worldAabbMax(30000,30000,30000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
//OverlappingPairCache* broadphase = new SimpleBroadphase(maxProxies,maxOverlap);
btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
//OverlappingPairCache* broadphase = new btSimpleBroadphase(maxProxies,maxOverlap);
#ifdef USE_PARALLEL_DISPATCHER
m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase);
@ -125,7 +125,7 @@ void VehicleDemo::setupPhysics()
m_physicsEnvironmentPtr->setGravity(0,-10,0);//0,0);//-10,0);
int i;
CollisionShape* groundShape = new BoxShape(SimdVector3(50,3,50));
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
#define USE_TRIMESH_GROUND 1
#ifdef USE_TRIMESH_GROUND
@ -134,7 +134,7 @@ void VehicleDemo::setupPhysics()
const float TRIANGLE_SIZE=20.f;
//create a triangle-mesh ground
int vertStride = sizeof(SimdVector3);
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
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);
SimdVector3* gVertices = new SimdVector3[totalVerts];
btVector3* gVertices = new btVector3[totalVerts];
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,
indexStride,
totalVerts,(float*) &gVertices[0].x(),vertStride);
groundShape = new BvhTriangleMeshShape(indexVertexArrays);
groundShape = new btBvhTriangleMeshShape(indexVertexArrays);
#endif //
SimdTransform tr;
btTransform tr;
tr.setIdentity();
tr.setOrigin(SimdVector3(0,-20.f,0));
tr.setOrigin(btVector3(0,-20.f,0));
//create ground object
LocalCreatePhysicsObject(false,0,tr,groundShape);
CollisionShape* chassisShape = new BoxShape(SimdVector3(1.f,0.5f,2.f));
tr.setOrigin(SimdVector3(0,0.f,0));
btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,0.5f,2.f));
tr.setOrigin(btVector3(0,0.f,0));
m_carChassis = LocalCreatePhysicsObject(true,800,tr,chassisShape);
@ -212,8 +212,8 @@ const float TRIANGLE_SIZE=20.f;
gVehicleConstraint = m_physicsEnvironmentPtr->getVehicleConstraint(constraintId);
SimdVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
RaycastVehicle::VehicleTuning tuning;
btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),0,2*CUBE_HALF_EXTENTS-wheelRadius);
btRaycastVehicle::btVehicleTuning tuning;
bool isFrontWheel=true;
int rightIndex = 0;
int upIndex = 1;
@ -225,18 +225,18 @@ const float TRIANGLE_SIZE=20.f;
(PHY__Vector3&)connectionPointCS0,
(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],
(PHY__Vector3&)connectionPointCS0,
(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;
gVehicleConstraint->AddWheel(&wheelMotionState[2],
(PHY__Vector3&)connectionPointCS0,
(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],
(PHY__Vector3&)connectionPointCS0,
(PHY__Vector3&)wheelDirectionCS0,(PHY__Vector3&)wheelAxleCS,suspensionRestLength,wheelRadius,isFrontWheel);
@ -281,8 +281,8 @@ void VehicleDemo::renderme()
float m[16];
int i;
CylinderShapeX wheelShape(SimdVector3(wheelWidth,wheelRadius,wheelRadius));
SimdVector3 wheelColor(1,0,0);
btCylinderShapeX wheelShape(btVector3(wheelWidth,wheelRadius,wheelRadius));
btVector3 wheelColor(1,0,0);
for (i=0;i<4;i++)
{
@ -319,14 +319,14 @@ void VehicleDemo::clientMoveAndDisplay()
#ifdef USE_QUICKPROF
Profiler::beginBlock("render");
btProfiler::beginBlock("render");
#endif //USE_QUICKPROF
renderme();
#ifdef USE_QUICKPROF
Profiler::endBlock("render");
btProfiler::endBlock("render");
#endif
glFlush();
glutSwapBuffers();
@ -420,7 +420,7 @@ void VehicleDemo::updateCamera()
//interpolate the camera height
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
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 <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;
SimdScalar basic_h_local_delta;
btScalar basic_h_local;
btScalar basic_h_local_delta;
basic_h_local = p * 0.5f;
basic_h_local_delta = basic_h_local * basic_h_local - q;
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[1] = - basic_h_local - basic_h_local_delta;
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;
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)
{
SimdScalar sqrtRadical = SimdSqrt(radical);
SimdScalar idenom = 1.0f/(2.0f * a);
btScalar sqrtRadical = btSqrt(radical);
btScalar idenom = 1.0f/(2.0f * a);
m_roots[0]=(-b + sqrtRadical) * idenom;
m_roots[1]=(-b - sqrtRadical) * idenom;
return 2;
@ -58,8 +58,8 @@ int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b,
#define cubic_rt(x) \
((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \
((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f))
((x) > 0.0f ? btPow((btScalar)(x), 0.333333333333333333333333f) : \
((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 */
/* 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;
SimdScalar delta, u, phi;
SimdScalar dummy;
btScalar p, q, r;
btScalar delta, u, phi;
btScalar dummy;
if (lead != 1.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 */
/* */
if (SimdEqual(a, SIMD_EPSILON)) {
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x + c = 0 */
/* */
if (SimdEqual(b, SIMD_EPSILON)) {
if (SimdEqual(c, SIMD_EPSILON)) {
if (btEqual(b, SIMD_EPSILON)) {
if (btEqual(c, SIMD_EPSILON)) {
return -1;
}
else {
@ -128,8 +128,8 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */
/* now use Cardano's formula */
/* */
if (SimdEqual(p, SIMD_EPSILON)) {
if (SimdEqual(q, SIMD_EPSILON)) {
if (btEqual(p, SIMD_EPSILON)) {
if (btEqual(q, SIMD_EPSILON)) {
/* */
/* 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. */
/* */
u = -q + SimdSqrt(delta);
u = -q + btSqrt(delta);
u = cubic_rt(u);
m_roots[0] = u - p / u - a;
return 1;
@ -160,19 +160,19 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */
/* Casus irreducibilis: we have three real roots */
/* */
r = SimdSqrt(-p);
r = btSqrt(-p);
p *= -r;
r *= 2.0;
phi = SimdAcos(-q / p) / 3.0f;
phi = btAcos(-q / p) / 3.0f;
dummy = SIMD_2_PI / 3.0f;
m_roots[0] = r * SimdCos(phi) - a;
m_roots[1] = r * SimdCos(phi + dummy) - a;
m_roots[2] = r * SimdCos(phi - dummy) - a;
m_roots[0] = r * btCos(phi) - a;
m_roots[1] = r * btCos(phi + dummy) - a;
m_roots[2] = r * btCos(phi - dummy) - a;
return 3;
}
else {
/* */
/* one single and one SimdScalar root */
/* one single and one btScalar root */
/* */
r = cubic_rt(-q);
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 */
/* 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;
SimdScalar u, v, w;
btScalar p, q ,r;
btScalar u, v, w;
int i, num_roots, num_tmp;
//SimdScalar tmp[2];
//btScalar tmp[2];
if (lead != 1.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 */
/* */
if (SimdEqual(a, SIMD_EPSILON)) {
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* 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 */
/* */
if (SimdEqual(c, SIMD_EPSILON)) {
if (SimdEqual(d, SIMD_EPSILON)) {
if (btEqual(c, SIMD_EPSILON)) {
if (btEqual(d, SIMD_EPSILON)) {
return -1;
}
else {
@ -254,7 +254,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
p = b - 6.0f * a * a;
q = a * (8.0f * a * a - 2.0f * b) + c;
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. */
/* */
@ -263,23 +263,23 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
if (m_roots[0] > 0.0f) {
if (num_roots > 1) {
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[3] = -u - a;
u = SimdSqrt(m_roots[0]);
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 4;
}
else {
u = SimdSqrt(m_roots[0]);
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
else {
u = SimdSqrt(m_roots[0]);
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
@ -288,7 +288,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
}
return 0;
}
else if (SimdEqual(r, SIMD_EPSILON)) {
else if (btEqual(r, SIMD_EPSILON)) {
/* */
/* 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;
v = 2.0f * w - p;
if (SimdEqual(u, SIMD_EPSILON))
if (btEqual(u, SIMD_EPSILON))
u = 0.0;
else if (u > 0.0f)
u = SimdSqrt(u);
u = btSqrt(u);
else
return 0;
if (SimdEqual(v, SIMD_EPSILON))
if (btEqual(v, SIMD_EPSILON))
v = 0.0;
else if (v > 0.0f)
v = SimdSqrt(v);
v = btSqrt(v);
else
return 0;
@ -343,7 +343,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
m_roots[i] -= a;
}
w += 2.0f *u;
SimdScalar tmp[2];
btScalar tmp[2];
tmp[0] = m_roots[0];
tmp[1] = m_roots[1];

12
Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.h
View File

@ -26,19 +26,19 @@ class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface
public:
BU_AlgebraicPolynomialSolver() {};
int Solve2Quadratic(SimdScalar p, SimdScalar q);
int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c);
int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c);
int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d);
int Solve2Quadratic(btScalar p, btScalar q);
int Solve2QuadraticFull(btScalar a,btScalar b, btScalar c);
int Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c);
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];
}
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 "BulletCollision/CollisionShapes/btCollisionShape.h"
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btTransform.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)
{
}

10
Extras/AlgebraicCCD/BU_Collidable.h
View File

@ -18,14 +18,14 @@ subject to the following restrictions:
#define BU_COLLIDABLE
class PolyhedralConvexShape;
class btPolyhedralConvexShape;
class BU_MotionStateInterface;
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/btPoint3.h>
class BU_Collidable
{
public:
BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape, void* userPointer);
BU_Collidable(BU_MotionStateInterface& motion,btPolyhedralConvexShape& shape, void* userPointer);
void* GetUserPointer() const
{
@ -41,7 +41,7 @@ public:
return m_motionState;
}
inline const PolyhedralConvexShape& GetShape() const
inline const btPolyhedralConvexShape& GetShape() const
{
return m_shape;
};
@ -49,7 +49,7 @@ public:
private:
BU_MotionStateInterface& m_motionState;
PolyhedralConvexShape& m_shape;
btPolyhedralConvexShape& m_shape;
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 "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
#include <SimdMinMax.h>
#include "LinearMath/SimdTransformUtil.h"
#include <btSimdMinMax.h>
#include "LinearMath/btTransformUtil.h"
BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
BU_CollisionPair::BU_CollisionPair(const btPolyhedralConvexShape* convexA,const btPolyhedralConvexShape* convexB,btScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(btVector3(0,0,0),btVector3(0,0,0)),
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(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
SimdVector3 linvelA,angvelA;
SimdVector3 linvelB,angvelB;
btVector3 linvelA,angvelA;
btVector3 linvelB,angvelB;
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
btTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
btTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
SimdQuaternion angularMotionA(0,0,0,1.f);
SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
SimdQuaternion angularMotionB(0,0,0,1);
btVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
btQuaternion angularMotionA(0,0,0,1.f);
btVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
btQuaternion angularMotionB(0,0,0,1);
result.m_fraction = 1.f;
SimdTransform impactTransA;
SimdTransform impactTransB;
btTransform impactTransA;
btTransform impactTransB;
int index=0;
SimdScalar toiUnscaled=result.m_fraction;
const SimdScalar toiUnscaledLimit = result.m_fraction;
btScalar toiUnscaled=result.m_fraction;
const btScalar toiUnscaledLimit = result.m_fraction;
SimdTransform a2w;
btTransform a2w;
a2w = fromA;
SimdTransform b2w = fromB;
btTransform b2w = fromB;
/* debugging code
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
btPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = b2w * pt;
char buf[1000];
@ -110,52 +110,52 @@ bool BU_CollisionPair::calcTimeOfImpact(
*/
SimdTransform b2wp = b2w;
btTransform b2wp = b2w;
b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
b2wp.setRotation( b2w.getRotation() + angularMotionB);
impactTransB = b2wp;
SimdTransform a2wp;
btTransform a2wp;
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
a2wp.setRotation(a2w.getRotation()+angularMotionA);
impactTransA = a2wp;
SimdTransform a2winv;
btTransform a2winv;
a2winv = a2w.inverse();
SimdTransform b2wpinv;
btTransform b2wpinv;
b2wpinv = b2wp.inverse();
SimdTransform b2winv;
btTransform b2winv;
b2winv = b2w.inverse();
SimdTransform a2wpinv;
btTransform a2wpinv;
a2wpinv = a2wp.inverse();
//Redon's version with concatenated transforms
SimdTransform relative;
btTransform relative;
relative = b2w * b2wpinv * a2wp * a2winv;
//relative = a2winv * a2wp * b2wpinv * b2w;
SimdQuaternion qrel;
btQuaternion qrel;
relative.getBasis().getRotation(qrel);
SimdVector3 linvel = relative.getOrigin();
btVector3 linvel = relative.getOrigin();
if (linvel.length() < SCREWEPSILON)
{
linvel.setValue(0.,0.,0.);
}
SimdVector3 angvel;
angvel[0] = 2.f * SimdAsin (qrel[0]);
angvel[1] = 2.f * SimdAsin (qrel[1]);
angvel[2] = 2.f * SimdAsin (qrel[2]);
btVector3 angvel;
angvel[0] = 2.f * btAsin (qrel[0]);
angvel[1] = 2.f * btAsin (qrel[1]);
angvel[2] = 2.f * btAsin (qrel[2]);
if (angvel.length() < SCREWEPSILON)
{
@ -165,10 +165,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//Redon's version with concatenated transforms
m_screwing = BU_Screwing(linvel,angvel);
SimdTransform w2s;
btTransform w2s;
m_screwing.LocalMatrix(w2s);
SimdTransform s2w;
btTransform s2w;
s2w = w2s.inverse();
//impactTransA = a2w;
@ -176,7 +176,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
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
//toi = 0;
@ -185,7 +185,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
btPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = impactTransB * pt;
char buf[1000];
@ -217,7 +217,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//for all edged in A check agains all edges in B
for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
{
SimdPoint3 pA0,pA1;
btPoint3 pA0,pA1;
m_convexA->GetEdge(ea,pA0,pA1);
@ -231,7 +231,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
for (int eb = 0; eb < numedgesB;eb++)
{
{
SimdPoint3 pB0,pB1;
btPoint3 pB0,pB1;
m_convexB->GetEdge(eb,pB0,pB1);
pB0= b2w * pB0;//in world space
@ -241,12 +241,12 @@ bool BU_CollisionPair::calcTimeOfImpact(
pB1 = w2s * pB1;//in screwing space
SimdScalar lambda,mu;
btScalar lambda,mu;
toiUnscaled = 1.;
SimdVector3 edgeDirA(pA1-pA0);
SimdVector3 edgeDirB(pB1-pB0);
btVector3 edgeDirA(pA1-pA0);
btVector3 edgeDirB(pB1-pB0);
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 !
SimdPoint3 vtx = pA0+lambda * (pA1-pA0);
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
btPoint3 vtx = pA0+lambda * (pA1-pA0);
btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdPoint3 hitptWorld = s2w * hitpt;
btPoint3 hitptWorld = s2w * hitpt;
{
if (toiUnscaled < result.m_fraction)
@ -269,7 +269,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
hit = true;
SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
btVector3 hitNormal = edgeDirB.cross(edgeDirA);
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
//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)
hitNormalWorld *= -1;
@ -312,7 +312,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//int v=3;
{
SimdPoint3 vtx;
btPoint3 vtx;
m_convexA->GetVertex(v,vtx);
vtx = a2w * vtx;//in world space
@ -326,23 +326,23 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
btVector3 planeNorm;
btPoint3 planeSupport;
m_convexB->GetPlane(planeNorm,planeSupport,p);
planeSupport = b2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm;
btVector3 planeNormWorld = b2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
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;
@ -368,11 +368,11 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
// printf("toiUnscaled %f\n",toiUnscaled );
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
btVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt;
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btPoint3 hitptWorld = s2w * hitpt;
hitpt = b2winv * hitptWorld;
@ -408,7 +408,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//int v=0;
{
SimdPoint3 vtx;
btPoint3 vtx;
m_convexB->GetVertex(v,vtx);
vtx = b2w * vtx;//in world space
@ -436,23 +436,23 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
btVector3 planeNorm;
btPoint3 planeSupport;
m_convexA->GetPlane(planeNorm,planeSupport,p);
planeSupport = a2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm;
btVector3 planeNormWorld = a2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
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;
@ -464,15 +464,15 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
if (toiUnscaled < toiUnscaledLimit)
{
SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//SimdScalar len = hitNormal.length()-1;
btPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
btVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//btScalar len = hitNormal.length()-1;
//assert( SimdFuzzyZero(len) );
//assert( btFuzzyZero(len) );
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt;
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btPoint3 hitptWorld = s2w * hitpt;
hitpt = a2winv * hitptWorld;
@ -522,19 +522,19 @@ bool BU_CollisionPair::calcTimeOfImpact(
} else
{
//SimdScalar vel = linearMotionB.length();
//btScalar vel = linearMotionB.length();
//todo: check this margin
result.m_fraction *= 0.99f;
//move B to new position
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();
impactTransB.setRotation(ornB);
//now transform A
SimdTransform a2s,a2b;
btTransform a2s,a2b;
a2s.mult( w2s , a2w);
a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
a2s.multInverseLeft(w2s,a2s);
@ -543,10 +543,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//transform by motion B
impactTransA.mult(impactTransB, a2b);
//normalize rotation
SimdQuaternion orn;
btQuaternion orn;
impactTransA.getBasis().getRotation(orn);
orn.normalize();
impactTransA.setBasis(SimdMatrix3x3(orn));
impactTransA.setBasis(btMatrix3x3(orn));
}
}
@ -555,7 +555,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
btPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = impactTransB * pt;
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 <SimdQuaternion.h>
#include <btQuaternion.h>
class PolyhedralConvexShape;
class btPolyhedralConvexShape;
///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:
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
virtual bool calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);
private:
const PolyhedralConvexShape* m_convexA;
const PolyhedralConvexShape* m_convexB;
const btPolyhedralConvexShape* m_convexA;
const btPolyhedralConvexShape* m_convexB;
BU_Screwing m_screwing;
SimdScalar m_tolerance;
btScalar m_tolerance;
};
#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_Screwing.h"
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btPoint3.h>
//#include "BU_IntervalArithmeticPolynomialSolver.h"
#include "BU_AlgebraicPolynomialSolver.h"
@ -36,37 +36,37 @@ BU_EdgeEdge::BU_EdgeEdge()
bool BU_EdgeEdge::GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lambda1,
SimdScalar& mu1
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lambda1,
btScalar& mu1
)
{
bool hit=false;
SimdScalar lambda;
SimdScalar mu;
btScalar lambda;
btScalar mu;
const SimdScalar w=screwAB.GetW();
const SimdScalar s=screwAB.GetS();
const btScalar w=screwAB.GetW();
const btScalar s=screwAB.GetS();
if (SimdFuzzyZero(s) &&
SimdFuzzyZero(w))
if (btFuzzyZero(s) &&
btFuzzyZero(w))
{
//no motion, no collision
return false;
}
if (SimdFuzzyZero(w) )
if (btFuzzyZero(w) )
{
//pure translation W=0, S <> 0
//no trig, f(t)=t
SimdScalar det = u.y()*v.x()-u.x()*v.y();
if (!SimdFuzzyZero(det))
btScalar det = u.y()*v.x()-u.x()*v.y();
if (!btFuzzyZero(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;
@ -74,7 +74,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
{
// 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 (t>=0 && t <= minTime)
{
@ -91,14 +91,14 @@ bool BU_EdgeEdge::GetTimeOfImpact(
}
} 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
if (SimdFuzzyZero(a.z()-c.z()))
if (btFuzzyZero(a.z()-c.z()))
{
//printf("NOT YET planar problem, 4 vertex=edge cases\n");
@ -110,7 +110,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else
{
SimdScalar mu = (a.z() - c.z())/v.z();
btScalar mu = (a.z() - c.z())/v.z();
if (0<=mu && mu <= 1)
{
// printf("NOT YET//u.z()=0,v.z()<>0\n");
@ -124,22 +124,22 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{
//u.z()<>0
if (SimdFuzzyZero(v.z()) )
if (btFuzzyZero(v.z()) )
{
//printf("u.z()<>0,v.z()=0\n");
lambda = (c.z() - a.z())/u.z();
if (0<=lambda && lambda <= 1)
{
//printf("u.z()<>0,v.z()=0\n");
SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
SimdScalar r2 = rotPt.length2();//px*px + py*py;
btPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
btScalar r2 = rotPt.length2();//px*px + py*py;
//either y=a*x+b, or x = a*x+b...
//depends on whether value v.x() is zero or not
SimdScalar aa;
SimdScalar bb;
btScalar aa;
btScalar bb;
if (SimdFuzzyZero(v.x()))
if (btFuzzyZero(v.x()))
{
aa = v.x()/v.y();
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();
}
SimdScalar disc = aa*aa*r2 + r2 - bb*bb;
btScalar disc = aa*aa*r2 + r2 - bb*bb;
if (disc <0)
{
//edge doesn't intersect the circle (motion of the vertex)
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;
if (SimdFuzzyZero(v.x()))
if (btFuzzyZero(v.x()))
{
intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
intersectPt.setX( aa*intersectPt.y()+bb );
@ -191,20 +191,20 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{
//two points...
//intersectionPoint edge with circle;
SimdPoint3 intersectPt;
btPoint3 intersectPt;
//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);
mu = ((intersectPt.getY()-c.getY())/v.getY());
if (0.f <= mu && mu <= 1.f)
{
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);
mu = ((intersectPt.getY()-c.getY())/v.getY());
if (0 <= mu && mu <= 1)
@ -214,16 +214,16 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} 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);
mu = ((intersectPt.getX()-c.getX())/v.getX());
if (0 <= mu && mu <= 1)
{
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);
mu = ((intersectPt.getX()-c.getX())/v.getX());
if (0.f <= mu && mu <= 1.f)
@ -247,7 +247,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{
//u.z()<>0,v.z()<>0
//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)
{
lambda1 = lambda;
@ -260,7 +260,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else
{
//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)
{
lambda1 = lambda;
@ -277,60 +277,60 @@ bool BU_EdgeEdge::GetTimeOfImpact(
}
bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
bool BU_EdgeEdge::GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lambda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lambda,
btScalar& mu
)
{
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;
int numroots = 0;
//SimdScalar eps=1e-15f;
//SimdScalar eps2=1e-20f;
SimdScalar s=screwAB.GetS();
SimdScalar w = screwAB.GetW();
//btScalar eps=1e-15f;
//btScalar eps2=1e-20f;
btScalar s=screwAB.GetS();
btScalar w = screwAB.GetW();
SimdScalar ax = a.x();
SimdScalar ay = a.y();
SimdScalar az = a.z();
SimdScalar cx = c.x();
SimdScalar cy = c.y();
SimdScalar cz = c.z();
SimdScalar vx = v.x();
SimdScalar vy = v.y();
SimdScalar vz = v.z();
SimdScalar ux = u.x();
SimdScalar uy = u.y();
SimdScalar uz = u.z();
btScalar ax = a.x();
btScalar ay = a.y();
btScalar az = a.z();
btScalar cx = c.x();
btScalar cy = c.y();
btScalar cz = c.z();
btScalar vx = v.x();
btScalar vy = v.y();
btScalar vz = v.z();
btScalar ux = u.x();
btScalar uy = u.y();
btScalar uz = u.z();
if (!SimdFuzzyZero(v.z()))
if (!btFuzzyZero(v.z()))
{
//Maple Autogenerated C code
SimdScalar t1,t2,t3,t4,t7,t8,t10;
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
SimdScalar t31,t32,t33,t34,t35,t36,t39,t40;
SimdScalar t41,t43,t48;
SimdScalar t63;
btScalar t1,t2,t3,t4,t7,t8,t10;
btScalar t13,t14,t15,t16,t17,t18,t19,t20;
btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
btScalar t31,t32,t33,t34,t35,t36,t39,t40;
btScalar t41,t43,t48;
btScalar t63;
SimdScalar aa,bb,cc,dd;//the coefficients
btScalar aa,bb,cc,dd;//the coefficients
t1 = v.y()*s; t2 = t1*u.x();
t3 = v.x()*s;
t4 = t3*u.y();
t7 = SimdTan(w/2.0f);
t7 = btTan(w/2.0f);
t8 = 1.0f/t7;
t10 = 1.0f/v.z();
aa = (t2-t4)*t8*t10;
@ -377,17 +377,17 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
} else
{
SimdScalar t1,t2,t3,t4,t7,t8,t10;
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
SimdScalar p1,p2,p3,p4;
btScalar t1,t2,t3,t4,t7,t8,t10;
btScalar t13,t14,t15,t16,t17,t18,t19,t20;
btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
btScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
btScalar p1,p2,p3,p4;
t1 = uy*s;
t2 = t1*vx;
t3 = ux*s;
t4 = t3*vy;
t7 = SimdTan(w/2.0f);
t7 = btTan(w/2.0f);
t8 = 1/t7;
t10 = 1/uz;
t13 = ux*az;
@ -435,38 +435,38 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
for (int i=0;i<numroots;i++)
{
//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i);
SimdScalar tau = polynomialSolver.GetRoot(i);
//btScalar tau = roots[i];//polynomialSolver.GetRoot(i);
btScalar tau = polynomialSolver.GetRoot(i);
//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);
SimdScalar B1=((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+
vx*az*SimdTan(1.f/2.f*w)-vx*cz*SimdTan(1.f/2.f*w)+
vx*s*tau)/SimdTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
btScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
btScalar B1=((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+
vx*az*btTan(1.f/2.f*w)-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);
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
SimdScalar lhs = (a.x()+lambda*u.x())
btScalar lhs = (a.x()+lambda*u.x())
*((1.f-tau*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));
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)-
vx*cz*SimdTan(1.f/2.f*w)+vx*s*tau)/(SimdTan(1.f/2.f*w)*vz))-
rhs = ((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+vx*az*btTan(1.f/2.f*w)-
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);
/*SimdScalar res = coefs[0]*tau*tau*tau+
/*btScalar res = coefs[0]*tau*tau*tau+
coefs[1]*tau*tau+
coefs[2]*tau+
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
if (t>=0.f && t<minTime)
{
@ -517,24 +517,24 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
//C -S
//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;
// now calculate the planeEquation for the vertex motion,
// and check if the intersectionpoint is at the positive side
SimdPoint3 rotPt1(SimdCos(rotW)*rotPt.x()-SimdSin(rotW)*rotPt.y(),
SimdSin(rotW)*rotPt.x()+SimdCos(rotW)*rotPt.y(),
btPoint3 rotPt1(btCos(rotW)*rotPt.x()-btSin(rotW)*rotPt.y(),
btSin(rotW)*rotPt.x()+btCos(rotW)*rotPt.y(),
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
SimdScalar planeD = planeNormal.dot(rotPt1);
//btPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
btScalar planeD = planeNormal.dot(rotPt1);
SimdScalar dist = (planeNormal.dot(intersectPt)-planeD);
btScalar dist = (planeNormal.dot(intersectPt)-planeD);
hit = (dist >= -0.001);
//if (hit)
@ -545,10 +545,10 @@ bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar r
// cos (alpha) = adjacent/hypothenuse;
//adjacent = dotproduct(ipedge,point);
//hypothenuse = sqrt(r2);
SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
SimdScalar hypo = rotRadius;
SimdScalar alpha = SimdAcos(adjacent/hypo);
SimdScalar t = alpha / rotW;
btScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
btScalar hypo = rotRadius;
btScalar alpha = btAcos(adjacent/hypo);
btScalar t = alpha / rotW;
if (t >= 0 && t < minTime)
{
hit = true;
@ -564,13 +564,13 @@ bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar r
bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
)
{

54
Extras/AlgebraicCCD/BU_EdgeEdge.h
View File

@ -18,13 +18,13 @@ subject to the following restrictions:
#define BU_EDGEEDGE
class BU_Screwing;
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdVector3.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btVector3.h>
//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.
class BU_EdgeEdge
@ -35,39 +35,39 @@ public:
BU_EdgeEdge();
bool GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
);
private:
bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime);
bool GetTimeOfImpactGeneralCase(
bool Calc2DRotationPointPoint(const btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime);
bool GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
);
bool GetTimeOfImpactVertexEdge(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
);

30
Extras/AlgebraicCCD/BU_MotionStateInterface.h
View File

@ -18,32 +18,32 @@ subject to the following restrictions:
#define BU_MOTIONSTATE
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <SimdQuaternion.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <btQuaternion.h>
class BU_MotionStateInterface
{
public:
virtual ~BU_MotionStateInterface(){};
virtual void SetTransform(const SimdTransform& trans) = 0;
virtual void GetTransform(SimdTransform& trans) const = 0;
virtual void SetTransform(const btTransform& trans) = 0;
virtual void GetTransform(btTransform& trans) const = 0;
virtual void SetPosition(const SimdPoint3& position) = 0;
virtual void GetPosition(SimdPoint3& position) const = 0;
virtual void SetPosition(const btPoint3& position) = 0;
virtual void GetPosition(btPoint3& position) const = 0;
virtual void SetOrientation(const SimdQuaternion& orientation) = 0;
virtual void GetOrientation(SimdQuaternion& orientation) const = 0;
virtual void SetOrientation(const btQuaternion& orientation) = 0;
virtual void GetOrientation(btQuaternion& orientation) const = 0;
virtual void SetBasis(const SimdMatrix3x3& basis) = 0;
virtual void GetBasis(SimdMatrix3x3& basis) const = 0;
virtual void SetBasis(const btMatrix3x3& basis) = 0;
virtual void GetBasis(btMatrix3x3& basis) const = 0;
virtual void SetLinearVelocity(const SimdVector3& linvel) = 0;
virtual void GetLinearVelocity(SimdVector3& linvel) const = 0;
virtual void SetLinearVelocity(const btVector3& linvel) = 0;
virtual void GetLinearVelocity(btVector3& linvel) const = 0;
virtual void GetAngularVelocity(SimdVector3& angvel) const = 0;
virtual void SetAngularVelocity(const SimdVector3& angvel) = 0;
virtual void GetAngularVelocity(btVector3& angvel) const = 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
#define BUM_POLYNOMIAL_SOLVER_INTERFACE
#include <LinearMath/SimdScalar.h>
#include <LinearMath/btScalar.h>
//
//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.
@ -26,13 +26,13 @@ public:
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"
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 SimdScalar dy=relLinVel[1];
const SimdScalar dz=relLinVel[2];
const SimdScalar wx=relAngVel[0];
const SimdScalar wy=relAngVel[1];
const SimdScalar wz=relAngVel[2];
const btScalar dx=relLinVel[0];
const btScalar dy=relLinVel[1];
const btScalar dz=relLinVel[2];
const btScalar wx=relAngVel[0];
const btScalar wy=relAngVel[1];
const btScalar wz=relAngVel[2];
// Compute the screwing parameters :
// 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)
// 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 (fabs(m_w)<SCREWEPSILON ) {
assert(m_w == 0.f);
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 ) {
assert(m_s == 0.);
m_s=0.;
m_u=SimdPoint3(0.,0.,1.);
m_o=SimdPoint3(0.,0.,0.);
m_u=btPoint3(0.,0.,1.);
m_o=btPoint3(0.,0.,0.);
}
else {
float t=1.f/m_s;
m_u=SimdPoint3(dx*t,dy*t,dz*t);
m_o=SimdPoint3(0.f,0.f,0.f);
m_u=btPoint3(dx*t,dy*t,dz*t);
m_o=btPoint3(0.f,0.f,0.f);
}
}
else { // there is some rotation
@ -60,35 +60,35 @@ BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngV
// we compute u
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
SimdPoint3 t(dx,dy,dz);
btPoint3 t(dx,dy,dz);
m_s=t.dot(m_u); // component along u
if (fabs(m_s)<SCREWEPSILON)
{
//printf("m_s component along u < SCREWEPSILION\n");
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
//SimdScalar len = n1.length2();
//btScalar len = n1.length2();
//(len >= BUM_EPSILON2) {
if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector
n1.normalize();
SimdVector3 n1orth=m_u.cross(n1);
btVector3 n1orth=m_u.cross(n1);
float n2x=SimdCos(0.5f*m_w);
float n2y=SimdSin(0.5f*m_w);
float n2x=btCos(0.5f*m_w);
float n2y=btSin(0.5f*m_w);
m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth);
}
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 :
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
// screwing axis (thanks to u), and then find two others orthogonal axes to
// 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))
{
// 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;
SimdMatrix3x3 mat;
btMatrix3x3 mat;
mat[0][0]=-m_u[1]*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][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]*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])));
@ -133,24 +133,24 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
}
else {
SimdMatrix3x3 m;
btMatrix3x3 m;
m[0][0]=1.;
m[1][0]=0.;
m[2][0]=0.;
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[0][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],
-SimdSign(m_u[2])*m_o[1],
-SimdSign(m_u[2])*m_o[2]
-btSign(m_u[2])*m_o[1],
-btSign(m_u[2])*m_o[2]
));
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
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 (
org.x()*SimdCos(m_w*t)-org.y()*SimdSin(m_w*t),
org.x()*SimdSin(m_w*t)+org.y()*SimdCos(m_w*t),
btPoint3 neworg (
org.x()*btCos(m_w*t)-org.y()*btSin(m_w*t),
org.x()*btSin(m_w*t)+org.y()*btCos(m_w*t),
org.z()+m_s*CalculateF(t));
SimdTransform newtr;
btTransform newtr;
newtr.setOrigin(neworg);
SimdMatrix3x3 basis = tr.getBasis();
SimdMatrix3x3 basisorg = tr.getBasis();
btMatrix3x3 basis = tr.getBasis();
btMatrix3x3 basisorg = tr.getBasis();
SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t);
SimdQuaternion tmpOrn;
btQuaternion rot(btVector3(0.,0.,1.),m_w*t);
btQuaternion tmpOrn;
tr.getBasis().getRotation(tmpOrn);
rot = rot * tmpOrn;
//to avoid numerical drift, normalize quaternion
rot.normalize();
newtr.setBasis(SimdMatrix3x3(rot));
newtr.setBasis(btMatrix3x3(rot));
return newtr;
}
SimdScalar BU_Screwing::CalculateF(SimdScalar t) const
btScalar BU_Screwing::CalculateF(btScalar t) const
{
SimdScalar result;
btScalar result;
if (!m_w)
{
result = t;
} 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;
}

42
Extras/AlgebraicCCD/BU_Screwing.h
View File

@ -18,9 +18,9 @@ subject to the following restrictions:
#define B_SCREWING_H
#include <LinearMath/SimdVector3.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btVector3.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btTransform.h>
#define SCREWEPSILON 0.00001f
@ -31,47 +31,47 @@ class BU_Screwing
public:
BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel);
BU_Screwing(const btVector3& relLinVel,const btVector3& relAngVel);
~BU_Screwing() {
};
SimdScalar CalculateF(SimdScalar t) const;
btScalar CalculateF(btScalar t) const;
//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(
pt.x()*SimdCos(m_w*t)-pt.y()*SimdSin(m_w*t),
pt.x()*SimdSin(m_w*t)+pt.y()*SimdCos(m_w*t),
return btPoint3(
pt.x()*btCos(m_w*t)-pt.y()*btSin(m_w*t),
pt.x()*btSin(m_w*t)+pt.y()*btCos(m_w*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(
vec.x()*SimdCos(m_w*t)-vec.y()*SimdSin(m_w*t),
vec.x()*SimdSin(m_w*t)+vec.y()*SimdCos(m_w*t),
return btVector3(
vec.x()*btCos(m_w*t)-vec.y()*btSin(m_w*t),
vec.x()*btSin(m_w*t)+vec.y()*btCos(m_w*t),
vec.z());
}
//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
void LocalMatrix(SimdTransform &t) const;
void LocalMatrix(btTransform &t) const;
inline const SimdVector3& GetU() const { return m_u;}
inline const SimdVector3& GetO() const {return m_o;}
inline const SimdScalar GetS() const{ return m_s;}
inline const SimdScalar GetW() const { return m_w;}
inline const btVector3& GetU() const { return m_u;}
inline const btVector3& GetO() const {return m_o;}
inline const btScalar GetS() const{ return m_s;}
inline const btScalar GetW() const { return m_w;}
private:
float m_w;
float m_s;
SimdVector3 m_u;
SimdVector3 m_o;
btVector3 m_u;
btVector3 m_o;
};
#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
#include <CollisionShapes/BU_MotionStateInterface.h>
#include <btCollisionShapes/BU_MotionStateInterface.h>
class BU_StaticMotionState :public BU_MotionStateInterface
{
public:
virtual ~BU_StaticMotionState(){};
virtual void SetTransform(const SimdTransform& trans)
virtual void SetTransform(const btTransform& trans)
{
m_trans = trans;
}
virtual void GetTransform(SimdTransform& trans) const
virtual void GetTransform(btTransform& trans) const
{
trans = m_trans;
}
virtual void SetPosition(const SimdPoint3& position)
virtual void SetPosition(const btPoint3& position)
{
m_trans.setOrigin( position );
}
virtual void GetPosition(SimdPoint3& position) const
virtual void GetPosition(btPoint3& position) const
{
position = m_trans.getOrigin();
}
virtual void SetOrientation(const SimdQuaternion& orientation)
virtual void SetOrientation(const btQuaternion& orientation)
{
m_trans.setRotation( orientation);
}
virtual void GetOrientation(SimdQuaternion& orientation) const
virtual void GetOrientation(btQuaternion& orientation) const
{
orientation = m_trans.getRotation();
}
virtual void SetBasis(const SimdMatrix3x3& basis)
virtual void SetBasis(const btMatrix3x3& basis)
{
m_trans.setBasis( basis);
}
virtual void GetBasis(SimdMatrix3x3& basis) const
virtual void GetBasis(btMatrix3x3& basis) const
{
basis = m_trans.getBasis();
}
virtual void SetLinearVelocity(const SimdVector3& linvel)
virtual void SetLinearVelocity(const btVector3& linvel)
{
m_linearVelocity = linvel;
}
virtual void GetLinearVelocity(SimdVector3& linvel) const
virtual void GetLinearVelocity(btVector3& linvel) const
{
linvel = m_linearVelocity;
}
virtual void SetAngularVelocity(const SimdVector3& angvel)
virtual void SetAngularVelocity(const btVector3& angvel)
{
m_angularVelocity = angvel;
}
virtual void GetAngularVelocity(SimdVector3& angvel) const
virtual void GetAngularVelocity(btVector3& angvel) const
{
angvel = m_angularVelocity;
}
@ -82,9 +82,9 @@ public:
protected:
SimdTransform m_trans;
SimdVector3 m_angularVelocity;
SimdVector3 m_linearVelocity;
btTransform m_trans;
btVector3 m_angularVelocity;
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_Screwing.h"
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdVector3.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btVector3.h>
#define USE_ALGEBRAIC
#ifdef USE_ALGEBRAIC
@ -30,7 +30,7 @@ subject to the following restrictions:
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
#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()
@ -41,9 +41,9 @@ BU_VertexPoly::BU_VertexPoly()
//false otherwise. If true, minTime contains the time of impact
bool BU_VertexPoly::GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& a,
const SimdVector4& planeEq,
SimdScalar &minTime,bool swapAB)
const btPoint3& a,
const btVector4& planeEq,
btScalar &minTime,bool swapAB)
{
bool hit = false;
@ -57,21 +57,21 @@ bool BU_VertexPoly::GetTimeOfImpact(
//case w<>0 and s<> 0
const SimdScalar w=screwAB.GetW();
const SimdScalar s=screwAB.GetS();
const btScalar w=screwAB.GetW();
const btScalar s=screwAB.GetS();
SimdScalar coefs[4];
const SimdScalar p=planeEq[0];
const SimdScalar q=planeEq[1];
const SimdScalar r=planeEq[2];
const SimdScalar d=planeEq[3];
btScalar coefs[4];
const btScalar p=planeEq[0];
const btScalar q=planeEq[1];
const btScalar r=planeEq[2];
const btScalar d=planeEq[3];
const SimdVector3 norm(p,q,r);
const btVector3 norm(p,q,r);
BU_Polynomial polynomialSolver;
int numroots = 0;
//SimdScalar eps=1e-80f;
//SimdScalar eps2=1e-100f;
//btScalar eps=1e-80f;
//btScalar eps2=1e-100f;
if (TestFuzzyZero(screwAB.GetS()) )
{
@ -93,7 +93,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
// W = 0 , S <> 0
//pax+qay+r(az+st)=d
SimdScalar dist = (d - a.dot(norm));
btScalar dist = (d - a.dot(norm));
if (TestFuzzyZero(r))
{
@ -108,7 +108,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
} else
{
SimdScalar etoi = (dist)/(r*screwAB.GetS());
btScalar etoi = (dist)/(r*screwAB.GetS());
if (swapAB)
etoi *= -1;
@ -124,7 +124,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
//ax^3+bx^2+cx+d=0
//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)
ietsje = 0.f;
@ -140,9 +140,9 @@ bool BU_VertexPoly::GetTimeOfImpact(
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
if (swapAB)
{

12
Extras/AlgebraicCCD/BU_VertexPoly.h
View File

@ -19,9 +19,9 @@ subject to the following restrictions:
class BU_Screwing;
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdScalar.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btScalar.h>
///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane.
class BU_VertexPoly
@ -30,9 +30,9 @@ public:
BU_VertexPoly();
bool GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& vtx,
const SimdVector4& planeEq,
SimdScalar &minTime,
const btPoint3& vtx,
const btVector4& planeEq,
btScalar &minTime,
bool swapAB);
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.
*/
#include "SphereSphereCollisionAlgorithm.h"
#include "btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
SphereSphereCollisionAlgorithm::SphereSphereCollisionAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
: CollisionAlgorithm(ci),
btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
: btCollisionAlgorithm(ci),
m_ownManifold(false),
m_manifoldPtr(mf)
{
@ -30,7 +30,7 @@ m_manifoldPtr(mf)
}
}
SphereSphereCollisionAlgorithm::~SphereSphereCollisionAlgorithm()
btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
{
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)
return;
CollisionObject* col0 = static_cast<CollisionObject*>(proxy0->m_clientObject);
CollisionObject* col1 = static_cast<CollisionObject*>(proxy1->m_clientObject);
SphereShape* sphere0 = (SphereShape*)col0->m_collisionShape;
SphereShape* sphere1 = (SphereShape*)col1->m_collisionShape;
btCollisionObject* col0 = static_cast<btCollisionObject*>(proxy0->m_clientObject);
btCollisionObject* col1 = static_cast<btCollisionObject*>(proxy1->m_clientObject);
btSphereShape* sphere0 = (btSphereShape*)col0->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();
SimdScalar radius0 = sphere0->GetRadius();
SimdScalar radius1 = sphere1->GetRadius();
btScalar radius0 = sphere0->GetRadius();
btScalar radius1 = sphere1->GetRadius();
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))
return;
///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)
SimdVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
///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
ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
btManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
resultOut->AddContactPoint(normalOnSurfaceB,pos1,dist);
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
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/btBroadphaseProxy.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.
/// Also provides the most basic sample for custom/user CollisionAlgorithm
class SphereSphereCollisionAlgorithm : public CollisionAlgorithm
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
{
bool m_ownManifold;
PersistentManifold* m_manifoldPtr;
btPersistentManifold* m_manifoldPtr;
public:
SphereSphereCollisionAlgorithm(const CollisionAlgorithmConstructionInfo& ci)
: CollisionAlgorithm(ci) {}
btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& 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.
*/
#include "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/SimdMinMax.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btSimdMinMax.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
@ -37,11 +37,11 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/EpaPolyhedron.h"
#include "NarrowPhaseCollision/Epa.h"
const SimdScalar 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 = 1e-2f;
const btScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
const SimdTransform& transformA, const SimdTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
Epa::Epa( btConvexShape* pConvexShapeA, btConvexShape* pConvexShapeB,
const btTransform& transformA, const btTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
m_pConvexShapeB( pConvexShapeB ),
m_transformA( transformA ),
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
SimdVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY;
btVector3 v( 1, 0, 0 );
btScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
btScalar delta = 0.f;
simplexSolver.reset();
@ -70,16 +70,16 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
{
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis();
btVector3 seperatingAxisInA = -v * m_transformA.getBasis();
btVector3 seperatingAxisInB = v * m_transformB.getBasis();
SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( pInA );
SimdPoint3 qWorld = m_transformB( qInB );
btPoint3 pWorld = m_transformA( pInA );
btPoint3 qWorld = m_transformB( qInB );
SimdVector3 w = pWorld - qWorld;
btVector3 w = pWorld - qWorld;
delta = v.dot( w );
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)
return false;
SimdScalar prevVSqrd = squaredDistance;
btScalar prevVSqrd = squaredDistance;
squaredDistance = v.length2();
// Is v converging to v(A-B) ?
@ -115,9 +115,9 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
++nbIterations;
}
SimdPoint3 simplexPoints[ 5 ];
SimdPoint3 wSupportPointsOnA[ 5 ];
SimdPoint3 wSupportPointsOnB[ 5 ];
btPoint3 simplexPoints[ 5 ];
btPoint3 wSupportPointsOnA[ 5 ];
btPoint3 wSupportPointsOnB[ 5 ];
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
// 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();
SimdVector3 v1;
SimdVector3 v2;
SimdVector3 v3;
btVector3 v1;
btVector3 v2;
btVector3 v3;
SimdVector3 e1;
btVector3 e1;
SimdScalar absx = abs( d.getX() );
SimdScalar absy = abs( d.getY() );
SimdScalar absz = abs( d.getZ() );
btScalar absx = abs( d.getX() );
btScalar absy = abs( d.getY() );
btScalar absz = abs( d.getZ() );
if ( absx < absy )
{
@ -186,14 +186,14 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
btVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
btVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
btPoint3 pWorld = m_transformA( p );
btPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 2 ] = pWorld;
wSupportPointsOnB[ 2 ] = qWorld;
@ -255,21 +255,21 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
// We have a triangle inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
SimdVector3 triangleNormal = v0.cross( v1 );
btVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
btVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
btVector3 triangleNormal = v0.cross( v1 );
triangleNormal.normalize();
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
btVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
btVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
btPoint3 pWorld = m_transformA( p );
btPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
@ -316,17 +316,17 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
btPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
simplexPoints[ initTetraIndices[ 1 ] ],
simplexPoints[ initTetraIndices[ 2 ] ],
simplexPoints[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
btPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
wSupportPointsOnA[ initTetraIndices[ 1 ] ],
wSupportPointsOnA[ initTetraIndices[ 2 ] ],
wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
btPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
wSupportPointsOnB[ initTetraIndices[ 1 ] ],
wSupportPointsOnB[ initTetraIndices[ 2 ] ],
wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
@ -398,16 +398,16 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
return true;
}
SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
btScalar Epa::CalcPenDepth( btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
{
SimdVector3 v;
btVector3 v;
SimdScalar upperBoundSqrd = SIMD_INFINITY;
SimdScalar vSqrd = 0;
btScalar upperBoundSqrd = SIMD_INFINITY;
btScalar vSqrd = 0;
#ifdef _DEBUG
SimdScalar prevVSqrd;
btScalar prevVSqrd;
#endif
SimdScalar delta;
btScalar delta;
bool isCloseEnough = false;
@ -445,20 +445,20 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
#endif //_DEBUG
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" );
SimdVector3 seperatingAxisInA = v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis();
btVector3 seperatingAxisInA = v * m_transformA.getBasis();
btVector3 seperatingAxisInB = -v * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
btPoint3 pWorld = m_transformA( p );
btPoint3 qWorld = m_transformB( q );
SimdPoint3 w = pWorld - qWorld;
btPoint3 w = pWorld - qWorld;
delta = v.dot( w );
// 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!" );
isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
@ -544,10 +544,10 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
return v.length();
}
bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
const SimdPoint3& point2, const SimdPoint3& point3 )
bool Epa::TetrahedronContainsOrigin( const btPoint3& point0, const btPoint3& point1,
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 ),
( point3 - point2 ).cross( point0 - point2 ),
( 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 ) );
}
bool Epa::TetrahedronContainsOrigin( SimdPoint3* pPoints )
bool Epa::TetrahedronContainsOrigin( btPoint3* pPoints )
{
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 SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD;
extern const btScalar EPA_MAX_RELATIVE_ERROR;
extern const btScalar EPA_MAX_RELATIVE_ERROR_SQRD;
class Epa
{
@ -34,30 +34,30 @@ class Epa
public :
Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
const SimdTransform& transformA, const SimdTransform& transformB );
Epa( btConvexShape* pConvexShapeA, btConvexShape* pConvexShapeB,
const btTransform& transformA, const btTransform& transformB );
~Epa();
bool Initialize( SimplexSolverInterface& simplexSolver );
bool Initialize( btSimplexSolverInterface& simplexSolver );
SimdScalar CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
btScalar CalcPenDepth( btPoint3& wWitnessOnA, btPoint3& wWitnessOnB );
private :
bool TetrahedronContainsOrigin( SimdPoint3* pPoints );
bool TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
const SimdPoint3& point2, const SimdPoint3& point3 );
bool TetrahedronContainsOrigin( btPoint3* pPoints );
bool TetrahedronContainsOrigin( const btPoint3& point0, const btPoint3& point1,
const btPoint3& point2, const btPoint3& point3 );
private :
//! Priority queue
std::vector< EpaFace* > m_faceEntries;
ConvexShape* m_pConvexShapeA;
ConvexShape* m_pConvexShapeB;
btConvexShape* m_pConvexShapeA;
btConvexShape* m_pConvexShapeB;
SimdTransform m_transformA;
SimdTransform m_transformB;
btTransform m_transformA;
btTransform m_transformB;
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.
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "NarrowPhaseCollision/EpaCommon.h"
@ -25,7 +25,7 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/EpaFace.h"
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar PLANE_THICKNESS = 1e-5f;
btScalar PLANE_THICKNESS = 1e-5f;
#endif
EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false )
@ -43,17 +43,17 @@ bool EpaFace::Initialize()
CollectVertices( m_pVertices );
const SimdVector3 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 e0 = m_pVertices[ 1 ]->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 SimdScalar e1Sqrd = e1.length2();
const SimdScalar e0e1 = e0.dot( e1 );
const btScalar e0Sqrd = e0.length2();
const btScalar e1Sqrd = e1.length2();
const btScalar e0e1 = e0.dot( e1 );
m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
const SimdScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
const SimdScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
const btScalar e0v0 = e0.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[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
@ -83,8 +83,8 @@ bool EpaFace::CalculatePlane()
// Traditional method
const SimdVector3 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 v1 = m_pVertices[ 1 ]->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 );
@ -99,10 +99,10 @@ bool EpaFace::CalculatePlane()
// Robust method
//SimdVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
//SimdVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
//btVector3 _v1 = m_pVertices[ 1 ]->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 );
@ -119,8 +119,8 @@ bool EpaFace::CalculatePlane()
//n /= 3;
//n.normalize();
//SimdVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
//SimdScalar d = c.dot( -n );
//btVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
//btScalar d = c.dot( -n );
//m_robustPlaneNormal = n;
//m_robustPlaneDistance = d;
@ -142,8 +142,8 @@ bool EpaFace::CalculatePlane()
void EpaFace::CalcClosestPoint()
{
const SimdVector3 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 e0 = m_pVertices[ 1 ]->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 +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant;
@ -151,20 +151,20 @@ void EpaFace::CalcClosestPoint()
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 SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
const btVector3 e0 = m_pVertices[ 1 ]->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 +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /
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 SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
const btVector3 e0 = m_pVertices[ 1 ]->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 +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /

22
Extras/EPA/EpaFace.h
View File

@ -21,7 +21,7 @@ class EpaVertex;
class EpaHalfEdge;
#ifdef EPA_POLYHEDRON_USE_PLANES
extern SimdScalar PLANE_THICKNESS;
extern btScalar PLANE_THICKNESS;
#endif
//! Note : This class is not supposed to be a base class
@ -44,8 +44,8 @@ class EpaFace
bool CalculatePlane();
#endif
void CalcClosestPoint();
void CalcClosestPointOnA( SimdVector3& closestPointOnA );
void CalcClosestPointOnB( SimdVector3& closestPointOnB );
void CalcClosestPointOnA( btVector3& closestPointOnA );
void CalcClosestPointOnB( btVector3& closestPointOnB );
bool IsAffinelyDependent() const;
bool IsClosestPointInternal() const;
@ -63,18 +63,18 @@ class EpaFace
EpaVertex* m_pVertices[ 3 ];
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 m_planeNormal;
SimdScalar m_planeDistance;
btVector3 m_planeNormal;
btScalar m_planeDistance;
//SimdVector3 m_robustPlaneNormal;
//SimdScalar m_robustPlaneDistance;
//btVector3 m_robustPlaneNormal;
//btScalar m_robustPlaneDistance;
#endif
SimdVector3 m_v;
SimdScalar m_vSqrd;
btVector3 m_v;
btScalar m_vSqrd;
SimdScalar m_determinant;
SimdScalar m_lambdas[ 2 ];
btScalar m_determinant;
btScalar m_lambdas[ 2 ];
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.
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/SimdMinMax.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btSimdMinMax.h"
#include <list>
@ -36,19 +36,19 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
SimdScalar g_GJKMaxRelError = 1e-3f;
SimdScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
btScalar g_GJKMaxRelError = 1e-3f;
btScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
class IDebugDraw* debugDraw )
bool EpaPenetrationDepthSolver::CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class btIDebugDraw* debugDraw )
{
EPA_DEBUG_ASSERT( pConvexA ,"Convex shape A is invalid!" );
EPA_DEBUG_ASSERT( pConvexB ,"Convex shape B is invalid!" );
SimdScalar penDepth;
btScalar penDepth;
#ifdef EPA_USE_HYBRID
bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
@ -70,17 +70,17 @@ bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSol
}
#ifdef EPA_USE_HYBRID
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v )
bool EpaPenetrationDepthSolver::HybridPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
btScalar& penDepth, btVector3& v )
{
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
btScalar squaredDistance = SIMD_INFINITY;
btScalar delta = 0.f;
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const SimdScalar marginSqrd = margin * margin;
const btScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const btScalar marginSqrd = margin * margin;
simplexSolver.reset();
@ -90,16 +90,16 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
SimdVector3 seperatingAxisInB = v * transformB.getBasis();
btVector3 seperatingAxisInA = -v * transformA.getBasis();
btVector3 seperatingAxisInB = v * transformB.getBasis();
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
btVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
btVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = transformA( pInA );
SimdPoint3 qWorld = transformB( qInB );
btPoint3 pWorld = transformA( pInA );
btPoint3 qWorld = transformB( qInB );
SimdVector3 w = pWorld - qWorld;
btVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
@ -117,7 +117,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@ -137,7 +137,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@ -148,7 +148,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
btScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
@ -160,7 +160,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@ -184,10 +184,10 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
}
#endif
SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
btScalar EpaPenetrationDepthSolver::EpaPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
{
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.
*/
extern SimdScalar g_GJKMaxRelError;
extern SimdScalar g_GJKMaxRelErrorSqrd;
extern btScalar g_GJKMaxRelError;
extern btScalar g_GJKMaxRelErrorSqrd;
//! Note : This class is not supposed to be a base class
class EpaPenetrationDepthSolver : public ConvexPenetrationDepthSolver
class EpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{
public :
bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
class IDebugDraw* debugDraw );
bool CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class btIDebugDraw* debugDraw );
private :
#ifdef EPA_USE_HYBRID
bool HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v );
bool HybridPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
btScalar& penDepth, btVector3& v );
#endif
SimdScalar EpaPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
btScalar EpaPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB );
};
#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.
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "Memory2.h"
#include <list>
@ -42,8 +42,8 @@ EpaPolyhedron::~EpaPolyhedron()
Destroy();
}
bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB,
bool EpaPolyhedron::Create( btPoint3* pInitialPoints,
btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
@ -134,8 +134,8 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
int axisIndex = axisOrderIndices[ axis ];
SimdScalar axisMin = SIMD_INFINITY;
SimdScalar axisMax = -SIMD_INFINITY;
btScalar axisMin = SIMD_INFINITY;
btScalar axisMax = -SIMD_INFINITY;
for ( int i = 0; i < 4; ++i )
{
@ -194,36 +194,36 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
//////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
btVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
btVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
btVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
btVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif
SimdVector3 planeNormal = v1.cross( v0 );
btVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
btScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
btScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif
#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)
{
EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
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)
{
EPA_DEBUG_ASSERT(0,"Point1 should be on plane!");
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)
{
EPA_DEBUG_ASSERT(0,"Point2 should be on plane!");
@ -235,7 +235,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
if ( planeDistance > 0 )
{
SimdVector3 tmp = pInitialPoints[ 1 ];
btVector3 tmp = pInitialPoints[ 1 ];
pInitialPoints[ 1 ] = pInitialPoints[ 2 ];
pInitialPoints[ 2 ] = tmp;
@ -256,16 +256,16 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
#else
finalPointsIndices[ 3 ] = -1;
SimdScalar absMaxDist = -SIMD_INFINITY;
SimdScalar maxDist;
btScalar absMaxDist = -SIMD_INFINITY;
btScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{
SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
SimdScalar absDist = abs( dist );
btScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
btScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) &&
!SimdEqual( dist, PLANE_THICKNESS ) )
!btEqual( dist, PLANE_THICKNESS ) )
{
absMaxDist = absDist;
maxDist = dist;
@ -283,7 +283,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
// Can swap indices only
SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
btPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ];
pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp;
@ -425,7 +425,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
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 )
{
@ -486,9 +486,9 @@ EpaHalfEdge* EpaPolyhedron::CreateHalfEdge()
return pNewHalfEdge;
}
EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB )
EpaVertex* EpaPolyhedron::CreateVertex( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB )
{
EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB );
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,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB,
bool EpaPolyhedron::Expand( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces )
{
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
#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 )
{
return false;
@ -601,7 +601,7 @@ int EpaPolyhedron::GetNbFaces() const
return m_faces.size();
}
void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
void EpaPolyhedron::DeleteVisibleFaces( const btPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges )
{
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
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;
if ( pointDist > PLANE_THICKNESS )
#else
SimdScalar dot = pAdjacentFace->m_v.dot( point );
btScalar dot = pAdjacentFace->m_v.dot( point );
if ( dot >= pAdjacentFace->m_vSqrd )
#endif
{

18
Extras/EPA/EpaPolyhedron.h
View File

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

14
Extras/EPA/EpaVertex.h
View File

@ -30,13 +30,13 @@ class EpaVertex
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,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ),
EpaVertex( const btPoint3& point,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ),
m_wSupportPointOnA( wSupportPointOnA ),
m_wSupportPointOnB( wSupportPointOnB )
{
@ -51,10 +51,10 @@ class EpaVertex
//! This is not necessary
//EpaHalfEdge* m_pHalfEdge;
SimdPoint3 m_point;
btPoint3 m_point;
SimdPoint3 m_wSupportPointOnA;
SimdPoint3 m_wSupportPointOnB;
btPoint3 m_wSupportPointOnA;
btPoint3 m_wSupportPointOnB;
};
#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]);
m_voronoiSolver.addVertex(w,p,q);
@ -38,8 +38,8 @@ void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
int i;
i=0;
SimdPoint3 vp1,vp2;
SimdPoint3 jp1,jp2;
btPoint3 vp1,vp2;
btPoint3 jp1,jp2;
/*
bool isClosest0 = m_voronoiSolver.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 result1 = 0;
SimdVector3 v0,v1;
btVector3 v0,v1;
result0 = m_voronoiSolver.closest(v0);
result1 = m_johnsonSolver.closest(v1);
@ -79,10 +79,10 @@ bool CombinedSimplexSolver::closest(SimdVector3& v)
return result1;
}
SimdScalar CombinedSimplexSolver::maxVertex()
btScalar CombinedSimplexSolver::maxVertex()
{
SimdScalar maxv0 = m_voronoiSolver.maxVertex();
SimdScalar maxv1 = m_johnsonSolver.maxVertex();
btScalar maxv0 = m_voronoiSolver.maxVertex();
btScalar maxv1 = m_johnsonSolver.maxVertex();
MY_ASSERT(maxv0 = maxv1);
if (m_useVoronoiSolver)
return maxv0;
@ -102,7 +102,7 @@ bool CombinedSimplexSolver::fullSimplex() const
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()
{
SimdPoint3 pBuf0[4];
SimdPoint3 qBuf0[4];
SimdPoint3 yBuf0[4];
SimdPoint3 pBuf1[4];
SimdPoint3 qBuf1[4];
SimdPoint3 yBuf1[4];
btPoint3 pBuf0[4];
btPoint3 qBuf0[4];
btPoint3 yBuf0[4];
btPoint3 pBuf1[4];
btPoint3 qBuf1[4];
btPoint3 yBuf1[4];
int verts0,verts1;
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 insimplex1 = m_johnsonSolver.inSimplex(w);
@ -163,9 +163,9 @@ bool CombinedSimplexSolver::inSimplex(const SimdVector3& w)
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_johnsonSolver.backup_closest(v1);
@ -189,10 +189,10 @@ bool CombinedSimplexSolver::emptySimplex() const
return empty1;
}
void CombinedSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
void CombinedSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{
SimdPoint3 tmpP1,tmpP2;
SimdPoint3 tmpJP1,tmpJP2;
btPoint3 tmpP1,tmpP2;
btPoint3 tmpJP1,tmpJP2;
m_voronoiSolver.compute_points(tmpP1,tmpP2);
m_johnsonSolver.compute_points(tmpJP1,tmpJP2);

20
Extras/ExtraSolid35/CombinedSimplexSolver.h
View File

@ -17,10 +17,10 @@
#include "Solid3JohnsonSimplexSolver.h"
/// CombinedSimplexSolver runs both Solid and Voronoi Simplex Solver for comparison
class CombinedSimplexSolver: public SimplexSolverInterface
class CombinedSimplexSolver: public btSimplexSolverInterface
{
VoronoiSimplexSolver m_voronoiSolver;
// VoronoiSimplexSolver m_johnsonSolver;
btVoronoiSimplexSolver m_voronoiSolver;
// btVoronoiSimplexSolver m_johnsonSolver;
Solid3JohnsonSimplexSolver m_johnsonSolver;
@ -35,23 +35,23 @@ class CombinedSimplexSolver: public SimplexSolverInterface
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 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 void compute_points(SimdPoint3& p1, SimdPoint3& p2);
virtual void compute_points(btPoint3& p1, btPoint3& p2);
virtual int numVertices() const;

154
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.cpp
View File

@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* 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
* packaging of this file.
*
@ -26,34 +26,34 @@
#include <vector>
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
#define ASSERT_MESSAGE
class ReplaceMeAccuracy {
public:
static SimdScalar rel_error2; // squared relative error in the computed distance
static SimdScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
static SimdScalar tol_error; // error tolerance if the distance is almost zero
static btScalar rel_error2; // squared relative error in the computed distance
static btScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
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;
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;
}
};
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;
SimdScalar ReplaceMeAccuracy::depth_tolerance = SimdScalar(1.0) + SimdScalar(2.0) * rel_error;
SimdScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
btScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error;
btScalar ReplaceMeAccuracy::depth_tolerance = btScalar(1.0) + btScalar(2.0) * rel_error;
btScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
@ -101,28 +101,28 @@ public:
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
{
return m_lambda1 >= SimdScalar(0.0) &&
m_lambda2 >= SimdScalar(0.0) &&
return m_lambda1 >= btScalar(0.0) &&
m_lambda2 >= btScalar(0.0) &&
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) +
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();
m_obsolete = true;
@ -132,18 +132,18 @@ public:
}
private:
void silhouette(int index, const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
void silhouette(int index, const btVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
int m_indices[3];
bool m_obsolete;
ReplaceMeFacet *m_adjFacets[3];
int m_adjEdges[3];
SimdScalar m_det;
SimdScalar m_lambda1;
SimdScalar m_lambda2;
SimdVector3 m_closest;
SimdScalar m_dist2;
btScalar m_det;
btScalar m_lambda1;
btScalar m_lambda2;
btVector3 m_closest;
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;
SimdVector3 v2 = verts[m_indices[2]] - p0;
SimdScalar v1dv1 = v1.length2();
SimdScalar v1dv2 = v1.dot(v2);
SimdScalar v2dv2 = v2.length2();
SimdScalar p0dv1 = p0.dot(v1);
SimdScalar p0dv2 = p0.dot(v2);
btVector3 v1 = verts[m_indices[1]] - p0;
btVector3 v2 = verts[m_indices[2]] - p0;
btScalar v1dv1 = v1.length2();
btScalar v1dv2 = v1.dot(v2);
btScalar v2dv2 = v2.length2();
btScalar p0dv1 = p0.dot(v1);
btScalar p0dv2 = p0.dot(v2);
m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative
//printf("m_det = %f\n",m_det);
@ -192,7 +192,7 @@ bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
return false;
}
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w,
void ReplaceMeFacet::silhouette(int index, const btVector3& w,
ReplaceMeEdgeBuffer& edgeBuffer)
{
if (!m_obsolete) {
@ -227,9 +227,9 @@ inline int ReplaceMeEdge::getTarget() const
const int MaxSupportPoints = 100;//1000;
const int MaxFacets = 200;//b2000;
static SimdPoint3 pBuf[MaxSupportPoints];
static SimdPoint3 qBuf[MaxSupportPoints];
static SimdVector3 yBuf[MaxSupportPoints];
static btPoint3 pBuf[MaxSupportPoints];
static btPoint3 qBuf[MaxSupportPoints];
static btVector3 yBuf[MaxSupportPoints];
static ReplaceMeFacet facetBuf[MaxFacets];
static int freeFacet = 0;
@ -249,7 +249,7 @@ public:
ReplaceMeFacetComp myFacetComp;
inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
SimdScalar lower2, SimdScalar upper2)
btScalar lower2, btScalar upper2)
{
assert(i0 != i1 && i0 != i2 && i1 != i2);
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,
const SimdVector3& p3, const SimdVector3& p4)
inline bool originInTetrahedron(const btVector3& p1, const btVector3& p2,
const btVector3& p3, const btVector3& p4)
{
SimdVector3 normal1 = (p2 - p1).cross(p3 - p1);
SimdVector3 normal2 = (p3 - p2).cross(p4 - p2);
SimdVector3 normal3 = (p4 - p3).cross(p1 - p3);
SimdVector3 normal4 = (p1 - p4).cross(p2 - p4);
btVector3 normal1 = (p2 - p1).cross(p3 - p1);
btVector3 normal2 = (p3 - p2).cross(p4 - p2);
btVector3 normal3 = (p4 - p3).cross(p1 - p3);
btVector3 normal4 = (p1 - p4).cross(p2 - p4);
return
(normal1.dot(p1) > SimdScalar(0.0)) != (normal1.dot(p4) > SimdScalar(0.0)) &&
(normal2.dot(p2) > SimdScalar(0.0)) != (normal2.dot(p1) > SimdScalar(0.0)) &&
(normal3.dot(p3) > SimdScalar(0.0)) != (normal3.dot(p2) > SimdScalar(0.0)) &&
(normal4.dot(p4) > SimdScalar(0.0)) != (normal4.dot(p3) > SimdScalar(0.0));
(normal1.dot(p1) > btScalar(0.0)) != (normal1.dot(p4) > btScalar(0.0)) &&
(normal2.dot(p2) > btScalar(0.0)) != (normal2.dot(p1) > btScalar(0.0)) &&
(normal3.dot(p3) > btScalar(0.0)) != (normal3.dot(p2) > btScalar(0.0)) &&
(normal4.dot(p4) > btScalar(0.0)) != (normal4.dot(p3) > btScalar(0.0));
}
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
bool Solid3EpaPenetrationDepth::CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transformA,const btTransform& transformB,
btVector3& v, btPoint3& pa, btPoint3& pb)
{
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
// 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();
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));
SimdMatrix3x3 rot_mat(rot);
btQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, btScalar(0.5));
btMatrix3x3 rot_mat(rot);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2;
btVector3 aux1 = dir.cross(btVector3(axis == 0, axis == 1, axis == 2));
btVector3 aux2 = rot_mat * aux1;
btVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.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
// the origin. First blow it up.
SimdVector3 v1 = yBuf[1] - yBuf[0];
SimdVector3 v2 = yBuf[2] - yBuf[0];
SimdVector3 vv = v1.cross(v2);
btVector3 v1 = yBuf[1] - yBuf[0];
btVector3 v2 = yBuf[2] - yBuf[0];
btVector3 vv = v1.cross(v2);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
@ -430,15 +430,15 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
num_facets = 0;
freeFacet = 0;
ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f0 = addFacet(0, 1, 2, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, btScalar(0.0), SIMD_INFINITY);
if (!f0 || f0->getDist2() == SimdScalar(0.0) ||
!f1 || f1->getDist2() == SimdScalar(0.0) ||
!f2 || f2->getDist2() == SimdScalar(0.0) ||
!f3 || f3->getDist2() == SimdScalar(0.0))
if (!f0 || f0->getDist2() == btScalar(0.0) ||
!f1 || f1->getDist2() == btScalar(0.0) ||
!f2 || f2->getDist2() == btScalar(0.0) ||
!f3 || f3->getDist2() == btScalar(0.0))
{
return false;
}
@ -461,7 +461,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY;
btScalar upper_bound2 = SIMD_INFINITY;
do {
facet = facetHeap[0];
@ -470,7 +470,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
if (!facet->isObsolete())
{
assert(facet->getDist2() > SimdScalar(0.0));
assert(facet->getDist2() > btScalar(0.0));
if (num_verts == MaxSupportPoints)
{
@ -487,16 +487,16 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
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.
//assert(far_dist2 > SimdScalar(0.0));
//assert(far_dist2 > btScalar(0.0));
//
// this is to avoid problems with implicit-sphere-touching contact
//
if (far_dist2 < SimdScalar(0.0))
if (far_dist2 < btScalar(0.0))
{
return false;
}

12
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h
View File

@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* 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
* packaging of this file.
*
@ -28,14 +28,14 @@
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
/// Solid3EpaPenetrationDepth contains the 'Expanding Polytope Algorithm' from Solid 3.5
class Solid3EpaPenetrationDepth : public ConvexPenetrationDepthSolver
class Solid3EpaPenetrationDepth : public btConvexPenetrationDepthSolver
{
public:
virtual bool CalcPenDepth(SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb);
virtual bool CalcPenDepth(btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transformA,const btTransform& transformB,
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.
*
* 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
* packaging of this file.
*
@ -22,7 +22,7 @@
*/
#include "Solid3JohnsonSimplexSolver.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
//#define USE_BACKUP_PROCEDURE
//#define FAST_CLOSEST
@ -48,7 +48,7 @@ void Solid3JohnsonSimplexSolver::reset()
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
void Solid3JohnsonSimplexSolver::addVertex(const btVector3& w)
{
assert(!fullSimplex());
m_last = 0;
@ -66,7 +66,7 @@ void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
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);
m_p[m_last] = p;
@ -83,12 +83,12 @@ bool Solid3JohnsonSimplexSolver::fullSimplex() const
return m_bits1 == 0xf;
}
SimdScalar Solid3JohnsonSimplexSolver::maxVertex()
btScalar Solid3JohnsonSimplexSolver::maxVertex()
{
return m_maxlen2;
}
bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
bool Solid3JohnsonSimplexSolver::closest(btVector3& v)
{
#ifdef FAST_CLOSEST
T_Bits s;
@ -125,14 +125,14 @@ bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
// Original GJK calls the backup procedure at this point.
#ifdef USE_BACKUP_PROCEDURE
backup_closest(SimdVector3& v);
backup_closest(btVector3& v);
#endif
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 i;
@ -155,7 +155,7 @@ int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, S
return num_verts;
}
bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w)
bool Solid3JohnsonSimplexSolver::inSimplex(const btVector3& w)
{
int i;
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;
for (s = m_all_bits; s != 0x0; --s)
{
if (subseteq(s, m_all_bits) && proper(s))
{
SimdVector3 u;
btVector3 u;
compute_vector(s, u);
SimdScalar dist2 = u.length2();
btScalar dist2 = u.length2();
if (dist2 < min_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);
p1.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
p2.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
btScalar sum = btScalar(0.0);
p1.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
p2.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i;
T_Bits bit;
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));
SimdScalar s = SimdScalar(1.0) / sum;
assert(sum > btScalar(0.0));
btScalar s = btScalar(1.0) / sum;
p1 *= s;
p2 *= s;
}
@ -272,12 +272,12 @@ bool Solid3JohnsonSimplexSolver::valid(T_Bits s)
{
if (contains(s, bit))
{
if (m_det[s][i] <= SimdScalar(0.0))
if (m_det[s][i] <= btScalar(0.0))
{
return false;
}
}
else if (m_det[s | bit][i] > SimdScalar(0.0))
else if (m_det[s | bit][i] > btScalar(0.0))
{
return false;
}
@ -292,7 +292,7 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
T_Bits bit;
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;
}
@ -300,11 +300,11 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
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);
SimdScalar sum = SimdScalar(0.0);
v .setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
m_maxlen2 = btScalar(0.0);
btScalar sum = btScalar(0.0);
v .setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i;
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;
}

38
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h
View File

@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* 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
* packaging of this file.
*
@ -30,7 +30,7 @@
/// Solid3JohnsonSimplexSolver contains Johnson subdistance algorithm from Solid 3.5 library
class Solid3JohnsonSimplexSolver : public SimplexSolverInterface
class Solid3JohnsonSimplexSolver : public btSimplexSolverInterface
{
private:
@ -42,22 +42,22 @@ private:
void compute_det();
bool valid(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
SimdVector3 m_edge[4][4];
btScalar m_det[16][4]; // cached sub-determinants
btVector3 m_edge[4][4];
#ifdef JOHNSON_ROBUST
SimdScalar m_norm[4][4];
btScalar m_norm[4][4];
#endif
SimdPoint3 m_p[4]; // support points of object A in local coordinates
SimdPoint3 m_q[4]; // support points of object B in local coordinates
SimdVector3 m_y[4]; // support points of A - B in world coordinates
SimdScalar m_ylen2[4]; // Squared lengths support points y
btPoint3 m_p[4]; // support points of object A in local coordinates
btPoint3 m_q[4]; // support points of object B in local coordinates
btVector3 m_y[4]; // support points of A - B in world coordinates
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
T_Bits m_bits1; // identifies current simplex
T_Bits m_last; // identifies last found support point
@ -69,7 +69,7 @@ private:
private:
void addVertex(const SimdVector3& w);
void addVertex(const btVector3& w);
public:
Solid3JohnsonSimplexSolver();
@ -78,23 +78,23 @@ public:
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 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 void compute_points(SimdPoint3& p1, SimdPoint3& p2) ;
virtual void compute_points(btPoint3& p1, btPoint3& p2) ;
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/CollisionShapes/btConvexShape.h"
#include "CcdPhysicsEnvironment.h"
#include "LinearMath/SimdTransformUtil.h"
#include "LinearMath/btTransformUtil.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btConeShape.h"
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
float gDeactivationTime = 2.f;
@ -39,8 +39,8 @@ float gAngularSleepingTreshold = 1.0f;
#include "BulletDynamics/Dynamics/btMassProps.h"
SimdVector3 startVel(0,0,0);//-10000);
CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
btVector3 startVel(0,0,0);//-10000);
CcdPhysicsController::CcdPhysicsController (const btCcdConstructionInfo& ci)
:m_cci(ci)
{
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];
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]);
trans.setRotation(orn);
return trans;
@ -78,11 +78,11 @@ SimdTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState
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;
@ -120,27 +120,27 @@ bool CcdPhysicsController::SynchronizeMotionStates(float time)
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]);
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->calculateWorldTransformations();
float scale[3];
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);
} else
{
SimdVector3 worldPos;
SimdQuaternion worldquat;
btVector3 worldPos;
btQuaternion worldquat;
m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
SimdTransform oldTrans = m_body->getCenterOfMassTransform();
SimdTransform newTrans(worldquat,worldPos);
btTransform oldTrans = m_body->getCenterOfMassTransform();
btTransform newTrans(worldquat,worldPos);
m_body->setCenterOfMassTransform(newTrans);
//need to keep track of previous position for friction effects...
@ -149,7 +149,7 @@ bool CcdPhysicsController::SynchronizeMotionStates(float time)
float scale[3];
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);
}
return true;
@ -219,8 +219,8 @@ void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dloc
{
m_body->activate();
SimdVector3 dloc(dlocX,dlocY,dlocZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
btVector3 dloc(dlocX,dlocY,dlocZ);
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
@ -239,15 +239,15 @@ void CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
{
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[8],rotval[9],rotval[10]);
SimdMatrix3x3 currentOrn;
btMatrix3x3 currentOrn;
GetWorldOrientation(currentOrn);
SimdTransform xform = m_body->getCenterOfMassTransform();
btTransform xform = m_body->getCenterOfMassTransform();
xform.setBasis(xform.getBasis()*(local ?
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];
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);
}
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];
quatImag1 = q[1];
quatImag2 = q[2];
@ -277,8 +277,8 @@ void CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float
{
m_body->activate();
m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
SimdTransform xform = m_body->getCenterOfMassTransform();
xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
btTransform xform = m_body->getCenterOfMassTransform();
xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
m_body->setCenterOfMassTransform(xform);
}
@ -287,8 +287,8 @@ void CcdPhysicsController::setPosition(float posX,float posY,float posZ)
{
m_body->activate();
m_MotionState->setWorldPosition(posX,posY,posZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
xform.setOrigin(SimdVector3(posX,posY,posZ));
btTransform xform = m_body->getCenterOfMassTransform();
xform.setOrigin(btVector3(posX,posY,posZ));
m_body->setCenterOfMassTransform(xform);
}
@ -298,7 +298,7 @@ void CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvel
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[1] = xform.getOrigin().y();
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)
{
if (!SimdFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
!SimdFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
!SimdFuzzyZero(m_cci.m_scaling.z()-scaleZ))
if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
!btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
!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())
{
@ -324,8 +324,8 @@ void CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
// physics methods
void CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
{
SimdVector3 torque(torqueX,torqueY,torqueZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
btVector3 torque(torqueX,torqueY,torqueZ);
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
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)
{
SimdVector3 force(forceX,forceY,forceZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
btVector3 force(forceX,forceY,forceZ);
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
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)
{
SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
btVector3 angvel(ang_velX,ang_velY,ang_velZ);
if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
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)
{
SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
btVector3 linVel(lin_velX,lin_velY,lin_velZ);
if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
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)
{
SimdVector3 impulse(impulseX,impulseY,impulseZ);
btVector3 impulse(impulseX,impulseY,impulseZ);
if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
SimdVector3 pos(attachX,attachY,attachZ);
btVector3 pos(attachX,attachY,attachZ);
m_body->activate();
@ -402,7 +402,7 @@ void CcdPhysicsController::SetActive(bool active)
// reading out information from physics
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();
linvY = linvel.y();
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)
{
const SimdVector3& angvel= m_body->getAngularVelocity();
const btVector3& angvel= m_body->getAngularVelocity();
angVelX = angvel.x();
angVelY = angvel.y();
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)
{
SimdVector3 pos(posX,posY,posZ);
SimdVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
SimdVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
btVector3 pos(posX,posY,posZ);
btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
linvX = linvel.x();
linvY = linvel.y();
linvZ = linvel.z();
@ -436,7 +436,7 @@ void CcdPhysicsController::setRigidBody(bool rigid)
if (!rigid)
{
//fake it for now
SimdVector3 inertia = m_body->getInvInertiaDiagLocal();
btVector3 inertia = m_body->getInvInertiaDiagLocal();
inertia[1] = 0.f;
m_body->setInvInertiaDiagLocal(inertia);
m_body->updateInertiaTensor();
@ -494,24 +494,24 @@ bool CcdPhysicsController::wantsSleeping()
PHY_IPhysicsController* CcdPhysicsController::GetReplica()
{
//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)
{
switch (cinfo.m_collisionShape->GetShapeType())
{
case SPHERE_SHAPE_PROXYTYPE:
{
SphereShape* orgShape = (SphereShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new SphereShape(*orgShape);
btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new btSphereShape(*orgShape);
break;
}
case CONE_SHAPE_PROXYTYPE:
{
ConeShape* orgShape = (ConeShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new ConeShape(*orgShape);
btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new btConeShape(*orgShape);
break;
}
@ -570,13 +570,13 @@ void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,flo
void DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
{
SimdPoint3 pos(posX,posY,posZ);
btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos );
}
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 );
}

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.
/// It contains the IMotionState and IDeformableMesh Interfaces.
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdScalar.h"
#include "LinearMath/SimdMatrix3x3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "PHY_IMotionState.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for CollisionShape access
class CollisionShape;
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for btCollisionShape access
class btCollisionShape;
extern float gDeactivationTime;
extern float gLinearSleepingTreshold;
@ -41,12 +41,12 @@ class CcdPhysicsEnvironment;
struct CcdConstructionInfo
struct btCcdConstructionInfo
{
///CollisionFilterGroups provides some optional usage of basic collision filtering
///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
{
DefaultFilter = 1,
@ -57,7 +57,7 @@ struct CcdConstructionInfo
};
CcdConstructionInfo()
btCcdConstructionInfo()
: m_gravity(0,0,0),
m_scaling(1.f,1.f,1.f),
m_mass(0.f),
@ -74,26 +74,26 @@ struct CcdConstructionInfo
{
}
SimdVector3 m_localInertiaTensor;
SimdVector3 m_gravity;
SimdVector3 m_scaling;
SimdScalar m_mass;
SimdScalar m_restitution;
SimdScalar m_friction;
SimdScalar m_linearDamping;
SimdScalar m_angularDamping;
btVector3 m_localInertiaTensor;
btVector3 m_gravity;
btVector3 m_scaling;
btScalar m_mass;
btScalar m_restitution;
btScalar m_friction;
btScalar m_linearDamping;
btScalar m_angularDamping;
int m_collisionFlags;
///optional use of collision group/mask:
///only collision with object goups that match the collision mask.
///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
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
CollisionShape* m_collisionShape;
btCollisionShape* m_collisionShape;
class PHY_IMotionState* m_MotionState;
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.
class CcdPhysicsController : public PHY_IPhysicsController
{
RigidBody* m_body;
btRigidBody* m_body;
class PHY_IMotionState* m_MotionState;
void* m_newClientInfo;
CcdConstructionInfo m_cci;//needed for replication
void GetWorldOrientation(SimdMatrix3x3& mat);
btCcdConstructionInfo m_cci;//needed for replication
void GetWorldOrientation(btMatrix3x3& mat);
void CreateRigidbody();
@ -122,14 +122,14 @@ class CcdPhysicsController : public PHY_IPhysicsController
int m_collisionDelay;
CcdPhysicsController (const CcdConstructionInfo& ci);
CcdPhysicsController (const btCcdConstructionInfo& ci);
virtual ~CcdPhysicsController();
RigidBody* GetRigidBody() { return m_body;}
btRigidBody* GetRigidBody() { return m_body;}
CollisionShape* GetCollisionShape() {
btCollisionShape* GetCollisionShape() {
return m_body->GetCollisionShape();
}
////////////////////////////////////
@ -206,9 +206,9 @@ class CcdPhysicsController : public PHY_IPhysicsController
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()
@ -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
{
@ -245,8 +245,8 @@ class DefaultMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations();
SimdTransform m_worldTransform;
SimdVector3 m_localScaling;
btTransform m_worldTransform;
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 <vector>
class CcdPhysicsController;
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
class TypedConstraint;
class SimulationIslandManager;
class CollisionDispatcher;
class Dispatcher;
class btTypedConstraint;
class btSimulationIslandManager;
class btCollisionDispatcher;
class btDispatcher;
//#include "btBroadphaseInterface.h"
//switch on/off new vehicle support
@ -37,10 +37,10 @@ class Dispatcher;
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class WrapperVehicle;
class PersistentManifold;
class BroadphaseInterface;
class OverlappingPairCache;
class IDebugDraw;
class btPersistentManifold;
class btBroadphaseInterface;
class btOverlappingPairCache;
class btIDebugDraw;
class PHY_IVehicle;
/// 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
class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
{
SimdVector3 m_gravity;
btVector3 m_gravity;
protected:
IDebugDraw* m_debugDrawer;
btIDebugDraw* m_debugDrawer;
//solver iterations
int m_numIterations;
@ -67,12 +67,12 @@ protected:
int m_profileTimings;
bool m_enableSatCollisionDetection;
ContactSolverInfo m_solverInfo;
btContactSolverInfo m_solverInfo;
SimulationIslandManager* m_islandManager;
btSimulationIslandManager* m_islandManager;
public:
CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
CcdPhysicsEnvironment(btDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0);
virtual ~CcdPhysicsEnvironment();
@ -82,7 +82,7 @@ protected:
/// Perform an integration step of duration 'timeStep'.
virtual void setDebugDrawer(IDebugDraw* debugDrawer)
virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
{
m_debugDrawer = debugDrawer;
}
@ -127,12 +127,12 @@ protected:
//Following the COLLADA physics specification for constraints
virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
btTransform& localAttachmentFrameRef,
btTransform& localAttachmentOther,
const btVector3& linearMinLimits,
const btVector3& linearMaxLimits,
const btVector3& angularMinLimits,
const btVector3& angularMaxLimits
);
@ -154,7 +154,7 @@ protected:
}
#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,
float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ);
@ -183,7 +183,7 @@ protected:
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);
class CollisionWorld* GetCollisionWorld()
class btCollisionWorld* GetCollisionWorld()
{
return m_collisionWorld;
}
const class CollisionWorld* GetCollisionWorld() const
const class btCollisionWorld* GetCollisionWorld() const
{
return m_collisionWorld;
}
SimulationIslandManager* GetSimulationIslandManager()
btSimulationIslandManager* GetSimulationIslandManager()
{
return m_islandManager;
}
const SimulationIslandManager* GetSimulationIslandManager() const
const btSimulationIslandManager* GetSimulationIslandManager() const
{
return m_islandManager;
}
class ConstraintSolver* GetConstraintSolver()
class btConstraintSolver* GetConstraintSolver()
{
return m_solver;
}
@ -253,9 +253,9 @@ protected:
std::vector<WrapperVehicle*> m_wrapperVehicles;
class CollisionWorld* m_collisionWorld;
class btCollisionWorld* m_collisionWorld;
class ConstraintSolver* m_solver;
class btConstraintSolver* m_solver;
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++;
//ASSERT(gNumManifold < 65535);
CollisionObject* body0 = (CollisionObject*)b0;
CollisionObject* body1 = (CollisionObject*)b1;
btCollisionObject* body0 = (btCollisionObject*)b0;
btCollisionObject* body1 = (btCollisionObject*)b1;
PersistentManifold* manifold = new PersistentManifold (body0,body1);
btPersistentManifold* manifold = new btPersistentManifold (body0,body1);
m_manifoldsPtr.push_back(manifold);
return manifold;
}
void ParallelIslandDispatcher::ClearManifold(PersistentManifold* manifold)
void ParallelIslandDispatcher::ClearManifold(btPersistentManifold* manifold)
{
manifold->ClearManifold();
}
void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
void ParallelIslandDispatcher::ReleaseManifold(btPersistentManifold* manifold)
{
gNumManifold2--;
@ -82,7 +82,7 @@ void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
ClearManifold(manifold);
std::vector<PersistentManifold*>::iterator i =
std::vector<btPersistentManifold*>::iterator i =
std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
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'!
//
void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback)
void ParallelIslandDispatcher::BuildAndProcessIslands(btCollisionObjectArray& collisionObjects, IslandCallback* callback)
{
int numBodies = collisionObjects.size();
for (int islandId=0;islandId<numBodies;islandId++)
{
std::vector<PersistentManifold*> islandmanifold;
std::vector<btPersistentManifold*> islandmanifold;
//int numSleeping = 0;
@ -115,7 +115,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if (colObj0->GetActivationState()== ACTIVE_TAG)
@ -132,12 +132,12 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
for (i=0;i<GetNumManifolds();i++)
{
PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
btPersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->GetBody0());
btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->GetBody1());
{
if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
@ -153,7 +153,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
colObj0->SetActivationState( ISLAND_SLEEPING );
@ -167,7 +167,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
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++;
CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
CollisionAlgorithmConstructionInfo ci;
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher = this;
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())
{
return new ConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
return new btConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
}
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())
{
return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
return new btCompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
} else
{
if (body1->m_collisionShape->IsCompound())
{
return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
return new btCompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
}
}
//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
bool hasResponse =
(!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &&
(!(colObj1.m_collisionFlags & CollisionObject::noContactResponse));
(!(colObj0.m_collisionFlags & btCollisionObject::noContactResponse)) &&
(!(colObj1.m_collisionFlags & btCollisionObject::noContactResponse));
hasResponse = hasResponse &&
(colObj0.IsActive() || colObj1.IsActive());
return hasResponse;
}
bool ParallelIslandDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
bool ParallelIslandDispatcher::NeedsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
{
CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
assert(body0);
assert(body1);
bool needsCollision = true;
if ((body0->m_collisionFlags & CollisionObject::isStatic) &&
(body1->m_collisionFlags & CollisionObject::isStatic))
if ((body0->m_collisionFlags & btCollisionObject::isStatic) &&
(body1->m_collisionFlags & btCollisionObject::isStatic))
needsCollision = false;
if ((!body0->IsActive()) && (!body1->IsActive()))
@ -266,23 +266,23 @@ bool ParallelIslandDispatcher::NeedsCollision(BroadphaseProxy& proxy0,Broadphase
}
///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.
ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold);
btManifoldResult* manifoldResult = new (&m_defaultManifoldResult) btManifoldResult(obj0,obj1,manifold);
return manifoldResult;
}
///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;
@ -296,7 +296,7 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
for (i=0;i<numPairs;i++)
{
BroadphasePair& pair = pairs[i];
btBroadphasePair& pair = pairs[i];
if (dispatcherId>= 0)
{
@ -310,7 +310,7 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
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);
} else
@ -324,13 +324,13 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
} else
{
//non-persistent algorithm dispatcher
CollisionAlgorithm* algo = FindAlgorithm(
btCollisionAlgorithm* algo = FindAlgorithm(
*pair.m_pProxy0,
*pair.m_pProxy1);
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);
} else

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

@ -24,7 +24,7 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include <vector>
class IDebugDraw;
class btIDebugDraw;
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
@ -35,28 +35,28 @@ class IDebugDraw;
///ParallelIslandDispatcher dispatches entire simulation islands in parallel.
///For both collision detection and constraint solving.
///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;
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:
UnionFind& GetUnionFind() { return m_unionFind;}
btUnionFind& GetUnionFind() { return m_unionFind;}
struct 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();
}
PersistentManifold* GetManifoldByIndexInternal(int index)
btPersistentManifold* GetManifoldByIndexInternal(int index)
{
return m_manifoldsPtr[index];
}
const PersistentManifold* GetManifoldByIndexInternal(int index) const
const btPersistentManifold* GetManifoldByIndexInternal(int index) const
{
return m_manifoldsPtr[index];
}
@ -88,37 +88,37 @@ public:
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
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
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;
}
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 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"
ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, OverlappingPairCache* pairCache):
ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, btOverlappingPairCache* pairCache):
CcdPhysicsEnvironment(dispatcher,pairCache)
{
@ -41,14 +41,14 @@ ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment()
bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{
// Make sure the broadphase / overlapping AABB paircache is up-to-date
OverlappingPairCache* scene = m_collisionWorld->GetPairCache();
btOverlappingPairCache* scene = m_collisionWorld->GetPairCache();
scene->RefreshOverlappingPairs();
// Find the connected sets that can be simulated in parallel
// Using union find
#ifdef USE_QUICKPROF
Profiler::beginBlock("IslandUnionFind");
btProfiler::beginBlock("IslandUnionFind");
#endif //USE_QUICKPROF
GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
@ -58,10 +58,10 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
int numConstraints = m_constraints.size();
for (i=0;i< numConstraints ; i++ )
{
TypedConstraint* constraint = m_constraints[i];
btTypedConstraint* constraint = m_constraints[i];
const RigidBody* colObj0 = &constraint->GetRigidBodyA();
const RigidBody* colObj1 = &constraint->GetRigidBodyB();
const btRigidBody* colObj0 = &constraint->GetRigidBodyA();
const btRigidBody* colObj1 = &constraint->GetRigidBodyB();
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands())))
@ -80,7 +80,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld());
#ifdef USE_QUICKPROF
Profiler::endBlock("IslandUnionFind");
btProfiler::endBlock("IslandUnionFind");
#endif //USE_QUICKPROF
@ -88,7 +88,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
///build simulation islands
#ifdef USE_QUICKPROF
Profiler::beginBlock("BuildIslands");
btProfiler::beginBlock("BuildIslands");
#endif //USE_QUICKPROF
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
@ -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);
CollisionObject* col1 = static_cast<CollisionObject*>(pair->m_pProxy1->m_clientObject);
btCollisionObject* col0 = static_cast<btCollisionObject*>(pair->m_pProxy0->m_clientObject);
btCollisionObject* col1 = static_cast<btCollisionObject*>(pair->m_pProxy1->m_clientObject);
if (col0->m_islandTag1 > col1->m_islandTag1)
{
@ -136,7 +136,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
//store constraint indices for each island
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)
{
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++)
{
PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
btPersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->GetBody0());
btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->GetBody1());
{
int islandTag = colObj0->m_islandTag1;
if (colObj1->m_islandTag1 > islandTag)
@ -169,15 +169,15 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
}
#ifdef USE_QUICKPROF
Profiler::endBlock("BuildIslands");
btProfiler::endBlock("BuildIslands");
#endif //USE_QUICKPROF
#ifdef USE_QUICKPROF
Profiler::beginBlock("SimulateIsland");
btProfiler::beginBlock("SimulateIsland");
#endif //USE_QUICKPROF
TypedConstraint** constraintBase = 0;
btTypedConstraint** constraintBase = 0;
if (m_constraints.size())
constraintBase = &m_constraints[0];
@ -198,7 +198,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
#ifdef USE_QUICKPROF
Profiler::endBlock("SimulateIsland");
btProfiler::endBlock("SimulateIsland");
#endif //USE_QUICKPROF
return true;

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

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

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

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

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

@ -17,9 +17,9 @@ subject to the following restrictions:
#define SIMULATION_ISLAND_H
#include <vector>
class BroadphaseInterface;
class Dispatcher;
class IDebugDraw;
class btBroadphaseInterface;
class btDispatcher;
class btIDebugDraw;
///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
@ -30,12 +30,12 @@ class SimulationIsland
public:
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_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()
@ -47,7 +47,7 @@ class SimulationIsland
void SyncMotionStates(float timeStep);
void UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* broadphase,float timeStep);
void UpdateAabbs(btIDebugDraw* debugDrawer,btBroadphaseInterface* broadphase,float timeStep);
};
#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
class CollisionShape;
class btCollisionShape;
// Properties of dynamic objects
struct PHY_ShapeProps {
@ -29,7 +29,7 @@ struct PHY_ShapeProps {
bool m_do_anisotropic; // Should I do anisotropic friction?
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?
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 vertsB[Hull::kMaxVerts];
@ -563,7 +563,7 @@ void Hull::ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB
#ifdef SHAPE_COLLIDER_USE_CACHING
// update cached pair
if (sep.m_separator != Separation::kFeatureNone)
if (sep.m_separator != btSeparation::kFeatureNone)
{
// re-use the separation
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,
HullContactCollector* hullContactCollector)
{
@ -765,7 +765,7 @@ int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Poin
Vector3 normalWorld = sep.m_axis;
// 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& 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
// 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));
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
// 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)
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 Hull& hullA,
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& hullB((const Hull&)*sep.m_pShapeB->GetShape());
sep.m_separator = Separation::kFeatureNone;
sep.m_separator = btSeparation::kFeatureNone;
sep.m_dist = MinValueF;
sep.m_featureA = sep.m_featureB = -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_dist = minDist;
sep.m_axis = planeWorld.GetNormal();
sep.m_separator = Separation::kFeatureB;
sep.m_separator = btSeparation::kFeatureB;
}
// 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_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_dist = dminA;
sep.m_axis = axis;
sep.m_separator = Separation::kFeatureBoth;
sep.m_separator = btSeparation::kFeatureBoth;
return true;
}
@ -1065,7 +1065,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
// sep.m_featureB = eb;
sep.m_dist = dminA;
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;
//static void ProcessHullHull(Separation& 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);
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 Bounds3 ComputeBounds(const Transform& transform) const;
@ -153,13 +153,13 @@ public:
/// Clips a face to the back of a 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 Hull& hullA,
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,
HullContactCollector* hullContactCollector);

4
Extras/SATConvexCollision/HullContactCollector.h
View File

@ -20,7 +20,7 @@ subject to the following restrictions:
class Vector3;
class Point3;
class Scalar;
struct Separation;
struct btSeparation;
///HullContactCollector collects the Hull computation to the contact point results
class HullContactCollector
@ -29,7 +29,7 @@ public:
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;

2
Extras/SATConvexCollision/Shape.h
View File

@ -26,7 +26,7 @@
struct Separation
struct btSeparation
{
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 "BulletDynamics/Dynamics/btRigidBody.h"
#include "btContactConstraint.h"
#include "Solve2LinearConstraint.h"
#include "btSolve2LinearConstraint.h"
#include "btContactSolverInfo.h"
#include "Dynamics/BU_Joint.h"
#include "Dynamics/ContactJoint.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#define USE_SOR_SOLVER
@ -69,13 +69,13 @@ m_erp(0.4f)
//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_CurJoint = 0;
RigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
btRigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
int numBodies = 0;
BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4];
@ -86,11 +86,11 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
int body0=-1,body1=-1;
PersistentManifold* manifold = manifoldPtr[j];
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->GetNumContacts() > 0)
{
body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies);
body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies);
body0 = ConvertBody((btRigidBody*)manifold->GetBody0(),bodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->GetBody1(),bodies,numBodies);
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)]
void dRfromQ1 (dMatrix3 R, const dQuaternion q)
{
// q = (s,vx,vy,vz)
SimdScalar qq1 = 2.f*q[1]*q[1];
SimdScalar qq2 = 2.f*q[2]*q[2];
SimdScalar qq3 = 2.f*q[3]*q[3];
btScalar qq1 = 2.f*q[1]*q[1];
btScalar qq2 = 2.f*q[2]*q[2];
btScalar qq3 = 2.f*q[3]*q[3];
_R(0,0) = 1.f - qq2 - qq3;
_R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
_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) )
{
@ -194,13 +194,13 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num
static ContactJoint gJointArray[MAX_JOINTS_1];
void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer)
void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
manifold->RefreshContactPoints(((btRigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
@ -209,31 +209,31 @@ void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Join
bool swapBodies = (bodyId0 < 0);
RigidBody* body0,*body1;
btRigidBody* body0,*body1;
if (swapBodies)
{
bodyId0 = _bodyId1;
bodyId1 = _bodyId0;
body0 = (RigidBody*)manifold->GetBody1();
body1 = (RigidBody*)manifold->GetBody0();
body0 = (btRigidBody*)manifold->GetBody1();
body1 = (btRigidBody*)manifold->GetBody0();
} else
{
body0 = (RigidBody*)manifold->GetBody0();
body1 = (RigidBody*)manifold->GetBody1();
body0 = (btRigidBody*)manifold->GetBody0();
body1 = (btRigidBody*)manifold->GetBody1();
}
assert(bodyId0 >= 0);
SimdVector3 color(0,1,0);
btVector3 color(0,1,0);
for (i=0;i<numContacts;i++)
{
if (debugDrawer)
{
const ManifoldPoint& cp = manifold->GetContactPoint(i);
const btManifoldPoint& cp = manifold->GetContactPoint(i);
debugDrawer->DrawContactPoint(
cp.m_positionWorldOnB,

14
Extras/quickstep/OdeConstraintSolver.h
View File

@ -16,14 +16,14 @@ subject to the following restrictions:
#ifndef ODE_CONSTRAINT_SOLVER_H
#define ODE_CONSTRAINT_SOLVER_H
#include "ConstraintSolver.h"
#include "btConstraintSolver.h"
class RigidBody;
class btRigidBody;
class BU_Joint;
/// 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
class OdeConstraintSolver : public ConstraintSolver
class OdeConstraintSolver : public btConstraintSolver
{
private:
@ -34,9 +34,9 @@ private:
float m_erp;
int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies);
void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer);
int ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies);
void ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
public:
@ -44,7 +44,7 @@ public:
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
///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