void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) { // only positions need to be updated -- data.m_jacobians and force // directions were set in the ctor and never change. if (m_numDofsFinalized != m_jacSizeBoth) { finalizeMultiDof(); } //don't crash if (m_numDofsFinalized != m_jacSizeBoth) return; if (m_maxAppliedImpulse==0.f) return; // note: we rely on the fact that data.m_jacobians are // always initialized to zero by the Constraint ctor int linkDoF = 0; unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF); // row 0: the lower bound jacobianA(0)[offsetA] = 1; jacobianB(0)[offsetB] = m_gearRatio; const btScalar posError = 0; const btVector3 dummy(0, 0, 0); btScalar erp = infoGlobal.m_erp; btScalar kp = 1; btScalar kd = 1; int numRows = getNumRows(); for (int row=0;row<numRows;row++) { btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); int dof = 0; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar auxVel = 0; if (m_gearAuxLink>=0) { auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; } currentVelocity += auxVel; //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; //btScalar velocityError = (m_desiredVelocity - currentVelocity); btScalar desiredRelativeVelocity = auxVel; fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; { //expect either prismatic or revolute joint type for now btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); switch (m_bodyA->getLink(m_linkA).m_jointType) { case btMultibodyLink::eRevolute: { constraintRow.m_contactNormal1.setZero(); constraintRow.m_contactNormal2.setZero(); btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; break; } case btMultibodyLink::ePrismatic: { btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); constraintRow.m_contactNormal1=prismaticAxisInWorld; constraintRow.m_contactNormal2=-prismaticAxisInWorld; constraintRow.m_relpos1CrossNormal.setZero(); constraintRow.m_relpos2CrossNormal.setZero(); break; } default: { btAssert(0); } }; } } }
MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) { btAssert(0); (void)other; return *this; }
void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) { (void)index; btAssert(m_useQuantization); int curNodeSubPart=-1; //get access info to trianglemesh data const unsigned char *vertexbase = 0; int numverts = 0; PHY_ScalarType type = PHY_INTEGER; int stride = 0; const unsigned char *indexbase = 0; int indexstride = 0; int numfaces = 0; PHY_ScalarType indicestype = PHY_INTEGER; btVector3 triangleVerts[3]; btVector3 aabbMin,aabbMax; const btVector3& meshScaling = meshInterface->getScaling(); int i; for (i=endNode-1; i>=firstNode; i--) { btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; if (curNode.isLeafNode()) { //recalc aabb from triangle data int nodeSubPart = curNode.getPartId(); int nodeTriangleIndex = curNode.getTriangleIndex(); if (nodeSubPart != curNodeSubPart) { if (curNodeSubPart >= 0) meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); curNodeSubPart = nodeSubPart; btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); for (int j=2; j>=0; j--) { int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); triangleVerts[j] = btVector3( graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); } else { double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); } } aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); aabbMin.setMin(triangleVerts[0]); aabbMax.setMax(triangleVerts[0]); aabbMin.setMin(triangleVerts[1]); aabbMax.setMax(triangleVerts[1]); aabbMin.setMin(triangleVerts[2]); aabbMax.setMax(triangleVerts[2]); quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1); } else { //combine aabb from both children btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; { for (int i=0; i<3; i++) { curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i]) curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i]; curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; } } } } if (curNodeSubPart >= 0) meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); }
/** basic algorithm: - convert input aabb to local coordinates (scale down and shift for local origin) - convert input aabb to a range of heightfield grid points (quantize) - iterate over all triangles in that subset of the grid */ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { // scale down the input aabb's so they are in local (non-scaled) coordinates btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); // account for local origin localAabbMin += m_localOrigin; localAabbMax += m_localOrigin; //quantize the aabbMin and aabbMax, and adjust the start/end ranges int quantizedAabbMin[3]; int quantizedAabbMax[3]; quantizeWithClamp(quantizedAabbMin, localAabbMin,0); quantizeWithClamp(quantizedAabbMax, localAabbMax,1); // expand the min/max quantized values // this is to catch the case where the input aabb falls between grid points! for (int i = 0; i < 3; ++i) { quantizedAabbMin[i]--; quantizedAabbMax[i]++; } int startX=0; int endX=m_heightStickWidth-1; int startJ=0; int endJ=m_heightStickLength-1; switch (m_upAxis) { case 0: { if (quantizedAabbMin[1]>startX) startX = quantizedAabbMin[1]; if (quantizedAabbMax[1]<endX) endX = quantizedAabbMax[1]; if (quantizedAabbMin[2]>startJ) startJ = quantizedAabbMin[2]; if (quantizedAabbMax[2]<endJ) endJ = quantizedAabbMax[2]; break; } case 1: { if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0]<endX) endX = quantizedAabbMax[0]; if (quantizedAabbMin[2]>startJ) startJ = quantizedAabbMin[2]; if (quantizedAabbMax[2]<endJ) endJ = quantizedAabbMax[2]; break; }; case 2: { if (quantizedAabbMin[0]>startX) startX = quantizedAabbMin[0]; if (quantizedAabbMax[0]<endX) endX = quantizedAabbMax[0]; if (quantizedAabbMin[1]>startJ) startJ = quantizedAabbMin[1]; if (quantizedAabbMax[1]<endJ) endJ = quantizedAabbMax[1]; break; } default: { //need to get valid m_upAxis btAssert(0); } } for(int j=startJ; j<endJ; j++) { for(int x=startX; x<endX; x++) { btVector3 vertices[3]; if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision && !(j & 1))) { //first triangle getVertex(x,j,vertices[0]); getVertex(x+1,j,vertices[1]); getVertex(x+1,j+1,vertices[2]); callback->processTriangle(vertices,x,j); //second triangle // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman getVertex(x+1,j+1,vertices[1]); getVertex(x,j+1,vertices[2]); callback->processTriangle(vertices,x,j); } else { //first triangle getVertex(x,j,vertices[0]); getVertex(x,j+1,vertices[1]); getVertex(x+1,j,vertices[2]); callback->processTriangle(vertices,x,j); //second triangle getVertex(x+1,j,vertices[0]); //getVertex(x,j+1,vertices[1]); getVertex(x+1,j+1,vertices[2]); callback->processTriangle(vertices,x,j); } } } }
void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const { btAssert(0); }
void SpuContactManifoldCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { btAssert(0); }
void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) #endif { m_cachedSeparatingDistance = 0.f; btScalar distance=btScalar(0.); btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 pointOnA,pointOnB; btTransform localTransA = input.m_transformA; btTransform localTransB = input.m_transformB; btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); btScalar marginA = m_marginA; btScalar marginB = m_marginB; gNumGjkChecks++; #ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("inside gjk\n"); #endif //for CCD we don't use margins if (m_ignoreMargin) { marginA = btScalar(0.); marginB = btScalar(0.); #ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("ignoring margin\n"); #endif } m_curIter = 0; int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? m_cachedSeparatingAxis.setValue(0,1,0); bool isValid = false; bool checkSimplex = false; bool checkPenetration = true; m_degenerateSimplex = 0; m_lastUsedMethod = -1; { btScalar squaredDistance = BT_LARGE_FLOAT; btScalar delta = btScalar(0.); btScalar margin = marginA + marginB; m_simplexSolver->reset(); for ( ; ; ) //while (true) { btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); #if 1 btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); // btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA); // btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB); #else #ifdef __SPU__ btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); #else btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); #ifdef TEST_NON_VIRTUAL btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); btAssert((pInAv-pInA).length() < 0.0001); btAssert((qInBv-qInB).length() < 0.0001); #endif // #endif //__SPU__ #endif btVector3 pWorld = localTransA(pInA); btVector3 qWorld = localTransB(qInB); #ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("got local supporting vertices\n"); #endif if (check2d) { pWorld[2] = 0.f; qWorld[2] = 0.f; } btVector3 w = pWorld - qWorld; delta = m_cachedSeparatingAxis.dot(w); // potential exit, they don't overlap if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) { m_degenerateSimplex = 10; checkSimplex=true; //checkPenetration = false; break; } //exit 0: the new point is already in the simplex, or we didn't come any closer if (m_simplexSolver->inSimplex(w)) { m_degenerateSimplex = 1; checkSimplex = true; break; } // are we getting any closer ? btScalar f0 = squaredDistance - delta; btScalar f1 = squaredDistance * REL_ERROR2; if (f0 <= f1) { if (f0 <= btScalar(0.)) { m_degenerateSimplex = 2; } else { m_degenerateSimplex = 11; } checkSimplex = true; break; } #ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("addVertex 1\n"); #endif //add current vertex to simplex m_simplexSolver->addVertex(w, pWorld, qWorld); #ifdef DEBUG_SPU_COLLISION_DETECTION spu_printf("addVertex 2\n"); #endif btVector3 newCachedSeparatingAxis; //calculate the closest point to the origin (update vector v) if (!m_simplexSolver->closest(newCachedSeparatingAxis)) { m_degenerateSimplex = 3; checkSimplex = true; break; } if(newCachedSeparatingAxis.length2()<REL_ERROR2) { m_cachedSeparatingAxis = newCachedSeparatingAxis; m_degenerateSimplex = 6; checkSimplex = true; break; } btScalar previousSquaredDistance = squaredDistance; squaredDistance = newCachedSeparatingAxis.length2(); #if 0 ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo if (squaredDistance>previousSquaredDistance) { m_degenerateSimplex = 7; squaredDistance = previousSquaredDistance; checkSimplex = false; break; } #endif // //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); //are we getting any closer ? if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) { // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); checkSimplex = true; m_degenerateSimplex = 12; break; } m_cachedSeparatingAxis = newCachedSeparatingAxis; //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject if (m_curIter++ > gGjkMaxIter) { #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION) printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", m_cachedSeparatingAxis.getX(), m_cachedSeparatingAxis.getY(), m_cachedSeparatingAxis.getZ(), squaredDistance, m_minkowskiA->getShapeType(), m_minkowskiB->getShapeType()); #endif break; } bool check = (!m_simplexSolver->fullSimplex()); //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); if (!check) { //do we need this backup_closest here ? // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); m_degenerateSimplex = 13; break; } } if (checkSimplex) { m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = m_cachedSeparatingAxis; btScalar lenSqr =m_cachedSeparatingAxis.length2(); //valid normal if (lenSqr < 0.0001) { m_degenerateSimplex = 5; } if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) { btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); normalInB *= rlen; //normalize btScalar s = btSqrt(squaredDistance); btAssert(s > btScalar(0.0)); pointOnA -= m_cachedSeparatingAxis * (marginA / s); pointOnB += m_cachedSeparatingAxis * (marginB / s); distance = ((btScalar(1.)/rlen) - margin); isValid = true; m_lastUsedMethod = 1; } else { m_lastUsedMethod = 2; } } bool catchDegeneratePenetrationCase = (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); //if (checkPenetration && !isValid) if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) { //penetration case //if there is no way to handle penetrations, bail out if (m_penetrationDepthSolver) { // Penetration depth case. btVector3 tmpPointOnA,tmpPointOnB; gNumDeepPenetrationChecks++; m_cachedSeparatingAxis.setZero(); bool isValid2 = m_penetrationDepthSolver->calcPenDepth( *m_simplexSolver, m_minkowskiA,m_minkowskiB, localTransA,localTransB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, debugDraw ); if (isValid2) { btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; btScalar lenSqr = tmpNormalInB.length2(); if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB = m_cachedSeparatingAxis; lenSqr = m_cachedSeparatingAxis.length2(); } if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { tmpNormalInB /= btSqrt(lenSqr); btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); //only replace valid penetrations when the result is deeper (check) if (!isValid || (distance2 < distance)) { distance = distance2; pointOnA = tmpPointOnA; pointOnB = tmpPointOnB; normalInB = tmpNormalInB; isValid = true; m_lastUsedMethod = 3; } else { m_lastUsedMethod = 8; } } else { m_lastUsedMethod = 9; } } else { ///this is another degenerate case, where the initial GJK calculation reports a degenerate case ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) ///reports a valid positive distance. Use the results of the second GJK instead of failing. ///thanks to Jacob.Langford for the reproduction case ///http://code.google.com/p/bullet/issues/detail?id=250 if (m_cachedSeparatingAxis.length2() > btScalar(0.)) { btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; //only replace valid distances when the distance is less if (!isValid || (distance2 < distance)) { distance = distance2; pointOnA = tmpPointOnA; pointOnB = tmpPointOnB; pointOnA -= m_cachedSeparatingAxis * marginA ; pointOnB += m_cachedSeparatingAxis * marginB ; normalInB = m_cachedSeparatingAxis; normalInB.normalize(); isValid = true; m_lastUsedMethod = 6; } else { m_lastUsedMethod = 5; } } } } } } if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) { #if 0 ///some debugging // if (check2d) { printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]); printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex); } #endif if (m_fixContactNormalDirection) { ///@workaround for sticky convex collisions //in some degenerate cases (usually when the use uses very small margins) //the contact normal is pointing the wrong direction //so fix it now (until we can deal with all degenerate cases in GJK and EPA) //contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A //We like to use a dot product of the normal against the difference of the centroids, //once the centroid is available in the API //until then we use the center of the aabb to approximate the centroid btVector3 aabbMin,aabbMax; m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax); btVector3 posA = (aabbMax+aabbMin)*btScalar(0.5); m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax); btVector3 posB = (aabbMin+aabbMax)*btScalar(0.5); btVector3 diff = posA-posB; if (diff.dot(normalInB) < 0.f) normalInB *= -1.f; } m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; output.addContactPoint( normalInB, pointOnB+positionOffset, distance); } }
void OpenGL2Renderer::renderPhysicsWorld(int numObjects, btCollisionObject** objectArray) { init(); updateCamera(); if (1) { if(m_enableshadows) { glClear(GL_STENCIL_BUFFER_BIT); glEnable(GL_CULL_FACE); GLint err = glGetError(); btAssert(err==GL_NO_ERROR); renderscene(0,numObjects,objectArray); err = glGetError(); btAssert(err==GL_NO_ERROR); glDisable(GL_LIGHTING); glDepthMask(GL_FALSE); glDepthFunc(GL_LEQUAL); glEnable(GL_STENCIL_TEST); glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE); glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL); glFrontFace(GL_CCW); glStencilOp(GL_KEEP,GL_KEEP,GL_INCR); renderscene(1,numObjects,objectArray); glFrontFace(GL_CW); glStencilOp(GL_KEEP,GL_KEEP,GL_DECR); renderscene(1,numObjects,objectArray); glFrontFace(GL_CCW); err = glGetError(); btAssert(err==GL_NO_ERROR); glPolygonMode(GL_FRONT,GL_FILL); glPolygonMode(GL_BACK,GL_FILL); glShadeModel(GL_SMOOTH); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LESS); glEnable(GL_LIGHTING); glDepthMask(GL_TRUE); glCullFace(GL_BACK); glFrontFace(GL_CCW); glEnable(GL_CULL_FACE); glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE); glDepthFunc(GL_LEQUAL); glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL ); glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP ); glDisable(GL_LIGHTING); renderscene(2,numObjects,objectArray); glEnable(GL_LIGHTING); glDepthFunc(GL_LESS); glDisable(GL_STENCIL_TEST); glDisable(GL_CULL_FACE); err = glGetError(); btAssert(err==GL_NO_ERROR); } else { glDisable(GL_CULL_FACE); renderscene(0,numObjects,objectArray); } int xOffset = 10; int yStart = 20; int yIncr = 20; glDisable(GL_LIGHTING); glColor3f(0, 0, 0); if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0) { setOrthographicProjection(); //showProfileInfo(xOffset,yStart,yIncr); #if 0//#ifdef USE_QUICKPROF if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings) { static int counter = 0; counter++; std::map<std::string, hidden::ProfileBlock*>::iterator iter; for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter) { char blockTime[128]; sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT)); glRasterPos3f(xOffset,yStart,0); GLDebugDrawString(BMF_GetFont(BMF_kHelvetica10),blockTime); yStart += yIncr; } } #endif //USE_QUICKPROF resetPerspectiveProjection(); } glDisable(GL_LIGHTING); } updateCamera(); }
void OpenGL2Renderer::updateCamera() { GLint err = glGetError(); btAssert(err==GL_NO_ERROR); glMatrixMode(GL_PROJECTION); glLoadIdentity(); btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg btQuaternion rot(m_cameraUp,razi); btVector3 eyePos(0,0,0); eyePos[m_forwardAxis] = -m_cameraDistance; btVector3 forward(eyePos[0],eyePos[1],eyePos[2]); if (forward.length2() < SIMD_EPSILON) { forward.setValue(1.f,0.f,0.f); } btVector3 right = m_cameraUp.cross(forward); btQuaternion roll(right,-rele); err = glGetError(); btAssert(err==GL_NO_ERROR); eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos; m_cameraPosition[0] = eyePos.getX(); m_cameraPosition[1] = eyePos.getY(); m_cameraPosition[2] = eyePos.getZ(); m_cameraPosition += m_cameraTargetPosition; if (m_openglViewportWidth == 0 && m_openglViewportHeight == 0) return; btScalar aspect; btVector3 extents; aspect = m_openglViewportWidth / (btScalar)m_openglViewportHeight; extents.setValue(aspect * 1.0f, 1.0f,0); if (m_ortho) { // reset matrix glLoadIdentity(); extents *= m_cameraDistance; btVector3 lower = m_cameraTargetPosition - extents; btVector3 upper = m_cameraTargetPosition + extents; glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); } else { glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2], m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2], m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ()); } err = glGetError(); btAssert(err==GL_NO_ERROR); }
void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { (void)aabbMin; (void)aabbMax; int numtotalphysicsverts = 0; int part, graphicssubparts = getNumSubParts(); const unsigned char * vertexbase; const unsigned char * indexbase; int indexstride; PHY_ScalarType type; PHY_ScalarType gfxindextype; int stride, numverts, numtriangles; int gfxindex; btVector3 triangle[3]; btVector3 meshScaling = getScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype for (part=0; part<graphicssubparts ; part++) { getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); numtotalphysicsverts+=numtriangles*3; //upper bound ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build ///so disable this feature by default ///see patch http://code.google.com/p/bullet/issues/detail?id=213 switch (type) { case PHY_FLOAT: { float* graphicsbase; switch (gfxindextype) { case PHY_INTEGER: { for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } case PHY_SHORT: { for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } case PHY_UCHAR: { for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue(graphicsbase[0]*meshScaling.getX(), graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } default: btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); } break; } case PHY_DOUBLE: { double* graphicsbase; switch (gfxindextype) { case PHY_INTEGER: { for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } case PHY_SHORT: { for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } case PHY_UCHAR: { for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(), (btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } default: btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); } break; } default: btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } }
void OpenGL2Renderer::renderscene(int pass, int numObjects, btCollisionObject** objArray) { GLint err = glGetError(); btAssert(err==GL_NO_ERROR); ATTRIBUTE_ALIGNED16(btScalar) m[16]; btMatrix3x3 rot;rot.setIdentity(); btVector3 wireColor(1,0,0); for(int i=0;i<numObjects;i++) { const btCollisionObject* colObj=objArray[i]; const btRigidBody* body=btRigidBody::upcast(colObj); if(body&&body->getMotionState()) { btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState(); myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m); rot=myMotionState->m_graphicsWorldTrans.getBasis(); } else { colObj->getWorldTransform().getOpenGLMatrix(m); rot=colObj->getWorldTransform().getBasis(); } btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation if(i&1) wireColor=btVector3(0.f,0.0f,1.f); ///color differently for active, sleeping, wantsdeactivation states if (colObj->getActivationState() == 1) //active { if (i & 1) { wireColor += btVector3 (1.f,0.f,0.f); } else { wireColor += btVector3 (.5f,0.f,0.f); } } if(colObj->getActivationState()==2) //ISLAND_SLEEPING { if(i&1) { wireColor += btVector3 (0.f,1.f, 0.f); } else { wireColor += btVector3 (0.f,0.5f,0.f); } } err = glGetError(); btAssert(err==GL_NO_ERROR); btVector3 aabbMin(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); btVector3 aabbMax(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); //world->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax); //aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); //aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); // printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ()); // printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ()); // m_dynamicsWorld->getDebugDrawer()->drawAabb(aabbMin,aabbMax,btVector3(1,1,1)); if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe)) { switch(pass) { case 0: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),aabbMin,aabbMax);break; case 1: m_shapeDrawer->drawShadow(m,m_sundirection*rot,colObj->getCollisionShape(),aabbMin,aabbMax);break; case 2: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor*btScalar(0.3),0,aabbMin,aabbMax);break; } } } }
///fills the dataBuffer and returns the struct name (and 0 on failure) const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const { btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer; trimeshData->m_numMeshParts = getNumSubParts(); //void* uniquePtr = 0; trimeshData->m_meshPartsPtr = 0; if (trimeshData->m_numMeshParts) { btChunk* chunk = serializer->allocate(sizeof(btMeshPartData), trimeshData->m_numMeshParts); btMeshPartData* memPtr = (btMeshPartData*)chunk->m_oldPtr; trimeshData->m_meshPartsPtr = (btMeshPartData *)serializer->getUniquePointer(memPtr); // int numtotalphysicsverts = 0; int part, graphicssubparts = getNumSubParts(); const unsigned char * vertexbase; const unsigned char * indexbase; int indexstride; PHY_ScalarType type; PHY_ScalarType gfxindextype; int stride, numverts, numtriangles; int gfxindex; // btVector3 triangle[3]; // btVector3 meshScaling = getScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype for (part=0; part<graphicssubparts ; part++, memPtr++) { getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); memPtr->m_numTriangles = numtriangles;//indices = 3*numtriangles memPtr->m_numVertices = numverts; memPtr->m_indices16 = 0; memPtr->m_indices32 = 0; memPtr->m_3indices16 = 0; memPtr->m_3indices8 = 0; memPtr->m_vertices3f = 0; memPtr->m_vertices3d = 0; switch (gfxindextype) { case PHY_INTEGER: { int numindices = numtriangles*3; if (numindices) { btChunk* chunk = serializer->allocate(sizeof(btIntIndexData), numindices); btIntIndexData* tmpIndices = (btIntIndexData*)chunk->m_oldPtr; memPtr->m_indices32 = (btIntIndexData*)serializer->getUniquePointer(tmpIndices); for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); tmpIndices[gfxindex*3].m_value = tri_indices[0]; tmpIndices[gfxindex*3+1].m_value = tri_indices[1]; tmpIndices[gfxindex*3+2].m_value = tri_indices[2]; } serializer->finalizeChunk(chunk, "btIntIndexData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } case PHY_SHORT: { if (numtriangles) { btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData), numtriangles); btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr; memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices); for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; } serializer->finalizeChunk(chunk, "btShortIntIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } case PHY_UCHAR: { if (numtriangles) { btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData), numtriangles); btCharIndexTripletData* tmpIndices = (btCharIndexTripletData*)chunk->m_oldPtr; memPtr->m_3indices8 = (btCharIndexTripletData*) serializer->getUniquePointer(tmpIndices); for (gfxindex=0; gfxindex<numtriangles; gfxindex++) { unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; } serializer->finalizeChunk(chunk, "btCharIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } default: { btAssert(0); //unknown index type } } switch (type) { case PHY_FLOAT: { float* graphicsbase; if (numverts) { btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData), numverts); btVector3FloatData* tmpVertices = (btVector3FloatData*) chunk->m_oldPtr; memPtr->m_vertices3f = (btVector3FloatData *)serializer->getUniquePointer(tmpVertices); for (int i=0; i<numverts; i++) { graphicsbase = (float*)(vertexbase+i*stride); tmpVertices[i].m_floats[0] = graphicsbase[0]; tmpVertices[i].m_floats[1] = graphicsbase[1]; tmpVertices[i].m_floats[2] = graphicsbase[2]; } serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } case PHY_DOUBLE: { if (numverts) { btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData), numverts); btVector3DoubleData* tmpVertices = (btVector3DoubleData*) chunk->m_oldPtr; memPtr->m_vertices3d = (btVector3DoubleData *) serializer->getUniquePointer(tmpVertices); for (int i=0; i<numverts; i++) { double* graphicsbase = (double*)(vertexbase+i*stride);//for now convert to float, might leave it at double tmpVertices[i].m_floats[0] = graphicsbase[0]; tmpVertices[i].m_floats[1] = graphicsbase[1]; tmpVertices[i].m_floats[2] = graphicsbase[2]; } serializer->finalizeChunk(chunk, "btVector3DoubleData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } default: btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } serializer->finalizeChunk(chunk, "btMeshPartData", BT_ARRAY_CODE, chunk->m_oldPtr); } m_scaling.serializeFloat(trimeshData->m_scaling); return "btStridingMeshInterfaceData"; }
void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) { btAssert(!m_useSolveConstraintObsolete); int i, s = info->rowskip; // transforms in world space btTransform trA = transA*m_rbAFrame; btTransform trB = transB*m_rbBFrame; // pivot point btVector3 pivotAInW = trA.getOrigin(); btVector3 pivotBInW = trB.getOrigin(); #if 1 // difference between frames in WCS btVector3 ofs = trB.getOrigin() - trA.getOrigin(); // now get weight factors depending on masses btScalar miA = getRigidBodyA().getInvMass(); btScalar miB = getRigidBodyB().getInvMass(); bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); btScalar miS = miA + miB; btScalar factA, factB; if(miS > btScalar(0.f)) { factA = miB / miS; } else { factA = btScalar(0.5f); } factB = btScalar(1.0f) - factA; // get the desired direction of hinge axis // as weighted sum of Z-orthos of frameA and frameB in WCS btVector3 ax1A = trA.getBasis().getColumn(2); btVector3 ax1B = trB.getBasis().getColumn(2); btVector3 ax1 = ax1A * factA + ax1B * factB; ax1.normalize(); // fill first 3 rows // we want: velA + wA x relA == velB + wB x relB btTransform bodyA_trans = transA; btTransform bodyB_trans = transB; int s0 = 0; int s1 = s; int s2 = s * 2; int nrow = 2; // last filled row btVector3 tmpA, tmpB, relA, relB, p, q; // get vector from bodyB to frameB in WCS relB = trB.getOrigin() - bodyB_trans.getOrigin(); // get its projection to hinge axis btVector3 projB = ax1 * relB.dot(ax1); // get vector directed from bodyB to hinge axis (and orthogonal to it) btVector3 orthoB = relB - projB; // same for bodyA relA = trA.getOrigin() - bodyA_trans.getOrigin(); btVector3 projA = ax1 * relA.dot(ax1); btVector3 orthoA = relA - projA; btVector3 totalDist = projA - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to hinge axis p = orthoB * factA + orthoA * factB; btScalar len2 = p.length2(); if(len2 > SIMD_EPSILON) { p /= btSqrt(len2); } else { p = trA.getBasis().getColumn(1); } // make one more ortho q = ax1.cross(p); // fill three rows tmpA = relA.cross(p); tmpB = relB.cross(p); for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; tmpA = relA.cross(q); tmpB = relB.cross(q); if(hasStaticBody && getSolveLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); if(hasStaticBody) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation tmpB *= factB; tmpA *= factA; } for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; btScalar k = info->fps * info->erp; if (!m_angularOnly) { for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; // compute three elements of right hand side btScalar rhs = k * p.dot(ofs); info->m_constraintError[s0] = rhs; rhs = k * q.dot(ofs); info->m_constraintError[s1] = rhs; rhs = k * ax1.dot(ofs); info->m_constraintError[s2] = rhs; } // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. int s3 = 3 * s; int s4 = 4 * s; info->m_J1angularAxis[s3 + 0] = p[0]; info->m_J1angularAxis[s3 + 1] = p[1]; info->m_J1angularAxis[s3 + 2] = p[2]; info->m_J1angularAxis[s4 + 0] = q[0]; info->m_J1angularAxis[s4 + 1] = q[1]; info->m_J1angularAxis[s4 + 2] = q[2]; info->m_J2angularAxis[s3 + 0] = -p[0]; info->m_J2angularAxis[s3 + 1] = -p[1]; info->m_J2angularAxis[s3 + 2] = -p[2]; info->m_J2angularAxis[s4 + 0] = -q[0]; info->m_J2angularAxis[s4 + 1] = -q[1]; info->m_J2angularAxis[s4 + 2] = -q[2]; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. k = info->fps * info->erp; btVector3 u = ax1A.cross(ax1B); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); #endif // check angular limits nrow = 4; // last filled row int srow; btScalar limit_err = btScalar(0.0); int limit = 0; if(getSolveLimit()) { #ifdef _BT_USE_CENTER_LIMIT_ limit_err = m_limit.getCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row int powered = 0; if(getEnableAngularMotor()) { powered = 1; } if(limit || powered) { nrow++; srow = nrow * info->rowskip; info->m_J1angularAxis[srow+0] = ax1[0]; info->m_J1angularAxis[srow+1] = ax1[1]; info->m_J1angularAxis[srow+2] = ax1[2]; info->m_J2angularAxis[srow+0] = -ax1[0]; info->m_J2angularAxis[srow+1] = -ax1[1]; info->m_J2angularAxis[srow+2] = -ax1[2]; btScalar lostop = getLowerLimit(); btScalar histop = getUpperLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective powered = 0; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; if(powered) { if(m_flags & BT_HINGE_FLAGS_CFM_NORM) { info->cfm[srow] = m_normalCFM; } btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; info->m_lowerLimit[srow] = - m_maxMotorImpulse; info->m_upperLimit[srow] = m_maxMotorImpulse; } if(limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; if(m_flags & BT_HINGE_FLAGS_CFM_STOP) { info->cfm[srow] = m_stopCFM; } if(lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } else if(limit == 1) { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } else { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #ifdef _BT_USE_CENTER_LIMIT_ btScalar bounce = m_limit.getRelaxationFactor(); #else btScalar bounce = m_relaxationFactor; #endif if(bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); vel -= angVelB.dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if(limit == 1) { // low limit if(vel < 0) { btScalar newc = -bounce * vel; if(newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else { // high limit - all those computations are reversed if(vel > 0) { btScalar newc = -bounce * vel; if(newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } #ifdef _BT_USE_CENTER_LIMIT_ info->m_constraintError[srow] *= m_limit.getBiasFactor(); #else info->m_constraintError[srow] *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered }
void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) { btAssert(!m_useSolveConstraintObsolete); int i, skip = info->rowskip; // transforms in world space btTransform trA = transA*m_rbAFrame; btTransform trB = transB*m_rbBFrame; // pivot point btVector3 pivotAInW = trA.getOrigin(); btVector3 pivotBInW = trB.getOrigin(); #if 0 if (0) { for (i=0; i<6; i++) { info->m_J1linearAxis[i*skip]=0; info->m_J1linearAxis[i*skip+1]=0; info->m_J1linearAxis[i*skip+2]=0; info->m_J1angularAxis[i*skip]=0; info->m_J1angularAxis[i*skip+1]=0; info->m_J1angularAxis[i*skip+2]=0; info->m_J2angularAxis[i*skip]=0; info->m_J2angularAxis[i*skip+1]=0; info->m_J2angularAxis[i*skip+2]=0; info->m_constraintError[i*skip]=0.f; } } #endif //#if 0 // linear (all fixed) if (!m_angularOnly) { info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[skip + 1] = 1; info->m_J1linearAxis[2 * skip + 2] = 1; } btVector3 a1 = pivotAInW - transA.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); btVector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } btVector3 a2 = pivotBInW - transB.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); a2.getSkewSymmetricMatrix(angular0,angular1,angular2); } // linear RHS btScalar k = info->fps * info->erp; if (!m_angularOnly) { for(i = 0; i < 3; i++) { info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); } } // make rotations around X and Y equal // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. // get hinge axis (Z) btVector3 ax1 = trA.getBasis().getColumn(2); // get 2 orthos to hinge axis (X, Y) btVector3 p = trA.getBasis().getColumn(0); btVector3 q = trA.getBasis().getColumn(1); // set the two hinge angular rows int s3 = 3 * info->rowskip; int s4 = 4 * info->rowskip; info->m_J1angularAxis[s3 + 0] = p[0]; info->m_J1angularAxis[s3 + 1] = p[1]; info->m_J1angularAxis[s3 + 2] = p[2]; info->m_J1angularAxis[s4 + 0] = q[0]; info->m_J1angularAxis[s4 + 1] = q[1]; info->m_J1angularAxis[s4 + 2] = q[2]; info->m_J2angularAxis[s3 + 0] = -p[0]; info->m_J2angularAxis[s3 + 1] = -p[1]; info->m_J2angularAxis[s3 + 2] = -p[2]; info->m_J2angularAxis[s4 + 0] = -q[0]; info->m_J2angularAxis[s4 + 1] = -q[1]; info->m_J2angularAxis[s4 + 2] = -q[2]; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1,ax2 are the unit length hinge axes as computed from body1 and // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). // if `theta' is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. btVector3 ax2 = trB.getBasis().getColumn(2); btVector3 u = ax1.cross(ax2); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); // check angular limits int nrow = 4; // last filled row int srow; btScalar limit_err = btScalar(0.0); int limit = 0; if(getSolveLimit()) { #ifdef _BT_USE_CENTER_LIMIT_ limit_err = m_limit.getCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row int powered = 0; if(getEnableAngularMotor()) { powered = 1; } if(limit || powered) { nrow++; srow = nrow * info->rowskip; info->m_J1angularAxis[srow+0] = ax1[0]; info->m_J1angularAxis[srow+1] = ax1[1]; info->m_J1angularAxis[srow+2] = ax1[2]; info->m_J2angularAxis[srow+0] = -ax1[0]; info->m_J2angularAxis[srow+1] = -ax1[1]; info->m_J2angularAxis[srow+2] = -ax1[2]; btScalar lostop = getLowerLimit(); btScalar histop = getUpperLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective powered = 0; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; if(powered) { if(m_flags & BT_HINGE_FLAGS_CFM_NORM) { info->cfm[srow] = m_normalCFM; } btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; info->m_lowerLimit[srow] = - m_maxMotorImpulse; info->m_upperLimit[srow] = m_maxMotorImpulse; } if(limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; if(m_flags & BT_HINGE_FLAGS_CFM_STOP) { info->cfm[srow] = m_stopCFM; } if(lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } else if(limit == 1) { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } else { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #ifdef _BT_USE_CENTER_LIMIT_ btScalar bounce = m_limit.getRelaxationFactor(); #else btScalar bounce = m_relaxationFactor; #endif if(bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); vel -= angVelB.dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if(limit == 1) { // low limit if(vel < 0) { btScalar newc = -bounce * vel; if(newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else { // high limit - all those computations are reversed if(vel > 0) { btScalar newc = -bounce * vel; if(newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } #ifdef _BT_USE_CENTER_LIMIT_ info->m_constraintError[srow] *= m_limit.getBiasFactor(); #else info->m_constraintError[srow] *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered }
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) { // int i=1; int numDim = BTMBP2PCONSTRAINT_DIM; for (int i=0;i<numDim;i++) { btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint)); constraintRow.m_relpos1CrossNormal.setValue(0,0,0); constraintRow.m_contactNormal1.setValue(0,0,0); constraintRow.m_relpos2CrossNormal.setValue(0,0,0); constraintRow.m_contactNormal2.setValue(0,0,0); constraintRow.m_angularComponentA.setValue(0,0,0); constraintRow.m_angularComponentB.setValue(0,0,0); constraintRow.m_solverBodyIdA = data.m_fixedBodyId; constraintRow.m_solverBodyIdB = data.m_fixedBodyId; btVector3 contactNormalOnB(0,0,0); #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST contactNormalOnB[i] = -1; #else contactNormalOnB[i%3] = -1; #endif btScalar penetration = 0; // Convert local points back to world btVector3 pivotAworld = m_pivotInA; if (m_rigidBodyA) { constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; } else { if (m_bodyA) pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); } btVector3 pivotBworld = m_pivotInB; if (m_rigidBodyB) { constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; } else { if (m_bodyB) pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); } btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST fillMultiBodyConstraint(constraintRow, data, 0, 0, contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse ); //@todo: support the case of btMultiBody versus btRigidBody, //see btPoint2PointConstraint::getInfo2NonVirtual #else const btVector3 dummy(0, 0, 0); btAssert(m_bodyA->isMultiDof()); btScalar* jac1 = jacobianA(i); const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy; const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy; m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); fillMultiBodyConstraint(constraintRow, data, jac1, 0, dummy, dummy, dummy, //sucks but let it be this way "for the time being" posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse ); #endif } }
void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val) { btAssert((index >= 0) && (index < 6)); m_equilibriumPoint[index] = val; }
bool btPolyhedralConvexShape::initializePolyhedralFeatures() { if (m_polyhedron) btAlignedFree(m_polyhedron); void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); m_polyhedron = new (mem) btConvexPolyhedron; btAlignedObjectArray<btVector3> orgVertices; for (int i=0;i<getNumVertices();i++) { btVector3& newVertex = orgVertices.expand(); getVertex(i,newVertex); } #if 0 btAlignedObjectArray<btVector3> planeEquations; btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); btAlignedObjectArray<btVector3> shiftedPlaneEquations; for (int p=0;p<planeEquations.size();p++) { btVector3 plane = planeEquations[p]; plane[3] -= getMargin(); shiftedPlaneEquations.push_back(plane); } btAlignedObjectArray<btVector3> tmpVertices; btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); btConvexHullComputer conv; conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); #else btConvexHullComputer conv; conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); #endif btAlignedObjectArray<btVector3> faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); btConvexHullComputer* convexUtil = &conv; btAlignedObjectArray<btFace> tmpFaces; tmpFaces.resize(numFaces); int numVertices = convexUtil->vertices.size(); m_polyhedron->m_vertices.resize(numVertices); for (int p=0;p<numVertices;p++) { m_polyhedron->m_vertices[p] = convexUtil->vertices[p]; } for (int i=0;i<numFaces;i++) { int face = convexUtil->faces[i]; //printf("face=%d\n",face); const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; const btConvexHullComputer::Edge* edge = firstEdge; btVector3 edges[3]; int numEdges = 0; //compute face normals btScalar maxCross2 = 0.f; int chosenEdge = -1; do { int src = edge->getSourceVertex(); tmpFaces[i].m_indices.push_back(src); int targ = edge->getTargetVertex(); btVector3 wa = convexUtil->vertices[src]; btVector3 wb = convexUtil->vertices[targ]; btVector3 newEdge = wb-wa; newEdge.normalize(); if (numEdges<2) edges[numEdges++] = newEdge; edge = edge->getNextEdgeOfFace(); } while (edge!=firstEdge); btScalar planeEq = 1e30f; if (numEdges==2) { faceNormals[i] = edges[0].cross(edges[1]); faceNormals[i].normalize(); tmpFaces[i].m_plane[0] = faceNormals[i].getX(); tmpFaces[i].m_plane[1] = faceNormals[i].getY(); tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); tmpFaces[i].m_plane[3] = planeEq; } else { btAssert(0);//degenerate? faceNormals[i].setZero(); } for (int v=0;v<tmpFaces[i].m_indices.size();v++) { btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); if (planeEq>eq) { planeEq=eq; } } tmpFaces[i].m_plane[3] = -planeEq; } //merge coplanar faces and copy them to m_polyhedron btScalar faceWeldThreshold= 0.999f; btAlignedObjectArray<int> todoFaces; for (int i=0;i<tmpFaces.size();i++) todoFaces.push_back(i); while (todoFaces.size()) { btAlignedObjectArray<int> coplanarFaceGroup; int refFace = todoFaces[todoFaces.size()-1]; coplanarFaceGroup.push_back(refFace); btFace& faceA = tmpFaces[refFace]; todoFaces.pop_back(); btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); for (int j=todoFaces.size()-1;j>=0;j--) { int i = todoFaces[j]; btFace& faceB = tmpFaces[i]; btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) { coplanarFaceGroup.push_back(i); todoFaces.remove(i); } } if (coplanarFaceGroup.size()>1) { //do the merge: use Graham Scan 2d convex hull btAlignedObjectArray<GrahamVector2> orgpoints; for (int i=0;i<coplanarFaceGroup.size();i++) { // m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); btFace& face = tmpFaces[coplanarFaceGroup[i]]; btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); btVector3 xyPlaneNormal(0,0,1); btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal); for (int f=0;f<face.m_indices.size();f++) { int orgIndex = face.m_indices[f]; btVector3 pt = m_polyhedron->m_vertices[orgIndex]; btVector3 rotatedPt = quatRotate(rotationArc,pt); rotatedPt.setZ(0); bool found = false; for (int i=0;i<orgpoints.size();i++) { //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) if (orgpoints[i].m_orgIndex == orgIndex) { found=true; break; } } if (!found) orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex)); } } btFace combinedFace; for (int i=0;i<4;i++) combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; btAlignedObjectArray<GrahamVector2> hull; GrahamScanConvexHull2D(orgpoints,hull); for (int i=0;i<hull.size();i++) { combinedFace.m_indices.push_back(hull[i].m_orgIndex); } m_polyhedron->m_faces.push_back(combinedFace); } else { for (int i=0;i<coplanarFaceGroup.size();i++) { m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); } } } m_polyhedron->initialize(); return true; }
void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) { btAssert((index >= 0) && (index < 6)); m_springStiffness[index] = stiffness; }
btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { btAssert(0); return 1.f; }
void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) { btAssert((index >= 0) && (index < 6)); m_springDamping[index] = damping; }
void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) { // compute the centroid of the polygon in cx,cy int i,j; btScalar a,cx,cy,q; if (n==1) { cx = p[0]; cy = p[1]; } else if (n==2) { cx = btScalar(0.5)*(p[0] + p[2]); cy = btScalar(0.5)*(p[1] + p[3]); } else { a = 0; cx = 0; cy = 0; for (i=0; i<(n-1); i++) { q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; a += q; cx += q*(p[i*2]+p[i*2+2]); cy += q*(p[i*2+1]+p[i*2+3]); } q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; if (btFabs(a+q) > SIMD_EPSILON) { a = 1.f/(btScalar(3.0)*(a+q)); } else { a=BT_LARGE_FLOAT; } cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); } // compute the angle of each point w.r.t. the centroid btScalar A[8]; for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx); // search for points that have angles closest to A[i0] + i*(2*pi/m). int avail[8]; for (i=0; i<n; i++) avail[i] = 1; avail[i0] = 0; iret[0] = i0; iret++; for (j=1; j<m; j++) { a = btScalar(j)*(2*M__PI/m) + A[i0]; if (a > M__PI) a -= 2*M__PI; btScalar maxdiff=1e9,diff; *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 for (i=0; i<n; i++) { if (avail[i]) { diff = btFabs (A[i]-a); if (diff > M__PI) diff = 2*M__PI - diff; if (diff < maxdiff) { maxdiff = diff; *iret = i; } } } #if defined(DEBUG) || defined (_DEBUG) btAssert (*iret != i0); // ensure iret got set #endif avail[*iret] = 0; iret++; } }
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) { // only positions need to be updated -- data.m_jacobians and force // directions were set in the ctor and never change. if (m_numDofsFinalized != m_jacSizeBoth) { finalizeMultiDof(); } //don't crash if (m_numDofsFinalized != m_jacSizeBoth) return; if (m_maxAppliedImpulse==0.f) return; const btScalar posError = 0; const btVector3 dummy(0, 0, 0); for (int row=0;row<getNumRows();row++) { btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); int dof = 0; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; btScalar velocityError = (m_desiredVelocity - currentVelocity); btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; if (rhs>m_rhsClamp) { rhs=m_rhsClamp; } if (rhs<-m_rhsClamp) { rhs=-m_rhsClamp; } fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; { //expect either prismatic or revolute joint type for now btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); switch (m_bodyA->getLink(m_linkA).m_jointType) { case btMultibodyLink::eRevolute: { constraintRow.m_contactNormal1.setZero(); constraintRow.m_contactNormal2.setZero(); btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; break; } case btMultibodyLink::ePrismatic: { btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); constraintRow.m_contactNormal1=prismaticAxisInWorld; constraintRow.m_contactNormal2=-prismaticAxisInWorld; constraintRow.m_relpos1CrossNormal.setZero(); constraintRow.m_relpos2CrossNormal.setZero(); break; } default: { btAssert(0); } }; } } }
void btHeightfieldTerrainShape::initialize ( int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, PHY_ScalarType hdt, bool flipQuadEdges ) { // validation btAssert(heightStickWidth > 1 && "bad width"); btAssert(heightStickLength > 1 && "bad length"); btAssert(heightfieldData && "null heightfield data"); // btAssert(heightScale) -- do we care? Trust caller here btAssert(minHeight <= maxHeight && "bad min/max height"); btAssert(upAxis >= 0 && upAxis < 3 && "bad upAxis--should be in range [0,2]"); btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT && "Bad height data type enum"); // initialize member variables m_shapeType = TERRAIN_SHAPE_PROXYTYPE; m_heightStickWidth = heightStickWidth; m_heightStickLength = heightStickLength; m_minHeight = minHeight; m_maxHeight = maxHeight; m_width = (btScalar) (heightStickWidth - 1); m_length = (btScalar) (heightStickLength - 1); m_heightScale = heightScale; m_heightfieldDataUnknown = heightfieldData; m_heightDataType = hdt; m_flipQuadEdges = flipQuadEdges; m_useDiamondSubdivision = false; m_useZigzagSubdivision = false; m_upAxis = upAxis; m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) { case 0: { m_localAabbMin.setValue(m_minHeight, 0, 0); m_localAabbMax.setValue(m_maxHeight, m_width, m_length); break; } case 1: { m_localAabbMin.setValue(0, m_minHeight, 0); m_localAabbMax.setValue(m_width, m_maxHeight, m_length); break; }; case 2: { m_localAabbMin.setValue(0, 0, m_minHeight); m_localAabbMax.setValue(m_width, m_length, m_maxHeight); break; } default: { //need to get valid m_upAxis btAssert(0 && "Bad m_upAxis"); } } // remember origin (defined as exact middle of aabb) m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); }
void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const { btAssert (0); }
//not yet bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const { btAssert(0); return false; }
void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const { btAssert(0); }
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); { BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies btAlignedObjectArray<btQuaternion> world_to_local; btAlignedObjectArray<btVector3> local_origin; for (int b=0;b<m_multiBodies.size();b++) { btMultiBody* bod = m_multiBodies[b]; bool isSleeping = false; if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { isSleeping = true; } for (int b=0;b<bod->getNumLinks();b++) { if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) isSleeping = true; } if (!isSleeping) { int nLinks = bod->getNumLinks(); ///base + num links world_to_local.resize(nLinks+1); local_origin.resize(nLinks+1); bod->stepPositions(timeStep); world_to_local[0] = bod->getWorldToBaseRot(); local_origin[0] = bod->getBasePos(); if (bod->getBaseCollider()) { btVector3 posr = local_origin[0]; float pos[4]={posr.x(),posr.y(),posr.z(),1}; float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; btTransform tr; tr.setIdentity(); tr.setOrigin(posr); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); bod->getBaseCollider()->setWorldTransform(tr); } for (int k=0;k<bod->getNumLinks();k++) { const int parent = bod->getParent(k); world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); } for (int m=0;m<bod->getNumLinks();m++) { btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; if (col) { int link = col->m_link; btAssert(link == m); int index = link+1; btVector3 posr = local_origin[index]; float pos[4]={posr.x(),posr.y(),posr.z(),1}; float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; btTransform tr; tr.setIdentity(); tr.setOrigin(posr); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); } } } else { bod->clearVelocities(); } } } }
void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btRigidBody* body1, btRigidBody* body2, const btMatrix3x3& world2A, const btMatrix3x3& world2B, const btVector3& invInertiaADiag, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, const btVector3& rel_posA1, const btVector3& invInertiaBDiag, const btScalar invMassB, const btVector3& linvelB,const btVector3& angvelB, const btVector3& rel_posA2, btScalar depthA, const btVector3& normalA, const btVector3& rel_posB1,const btVector3& rel_posB2, btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1) { (void)linvelA; (void)linvelB; (void)angvelB; (void)angvelA; imp0 = btScalar(0.); imp1 = btScalar(0.); btScalar len = btFabs(normalA.length()) - btScalar(1.); if (btFabs(len) >= SIMD_EPSILON) return; btAssert(len < SIMD_EPSILON); //this jacobian entry could be re-used for all iterations btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv btScalar massTerm = btScalar(1.) / (invMassA + invMassB); // calculate rhs (or error) terms const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; // dC/dv * dv = -C // jacobian * impulse = -error // //impulse = jacobianInverse * -error // inverting 2x2 symmetric system (offdiagonal are equal!) // btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; //[a b] [d -c] //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] }
void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) { ///not yet btAssert(0); }
void btParticlesDynamicsWorld::initCLKernels(int argc, char** argv) { cl_int ciErrNum; if (!m_cxMainContext) { // m_cxMainContext = clCreateContextFromType(0, CL_DEVICE_TYPE_ALL, NULL, NULL, &ciErrNum); m_cxMainContext = btOclCommon::createContextFromType(CL_DEVICE_TYPE_ALL, &ciErrNum); oclCHECKERROR(ciErrNum, CL_SUCCESS); m_cdDevice = btOclGetMaxFlopsDev(m_cxMainContext); btOclPrintDevInfo(m_cdDevice); // create a command-queue m_cqCommandQue = clCreateCommandQueue(m_cxMainContext, m_cdDevice, 0, &ciErrNum); oclCHECKERROR(ciErrNum, CL_SUCCESS); } // Program Setup size_t program_length; char* fileName = "ParticlesOCL.cl"; FILE * fp = fopen(fileName, "rb"); char newFileName[512]; if (fp == NULL) { sprintf(newFileName,"..//%s",fileName); fp = fopen(newFileName, "rb"); if (fp) fileName = newFileName; } if (fp == NULL) { sprintf(newFileName,"Demos//ParticlesOpenCL//%s",fileName); fp = fopen(newFileName, "rb"); if (fp) fileName = newFileName; } if (fp == NULL) { sprintf(newFileName,"..//..//..//..//..//Demos//ParticlesOpenCL//%s",fileName); fp = fopen(newFileName, "rb"); if (fp) fileName = newFileName; else { printf("cannot find %s\n",newFileName); exit(0); } } // char *source = oclLoadProgSource(".//Demos//SpheresGrid//SpheresGrid.cl", "", &program_length); //char *source = btOclLoadProgSource(".//Demos//SpheresOpenCL//Shared//SpheresGrid.cl", "", &program_length); char *source = btOclLoadProgSource(fileName, "", &program_length); if(source == NULL) { printf("ERROR : OpenCL can't load file %s\n", fileName); } // oclCHECKERROR (source == NULL, oclFALSE); btAssert(source != NULL); // create the program printf("OpenCL compiles %s ...", fileName); m_cpProgram = clCreateProgramWithSource(m_cxMainContext, 1, (const char**)&source, &program_length, &ciErrNum); oclCHECKERROR(ciErrNum, CL_SUCCESS); free(source); // build the program ciErrNum = clBuildProgram(m_cpProgram, 0, NULL, "-I .", NULL, NULL); if(ciErrNum != CL_SUCCESS) { // write out standard error // oclLog(LOGBOTH | ERRORMSG, (double)ciErrNum, STDERROR); // write out the build log and ptx, then exit char cBuildLog[10240]; // char* cPtx; // size_t szPtxLength; clGetProgramBuildInfo(m_cpProgram, btOclGetFirstDev(m_cxMainContext), CL_PROGRAM_BUILD_LOG, sizeof(cBuildLog), cBuildLog, NULL ); // oclGetProgBinary(m_cpProgram, oclGetFirstDev(m_cxMainContext), &cPtx, &szPtxLength); // oclLog(LOGBOTH | CLOSELOG, 0.0, "\n\nLog:\n%s\n\n\n\n\nPtx:\n%s\n\n\n", cBuildLog, cPtx); printf("\n\n%s\n\n\n", cBuildLog); printf("Press ENTER key to terminate the program\n"); getchar(); exit(-1); } printf("OK\n"); // create the kernels postInitDeviceData(); initKernel(PARTICLES_KERNEL_COMPUTE_CELL_ID, "kComputeCellId"); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_COMPUTE_CELL_ID].m_kernel, 1, sizeof(cl_mem), (void*) &m_dPos); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_COMPUTE_CELL_ID].m_kernel, 2, sizeof(cl_mem), (void*) &m_dPosHash); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_COMPUTE_CELL_ID].m_kernel, 3, sizeof(cl_mem), (void*) &m_dSimParams); oclCHECKERROR(ciErrNum, CL_SUCCESS); initKernel(PARTICLES_KERNEL_INTEGRATE_MOTION, "kIntegrateMotion"); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_INTEGRATE_MOTION].m_kernel, 1, sizeof(cl_mem), (void *) &m_dPos); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_INTEGRATE_MOTION].m_kernel, 2, sizeof(cl_mem), (void *) &m_dVel); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_INTEGRATE_MOTION].m_kernel, 3, sizeof(cl_mem), (void *) &m_dSimParams); oclCHECKERROR(ciErrNum, CL_SUCCESS); initKernel(PARTICLES_KERNEL_CLEAR_CELL_START, "kClearCellStart"); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_CLEAR_CELL_START].m_kernel, 0, sizeof(int), (void *) &m_numGridCells); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_CLEAR_CELL_START].m_kernel, 1, sizeof(cl_mem), (void*) &m_dCellStart); initKernel(PARTICLES_KERNEL_FIND_CELL_START, "kFindCellStart"); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_FIND_CELL_START].m_kernel, 1, sizeof(cl_mem), (void*) &m_dPosHash); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_FIND_CELL_START].m_kernel, 2, sizeof(cl_mem), (void*) &m_dCellStart); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_FIND_CELL_START].m_kernel, 3, sizeof(cl_mem), (void*) &m_dPos); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_FIND_CELL_START].m_kernel, 4, sizeof(cl_mem), (void*) &m_dVel); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_FIND_CELL_START].m_kernel, 5, sizeof(cl_mem), (void*) &m_dSortedPos); ciErrNum |= clSetKernelArg(m_kernels[PARTICLES_KERNEL_FIND_CELL_START].m_kernel, 6, sizeof(cl_mem), (void*) &m_dSortedVel); oclCHECKERROR(ciErrNum, CL_SUCCESS); initKernel(PARTICLES_KERNEL_COLLIDE_PARTICLES, "kCollideParticles"); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_COLLIDE_PARTICLES].m_kernel, 1, sizeof(cl_mem), (void*) &m_dVel); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_COLLIDE_PARTICLES].m_kernel, 2, sizeof(cl_mem), (void*) &m_dSortedPos); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_COLLIDE_PARTICLES].m_kernel, 3, sizeof(cl_mem), (void*) &m_dSortedVel); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_COLLIDE_PARTICLES].m_kernel, 4, sizeof(cl_mem), (void*) &m_dPosHash); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_COLLIDE_PARTICLES].m_kernel, 5, sizeof(cl_mem), (void*) &m_dCellStart); ciErrNum = clSetKernelArg(m_kernels[PARTICLES_KERNEL_COLLIDE_PARTICLES].m_kernel, 6, sizeof(cl_mem), (void*) &m_dSimParams); initKernel(PARTICLES_KERNEL_BITONIC_SORT_CELL_ID_LOCAL, "kBitonicSortCellIdLocal"); initKernel(PARTICLES_KERNEL_BITONIC_SORT_CELL_ID_LOCAL_1, "kBitonicSortCellIdLocal1"); initKernel(PARTICLES_KERNEL_BITONIC_SORT_CELL_ID_MERGE_GLOBAL, "kBitonicSortCellIdMergeGlobal"); initKernel(PARTICLES_KERNEL_BITONIC_SORT_CELL_ID_MERGE_LOCAL, "kBitonicSortCellIdMergeLocal"); }