public override Contact3D GenerateContact(RigidBody3D rb, float dt)
{
//Profiler says this method takes up a lot of processing time (AKA, it's run quite often)
if (rb as SphereBody != null)
{
SphereBody c = rb as SphereBody;
Vector3 normal = rb.Pos - this.Pos;
float normLen = normal.Length();
float dist = normLen - (this.radius + c.radius);
normal /= normLen;
Vector3 pa = this.Pos + normal * this.radius;
Vector3 pb = rb.Pos - normal * c.radius;
return(new Contact3D(normal, dist, this, rb, pa, pb));
}
else if (rb as PlaneBody != null)
{
PlaneBody pb = rb as PlaneBody;
return(pb.GenerateContact(this, dt));
}
else
{
throw new NotImplementedException();
}
}
public override Contact3D GenerateContact(RigidBody3D rb, float dt)
{
//Profiler says this method takes up a lot of processing time (AKA, it's run quite often)
if (rb as SphereBody != null)
{
SphereBody c = rb as SphereBody;
Vector3 normal = rb.Pos - this.Pos;
float normLen = normal.Length();
float dist = normLen - (this.radius + c.radius);
normal /= normLen;
Vector3 pa = this.Pos + normal * this.radius;
Vector3 pb = rb.Pos - normal * c.radius;
return new Contact3D(normal, dist, this, rb, pa, pb);
}
else if (rb as PlaneBody != null)
{
PlaneBody pb = rb as PlaneBody;
return pb.GenerateContact(this, dt);
}
else
{
throw new NotImplementedException();
}
}
public override GraphicsToolkit.Physics._3D.Contact3D GenerateContact(RigidBody3D rb, float dt)
{
if (rb as SphereBody != null)
{
SphereBody c = rb as SphereBody; //sphere that this body is colliding with
Vector3 pa = c.Pos; //point on this body closest to the sphere
Vector2 XYVec = new Vector2(pa.X - 0.5f, pa.Y - 0.5f);
if (XYVec.LengthSquared() < 1) //if inside bounding box but outside actual block
{
pa.X = ((pa.X - 0.5f) / XYVec.Length()) + 0.5f;
pa.Y = ((pa.Y - 0.5f) / XYVec.Length()) + 0.5f;
}
pa.X = MathHelper.Clamp(pa.X, -0.5f, 0.5f);
pa.Y = MathHelper.Clamp(pa.Y, -0.5f, 0.5f);
pa.Z = MathHelper.Clamp(pa.Z, -0.5f, 0.5f);
Vector3 normal = rb.Pos - pa;
float normLen = normal.Length();
float dist = normLen - c.Radius; //distance from block to sphere
normal /= normLen; //normalize normal
Vector3 pb = rb.Pos - normal * c.Radius; //closest point on sphere
return new Contact3D(normal, dist, this, rb, pa, pb);
}
else
{
throw new NotImplementedException();
}
}
public void AddRigidBody(RigidBody3D rb)
{
if (rb as PlaneBody != null)
{
lines.Add(rb);
}
else
{
bodies.Add(rb);
}
}
public Contact3D(Vector3 normal, float d, RigidBody3D a, RigidBody3D b, Vector3 pointA, Vector3 pointB)
{
Normal = normal;
Dist = d;
A = a;
B = b;
this.pointA = pointA;
this.pointB = pointB;
Normal = new Vector3((float)(Normal.X + (rand.NextDouble() - 0.5) / 100f), (float)(Normal.Y + (rand.NextDouble() - 0.5) / 100f), (float)(Normal.Z + (rand.NextDouble() - 0.5) / 100f));
//I = (1+e)*N*(Vr • N) / (1/Ma + 1/Mb)
//Impulse = (Vector3.Dot(b.Vel - a.Vel, normal) / (a.InvMass + b.InvMass));
ImpulseN = 0;
}
public override GraphicsToolkit.Physics._3D.Contact3D GenerateContact(RigidBody3D rb, float dt)
{
if (rb as SphereBody != null)
{
SphereBody c = rb as SphereBody; //sphere that this body is colliding with
Vector3 pa = new Vector3();
float slope = -2f;
if (c.Pos.Y < ((slope * c.Pos.X) + 0.5f)) //if the point is under the line
{
pa.X = MathHelper.Clamp(c.Pos.X, -0.5f, 0.5f);
pa.Y = MathHelper.Clamp(c.Pos.Y, -0.5f, 0.5f);
pa.Z = MathHelper.Clamp(c.Pos.Z, -0.5f, 0.5f);
}
else
{
pa.X = ((c.Pos.X / slope) - 0.5f + c.Pos.Y) / (slope + (1 / slope));
pa.Y = (-1f / slope) * (pa.X - c.Pos.X) + c.Pos.Y;
pa.Z = c.Pos.Z;
if (pa.X > 0.5f)
{
pa.X = 0.5f;
pa.Y = -0.5f;
}
if (pa.Y > 0.5f)
{
pa.X = 0f;
pa.Y = 0.5f;
}
pa.Z = MathHelper.Clamp(pa.Z, -0.5f, 0.5f);
}
Vector3 normal = rb.Pos - pa;
float normLen = normal.Length();
float dist = normLen - c.Radius; //distance from block to sphere
normal /= normLen; //normalize normal
Vector3 pb = rb.Pos - normal * c.Radius; //closest point on sphere
return new Contact3D(normal, dist, this, rb, pa, pb);
}
else
{
throw new NotImplementedException();
}
}
public override Contact3D GenerateContact(RigidBody3D rb, float dt)
{
if (rb as SphereBody != null)
{
SphereBody c = rb as SphereBody;
float dist = DistanceToPoint(c.Pos) - c.Radius;
Vector3 pointOnPlane = c.Pos - (normal * (dist + c.Radius));
Vector3 pointOnBall = c.Pos - (normal * c.Radius);
return(new Contact3D(normal, dist, this, rb, pointOnPlane, pointOnBall));
}
else if (rb as PlaneBody != null)
{
return(new Contact3D());
}
else
{
throw new NotImplementedException();
}
}
public override Contact3D GenerateContact(RigidBody3D rb, float dt)
{
if (rb as SphereBody != null)
{
SphereBody c = rb as SphereBody;
float dist = DistanceToPoint(c.Pos) - c.Radius;
Vector3 pointOnPlane = c.Pos - (normal * (dist+c.Radius));
Vector3 pointOnBall = c.Pos - (normal * c.Radius);
return new Contact3D(normal, dist, this, rb, pointOnPlane, pointOnBall);
}
else if (rb as PlaneBody != null)
{
return new Contact3D();
}
else
{
throw new NotImplementedException();
}
}
public DistanceConstraint3D(RigidBody3D bodyA, RigidBody3D bodyB, float dist)
: base(bodyA, bodyB)
{
distance = dist;
}
public Constraint3D(RigidBody3D bodyA, RigidBody3D bodyB)
{
BodyA = bodyA;
BodyB = bodyB;
}
public abstract Contact3D GenerateContact(RigidBody3D rb, float dt);
public abstract Contact3D GenerateContact(RigidBody3D rb, float dt);
