///<summry>
/// Fills the given contact structure with the contact needed
/// to keep the cable from over-extending.
///</summry>
public override bool Check(Contact contact)
{
// Find the length of the cable
float length = currentLength();
// Check if we're over-extended
if (length < maxLength)
{
return false;
}
// Otherwise return the contact
// Calculate the normal
Vector3 normal = body[1].Position - body[0].Position;
normal.Normalize();
contact.ContactNormal = normal;
contact.Penetration = length - maxLength;
contact.restitution = restitution;
if (body[0].HasFiniteMass)
contact.ContactPoint = body[1].Position;// +body[1].getHalfSize();
if (body[1].HasFiniteMass)
contact.ContactPoint = body[0].Position;// +body[0].getHalfSize();
return true;
}
public bool addContact(Contact contact)
{
// Calculate the position of each connection point in world coordinates
Vector3 onePosWorld = body[0].GetPointInWorldSpace(position[0]);
Vector3 twoPosWorld = body[1].GetPointInWorldSpace(position[1]);
// Calculate the length of the joint
Vector3 oneToTwo = twoPosWorld - onePosWorld;
Vector3 normal = oneToTwo;
normal = Vector3.Normalize(normal);
float length = oneToTwo.Length();
// Check if it is violated
if (length > constant)
{
contact.body[0] = body[0];
contact.body[1] = body[1];
contact.ContactNormal = normal;
contact.ContactPoint = (onePosWorld + twoPosWorld) * 0.5f;
contact.Penetration = length - constant;
//TODO add friction and restitution to contactdata
contact.friction = 1.0f;
contact.restitution = 0;
return true;
}
return false;
}
public override bool generateContacts(Collidable other, Contact contact)
{
if (other as Box != null)
{
return contact.BoxAndBox();
}
else if (other as Sphere != null)
{
return contact.SphereAndBox();
}
return false;
}
public ContactData generateContacts(Collidable other)
{
Contact contact = new Contact(other, this.plane);
if (other as Sphere != null)
{
contact.SphereAndPlane();
}
if (other as Box != null)
{
contact.BoxAndHalfSpace();
}
return contact.GetContactData();
}
/*
protected override void updateBounding()
{
//TODO add update logic
}
public override Contact generateContacts(Collidable other)
{
Contact contactData = null;
return contactData;
}
*/
public bool generateContacts(Collidable other, Contact contact)
{
//TODO FixME
return false;
if (other as Sphere != null)
{
contact.SphereAndPlane();
}
if (other as Box != null)
{
contact.BoxAndHalfSpace();
}
}
public override ContactData generateContacts(Collidable other)
{
Contact contact = null;
contact = new Contact(this, other);
if (other as Box != null)
{
contact.BoxAndBox();
}
if (other as Sphere != null)
{
contact.SphereAndBox();
}
return contact.GetContactData();
}
public void FindPotentialCollisionsWith(List<Contact> Potentials, BVHNode other)
{
if (!CollidesWith(other))
{
if (!this.isLeaf())
this.FindPotentialCollisions(Potentials);
if (!other.isLeaf())
other.FindPotentialCollisions(Potentials);
return;
}
if (this.isLeaf() && other.isLeaf())
{
Contact temp = new Contact(this.Body, other.Body);
if (!temp.BothFixed())
Potentials.Add(temp);
}
else if (other.isLeaf() && !this.isLeaf())
{
this.Children[0].FindPotentialCollisionsWith(Potentials, other);
this.Children[1].FindPotentialCollisionsWith(Potentials, other);
this.FindPotentialCollisions(Potentials);
}
else if (this.isLeaf() && !other.isLeaf())
{
other.Children[0].FindPotentialCollisionsWith(Potentials, this);
other.Children[1].FindPotentialCollisionsWith(Potentials, this);
}
else
{
other.Children[0].FindPotentialCollisionsWith(Potentials, this.Children[0]);
other.Children[0].FindPotentialCollisionsWith(Potentials, this.Children[1]);
other.Children[1].FindPotentialCollisionsWith(Potentials, this.Children[0]);
other.Children[1].FindPotentialCollisionsWith(Potentials, this.Children[1]);
this.FindPotentialCollisions(Potentials);
other.FindPotentialCollisions(Potentials);
}
}
private Vector3 CalculateFrictionlessImpulse(Contact contactData, Matrix3[] inverseInertiaTensor)
{
Body one = contactData.body[0];
Body two = contactData.body[1];
Vector3 impulseContact;
// Build a vector that shows the change in velocity in
// world space for a unit impulse in the direction of the contact
// normal.
Vector3 torquePerUnitImpulse1 = Vector3.Cross(contactData.relativeContactPosition[0], contactData.ContactNormal);
Vector3 rotationPerUnitImpluse1 = inverseInertiaTensor[0].transform(torquePerUnitImpulse1);
Vector3 VelocityPerUnitImpulse1 = Vector3.Cross(rotationPerUnitImpluse1, contactData.relativeContactPosition[0]);
// Work out the change in velocity in contact coordiantes.
float deltaVelocity = Vector3.Dot(VelocityPerUnitImpulse1, contactData.ContactNormal);
// Add the linear component of velocity change
deltaVelocity += one.InverseMass;
// Check if we need to the second body's data
if (two != null)
{
// Go through the same transformation sequence again
Vector3 torquePerUnitImpulse2 = Vector3.Cross(contactData.relativeContactPosition[1], contactData.ContactNormal);
Vector3 rotationPerUnitImpluse2 = inverseInertiaTensor[1].transform(torquePerUnitImpulse2);
Vector3 VelocityPerUnitImpulse2 = Vector3.Cross(rotationPerUnitImpluse2, contactData.relativeContactPosition[1]);
// Add the change in velocity due to rotation
deltaVelocity += Vector3.Dot(VelocityPerUnitImpulse2, contactData.ContactNormal);
// Add the change in velocity due to linear motion
deltaVelocity += two.InverseMass;
}
// Calculate the required size of the impulse
impulseContact.X = contactData.desiredDeltaVelocity / deltaVelocity;
impulseContact.Y = 0;
impulseContact.Z = 0;
return impulseContact;
}
private Vector3 CalculateFrictionImpulse(Contact contactData, Matrix3[] inverseInertiaTensor)
{
Body one = contactData.body[0];
Body two = contactData.body[1];
Vector3 impulseContact;
float inverseMass = one.InverseMass;
// The equivalent of a cross product in matrices is multiplication
// by a skew symmetric matrix - we build the matrix for converting
// between linear and angular quantities.
Matrix3 impulseToTorque = new Matrix3();
impulseToTorque.setSkewSymmetric(contactData.relativeContactPosition[0]);
// Build the matrix to convert contact impulse to change in velocity
// in world coordinates.
Matrix3 deltaVelWorld = impulseToTorque;
deltaVelWorld *= inverseInertiaTensor[0];
deltaVelWorld *= impulseToTorque;
deltaVelWorld *= -1;
// Check if we need to add body two's data
if (two != null)
{
// Set the cross product matrix
impulseToTorque.setSkewSymmetric(contactData.relativeContactPosition[1]);
// Calculate the velocity change matrix
Matrix3 deltaVelWorldTwo = impulseToTorque;
deltaVelWorldTwo *= inverseInertiaTensor[1];
deltaVelWorldTwo *= impulseToTorque;
deltaVelWorldTwo *= -1;
// Add to the total delta velocity.
deltaVelWorld += deltaVelWorldTwo;
// Add to the inverse mass
inverseMass += two.InverseMass;
}
// Do a change of basis to convert into contact coordinates.
Matrix3 deltaVelocity = contactData.ContactToWorld.transpose();
deltaVelocity *= deltaVelWorld;
deltaVelocity *= contactData.ContactToWorld;
// Add in the linear velocity change
deltaVelocity.data[0] += inverseMass;
deltaVelocity.data[4] += inverseMass;
deltaVelocity.data[8] += inverseMass;
// Invert to get the impulse needed per unit velocity
Matrix3 impulseMatrix = deltaVelocity.inverse();
// Find the velocities that will be removed
Vector3 velKill = new Vector3(contactData.desiredDeltaVelocity,
-contactData.contactVelocity.Y,
-contactData.contactVelocity.Z);
// Find the impulse to kill target velocities
impulseContact = impulseMatrix.transform(velKill);
// Check for exceeding friction
float planarImpulse = (float)Math.Sqrt(Convert.ToDouble(impulseContact.Y * impulseContact.Y + impulseContact.Z * impulseContact.Z));
if ((planarImpulse > impulseContact.X * contactData.friction) && (planarImpulse != 0))
{
// We need to use dynamic friction
impulseContact.Y /= planarImpulse;
impulseContact.Z /= planarImpulse;
impulseContact.X = deltaVelocity.data[0] +
deltaVelocity.data[1] * contactData.friction * impulseContact.Y +
deltaVelocity.data[2] * contactData.friction * impulseContact.Z;
impulseContact.X = contactData.desiredDeltaVelocity / impulseContact.X;
impulseContact.Y *= contactData.friction * impulseContact.X;
impulseContact.Z *= contactData.friction * impulseContact.X;
}
return impulseContact;
}
//LEVEL 1
void calculateContactInformations(float duration)
{
// BoundingBox world = new BoundingBox();
// TODO !! make real world
// CollisionDetector collisionGenerator = new CollisionDetector(world, bodies);
CollisionDetector collisionGenerator = new CollisionDetector(bodies,planes);
this.contactDataList= collisionGenerator.ReDetect();
//if (contactDataList.Count == 0) return;
this.contactDataList.RemoveAll((Contact contact) => {
return !contact.Check();
});
foreach (Link con in conductorList)
{
Contact temp = new Contact(con.body[0], con.body[1]);
if (con.Check(temp))
contactDataList.Add(temp);
}
foreach (Joint joint in jointList)
{
Contact contact=new Contact(null,null);
if (joint.addContact(contact))
contactDataList.Add(contact);
}
}
public void ApplyVelocityChange(Contact contactData,out Vector3[] velocityChange,out Vector3[] rotationChange)
{
Body one = contactData.body[0];
Body two = contactData.body[1];
velocityChange = new Vector3[2];
rotationChange = new Vector3[2];
// Get hold of the inverse mass and inverse inertia tensor, both in
// world coordinates.
Matrix3[] inverseInertiaTensor = new Matrix3[2];
inverseInertiaTensor[0] = one.InverseInertiaTensorWorld;
if (two != null)
inverseInertiaTensor[1] = two.InverseInertiaTensorWorld;
// We will calculate the impulse for each contact axis
Vector3 impulseContact;
if (contactData.friction == 0.0f)
{
//ther is no friction
impulseContact = CalculateFrictionlessImpulse(contactData, inverseInertiaTensor);
}
else
{
// Otherwise we may have impulses that aren't in the direction of the
// contact, so we need the more complex version.
impulseContact = CalculateFrictionImpulse(contactData, inverseInertiaTensor);
}
// Convert impulse to world coordinates
Vector3 impulse = contactData.ContactToWorld.transform(impulseContact);
// Split in the impulse into linear and rotational components
Vector3 impulsiveTorqueOne = Vector3.Cross(contactData.relativeContactPosition[0],impulse);
rotationChange[0] = inverseInertiaTensor[0].transform(impulsiveTorqueOne);
velocityChange[0] = impulse * one.InverseMass;
// Apply the changes
one.AddVelocity(velocityChange[0]);
one.Rotation += rotationChange[0];
if (two != null)
{
// Work out body one's linear and angular changes
Vector3 impulsiveTorqueTwo = Vector3.Cross(impulse,contactData.relativeContactPosition[1]);
rotationChange[1] = inverseInertiaTensor[1].transform(impulsiveTorqueTwo);
velocityChange[1] = -impulse * two.InverseMass;
// And apply them.
two.AddVelocity(velocityChange[1]);
two.Rotation += rotationChange[1];
}
}
public void AddContactData(Contact contactdata)
{
contactDataList.Add(contactdata);
}
///<summary> /// Fills the given contact structure with the generated /// contact. The contact pointer should point to the first /// available contact in a contact array, where limit is the /// maximum number of contacts in the array that can be written /// to. The method returns the number of contacts that have /// been written. ///</summary> //public uint addContact(Contact contact); //public abstract uint addContact(); public abstract bool Check(Contact contact);
/**
* Fills the given contact structure with the contact needed
* to keep the rod from extending or compressing.
*/
public override bool Check(Contact contact)
{
// Find the length of the rod
float currentLen = currentLength();
// Check if we're over-extended
if (currentLen == length)
{
return false;
}
// Otherwise return the contact
// Calculate the normal
Vector3 normal = body[1].Position - body[0].Position;
normal.Normalize();
// The contact normal depends on whether we're extending or compressing
if (currentLen > length)
{
contact.ContactNormal = normal;
contact.Penetration = currentLen - length;
}
else
{
contact.ContactNormal = normal * -1;
contact.Penetration = length - currentLen;
}
if (body[0].HasFiniteMass)
contact.ContactPoint = body[1].Position;// +body[1].getHalfSize();
if (body[1].HasFiniteMass)
contact.ContactPoint = body[0].Position;// +body[0].getHalfSize();
// Always use zero restitution (no bounciness)
contact.restitution = 1;
return true;
}
/// <summary> /// generates contact information for this collidable body with another one /// and fill this info into an existing contact /// </summary> /// <param name="other"></param> /// <param name="contact">a contact to fill</param> /// <returns></returns> public abstract bool generateContacts(Collidable other,Contact contact);
