void addPickingConstraint(const btVector3& rayFrom, const btVector3& rayTo) { if (!dynamicsWorld) { return; } removePickingConstraint(); if (pickedObjectIndex <= 0 || pickedObjectIndex >= dynamicsWorld->getNumCollisionObjects()) { return; } pickedBody = btRigidBody::upcast(dynamicsWorld->getCollisionObjectArray()[pickedObjectIndex]); btVector3 pickPos = rayTo; btVector3 localPivot = pickedBody->getCenterOfMassTransform().inverse() * pickPos; pickConstraint = new btPoint2PointConstraint(*pickedBody,localPivot); pickedBody->setActivationState(DISABLE_DEACTIVATION); dynamicsWorld->addConstraint(pickConstraint,true); pickingDistance = (rayFrom-rayTo).length(); pickConstraint->m_setting.m_impulseClamp = 3.0f; pickConstraint->m_setting.m_tau = 0.001f; }
void HapticDevice::feedback(btDynamicsWorld &dynamic) { for(unsigned int i=0;i<m_nbDevices;i++) { bool ground_collide = false; // free move if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 )// (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2)) { m_hss[i].m_free.m_done = true; //m_hss[i].m_free.m_force = hduVector3Dd(0,0,0); } if(m_constraints[i] != NULL) { btRigidBody * myBody = &m_constraints[i]->getRigidBodyB(); btTransform myTrans = myBody->getWorldTransform(); m_effRenderPos = myTrans.getOrigin(); //Check collision if(m_constraints[i]->getUserConstraintPtr() != NULL) { //std::cout<< " se cas la " <<std::endl; m_hss[i].m_free.m_nbCollision = 1; //if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0) btCollisionObject * object = static_cast<btCollisionObject *>(m_constraints[i]->getUserConstraintPtr()); if(object->getInternalType()== btCollisionObject::CO_RIGID_BODY) { btRigidBody * collideBody = static_cast<btRigidBody *>(object); // collide with ground if(collideBody == m_ground) { m_hss[i].m_free.m_nbCollision = 2; ground_collide = true; } // collide with other object if(collideBody->getInvMass()!=0 && collideBody != m_ground) { if(m_itsConstraints[i] == NULL ) { // catch it if colide with it if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) == 0 ) { //create constraint btTransform bodyTrans = collideBody->getWorldTransform(); m_itsConstraints[i] = createConstraint(*myBody,*collideBody); dynamic.addConstraint(m_itsConstraints[i],true); m_newConstraint(m_ptr,collideBody,i); m_caught = collideBody; m_coll = true; m_devine = false; showTarget(collideBody); deactivateMove(); } }else // realise it when button 1 pressed if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0) { //remove constraint dynamic.removeConstraint(m_itsConstraints[i]); delete m_itsConstraints[i]; m_itsConstraints[i]=NULL; m_deleteConstraint(m_ptr,collideBody,i); //m_hss[i].setThrown(NULL); m_hss[i].m_free.m_done = true; showTarget(collideBody); m_variator = 0; m_coll = false; deactivateMove(); } } } } else { m_hss[i].m_free.m_nbCollision = 0; //if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0) if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0) { if(m_itsConstraints[i] != NULL ) { //remove constraint m_deleteConstraint(m_ptr,&m_itsConstraints[i]->getRigidBodyB(),i); dynamic.removeConstraint(m_itsConstraints[i]); delete m_itsConstraints[i]; m_itsConstraints[i]=NULL; m_hss[i].m_free.m_done = true; if(m_caught != NULL) showTarget(m_caught); m_coll = false; deactivateMove(); } } } if(m_itsConstraints[i]!=NULL ) m_hss[i].m_free.m_nbCollision = 1; else // launch an other target if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_2) == 0) { //m_canLaunch = true; } m_oldButtons[i]=m_hss[i].m_free.m_buttons; //put back cursor world position into device referencial <<<<< ------------------------------ btTransform offset(btMatrix3x3::getIdentity(),btVector3(0,0,-0.5)); m_effectors[i].setOrigin(myTrans.getOrigin()); m_effectors[i].mult(m_effectors[i],offset); btVector3 pos = m_cameraViews[i]->inverse()(myTrans.getOrigin()); pos*=SCALE_WORLD_TO_DEVICE; m_hss[i].m_free.m_realPosition.set(pos.getX(),pos.getY(),pos.getZ()+OFFSET_TO_CAMERA); } btTransform camInv = m_cameraViews[i]->inverse(); // compute feed back for ground if(ground_collide) { m_hss[i].m_free.m_force = groundForce(true,&(m_hss[i].m_free.m_position),&camInv); }else m_hss[i].m_free.m_force = hduVector3Dd(0,0,0); // detecte the direction HDdouble deplacement = betweenTwoPoints(m_hss[i].m_free.m_atThrowPos,m_hss[i].m_free.m_position); HDdouble distanceMax = Distance_max; hduVector3Dd move = m_hss[i].m_free.m_oldPosition - m_hss[i].m_free.m_position; HDdouble selectedDistance = 0; if( m_devine && m_thrownRigids != NULL && m_thrownRigids->size()>0 && deplacement > distanceMax && m_sible == 0 /* && !m_targetChoosen */){ HDdouble distance = Quick_Distance_max; //unsigned int targ = 0; for(unsigned int j = 0; j<m_thrownRigids->size(); j++){ HDdouble d = distanceToTrajectory(m_hss[i].m_free.m_position,j,&camInv); if((d)<distance){ if(m_Thrown != (*m_thrownRigids)[j]){ m_hss[i].m_free.m_currentThrown = (*m_thrownRigids)[j]; distance = d; m_Thrown = (*m_thrownRigids)[j]; m_impactPos = m_possibleImpact[j]; m_targetChoosen = true; if(m_Feedback){ activateMove(); } selectedDistance = d; m_index = j; m_sible = Nbr_frame_wait; } } } if(m_targetChoosen){ //showTarget(m_Thrown); addPrevious(m_Thrown); } else m_sible = 0; //m_time = Time; } if(m_canLaunch){ //m_time = Time; m_hss[i].m_free.m_done = true; m_variator = 0.001; m_sible = 0; m_devine = true; deactivateMove(); cleanHistory(); m_hss[i].m_free.m_atThrowPos = m_hss[i].m_free.m_position; } m_hss[i].m_free.m_oldPosition = m_hss[i].m_free.m_position ; if(m_hss[i].m_free.m_currentThrown != NULL && m_posSet && m_Feedback ){ hduVector3Dd impact = invertTransform(m_impactPos, &camInv); hduVector3Dd pos(m_hss[i].m_free.m_position); btVector3 balltest = ((*m_thrownRigids)[0])->getWorldTransform().getOrigin(); hduVector3Dd test = invertTransform(&balltest, &camInv); m_velocity = m_hss[i].m_free.m_velocity.magnitude(); if(pos[2]-5 >= test[2]){ if(!m_hss[i].m_free.m_done){ //hduVector3Dd helpForce = ForceToImpact(&pos,&impact); hduVector3Dd helpForce = magneticForce(&pos, &impact, &camInv); //hduVector3Dd helpForce = atomeForce(&pos, &impact, &camInv); if(!helpForce.isZero(EPSILON)){ hduVector3Dd force = 1.0 * helpForce; m_hss[i].m_free.m_force = force; m_Force = force; }else { //m_hss[i].m_free.m_force = m_Force; } } } } hdScheduleSynchronous(sScheduleIn, &m_hss, HD_DEFAULT_SCHEDULER_PRIORITY); m_selectedDistance = selectedDistance; if(m_sible > 0) m_sible--; } }
RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset) : m_ownerWorld (ownerWorld) { // Setup the geometry m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15), btScalar(0.20)); m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15), btScalar(0.28)); m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.40), btScalar(0.20)); m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); // Setup all the rigid bodies btTransform offset; offset.setIdentity(); offset.setOrigin(positionOffset); btTransform transform; transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.), btScalar(1.), btScalar(0.))); m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.), btScalar(1.2), btScalar(0.))); m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.), btScalar(1.6), btScalar(0.))); m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.))); m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.))); m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.))); m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.))); m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,M_PI_2); m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,M_PI_2); m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,-M_PI_2); m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,-M_PI_2); m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]); // Setup some damping on the m_bodies for (int i = 0; i < BODYPART_COUNT; ++i) { m_bodies[i]->setDamping(0.05, 0.85); m_bodies[i]->setDeactivationTime(0.8); m_bodies[i]->setSleepingThresholds(1.6, 2.5); } // Now setup the constraints btHingeConstraint* hingeC; btConeTwistConstraint* coneC; btTransform localA, localB; localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB); hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2)); m_joints[JOINT_PELVIS_SPINE] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); m_joints[JOINT_SPINE_HEAD] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_LEFT_HIP] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_LEFT_KNEE] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_RIGHT_HIP] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_RIGHT_KNEE] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_joints[JOINT_LEFT_SHOULDER] = coneC; m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB); // hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_LEFT_ELBOW] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); m_joints[JOINT_RIGHT_SHOULDER] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB); // hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_RIGHT_ELBOW] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true); }
TestRig (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed) : m_ownerWorld (ownerWorld) { btVector3 vUp(0, 1, 0); // // Setup geometry // float fBodySize = 0.25f; float fLegLength = 0.45f; float fForeLegLength = 0.75f; m_shapes[0] = new btCapsuleShape(btScalar(fBodySize), btScalar(0.10)); int i; for ( i=0; i<NUM_LEGS; i++) { m_shapes[1 + 2*i] = new btCapsuleShape(btScalar(0.10), btScalar(fLegLength)); m_shapes[2 + 2*i] = new btCapsuleShape(btScalar(0.08), btScalar(fForeLegLength)); } // // Setup rigid bodies // float fHeight = 0.5; btTransform offset; offset.setIdentity(); offset.setOrigin(positionOffset); // root btVector3 vRoot = btVector3(btScalar(0.), btScalar(fHeight), btScalar(0.)); btTransform transform; transform.setIdentity(); transform.setOrigin(vRoot); if (bFixed) { m_bodies[0] = localCreateRigidBody(btScalar(0.), offset*transform, m_shapes[0]); } else { m_bodies[0] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[0]); } // legs for ( i=0; i<NUM_LEGS; i++) { float fAngle = 2 * M_PI * i / NUM_LEGS; float fSin = sin(fAngle); float fCos = cos(fAngle); transform.setIdentity(); btVector3 vBoneOrigin = btVector3(btScalar(fCos*(fBodySize+0.5*fLegLength)), btScalar(fHeight), btScalar(fSin*(fBodySize+0.5*fLegLength))); transform.setOrigin(vBoneOrigin); // thigh btVector3 vToBone = (vBoneOrigin - vRoot).normalize(); btVector3 vAxis = vToBone.cross(vUp); transform.setRotation(btQuaternion(vAxis, M_PI_2)); m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[1+2*i]); // shin transform.setIdentity(); transform.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(fHeight-0.5*fForeLegLength), btScalar(fSin*(fBodySize+fLegLength)))); m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[2+2*i]); } // Setup some damping on the m_bodies for (i = 0; i < BODYPART_COUNT; ++i) { m_bodies[i]->setDamping(0.05, 0.85); m_bodies[i]->setDeactivationTime(0.8); //m_bodies[i]->setSleepingThresholds(1.6, 2.5); m_bodies[i]->setSleepingThresholds(0.5f, 0.5f); } // // Setup the constraints // btHingeConstraint* hingeC; //btConeTwistConstraint* coneC; btTransform localA, localB, localC; for ( i=0; i<NUM_LEGS; i++) { float fAngle = 2 * M_PI * i / NUM_LEGS; float fSin = sin(fAngle); float fCos = cos(fAngle); // hip joints localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,-fAngle,0); localA.setOrigin(btVector3(btScalar(fCos*fBodySize), btScalar(0.), btScalar(fSin*fBodySize))); localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB); hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8)); //hingeC->setLimit(btScalar(-0.1), btScalar(0.1)); m_joints[2*i] = hingeC; m_ownerWorld->addConstraint(m_joints[2*i], true); // knee joints localA.setIdentity(); localB.setIdentity(); localC.setIdentity(); localA.getBasis().setEulerZYX(0,-fAngle,0); localA.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(0.), btScalar(fSin*(fBodySize+fLegLength)))); localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; localC = m_bodies[2+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC); //hingeC->setLimit(btScalar(-0.01), btScalar(0.01)); hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2)); m_joints[1+2*i] = hingeC; m_ownerWorld->addConstraint(m_joints[1+2*i], true); } }
RagDoll2 (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,SimpleOpenGL3App* app) : m_ownerWorld (ownerWorld), m_app(app) { // Setup the geometry m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15), btScalar(0.20)); m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15), btScalar(0.28)); m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.10), btScalar(0.05)); m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); // Setup all the rigid bodies btTransform offset; offset.setIdentity(); offset.setOrigin(positionOffset); btTransform transform; transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.), btScalar(1.), btScalar(0.))); //m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS], btVector3(0,1,0)); // btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep); int n_links = 0; float mass = 1.f; btVector3 localInertia; m_shapes[BODYPART_PELVIS]->calculateLocalInertia(mass,localInertia); bool isFixedBase = true; bool canSleep = true; bool isMultiDof = false; btMultiBody * bod = new btMultiBody(n_links, mass, localInertia, isFixedBase, canSleep, isMultiDof); btTransform tr; tr = offset*transform; bod->setBasePos(tr.getOrigin()); bod->setWorldToBaseRot(tr.getRotation()); btVector3 vel(0,0,0); bod->setBaseVel(vel); #if 0 transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.), btScalar(1.2), btScalar(0.))); m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE],btVector3(0,0,1)); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.), btScalar(1.6), btScalar(0.))); m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.))); m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.))); m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.))); m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.))); m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,M_PI_2); m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,M_PI_2); m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,-M_PI_2); m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]); transform.setIdentity(); transform.setOrigin(btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.))); transform.getBasis().setEulerZYX(0,0,-M_PI_2); m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]); // Setup some damping on the m_bodies for (int i = 0; i < BODYPART_COUNT; ++i) { m_bodies[i]->setDamping(0.05, 0.85); m_bodies[i]->setDeactivationTime(0.8); m_bodies[i]->setSleepingThresholds(1.6, 2.5); } #endif #if 0 // Now setup the constraints btHingeConstraint* hingeC; btConeTwistConstraint* coneC; btTransform localA, localB; localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB); hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2)); m_joints[JOINT_PELVIS_SPINE] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); m_joints[JOINT_SPINE_HEAD] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_LEFT_HIP] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_LEFT_KNEE] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_RIGHT_HIP] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_RIGHT_KNEE] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_joints[JOINT_LEFT_SHOULDER] = coneC; m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB); // hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_LEFT_ELBOW] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.))); localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); m_joints[JOINT_RIGHT_SHOULDER] = coneC; coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true); localA.setIdentity(); localB.setIdentity(); localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB); // hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_RIGHT_ELBOW] = hingeC; hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true); #endif }