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