protected override void RebuildConstraint() { DestroyConstraint(); if (Validate()) { _constraint = _spring = new Generic6DofSpring2Constraint(_attachment0, _attachment1, (Matrix)_attachment0.CFrame, (Matrix)_attachment1.CFrame) { Userobject = this }; _spring.SetStiffness(0, _restitution, true); _spring.SetStiffness(1, _restitution, true); World.Physics.AddConstraint(this); } }
public void ConvertURDF2BulletInternal( URDFImporterInterface u2b, MultiBodyCreationInterface creation, URDF2BulletCachedData cache, int urdfLinkIndex, Matrix parentTransformInWorldSpace, GameObject parentGameObject, MultiBodyDynamicsWorld world1, bool createMultiBody, string pathPrefix, bool enableConstraints, ConvertURDFFlags flags = 0) { Matrix linkTransformInWorldSpace = Matrix.Identity; int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex); int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex); int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); RigidBody parentRigidBody = null; //b3Printf(); if (debugLevel >= BDebug.DebugType.Debug) { Debug.LogFormat("mb link index = {0}\n", mbLinkIndex); } Matrix parentLocalInertialFrame = Matrix.Identity; float parentMass = (1); BulletSharp.Math.Vector3 parentLocalInertiaDiagonal = new BulletSharp.Math.Vector3(1, 1, 1); if (urdfParentIndex == -2) { if (debugLevel >= BDebug.DebugType.Debug) { Debug.LogFormat("root link has no parent\n"); } } else { if (debugLevel >= BDebug.DebugType.Debug) { Debug.LogFormat("urdf parent index = {0}", urdfParentIndex); Debug.LogFormat("mb parent index = {0}", mbParentIndex); } parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex); u2b.getMassAndInertia(urdfParentIndex, out parentMass, out parentLocalInertiaDiagonal, out parentLocalInertialFrame); } float mass = 0; Matrix localInertialFrame = Matrix.Identity; BulletSharp.Math.Vector3 localInertiaDiagonal = new BulletSharp.Math.Vector3(0, 0, 0); u2b.getMassAndInertia(urdfLinkIndex, out mass, out localInertiaDiagonal, out localInertialFrame); Matrix parent2joint = Matrix.Identity; UrdfJointTypes jointType; BulletSharp.Math.Vector3 jointAxisInJointSpace; float jointLowerLimit; float jointUpperLimit; float jointDamping; float jointFriction; float jointMaxForce; float jointMaxVelocity; bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity); string linkName = u2b.getLinkName(urdfLinkIndex); if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0) { Matrix tmp = new Matrix(); Matrix.Invert(ref parentTransformInWorldSpace, out tmp); Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint); } else { if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0) { linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace; } else { linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint; } } GameObject gameObject = new GameObject(linkName); if (parentGameObject != null) { gameObject.transform.parent = parentGameObject.transform; } //-------------------- /* * bool hasParentJoint = loader.getJointInfo2(linkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity); * string linkName = loader.getLinkName(linkIndex); * * if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0) * { * Matrix tmp = parentTransformInWorldSpace.Inverse(); * Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint); * } * else * { * if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0) * { * linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace; * } * else * { * linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint; * } * } * * lgo.transform.position = linkTransformInWorldSpace.Origin.ToUnity(); */ //-------------------- gameObject.transform.position = linkTransformInWorldSpace.Origin.ToUnity(); gameObject.transform.rotation = linkTransformInWorldSpace.Rotation.ToUnity(); BCollisionShape compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame, gameObject); int graphicsIndex; { graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame); } if (compoundShape != null) { UrdfMaterialColor matColor; Color color2 = Color.red; Color specular = new Color(0.5f, 0.5f, 0.5f); if (u2b.getLinkColor2(urdfLinkIndex, out matColor)) { color2 = matColor.m_rgbaColor; specular = matColor.m_specularColor; } if (mass != 0) { if ((flags & ConvertURDFFlags.CUF_USE_URDF_INERTIA) == 0) { compoundShape.GetCollisionShape().CalculateLocalInertia(mass, out localInertiaDiagonal); Debug.Assert(localInertiaDiagonal[0] < 1e10); Debug.Assert(localInertiaDiagonal[1] < 1e10); Debug.Assert(localInertiaDiagonal[2] < 1e10); } URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo); //temporary inertia scaling until we load inertia from URDF if ((contactInfo.m_flags & URDF_LinkContactFlags.URDF_CONTACT_HAS_INERTIA_SCALING) != 0) { localInertiaDiagonal *= contactInfo.m_inertiaScaling; } } RigidBody linkRigidBody = null; Matrix inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame; if (!createMultiBody) { RigidBody body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape.GetCollisionShape()); linkRigidBody = body; world1.AddRigidBody(body); compoundShape.GetCollisionShape().UserIndex = (graphicsIndex); URDFLinkContactInfo contactInfo; u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo); ProcessContactParameters(contactInfo, body); creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex); cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame); //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body); } else { if (cache.m_bulletMultiBody == null) { // creating base bool canSleep = false; bool isFixedBase = (mass == 0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; //cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep); BMultiBody bmm = cache.m_bulletMultiBody = gameObject.AddComponent <BMultiBody>(); bmm.fixedBase = isFixedBase; bmm.canSleep = canSleep; bmm.baseMass = mass; //new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep); //if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0) //{ // cache.m_bulletMultiBody.BaseWorldTransform = (linkTransformInWorldSpace); //} //cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame); } } //create a joint if necessary BMultiBodyLink bmbl = null; if (hasParentJoint) { //==================== //btTransform offsetInA, offsetInB; //offsetInA = parentLocalInertialFrame.inverse() * parent2joint; //offsetInB = localInertialFrame.inverse(); //btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation(); //===================== Matrix offsetInA = new Matrix(), offsetInB = new Matrix(); Matrix.Invert(ref parentLocalInertialFrame, out offsetInA); offsetInA = offsetInA * parent2joint; Matrix.Invert(ref localInertialFrame, out offsetInB); Matrix offsetInAInv = new Matrix(); Matrix.Invert(ref offsetInA, out offsetInAInv); BulletSharp.Math.Quaternion parentRotToThis = offsetInB.GetRotation() * offsetInAInv.GetRotation(); Matrix tmp = new Matrix(); Matrix.Invert(ref parentTransformInWorldSpace, out tmp); Matrix tmp2 = new Matrix(); Matrix.Invert(ref parent2joint, out tmp2); Matrix tmp3 = new Matrix(); Matrix tmp4 = linkTransformInWorldSpace; Matrix link2joint = new Matrix(); Matrix.Multiply(ref tmp, ref tmp2, out tmp3); Matrix.Multiply(ref tmp3, ref tmp4, out link2joint); if (debugLevel >= BDebug.DebugType.Debug) { Matrix linkTransformInWorldSpaceInv = new Matrix(), parentTransformInWorldSpaceInv = new Matrix(); Matrix.Invert(ref linkTransformInWorldSpace, out linkTransformInWorldSpaceInv); Matrix.Invert(ref parentTransformInWorldSpace, out parentTransformInWorldSpaceInv); Debug.Log("Creating link " + linkName + " offsetInA=" + offsetInA.Origin + " offsetInB=" + offsetInB.Origin + " parentLocalInertialFrame=" + parentLocalInertialFrame.Origin + " localInertialFrame=" + localInertialFrame.Origin + " localTrnaform=" + " linkTransInWorldSpace=" + linkTransformInWorldSpace.Origin + " linkTransInWorldSpaceInv=" + linkTransformInWorldSpaceInv.Origin + " parentTransInWorldSpace=" + parentTransformInWorldSpace.Origin + " parentTransInWorldSpaceInv=" + parentTransformInWorldSpaceInv.Origin + " link2joint=" + link2joint.Origin + " jointType=" + jointType); } bool disableParentCollision = true; switch (jointType) { case UrdfJointTypes.URDFFloatingJoint: case UrdfJointTypes.URDFPlanarJoint: case UrdfJointTypes.URDFFixedJoint: { if ((jointType == UrdfJointTypes.URDFFloatingJoint) || (jointType == UrdfJointTypes.URDFPlanarJoint)) { Debug.Log("Warning: joint unsupported, creating a fixed joint instead."); } //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { //todo: adjust the center of mass transform and pivot axis properly /* * cache.m_bulletMultiBody.SetupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, * parentRotToThis, offsetInA.Origin, -offsetInB.Origin); */ bmbl = gameObject.AddComponent <BMultiBodyLink>(); bmbl.jointType = FeatherstoneJointType.Fixed; bmbl.mass = mass; bmbl.localPivotPosition = offsetInB.Origin.ToUnity(); } else { //b3Printf("Fixed joint\n"); Debug.LogError("TODO Setup 6dof "); /* * Generic6DofSpring2Constraint dof6 = null; * * //backward compatibility * if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0) * { * dof6 = creation.createFixedJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB); * } * else * { * dof6 = creation.createFixedJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA); * } * if (enableConstraints) * world1.AddConstraint(dof6, true); */ } break; } case UrdfJointTypes.URDFContinuousJoint: case UrdfJointTypes.URDFRevoluteJoint: { //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { //cache.m_bulletMultiBody.SetupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, // parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(), // -offsetInB.Origin, // disableParentCollision); bmbl = gameObject.AddComponent <BMultiBodyLink>(); bmbl.jointType = FeatherstoneJointType.Revolute; bmbl.mass = mass; bmbl.localPivotPosition = offsetInB.Origin.ToUnity(); bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity(); if (jointType == UrdfJointTypes.URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) { //string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit); BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>(); mbc.m_jointLowerLimit = jointLowerLimit; mbc.m_jointUpperLimit = jointUpperLimit; } /* * Debug.Log("=========== Creating joint for: " + gameObject.name); * Debug.Log("parentRotateToThis: " + parentRotToThis.ToUnity().eulerAngles); * Debug.Log("rotationAxis: " + bmbl.rotationAxis); * Debug.Log("offsetInA: " + offsetInA.Origin); * Debug.Log("negOffsetInB: " + -offsetInB.Origin); */ } else { Generic6DofSpring2Constraint dof6 = null; //backwards compatibility if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0) { dof6 = creation.createRevoluteJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); } else { dof6 = creation.createRevoluteJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); } if (enableConstraints) { world1.AddConstraint(dof6, true); } //b3Printf("Revolute/Continuous joint\n"); } break; } case UrdfJointTypes.URDFPrismaticJoint: { //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); if (createMultiBody) { //cache.m_bulletMultiBody.SetupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, // parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(), // -offsetInB.Origin, // disableParentCollision); bmbl = gameObject.AddComponent <BMultiBodyLink>(); bmbl.jointType = FeatherstoneJointType.Prismatic; bmbl.mass = mass; bmbl.localPivotPosition = offsetInB.Origin.ToUnity(); bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity(); if (jointLowerLimit <= jointUpperLimit) { //string name = u2b.getLinkName(urdfLinkIndex); //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit); BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>(); mbc.m_jointLowerLimit = jointLowerLimit; mbc.m_jointUpperLimit = jointUpperLimit; } //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); } else { Generic6DofSpring2Constraint dof6 = creation.createPrismaticJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit); if (enableConstraints) { world1.AddConstraint(dof6, true); } //b3Printf("Prismatic\n"); } break; } default: { //b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType); Debug.Assert(false); break; } } } if (createMultiBody) { if (bmbl != null) { bmbl.jointDamping = jointDamping; bmbl.jointFriction = jointFriction; //bmbl.jointLowerLimit = jointLowerLimit; //bmbl.jointUpperLimit = jointUpperLimit; //bmbl.jointMaxForce = jointMaxForce; //bmbl.jointMaxVelocity = jointMaxVelocity; } { /* * MultiBodyLinkCollider col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody); * * compoundShape.GetCollisionShape().UserIndex = (graphicsIndex); * * col.CollisionShape = (compoundShape.GetCollisionShape()); * * Matrix tr = Matrix.Identity; * * tr = linkTransformInWorldSpace; * //if we don't set the initial pose of the btCollisionObject, the simulator will do this * //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider * * col.WorldTransform = (tr); * * //base and fixed? . static, otherwise flag as dynamic * bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody.HasFixedBase) ? false : true; * CollisionFilterGroups collisionFilterGroup = isDynamic ? (CollisionFilterGroups.DefaultFilter) : (CollisionFilterGroups.StaticFilter); * CollisionFilterGroups collisionFilterMask = isDynamic ? (CollisionFilterGroups.AllFilter) : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter); * * CollisionFilterGroups colGroup = 0, colMask = 0; * UrdfCollisionFlags collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, out colGroup, out colMask); * if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_GROUP) != 0) * { * collisionFilterGroup = colGroup; * } * if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_MASK) != 0) * { * collisionFilterMask = colMask; * } * world1.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); * * color2 = Color.red;//(0.0,0.0,0.5); * Color specularColor = new Color(1, 1, 1); * UrdfMaterialColor matCol; * if (u2b.getLinkColor2(urdfLinkIndex, out matCol)) * { * color2 = matCol.m_rgbaColor; * specularColor = matCol.m_specularColor; * } * { * * * creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex, col, color2, specularColor); * } * { * * * u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, ref localInertialFrame, col, u2b.getBodyUniqueId()); * } * URDFLinkContactInfo contactInfo; * u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo); * * * ProcessContactParameters(contactInfo, col); * * if (mbLinkIndex >= 0) //???? double-check +/- 1 * { * cache.m_bulletMultiBody.GetLink(mbLinkIndex).Collider = col; * if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_PARENT) != 0) * { * cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; * } * if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) != 0) * { * cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION; * } * } * else * { * cache.m_bulletMultiBody.BaseCollider = (col); * } */ } } else { //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape); } } List <int> urdfChildIndices = new List <int>(); u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices); int numChildren = urdfChildIndices.Count; for (int i = 0; i < numChildren; i++) { int urdfChildLinkIndex = urdfChildIndices[i]; ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, gameObject, world1, createMultiBody, pathPrefix, enableConstraints, flags); } }
internal override bool _BuildConstraint() { BPhysicsWorld world = BPhysicsWorld.Get(); if (m_constraintPtr != null) { if (m_isInWorld && world != null) { m_isInWorld = false; world.RemoveConstraint(m_constraintPtr); } } BRigidBody targetRigidBodyA = GetComponent <BRigidBody>(); if (targetRigidBodyA == null) { Debug.LogError("BGeneric6DofSpringConstraint needs to be added to a component with a BRigidBody."); return(false); } if (!targetRigidBodyA.isInWorld) { world.AddRigidBody(targetRigidBodyA); } if (m_localConstraintAxisX == Vector3.zero) { Debug.LogError("Constaint axis cannot be zero vector"); return(false); } RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject(); if (rba == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body"); return(false); } if (m_constraintType == ConstraintType.constrainToAnotherBody) { if (m_otherRigidBody == null) { Debug.LogError("Other rigid body must not be null"); return(false); } if (!m_otherRigidBody.isInWorld) { world.AddRigidBody(m_otherRigidBody); } RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject(); if (rbb == null) { Debug.LogError("Constraint could not get bullet RigidBody from target rigid body"); return(false); } BM.Matrix frameInA, frameInOther; string errormsg = ""; if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg)) { m_constraintPtr = new Generic6DofSpring2Constraint(rba, rbb, frameInA, frameInOther); } else { Debug.LogError(errormsg); return(false); } } else { // TODO // m_constraintPtr = new Generic6DofSpringConstraint(rba, m_localConstraintPoint.ToBullet(), m_localConstraintAxisX.ToBullet(), false); } if (m_setLimit) { ((Generic6DofSpring2Constraint)m_constraintPtr).SetLimit((int)m_springAxis, m_lowLimit, m_highLimit); } ((Generic6DofSpring2Constraint)m_constraintPtr).EnableSpring((int)m_springAxis, true); ((Generic6DofSpring2Constraint)m_constraintPtr).SetStiffness((int)m_springAxis, m_stiffness); ((Generic6DofSpring2Constraint)m_constraintPtr).SetDamping((int)m_springAxis, m_damping); ((Generic6DofSpring2Constraint)m_constraintPtr).SetBounce((int)m_springAxis, m_bounce); ((Generic6DofSpring2Constraint)m_constraintPtr).SetEquilibriumPoint(); ((Generic6DofSpring2Constraint)m_constraintPtr).LinearLowerLimit = m_linearLimitLower.ToBullet(); ((Generic6DofSpring2Constraint)m_constraintPtr).LinearUpperLimit = m_linearLimitUpper.ToBullet(); ((Generic6DofSpring2Constraint)m_constraintPtr).AngularLowerLimit = m_angularLimitLowerRadians.ToBullet(); ((Generic6DofSpring2Constraint)m_constraintPtr).AngularUpperLimit = m_angularLimitUpperRadians.ToBullet(); m_constraintPtr.Userobject = this; m_constraintPtr.DebugDrawSize = m_debugDrawSize; m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold; m_constraintPtr.OverrideNumSolverIterations = m_overrideNumSolverIterations; return(true); }
void Instalize(RigidBodyBone[] rbodies) { //Matrix JointPos = Matrix.RotationYawPitchRoll(jcon.Rotation.Y, jcon.Rotation.X, jcon.Rotation.Z); //JointPos *= Matrix.Translation(GetVec3(jcon.Position)); //look for no second body RigidBody body1 = null; RigidBody body2 = null; try { body1 = rbodies[JointParameters.RigitBody1].Body; try { body2 = rbodies[JointParameters.RigitBody2].Body; } catch (IndexOutOfRangeException) { } } catch (IndexOutOfRangeException) { try { body1 = rbodies[JointParameters.RigitBody2].Body; } catch (IndexOutOfRangeException) { return; } } var jointSpace = Matrix.RotationYawPitchRoll(JointParameters.Rotation.Y, JointParameters.Rotation.X, JointParameters.Rotation.Z) * Matrix.Translation(GetVec3(JointParameters.Position)); //Convert left to right coordinates var reverce = Matrix.Scaling(new Vector3(1, 1, -1)); jointSpace = reverce * jointSpace * reverce; var temp1 = body1.WorldTransform; temp1.Invert(); var conn1 = jointSpace * temp1; Matrix conn2 = Matrix.Identity; if (body2 != null) { //calculating joint arms space var temp2 = body2.WorldTransform; //temp2.M43 += 0.1f; //body2.WorldTransform = temp2; temp2.Invert(); conn2 = jointSpace * temp2; } //conn2 = CleanLowValues(conn2); //conn1 = CleanLowValues(conn1); switch (JointParameters.Type) { case JointType.ConeTwist: ConeTwistConstraint jointCone = null; if (body2 != null) { jointCone = new ConeTwistConstraint(body1, body2, conn1, conn2); } else { jointCone = new ConeTwistConstraint(body1, conn1); } break; case JointType.SpringSixDOF: //the only one used //Jitter to hight /* * Generic6DofSpringConstraint jointSpring6 = null; * if (body2 != null) * jointSpring6 = new Generic6DofSpringConstraint(body1, body2, conn1, conn2,true); * else * jointSpring6 = new Generic6DofSpringConstraint(body1, conn1,true); */ //Has low stiffness limit, disabling explode bodys TODO: increse limit Generic6DofSpring2Constraint jointSpring6 = null; if (body2 != null) { jointSpring6 = new Generic6DofSpring2Constraint(body1, body2, conn1, conn2); } else { jointSpring6 = new Generic6DofSpring2Constraint(body1, conn1); } jointSpring6.AngularLowerLimit = GetVec3(JointParameters.RotMin); jointSpring6.AngularUpperLimit = GetVec3(JointParameters.RotMax); jointSpring6.LinearLowerLimit = GetVec3(JointParameters.PosMin); jointSpring6.LinearUpperLimit = GetVec3(JointParameters.PosMax); for (int i = 0; i < 3; i++) { if (JointParameters.PosSpring[i] != 0) { jointSpring6.EnableSpring(i, true); jointSpring6.SetStiffness(i, JointParameters.PosSpring[i]); } } for (int i = 0; i < 3; i++) { if (JointParameters.RotSpring[i] != 0) { jointSpring6.EnableSpring(i + 3, true); jointSpring6.SetStiffness(i + 3, JointParameters.RotSpring[i]); } } /* * jointSpring6.SetDamping(0, 0.1f); * jointSpring6.SetDamping(1, 0.1f); * jointSpring6.SetDamping(2, 0.1f); * jointSpring6.SetDamping(3, 0.1f); * jointSpring6.SetDamping(4, 0.1f); * jointSpring6.SetDamping(5, 0.1f); */ jointSpring6.SetParam(ConstraintParam.Erp, 0.6f); jointSpring6.SetParam(ConstraintParam.Cfm, 0.6f); jointSpring6.SetEquilibriumPoint(); Constraint = jointSpring6; break; } }