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;
}
}
