btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) , m_frameInB(frameInB) , m_rotateOrder(rotOrder) , m_flags(0) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; calculateTransforms(); }
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) :btTypedObject(type), m_userConstraintType(-1), m_userConstraintId(-1), m_needsFeedback(false), m_rbA(rbA), m_rbB(getFixedBody()), m_appliedImpulse(btScalar(0.)), m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { }
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameB), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(false) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; calculateTransforms(); }
btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), m_useSolveConstraintObsolete(false), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); initParams(); }
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) :btTypedObject(type), m_userConstraintType(-1), m_userConstraintId(-1), m_breakingImpulseThreshold(SIMD_INFINITY), m_isEnabled(true), m_needsFeedback(false), m_rbA(rbA), m_rbB(getFixedBody()), m_appliedImpulse(btScalar(0.)), m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) { }
btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) { updateWheelTransformsWS( wheel,false); btScalar depth = -1; btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; wheel.m_raycastInfo.m_contactPointWS = source + rayvector; const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; btScalar param = btScalar(0.); btVehicleRaycaster::btVehicleRaycasterResult rayResults; btAssert(m_vehicleRaycaster); void* object = m_vehicleRaycaster->castRay(source,target,rayResults); wheel.m_raycastInfo.m_groundObject = 0; if (object) { param = rayResults.m_distFraction; depth = raylen * rayResults.m_distFraction; wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; wheel.m_raycastInfo.m_isInContact = true; wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!; //wheel.m_raycastInfo.m_groundObject = object; btScalar hitDistance = param*raylen; wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; //clamp on max suspension travel btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; } if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; } wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); btVector3 chassis_velocity_at_contactPoint; btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); if ( denominator >= btScalar(-0.1)) { wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { btScalar inv = btScalar(-1.) / denominator; wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_clippedInvContactDotSuspension = inv; } } else { //put wheel info as in rest position wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; wheel.m_clippedInvContactDotSuspension = btScalar(1.0); } return depth; }
bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2) { int i; for (i=0;i<bulletFile2->m_bvhs.size();i++) { btOptimizedBvh* bvh = createOptimizedBvh(); if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btQuantizedBvhDoubleData* bvhData = (btQuantizedBvhDoubleData*)bulletFile2->m_bvhs[i]; bvh->deSerializeDouble(*bvhData); } else { btQuantizedBvhFloatData* bvhData = (btQuantizedBvhFloatData*)bulletFile2->m_bvhs[i]; bvh->deSerializeFloat(*bvhData); } m_bvhMap.insert(bulletFile2->m_bvhs[i],bvh); } 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) { // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); shapeMap.insert(shapeData,shape); } if (shape&& shapeData->m_name) { char* newname = duplicateName(shapeData->m_name); m_objectNameMap.insert(shape,newname); m_nameShapeMap.insert(newname,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 (shape->isNonMoving()) { mass = 0.f; } if (mass) { shape->calculateLocalInertia(mass,localInertia); } bool isDynamic = mass!=0.f; btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape,colObjData->m_collisionObjectData.m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY 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 (shape->isNonMoving()) { mass = 0.f; } if (mass) { shape->calculateLocalInertia(mass,localInertia); } bool isDynamic = mass!=0.f; btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape,colObjData->m_collisionObjectData.m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY 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,colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY 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,colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY 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(); } btTypedConstraint* constraint = 0; switch (constraintData->m_objectType) { case POINT2POINT_CONSTRAINT_TYPE: { 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 = createPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); } else { btVector3 pivotInA; pivotInA.deSerializeDouble(p2pData->m_pivotInA); constraint = createPoint2PointConstraint(*rbA,pivotInA); } } else { btPoint2PointConstraintFloatData* p2pData = (btPoint2PointConstraintFloatData*)constraintData; if (rbA&& rbB) { btVector3 pivotInA,pivotInB; pivotInA.deSerializeFloat(p2pData->m_pivotInA); pivotInB.deSerializeFloat(p2pData->m_pivotInB); constraint = createPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); } else { btVector3 pivotInA; pivotInA.deSerializeFloat(p2pData->m_pivotInA); constraint = createPoint2PointConstraint(*rbA,pivotInA); } } 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 = createHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0); } else { btTransform rbAFrame; rbAFrame.deSerializeDouble(hingeData->m_rbAFrame); hinge = createHingeConstraint(*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 = createHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0); } else { btTransform rbAFrame; rbAFrame.deSerializeFloat(hingeData->m_rbAFrame); hinge = createHingeConstraint(*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)); } constraint = hinge; 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 = createConeTwistConstraint(*rbA,*rbB,rbAFrame,rbBFrame); } else { btTransform rbAFrame; rbAFrame.deSerializeFloat(coneData->m_rbAFrame); coneTwist = createConeTwistConstraint(*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); constraint = coneTwist; 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 = createGeneric6DofConstraint(*rbA,*rbB,rbAFrame,rbBFrame,dofData->m_useLinearReferenceFrameA!=0); } else { if (rbB) { btTransform rbBFrame; rbBFrame.deSerializeFloat(dofData->m_rbBFrame); dof = createGeneric6DofConstraint(*rbB,rbBFrame,dofData->m_useLinearReferenceFrameA!=0); } else { printf("Error in btWorldImporter::createGeneric6DofConstraint: missing rbB\n"); } } if (dof) { 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); } constraint = dof; 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 = createSliderConstraint(*rbA,*rbB,rbAFrame,rbBFrame,sliderData->m_useLinearReferenceFrameA!=0); } else { btTransform rbBFrame; rbBFrame.deSerializeFloat(sliderData->m_rbBFrame); slider = createSliderConstraint(*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); constraint = slider; break; } default: { printf("unknown constraint type\n"); } }; if (constraint) { constraint->setDbgDrawSize(constraintData->m_dbgDrawSize); if (constraintData->m_name) { char* newname = duplicateName(constraintData->m_name); m_nameConstraintMap.insert(newname,constraint); m_objectNameMap.insert(constraint,newname); } if(m_dynamicsWorld) m_dynamicsWorld->addConstraint(constraint,constraintData->m_disableCollisionsBetweenLinkedBodies!=0); } } return true; }
btTypedConstraint* gkDynamicsWorld::createConstraint(btRigidBody* rbA, btRigidBody*rbB, const gkPhysicsConstraintProperties& props) { btVector3 pivotInA(gkMathUtils::get(props.m_pivot)); btVector3 pivotInB(0,0,0); pivotInB = rbB ? rbB->getCenterOfMassTransform().inverse()(rbA->getCenterOfMassTransform()(pivotInA)) : rbA->getCenterOfMassTransform() * pivotInA; //localConstraintFrameBasis btMatrix3x3 localCFrame; localCFrame.setEulerZYX(props.m_axis.x, props.m_axis.y, props.m_axis.z); btVector3 axisInA = localCFrame.getColumn(0); btVector3 axis1 = localCFrame.getColumn(1); btVector3 axis2 = localCFrame.getColumn(2); bool angularOnly = false; btTypedConstraint* constraint = 0; if (props.m_type == GK_BALL_CONSTRAINT) { btPoint2PointConstraint* p2p = 0; if (rbB) p2p = new btPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); else p2p = new btPoint2PointConstraint(*rbA,pivotInA); constraint = p2p; } else if (props.m_type == GK_HINGE_CONSTRAINT) { btHingeConstraint* hinge = 0; if (rbB) { btVector3 axisInB = rbB->getCenterOfMassTransform().getBasis().inverse() * (rbA->getCenterOfMassTransform().getBasis() * axisInA); hinge = new btHingeConstraint(*rbA, *rbB, pivotInA, pivotInB, axisInA, axisInB); } else { hinge = new btHingeConstraint(*rbA, pivotInA, axisInA); } hinge->setAngularOnly(angularOnly); constraint = hinge; } else if (props.m_type == GK_D6_CONSTRAINT) { btTransform frameInA; btTransform frameInB; if (axis1.length() == 0.0) { btPlaneSpace1(axisInA, axis1, axis2); } frameInA.getBasis().setValue(axisInA.x(), axis1.x(), axis2.x(), axisInA.y(), axis1.y(), axis2.y(), axisInA.z(), axis1.z(), axis2.z()); frameInA.setOrigin( pivotInA ); btTransform inv = rbB ? rbB->getCenterOfMassTransform().inverse() : btTransform::getIdentity(); btTransform globalFrameA = rbA->getCenterOfMassTransform() * frameInA; frameInB = inv * globalFrameA; bool useReferenceFrameA = true; if (!rbB) rbB = getFixedBody(); btGeneric6DofSpringConstraint* genericConstraint = new btGeneric6DofSpringConstraint(*rbA, *rbB, frameInA, frameInB, useReferenceFrameA); //if it is a generic 6DOF constraint, set all the limits accordingly int dof, dofbit=1; for (dof=0;dof<6;dof++) { if (props.m_flag & dofbit) { ///this access is a bloated, will probably make special cases for common arrays btScalar minLimit = props.m_minLimit[dof]; btScalar maxLimit = props.m_maxLimit[dof]; genericConstraint->setLimit(dof, minLimit, maxLimit); } else { //minLimit > maxLimit means free(disabled limit) for this degree of freedom genericConstraint->setLimit(dof, 1, -1); } dofbit<<=1; } constraint = genericConstraint; } else if (props.m_type == GK_CONETWIST_CONSTRAINT) { //TODO: implement conetwist constraint } return constraint; }
bool Wheel::update(btScalar dt, WheelContact & contact) { if (!updateContact(2 * radius)) return false; const Surface * surface = ray.getSurface(); if (!surface) return false; contact.frictionCoeff = tire.getTread() * surface->frictionTread + (1.0 - tire.getTread()) * surface->frictionNonTread; btRigidBody * bodyA = body; btRigidBody * bodyB = &getFixedBody(); if (btRigidBody::upcast(ray.m_collisionObject)) { bodyB = btRigidBody::upcast(ray.m_collisionObject); } btVector3 wheelTangent1 = transform.getBasis().getColumn(1); // forward btVector3 wheelTangent2 = transform.getBasis().getColumn(0); // right btVector3 wheelNormal = transform.getBasis().getColumn(2); // up btVector3 contactNormal = ray.getNormal(); btVector3 contactPointA = ray.getPoint(); btVector3 contactPointB = ray.getPoint(); btScalar stiffnessConstant = suspension.getStiffness(); btScalar dampingConstant = suspension.getDamping(); btScalar displacement = suspension.getDisplacement(); // update constraints btVector3 rA = contactPointA - bodyA->getCenterOfMassPosition(); btVector3 rB = contactPointB - bodyB->getCenterOfMassPosition(); btVector3 contactTangent1 = wheelTangent1 - contactNormal * contactNormal.dot(wheelTangent1); btVector3 contactTangent2 = wheelTangent2 - contactNormal * contactNormal.dot(wheelTangent2); contactTangent1.normalize(); contactTangent2.normalize(); // project wheel normal onto contact forward facing plane to calculate camber btVector3 projNormal = wheelNormal - wheelNormal.dot(contactTangent1) * contactTangent1; projNormal.normalize(); contact.camber = 0;//btAcos(projNormal.dot(contactNormal)) * SIMD_DEGS_PER_RAD; fixme contact.vR = shaft.getAngularVelocity() * radius; contact.bodyA = bodyA; contact.bodyB = bodyB; contact.rA = rA; contact.rB = rB; btVector3 vA = bodyA->getLinearVelocity() + bodyA->getAngularVelocity().cross(rA); btVector3 vB = bodyB->getLinearVelocity() + bodyB->getAngularVelocity().cross(rB); btVector3 vAB = vA - vB; // set suspension constraint { // CFM and ERP from spring stiffness and damping constants btScalar softness = 1.0f / (dt * (dt * stiffnessConstant + dampingConstant)); btScalar biasFactor = stiffnessConstant / (dt * stiffnessConstant + dampingConstant); btScalar velocityError = -biasFactor * displacement; btVector3 normal = contactNormal; btScalar denomA = bodyA->computeImpulseDenominator(contactPointA, normal); btScalar denomB = bodyB->computeImpulseDenominator(contactPointB, normal); btScalar jacDiagInv = 1 / (denomA + denomB + softness); contact.response.jacDiagInv = jacDiagInv; contact.response.rhs = -velocityError * jacDiagInv; contact.response.cfm = -softness * jacDiagInv; contact.response.lowerLimit = 0; contact.response.upperLimit = SIMD_INFINITY; contact.response.accumImpulse = 0; contact.response.normal = normal; contact.response.angularCompA = bodyA->getInvInertiaTensorWorld() * rA.cross(normal); contact.response.angularCompB = bodyB->getInvInertiaTensorWorld() * rB.cross(normal); } // set longitudinal friction constraint { btVector3 normal = contactTangent1; btScalar denomA = bodyA->computeImpulseDenominator(contactPointA, normal); btScalar denomB = bodyB->computeImpulseDenominator(contactPointB, normal); btScalar jacDiagInv = 1 / (denomA + denomB); btScalar velocityError = vAB.dot(normal) - contact.vR; contact.v1 = velocityError + contact.vR; contact.friction1.jacDiagInv = jacDiagInv; contact.friction1.rhs = -velocityError * jacDiagInv; contact.friction1.cfm = 0; contact.friction1.lowerLimit = 0; contact.friction1.upperLimit = 0; contact.friction1.accumImpulse = 0; contact.friction1.normal = normal; contact.friction1.angularCompA = bodyA->getInvInertiaTensorWorld() * rA.cross(normal); contact.friction1.angularCompB = bodyB->getInvInertiaTensorWorld() * rB.cross(normal); } // set lateral friction constraint { btVector3 normal = contactTangent2; btScalar denomA = bodyA->computeImpulseDenominator(contactPointA, normal); btScalar denomB = bodyB->computeImpulseDenominator(contactPointB, normal); btScalar jacDiagInv = 1 / (denomA + denomB); btScalar velocityError = vAB.dot(normal); contact.v2 = velocityError; contact.friction2.jacDiagInv = jacDiagInv; contact.friction2.rhs = -velocityError * jacDiagInv; contact.friction2.cfm = 0; contact.friction2.lowerLimit = 0; contact.friction2.upperLimit = 0; contact.friction2.accumImpulse = 0; contact.friction2.normal = normal; contact.friction2.angularCompA = bodyA->getInvInertiaTensorWorld() * rA.cross(normal); contact.friction2.angularCompB = bodyB->getInvInertiaTensorWorld() * rB.cross(normal); } /* // ABS abs_active = false; btScalar brake_torque = brake.getTorque(); btScalar slide = tire.getSlide(); btScalar ideal_slide = tire.getIdealSlide(); if (abs_enabled && brake_torque > 1E-3 && angvel > 1 && slide < -ideal_slide) { // predict new angvel btScalar angvel_delta = shaft.getAngularVelocity() - angvel; btScalar angvel_new = shaft.getAngularVelocity() + angvel_delta; // calculate brake torque correction to reach 95% ideal_slide btScalar angvel_target = (0.95 * ideal_slide + 1) * contact.v1 / radius; angvel_delta = angvel_new - angvel_target; if (angvel_delta < 0) { // set brake torque to reach angvel_target brake_torque += angvel_delta / dt * shaft.getInertia(); btScalar factor = brake_torque / brake.getMaxTorque(); btClamp(factor, btScalar(0), btScalar(1)); brake.setBrakeFactor(factor); abs_active = true; } } // TCS tcs_active = false; if (tcs_enabled && slide > ideal_slide) { // predict new angvel btScalar angvel_delta = shaft.getAngularVelocity() - angvel; btScalar angvel_new = shaft.getAngularVelocity() + angvel_delta; // calculate brake torque correction to reach 95% ideal_slide btScalar angvel_target = (0.95 * ideal_slide + 1) * contact.v1 / radius; angvel_delta = angvel_new - angvel_target; if (angvel_delta > 0) { // set brake torque to reach angvel_target btScalar brake_torque = angvel_delta / dt * shaft.getInertia(); btScalar factor = brake_torque / brake.getMaxTorque(); btClamp(factor, brake.getBrakeFactor(), btScalar(1)); brake.setBrakeFactor(factor); tcs_active = true; } } // store old angular velocity angvel = shaft.getAngularVelocity(); */ return true; }
bool btBulletWorldImporter::convertAllObjects( bParse::btBulletFile* bulletFile2) { m_shapeMap.clear(); m_bodyMap.clear(); int i; for (i=0;i<bulletFile2->m_bvhs.size();i++) { btOptimizedBvh* bvh = createOptimizedBvh(); if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btQuantizedBvhDoubleData* bvhData = (btQuantizedBvhDoubleData*)bulletFile2->m_bvhs[i]; bvh->deSerializeDouble(*bvhData); } else { btQuantizedBvhFloatData* bvhData = (btQuantizedBvhFloatData*)bulletFile2->m_bvhs[i]; bvh->deSerializeFloat(*bvhData); } m_bvhMap.insert(bulletFile2->m_bvhs[i],bvh); } for (i=0;i<bulletFile2->m_collisionShapes.size();i++) { btCollisionShapeData* shapeData = (btCollisionShapeData*)bulletFile2->m_collisionShapes[i]; btCollisionShape* shape = convertCollisionShape(shapeData); if (shape) { // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); m_shapeMap.insert(shapeData,shape); } if (shape&& shapeData->m_name) { char* newname = duplicateName(shapeData->m_name); m_objectNameMap.insert(shape,newname); m_nameShapeMap.insert(newname,shape); } } for (int i=0;i<bulletFile2->m_dynamicsWorldInfo.size();i++) { if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btDynamicsWorldDoubleData* solverInfoData = (btDynamicsWorldDoubleData*)bulletFile2->m_dynamicsWorldInfo[i]; btContactSolverInfo solverInfo; btVector3 gravity; gravity.deSerializeDouble(solverInfoData->m_gravity); solverInfo.m_tau = btScalar(solverInfoData->m_solverInfo.m_tau); solverInfo.m_damping = btScalar(solverInfoData->m_solverInfo.m_damping); solverInfo.m_friction = btScalar(solverInfoData->m_solverInfo.m_friction); solverInfo.m_timeStep = btScalar(solverInfoData->m_solverInfo.m_timeStep); solverInfo.m_restitution = btScalar(solverInfoData->m_solverInfo.m_restitution); solverInfo.m_maxErrorReduction = btScalar(solverInfoData->m_solverInfo.m_maxErrorReduction); solverInfo.m_sor = btScalar(solverInfoData->m_solverInfo.m_sor); solverInfo.m_erp = btScalar(solverInfoData->m_solverInfo.m_erp); solverInfo.m_erp2 = btScalar(solverInfoData->m_solverInfo.m_erp2); solverInfo.m_globalCfm = btScalar(solverInfoData->m_solverInfo.m_globalCfm); solverInfo.m_splitImpulsePenetrationThreshold = btScalar(solverInfoData->m_solverInfo.m_splitImpulsePenetrationThreshold); solverInfo.m_splitImpulseTurnErp = btScalar(solverInfoData->m_solverInfo.m_splitImpulseTurnErp); solverInfo.m_linearSlop = btScalar(solverInfoData->m_solverInfo.m_linearSlop); solverInfo.m_warmstartingFactor = btScalar(solverInfoData->m_solverInfo.m_warmstartingFactor); solverInfo.m_maxGyroscopicForce = btScalar(solverInfoData->m_solverInfo.m_maxGyroscopicForce); solverInfo.m_singleAxisRollingFrictionThreshold = btScalar(solverInfoData->m_solverInfo.m_singleAxisRollingFrictionThreshold); solverInfo.m_numIterations = solverInfoData->m_solverInfo.m_numIterations; solverInfo.m_solverMode = solverInfoData->m_solverInfo.m_solverMode; solverInfo.m_restingContactRestitutionThreshold = solverInfoData->m_solverInfo.m_restingContactRestitutionThreshold; solverInfo.m_minimumSolverBatchSize = solverInfoData->m_solverInfo.m_minimumSolverBatchSize; solverInfo.m_splitImpulse = solverInfoData->m_solverInfo.m_splitImpulse; setDynamicsWorldInfo(gravity,solverInfo); } else { btDynamicsWorldFloatData* solverInfoData = (btDynamicsWorldFloatData*)bulletFile2->m_dynamicsWorldInfo[i]; btContactSolverInfo solverInfo; btVector3 gravity; gravity.deSerializeFloat(solverInfoData->m_gravity); solverInfo.m_tau = solverInfoData->m_solverInfo.m_tau; solverInfo.m_damping = solverInfoData->m_solverInfo.m_damping; solverInfo.m_friction = solverInfoData->m_solverInfo.m_friction; solverInfo.m_timeStep = solverInfoData->m_solverInfo.m_timeStep; solverInfo.m_restitution = solverInfoData->m_solverInfo.m_restitution; solverInfo.m_maxErrorReduction = solverInfoData->m_solverInfo.m_maxErrorReduction; solverInfo.m_sor = solverInfoData->m_solverInfo.m_sor; solverInfo.m_erp = solverInfoData->m_solverInfo.m_erp; solverInfo.m_erp2 = solverInfoData->m_solverInfo.m_erp2; solverInfo.m_globalCfm = solverInfoData->m_solverInfo.m_globalCfm; solverInfo.m_splitImpulsePenetrationThreshold = solverInfoData->m_solverInfo.m_splitImpulsePenetrationThreshold; solverInfo.m_splitImpulseTurnErp = solverInfoData->m_solverInfo.m_splitImpulseTurnErp; solverInfo.m_linearSlop = solverInfoData->m_solverInfo.m_linearSlop; solverInfo.m_warmstartingFactor = solverInfoData->m_solverInfo.m_warmstartingFactor; solverInfo.m_maxGyroscopicForce = solverInfoData->m_solverInfo.m_maxGyroscopicForce; solverInfo.m_singleAxisRollingFrictionThreshold = solverInfoData->m_solverInfo.m_singleAxisRollingFrictionThreshold; solverInfo.m_numIterations = solverInfoData->m_solverInfo.m_numIterations; solverInfo.m_solverMode = solverInfoData->m_solverInfo.m_solverMode; solverInfo.m_restingContactRestitutionThreshold = solverInfoData->m_solverInfo.m_restingContactRestitutionThreshold; solverInfo.m_minimumSolverBatchSize = solverInfoData->m_solverInfo.m_minimumSolverBatchSize; solverInfo.m_splitImpulse = solverInfoData->m_solverInfo.m_splitImpulse; setDynamicsWorldInfo(gravity,solverInfo); } } for (i=0;i<bulletFile2->m_rigidBodies.size();i++) { if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION) { btRigidBodyDoubleData* colObjData = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i]; convertRigidBodyDouble(colObjData); } else { btRigidBodyFloatData* colObjData = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i]; convertRigidBodyFloat(colObjData); } } 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 = m_shapeMap.find(colObjData->m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; startTransform.deSerializeDouble(colObjData->m_worldTransform); btCollisionShape* shape = (btCollisionShape*)*shapePtr; btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); body->setFriction(btScalar(colObjData->m_friction)); body->setRestitution(btScalar(colObjData->m_restitution)); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY m_bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } else { btCollisionObjectFloatData* colObjData = (btCollisionObjectFloatData*)bulletFile2->m_collisionObjects[i]; btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); if (shapePtr && *shapePtr) { btTransform startTransform; colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; startTransform.deSerializeFloat(colObjData->m_worldTransform); btCollisionShape* shape = (btCollisionShape*)*shapePtr; btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; if (trimesh->getTriangleInfoMap()) { body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); } } #endif //USE_INTERNAL_EDGE_UTILITY m_bodyMap.insert(colObjData,body); } else { printf("error: no shape found\n"); } } } for (i=0;i<bulletFile2->m_constraints.size();i++) { btTypedConstraintData2* constraintData = (btTypedConstraintData2*)bulletFile2->m_constraints[i]; btTypedConstraintFloatData* singleC = (btTypedConstraintFloatData*)bulletFile2->m_constraints[i]; btTypedConstraintDoubleData* doubleC = (btTypedConstraintDoubleData*)bulletFile2->m_constraints[i]; btCollisionObject** colAptr = m_bodyMap.find(constraintData->m_rbA); btCollisionObject** colBptr = m_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(); } if (!rbA && !rbB) continue; bool isDoublePrecisionData = (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)!=0; if (isDoublePrecisionData) { if (bulletFile2->getVersion()>=282) { btTypedConstraintDoubleData* dc = (btTypedConstraintDoubleData*)constraintData; convertConstraintDouble(dc, rbA,rbB, bulletFile2->getVersion()); } else { //double-precision constraints were messed up until 2.82, try to recover data... btTypedConstraintData* oldData = (btTypedConstraintData*)constraintData; convertConstraintBackwardsCompatible281(oldData, rbA,rbB, bulletFile2->getVersion()); } } else { btTypedConstraintFloatData* dc = (btTypedConstraintFloatData*)constraintData; convertConstraintFloat(dc, rbA,rbB, bulletFile2->getVersion()); } } return true; }
btScalar btKart::rayCast(unsigned int index) { btWheelInfo &wheel = m_wheelInfo[index]; // Work around a bullet problem: when using a convex hull the raycast // would sometimes hit the chassis (which does not happen when using a // box shape). Therefore set the collision mask in the chassis body so // that it is not hit anymore. short int old_group=0; if(m_chassisBody->getBroadphaseHandle()) { old_group = m_chassisBody->getBroadphaseHandle() ->m_collisionFilterGroup; m_chassisBody->getBroadphaseHandle()->m_collisionFilterGroup = 0; } updateWheelTransformsWS( wheel,false); btScalar max_susp_len = wheel.getSuspensionRestLength() + wheel.m_maxSuspensionTravel; // Do a slightly longer raycast to see if the kart might soon hit the // ground and some 'cushioning' is needed to avoid that the chassis // hits the ground. btScalar raylen = max_susp_len + 0.5f; btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; wheel.m_raycastInfo.m_contactPointWS = source + rayvector; const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; btVehicleRaycaster::btVehicleRaycasterResult rayResults; btAssert(m_vehicleRaycaster); void* object = m_vehicleRaycaster->castRay(source,target,rayResults); wheel.m_raycastInfo.m_groundObject = 0; btScalar depth = raylen * rayResults.m_distFraction; if (object && depth < max_susp_len) { wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; wheel.m_raycastInfo.m_contactNormalWS.normalize(); wheel.m_raycastInfo.m_isInContact = true; ///@todo for driving on dynamic/movable objects!; wheel.m_raycastInfo.m_triangle_index = rayResults.m_triangle_index;; wheel.m_raycastInfo.m_groundObject = &getFixedBody(); wheel.m_raycastInfo.m_suspensionLength = depth; //clamp on max suspension travel btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravel; btScalar maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.m_maxSuspensionTravel; if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; } if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) { wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; } wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); btVector3 chassis_velocity_at_contactPoint; btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); if ( denominator >= btScalar(-0.1)) { wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { btScalar inv = btScalar(-1.) / denominator; wheel.m_suspensionRelativeVelocity = projVel * inv; wheel.m_clippedInvContactDotSuspension = inv; } } else { depth = btScalar(-1.0); //put wheel info as in rest position wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); wheel.m_suspensionRelativeVelocity = btScalar(0.0); wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; wheel.m_clippedInvContactDotSuspension = btScalar(1.0); } #define USE_VISUAL #ifndef USE_VISUAL m_visual_contact_point[index] = rayResults.m_hitPointInWorld; #else if(index==2 || index==3) { btTransform chassisTrans = getChassisWorldTransform(); if (getRigidBody()->getMotionState()) { getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); } btQuaternion q(m_visual_rotation, 0, 0); btQuaternion rot_new = chassisTrans.getRotation() * q; chassisTrans.setRotation(rot_new); btVector3 pos = m_kart->getKartModel()->getWheelGraphicsPosition(index); pos.setZ(pos.getZ()*0.9f); btVector3 source = chassisTrans( pos ); btVector3 target = source + rayvector; btVehicleRaycaster::btVehicleRaycasterResult rayResults; void* object = m_vehicleRaycaster->castRay(source,target,rayResults); m_visual_contact_point[index] = rayResults.m_hitPointInWorld; m_visual_contact_point[index-2] = source; m_visual_wheels_touch_ground &= (object!=NULL); } #endif if(m_chassisBody->getBroadphaseHandle()) { m_chassisBody->getBroadphaseHandle()->m_collisionFilterGroup = old_group; } return depth; } // rayCast
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; }