public Triangle(Vector3 pos1, Vector3 pos2, Vector3 pos3)
{
Pos1 = pos1;
Pos2 = pos2;
Pos3 = pos3;
Normal = VMath.Cross(Pos3 - Pos1, Pos2 - Pos1);
}
public void ApplyImpulse(Vector2R impulse, Vector2R contactVector)
{
//
velocity += invmass * impulse;
//if (float.IsNaN(velocity.X)) System.Diagnostics.Debugger.Break();
angularVelocity += invinertia * (float)VMath.Cross(contactVector, impulse);
//if (float.IsInfinity(angularVelocity)) System.Diagnostics.Debugger.Break();
}
public bool IsIntersectWithRay(Ray ray)
{
var cross1 = VMath.Cross(Pos2 - Pos1, ray.pos - Pos1);
var dot1 = VMath.Dot(cross1, ray.vec);
var cross2 = VMath.Cross(Pos3 - Pos2, ray.pos - Pos2);
var dot2 = VMath.Dot(cross2, ray.vec);
if (dot1 > 0 ^ dot2 > 0)
{
return(false);
}
var cross3 = VMath.Cross(Pos1 - Pos3, ray.pos - Pos3);
var dot3 = VMath.Dot(cross3, ray.vec);
return(!(dot2 > 0 ^ dot3 > 0));
}
public override void ComputeMass(float density)
{
//calculate centroid and moment of inertia
Vector2R c = new Vector2R(0, 0); // centroid
double area = 0;
double I = 0;
double k_inv3 = 1.0 / 3.0;
for (int i1 = 0; i1 < vertexCount; i1++)
{
Vector2R p1 = vertices[i1];
int i2 = i1 + 1 < vertexCount ? i1 + 1 : 0;
Vector2R p2 = vertices[i2];
double D = VMath.Cross(p1, p2);
double triangleArea = 0.5 * D;
area += triangleArea;
//use area to weight the centroid average, not just the vertex position
c += VMath.MultVectDouble(p1 + p2, triangleArea * k_inv3); // triangleArea * k_inv3 * (p1 + p2);
double intx2 = p1.X * p1.X + p2.X * p1.X + p2.X * p2.X;
double inty2 = p1.Y * p1.Y + p2.Y * p1.Y + p2.Y * p2.Y;
I += (0.25 * k_inv3 * D) * (intx2 + inty2);
}
c = VMath.MultVectDouble(c, 1.0 / area);
//translate verticies to centroid (make centroid (0,0)
//for the polygon in model space)
//Not really necessary but I like doing this anyway
for (int i = 0; i < vertexCount; i++)
{
vertices[i] -= c;
}
body.mass = density * (float)area;
body.inertia = (float)I * density;
}
public void Initialize()
{
e = Math.Min(a.restitution, b.restitution);
sf = Math.Sqrt(a.staticFriction * a.staticFriction);
df = Math.Sqrt(a.dynamicFriction * a.dynamicFriction);
for (int i = 0; i < contact_count; i++)
{
Vector2R ra = contacts[i] - a.pos;
Vector2R rb = contacts[i] - b.pos;
Vector2R crossprod_b = VMath.Cross(b.angularVelocity, rb);
Vector2R crossprod_a = VMath.Cross(a.angularVelocity, ra);
Vector2R rv = b.velocity + crossprod_b - a.velocity - crossprod_a;
//Vector2 rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
// a.velocity - VMath.Cross(a.angularVelocity, ra);
//if (b.velocity.IsFucked()) System.Diagnostics.Debugger.Break(); //removelater
//if (a.velocity.IsFucked()) System.Diagnostics.Debugger.Break(); //removelater
//if (rv.Length() < (dt,gravity).LengthSquared() + EPSILON) e = 0.0f;
}
}
public void ApplyImpulse()
{
if (GMath.Equal(a.invmass + b.invmass, 0))
{
InfinitMassCorrection();
return;
}
for (int i = 0; i < contact_count; i++)
{
//calcuate radii from COM to contact
Vector2R ra = contacts[i] - a.pos;
Vector2R rb = contacts[i] - b.pos;
//relative velocity
Vector2R rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
a.velocity - VMath.Cross(a.angularVelocity, ra);
//relative velocity along the normal
double contactVel = Vector2R.Dot(rv, normal);
//do not resolve if velocities are seperating
if (contactVel > 0)
{
return;
}
double raCrossN = VMath.Cross(ra, normal);
double rbCrossN = VMath.Cross(rb, normal);
double invMassSum = a.invmass + b.invmass + (raCrossN * raCrossN) * a.invinertia + (rbCrossN * rbCrossN) * b.invinertia;
//calculate impulse scalar
double j = -(1.0 + e) * contactVel;
j /= invMassSum;
j /= (double)contact_count;
//apply impulse
Vector2R impulse = VMath.MultVectDouble(normal, j); // normal * j;
a.ApplyImpulse(-impulse, ra);
b.ApplyImpulse(impulse, rb);
//friction impulse
rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
a.velocity - VMath.Cross(a.angularVelocity, ra);
Vector2R t = rv - (normal * Vector2R.Dot(rv, normal));
//t.Normalize();
VMath.NormalizeSafe(ref t);
//j tangent magnitude
double jt = -Vector2R.Dot(rv, t);
jt /= invMassSum;
jt /= (double)contact_count;
//don't apply tiny friction impulses
if (GMath.Equal(jt, 0.0))
{
return;
}
//coulumbs law
Vector2R tangentImpulse;
if (Math.Abs(jt) < j * sf)
{
tangentImpulse = VMath.MultVectDouble(t, df); // t * df;
}
else
{
tangentImpulse = VMath.MultVectDouble(t, -j * df); // t * -j * df
}
//apply friction impulse
a.ApplyImpulse(-tangentImpulse, ra);
b.ApplyImpulse(tangentImpulse, rb);
}
}
public void Set(Vector2R[] verts, int count)
{
//no hulls with less than 3 verticies (ensure actual polygon)
//Debug.Assert(count > 2 && count < MaxPolyVertexCount);
count = Math.Min(count, MaxPolyVertexCount);
//find the right most point in the hull
int rightMost = 0;
double highestXCoord = verts[0].X;
for (int i = 1; i < count; i++)
{
double x = verts[0].X;
if (x > highestXCoord)
{
highestXCoord = x;
rightMost = i;
}
//if matching x then take farthest negative y
else if (x == highestXCoord && verts[i].Y < verts[rightMost].Y)
{
rightMost = i;
}
}
int[] hull = new int[MaxPolyVertexCount];
int outCount = 0;
int indexHull = rightMost;
for (;;)
{
hull[outCount] = indexHull;
// search for next index that wraps around the hull
// by computing cross products to find the most counter-clockwise
// vertex in the set, given the previous hull index
int nextHullIndex = 0;
for (int i = 1; i < count; i++)
{
//skip if same coordinate as we need three unique
//points in the set to perform a cross product
if (nextHullIndex == indexHull)
{
nextHullIndex = i;
continue;
}
// cross every set of three unquie verticies
// record each counter clockwise third vertex and add
// to the output hull
Vector2R e1 = verts[nextHullIndex] - verts[hull[outCount]];
Vector2R e2 = verts[i] - verts[hull[outCount]];
double c = VMath.Cross(e1, e2);
if (c < 0.0f)
{
nextHullIndex = i;
}
// Cross product is zero then e vectors are on same line
// therefor want to record vertex farthest along that line
if (c == 0.0f && e2.LengthSquared() > e1.LengthSquared())
{
nextHullIndex = i;
}
}
outCount++;
indexHull = nextHullIndex;
//conclude algorithm upon wraparound
if (nextHullIndex == rightMost)
{
vertexCount = outCount;
break;
}
}
float maxDist = 0;
// Copy vertices into shape's vertices
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = verts[hull[i]];
float dist = Vector2R.Distance(Vector2R.Zero, vertices[i]);
if (dist > maxDist)
{
maxDist = dist;
}
}
polyReach = maxDist * 2;
ComputeNormals();
}
