private void LoadJoint(UrdfJoint joint)
{
RigidBody childRigidBody;
if (!_linkToRigidBody.TryGetValue(joint.Child.Name, out childRigidBody))
{
return;
}
RigidBody parentRigidBody;
if (joint.Parent.Collisions.Any())
{
if (!_linkToRigidBody.TryGetValue(joint.Parent.Name, out parentRigidBody))
{
return;
}
}
else
{
parentRigidBody = TypedConstraint.GetFixedBody();
}
TypedConstraint constraint;
if (joint is UrdfContinuousJoint)
{
constraint = CreateRevoluteJoint(childRigidBody, parentRigidBody);
}
else if (joint is UrdfFixedJoint)
{
Matrix childFrame = ParseInertia(joint.Child.Inertial.Inertia);
childFrame = ParsePose(joint.Origin);
constraint = CreateFixedJoint(childRigidBody, parentRigidBody, childFrame);
}
else
{
//throw new NotImplementedException();
return;
}
World.AddConstraint(constraint, true);
}
HingeConstraint _AddGateHinge()
{
Matrix AFrame = Matrix.RotationX(0) * Matrix.RotationZ(90.0f.ToRad()) * Matrix.RotationY(90.0f.ToRad()) * Matrix.Translation(offset); // rotatione required to make X axis pivot
Matrix BFrame = AFrame * matrix;
//var hinge = new HingeConstraint(
// body,
// Vector3.Zero + base.offset,
// Vector3.UnitX,
// true);
//var ma = hinge.AFrame;
//var mb = hinge.BFrame;
//var oa = hinge.FrameOffsetA;
//var ob = hinge.FrameOffsetB;
//var im = matrix;
//im.Invert();
//var mb2 = mb * im;
//hinge.Dispose();
var hinge = new HingeConstraint(
body,
TypedConstraint.GetFixedBody(),
AFrame,
BFrame,
true) ;
if (_type!= GateType.Spinner)
{
hinge.SetLimit(_startAngle, _endAngle, 0.0f); // revers angles to have same as in VPX behavior
}
//body.ActivationState = ActivationState.DisableDeactivation;
//body.SetSleepingThresholds(float.MaxValue, 0.0f); // no sleep for flippers
return hinge;
}
