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 normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp; btScalar k = info->fps * normalErp; 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]; for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i]; for (i=0; i<3; i++) info->m_J2linearAxis[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 * normalErp;//?? 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 bool powered = getEnableAngularMotor(); 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 = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; 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 }
int btGeneric6DofConstraint::get_limit_motor_info2( btRotationalLimitMotor * limot, btRigidBody * body0, btRigidBody * body1, btConstraintInfo2 *info, int row, btVector3& ax1, int rotational) { int srow = row * info->rowskip; int powered = limot->m_enableMotor; int limit = limot->m_currentLimit; if (powered || limit) { // if the joint is powered, or has joint limits, add in the extra row btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; btScalar *J2 = rotational ? info->m_J2angularAxis : 0; J1[srow+0] = ax1[0]; J1[srow+1] = ax1[1]; J1[srow+2] = ax1[2]; if(rotational) { J2[srow+0] = -ax1[0]; J2[srow+1] = -ax1[1]; J2[srow+2] = -ax1[2]; } if((!rotational)) { btVector3 ltd; // Linear Torque Decoupling vector btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition(); ltd = c.cross(ax1); info->m_J1angularAxis[srow+0] = ltd[0]; info->m_J1angularAxis[srow+1] = ltd[1]; info->m_J1angularAxis[srow+2] = ltd[2]; c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition(); ltd = -c.cross(ax1); info->m_J2angularAxis[srow+0] = ltd[0]; info->m_J2angularAxis[srow+1] = ltd[1]; info->m_J2angularAxis[srow+2] = ltd[2]; } // if we're limited low and high simultaneously, the joint motor is // ineffective if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; info->m_constraintError[srow] = btScalar(0.f); if (powered) { info->cfm[srow] = 0.0f; if(!limit) { btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; btScalar mot_fact = getMotorFactor( limot->m_currentPosition, limot->m_loLimit, limot->m_hiLimit, tag_vel, info->fps * info->erp); info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; info->m_lowerLimit[srow] = -limot->m_maxMotorForce; info->m_upperLimit[srow] = limot->m_maxMotorForce; } } if(limit) { btScalar k = info->fps * limot->m_ERP; if(!rotational) { info->m_constraintError[srow] += k * limot->m_currentLimitError; } else { info->m_constraintError[srow] += -k * limot->m_currentLimitError; } info->cfm[srow] = 0.0f; if (limot->m_loLimit == limot->m_hiLimit) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } else { if (limit == 1) { info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } else { info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // deal with bounce if (limot->m_bounce > 0) { // calculate joint velocity btScalar vel; if (rotational) { vel = body0->getAngularVelocity().dot(ax1); if (body1) vel -= body1->getAngularVelocity().dot(ax1); } else { vel = body0->getLinearVelocity().dot(ax1); if (body1) vel -= body1->getLinearVelocity().dot(ax1); } // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { if (vel < 0) { btScalar newc = -limot->m_bounce* vel; if (newc > info->m_constraintError[srow]) info->m_constraintError[srow] = newc; } } else { if (vel > 0) { btScalar newc = -limot->m_bounce * vel; if (newc < info->m_constraintError[srow]) info->m_constraintError[srow] = newc; } } } } } return 1; } else return 0; }
void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const { //moving concave objects not supported inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); }
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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 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); }
btScalar btRotationalLimitMotor::solveAngularLimits( btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) { if (needApplyTorques()==false) return 0.0f; btScalar target_velocity = m_targetVelocity; btScalar maxMotorForce = m_maxMotorForce; //current error correction if (m_currentLimit!=0) { target_velocity = -m_ERP*m_currentLimitError/(timeStep); maxMotorForce = m_maxLimitForce; } maxMotorForce *= timeStep; // current velocity difference btVector3 angVelA; bodyA.getAngularVelocity(angVelA); btVector3 angVelB; bodyB.getAngularVelocity(angVelB); btVector3 vel_diff; vel_diff = angVelA-angVelB; btScalar rel_vel = axis.dot(vel_diff); // correction velocity btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) { return 0.0f;//no need for applying force } // correction impulse btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse btScalar clippedMotorImpulse; ///@todo: should clip against accumulated impulse if (unclippedMotorImpulse>0.0f) { clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; } else { clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; } // sort with accumulated impulses btScalar lo = btScalar(-BT_LARGE_FLOAT); btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar oldaccumImpulse = m_accumulatedImpulse; btScalar sum = oldaccumImpulse + clippedMotorImpulse; m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; btVector3 motorImp = clippedMotorImpulse * axis; //body0->applyTorqueImpulse(motorImp); //body1->applyTorqueImpulse(-motorImp); bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); return clippedMotorImpulse; }
btRigidBody* Physics::addRigidSphere(Ogre::Entity* entity, Ogre::SceneNode* node, btScalar mass, btScalar rest, btVector3 localInertia, btVector3 origin, btQuaternion *rotation) { Ogre::Vector3 s = entity->getBoundingBox().getHalfSize(); btCollisionShape *sphereShape = new btSphereShape( btScalar(s[0]) ); addRigidBody(entity, node, sphereShape, mass, rest, localInertia, origin, rotation); };
SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30)) { m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); }
bool Skeleton_Builder::BuildTail(float tail_width, float tail_height, float torso_height) { Bone* UTail = new Bone(); Bone* MTail = new Bone(); Bone* LTail = new Bone(); Bone* LLTail = new Bone(); tail_height /= 4; Ogre::Quaternion neckrotation; neckrotation.FromAngleAxis(Ogre::Radian(Ogre::Degree(mTailIncline)), Ogre::Vector3::UNIT_X); btTransform t; btVector3 min, max; mHipNode->GetCollider()->getAabb(t, min, max); mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2); mBuilder->SetRelativePosition(Ogre::Vector3(0, -(torso_height /2), mDepth / 2), neckrotation, *mHipNode->GetNode()); if (!mBuilder->BuildBone(*LLTail, Bone::BoneType::Tail)) return false; mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2); mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height , 0), neckrotation, *LLTail->GetNode()); if (!mBuilder->BuildBone(*LTail, Bone::BoneType::Tail)) return false; mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2); mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height, 0), neckrotation, *LTail->GetNode()); if (!mBuilder->BuildBone(*MTail, Bone::BoneType::Tail)) return false; mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2); mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height, 0), neckrotation, *MTail->GetNode()); if (!mBuilder->BuildBone(*UTail, Bone::BoneType::Tail)) return false; if (mHasMuscle) { mMuscleBuilder->CreateMuscle(mHipNode, LLTail, mMuscles); mMuscleBuilder->CreateMuscle(LLTail, LTail, mMuscles); mMuscleBuilder->CreateMuscle(LTail, MTail, mMuscles); mMuscleBuilder->CreateMuscle(MTail, UTail, mMuscles); } //add joints btTransform localA, localB; btVector3 JointPos; JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (torso_height / 2), 0)); SetJointTransform(localA, localB, JointPos, mHipNode, LLTail, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* shoulderNeckConst = new btConeTwistConstraint(*mHipNode->GetRigidBody(), *LLTail->GetRigidBody(), localA, localB); shoulderNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4),btScalar(M_PI_4 / 4)); JointPos = Utils::OgreBTVector(LLTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0)); SetJointTransform(localA, localB, JointPos, LLTail, LTail, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* LNeckConst = new btConeTwistConstraint(*LLTail->GetRigidBody(), *LTail->GetRigidBody(), localA, localB); LNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); JointPos = Utils::OgreBTVector(LTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0)); SetJointTransform(localA, localB, JointPos, LTail, MTail, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* MNeckConst = new btConeTwistConstraint(*LTail->GetRigidBody(), *MTail->GetRigidBody(), localA, localB); MNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); JointPos = Utils::OgreBTVector(MTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0)); SetJointTransform(localA, localB, JointPos, MTail, UTail, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* HNeckConst = new btConeTwistConstraint(*MTail->GetRigidBody(), *UTail->GetRigidBody(), localA, localB); HNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); mConstraints.push_back(shoulderNeckConst); mConstraints.push_back(LNeckConst); mConstraints.push_back(MNeckConst); mConstraints.push_back(HNeckConst); return true; }
bool btGjkConvexCast::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, const btTransform& fromB, const btTransform& toB, CastResult& result) { m_simplexSolver->reset(); /// compute linear velocity for this interval, to interpolate //assume no rotation/angular velocity, assert here? btVector3 linVelA, linVelB; linVelA = toA.getOrigin()-fromA.getOrigin(); linVelB = toB.getOrigin()-fromB.getOrigin(); btScalar radius = btScalar(0.001); btScalar lambda = btScalar(0.); btVector3 v(1,0,0); int maxIter = MAX_ITERATIONS; btVector3 n; n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); bool hasResult = false; btVector3 c; btVector3 r = (linVelA-linVelB); btScalar lastLambda = lambda; //btScalar epsilon = btScalar(0.001); int numIter = 0; //first solution, using GJK btTransform identityTrans; identityTrans.setIdentity(); // result.drawCoordSystem(sphereTr); btPointCollector pointCollector; btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver,0);//m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD // gjk.setIgnoreMargin(true); input.m_transformA = fromA; input.m_transformB = fromB; gjk.getClosestPoints(input, pointCollector,0); hasResult = pointCollector.m_hasResult; c = pointCollector.m_pointInWorld; if (hasResult) { btScalar dist; dist = pointCollector.m_distance; n = pointCollector.m_normalOnBInWorld; //not close enough while (dist > radius) { numIter++; if (numIter > maxIter) { return false; //todo: report a failure } btScalar dLambda = btScalar(0.); btScalar projectedLinearVelocity = r.dot(n); dLambda = dist / (projectedLinearVelocity); lambda = lambda - dLambda; if (lambda > btScalar(1.)) return false; if (lambda < btScalar(0.)) return false; //todo: next check with relative epsilon if (lambda <= lastLambda) { return false; //n.setValue(0,0,0); break; } lastLambda = lambda; //interpolate to next lambda result.DebugDraw( lambda ); input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda); input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); gjk.getClosestPoints(input, pointCollector,0); if (pointCollector.m_hasResult) { if (pointCollector.m_distance < btScalar(0.)) { result.m_fraction = lastLambda; n = pointCollector.m_normalOnBInWorld; result.m_normal=n; result.m_hitPoint = pointCollector.m_pointInWorld; return true; } c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; dist = pointCollector.m_distance; } else { //?? return false; } } //is n normalized? //don't report time of impact for motion away from the contact normal (or causes minor penetration) if (n.dot(r)>=-result.m_allowedPenetration) return false; result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; return true; } return false; }
bool Skeleton_Builder::BuildSkeleton(Skeleton& newSkel, Ogre::Vector3 pos) { if (mLeg == LegType::None || mHeight <= 10) // check to ensure all attribs required return false; float neck_height; float torso_height; float Leg_height; float tail_height; //create base node for skeleton mBase = mSceneMgr->getRootSceneNode()->createChildSceneNode(pos); // set block area heights // if longneck neck/legs ratio = 2:1 // if shortneck neck/legs ratio = 1:2 // if upright torso 40% // else torso 25% if (IsUpright()) { auto onepercent = mHeight / 100; Leg_height = onepercent * 40; if (IsLongNeck()) { neck_height = onepercent * 40; torso_height = onepercent * 20; } else { neck_height = onepercent * 20; torso_height = onepercent * 40; } } else { auto onepercent = mHeight / 100; torso_height = onepercent * 25; if (IsLongNeck()) { neck_height = onepercent * 50; Leg_height = onepercent * 25; } else { neck_height = onepercent * 25; Leg_height = onepercent * 50; } } if (IsLongTail()) { auto onepercent = mHeight / 100; tail_height = onepercent * 50; } else { auto onepercent = mHeight / 100; tail_height = onepercent * 25; } //set block area widths float neck_width; float torso_width; float Leg_width; float Arm_width; auto onepercentW = mWidth / 100; // arms take 35% of width i.e 17.5% each side if needed torso_width = onepercentW * 65; Leg_width = onepercentW * 25; // ensures gaps between legs like realistic bipeds Arm_width = onepercentW * 17.5f; neck_width = onepercentW * 35; //build the torso here Bone* CentreTorso = new Bone(); if (IsUpright()) { Bone* TopTorso = new Bone(); Bone* BotTorso = new Bone(); //build 3 torso pieces torso_height /= 3; //build centre piece mBuilder->SetDimensions(torso_width, torso_height, mDepth); mBuilder->SetRelativePosition(Ogre::Vector3::ZERO, Ogre::Quaternion::IDENTITY,*mBase); if (!mBuilder->BuildBone(*CentreTorso,Bone::BoneType::Torso)) return false; //build bot piece mBuilder->SetDimensions(torso_width, torso_height, mDepth); mBuilder->SetRelativePosition(Ogre::Vector3(0, -(torso_height), 0), Ogre::Quaternion::IDENTITY, *CentreTorso->GetNode()); if (!mBuilder->BuildBone(*BotTorso,Bone::BoneType::Hip)) return false; //build top piece mBuilder->SetDimensions(torso_width, torso_height, mDepth); mBuilder->SetRelativePosition(Ogre::Vector3(0, (torso_height), 0), Ogre::Quaternion::IDENTITY, *CentreTorso->GetNode()); if (!mBuilder->BuildBone(*TopTorso, Bone::BoneType::Shoulder)) return false; mShouldernode = TopTorso; mHipNode = BotTorso; mBones.push_back(CentreTorso); mBones.push_back(TopTorso); mBones.push_back(BotTorso); //add muscles mMuscleBuilder->CreateMuscle(CentreTorso, TopTorso, mMuscles); mMuscleBuilder->CreateMuscle(CentreTorso, BotTorso, mMuscles); btTransform TransA; btTransform TransB; btVector3 JointPos; // world position of transform /*add constraints*/ // JOINT POS IS THE POSITION ON THE EDGE OF THE BONE ATTACH I.E BONE POS + EDGE OF BONE IN DIRECTION TO NEXT JointPos = Utils::OgreBTVector(CentreTorso->GetNode()->_getDerivedPosition() + (torso_height / 2)); SetJointTransform(TransA, TransB, JointPos, CentreTorso, TopTorso, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* TopSpine = new btConeTwistConstraint(*CentreTorso->GetRigidBody(), *TopTorso->GetRigidBody(), TransA, TransB); TopSpine->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); JointPos = Utils::OgreBTVector(CentreTorso->GetNode()->_getDerivedPosition() - (torso_height / 2)); SetJointTransform(TransA, TransB, JointPos, CentreTorso, BotTorso, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* BotSpine = new btConeTwistConstraint(*CentreTorso->GetRigidBody(), *BotTorso->GetRigidBody(), TransA, TransB); BotSpine->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); mConstraints.push_back(TopSpine); mConstraints.push_back(BotSpine); } else { //Bone* CentreTorso = new Bone(); //build centre piece mBuilder->SetDimensions(torso_width, torso_height, mDepth); mBuilder->SetRelativePosition(Ogre::Vector3::ZERO, Ogre::Quaternion::IDENTITY, *mBase); if (!mBuilder->BuildBone(*CentreTorso,Bone::BoneType::HipShoulder)) return false; mBones.push_back(CentreTorso); mShouldernode = CentreTorso; mHipNode = CentreTorso; } if (mFixed) CentreTorso->GetRigidBody()->setMassProps(0, btVector3(0, 0, 0)); //if (!BuildArm(Arm_width, Leg_height, torso_width) || !BuildNeck(neck_width, neck_height, torso_height) || !BuildLeg(Leg_width, Leg_height, torso_width, torso_height)) // return false; if (!mHidearms) if (!BuildArm(Arm_width, torso_height, torso_width)) return false; if (!mHideneck) if (!BuildNeck(neck_width, neck_height, torso_height)) return false; if (!mHidelegs) if (!BuildLeg(Leg_width, Leg_height, torso_width, torso_height)) return false; if (IsShortTail() || IsLongTail()) if(!BuildTail(neck_width,tail_height, torso_height)) return false; newSkel = Skeleton(mBones,mMuscles, mConstraints, mBase, mWorld); ClearData(); return true; }
bool Skeleton_Builder::BuildLeg(float leg_width, float leg_height, float torso_width, float torso_height) { auto halfgap = (torso_width / 2) - leg_width; auto legnodepos = halfgap + leg_width / 2; Bone* LUpperLeg = new Bone(); Bone* LLowerLeg = new Bone(); Bone* LFoot = new Bone(); Bone* RUpperLeg = new Bone(); Bone* RLowerLeg = new Bone(); Bone* RFoot = new Bone(); if (mLeg == LegType::Uninverted) { mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f); mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, -((torso_height / 2) + (leg_height / 4)), 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode()); if (!mBuilder->BuildBone(*LUpperLeg,Bone::BoneType::LUpperLeg)) return false; mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f); mBuilder->SetRelativePosition(Ogre::Vector3(-legnodepos, -((torso_height / 2) + (leg_height / 4)), 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode()); if (!mBuilder->BuildBone(*RUpperLeg, Bone::BoneType::RUpperLeg)) return false; mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f); mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *LUpperLeg->GetNode()); if (!mBuilder->BuildBone(*LLowerLeg, Bone::BoneType::LowerLeg)) return false; mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f); mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *RUpperLeg->GetNode()); if (!mBuilder->BuildBone(*RLowerLeg, Bone::BoneType::LowerLeg)) return false; // foot generation mBuilder->SetDimensions(leg_width*1.5f, leg_height / 4, mDepth); mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *LLowerLeg->GetNode()); if (!mBuilder->BuildBone(*LFoot,Bone::BoneType::Foot)) return false; mBuilder->SetDimensions(leg_width*1.5f, leg_height / 4, mDepth); mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *RLowerLeg->GetNode()); if (!mBuilder->BuildBone(*RFoot, Bone::BoneType::Foot)) return false; } else { //mBuilder->SetDimensions(leg_width, leg_height / 2); //mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode()); //if (!mBuilder->BuildBone(*LUpperLeg)) // return false; //mBuilder->SetDimensions(leg_width, leg_height / 2); //mBuilder->SetRelativePosition(Ogre::Vector3(0, leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *LUpperLeg->GetNode()); //if (!mBuilder->BuildBone(*LLowerLeg)) // return false; // foot generation //mBuilder->SetDimensions(leg_width, leg_height / 2); //mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, Hip->GetNode()); //if (!mBuilder->BuildBone(LUpperLeg)) // return false; //mBuilder->SetDimensions(leg_width, leg_height / 2); //mBuilder->SetRelativePosition(Ogre::Vector3(-legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode()); //if (!mBuilder->BuildBone(*RUpperLeg)) // return false; //mBuilder->SetDimensions(leg_width, leg_height / 2); //mBuilder->SetRelativePosition(Ogre::Vector3(0, leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *RUpperLeg->GetNode()); //if (!mBuilder->BuildBone(*RLowerLeg)) // return false; // foot generation //mBuilder->SetDimensions(leg_width, leg_height / 2); //mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, Hip->GetNode()); //if (!mBuilder->BuildBone(LUpperLeg)) // return false; } mBones.push_back(LUpperLeg); mBones.push_back(RUpperLeg); mBones.push_back(LLowerLeg); mBones.push_back(RLowerLeg); mBones.push_back(LFoot); mBones.push_back(RFoot); if (mHasMuscle) { mMuscleBuilder->CreateMuscle(mHipNode, LUpperLeg, mMuscles); mMuscleBuilder->CreateMuscle(LUpperLeg, LLowerLeg, mMuscles); mMuscleBuilder->CreateMuscle(LLowerLeg, LFoot, mMuscles); mMuscleBuilder->CreateMuscle(mHipNode, RUpperLeg, mMuscles); mMuscleBuilder->CreateMuscle(RUpperLeg, RLowerLeg, mMuscles); mMuscleBuilder->CreateMuscle(RLowerLeg, RFoot, mMuscles); } //add joints Ogre::Vector3 offset; btTransform localA, localB; btVector3 JointPos; JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(legnodepos, -(torso_height / 2), 0)); SetJointTransform(localA, localB, JointPos, mHipNode, LUpperLeg, btVector3(0, btScalar(M_PI_2), 0)); btConeTwistConstraint* LhipConst = new btConeTwistConstraint(*mHipNode->GetRigidBody(), *LUpperLeg->GetRigidBody(), localA, localB); LhipConst->setLimit(btScalar(M_PI_4), btScalar(M_PI_4), 0); JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(-legnodepos, -(torso_height / 2), 0)); SetJointTransform(localA, localB, JointPos, mHipNode, RUpperLeg, btVector3(0, 0, btScalar(M_PI_4))); btConeTwistConstraint* RhipConst = new btConeTwistConstraint(*mHipNode->GetRigidBody(), *RUpperLeg->GetRigidBody(), localA, localB); RhipConst->setLimit(btScalar(M_PI_4), btScalar(M_PI_4), 0); JointPos = Utils::OgreBTVector(LUpperLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0)); SetJointTransform(localA, localB, JointPos, LUpperLeg, LLowerLeg, btVector3(0, btScalar(M_PI_2), 0)); btHingeConstraint* LLegConst = new btHingeConstraint(*LUpperLeg->GetRigidBody(), *LLowerLeg->GetRigidBody(), localA, localB); LLegConst->setLimit(0, btScalar(M_PI_2)); JointPos = Utils::OgreBTVector(RUpperLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0)); SetJointTransform(localA, localB, JointPos, RUpperLeg, RLowerLeg, btVector3(0, btScalar(M_PI_2), 0)); btHingeConstraint* RLegConst = new btHingeConstraint(*RUpperLeg->GetRigidBody(), *RLowerLeg->GetRigidBody(), localA, localB); RLegConst->setLimit(0, btScalar(M_PI_2)); JointPos = Utils::OgreBTVector(LLowerLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0)); SetJointTransform(localA, localB, JointPos, LLowerLeg, LFoot, btVector3(0, btScalar(M_PI_2), 0)); btHingeConstraint* LFootConst = new btHingeConstraint(*LLowerLeg->GetRigidBody(), *LFoot->GetRigidBody(), localA, localB); LFootConst->setLimit(0, btScalar(M_PI_2)); JointPos = Utils::OgreBTVector(RLowerLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0)); SetJointTransform(localA, localB, JointPos, RLowerLeg, RFoot, btVector3(0, btScalar(M_PI_2), 0)); btHingeConstraint* RFootConst = new btHingeConstraint(*RLowerLeg->GetRigidBody(), *RFoot->GetRigidBody(), localA, localB); RFootConst->setLimit(0, btScalar(M_PI_2)); mConstraints.push_back(LhipConst); mConstraints.push_back(RhipConst); mConstraints.push_back(LLegConst); mConstraints.push_back(RLegConst); mConstraints.push_back(LFootConst); mConstraints.push_back(RFootConst); return true; }
bool Skeleton_Builder::BuildNeck(float neck_width, float neck_height, float torso_height) { Bone* UNeck = new Bone(); Bone* MNeck = new Bone(); Bone* LNeck = new Bone(); Bone* LLNeck = new Bone(); neck_height /= 4; mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f); Ogre::Quaternion neckrotation; neckrotation.FromAngleAxis(Ogre::Radian(Ogre::Degree(mNeckIncline)), Ogre::Vector3::UNIT_X); mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *mShouldernode->GetNode()); if (!mBuilder->BuildBone(*LLNeck,Bone::BoneType::Neck)) return false; mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f); mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *LLNeck->GetNode()); if (!mBuilder->BuildBone(*LNeck, Bone::BoneType::Neck)) return false; mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f); mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *LNeck->GetNode()); if (!mBuilder->BuildBone(*MNeck, Bone::BoneType::Neck)) return false; mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f); mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *MNeck->GetNode()); if (!mBuilder->BuildBone(*UNeck, Bone::BoneType::Neck)) return false; if (mHasMuscle) { mMuscleBuilder->CreateMuscle(mShouldernode, LLNeck, mMuscles); mMuscleBuilder->CreateMuscle(LLNeck, LNeck, mMuscles); mMuscleBuilder->CreateMuscle(LNeck, MNeck, mMuscles); mMuscleBuilder->CreateMuscle(MNeck, UNeck, mMuscles); } //add joints btTransform localA, localB; btVector3 JointPos; JointPos = Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (torso_height / 2), 0)); SetJointTransform(localA, localB, JointPos, mShouldernode, LLNeck, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* shoulderNeckConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *LLNeck->GetRigidBody(), localA, localB); shoulderNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); JointPos = Utils::OgreBTVector(LLNeck->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (neck_height / 2), 0)); SetJointTransform(localA, localB, JointPos, LLNeck, LNeck, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* LNeckConst = new btConeTwistConstraint(*LLNeck->GetRigidBody(), *LNeck->GetRigidBody(), localA, localB); LNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (neck_height / 2), 0)); SetJointTransform(localA, localB, JointPos, LNeck, MNeck, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* MNeckConst = new btConeTwistConstraint(*LNeck->GetRigidBody(), *MNeck->GetRigidBody(), localA, localB); MNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4)); JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (neck_height / 2), 0)); SetJointTransform(localA, localB, JointPos, MNeck, UNeck, btVector3(0, btScalar(-M_PI_2), 0)); btConeTwistConstraint* HNeckConst = new btConeTwistConstraint(*MNeck->GetRigidBody(), *UNeck->GetRigidBody(), localA, localB); HNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4),btScalar(M_PI_4 / 4)); mConstraints.push_back(shoulderNeckConst); mConstraints.push_back(LNeckConst); mConstraints.push_back(MNeckConst); mConstraints.push_back(HNeckConst); return true; }
bool Skeleton_Builder::BuildArm(float arm_Width, float arm_height, float torso_width) { // 3 blocks no hands // only height/ width changes //if (mArm == ArmType::ShortArms) //{ // //half height of legs // Bone* LUpperArm = new Bone(); // Bone* LLowerArm = new Bone(); // //build shoulder piece // torso_width /= 2; // mBuilder->SetDimensions(arm_Width, arm_height/2); // mBuilder->SetRelativePosition(Ogre::Vector3(torso_width, 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode()); // if (!mBuilder->BuildBone(*LUpperArm)) // return false; // // mBuilder->SetDimensions(arm_Width, arm_height / 2); // mBuilder->SetRelativePosition(Ogre::Vector3(0, arm_height / 2, 0), Ogre::Quaternion::IDENTITY, *LUpperArm->GetNode()); // if (!mBuilder->BuildBone(*LLowerArm)) // return false; // mBones.push_back(LUpperArm); // mBones.push_back(LLowerArm); // //Bone* RUpperArm = new Bone(); // //Bone* RLowerArm = new Bone(); // //mBuilder->SetDimensions(arm_Width, arm_height / 2); // //mBuilder->SetRelativePosition(Ogre::Vector3(-torso_width, 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode()); // //if (!mBuilder->BuildBone(*RUpperArm)) // // return false; // // // //mBuilder->SetDimensions(arm_Width, arm_height / 2); // //mBuilder->SetRelativePosition(Ogre::Vector3(0, arm_height / 2, 0), Ogre::Quaternion::IDENTITY, *RUpperArm->GetNode()); // //if (!mBuilder->BuildBone(*RLowerArm)) // // return false; // //mBones.push_back(RUpperArm); // //mBones.push_back(RLowerArm); // //add joints // btTransform localA, localB; // Ogre::Vector3 offset; // offset = Ogre::Vector3(torso_width, 0, 0); // //auto vec = mShouldernode->GetNode()->_getDerivedPosition(); // //vec += offset; // SetJointTransform(localA, localB, Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() + offset), mShouldernode, LUpperArm); // btConeTwistConstraint* LshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *LUpperArm->GetRigidBody(), localA, localB); // LshoulderConst->setLimit(M_PI_2, M_PI_2, 0); // //SetJointTransform(localA, localB, Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() - offset), mShouldernode, RUpperArm); // //btConeTwistConstraint* RshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *RUpperArm->GetRigidBody(), localA, localB); // //RshoulderConst->setLimit(M_PI_2, M_PI_2, 0); // offset = Ogre::Vector3(0, arm_height/4, 0); // SetJointTransform(localA, localB, Utils::OgreBTVector(LUpperArm->GetNode()->_getDerivedPosition() - offset), LUpperArm, LLowerArm); // btHingeConstraint* LArmConst = new btHingeConstraint(*LUpperArm->GetRigidBody(), *LLowerArm->GetRigidBody(), localA, localB); // //SetJointTransform(localA, localB, Utils::OgreBTVector(RUpperArm->GetNode()->_getDerivedPosition() - offset), RUpperArm, RLowerArm); // //btHingeConstraint* RArmConst = new btHingeConstraint(*RUpperArm->GetRigidBody(), *RLowerArm->GetRigidBody(), localA, localB); // mConstraints.push_back(LshoulderConst); // mConstraints.push_back(LArmConst); // //mConstraints.push_back(RshoulderConst); // //mConstraints.push_back(RArmConst); // //} //else if (mArm == ArmType::LongArms) { //half height of legs Bone* LUpperArm = new Bone(); Bone* RUpperArm = new Bone(); Bone* LLowerArm = new Bone(); Bone* RLowerArm = new Bone(); //build shoulder piece //torso_width /= 2; mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f); mBuilder->SetRelativePosition(Ogre::Vector3(((torso_width / 2) + arm_Width/2), 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode()); if (!mBuilder->BuildBone(*LUpperArm,Bone::BoneType::LUpperArm)) return false; mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f); mBuilder->SetRelativePosition(Ogre::Vector3(-((torso_width / 2) + arm_Width/2), 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode()); if (!mBuilder->BuildBone(*RUpperArm, Bone::BoneType::RUpperArm)) return false; mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f); mBuilder->SetRelativePosition(Ogre::Vector3(0, -(arm_height), 0), Ogre::Quaternion::IDENTITY, *LUpperArm->GetNode()); if (!mBuilder->BuildBone(*LLowerArm, Bone::BoneType::LowerArm)) return false; mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f); mBuilder->SetRelativePosition(Ogre::Vector3(0, -(arm_height), 0), Ogre::Quaternion::IDENTITY, *RUpperArm->GetNode()); if (!mBuilder->BuildBone(*RLowerArm, Bone::BoneType::LowerArm)) return false; mBones.push_back(LUpperArm); mBones.push_back(RUpperArm); mBones.push_back(LLowerArm); mBones.push_back(RLowerArm); if (mHasMuscle) { mMuscleBuilder->CreateMuscle(mShouldernode, LUpperArm, mMuscles); mMuscleBuilder->CreateMuscle(LUpperArm, LLowerArm, mMuscles); mMuscleBuilder->CreateMuscle(mShouldernode, RUpperArm, mMuscles); mMuscleBuilder->CreateMuscle(RUpperArm, RLowerArm, mMuscles); } //add joints btTransform localA, localB; btVector3 JointPos; JointPos = Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() + (torso_width / 2 + arm_Width * 0.10f)); SetJointTransform(localA, localB, JointPos, mShouldernode, LUpperArm, btVector3(0, 0, 0)); // this needed a special case localA.getBasis().setEulerZYX(0,0,btScalar(M_PI)); localB.getBasis().setEulerZYX(0, 0, btScalar(M_PI_2)); btConeTwistConstraint* LshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *LUpperArm->GetRigidBody(), localA, localB); LshoulderConst->setLimit(btScalar(M_PI_2), btScalar(M_PI_2), 0); JointPos = Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() - (torso_width / 2 + arm_Width * 0.10f)); SetJointTransform(localA, localB, JointPos, mShouldernode, RUpperArm, btVector3(0, 0, 0)); localA.getBasis().setEulerZYX(0, 0, 0); localB.getBasis().setEulerZYX(0, 0, btScalar(M_PI_2)); btConeTwistConstraint* RshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *RUpperArm->GetRigidBody(), localA, localB); RshoulderConst->setLimit(btScalar(M_PI_2), btScalar(M_PI_2), 0); JointPos = Utils::OgreBTVector(LUpperArm->GetNode()->_getDerivedPosition() - arm_height / 2); SetJointTransform(localA, localB, JointPos, LUpperArm, LLowerArm, btVector3(0, btScalar(M_PI_2), 0)); btHingeConstraint* LArmConst = new btHingeConstraint(*LUpperArm->GetRigidBody(), *LLowerArm->GetRigidBody(), localA, localB); LArmConst->setLimit(0, btScalar(M_PI_2)); JointPos = Utils::OgreBTVector(RUpperArm->GetNode()->_getDerivedPosition() - arm_height / 2); SetJointTransform(localA, localB, JointPos, RUpperArm, RLowerArm, btVector3(0, btScalar(M_PI_2), 0)); btHingeConstraint* RArmConst = new btHingeConstraint(*RUpperArm->GetRigidBody(), *RLowerArm->GetRigidBody(), localA, localB); RArmConst->setLimit(0, btScalar(M_PI_2)); mConstraints.push_back(LshoulderConst); mConstraints.push_back(RshoulderConst); mConstraints.push_back(LArmConst); mConstraints.push_back(RArmConst); //auto prestore = mMuscleBuilder->CreateMuscle(LUpperArm, LLowerArm); //for (auto m : prestore) // mMuscles.push_back(m); } // means no arms to continue return true; }
void SerializeDemo::initPhysics() { setTexturing(true); setShadows(true); setCameraDistance(btScalar(SCALING*50.)); setupEmptyDynamicsWorld(); #ifdef DESERIALIZE_SOFT_BODIES m_fileLoader = new MySoftBulletWorldImporter((btSoftRigidDynamicsWorld*)m_dynamicsWorld); #else m_fileLoader = new btBulletWorldImporter(m_dynamicsWorld); #endif //DESERIALIZE_SOFT_BODIES // fileLoader->setVerboseMode(true); if (!m_fileLoader->loadFile("testFile.bullet")) // if (!m_fileLoader->loadFile("../SoftDemo/testFile.bullet")) { ///create a few basic rigid bodies and save them to testFile.bullet btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); // btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); btCollisionObject* groundObject = 0; m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: { btScalar mass(0.); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) groundShape->calculateLocalInertia(mass,localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); //add the body to the dynamics world m_dynamicsWorld->addRigidBody(body); groundObject = body; } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance int numSpheres = 2; btVector3 positions[2] = {btVector3(0.1f,0.2f,0.3f),btVector3(0.4f,0.5f,0.6f)}; btScalar radii[2] = {0.3f,0.4f}; btMultiSphereShape* colShape = new btMultiSphereShape(positions,radii,numSpheres); //btCollisionShape* colShape = new btCapsuleShapeZ(SCALING*1,SCALING*1); //btCollisionShape* colShape = new btCylinderShapeZ(btVector3(SCALING*1,SCALING*1,SCALING*1)); //btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) colShape->calculateLocalInertia(mass,localInertia); float start_x = START_POS_X - ARRAY_SIZE_X/2; float start_y = START_POS_Y; float start_z = START_POS_Z - ARRAY_SIZE_Z/2; for (int k=0;k<ARRAY_SIZE_Y;k++) { for (int i=0;i<ARRAY_SIZE_X;i++) { for(int j = 0;j<ARRAY_SIZE_Z;j++) { startTransform.setOrigin(SCALING*btVector3( btScalar(2.0*i + start_x), btScalar(20+2.0*k + start_y), btScalar(2.0*j + start_z))); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); body->setActivationState(ISLAND_SLEEPING); m_dynamicsWorld->addRigidBody(body); body->setActivationState(ISLAND_SLEEPING); } } } } int maxSerializeBufferSize = 1024*1024*5; btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize); static char* groundName = "GroundName"; serializer->registerNameForPointer(groundObject, groundName); for (int i=0;i<m_collisionShapes.size();i++) { char* name = new char[20]; sprintf(name,"name%d",i); serializer->registerNameForPointer(m_collisionShapes[i],name); } btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*(btRigidBody*)getDynamicsWorld()->getCollisionObjectArray()[2],btVector3(0,1,0)); m_dynamicsWorld->addConstraint(p2p); const char* name = "constraintje"; serializer->registerNameForPointer(p2p,name); m_dynamicsWorld->serialize(serializer); FILE* f2 = fopen("testFile.bullet","wb"); fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2); fclose(f2); } //clientResetScene(); }
void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap) { //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! if (trimeshShape->getTriangleInfoMap()) return; trimeshShape->setTriangleInfoMap(triangleInfoMap); btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); const btVector3& meshScaling = meshInterface->getScaling(); for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) { 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; //PHY_ScalarType indexType=0; btVector3 triangleVerts[3]; meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); btVector3 aabbMin,aabbMax; for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) { unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangleVerts[0]); aabbMax.setMax(triangleVerts[0]); aabbMin.setMin(triangleVerts[1]); aabbMax.setMax(triangleVerts[1]); aabbMin.setMin(triangleVerts[2]); aabbMax.setMax(triangleVerts[2]); btConnectivityProcessor connectivityProcessor; connectivityProcessor.m_partIdA = partId; connectivityProcessor.m_triangleIndexA = triangleIndex; connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; connectivityProcessor.m_triangleInfoMap = triangleInfoMap; trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax); } } }
HeightmapNode::HeightmapNode(EditorScene * _scene, LandscapeNode *_land) : SceneNode() , position(0,0,0) , rotation(0) { SetName("HeightmapNode"); editorScene = _scene; SetVisible(false); land = _land; DVASSERT(land); //Get LandSize; Vector3 landSize; AABBox3 transformedBox; land->GetBoundingBox().GetTransformedBox(land->GetWorldTransform(), transformedBox); landSize = transformedBox.max - transformedBox.min; heightmap = land->GetHeightmap(); sizeInMeters = landSize.x; areaScale = sizeInMeters / heightmap->Size(); maxHeight = landSize.z; heightScale = maxHeight / 65535.f; float32 minHeight = 0.0f; heightmapTexture = NULL; debugVertices.resize(heightmap->Size() * heightmap->Size()); debugIndices.resize(heightmap->Size() * heightmap->Size() * 6); renderDataObject = new RenderDataObject(); renderDataObject->SetStream(EVF_VERTEX, TYPE_FLOAT, 3, sizeof(LandscapeNode::LandscapeVertex), &debugVertices[0].position); renderDataObject->SetStream(EVF_TEXCOORD0, TYPE_FLOAT, 2, sizeof(LandscapeNode::LandscapeVertex), &debugVertices[0].texCoord); int32 step = 1; int32 indexIndex = 0; int32 quadWidth = heightmap->Size(); for(int32 y = 0; y < heightmap->Size() - 1; y += step) { for(int32 x = 0; x < heightmap->Size() - 1; x += step) { debugIndices[indexIndex++] = x + y * quadWidth; debugIndices[indexIndex++] = (x + step) + y * quadWidth; debugIndices[indexIndex++] = x + (y + step) * quadWidth; debugIndices[indexIndex++] = (x + step) + y * quadWidth; debugIndices[indexIndex++] = (x + step) + (y + step) * quadWidth; debugIndices[indexIndex++] = x + (y + step) * quadWidth; } } uint16 *dt = heightmap->Data(); size.x = heightmap->Size(); size.y = heightmap->Size(); hmap.resize(heightmap->Size() * heightmap->Size()); for (int32 y = 0; y < heightmap->Size(); ++y) { for (int32 x = 0; x < heightmap->Size(); ++x) { int32 index = x + (y) * heightmap->Size(); float32 mapValue = dt[index] * heightScale; SetValueToMap(x, y, mapValue, transformedBox); debugVertices[index].texCoord = Vector2((float32)x / (float32)(heightmap->Size() - 1), (float32)y / (float32)(heightmap->Size() - 1)); } } bool flipQuadEdges = true; colShape = new btHeightfieldTerrainShape(heightmap->Size(), heightmap->Size() , &hmap.front(), heightScale, minHeight, maxHeight , 2, PHY_FLOAT , flipQuadEdges); colShape->setLocalScaling(btVector3(areaScale, areaScale, 1.0f)); // Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(0.0f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) colShape->calculateLocalInertia(mass,localInertia); startTransform.setOrigin(btVector3( btScalar(position.x), btScalar(position.y), btScalar(maxHeight/2.0f + position.z))); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects motionSate = new btDefaultMotionState(startTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionSate,colShape,localInertia); rbInfo.m_friction = 0.9f; body = new btRigidBody(rbInfo); // btVector3 mn; // btVector3 mx; // body->getAabb(mn, mx); // btVector3 sz = mx - mn; // Logger::Debug("land size = %f, %f, %f", sz.getX(), sz.getY(), sz.getZ()); collisionObject = new btCollisionObject(); collisionObject->setWorldTransform(startTransform); collisionObject->setCollisionShape(colShape); editorScene->landCollisionWorld->addCollisionObject(collisionObject); editorScene->landCollisionWorld->updateAabbs(); cursor = new LandscapeCursor(); }
/// Changes a btManifoldPoint collision normal to the normal from the mesh. void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) { //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; btBvhTriangleMeshShape* trimesh = 0; if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); else trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; int hash = btGetHash(partId0,index0); btTriangleInfo* info = triangleInfoMapPtr->find(hash); if (!info) return; btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f; const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); btVector3 v0,v1,v2; tri_shape->getVertex(0,v0); tri_shape->getVertex(1,v1); tri_shape->getVertex(2,v2); //btVector3 center = (v0+v1+v2)*btScalar(1./3.); btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0); btVector3 tri_normal; tri_shape->calcNormal(tri_normal); //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); btVector3 nearest; btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest); btVector3 contact = cp.m_localPointB; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW const btTransform& tr = colObj0->getWorldTransform(); btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW bool isNearEdge = false; int numConcaveEdgeHits = 0; int numConvexEdgeHits = 0; btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; localContactNormalOnB.normalize();//is this necessary? // Get closest edge int bestedge=-1; btScalar disttobestedge=BT_LARGE_FLOAT; // // Edge 0 -> 1 if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { btVector3 nearest; btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); btScalar len=(contact-nearest).length(); // if( len < disttobestedge ) { bestedge=0; disttobestedge=len; } } // Edge 1 -> 2 if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { btVector3 nearest; btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); btScalar len=(contact-nearest).length(); // if( len < disttobestedge ) { bestedge=1; disttobestedge=len; } } // Edge 2 -> 0 if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { btVector3 nearest; btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); btScalar len=(contact-nearest).length(); // if( len < disttobestedge ) { bestedge=2; disttobestedge=len; } } #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); #endif if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) if( bestedge==0 ) { btVector3 edge(v0-v1); isNearEdge = true; if (info->m_edgeV0V1Angle==btScalar(0)) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 nA = swapFactor * tri_normal; btQuaternion orn(edge,info->m_edgeV0V1Angle); btVector3 computedNormalB = quatRotate(orn,tri_normal); if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) computedNormalB*=-1; btVector3 nB = swapFactor*computedNormalB; btScalar NdotA = localContactNormalOnB.dot(nA); btScalar NdotB = localContactNormalOnB.dot(nB); bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); #ifdef DEBUG_INTERNAL_EDGE { btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); } #endif //DEBUG_INTERNAL_EDGE if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; btVector3 clampedLocalNormal; bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) { btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. (what about cp.m_distance1?) cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); } } } } } } btNearestPointInLineSegment(contact,v1,v2,nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); #endif if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) if( bestedge==1 ) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 edge(v1-v2); isNearEdge = true; if (info->m_edgeV1V2Angle == btScalar(0)) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0; btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 nA = swapFactor * tri_normal; btQuaternion orn(edge,info->m_edgeV1V2Angle); btVector3 computedNormalB = quatRotate(orn,tri_normal); if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) computedNormalB*=-1; btVector3 nB = swapFactor*computedNormalB; #ifdef DEBUG_INTERNAL_EDGE { btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); } #endif //DEBUG_INTERNAL_EDGE btScalar NdotA = localContactNormalOnB.dot(nA); btScalar NdotB = localContactNormalOnB.dot(nB); bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; btVector3 clampedLocalNormal; bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) { btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); } } } } } } btNearestPointInLineSegment(contact,v2,v0,nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); #endif if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) if( bestedge==2 ) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 edge(v2-v0); if (info->m_edgeV2V0Angle==btScalar(0)) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0; btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 nA = swapFactor * tri_normal; btQuaternion orn(edge,info->m_edgeV2V0Angle); btVector3 computedNormalB = quatRotate(orn,tri_normal); if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) computedNormalB*=-1; btVector3 nB = swapFactor*computedNormalB; #ifdef DEBUG_INTERNAL_EDGE { btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); } #endif //DEBUG_INTERNAL_EDGE btScalar NdotA = localContactNormalOnB.dot(nA); btScalar NdotB = localContactNormalOnB.dot(nB); bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; // printf("hitting convex edge\n"); btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; btVector3 clampedLocalNormal; bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) { btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); } } } } } } #ifdef DEBUG_INTERNAL_EDGE { btVector3 color(0,1,1); btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); } #endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { if (numConcaveEdgeHits>0) { if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0) { //fix tri_normal so it pointing the same direction as the current local contact normal if (tri_normal.dot(localContactNormalOnB) < 0) { tri_normal *= -1; } cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal; } else { btVector3 newNormal = tri_normal *frontFacing; //if the tri_normal is pointing opposite direction as the current local contact normal, skip it btScalar d = newNormal.dot(localContactNormalOnB) ; if (d< 0) { return; } //modify the normal to be the triangle normal (or backfacing normal) cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal; } // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); } } }
void BasicDemo::initPhysics() { setTexturing(true); setShadows(true); setCameraDistance(btScalar(SCALING*50.)); ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btDefaultCollisionConfiguration(); //m_collisionConfiguration->setConvexConvexMultipointIterations(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); m_broadphase = new btDbvtBroadphase(); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; m_solver = sol; m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); ///create a few basic rigid bodies btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); // btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-50,0)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: { btScalar mass(0.); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) groundShape->calculateLocalInertia(mass,localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); //add the body to the dynamics world m_dynamicsWorld->addRigidBody(body); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0,0,0); if (isDynamic) colShape->calculateLocalInertia(mass,localInertia); float start_x = START_POS_X - ARRAY_SIZE_X/2; float start_y = START_POS_Y; float start_z = START_POS_Z - ARRAY_SIZE_Z/2; for (int k=0; k<ARRAY_SIZE_Y; k++) { for (int i=0; i<ARRAY_SIZE_X; i++) { for(int j = 0; j<ARRAY_SIZE_Z; j++) { startTransform.setOrigin(SCALING*btVector3( 2.0*i + start_x, 20+2.0*k + start_y, 2.0*j + start_z)); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); //body->setContactProcessingThreshold(colShape->getContactBreakingThreshold()); body->setActivationState(ISLAND_SLEEPING); m_dynamicsWorld->addRigidBody(body); body->setActivationState(ISLAND_SLEEPING); } } } } clientResetScene(); }
bool btSubsimplexConvexCast::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, const btTransform& fromB, const btTransform& toB, CastResult& result) { m_simplexSolver->reset(); btVector3 linVelA,linVelB; linVelA = toA.getOrigin()-fromA.getOrigin(); linVelB = toB.getOrigin()-fromB.getOrigin(); btScalar lambda = btScalar(0.); btTransform interpolatedTransA = fromA; btTransform interpolatedTransB = fromB; ///take relative motion btVector3 r = (linVelA-linVelB); btVector3 v; btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis())); btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis())); v = supVertexA-supVertexB; int maxIter = sscc_MAX_ITERATIONS; btVector3 n; n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); bool hasResult = false; btVector3 c; btScalar lastLambda = lambda; btScalar dist2 = v.length2(); #ifdef BT_USE_DOUBLE_PRECISION btScalar epsilon = btScalar(0.0001); #else btScalar epsilon = btScalar(0.0001); #endif //BT_USE_DOUBLE_PRECISION btVector3 w,p; btScalar VdotR; while ( (dist2 > epsilon) && maxIter--) { supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); w = supVertexA-supVertexB; btScalar VdotW = v.dot(w); if (lambda > btScalar(1.0)) { return false; } if ( VdotW > btScalar(0.)) { VdotR = v.dot(r); if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) return false; else { lambda = lambda - VdotW / VdotR; //interpolate to next lambda // x = s + lambda * r; interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); //m_simplexSolver->reset(); //check next line w = supVertexA-supVertexB; lastLambda = lambda; n = v; hasResult = true; } } ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc. if (!m_simplexSolver->inSimplex(w)) m_simplexSolver->addVertex( w, supVertexA , supVertexB); if (m_simplexSolver->closest(v)) { dist2 = v.length2(); hasResult = true; //todo: check this normal for validity //n=v; //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); //printf("DIST2=%f\n",dist2); //printf("numverts = %i\n",m_simplexSolver->numVertices()); } else { dist2 = btScalar(0.); } } //int numiter = sscc_MAX_ITERATIONS - maxIter; // printf("number of iterations: %d", numiter); //don't report a time of impact when moving 'away' from the hitnormal result.m_fraction = lambda; if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON)) result.m_normal = n.normalized(); else result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)); //don't report time of impact for motion away from the contact normal (or causes minor penetration) if (result.m_normal.dot(r)>=-result.m_allowedPenetration) return false; btVector3 hitA,hitB; m_simplexSolver->compute_points(hitA,hitB); result.m_hitPoint=hitB; return true; }
void myinit() { // GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) }; GLfloat light_ambient[] = { btScalar(1.0), btScalar(1.2), btScalar(0.2), btScalar(1.0) }; GLfloat light_diffuse[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0) }; GLfloat light_specular[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0 )}; /* light_position is NOT default value */ GLfloat light_position0[] = { btScalar(1000.0), btScalar(1000.0), btScalar(1000.0), btScalar(0.0 )}; GLfloat light_position1[] = { btScalar(-1.0), btScalar(-10.0), btScalar(-1.0), btScalar(0.0) }; glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse); glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular); glLightfv(GL_LIGHT0, GL_POSITION, light_position0); glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient); glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse); glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular); glLightfv(GL_LIGHT1, GL_POSITION, light_position1); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_LIGHT1); // glShadeModel(GL_FLAT);//GL_SMOOTH); glShadeModel(GL_SMOOTH); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LESS); glClearColor(float(0.7),float(0.7),float(0.7),float(0)); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); static bool m_textureenabled = true; static bool m_textureinitialized = false; if(m_textureenabled) { if(!m_textureinitialized) { glActiveTexture(GL_TEXTURE0); GLubyte* image=new GLubyte[256*256*3]; for(int y=0;y<256;++y) { const int t=y>>5; GLubyte* pi=image+y*256*3; for(int x=0;x<256;++x) { const int s=x>>5; const GLubyte b=180; GLubyte c=b+((s+t&1)&1)*(255-b); pi[0]=255; pi[1]=c; pi[2]=c; pi+=3; } } glGenTextures(1,(GLuint*)&m_texturehandle); glBindTexture(GL_TEXTURE_2D,m_texturehandle); glTexEnvf(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_MODULATE); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT); glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT); gluBuild2DMipmaps(GL_TEXTURE_2D,3,256,256,GL_RGB,GL_UNSIGNED_BYTE,image); delete[] image; m_textureinitialized=true; } // glMatrixMode(GL_TEXTURE); // glLoadIdentity(); // glMatrixMode(GL_MODELVIEW); glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D,m_texturehandle); } else
bool m_useWalkDirection; btScalar m_velocityTimeInterval; int m_upAxis; static btVector3* getUpAxisDirections(); bool m_interpolateUp; bool full_drop; bool bounce_fix; btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal); btVector3 parallelComponent(const btVector3& direction, const btVector3& normal); btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal); bool recoverFromPenetration(btCollisionWorld* collisionWorld); void stepUp(btCollisionWorld* collisionWorld); void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); void stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove); void stepDown(btCollisionWorld* collisionWorld, btScalar dt); public: BT_DECLARE_ALIGNED_ALLOCATOR(); KinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, int upAxis = 1); ~KinematicCharacterController(); ///btActionInterface interface virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) { preStep(collisionWorld); playerStep(collisionWorld, deltaTime);
bool btBulletWorldImporter::loadFileFromMemory( bParse::btBulletFile* bulletFile2) { bool ok = (bulletFile2->getFlags()& bParse::FD_OK)!=0; if (ok) bulletFile2->parse(m_verboseDumpAllTypes); else return false; if (m_verboseDumpAllTypes) { bulletFile2->dumpChunks(bulletFile2->getFileDNA()); } int i; btHashMap<btHashPtr,btCollisionShape*> shapeMap; for (i=0;i<bulletFile2->m_collisionShapes.size();i++) { btCollisionShapeData* shapeData = (btCollisionShapeData*)bulletFile2->m_collisionShapes[i]; btCollisionShape* shape = convertCollisionShape(shapeData); if (shape) shapeMap.insert(shapeData,shape); } btHashMap<btHashPtr,btCollisionObject*> bodyMap; for (i=0;i<bulletFile2->m_rigidBodies.size();i++) { if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btRigidBodyDoubleData* colObjData = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i]; btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f); btVector3 localInertia; localInertia.setZero(); btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeDouble(colObjData->m_collisionObjectData.m_worldTransform); // startTransform.setBasis(btMatrix3x3::getIdentity()); btCollisionShape* shape = (btCollisionShape*)*shapePtr; if (mass) { shape->calculateLocalInertia(mass,localInertia); } bool isDynamic = mass!=0.f; btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape); bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } else { btRigidBodyFloatData* colObjData = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i]; btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f); btVector3 localInertia; localInertia.setZero(); btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeFloat(colObjData->m_collisionObjectData.m_worldTransform); // startTransform.setBasis(btMatrix3x3::getIdentity()); btCollisionShape* shape = (btCollisionShape*)*shapePtr; if (mass) { shape->calculateLocalInertia(mass,localInertia); } bool isDynamic = mass!=0.f; btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape); bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } } for (i=0;i<bulletFile2->m_collisionObjects.size();i++) { if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btCollisionObjectDoubleData* colObjData = (btCollisionObjectDoubleData*)bulletFile2->m_collisionObjects[i]; btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeDouble(colObjData->m_worldTransform); btCollisionShape* shape = (btCollisionShape*)*shapePtr; btCollisionObject* body = createCollisionObject(startTransform,shape); bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } else { btCollisionObjectFloatData* colObjData = (btCollisionObjectFloatData*)bulletFile2->m_collisionObjects[i]; btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; startTransform.deSerializeFloat(colObjData->m_worldTransform); btCollisionShape* shape = (btCollisionShape*)*shapePtr; btCollisionObject* body = createCollisionObject(startTransform,shape); bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } printf("bla"); } for (i=0;i<bulletFile2->m_constraints.size();i++) { btTypedConstraintData* constraintData = (btTypedConstraintData*)bulletFile2->m_constraints[i]; btCollisionObject** colAptr = bodyMap.find(constraintData->m_rbA); btCollisionObject** colBptr = bodyMap.find(constraintData->m_rbB); btRigidBody* rbA = 0; btRigidBody* rbB = 0; if (colAptr) { rbA = btRigidBody::upcast(*colAptr); if (!rbA) rbA = &getFixedBody(); } if (colBptr) { rbB = btRigidBody::upcast(*colBptr); if (!rbB) rbB = &getFixedBody(); } switch (constraintData->m_objectType) { case POINT2POINT_CONSTRAINT_TYPE: { btPoint2PointConstraint* constraint = 0; if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btPoint2PointConstraintDoubleData* p2pData = (btPoint2PointConstraintDoubleData*)constraintData; if (rbA && rbB) { btVector3 pivotInA,pivotInB; pivotInA.deSerializeDouble(p2pData->m_pivotInA); pivotInB.deSerializeDouble(p2pData->m_pivotInB); constraint = new btPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); } else { btVector3 pivotInA; pivotInA.deSerializeDouble(p2pData->m_pivotInA); constraint = new btPoint2PointConstraint(*rbA,pivotInA); } } else { btPoint2PointConstraintFloatData* p2pData = (btPoint2PointConstraintFloatData*)constraintData; if (rbA&& rbB) { btVector3 pivotInA,pivotInB; pivotInA.deSerializeFloat(p2pData->m_pivotInA); pivotInB.deSerializeFloat(p2pData->m_pivotInB); constraint = new btPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); } else { btVector3 pivotInA; pivotInA.deSerializeFloat(p2pData->m_pivotInA); constraint = new btPoint2PointConstraint(*rbA,pivotInA); } } m_dynamicsWorld->addConstraint(constraint,constraintData->m_disableCollisionsBetweenLinkedBodies!=0); constraint->setDbgDrawSize(constraintData->m_dbgDrawSize); break; } case HINGE_CONSTRAINT_TYPE: { btHingeConstraint* hinge = 0; if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btHingeConstraintDoubleData* hingeData = (btHingeConstraintDoubleData*)constraintData; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeDouble(hingeData->m_rbAFrame); rbBFrame.deSerializeDouble(hingeData->m_rbBFrame); hinge = new btHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0); } else { btTransform rbAFrame; rbAFrame.deSerializeDouble(hingeData->m_rbAFrame); hinge = new btHingeConstraint(*rbA,rbAFrame,hingeData->m_useReferenceFrameA!=0); } if (hingeData->m_enableAngularMotor) { hinge->enableAngularMotor(true,hingeData->m_motorTargetVelocity,hingeData->m_maxMotorImpulse); } hinge->setAngularOnly(hingeData->m_angularOnly!=0); hinge->setLimit(btScalar(hingeData->m_lowerLimit),btScalar(hingeData->m_upperLimit),btScalar(hingeData->m_limitSoftness),btScalar(hingeData->m_biasFactor),btScalar(hingeData->m_relaxationFactor)); } else { btHingeConstraintFloatData* hingeData = (btHingeConstraintFloatData*)constraintData; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(hingeData->m_rbAFrame); rbBFrame.deSerializeFloat(hingeData->m_rbBFrame); hinge = new btHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0); } else { btTransform rbAFrame; rbAFrame.deSerializeFloat(hingeData->m_rbAFrame); hinge = new btHingeConstraint(*rbA,rbAFrame,hingeData->m_useReferenceFrameA!=0); } if (hingeData->m_enableAngularMotor) { hinge->enableAngularMotor(true,hingeData->m_motorTargetVelocity,hingeData->m_maxMotorImpulse); } hinge->setAngularOnly(hingeData->m_angularOnly!=0); hinge->setLimit(btScalar(hingeData->m_lowerLimit),btScalar(hingeData->m_upperLimit),btScalar(hingeData->m_limitSoftness),btScalar(hingeData->m_biasFactor),btScalar(hingeData->m_relaxationFactor)); } m_dynamicsWorld->addConstraint(hinge,constraintData->m_disableCollisionsBetweenLinkedBodies!=0); hinge->setDbgDrawSize(constraintData->m_dbgDrawSize); break; } case CONETWIST_CONSTRAINT_TYPE: { btConeTwistConstraintData* coneData = (btConeTwistConstraintData*)constraintData; btConeTwistConstraint* coneTwist = 0; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(coneData->m_rbAFrame); rbBFrame.deSerializeFloat(coneData->m_rbBFrame); coneTwist = new btConeTwistConstraint(*rbA,*rbB,rbAFrame,rbBFrame); } else { btTransform rbAFrame; rbAFrame.deSerializeFloat(coneData->m_rbAFrame); coneTwist = new btConeTwistConstraint(*rbA,rbAFrame); } coneTwist->setLimit(coneData->m_swingSpan1,coneData->m_swingSpan2,coneData->m_twistSpan,coneData->m_limitSoftness,coneData->m_biasFactor,coneData->m_relaxationFactor); coneTwist->setDamping(coneData->m_damping); m_dynamicsWorld->addConstraint(coneTwist,constraintData->m_disableCollisionsBetweenLinkedBodies!=0); coneTwist->setDbgDrawSize(constraintData->m_dbgDrawSize); break; } case D6_CONSTRAINT_TYPE: { btGeneric6DofConstraintData* dofData = (btGeneric6DofConstraintData*)constraintData; btGeneric6DofConstraint* dof = 0; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(dofData->m_rbAFrame); rbBFrame.deSerializeFloat(dofData->m_rbBFrame); dof = new btGeneric6DofConstraint(*rbA,*rbB,rbAFrame,rbBFrame,dofData->m_useLinearReferenceFrameA!=0); } else { btTransform rbBFrame; rbBFrame.deSerializeFloat(dofData->m_rbBFrame); dof = new btGeneric6DofConstraint(*rbB,rbBFrame,dofData->m_useLinearReferenceFrameA!=0); } btVector3 angLowerLimit,angUpperLimit, linLowerLimit,linUpperlimit; angLowerLimit.deSerializeFloat(dofData->m_angularLowerLimit); angUpperLimit.deSerializeFloat(dofData->m_angularUpperLimit); linLowerLimit.deSerializeFloat(dofData->m_linearLowerLimit); linUpperlimit.deSerializeFloat(dofData->m_linearUpperLimit); dof->setAngularLowerLimit(angLowerLimit); dof->setAngularUpperLimit(angUpperLimit); dof->setLinearLowerLimit(linLowerLimit); dof->setLinearUpperLimit(linUpperlimit); m_dynamicsWorld->addConstraint(dof,constraintData->m_disableCollisionsBetweenLinkedBodies!=0); dof->setDbgDrawSize(constraintData->m_dbgDrawSize); break; } case SLIDER_CONSTRAINT_TYPE: { btSliderConstraintData* sliderData = (btSliderConstraintData*)constraintData; btSliderConstraint* slider = 0; if (rbA&& rbB) { btTransform rbAFrame,rbBFrame; rbAFrame.deSerializeFloat(sliderData->m_rbAFrame); rbBFrame.deSerializeFloat(sliderData->m_rbBFrame); slider = new btSliderConstraint(*rbA,*rbB,rbAFrame,rbBFrame,sliderData->m_useLinearReferenceFrameA!=0); } else { btTransform rbBFrame; rbBFrame.deSerializeFloat(sliderData->m_rbBFrame); slider = new btSliderConstraint(*rbB,rbBFrame,sliderData->m_useLinearReferenceFrameA!=0); } slider->setLowerLinLimit(sliderData->m_linearLowerLimit); slider->setUpperLinLimit(sliderData->m_linearUpperLimit); slider->setLowerAngLimit(sliderData->m_angularLowerLimit); slider->setUpperAngLimit(sliderData->m_angularUpperLimit); slider->setUseFrameOffset(sliderData->m_useOffsetForConstraintFrame!=0); m_dynamicsWorld->addConstraint(slider,constraintData->m_disableCollisionsBetweenLinkedBodies!=0); slider->setDbgDrawSize(constraintData->m_dbgDrawSize); break; } default: { printf("unknown constraint type\n"); } }; } return true; }
void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax) { m_useQuantization = useQuantizedAabbCompression; // NodeArray triangleNodes; struct NodeTriangleCallback : public btInternalTriangleIndexCallback { NodeArray& m_triangleNodes; NodeTriangleCallback& operator=(NodeTriangleCallback& other) { m_triangleNodes = other.m_triangleNodes; return *this; } NodeTriangleCallback(NodeArray& triangleNodes) :m_triangleNodes(triangleNodes) { } virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) { btOptimizedBvhNode node; btVector3 aabbMin,aabbMax; aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); aabbMax.setMax(triangle[1]); aabbMin.setMin(triangle[2]); aabbMax.setMax(triangle[2]); //with quantization? node.m_aabbMinOrg = aabbMin; node.m_aabbMaxOrg = aabbMax; node.m_escapeIndex = -1; //for child nodes node.m_subPart = partId; node.m_triangleIndex = triangleIndex; m_triangleNodes.push_back(node); } }; struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback { QuantizedNodeArray& m_triangleNodes; const btQuantizedBvh* m_optimizedTree; // for quantization QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) { m_triangleNodes = other.m_triangleNodes; m_optimizedTree = other.m_optimizedTree; return *this; } QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree) :m_triangleNodes(triangleNodes),m_optimizedTree(tree) { } virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) { // The partId and triangle index must fit in the same (positive) integer btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS)); btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS))); //negative indices are reserved for escapeIndex btAssert(triangleIndex>=0); btQuantizedBvhNode node; btVector3 aabbMin,aabbMax; aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); aabbMax.setMax(triangle[1]); aabbMin.setMin(triangle[2]); aabbMax.setMax(triangle[2]); //PCK: add these checks for zero dimensions of aabb const btScalar MIN_AABB_DIMENSION = btScalar(0.002); const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001); if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION) { aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION); aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION); } if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION) { aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION); aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION); } if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION) { aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION); aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION); } m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_triangleNodes.push_back(node); } }; int numLeafNodes = 0; if (m_useQuantization) { //initialize quantization values setQuantizationValues(bvhAabbMin,bvhAabbMax); QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_quantizedLeafNodes.size(); m_quantizedContiguousNodes.resize(2*numLeafNodes); } else { NodeTriangleCallback callback(m_leafNodes); btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_leafNodes.size(); m_contiguousNodes.resize(2*numLeafNodes); } m_curNodeIndex = 0; buildTree(0,numLeafNodes); ///if the entire tree is small then subtree size, we need to create a header info for the tree if(m_useQuantization && !m_SubtreeHeaders.size()) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); subtree.m_rootNodeIndex = 0; subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); } //PCK: update the copy of the size m_subtreeHeaderCount = m_SubtreeHeaders.size(); //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary m_quantizedLeafNodes.clear(); m_leafNodes.clear(); }
btRigidBody& btBulletWorldImporter::getFixedBody() { static btRigidBody s_fixed(0, 0,0); s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); return s_fixed; }
btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos) { ///find relative velocity // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); btVector3 vel1; bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); /// apply displacement correction //positional error (zeroth order error) btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); btScalar lo = btScalar(-BT_LARGE_FLOAT); btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar minLimit = m_lowerLimit[limit_index]; btScalar maxLimit = m_upperLimit[limit_index]; //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = btScalar(0.); } else { if (depth < minLimit) { depth -= minLimit; hi = btScalar(0.); } else { return 0.0f; } } } } btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; btScalar sum = oldNormalImpulse + normalImpulse; m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; //body1.applyImpulse( impulse_vector, rel_pos1); //body2.applyImpulse(-impulse_vector, rel_pos2); btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); return normalImpulse; }
btCollisionShape* btBulletWorldImporter::convertCollisionShape( btCollisionShapeData* shapeData ) { btCollisionShape* shape = 0; switch (shapeData->m_shapeType) { case STATIC_PLANE_PROXYTYPE: { btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData; btVector3 planeNormal,localScaling; planeNormal.deSerializeFloat(planeData->m_planeNormal); localScaling.deSerializeFloat(planeData->m_localScaling); shape = new btStaticPlaneShape(planeNormal,btScalar(planeData->m_planeConstant)); shape->setLocalScaling(localScaling); break; } case GIMPACT_SHAPE_PROXYTYPE: { btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData; if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE) { btTriangleIndexVertexArray* meshInterface = createMeshInterface(gimpactData->m_meshInterface); btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface); btVector3 localScaling; localScaling.deSerializeFloat(gimpactData->m_localScaling); gimpactShape->setLocalScaling(localScaling); gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin)); gimpactShape->updateBound(); shape = gimpactShape; } else { printf("unsupported gimpact sub type\n"); } break; } case CYLINDER_SHAPE_PROXYTYPE: case CAPSULE_SHAPE_PROXYTYPE: case BOX_SHAPE_PROXYTYPE: case SPHERE_SHAPE_PROXYTYPE: case MULTI_SPHERE_SHAPE_PROXYTYPE: case CONVEX_HULL_SHAPE_PROXYTYPE: { btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; btVector3 implicitShapeDimensions; implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); btVector3 localScaling; localScaling.deSerializeFloat(bsd->m_localScaling); btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin); switch (shapeData->m_shapeType) { case BOX_SHAPE_PROXYTYPE: { shape = createBoxShape(implicitShapeDimensions/localScaling+margin); break; } case SPHERE_SHAPE_PROXYTYPE: { shape = createSphereShape(implicitShapeDimensions.getX()); break; } case CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; switch (capData->m_upAxis) { case 0: { shape = createCapsuleShapeX(implicitShapeDimensions.getY(),2*implicitShapeDimensions.getX()); break; } case 1: { shape = createCapsuleShapeY(implicitShapeDimensions.getX(),2*implicitShapeDimensions.getY()); break; } case 2: { shape = createCapsuleShapeZ(implicitShapeDimensions.getX(),2*implicitShapeDimensions.getZ()); break; } default: { printf("error: wrong up axis for btCapsuleShape\n"); } }; break; } case CYLINDER_SHAPE_PROXYTYPE: { btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData; btVector3 halfExtents = implicitShapeDimensions+margin; switch (cylData->m_upAxis) { case 0: { shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX()); break; } case 1: { shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY()); break; } case 2: { shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ()); break; } default: { printf("unknown Cylinder up axis\n"); } }; break; } case MULTI_SPHERE_SHAPE_PROXYTYPE: { btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; int numSpheres = mss->m_localPositionArraySize; btAlignedObjectArray<btVector3> tmpPos; btAlignedObjectArray<btScalar> radii; radii.resize(numSpheres); tmpPos.resize(numSpheres); int i; for ( i=0;i<numSpheres;i++) { tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); radii[i] = mss->m_localPositionArrayPtr[i].m_radius; } shape = new btMultiSphereShape(&tmpPos[0],&radii[0],numSpheres); break; } case CONVEX_HULL_SHAPE_PROXYTYPE: { // int sz = sizeof(btConvexHullShapeData); // int sz2 = sizeof(btConvexInternalShapeData); // int sz3 = sizeof(btCollisionShapeData); btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; int numPoints = convexData->m_numUnscaledPoints; btAlignedObjectArray<btVector3> tmpPoints; tmpPoints.resize(numPoints); int i; for ( i=0;i<numPoints;i++) { #ifdef BT_USE_DOUBLE_PRECISION if (convexData->m_unscaledPointsDoublePtr) tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); if (convexData->m_unscaledPointsFloatPtr) tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); #else if (convexData->m_unscaledPointsFloatPtr) tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); if (convexData->m_unscaledPointsDoublePtr) tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); #endif //BT_USE_DOUBLE_PRECISION } btConvexHullShape* hullShape = createConvexHullShape(); for (i=0;i<numPoints;i++) { hullShape->addPoint(tmpPoints[i]); } shape = hullShape; break; } default: { printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType); } } if (shape) { shape->setMargin(bsd->m_collisionMargin); btVector3 localScaling; localScaling.deSerializeFloat(bsd->m_localScaling); shape->setLocalScaling(localScaling); } break; } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData; btTriangleIndexVertexArray* meshInterface = createMeshInterface(trimesh->m_meshInterface); btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); meshInterface->setScaling(scaling); btCollisionShape* trimeshShape = createBvhTriangleMeshShape(meshInterface); trimeshShape->setMargin(trimesh->m_collisionMargin); shape = trimeshShape; //printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin); break; } case COMPOUND_SHAPE_PROXYTYPE: { btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; btCompoundShape* compoundShape = createCompoundShape(); btAlignedObjectArray<btCollisionShape*> childShapes; for (int i=0;i<compoundData->m_numChildShapes;i++) { btCollisionShape* childShape = convertCollisionShape(compoundData->m_childShapePtr[i].m_childShape); if (childShape) { btTransform localTransform; localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); compoundShape->addChildShape(localTransform,childShape); } else { printf("error: couldn't create childShape for compoundShape\n"); } } shape = compoundShape; break; } default: { printf("unsupported shape type (%d)\n",shapeData->m_shapeType); } } return shape; }
static btDbvtNode* topdown(btDbvt* pdbvt, tNodeArray& leaves, int bu_treshold) { static const btVector3 axis[]={btVector3(1,0,0), btVector3(0,1,0), btVector3(0,0,1)}; if(leaves.size()>1) { if(leaves.size()>bu_treshold) { const btDbvtVolume vol=bounds(leaves); const btVector3 org=vol.Center(); tNodeArray sets[2]; int bestaxis=-1; int bestmidp=leaves.size(); int splitcount[3][2]={{0,0},{0,0},{0,0}}; int i; for( i=0;i<leaves.size();++i) { const btVector3 x=leaves[i]->volume.Center()-org; for(int j=0;j<3;++j) { ++splitcount[j][btDot(x,axis[j])>0?1:0]; } } for( i=0;i<3;++i) { if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) { const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); if(midp<bestmidp) { bestaxis=i; bestmidp=midp; } } } if(bestaxis>=0) { sets[0].reserve(splitcount[bestaxis][0]); sets[1].reserve(splitcount[bestaxis][1]); split(leaves,sets[0],sets[1],org,axis[bestaxis]); } else { sets[0].reserve(leaves.size()/2+1); sets[1].reserve(leaves.size()/2); for(int i=0,ni=leaves.size();i<ni;++i) { sets[i&1].push_back(leaves[i]); } } btDbvtNode* node=createnode(pdbvt,0,vol,0); node->childs[0]=topdown(pdbvt,sets[0],bu_treshold); node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); node->childs[0]->parent=node; node->childs[1]->parent=node; return(node); } else { bottomup(pdbvt,leaves); return(leaves[0]); } } return(leaves[0]); }
void RollingFrictionDemo::initPhysics() { m_guiHelper->setUpAxis(2); ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btDefaultCollisionConfiguration(); //m_collisionConfiguration->setConvexConvexMultipointIterations(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); m_broadphase = new btDbvtBroadphase(); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; m_solver = sol; m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); // m_dynamicsWorld->getSolverInfo().m_singleAxisRollingFrictionThreshold = 0.f;//faster but lower quality m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { ///create a few basic rigid bodies btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(25.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0, 0, -28)); groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0, 0, 0); if (isDynamic) groundShape->calculateLocalInertia(mass, localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); body->setFriction(.5); //add the body to the dynamics world m_dynamicsWorld->addRigidBody(body); } { ///create a few basic rigid bodies btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.), btScalar(100.), btScalar(50.))); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0, 0, -54)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.f); btVector3 localInertia(0, 0, 0); if (isDynamic) groundShape->calculateLocalInertia(mass, localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); body->setFriction(.1); //add the body to the dynamics world m_dynamicsWorld->addRigidBody(body); } { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance #define NUM_SHAPES 10 btCollisionShape* colShapes[NUM_SHAPES] = { new btSphereShape(btScalar(0.5)), new btCapsuleShape(0.25, 0.5), new btCapsuleShapeX(0.25, 0.5), new btCapsuleShapeZ(0.25, 0.5), new btConeShape(0.25, 0.5), new btConeShapeX(0.25, 0.5), new btConeShapeZ(0.25, 0.5), new btCylinderShape(btVector3(0.25, 0.5, 0.25)), new btCylinderShapeX(btVector3(0.5, 0.25, 0.25)), new btCylinderShapeZ(btVector3(0.25, 0.25, 0.5)), }; for (int i = 0; i < NUM_SHAPES; i++) m_collisionShapes.push_back(colShapes[i]); /// Create Dynamic Objects btTransform startTransform; startTransform.setIdentity(); btScalar mass(1.f); //rigidbody is dynamic if and only if mass is non zero, otherwise static float start_x = START_POS_X - ARRAY_SIZE_X / 2; float start_y = START_POS_Y; float start_z = START_POS_Z - ARRAY_SIZE_Z / 2; { int shapeIndex = 0; for (int k = 0; k < ARRAY_SIZE_Y; k++) { for (int i = 0; i < ARRAY_SIZE_X; i++) { for (int j = 0; j < ARRAY_SIZE_Z; j++) { startTransform.setOrigin(SCALING * btVector3( btScalar(2.0 * i + start_x), btScalar(2.0 * j + start_z), btScalar(20 + 2.0 * k + start_y))); shapeIndex++; btCollisionShape* colShape = colShapes[shapeIndex % NUM_SHAPES]; bool isDynamic = (mass != 0.f); btVector3 localInertia(0, 0, 0); if (isDynamic) colShape->calculateLocalInertia(mass, localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); body->setFriction(1.f); body->setRollingFriction(.1); body->setSpinningFriction(0.1); body->setAnisotropicFriction(colShape->getAnisotropicRollingFrictionDirection(), btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); m_dynamicsWorld->addRigidBody(body); } } } } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); if (0) { btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); char resourcePath[1024]; if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024)) { FILE* f = fopen(resourcePath, "wb"); fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f); fclose(f); } } }
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_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 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_J2linearAxis[i*skip]=0; info->m_J2linearAxis[i*skip+1]=0; info->m_J2linearAxis[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; info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[skip + 1] = -1; info->m_J2linearAxis[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 normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; btScalar k = info->fps * normalErp; 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 bool powered = getEnableAngularMotor(); 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 = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; 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 }