protected override float GetDistance(System.Numerics.Vector3 start, System.Numerics.Vector3 end)
{
float distance;
Vector3Ex.Distance(ref start, ref end, out distance);
return(distance);
}
/// <summary>
/// Constructs a new distance joint.
/// </summary>
/// <param name="connectionA">First bone connected by the joint.</param>
/// <param name="connectionB">Second bone connected by the joint.</param>
/// <param name="anchorA">Anchor point on the first bone in world space.</param>
/// <param name="anchorB">Anchor point on the second bone in world space.</param>
public IKDistanceJoint(Bone connectionA, Bone connectionB, System.Numerics.Vector3 anchorA, System.Numerics.Vector3 anchorB)
: base(connectionA, connectionB)
{
AnchorA = anchorA;
AnchorB = anchorB;
Vector3Ex.Distance(ref anchorA, ref anchorB, out distance);
}
protected internal override void GetContactInformation(int index, out ContactInformation info)
{
info.Contact = ContactManifold.contacts.Elements[index];
//Find the contact's normal force.
float totalNormalImpulse = 0;
info.NormalImpulse = 0;
for (int i = 0; i < contactConstraint.penetrationConstraints.Count; i++)
{
totalNormalImpulse += contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse;
if (contactConstraint.penetrationConstraints.Elements[i].contact == info.Contact)
{
info.NormalImpulse = contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse;
}
}
//Compute friction force. Since we are using central friction, this is 'faked.'
float radius;
Vector3Ex.Distance(ref contactConstraint.slidingFriction.manifoldCenter, ref info.Contact.Position, out radius);
if (totalNormalImpulse > 0)
{
info.FrictionImpulse = (info.NormalImpulse / totalNormalImpulse) * (contactConstraint.slidingFriction.accumulatedImpulse.Length() + contactConstraint.twistFriction.accumulatedImpulse * radius);
}
else
{
info.FrictionImpulse = 0;
}
//Compute relative velocity
System.Numerics.Vector3 velocity;
//If the pair is handling some type of query and does not actually have supporting entities, then consider the velocity contribution to be zero.
if (EntityA != null)
{
Vector3Ex.Subtract(ref info.Contact.Position, ref EntityA.position, out velocity);
Vector3Ex.Cross(ref EntityA.angularVelocity, ref velocity, out velocity);
Vector3Ex.Add(ref velocity, ref EntityA.linearVelocity, out info.RelativeVelocity);
}
else
{
info.RelativeVelocity = new System.Numerics.Vector3();
}
if (EntityB != null)
{
Vector3Ex.Subtract(ref info.Contact.Position, ref EntityB.position, out velocity);
Vector3Ex.Cross(ref EntityB.angularVelocity, ref velocity, out velocity);
Vector3Ex.Add(ref velocity, ref EntityB.linearVelocity, out velocity);
Vector3Ex.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity);
}
info.Pair = this;
}
internal void VerifyContributions()
{
switch (State)
{
case SimplexState.Point:
if (Vector3Ex.Distance(SimplexA.A - SimplexB.A, A) > .0001f)
{
Debug.WriteLine("break.");
}
break;
case SimplexState.Segment:
if (Vector3Ex.Distance(SimplexA.A - SimplexB.A, A) > .0001f)
{
Debug.WriteLine("break.");
}
if (Vector3Ex.Distance(SimplexA.B - SimplexB.B, B) > .0001f)
{
Debug.WriteLine("break.");
}
break;
case SimplexState.Triangle:
if (Vector3Ex.Distance(SimplexA.A - SimplexB.A, A) > .0001f)
{
Debug.WriteLine("break.");
}
if (Vector3Ex.Distance(SimplexA.B - SimplexB.B, B) > .0001f)
{
Debug.WriteLine("break.");
}
if (Vector3Ex.Distance(SimplexA.C - SimplexB.C, C) > .0001f)
{
Debug.WriteLine("break.");
}
break;
case SimplexState.Tetrahedron:
if (Vector3Ex.Distance(SimplexA.A - SimplexB.A, A) > .0001f)
{
Debug.WriteLine("break.");
}
if (Vector3Ex.Distance(SimplexA.B - SimplexB.B, B) > .0001f)
{
Debug.WriteLine("break.");
}
if (Vector3Ex.Distance(SimplexA.C - SimplexB.C, C) > .0001f)
{
Debug.WriteLine("break.");
}
if (Vector3Ex.Distance(SimplexA.D - SimplexB.D, D) > .0001f)
{
Debug.WriteLine("break.");
}
break;
}
}
