bool CDynamics3DEntity::MoveTo(const CVector3& c_position, const CQuaternion& c_orientation, bool b_check_only) { /* Move the body to the new position */ dBodySetPosition(m_tBody, c_position.GetX(), c_position.GetY(), c_position.GetZ()); /* Rotate the body to the new orientation */ dQuaternion tQuat = { c_orientation.GetW(), c_orientation.GetX(), c_orientation.GetY(), c_orientation.GetZ() }; dBodySetQuaternion(m_tBody, tQuat); /* Check for collisions */ bool bCollisions = m_cEngine.IsEntityColliding(m_tEntitySpace); if(bCollisions || b_check_only) { /* * Undo the changes if there is a collision or * if the move was just a check */ const CVector3& cPosition = GetEmbodiedEntity().GetPosition(); dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ()); const CQuaternion& cOrientation = GetEmbodiedEntity().GetOrientation(); dQuaternion tQuat2 = { cOrientation.GetW(), cOrientation.GetX(), cOrientation.GetY(), cOrientation.GetZ() }; dBodySetQuaternion(m_tBody, tQuat2); return !bCollisions; } else { /* Set the new position and orientation */ GetEmbodiedEntity().SetPosition(c_position); GetEmbodiedEntity().SetOrientation(c_orientation); return true; } }
CVector3& CVector3::Rotate(const CQuaternion& c_quaternion) { CQuaternion cResult; cResult = c_quaternion; cResult *= CQuaternion(0.0f, m_fX, m_fY, m_fZ); cResult *= c_quaternion.Inverse(); m_fX = cResult.GetX(); m_fY = cResult.GetY(); m_fZ = cResult.GetZ(); return *this; }
SICALLBACK MOM_SetAttributes_Evaluate( ICENodeContext& in_ctxt ) { // The current output port being evaluated... ULONG out_portID = in_ctxt.GetEvaluatedOutputPortID( ); if(gSimulation == NULL) return CStatus::OK; switch( out_portID ) { case ID_OUT_base: { CDataArrayLong baseData( in_ctxt, ID_IN_base ); CDataArrayLong idData( in_ctxt, ID_IN_id ); rbdID rbd_ID; CIndexSet indexSet( in_ctxt ); // Get the output port array ... CDataArrayLong outData( in_ctxt ); // get all of the input SET data! CDataArrayBool setPosData( in_ctxt, ID_IN_set_position); CDataArrayBool setRotData( in_ctxt, ID_IN_set_orientation); CDataArrayBool setLinvelData( in_ctxt, ID_IN_set_linvelocity); CDataArrayBool setAngvelData( in_ctxt, ID_IN_set_angvelocity); CDataArrayBool setStateData( in_ctxt, ID_IN_set_state); CDataArrayBool setMassData( in_ctxt, ID_IN_set_mass); CDataArrayBool setBounceData( in_ctxt, ID_IN_set_bounce); CDataArrayBool setFrictionData( in_ctxt, ID_IN_set_friction); CDataArrayBool setLindampData( in_ctxt, ID_IN_set_lindamping); CDataArrayBool setAngdampData( in_ctxt, ID_IN_set_angdamping); CDataArrayBool setLintreshData( in_ctxt, ID_IN_set_lintreshold); CDataArrayBool setAngtreshData( in_ctxt, ID_IN_set_angtreshold); // get all of the input data! CDataArrayVector3f posData( in_ctxt, ID_IN_position); CDataArrayVector3f rotData( in_ctxt, ID_IN_orientation); CDataArrayVector3f linvelData( in_ctxt, ID_IN_linvelocity); CDataArrayVector3f angvelData( in_ctxt, ID_IN_angvelocity); CDataArrayLong stateData( in_ctxt, ID_IN_state); CDataArrayFloat massData( in_ctxt, ID_IN_mass); CDataArrayFloat bounceData( in_ctxt, ID_IN_bounce); CDataArrayFloat frictionData( in_ctxt, ID_IN_friction); CDataArrayFloat lindampData( in_ctxt, ID_IN_lindamping); CDataArrayFloat angdampData( in_ctxt, ID_IN_angdamping); CDataArrayFloat lintreshData( in_ctxt, ID_IN_lintreshold); CDataArrayFloat angtreshData( in_ctxt, ID_IN_angtreshold); // get the index set iterator btTransform bodyTransform; CVector3f bodyPos,linvel,angvel; btQuaternion bodyRot; CRotation rot; CQuaternion quat; CVector3f anglesf; CVector3 angles; for(CIndexSet::Iterator it = indexSet.Begin(); it.HasNext(); it.Next()) { rbd_ID.primary = (int)(baseData.IsConstant() ? baseData[0] : baseData[it]); rbd_ID.secondary = (int)(idData.IsConstant() ? idData[0] : idData[it]); btRigidBodyReference * bodyRef = gSimulation->GetRigidBody(rbd_ID); if(bodyRef != NULL) { // take care of the positions if((setPosData.IsConstant() ? setPosData[0] : setPosData[it]) == true) { bodyPos = posData.IsConstant() ? posData[0] : posData[it]; bodyTransform = bodyRef->GetWorldTransform(); bodyTransform.setOrigin(btVector3(bodyPos.GetX(),bodyPos.GetY(),bodyPos.GetZ())); bodyRef->SetWorldTransform(bodyTransform); } // take care of the orientations if((setRotData.IsConstant() ? setRotData[0] : setRotData[it]) == true) { anglesf = rotData.IsConstant() ? rotData[0] : rotData[it]; rot.SetFromXYZAngles(DegreesToRadians(anglesf.GetX()),DegreesToRadians(anglesf.GetY()),DegreesToRadians(anglesf.GetZ())); quat = rot.GetQuaternion(); bodyTransform = bodyRef->GetWorldTransform(); bodyTransform.setRotation(btQuaternion(quat.GetX(),quat.GetY(),quat.GetZ(),quat.GetW())); bodyRef->SetWorldTransform(bodyTransform); } // take care of the linear velocity if((setLinvelData.IsConstant() ? setLinvelData[0] : setLinvelData[it]) == true) { linvel = linvelData.IsConstant() ? linvelData[0] : linvelData[it]; bodyRef->body->setLinearVelocity(btVector3(linvel.GetX(),linvel.GetY(),linvel.GetZ())); } // take care of the angular velocity if((setAngvelData.IsConstant() ? setAngvelData[0] : setAngvelData[it]) == true) { angvel = angvelData.IsConstant() ? angvelData[0] : angvelData[it]; bodyRef->body->setAngularVelocity(btVector3(angvel.GetX(),angvel.GetY(),angvel.GetZ())); } // take care of the state if((setStateData.IsConstant() ? setStateData[0] : setStateData[it]) == true) { int state = stateData.IsConstant() ? stateData[0] : stateData[it]; if(state == 0) bodyRef->body->forceActivationState(ACTIVE_TAG); else if(state == 1) bodyRef->body->forceActivationState(ISLAND_SLEEPING); else if(state == 2) bodyRef->body->forceActivationState(DISABLE_SIMULATION); } // take care of the mass if((setMassData.IsConstant() ? setMassData[0] : setMassData[it]) == true) { // compute the inertia bodyRef->mass = massData.IsConstant() ? massData[0] : massData[it]; btVector3 inertia(0,0,0); if(bodyRef->mass > 0.0f) bodyRef->body->getCollisionShape()->calculateLocalInertia(bodyRef->mass,inertia); bodyRef->body->setMassProps(bodyRef->mass,inertia); } // take care of the bounce if((setBounceData.IsConstant() ? setBounceData[0] : setBounceData[it]) == true) { bodyRef->body->setRestitution(bounceData.IsConstant() ? bounceData[0] : bounceData[it]); } // take care of the friction if((setFrictionData.IsConstant() ? setFrictionData[0] : setFrictionData[it]) == true) { bodyRef->body->setFriction(frictionData.IsConstant() ? frictionData[0] : frictionData[it]); } // take care of the linear damping if((setLindampData.IsConstant() ? setLindampData[0] : setLindampData[it]) == true) { float angdamp = bodyRef->body->getAngularDamping(); bodyRef->body->setDamping(lindampData.IsConstant() ? lindampData[0] : lindampData[it],angdamp); } // take care of the angular damping if((setAngdampData.IsConstant() ? setAngdampData[0] : setAngdampData[it]) == true) { float lindamp = bodyRef->body->getLinearDamping(); bodyRef->body->setDamping(lindamp,angdampData.IsConstant() ? angdampData[0] : angdampData[it]); } // take care of the linear treshold if((setLintreshData.IsConstant() ? setLintreshData[0] : setLintreshData[it]) == true) { float angtresh = bodyRef->body->getAngularSleepingThreshold(); bodyRef->body->setSleepingThresholds(lintreshData.IsConstant() ? lintreshData[0] : lintreshData[it],angtresh); } // take care of the angular treshold if((setAngtreshData.IsConstant() ? setAngtreshData[0] : setAngtreshData[it]) == true) { float lintresh = bodyRef->body->getLinearSleepingThreshold(); bodyRef->body->setSleepingThresholds(lintresh,angtreshData.IsConstant() ? angtreshData[0] : angtreshData[it]); } } outData[it] = rbd_ID.primary; } break; } }; return CStatus::OK; }