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 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());
}
}
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));
}
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();
}
//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/");
}
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/");
}
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");
}
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();
}
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);
}
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.
}
}
