void SimObject::setRotation(const Matrix3d& rot) { Matrix3d childRotation(this->rotation); childRotation.transpose(); childRotation = rot * childRotation; rotation = rot; ObjectList::const_iterator listPos; for(listPos = childNodes.begin(); listPos != childNodes.end(); ++listPos) { (*listPos)->rotate(childRotation, position); } }
void HavokImport::createRagdollRigidBody(INode* n, INode* parent, INode* ragdollParent, bhkRigidBodyRef rbody) { const int MaxChar = 512; char buffer[MaxChar]; //TSTR name(A2THelper(buffer, parent->GetName().c_str(), _countof(buffer))); n->SetName(FormatText(TEXT("Ragdoll_%s"), parent->GetName())); Object *pObj = n->GetObjectRef(); IDerivedObject *dobj = nullptr; if (n->SuperClassID() == GEN_DERIVOB_CLASS_ID) dobj = static_cast<IDerivedObject*>(pObj); else { dobj = CreateDerivedObject(pObj); } MotionSystem msys = rbody->GetMotionSystem(); //? MotionQuality qtype = rbody->GetQualityType(); float mass = rbody->GetMass(); float lindamp = rbody->GetLinearDamping(); float angdamp = rbody->GetAngularDamping(); float frict = rbody->GetFriction(); float resti = rbody->GetRestitution(); float maxlinvel = rbody->GetMaxLinearVelocity(); float maxangvel = rbody->GetMaxAngularVelocity(); float pendepth = rbody->GetPenetrationDepth(); InertiaMatrix im = rbody->GetInertia(); Modifier* rbMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_RIGIDBODY_MODIFIER_CLASS_ID); if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_RB_MOD_PBLOCK)) { //These are fundamental parameters rbParameters->SetValue(PA_RB_MOD_MASS, 0, mass, 0); rbParameters->SetValue(PA_RB_MOD_RESTITUTION, 0, resti, 0); rbParameters->SetValue(PA_RB_MOD_FRICTION, 0, frict, 0); rbParameters->SetValue(PA_RB_MOD_INERTIA_TENSOR, 0, Point3(im[0][0],im[1][1],im[2][2]), 0); rbParameters->SetValue(PA_RB_MOD_LINEAR_DAMPING, 0, lindamp, 0); rbParameters->SetValue(PA_RB_MOD_CHANGE_ANGULAR_DAMPING, 0, angdamp, 0); rbParameters->SetValue(PA_RB_MOD_MAX_LINEAR_VELOCITY, 0, maxlinvel, 0); rbParameters->SetValue(PA_RB_MOD_MAX_ANGULAR_VELOCITY, 0, maxangvel, 0); rbParameters->SetValue(PA_RB_MOD_ALLOWED_PENETRATION_DEPTH, 0, pendepth, 0); rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_FIXED, 0); rbParameters->SetValue(PA_RB_MOD_SOLVER_DEACTIVATION, 0, SD_LOW, 0); rbParameters->SetValue(PA_RB_MOD_DEACTIVATOR_TYPE, 0, DT_LOW, 0); /*body->SetMotionSystem(MotionSystem::MO_SYS_BOX); body->SetDeactivatorType(DeactivatorType::DEACTIVATOR_NEVER); body->SetSolverDeactivation(SolverDeactivation::SOLVER_DEACTIVATION_LOW); body->SetQualityType(MO_QUAL_FIXED);*/ /*switch (qtype) { case MO_QUAL_INVALID: break; case MO_QUAL_FIXED: rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_FIXED, 0); break; case MO_QUAL_KEYFRAMED: rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED, 0); break; case MO_QUAL_DEBRIS: rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_DEBRIS, 0); break; case MO_QUAL_MOVING: rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_MOVING, 0); break; case MO_QUAL_CRITICAL: rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_CRITICAL, 0); break; case MO_QUAL_BULLET: rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_BULLET, 0); break; case MO_QUAL_USER: break; case MO_QUAL_CHARACTER: break; case MO_QUAL_KEYFRAMED_REPORT: rbParameters->SetValue(PA_RB_MOD_QUALITY_TYPE, 0, QT_KEYFRAMED_REPORTING, 0); break; }*/ } //Link Rigid Body to parent Rigid Body ICustAttribContainer* cc = rbMod->GetCustAttribContainer(); if (!cc) { rbMod->AllocCustAttribContainer(); cc = rbMod->GetCustAttribContainer(); } CustAttrib* c = (CustAttrib*)CreateInstance(CUST_ATTRIB_CLASS_ID, Class_ID(0x6e663460, 0x32682c72)); IParamBlock2* custModParameters = c->GetParamBlock(0); custModParameters->SetValue(0, 0, parent, 0); cc->InsertCustAttrib(0, c); Modifier* constraintMod = nullptr; vector< bhkSerializableRef > constraints = rbody->GetConstraints(); //Rigid Body constraints if (ragdollParent) { for (vector< bhkSerializableRef >::iterator it = constraints.begin(); it != constraints.end(); ) { bhkConstraintRef constraint = bhkConstraintRef(*it); if (constraint->IsDerivedType(bhkLimitedHingeConstraint::TYPE)) { bhkLimitedHingeConstraintRef limitedHingeConstraint = bhkLimitedHingeConstraintRef(*it); LimitedHingeDescriptor lh = limitedHingeConstraint->GetLimitedHinge(); constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_HINGE_CLASS_ID); if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) { constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0); Point3 origin(0, 0, 0); Matrix3 parentRotation(TOPOINT3(lh.perp2AxleInA1), TOPOINT3(lh.perp2AxleInA2), TOPOINT3(lh.axleA), origin); Matrix3 childRotation(TOPOINT3(lh.perp2AxleInB1), TOPOINT3(lh.perp2AxleInB2), TOPOINT3(lh.axleB), origin); //Matrix3 parentRotation(true); //MatrixFromNormal(TOPOINT3(lh.axleA), parentRotation); //Matrix3 childRotation(true); //MatrixFromNormal(TOPOINT3(lh.axleB), childRotation); constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0); } if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) { constraintParameters->SetValue(PA_HINGE_MOD_IS_LIMITED, 0, 1, 0); constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MIN, 0, TODEG(lh.minAngle), 0); constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MAX, 0, TODEG(lh.maxAngle), 0); constraintParameters->SetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, lh.maxFriction, 0); // constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0); } } else if (constraint->IsDerivedType(bhkRagdollConstraint::TYPE)) { bhkRagdollConstraintRef ragdollConstraint = bhkRagdollConstraintRef(*it); RagdollDescriptor rag = ragdollConstraint->GetRagdoll(); constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_RAGDOLL_CLASS_ID); if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) { constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0); //TOVECTOR3(rag.twistA); //MatrixFromNormal(TOPOINT3(rag.twistA), parentRotation); Point3 origin(0,0,0); Matrix3 parentRotation(TOPOINT3(rag.planeA),TOPOINT3(rag.motorA),TOPOINT3(rag.twistA),origin); //TOVECTOR3(rag.twistB); //MatrixFromNormal(TOPOINT3(rag.twistB), childRotation); Matrix3 childRotation(TOPOINT3(rag.planeB), TOPOINT3(rag.motorB), TOPOINT3(rag.twistB), origin); constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotB), 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotA), 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0); } if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) { constraintParameters->SetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, TODEG(rag.coneMaxAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, TODEG(rag.planeMinAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, TODEG(rag.planeMaxAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, TODEG(rag.twistMinAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, TODEG(rag.twistMaxAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, rag.maxFriction, 0); } } else if (constraint->IsDerivedType(bhkMalleableConstraint::TYPE)) { bhkMalleableConstraintRef malleableConstraint = bhkMalleableConstraintRef(*it); if (malleableConstraint->GetConstraintType() == (unsigned int)2) { LimitedHingeDescriptor lh = malleableConstraint->GetLimitedHinge(); constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_HINGE_CLASS_ID); if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) { constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0); Point3 origin(0, 0, 0); Matrix3 parentRotation(TOPOINT3(lh.perp2AxleInA1), TOPOINT3(lh.perp2AxleInA2), TOPOINT3(lh.axleA), origin); Matrix3 childRotation(TOPOINT3(lh.perp2AxleInB1), TOPOINT3(lh.perp2AxleInB2), TOPOINT3(lh.axleB), origin); //Matrix3 parentRotation(true); //MatrixFromNormal(TOPOINT3(lh.axleA), parentRotation); //Matrix3 childRotation(true); //MatrixFromNormal(TOPOINT3(lh.axleB), childRotation); constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0); } if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_HINGE_MOD_PBLOCK)) { constraintParameters->SetValue(PA_HINGE_MOD_IS_LIMITED, 0, 1, 0); constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MIN, 0, TODEG(lh.minAngle), 0); constraintParameters->SetValue(PA_HINGE_MOD_LIMIT_MAX, 0, TODEG(lh.maxAngle), 0); constraintParameters->SetValue(PA_HINGE_MOD_MAX_FRICTION_TORQUE, 0, lh.maxFriction, 0); // constraintParameters->SetValue(PA_HINGE_MOD_MOTOR_TYPE, 0, lh.motor., 0); } } else if (malleableConstraint->GetConstraintType() == (unsigned int)7) { RagdollDescriptor rag = malleableConstraint->GetRagdoll(); constraintMod = (Modifier*)CreateInstance(OSM_CLASS_ID, HK_CONSTRAINT_RAGDOLL_CLASS_ID); if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_CONSTRAINT_MOD_COMMON_SPACES_PARAMS)) { constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_NODE, 0, ragdollParent, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_ROTATION_LOCK, 0, 0, 0); //TOVECTOR3(rag.twistA); //MatrixFromNormal(TOPOINT3(rag.twistA), parentRotation); Point3 origin(0, 0, 0); Matrix3 parentRotation(TOPOINT3(rag.planeA), TOPOINT3(rag.motorA), TOPOINT3(rag.twistA), origin); //TOVECTOR3(rag.twistB); //MatrixFromNormal(TOPOINT3(rag.twistB), childRotation); Matrix3 childRotation(TOPOINT3(rag.planeB), TOPOINT3(rag.motorB), TOPOINT3(rag.twistB), origin); constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotB), 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_CHILD_SPACE_ROTATION, 0, childRotation, 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_TRANSLATION, 0, TOPOINT3(rag.pivotA), 0); constraintParameters->SetValue(PA_CONSTRAINT_MOD_PARENT_SPACE_ROTATION, 0, parentRotation, 0); } if (IParamBlock2* constraintParameters = constraintMod->GetParamBlockByID(PB_RAGDOLL_MOD_PBLOCK)) { constraintParameters->SetValue(PA_RAGDOLL_MOD_CONE_ANGLE, 0, TODEG(rag.coneMaxAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MIN, 0, TODEG(rag.planeMinAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_PLANE_MAX, 0, TODEG(rag.planeMaxAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MIN, 0, TODEG(rag.twistMinAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_TWIST_MAX, 0, TODEG(rag.twistMaxAngle), 0); constraintParameters->SetValue(PA_RAGDOLL_MOD_MAX_FRICTION_TORQUE, 0, rag.maxFriction, 0); } } } ++it; } } dobj->SetAFlag(A_LOCK_TARGET); dobj->AddModifier(rbMod); if (constraintMod) dobj->AddModifier(constraintMod); dobj->ClearAFlag(A_LOCK_TARGET); n->SetObjectRef(dobj); }