static void debugDrawAllBatches(const btBatchedConstraints* bc,
btConstraintArray* constraints,
const btAlignedObjectArray<btSolverBody>& bodies)
{
BT_PROFILE("debugDrawAllBatches");
if (bc && bc->m_debugDrawer && bc->m_phases.size() > 0)
{
btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
btVector3 bboxMax = -bboxMin;
for (int iBody = 0; iBody < bodies.size(); ++iBody)
{
const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
bboxMin.setMin(pos);
bboxMax.setMax(pos);
}
btVector3 bboxExtent = bboxMax - bboxMin;
btVector3 offsetBase = btVector3(0, bboxExtent.y() * 1.1f, 0);
btVector3 offsetStep = btVector3(0, 0, bboxExtent.z() * 1.1f);
int numPhases = bc->m_phases.size();
for (int iPhase = 0; iPhase < numPhases; ++iPhase)
{
float b = float(iPhase) / float(numPhases - 1);
btVector3 color0 = btVector3(1, 0, b);
btVector3 color1 = btVector3(0, 1, b);
btVector3 offset = offsetBase + offsetStep * (float(iPhase) - float(numPhases - 1) * 0.5);
debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
}
}
}
virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime)
{
m_tmpText.m_itemUniqueId = m_uidGenerator++;
m_tmpText.m_lifeTime = lifeTime;
m_tmpText.textSize = size;
int len = strlen(txt);
strcpy(m_tmpText.m_text,txt);
m_tmpText.m_textPositionXYZ[0] = positionXYZ[0];
m_tmpText.m_textPositionXYZ[1] = positionXYZ[1];
m_tmpText.m_textPositionXYZ[2] = positionXYZ[2];
m_tmpText.m_textColorRGB[0] = textColorRGB[0];
m_tmpText.m_textColorRGB[1] = textColorRGB[1];
m_tmpText.m_textColorRGB[2] = textColorRGB[2];
m_cs->lock();
m_cs->setSharedParam(1, eGUIUserDebugAddText);
m_cs->unlock();
while (m_cs->getSharedParam(1) != eGUIHelperIdle)
{
b3Clock::usleep(150);
}
return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
}
void ExampleEntries::initExampleEntries()
{
m_data->m_allExamples.clear();
for (int i=0;i<gAdditionalRegisteredExamples.size();i++)
{
m_data->m_allExamples.push_back(gAdditionalRegisteredExamples[i]);
}
int numDefaultEntries = sizeof(gDefaultExamples)/sizeof(ExampleEntry);
for (int i=0;i<numDefaultEntries;i++)
{
m_data->m_allExamples.push_back(gDefaultExamples[i]);
}
if (m_data->m_allExamples.size()==0)
{
{
ExampleEntry e(0,"Empty");
m_data->m_allExamples.push_back(e);
}
{
ExampleEntry e(1,"Empty","Empty Description", EmptyExample::CreateFunc);
m_data->m_allExamples.push_back(e);
}
}
}
void openFileDemo(const char* filename)
{
deleteDemo();
s_guiHelper= new OpenGLGuiHelper(s_app, sUseOpenGL2);
s_guiHelper->setVisualizerFlagCallback(OpenGLExampleBrowserVisualizerFlagCallback);
s_parameterInterface->removeAllParameters();
CommonExampleOptions options(s_guiHelper,1);
options.m_fileName = filename;
char fullPath[1024];
sprintf(fullPath, "%s", filename);
b3FileUtils::toLower(fullPath);
for (int i=0;i<gFileImporterByExtension.size();i++)
{
if (strstr(fullPath, gFileImporterByExtension[i].m_extension.c_str()))
{
sCurrentDemo = gFileImporterByExtension[i].m_createFunc(options);
}
}
if (sCurrentDemo)
{
sCurrentDemo->initPhysics();
sCurrentDemo->resetCamera();
}
}
void updatePhysicsWorld()
{
static int counter = 0;
// Change wind velocity a bit based on a frame counter
if( (counter % 400) == 0 )
{
_windAngle = (_windAngle + 0.05f);
if( _windAngle > (2*3.141) )
_windAngle = 0;
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
btSoftBody *cloth = 0;
cloth = m_flags[flagIndex];
float localWind = _windAngle + 0.5*(((float(rand())/RAND_MAX))-0.1);
float xCoordinate = cos(localWind)*_windStrength;
float zCoordinate = sin(localWind)*_windStrength;
cloth->setWindVelocity( btVector3(xCoordinate, 0, zCoordinate) );
}
}
//btVector3 origin( capCollider->getWorldTransform().getOrigin() );
//origin.setX( origin.getX() + 0.05 );
//capCollider->getWorldTransform().setOrigin( origin );
counter++;
}
void CcdPhysicsDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//simple dynamics world doesn't handle fixed-time-stepping
//float ms = getDeltaTimeMicroseconds();
///step the simulation
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(1./60.,0);//ms / 1000000.f);
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
}
renderme();
displayText();
#if 0
for (int i=0;i<debugContacts.size();i++)
{
getDynamicsWorld()->getDebugDrawer()->drawContactPoint(debugContacts[i],debugNormals[i],0,0,btVector3(1,0,0));
}
#endif
glFlush();
swapBuffers();
}
void ExitBulletPhysics()
{
//cleanup in the reverse order of creation/initialization
for (int i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0; i--) //remove the rigidbodies from the dynamics world and delete them
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if(body && body->getMotionState())
delete body->getMotionState();
m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
for (int j=0; j<m_collisionShapes.size(); j++) //delete collision shapes
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
}
static inline void add(btAlignedObjectArray<T>& items,const Q& value)
{
for(int i=0,ni=items.size();i<ni;++i)
{
items[i]+=value;
}
}
void MotorDemo::setMotorTargets(btScalar deltaTime)
{
float ms = deltaTime*1000000.;
float minFPS = 1000000.f/60.f;
if (ms > minFPS)
ms = minFPS;
m_Time += ms;
//
// set per-frame sinusoidal position targets using angular motor (hacky?)
//
for (int r=0; r<m_rigs.size(); r++)
{
for (int i=0; i<2*NUM_LEGS; i++)
{
btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_rigs[r]->GetJoints()[i]);
btScalar fCurAngle = hingeC->getHingeAngle();
btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
btScalar fTargetAngle = 0.5 * (1 + sin(2 * M_PI * fTargetPercent));
btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
btScalar fAngleError = fTargetLimitAngle - fCurAngle;
btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
}
}
}
/// removes all objects and shapes from the world
void TerrainDemo::clearWorld(void)
{
//remove the rigidbodies from the dynamics world and delete them
int i;
for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject( obj );
delete obj;
}
//delete collision shapes
for (int j=0;j<m_collisionShapes.size();j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
// delete raw heightfield data
delete m_rawHeightfieldData;
m_rawHeightfieldData = NULL;
}
void doFlags()
{
//float ms = getDeltaTimeMicroseconds();
btScalar dt = (btScalar)m_clock.getTimeMicroseconds();
m_clock.reset();
///step the simulation
if( m_dynamicsWorld )
{
m_dynamicsWorld->stepSimulation(dt/1000000.);
static int frameCount = 0;
frameCount++;
if (frameCount==100)
{
m_dynamicsWorld->stepSimulation(1./60.,0);
CProfileManager::dumpAll();
}
updatePhysicsWorld();
//m_dynamicsWorld->setDebugDrawer(&debugDraw);
//debugDraw.setDebugMode(btIDebugDraw::DBG_DrawWireframe);
//g_solver->copyBackToSoftBodies();
//m_dynamicsWorld->debugDrawWorld();
}
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor );
cloths[flagIndex].draw();
}
}
void render()
{
btScalar childMat[16];
m_bulletObject->getWorldTransform().getOpenGLMatrix(childMat);
if (m_texture)
{
m_texture->initOpenGLTexture();
glBindTexture(GL_TEXTURE_2D,m_texture->m_textureName);
glEnable(GL_TEXTURE_2D);
glDisable(GL_TEXTURE_GEN_S);
glDisable(GL_TEXTURE_GEN_T);
glDisable(GL_TEXTURE_GEN_R);
glBlendFunc(GL_SRC_ALPHA,GL_ONE);
glDepthFunc (GL_LEQUAL);
glDisable(GL_BLEND);
glEnable (GL_DEPTH_TEST);
glMatrixMode(GL_TEXTURE);
glMatrixMode(GL_MODELVIEW);
} else
{
glDisable(GL_TEXTURE_2D);
}
glDisable(GL_LIGHTING);
glPushMatrix();
btglMultMatrix(childMat);
//glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
glBegin(GL_TRIANGLES);
glColor4f(1, 1, 1,1);
for (int i=0;i<m_indices.size();i++)
{
glNormal3f(1.f,0.f,0.f);
glTexCoord2f(m_vertices[m_indices[i]].m_uv1[0],m_vertices[m_indices[i]].m_uv1[1]);
glVertex3f(m_vertices[m_indices[i]].m_localxyz.getX(),m_vertices[m_indices[i]].m_localxyz.getY(),m_vertices[m_indices[i]].m_localxyz.getZ());
}
glEnd();
glPopMatrix();
}
/*
for(int i = 0; i < box_body.size(); ++i) {
glPushMatrix();
btTransform trans;
box_body[i]->getMotionState()->getWorldTransform(trans);
float matrix[16];
trans.getOpenGLMatrix(matrix);
glMultMatrixf(matrix);
glEnableClientState(GL_VERTEX_ARRAY);
// glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
glVertexPointer(3, GL_FLOAT, 0, verticesBox);
// glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vbo[1]);
glColor4f(255.0f, 0.0f, 0.0f, 255.0f);
glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[0]);
glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[4]);
glColor4f(0.0f, 0.0f, 255.0f, 255.0f);
glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[8]);
glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[12]);
glColor4f(0.0f, 255.0f, 0.0f, 255.0f);
glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[16]);
glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[20]);
glDisableClientState(GL_VERTEX_ARRAY);
glPopMatrix();
}
//
}
*/
void renderShootBox(float x, float y, float z, GLuint program, ESMatrix* pMat, ESMatrix* vMat){
//*
for(int i = 0; i < box_body.size(); ++i) {
btTransform trans;
//world->removeRigidBody(body);
//motionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1), btVector3(0,y,0)));
//body->setMotionState(motionState);
//body->setAngularVelocity(btVector3(x,0,z));
box_body[i]->getMotionState()->getWorldTransform(trans);
trans.setRotation(btQuaternion(btVector3(0,0,1),SIMD_PI*0.01*x));
// btTransform groundTransform;
// groundTransform.setIdentity();
// groundTransform.setOrigin( btVector3(0,-10,0) );
// groundTransform.setRotation(btQuaternion(btVector3(0,0,1),SIMD_PI*0.01 * x));
// //delete motionState;
// motionState = new btDefaultMotionState(groundTransform);
// box_body[i]->setMotionState(motionState);
float matrix[16] ;
trans.getOpenGLMatrix(matrix);
ESMatrix mvMat;
ESMatrix mvpMat;
esMatrixMultiply(&mvMat, (ESMatrix*)matrix, vMat);
esMatrixMultiply(&mvpMat, &mvMat, pMat);
glUseProgram(program);
glUniformMatrix4fv( glGetUniformLocation(program, "mvpMat"), 1, false, (GLfloat*)&mvpMat );
glFrontFace(GL_CW);
glVertexAttribPointer(0, 3, GL_FLOAT, false, 0, verticesBox);
glEnableVertexAttribArray(0);
int index = (i)%5;
switch(index){
case 0:glUniform4f( glGetUniformLocation(program, "_Color"), 0.0f, 255.0f, 0.0f, 0.0f); break;
case 1:glUniform4f( glGetUniformLocation(program, "_Color"), 0.0f, 255.0f, 255.0f, 0.0f); break;
case 2:glUniform4f( glGetUniformLocation(program, "_Color"), 255.0f, 0.0f, 255.0f, 0.0f); break;
case 3:glUniform4f( glGetUniformLocation(program, "_Color"), 0.0f, 0.0f, 0.0f, 0.0f); break;
case 4:glUniform4f( glGetUniformLocation(program, "_Color"), 255.0f, 255.0f, 0.0f, 0.0f); break;
}
// glUniform4f( glGetUniformLocation(program, "_Color"), 255.0f, 0.0f, 0.0f, 255.0f);
// glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[0]);
// glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[4]);
// glUniform4f( glGetUniformLocation(program, "_Color"), , 255.0f);
glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[8]);
// glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[12]);
// glUniform4f( glGetUniformLocation(program, "_Color"), , 255.0f);
// glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[16]);
// glDrawElements(GL_TRIANGLE_STRIP, 4, GL_UNSIGNED_SHORT, &indicesBox[20]);
glUseProgram(0);
}
//*/
}
void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
{
const int numbrushes = planeEquations.size();
// brute force:
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++)
{
const btVector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
const btVector3& N3 = planeEquations[k];
btVector3 n2n3; n2n3 = N2.cross(N3);
btVector3 n3n1; n3n1 = N3.cross(N1);
btVector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > btScalar(0.0001) ) &&
( n3n1.length2() > btScalar(0.0001) ) &&
( n1n2.length2() > btScalar(0.0001) ) )
{
//point P out of 3 plane equations:
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.000001))
{
quotient = btScalar(-1.) / quotient;
n2n3 *= N1[3];
n3n1 *= N2[3];
n1n2 *= N3[3];
btVector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}
}
}
}
}
}
}
void MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime)
{
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
}
btScalar timeStep = 1./240.f;
m_dynamicsWorld->stepSimulation(timeStep,0);
static int count = 0;
if ((count& 0x0f)==0)
{
if (m_motor)
{
float force = m_motor->getAppliedImpulse(0)/timeStep;
b3Printf("motor applied force = %f\n", force);
}
for (int i=0;i<m_jointFeedbacks.size();i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}
// Function to remove an object from a vector maintaining correct ordering of the vector
template< typename T > static void removeFromVector( btAlignedObjectArray< T > &vectorToUpdate, int indexToRemove )
{
int currentSize = vectorToUpdate.size();
for( int i = indexToRemove; i < (currentSize-1); ++i )
{
vectorToUpdate[i] = vectorToUpdate[i+1];
}
if( currentSize > 0 )
vectorToUpdate.resize( currentSize - 1 );
}
virtual void flushLines()
{
int sz = m_linePoints.size();
if (sz)
{
float debugColor[4];
debugColor[0] = m_currentLineColor.x();
debugColor[1] = m_currentLineColor.y();
debugColor[2] = m_currentLineColor.z();
debugColor[3] = 1.f;
m_glApp->m_renderer->drawLines(&m_linePoints[0].x,debugColor,
m_linePoints.size(),sizeof(MyDebugVec3),
&m_lineIndices[0],
m_lineIndices.size(),
1);
m_linePoints.clear();
m_lineIndices.clear();
}
}
virtual ~GIM_ConvexDecomposition()
{
int i;
for (i=0;i<m_convexShapes.size();i++)
{
btCollisionShape* shape = m_convexShapes[i];
delete shape;
}
}
bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const
{
//
// validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase
//
int errors = 0;
const int kUnassignedBatch = -1;
btAlignedObjectArray<int> bodyBatchId;
for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase)
{
bodyBatchId.resizeNoInitialize(0);
bodyBatchId.resize(bodies.size(), kUnassignedBatch);
const Range& phase = m_phases[iPhase];
for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
{
const Range& batch = m_batches[iBatch];
for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
{
int iCons = m_constraintIndices[iiCons];
const btSolverConstraint& cons = constraints->at(iCons);
const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA];
const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB];
if (!bodyA.internalGetInvMass().isZero())
{
int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA];
if (thisBodyBatchId == kUnassignedBatch)
{
bodyBatchId[cons.m_solverBodyIdA] = iBatch;
}
else if (thisBodyBatchId != iBatch)
{
btAssert(!"dynamic body is used in 2 different batches in the same phase");
errors++;
}
}
if (!bodyB.internalGetInvMass().isZero())
{
int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB];
if (thisBodyBatchId == kUnassignedBatch)
{
bodyBatchId[cons.m_solverBodyIdB] = iBatch;
}
else if (thisBodyBatchId != iBatch)
{
btAssert(!"dynamic body is used in 2 different batches in the same phase");
errors++;
}
}
}
}
}
return errors == 0;
}
void Planar2D::exitPhysics()
{
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
int i;
if (m_dynamicsWorld)
for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ; i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject( obj );
delete obj;
}
//delete collision shapes
for (int j=0; j<m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
m_collisionShapes.clear();
delete m_dynamicsWorld;
delete m_solver;
delete m_broadphase;
delete m_dispatcher;
delete m_collisionConfiguration;
delete m_convexAlgo2d;
delete m_pdSolver;
delete m_simplexSolver;
delete m_box2dbox2dAlgo;
m_dynamicsWorld = 0;
m_solver = 0;
m_broadphase = 0;
m_dispatcher = 0;
m_collisionConfiguration = 0;
m_convexAlgo2d=0;
m_pdSolver = 0;
m_simplexSolver = 0;
m_box2dbox2dAlgo = 0;
}
void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,btTransform& upAxisTrans, float& unitMeterScaling)
{
upAxisTrans.setIdentity();
unitMeterScaling=1;
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName,"rb");
if (file)
{
int size=0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
{
if (size)
{
printf("Open DAE file of %d bytes\n",size);
Assimp::Importer importer;
//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT);
// importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
aiScene const* scene = importer.ReadFile(relativeFileName,
aiProcess_JoinIdenticalVertices |
//aiProcess_RemoveComponent |
aiProcess_SortByPType |
aiProcess_Triangulate);
if (scene)
{
shape = &visualShapes.expand();
shape->m_scaling[0] = 1;
shape->m_scaling[1] = 1;
shape->m_scaling[2] = 1;
shape->m_scaling[3] = 1;
int index = 0;
shape->m_indices = new b3AlignedObjectArray<int>();
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
aiMatrix4x4 ident;
addMeshParts(scene, scene->mRootNode, shape, ident);
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = visualShapes.size()-1;
}
}
}
}
}
virtual void drawLine(const btVector3& from1,const btVector3& to1,const btVector3& color1)
{
//float from[4] = {from1[0],from1[1],from1[2],from1[3]};
//float to[4] = {to1[0],to1[1],to1[2],to1[3]};
//float color[4] = {color1[0],color1[1],color1[2],color1[3]};
//m_glApp->m_instancingRenderer->drawLine(from,to,color);
if (m_currentLineColor!=color1 || m_linePoints.size() >= BT_LINE_BATCH_SIZE)
{
flushLines();
m_currentLineColor = color1;
}
MyDebugVec3 from(from1);
MyDebugVec3 to(to1);
m_linePoints.push_back(from);
m_linePoints.push_back(to);
m_lineIndices.push_back(m_lineIndices.size());
m_lineIndices.push_back(m_lineIndices.size());
}
void enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
cmd.m_sequenceNumber = m_sequenceNumberGenerator++;
cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds();
if (m_verboseOutput)
{
b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
}
}
void TestHingeTorque::stepSimulation(float deltaTime)
{
if (0)//m_once)
{
m_once=false;
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
btRigidBody& bodyA = hinge->getRigidBodyA();
btTransform trA = bodyA.getWorldTransform();
btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10);
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10);
}
m_dynamicsWorld->stepSimulation(1./240,0);
static int count = 0;
if ((count& 0x0f)==0)
{
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
base->getAngularVelocity()[1],
base->getAngularVelocity()[2]);
btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
child->getAngularVelocity()[1],
child->getAngularVelocity()[2]);
for (int i=0;i<m_jointFeedback.size();i++)
{
b3Printf("Applied force at the COM/Inertial frame B[%d]:(%f,%f,%f), torque B:(%f,%f,%f)\n", i,
m_jointFeedback[i]->m_appliedForceBodyB.x(),
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
}
}
count++;
//CommonRigidBodyBase::stepSimulation(deltaTime);
}
bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
if (planeEquation.dot(N1) > btScalar(0.999))
{
return false;
}
}
return true;
}
SampleJobInterface* consumeJob()
{
SampleJobInterface* job = 0;
m_cs->lock();
int sz = m_jobQueue.size();
if (sz)
{
job = m_jobQueue[sz-1];
m_jobQueue.pop_back();
}
m_cs->unlock();
return job;
}
// TODO: this routine appears to be numerically instable ...
void getVerticesInsidePlanes(const btAlignedObjectArray<btVector3>& planes,
btAlignedObjectArray<btVector3>& verticesOut,
std::set<int>& planeIndicesOut)
{
// Based on btGeometryUtil.cpp (Gino van den Bergen / Erwin Coumans)
verticesOut.resize(0);
planeIndicesOut.clear();
const int numPlanes = planes.size();
int i, j, k, l;
for (i = 0; i < numPlanes; i++)
{
const btVector3& N1 = planes[i];
for (j = i + 1; j < numPlanes; j++)
{
const btVector3& N2 = planes[j];
btVector3 n1n2 = N1.cross(N2);
if (n1n2.length2() > btScalar(0.0001))
{
for (k = j + 1; k < numPlanes; k++)
{
const btVector3& N3 = planes[k];
btVector3 n2n3 = N2.cross(N3);
btVector3 n3n1 = N3.cross(N1);
if ((n2n3.length2() > btScalar(0.0001)) && (n3n1.length2() > btScalar(0.0001) ))
{
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.0001))
{
btVector3 potentialVertex = (n2n3 * N1[3] + n3n1 * N2[3] + n1n2 * N3[3]) * (btScalar(-1.) / quotient);
for (l = 0; l < numPlanes; l++)
{
const btVector3& NP = planes[l];
if (btScalar(NP.dot(potentialVertex))+btScalar(NP[3]) > btScalar(0.000001))
break;
}
if (l == numPlanes)
{
// vertex (three plane intersection) inside all planes
verticesOut.push_back(potentialVertex);
planeIndicesOut.insert(i);
planeIndicesOut.insert(j);
planeIndicesOut.insert(k);
}
}
}
}
}
}
}
}
bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
{
int numvertices = vertices.size();
for (int i=0;i<numvertices;i++)
{
const btVector3& N1 = vertices[i];
btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
if (dist>btScalar(0.))
{
return false;
}
}
return true;
}
bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin)
{
int numbrushes = planeEquations.size();
for (int i = 0; i < numbrushes; i++)
{
const btVector3& N1 = planeEquations[i];
btScalar dist = btScalar(N1.dot(point)) + btScalar(N1[3]) - margin;
if (dist > btScalar(0.))
{
return false;
}
}
return true;
}
void MotorDemo::exitPhysics()
{
int i;
for (i=0;i<m_rigs.size();i++)
{
TestRig* rig = m_rigs[i];
delete rig;
}
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
{
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_dynamicsWorld->removeCollisionObject( obj );
delete obj;
}
//delete collision shapes
for (int j=0;j<m_collisionShapes.size();j++)
{
btCollisionShape* shape = m_collisionShapes[j];
delete shape;
}
//delete dynamics world
delete m_dynamicsWorld;
//delete solver
delete m_solver;
//delete broadphase
delete m_broadphase;
//delete dispatcher
delete m_dispatcher;
delete m_collisionConfiguration;
}
