virtual void GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
{
SimdTransform trans = m_vehicle->GetWheelTransformWS(wheelIndex);
posX = trans.getOrigin().x();
posY = trans.getOrigin().y();
posZ = trans.getOrigin().z();
}
ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr)
:m_manifoldPtr(manifoldPtr)
{
m_transAInv = transA.inverse();
m_transBInv = transB.inverse();
}
virtual void GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
{
SimdTransform trans = m_vehicle->GetWheelTransformWS(wheelIndex);
SimdQuaternion quat = trans.getRotation();
SimdMatrix3x3 orn2(quat);
quatX = trans.getRotation().x();
quatY = trans.getRotation().y();
quatZ = trans.getRotation().z();
quatW = trans.getRotation()[3];
//printf("test");
}
void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
{
SimdTransform rayFromTrans,rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFromWorld);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayToWorld);
//do culling based on aabb (rayFrom/rayTo)
SimdVector3 rayAabbMin = rayFromWorld;
SimdVector3 rayAabbMax = rayFromWorld;
rayAabbMin.setMin(rayToWorld);
rayAabbMax.setMax(rayToWorld);
/// brute force go over all objects. Once there is a broadphase, use that, or
/// add a raycast against aabb first.
std::vector<CollisionObject*>::iterator iter;
for (iter=m_collisionObjects.begin();
!(iter==m_collisionObjects.end()); iter++)
{
CollisionObject* collisionObject= (*iter);
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
//check aabb overlap
if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
{
RayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
collisionObject->m_collisionShape,
collisionObject->m_worldTransform,
resultCallback);
}
}
}
void SyncWheels()
{
int numWheels = GetNumWheels();
int i;
for (i=0;i<numWheels;i++)
{
WheelInfo& info = m_vehicle->GetWheelInfo(i);
PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
m_vehicle->UpdateWheelTransform(i);
SimdTransform trans = m_vehicle->GetWheelTransformWS(i);
SimdQuaternion orn = trans.getRotation();
const SimdVector3& pos = trans.getOrigin();
motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
}
}
void renderme()
{
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
SimdTransform transA;
transA.setIdentity();
float pos[3];
float rot[4];
ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);
SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
transA.setRotation(q);
SimdPoint3 dpos;
dpos.setValue(pos[0],pos[1],pos[2]);
transA.setOrigin( dpos );
transA.getOpenGLMatrix( m );
SimdVector3 wireColor(0.f,0.f,1.f); //wants deactivation
///color differently for active, sleeping, wantsdeactivation states
if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
{
wireColor = SimdVector3 (1.f,0.f,0.f);
}
if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
wireColor = SimdVector3 (0.f,1.f,0.f);
}
char extraDebug[125];
//sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
shapePtr[shapeIndex[i]]->SetExtraDebugInfo(extraDebug);
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[shapeIndex[i]],wireColor,getDebugMode());
}
}
///
/// Debugging method CalcClosest calculates the closest point to the origin, using m_simplexSolver
///
void GL_Simplex1to4::CalcClosest(float* m)
{
SimdTransform tr;
tr.setFromOpenGLMatrix(m);
GL_ShapeDrawer::DrawCoordSystem();
if (m_simplexSolver)
{
m_simplexSolver->reset();
bool res;
SimdVector3 v;
SimdPoint3 pBuf[4];
SimdPoint3 qBuf[4];
SimdPoint3 yBuf[4];
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));
res = m_simplexSolver->closest(v);
int res = m_simplexSolver->getSimplex(pBuf, qBuf, yBuf);
}
//draw v?
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(1.f, 0.f, 0.f);
glVertex3f(0.f, 0.f, 0.f);
glVertex3f(v.x(),v.y(),v.z());
glEnd();
glEnable(GL_LIGHTING);
}
}
void clientMoveAndDisplay()
{
if ( !g_pauseAnim )
{
SimdMatrix3x3 rot;
rot.setEulerZYX( g_animAngle * 0.05, g_animAngle * 0.05, g_animAngle * 0.05 );
SimdTransform t;
t.setIdentity();
t.setBasis( rot );
//g_convexShapesTransform[ 0 ].mult( g_convexShapesTransform[ 0 ], t );
g_convexShapesTransform[ 1 ].mult( g_convexShapesTransform[ 1 ], t );
g_shapesPenetrate = calcPenDepth();
}
clientDisplay();
}
virtual void AddConvexVerticesCollider(std::vector<SimdVector3>& vertices, bool isEntity, const SimdVector3& entityTargetLocation)
{
///perhaps we can do something special with entities (isEntity)
///like adding a collision Triggering (as example)
if (vertices.size() > 0)
{
bool isDynamic = false;
float mass = 0.f;
SimdTransform startTransform;
//can use a shift
startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,0,-10.f));
//this create an internal copy of the vertices
CollisionShape* shape = new ConvexHullShape(&vertices[0],vertices.size());
m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape);
}
}
void LinearConvexCastDemo::initPhysics()
{
setCameraDistance(30.f);
tr[0].setOrigin(SimdVector3(0,0,0));
tr[1].setOrigin(SimdVector3(0,10,0));
SimdMatrix3x3 basisA;
basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
-0.00029313788f,0.99753088f,0.070228584f,
-0.00089153741f,-0.070228823f,0.99753052f);
SimdMatrix3x3 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);
tr[0].setBasis(basisA);
tr[1].setBasis(basisB);
SimdVector3 boxHalfExtentsA(0.2,4,4);
SimdVector3 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));
*/
BoxShape* boxB = new BoxShape(boxHalfExtentsB);
shapePtr[0] = boxA;
shapePtr[1] = boxB;
shapePtr[0]->SetMargin(0.01f);
shapePtr[1]->SetMargin(0.01f);
SimdTransform tr;
tr.setIdentity();
}
int main(int argc,char** argv)
{
setCameraDistance(20.f);
tr[0].setOrigin(SimdVector3(0.0013328250f,8.1363249f,7.0390840f));
tr[1].setOrigin(SimdVector3(0.00000000f,9.1262732f,2.0343180f));
//tr[0].setOrigin(SimdVector3(0,0,0));
//tr[1].setOrigin(SimdVector3(0,10,0));
SimdMatrix3x3 basisA;
basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
-0.00029313788f,0.99753088f,0.070228584f,
-0.00089153741f,-0.070228823f,0.99753052f);
SimdMatrix3x3 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);
tr[0].setBasis(basisA);
tr[1].setBasis(basisB);
SimdVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
SimdVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);
BoxShape boxA(boxHalfExtentsA);
BoxShape boxB(boxHalfExtentsB);
shapePtr[0] = &boxA;
shapePtr[1] = &boxB;
SimdTransform tr;
tr.setIdentity();
return glutmain(argc, argv,screenWidth,screenHeight,"Collision Demo");
}
void PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
//not yet, return box inertia
float margin = GetMargin();
SimdTransform ident;
ident.setIdentity();
SimdVector3 aabbMin,aabbMax;
GetAabb(ident,aabbMin,aabbMax);
SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
SimdScalar lx=2.f*(halfExtents.x()+margin);
SimdScalar ly=2.f*(halfExtents.y()+margin);
SimdScalar lz=2.f*(halfExtents.z()+margin);
const SimdScalar x2 = lx*lx;
const SimdScalar y2 = ly*ly;
const SimdScalar z2 = lz*lz;
const SimdScalar scaledmass = mass * 0.08333333f;
inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
}
Transform GetTransformFromSimdTransform(const SimdTransform& trans)
{
//const SimdVector3& rowA0 = trans.getBasis().getRow(0);
////const SimdVector3& rowA1 = trans.getBasis().getRow(1);
//const SimdVector3& rowA2 = trans.getBasis().getRow(2);
SimdVector3 rowA0 = trans.getBasis().getColumn(0);
SimdVector3 rowA1 = trans.getBasis().getColumn(1);
SimdVector3 rowA2 = trans.getBasis().getColumn(2);
Vector3 x(rowA0.getX(),rowA0.getY(),rowA0.getZ());
Vector3 y(rowA1.getX(),rowA1.getY(),rowA1.getZ());
Vector3 z(rowA2.getX(),rowA2.getY(),rowA2.getZ());
Matrix33 ornA(x,y,z);
Point3 transA(
trans.getOrigin().getX(),
trans.getOrigin().getY(),
trans.getOrigin().getZ());
return Transform(ornA,transA);
}
void ConvexDecompositionDemo::initPhysics(const char* filename)
{
ConvexDecomposition::WavefrontObj wo;
tcount = wo.loadObj(filename);
CollisionDispatcher* dispatcher = new CollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
//OverlappingPairCache* broadphase = new SimpleBroadphase();
m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
m_physicsEnvironmentPtr->setDeactivationTime(2.f);
m_physicsEnvironmentPtr->setGravity(0,-10,0);
SimdTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(0,-4,0));
LocalCreatePhysicsObject(false,0,startTransform,new BoxShape(SimdVector3(30,2,30)));
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
ConvexDecompositionDemo* m_convexDemo;
public:
MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
:m_convexDemo(demo),
mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
TriangleMesh* trimesh = new TriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult\n");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroid.setValue(0,0,0);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
centroid += vertex0;
centroid += vertex1;
centroid += vertex2;
}
}
centroid *= 1.f/(float(result.mHullTcount) * 3);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
trimesh->AddTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
bool isDynamic = true;
float mass = 1.f;
CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh);
SimdTransform trans;
trans.setIdentity();
trans.setOrigin(centroid);
m_convexDemo->LocalCreatePhysicsObject(isDynamic, mass, trans,convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
TriangleMesh* trimesh = new TriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f);
for (int i=0;i<wo.mTriCount;i++)
{
int index0 = wo.mIndices[i*3];
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
trimesh->AddTriangle(vertex0,vertex1,vertex2);
}
CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh);
bool isDynamic = true;
float mass = 1.f;
SimdTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(SimdVector3(20,2,0));
LocalCreatePhysicsObject(isDynamic, mass, startTransform,convexShape);
}
if (tcount)
{
char outputFileName[512];
strcpy(outputFileName,filename);
char *dot = strstr(outputFileName,".");
if ( dot )
*dot = 0;
strcat(outputFileName,"_convex.obj");
FILE* outputFile = fopen(outputFileName,"wb");
unsigned int depth = 7;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.01;
printf("WavefrontObj num triangles read %i",tcount);
ConvexDecomposition::DecompDesc desc;
desc.mVcount = wo.mVertexCount;
desc.mVertices = wo.mVertices;
desc.mTcount = wo.mTriCount;
desc.mIndices = (unsigned int *)wo.mIndices;
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
MyConvexDecomposition convexDecomposition(outputFile,this);
desc.mCallback = &convexDecomposition;
//convexDecomposition.performConvexDecomposition(desc);
ConvexBuilder cb(desc.mCallback);
cb.process(desc);
if (outputFile)
fclose(outputFile);
}
m_physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
}
void clientDisplay(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
int i;
GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.SetCachedSeperatingAxis(seperatingAxis);
PointCollector gjkOutput;
GjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.GetClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
SimdVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
glEnd();
}
for (i=0; i<numObjects; i++)
{
tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());
}
simplex.SetSimplexSolver(&sGjkSimplexSolver);
SimdPoint3 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;
ident.setIdentity();
ident.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,&simplex,SimdVector3(1,1,1),getDebugMode());
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
// pitch += 0.005f;
// yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ)
{
CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
ASSERT(rb0);
SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
SimdVector3 axisInA(axisX,axisY,axisZ);
SimdVector3 axisInB = rb1 ?
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * axisInA;
bool angularOnly = false;
switch (type)
{
case PHY_POINT2POINT_CONSTRAINT:
{
Point2PointConstraint* p2p = 0;
if (rb1)
{
p2p = new Point2PointConstraint(*rb0,
*rb1,pivotInA,pivotInB);
} else
{
p2p = new Point2PointConstraint(*rb0,
pivotInA);
}
m_constraints.push_back(p2p);
p2p->SetUserConstraintId(gConstraintUid++);
p2p->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return p2p->GetUserConstraintId();
break;
}
case PHY_GENERIC_6DOF_CONSTRAINT:
{
Generic6DofConstraint* genericConstraint = 0;
if (rb1)
{
SimdTransform frameInA;
SimdTransform frameInB;
SimdVector3 axis1, axis2;
SimdPlaneSpace1( axisInA, axis1, axis2 );
frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
axisInA.y(), axis1.y(), axis2.y(),
axisInA.z(), axis1.z(), axis2.z() );
SimdPlaneSpace1( axisInB, axis1, axis2 );
frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
axisInB.y(), axis1.y(), axis2.y(),
axisInB.z(), axis1.z(), axis2.z() );
frameInA.setOrigin( pivotInA );
frameInB.setOrigin( pivotInB );
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
} else
{
// TODO: Implement single body case...
}
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
break;
}
case PHY_ANGULAR_CONSTRAINT:
angularOnly = true;
case PHY_LINEHINGE_CONSTRAINT:
{
HingeConstraint* hinge = 0;
if (rb1)
{
hinge = new HingeConstraint(
*rb0,
*rb1,pivotInA,pivotInB,axisInA,axisInB);
} else
{
hinge = new HingeConstraint(*rb0,
pivotInA,axisInA);
}
hinge->setAngularOnly(angularOnly);
m_constraints.push_back(hinge);
hinge->SetUserConstraintId(gConstraintUid++);
hinge->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return hinge->GetUserConstraintId();
break;
}
#ifdef NEW_BULLET_VEHICLE_SUPPORT
case PHY_VEHICLE_CONSTRAINT:
{
RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
RigidBody* chassis = rb0;
DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
m_wrapperVehicles.push_back(wrapperVehicle);
vehicle->SetUserConstraintId(gConstraintUid++);
vehicle->SetUserConstraintType(type);
return vehicle->GetUserConstraintId();
break;
};
#endif //NEW_BULLET_VEHICLE_SUPPORT
default:
{
}
};
//RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
return 0;
}
void Raytracer::displayCallback()
{
updateCamera();
for (int i=0;i<numObjects;i++)
{
transforms[i].setIdentity();
SimdVector3 pos(-3.5f+i*2.5f,0.f,0.f);
transforms[i].setOrigin( pos );
SimdQuaternion orn;
if (i < 2)
{
orn.setEuler(yaw,pitch,roll);
transforms[i].setRotation(orn);
}
}
myMink.SetTransformA(SimdTransform(transforms[0].getRotation()));
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
if (once)
{
glGenTextures(1, &glTextureId);
glBindTexture(GL_TEXTURE_2D,glTextureId );
once = 0;
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
}
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
#define RAYTRACER
#ifdef RAYTRACER
SimdVector4 rgba(1.f,0.f,0.f,0.5f);
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
SimdVector3 rayFrom = getCameraPosition();
SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
SimdVector3 rightOffset;
SimdVector3 vertical(0.f,1.f,0.f);
SimdVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
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(screenWidth);
SimdVector3 dVert = vertical * 1.f/float(screenHeight);
SimdTransform rayFromTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
SimdTransform rayFromLocal;
SimdTransform rayToLocal;
SphereShape pointShape(0.0f);
///clear texture
for (int x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
SimdVector4 rgba(0.f,0.f,0.f,0.f);
raytracePicture->SetPixel(x,y,rgba);
}
}
ConvexCast::CastResult rayResult;
SimdTransform rayToTrans;
rayToTrans.setIdentity();
SimdVector3 rayTo;
for (int x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
rayToTrans.setOrigin(rayTo);
for (int s=0;s<numObjects;s++)
{
// rayFromLocal = transforms[s].inverse()* rayFromTrans;
// rayToLocal = transforms[s].inverse()* rayToTrans;
//choose the continuous collision detection method
SubsimplexConvexCast 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'
// BU_CollisionPair convexCaster(&ptShape,shapePtr[0]);
//reset previous result
rayResult.m_fraction = 1.f;
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
{
//float fog = 1.f - 0.1f * rayResult.m_fraction;
rayResult.m_normal.normalize();
SimdVector3 worldNormal;
worldNormal = transforms[s].getBasis() *rayResult.m_normal;
float light = worldNormal.dot(SimdVector3(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);
raytracePicture->SetPixel(x,y,rgba);
} else
{
//clear is already done
//rgba = SimdVector4(0.f,0.f,0.f,0.f);
//raytracePicture->SetPixel(x,y,rgba);
}
}
}
}
#define TEST_PRINTF
#ifdef TEST_PRINTF
extern BMF_FontData BMF_font_helv10;
raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10);
char buffer[256];
sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects);
raytracePicture->Printf(buffer,&BMF_font_helv10,0,10);
#endif //TEST_PRINTF
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity(); // Reset The Modelview Matrix
glTranslatef(0.0f,0.0f,-3.0f); // Move Into The Screen 5 Units
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,glTextureId );
const unsigned char *ptr = raytracePicture->GetBuffer();
glTexImage2D(GL_TEXTURE_2D,
0,
GL_RGBA,
raytracePicture->GetWidth(),raytracePicture->GetHeight(),
0,
GL_RGBA,
GL_UNSIGNED_BYTE,
ptr);
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f (1,1,1,1); // alpha=0.5=half visible
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex2f(-1,1);
glTexCoord2f(1.0f, 0.0f);
glVertex2f(1,1);
glTexCoord2f(1.0f, 1.0f);
glVertex2f(1,-1);
glTexCoord2f(0.0f, 1.0f);
glVertex2f(-1,-1);
glEnd();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
#endif //RAYRACER
glDisable(GL_TEXTURE_2D);
glDisable(GL_DEPTH_TEST);
GL_ShapeDrawer::DrawCoordSystem();
glPushMatrix();
/*
/// normal opengl rendering
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
transA.getOpenGLMatrix( m );
/// draw the simplex
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1));
/// calculate closest point from simplex to the origin, and draw this vector
simplex.CalcClosest(m);
}
*/
glPopMatrix();
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
{
int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);
switch (num_verts)
{
case 1:
// Touching contact. Yes, we have a collision,
// but no penetration.
return false;
case 2:
{
// 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();
int axis = dir.furthestAxis();
static SimdScalar 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);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
yBuf[2] = pBuf[2] - qBuf[2];
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(aux2*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-aux2)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex(aux3*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex((-aux3)*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[1] = pBuf[4];
qBuf[1] = qBuf[4];
yBuf[1] = yBuf[4];
}
else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[0] = pBuf[4];
qBuf[0] = qBuf[4];
yBuf[0] = yBuf[4];
}
else
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
case 3:
{
// 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);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex((-vv)*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex(vv*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4]))
{
pBuf[3] = pBuf[4];
qBuf[3] = qBuf[4];
yBuf[3] = yBuf[4];
}
else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
}
// We have a tetrahedron inside the Minkowski sum containing
// the origin (if GJK did it's job right ;-)
if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// assert(false);
return false;
}
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);
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))
{
return false;
}
f0->link(0, f1, 2);
f0->link(1, f3, 2);
f0->link(2, f2, 0);
f1->link(0, f2, 2);
f1->link(1, f3, 0);
f2->link(1, f3, 1);
if (num_facets == 0)
{
return false;
}
// at least one facet on the heap.
ReplaceMeEdgeBuffer edgeBuffer(20);
ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY;
do {
facet = facetHeap[0];
std::pop_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp);
--num_facets;
if (!facet->isObsolete())
{
assert(facet->getDist2() > SimdScalar(0.0));
if (num_verts == MaxSupportPoints)
{
#ifdef DEBUG
std::cout << "Ouch, no convergence!!!" << std::endl;
#endif
ASSERT_MESSAGE(false,"Error: pendepth calc failed");
break;
}
pBuf[num_verts] = transformA(convexA->LocalGetSupportingVertex((facet->getClosest())*transformA.getBasis()));
qBuf[num_verts] = transformB(convexB->LocalGetSupportingVertex((-facet->getClosest())*transformB.getBasis()));
yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts];
int index = num_verts++;
SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest());
// Make sure the support mapping is OK.
//assert(far_dist2 > SimdScalar(0.0));
//
// this is to avoid problems with implicit-sphere-touching contact
//
if (far_dist2 < SimdScalar(0.0))
{
return false;
}
GEN_set_min(upper_bound2, (far_dist2 * far_dist2) / facet->getDist2());
if (upper_bound2 <= ReplaceMeAccuracy::depth_tolerance * facet->getDist2()
#define CHECK_NEW_SUPPORT
#ifdef CHECK_NEW_SUPPORT
|| yBuf[index] == yBuf[(*facet)[0]]
|| yBuf[index] == yBuf[(*facet)[1]]
|| yBuf[index] == yBuf[(*facet)[2]]
#endif
)
{
break;
}
// Compute the silhouette cast by the new vertex
// Note that the new vertex is on the positive side
// of the current facet, so the current facet is will
// not be in the convex hull. Start local search
// from this facet.
facet->silhouette(yBuf[index], edgeBuffer);
if (edgeBuffer.empty())
{
return false;
}
ReplaceMeEdgeBuffer::const_iterator it = edgeBuffer.begin();
ReplaceMeFacet *firstFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!firstFacet)
{
break;
}
firstFacet->link(0, (*it).getFacet(), (*it).getIndex());
ReplaceMeFacet *lastFacet = firstFacet;
++it;
for (; it != edgeBuffer.end(); ++it)
{
ReplaceMeFacet *newFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!newFacet)
{
break;
}
if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex()))
{
break;
}
if (!newFacet->link(2, lastFacet, 1))
{
break;
}
lastFacet = newFacet;
}
if (it != edgeBuffer.end())
{
break;
}
firstFacet->link(2, lastFacet, 1);
}
}
while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2);
#ifdef DEBUG
std::cout << "#facets left = " << num_facets << std::endl;
#endif
v = facet->getClosest();
pa = facet->getClosestPoint(pBuf);
pb = facet->getClosestPoint(qBuf);
return true;
}
int main(int argc,char** argv)
{
int i;
for (i=0;i<numObjects;i++)
{
if (i>0)
{
shapePtr[i] = prebuildShapePtr[1];
shapeIndex[i] = 1;//sphere
}
else
{
shapeIndex[i] = 0;
shapePtr[i] = prebuildShapePtr[0];
}
}
ConvexDecomposition::WavefrontObj wo;
char* filename = "file.obj";
tcount = wo.loadObj(filename);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
public:
MyConvexDecomposition (FILE* outputFile)
:mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
TriangleMesh* trimesh = new TriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult\n");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroids[numObjects] = SimdVector3(0,0,0);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
centroids[numObjects] += vertex0;
centroids[numObjects]+= vertex1;
centroids[numObjects]+= vertex2;
}
}
centroids[numObjects] *= 1.f/(float(result.mHullTcount) * 3);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroids[numObjects];
vertex1 -= centroids[numObjects];
vertex2 -= centroids[numObjects];
trimesh->AddTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
shapeIndex[numObjects] = numObjects;
shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
numObjects = 1; //always have the ground object first
TriangleMesh* trimesh = new TriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f);
for (int i=0;i<wo.mTriCount;i++)
{
int index0 = wo.mIndices[i*3];
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
trimesh->AddTriangle(vertex0,vertex1,vertex2);
}
shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh);
}
if (tcount)
{
char outputFileName[512];
strcpy(outputFileName,filename);
char *dot = strstr(outputFileName,".");
if ( dot )
*dot = 0;
strcat(outputFileName,"_convex.obj");
FILE* outputFile = fopen(outputFileName,"wb");
unsigned int depth = 7;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.01;
printf("WavefrontObj num triangles read %i",tcount);
ConvexDecomposition::DecompDesc desc;
desc.mVcount = wo.mVertexCount;
desc.mVertices = wo.mVertices;
desc.mTcount = wo.mTriCount;
desc.mIndices = (unsigned int *)wo.mIndices;
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
MyConvexDecomposition convexDecomposition(outputFile);
desc.mCallback = &convexDecomposition;
//convexDecomposition.performConvexDecomposition(desc);
ConvexBuilder cb(desc.mCallback);
int ret = cb.process(desc);
if (outputFile)
fclose(outputFile);
}
CollisionDispatcher* dispatcher = new CollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
//OverlappingPairCache* broadphase = new SimpleBroadphase();
physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
physicsEnvironmentPtr->setDeactivationTime(2.f);
physicsEnvironmentPtr->setGravity(0,-10,0);
PHY_ShapeProps shapeProps;
shapeProps.m_do_anisotropic = false;
shapeProps.m_do_fh = false;
shapeProps.m_do_rot_fh = false;
shapeProps.m_friction_scaling[0] = 1.;
shapeProps.m_friction_scaling[1] = 1.;
shapeProps.m_friction_scaling[2] = 1.;
shapeProps.m_inertia = 1.f;
shapeProps.m_lin_drag = 0.2f;
shapeProps.m_ang_drag = 0.1f;
shapeProps.m_mass = 10.0f;
PHY_MaterialProps materialProps;
materialProps.m_friction = 10.5f;
materialProps.m_restitution = 0.0f;
CcdConstructionInfo ccdObjectCi;
ccdObjectCi.m_friction = 0.5f;
ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag;
ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag;
SimdTransform tr;
tr.setIdentity();
for (i=0;i<numObjects;i++)
{
shapeProps.m_shape = shapePtr[shapeIndex[i]];
shapeProps.m_shape->SetMargin(0.05f);
bool isDyna = i>0;
//if (i==1)
// isDyna=false;
if (0)//i==1)
{
SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI);
ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
}
if (i>0)
{
switch (i)
{
case 1:
{
ms[i].setWorldPosition(0,10,0);
//for testing, rotate the ground cube so the stack has to recover a bit
break;
}
case 2:
{
ms[i].setWorldPosition(0,8,2);
break;
}
default:
ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0);
}
float quatIma0,quatIma1,quatIma2,quatReal;
SimdQuaternion quat;
SimdVector3 axis(0,0,1);
SimdScalar angle=0.5f;
quat.setRotation(axis,angle);
ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]);
} else
{
ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0);
}
ccdObjectCi.m_MotionState = &ms[i];
ccdObjectCi.m_gravity = SimdVector3(0,0,0);
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
if (!isDyna)
{
shapeProps.m_mass = 0.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
ccdObjectCi.m_collisionFlags = CollisionObject::isStatic;
}
else
{
shapeProps.m_mass = 1.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
ccdObjectCi.m_collisionFlags = 0;
}
SimdVector3 localInertia;
if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
{
//take inertia from first shape
shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
} else
{
shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
}
ccdObjectCi.m_localInertiaTensor = localInertia;
ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]];
physObjects[i]= new CcdPhysicsController( ccdObjectCi);
// Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
//Experimental: better estimation of CCD Time of Impact:
//physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS;
physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]);
if (i==1)
{
//physObjects[i]->SetAngularVelocity(0,0,-2,true);
}
physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
}
//create a constraint
if (createConstraint)
{
//physObjects[i]->SetAngularVelocity(0,0,-2,true);
int constraintId;
float pivotX=CUBE_HALF_EXTENTS,
pivotY=-CUBE_HALF_EXTENTS,
pivotZ=CUBE_HALF_EXTENTS;
float axisX=1,axisY=0,axisZ=0;
HingeConstraint* hinge = 0;
SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 axisInA(0,1,0);
SimdVector3 axisInB(0,-1,0);
RigidBody* rb0 = physObjects[1]->GetRigidBody();
RigidBody* rb1 = physObjects[2]->GetRigidBody();
hinge = new HingeConstraint(
*rb0,
*rb1,pivotInA,pivotInB,axisInA,axisInB);
physicsEnvironmentPtr->m_constraints.push_back(hinge);
hinge->SetUserConstraintId(100);
hinge->SetUserConstraintType(PHY_LINEHINGE_CONSTRAINT);
}
clientResetScene();
setCameraDistance(26.f);
return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/");
}
//to be implemented by the demo
void renderme()
{
debugDrawer.SetDebugMode(getDebugMode());
//render the hinge axis
if (createConstraint)
{
SimdVector3 color(1,0,0);
SimdVector3 dirLocal(0,1,0);
SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA);
SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB);
SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
debugDrawer.DrawLine(from,from+dirWorldA,color);
debugDrawer.DrawLine(fromB,fromB+dirWorldB,color);
}
float m[16];
int i;
if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP)
{
//don't use Bullet, use quickstep
physicsEnvironmentPtr->setSolverType(0);
} else
{
//Bullet LCP solver
physicsEnvironmentPtr->setSolverType(1);
}
if (getDebugMode() & IDebugDraw::DBG_EnableCCD)
{
physicsEnvironmentPtr->setCcdMode(3);
} else
{
physicsEnvironmentPtr->setCcdMode(0);
}
bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison);
physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);
#ifdef USE_HULL
//some testing code for SAT
if (isSatEnabled)
{
for (int s=0;s<numShapes;s++)
{
CollisionShape* shape = shapePtr[s];
if (shape->IsPolyhedral())
{
PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(shape);
if (!polyhedron->m_optionalHull)
{
//first convert vertices in 'Point3' format
int numPoints = polyhedron->GetNumVertices();
Point3* points = new Point3[numPoints+1];
//first 4 points should not be co-planar, so add central point to satisfy MakeHull
points[0] = Point3(0.f,0.f,0.f);
SimdVector3 vertex;
for (int p=0;p<numPoints;p++)
{
polyhedron->GetVertex(p,vertex);
points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ());
}
Hull* hull = Hull::MakeHull(numPoints+1,points);
polyhedron->m_optionalHull = hull;
}
}
}
}
#endif //USE_HULL
for (i=0;i<numObjects;i++)
{
SimdTransform transA;
transA.setIdentity();
float pos[3];
float rot[4];
ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);
SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
transA.setRotation(q);
SimdPoint3 dpos;
dpos.setValue(pos[0],pos[1],pos[2]);
transA.setOrigin( dpos );
transA.getOpenGLMatrix( m );
SimdVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if (i & 1)
{
wireColor = SimdVector3(0.f,0.0f,1.f);
}
///color differently for active, sleeping, wantsdeactivation states
if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
{
if (i & 1)
{
wireColor += SimdVector3 (1.f,0.f,0.f);
} else
{
wireColor += SimdVector3 (.5f,0.f,0.f);
}
}
if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
if (i & 1)
{
wireColor += SimdVector3 (0.f,1.f, 0.f);
} else
{
wireColor += SimdVector3 (0.f,0.5f,0.f);
}
}
char extraDebug[125];
sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug);
GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode());
///this block is just experimental code to show some internal issues with replacing shapes on the fly.
if (getDebugMode()!=0 && (i>0))
{
if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
{
physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]);
//remove the persistent collision pairs that were created based on the previous shape
BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle;
physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy);
SimdVector3 newinertia;
SimdScalar newmass = 10.f;
physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia);
physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia);
physObjects[i]->GetRigidBody()->updateInertiaTensor();
}
}
}
if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText))
{
float xOffset = 10.f;
float yStart = 20.f;
float yIncr = -2.f;
char buf[124];
glColor3f(0, 0, 0);
#ifdef USE_QUICKPROF
if ( getDebugMode() & IDebugDraw::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)
{
char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
}
}
#endif //USE_QUICKPROF
//profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT);
//<< std::endl;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"mouse to interact");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"space to reset");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"cursor keys and z,x to navigate");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"i to toggle simulation, s single step");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"q to quit");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"d to toggle deactivation");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"a to draw temporal AABBs");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"h to toggle help text");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP);
bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD);
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"m Bullet GJK = %i",!isSatEnabled);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"n Bullet LCP = %i",useBulletLCP);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"+- shooting speed = %10.2f",bulletSpeed);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
}
}
int main(int argc,char** argv)
{
CollisionDispatcher* dispatcher = new CollisionDispatcher();
SimdVector3 worldAabbMin(-10000,-10000,-10000);
SimdVector3 worldAabbMax(10000,10000,10000);
BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
//BroadphaseInterface* broadphase = new SimpleBroadphase();
physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
physicsEnvironmentPtr->setDeactivationTime(2.f);
physicsEnvironmentPtr->setGravity(0,-10,0);
PHY_ShapeProps shapeProps;
shapeProps.m_do_anisotropic = false;
shapeProps.m_do_fh = false;
shapeProps.m_do_rot_fh = false;
shapeProps.m_friction_scaling[0] = 1.;
shapeProps.m_friction_scaling[1] = 1.;
shapeProps.m_friction_scaling[2] = 1.;
shapeProps.m_inertia = 1.f;
shapeProps.m_lin_drag = 0.2f;
shapeProps.m_ang_drag = 0.1f;
shapeProps.m_mass = 10.0f;
PHY_MaterialProps materialProps;
materialProps.m_friction = 10.5f;
materialProps.m_restitution = 0.0f;
CcdConstructionInfo ccdObjectCi;
ccdObjectCi.m_friction = 0.5f;
ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag;
ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag;
SimdTransform tr;
tr.setIdentity();
int i;
for (i=0;i<numObjects;i++)
{
if (i>0)
{
shapeIndex[i] = 1;//sphere
}
else
shapeIndex[i] = 0;
}
for (i=0;i<numObjects;i++)
{
shapeProps.m_shape = shapePtr[shapeIndex[i]];
shapeProps.m_shape->SetMargin(0.05f);
bool isDyna = i>0;
//if (i==1)
// isDyna=false;
if (0)//i==1)
{
SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI);
ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
}
if (i>0)
{
switch (i)
{
case 1:
{
ms[i].setWorldPosition(0,10,0);
//for testing, rotate the ground cube so the stack has to recover a bit
break;
}
case 2:
{
ms[i].setWorldPosition(0,8,2);
break;
}
default:
ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0);
}
float quatIma0,quatIma1,quatIma2,quatReal;
SimdQuaternion quat;
SimdVector3 axis(0,0,1);
SimdScalar angle=0.5f;
quat.setRotation(axis,angle);
ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]);
} else
{
ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0);
}
ccdObjectCi.m_MotionState = &ms[i];
ccdObjectCi.m_gravity = SimdVector3(0,0,0);
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
if (!isDyna)
{
shapeProps.m_mass = 0.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
ccdObjectCi.m_collisionFlags = CollisionObject::isStatic;
}
else
{
shapeProps.m_mass = 1.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
ccdObjectCi.m_collisionFlags = 0;
}
SimdVector3 localInertia;
if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
{
//take inertia from first shape
shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
} else
{
shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
}
ccdObjectCi.m_localInertiaTensor = localInertia;
ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]];
physObjects[i]= new CcdPhysicsController( ccdObjectCi);
// Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
//Experimental: better estimation of CCD Time of Impact:
//physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS;
physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]);
if (i==1)
{
//physObjects[i]->SetAngularVelocity(0,0,-2,true);
}
physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
}
clientResetScene();
{
//physObjects[i]->SetAngularVelocity(0,0,-2,true);
int constraintId;
float pivotX=CUBE_HALF_EXTENTS,
pivotY=CUBE_HALF_EXTENTS,
pivotZ=CUBE_HALF_EXTENTS;
float axisX=0,axisY=1,axisZ=0;
constraintId =physicsEnvironmentPtr->createConstraint(
physObjects[1],
//0,
physObjects[2],
////PHY_POINT2POINT_CONSTRAINT,
PHY_GENERIC_6DOF_CONSTRAINT,//can leave any of the 6 degree of freedom 'free' or 'locked'
//PHY_LINEHINGE_CONSTRAINT,
pivotX,pivotY,pivotZ,
axisX,axisY,axisZ
);
}
setCameraDistance(26.f);
return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/");
}
void CollisionWorld::RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans,
CollisionObject* collisionObject,
const CollisionShape* collisionShape,
const SimdTransform& colObjWorldTransform,
RayResultCallback& resultCallback)
{
SphereShape pointShape(0.0f);
if (collisionShape->IsConvex())
{
ConvexCast::CastResult castResult;
castResult.m_fraction = 1.f;//??
ConvexShape* convexShape = (ConvexShape*) collisionShape;
VoronoiSimplexSolver simplexSolver;
SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.length2() > 0.0001f)
{
castResult.m_normal.normalize();
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
CollisionWorld::LocalRayResult localRayResult
(
collisionObject,
0,
castResult.m_normal,
castResult.m_fraction
);
resultCallback.AddSingleResult(localRayResult);
}
}
}
}
else
{
if (collisionShape->IsConcave())
{
TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape;
SimdTransform worldTocollisionObject = colObjWorldTransform.inverse();
SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
//ConvexCast::CastResult
struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback
{
CollisionWorld::RayResultCallback* m_resultCallback;
CollisionObject* m_collisionObject;
TriangleMeshShape* m_triangleMesh;
BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh):
TriangleRaycastCallback(from,to),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
{
}
virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
{
CollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
CollisionWorld::LocalRayResult rayResult
(m_collisionObject,
&shapeInfo,
hitNormalLocal,
hitFraction);
return m_resultCallback->AddSingleResult(rayResult);
}
};
BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
SimdVector3 rayAabbMinLocal = rayFromLocal;
rayAabbMinLocal.setMin(rayToLocal);
SimdVector3 rayAabbMaxLocal = rayFromLocal;
rayAabbMaxLocal.setMax(rayToLocal);
triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
} else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape->IsCompound())
{
const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape);
int i=0;
for (i=0;i<compoundShape->GetNumChildShapes();i++)
{
SimdTransform childTrans = compoundShape->GetChildTransform(i);
const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i);
SimdTransform childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
TriangleMesh* trimesh = new TriangleMesh();
SimdVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult\n");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroid.setValue(0,0,0);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
centroid += vertex0;
centroid += vertex1;
centroid += vertex2;
}
}
centroid *= 1.f/(float(result.mHullTcount) * 3);
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
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]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
trimesh->AddTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
bool isDynamic = true;
float mass = 1.f;
CollisionShape* convexShape = new ConvexTriangleMeshShape(trimesh);
SimdTransform trans;
trans.setIdentity();
trans.setOrigin(centroid);
m_convexDemo->LocalCreatePhysicsObject(isDynamic, mass, trans,convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
void LinearConvexCastDemo::displayCallback(void)
{
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());
}
int shapeIndex = 1;
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[shapeIndex].setRotation(orn);
if (m_stepping || m_singleStep)
{
m_singleStep = false;
pitch += 0.005f;
yaw += 0.01f;
}
SimdVector3 fromA(-25,11,0);
SimdVector3 toA(15,11,0);
SimdQuaternion ornFromA(0.f,0.f,0.f,1.f);
SimdQuaternion ornToA(0.f,0.f,0.f,1.f);
SimdTransform rayFromWorld(ornFromA,fromA);
SimdTransform rayToWorld(ornToA,toA);
tr[0] = rayFromWorld;
if (drawLine)
{
glBegin(GL_LINES);
glColor3f(0, 0, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z());
glEnd();
}
//now perform a raycast on the shapes, in local (shape) space
//choose one of the following lines
for (i=1;i<numObjects;i++)
{
ContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0);
GjkConvexCast 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);
gGjkSimplexSolver.reset();
ConvexCast::CastResult rayResult;
if (convexCaster3.calcTimeOfImpact(rayFromWorld,rayToWorld,tr[i],tr[i],rayResult))
{
glDisable(GL_DEPTH_TEST);
SimdVector3 hitPoint;
hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction);
//draw the raycast result
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(hitPoint.x(),hitPoint.y(),hitPoint.z());
glEnd();
glEnable(GL_DEPTH_TEST);
SimdTransform toTransWorld;
toTransWorld = tr[0];
toTransWorld.setOrigin(hitPoint);
toTransWorld.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,1),getDebugMode());
}
}
glFlush();
glutSwapBuffers();
}
int main(int argc,char** argv)
{
setCameraDistance(30.f);
#define TRISIZE 10.f
#ifdef DEBUG_MESH
SimdVector3 vert0(-TRISIZE ,0,TRISIZE );
SimdVector3 vert1(TRISIZE ,10,TRISIZE );
SimdVector3 vert2(TRISIZE ,0,-TRISIZE );
meshData.AddTriangle(vert0,vert1,vert2);
SimdVector3 vert3(-TRISIZE ,0,TRISIZE );
SimdVector3 vert4(TRISIZE ,0,-TRISIZE );
SimdVector3 vert5(-TRISIZE ,0,-TRISIZE );
meshData.AddTriangle(vert3,vert4,vert5);
#else
#ifdef ODE_MESH
SimdVector3 Size = SimdVector3(15.f,15.f,12.5f);
gVertices[0][0] = -Size[0];
gVertices[0][1] = Size[2];
gVertices[0][2] = -Size[1];
gVertices[1][0] = Size[0];
gVertices[1][1] = Size[2];
gVertices[1][2] = -Size[1];
gVertices[2][0] = Size[0];
gVertices[2][1] = Size[2];
gVertices[2][2] = Size[1];
gVertices[3][0] = -Size[0];
gVertices[3][1] = Size[2];
gVertices[3][2] = Size[1];
gVertices[4][0] = 0;
gVertices[4][1] = 0;
gVertices[4][2] = 0;
gIndices[0] = 0;
gIndices[1] = 1;
gIndices[2] = 4;
gIndices[3] = 1;
gIndices[4] = 2;
gIndices[5] = 4;
gIndices[6] = 2;
gIndices[7] = 3;
gIndices[8] = 4;
gIndices[9] = 3;
gIndices[10] = 0;
gIndices[11] = 4;
int vertStride = sizeof(SimdVector3);
int indexStride = 3*sizeof(int);
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(NUM_TRIANGLES,
gIndices,
indexStride,
NUM_VERTICES,(float*) &gVertices[0].x(),vertStride);
//shapePtr[4] = new TriangleMeshShape(indexVertexArrays);
shapePtr[4] = new BvhTriangleMeshShape(indexVertexArrays);
#else
int vertStride = sizeof(SimdVector3);
int indexStride = 3*sizeof(int);
const int NUM_VERTS_X = 50;
const int NUM_VERTS_Y = 50;
const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
SimdVector3* gVertices = new SimdVector3[totalVerts];
int* gIndices = new int[totalTriangles*3];
int i;
for ( i=0;i<NUM_VERTS_X;i++)
{
for (int j=0;j<NUM_VERTS_Y;j++)
{
gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*10.f,2.f*sinf((float)i)*cosf((float)j),(j-NUM_VERTS_Y*0.5f)*10.f);
}
}
int index=0;
for ( i=0;i<NUM_VERTS_X-1;i++)
{
for (int j=0;j<NUM_VERTS_Y-1;j++)
{
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
}
}
TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts,(float*) &gVertices[0].x(),vertStride);
//shapePtr[4] = new TriangleMeshShape(indexVertexArrays);
shapePtr[4] = new BvhTriangleMeshShape(indexVertexArrays);
#endif
#endif//DEBUG_MESH
// GLDebugDrawer debugDrawer;
//ConstraintSolver* solver = new SimpleConstraintSolver;
ConstraintSolver* solver = new OdeConstraintSolver;
CollisionDispatcher* dispatcher = new CollisionDispatcher();
BroadphaseInterface* broadphase = new SimpleBroadphase();
physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
physicsEnvironmentPtr->setGravity(-1,-10,1);
PHY_ShapeProps shapeProps;
shapeProps.m_do_anisotropic = false;
shapeProps.m_do_fh = false;
shapeProps.m_do_rot_fh = false;
shapeProps.m_friction_scaling[0] = 1.;
shapeProps.m_friction_scaling[1] = 1.;
shapeProps.m_friction_scaling[2] = 1.;
shapeProps.m_inertia = 1.f;
shapeProps.m_lin_drag = 0.95999998f;
shapeProps.m_ang_drag = 0.89999998f;
shapeProps.m_mass = 1.0f;
PHY_MaterialProps materialProps;
materialProps.m_friction = 0.f;// 50.5f;
materialProps.m_restitution = 0.1f;
CcdConstructionInfo ccdObjectCi;
ccdObjectCi.m_friction = 0.f;//50.5f;
ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag;
ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag;
SimdTransform tr;
tr.setIdentity();
for (i=0;i<numObjects;i++)
{
if (i>0)
shapeIndex[i] = 1;//2 = tetrahedron
else
shapeIndex[i] = 4;
}
for (i=0;i<numObjects;i++)
{
shapeProps.m_shape = shapePtr[shapeIndex[i]];
bool isDyna = i>0;
if (!i)
{
//SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI);
//ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
//ms[i].setWorldPosition(0,-10,0);
} else
{
ms[i].setWorldPosition(10,i*15-10,0);
}
//either create a few stacks, to show several islands, or create 1 large stack, showing stability
//ms[i].setWorldPosition((i*5) % 30,i*15-10,0);
ccdObjectCi.m_MotionState = &ms[i];
ccdObjectCi.m_gravity = SimdVector3(0,0,0);
ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
if (!isDyna)
{
shapeProps.m_mass = 0.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
}
else
{
shapeProps.m_mass = 1.f;
ccdObjectCi.m_mass = shapeProps.m_mass;
}
SimdVector3 localInertia;
if (shapeProps.m_mass>0.f)
{
shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
} else
{
localInertia.setValue(0.f,0.f,0.f);
}
ccdObjectCi.m_localInertiaTensor = localInertia;
ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]];
physObjects[i]= new CcdPhysicsController( ccdObjectCi);
physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]);
/* if (i==0)
{
physObjects[i]->SetAngularVelocity(0,0,-2,true);
physObjects[i]->GetRigidBody()->setDamping(0,0);
}
*/
//for the line that represents the AABB extents
// physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);
}
return glutmain(argc, argv,640,480,"Static Concave Mesh Demo");
}
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v )
{
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const SimdScalar marginSqrd = margin * margin;
simplexSolver.reset();
int nbIterations = 0;
while ( true )
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
SimdVector3 seperatingAxisInB = v * transformB.getBasis();
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = transformA( pInA );
SimdPoint3 qWorld = transformB( qInB );
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
{
// Convex shapes do not overlap
// Returning true means that Hybrid's result is ok and there's no need to run EPA
penDepth = 0;
return true;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
//add current vertex to simplex
simplexSolver.addVertex( w, pWorld, qWorld );
//calculate the closest point to the origin (update vector v)
if ( !simplexSolver.closest( v ) )
{
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance )
{
simplexSolver.backup_closest( v );
squaredDistance = v.length2();
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength;
// Returning true means that Hybrid's result is ok and there's no need to run EPA
return true;
}
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
{
// Convex Shapes intersect - we need to run EPA
// Returning false means that Hybrid couldn't do anything for us
// and that we need to run EPA to calculate the pen depth
return false;
}
++nbIterations;
}
}
bool BU_CollisionPair::calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
CastResult& result)
{
SimdVector3 linvelA,angvelA;
SimdVector3 linvelB,angvelB;
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
SimdTransformUtil::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);
result.m_fraction = 1.f;
SimdTransform impactTransA;
SimdTransform impactTransB;
int index=0;
SimdScalar toiUnscaled=result.m_fraction;
const SimdScalar toiUnscaledLimit = result.m_fraction;
SimdTransform a2w;
a2w = fromA;
SimdTransform b2w = fromB;
/* debugging code
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = b2w * pt;
char buf[1000];
if (pt.y() < 0.)
{
sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
} else
{
sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
}
}
*/
SimdTransform b2wp = b2w;
b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
b2wp.setRotation( b2w.getRotation() + angularMotionB);
impactTransB = b2wp;
SimdTransform a2wp;
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
a2wp.setRotation(a2w.getRotation()+angularMotionA);
impactTransA = a2wp;
SimdTransform a2winv;
a2winv = a2w.inverse();
SimdTransform b2wpinv;
b2wpinv = b2wp.inverse();
SimdTransform b2winv;
b2winv = b2w.inverse();
SimdTransform a2wpinv;
a2wpinv = a2wp.inverse();
//Redon's version with concatenated transforms
SimdTransform relative;
relative = b2w * b2wpinv * a2wp * a2winv;
//relative = a2winv * a2wp * b2wpinv * b2w;
SimdQuaternion qrel;
relative.getBasis().getRotation(qrel);
SimdVector3 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]);
if (angvel.length() < SCREWEPSILON)
{
angvel.setValue(0.f,0.f,0.f);
}
//Redon's version with concatenated transforms
m_screwing = BU_Screwing(linvel,angvel);
SimdTransform w2s;
m_screwing.LocalMatrix(w2s);
SimdTransform s2w;
s2w = w2s.inverse();
//impactTransA = a2w;
//impactTransB = b2w;
bool hit = false;
if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
{
//W = 0 , S = 0 , no collision
//toi = 0;
/*
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = impactTransB * pt;
char buf[1000];
if (pt.y() < 0.)
{
sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
else
{
sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
}
}
*/
return false;//don't continue moving within epsilon
}
#define EDGEEDGE
#ifdef EDGEEDGE
BU_EdgeEdge edgeEdge;
//for all edged in A check agains all edges in B
for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
{
SimdPoint3 pA0,pA1;
m_convexA->GetEdge(ea,pA0,pA1);
pA0= a2w * pA0;//in world space
pA0 = w2s * pA0;//in screwing space
pA1= a2w * pA1;//in world space
pA1 = w2s * pA1;//in screwing space
int numedgesB = m_convexB->GetNumEdges();
for (int eb = 0; eb < numedgesB;eb++)
{
{
SimdPoint3 pB0,pB1;
m_convexB->GetEdge(eb,pB0,pB1);
pB0= b2w * pB0;//in world space
pB0 = w2s * pB0;//in screwing space
pB1= b2w * pB1;//in world space
pB1 = w2s * pB1;//in screwing space
SimdScalar lambda,mu;
toiUnscaled = 1.;
SimdVector3 edgeDirA(pA1-pA0);
SimdVector3 edgeDirB(pB1-pB0);
if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
{
//printf("edgeedge potential hit\n");
if (toiUnscaled>=0)
{
if (toiUnscaled < toiUnscaledLimit)
{
//inside check is already done by checking the mu and gamma !
SimdPoint3 vtx = pA0+lambda * (pA1-pA0);
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdPoint3 hitptWorld = s2w * hitpt;
{
if (toiUnscaled < result.m_fraction)
result.m_fraction = toiUnscaled;
hit = true;
SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
hitNormal.normalize();
//an approximated normal can be calculated by taking the cross product of both edges
//take care of the sign !
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
if (dist > 0)
hitNormalWorld *= -1;
//todo: this is the wrong point, because b2winv is still at begin of motion
// not at time-of-impact location!
//bhitpt = b2winv * hitptWorld;
// m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
}
}
}
}
}
index++;
}
};
#endif //EDGEEDGE
#define VERTEXFACE
#ifdef VERTEXFACE
// for all vertices in A, for each face in B,do vertex-face
{
const int numvertsA = m_convexA->GetNumVertices();
for (int v=0;v<numvertsA;v++)
//int v=3;
{
SimdPoint3 vtx;
m_convexA->GetVertex(v,vtx);
vtx = a2w * vtx;//in world space
vtx = w2s * vtx;//in screwing space
const int numplanesB = m_convexB->GetNumPlanes();
for (int p = 0 ; p < numplanesB; p++)
//int p=2;
{
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
m_convexB->GetPlane(planeNorm,planeSupport,p);
planeSupport = b2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm);
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxApolyB;
toiUnscaled = 1.;
if ((p==2) && (v==6))
{
// printf("%f toiUnscaled\n",toiUnscaled);
}
if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
{
if (toiUnscaled >= 0. )
{
//not only collect the first point, get every contactpoint, later we have to check the
//manifold properly!
if (toiUnscaled <= toiUnscaledLimit)
{
// printf("toiUnscaled %f\n",toiUnscaled );
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt;
hitpt = b2winv * hitptWorld;
//vertex has to be 'within' the facet's boundary
if (m_convexB->IsInside(hitpt,m_tolerance))
{
// m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
if (toiUnscaled < result.m_fraction)
result.m_fraction= toiUnscaled;
hit = true;
}
}
}
}
}
index++;
}
}
}
//
// for all vertices in B, for each face in A,do vertex-face
//copy and pasted from all verts A -> all planes B so potential typos!
//todo: make this into one method with a kind of 'swapped' logic
//
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
//int v=0;
{
SimdPoint3 vtx;
m_convexB->GetVertex(v,vtx);
vtx = b2w * vtx;//in world space
/*
char buf[1000];
if (vtx.y() < 0.)
{
sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
*/
vtx = w2s * vtx;//in screwing space
const int numplanesA = m_convexA->GetNumPlanes();
for (int p = 0 ; p < numplanesA; p++)
//int p=2;
{
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
m_convexA->GetPlane(planeNorm,planeSupport,p);
planeSupport = a2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm);
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxBpolyA;
toiUnscaled = 1.;
if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
{
if (toiUnscaled>=0.)
{
if (toiUnscaled < toiUnscaledLimit)
{
SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//SimdScalar len = hitNormal.length()-1;
//assert( SimdFuzzyZero(len) );
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt;
hitpt = a2winv * hitptWorld;
//vertex has to be 'within' the facet's boundary
if (m_convexA->IsInside(hitpt,m_tolerance))
{
// m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
if (toiUnscaled <result.m_fraction)
result.m_fraction = toiUnscaled;
hit = true;
}
}
}
}
}
}
index++;
}
}
#endif// VERTEXFACE
//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
//in addition there are contact points from previous frames
//we have to cleanup the manifold, using an additional epsilon/tolerance
//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
//else throw it away
if (hit)
{
//try to avoid numerical drift on close contact
if (result.m_fraction < 0.00001)
{
// printf("toiUnscaledMin< 0.00001\n");
impactTransA = a2w;
impactTransB = b2w;
} else
{
//SimdScalar 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;
ornB.normalize();
impactTransB.setRotation(ornB);
//now transform A
SimdTransform a2s,a2b;
a2s.mult( w2s , a2w);
a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
a2s.multInverseLeft(w2s,a2s);
a2b.multInverseLeft(b2w, a2s);
//transform by motion B
impactTransA.mult(impactTransB, a2b);
//normalize rotation
SimdQuaternion orn;
impactTransA.getBasis().getRotation(orn);
orn.normalize();
impactTransA.setBasis(SimdMatrix3x3(orn));
}
}
/*
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = impactTransB * pt;
char buf[1000];
if (pt.y() < 0.)
{
sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
else
{
sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
}
}
*/
return hit;
}
