public static float CalcEffectiveMass(PBody b1, PBody b2, Vector2f r1,
Vector2f r2, Vector2f normal)
{
float rn1 = normal.Dot(r1);
float rn2 = normal.Dot(r2);
return(1.0F / (b1.invM + b2.invM + b1.invI
* ((r1.x * r1.x + r1.y * r1.y) - rn1 * rn1) + b2.invI
* ((r2.x * r2.x + r2.y * r2.y) - rn2 * rn2)));
}
public void CalculateCircumcircle(IList <Vector2f> vertices)
{
Vector2f p = vertices[i0];
Vector2f q = vertices[i1];
Vector2f r = vertices[i2];
// calculate the intersection of two perpendicular bisectors
Vector2f pq = q - p;
Vector2f qr = r - q;
// check winding
if (Vector2f.Cross(pq, qr) < 0.0f)
{
throw new InvalidOperationException("Triangle winding order incorrect");
}
// mid-points of edges
Vector2f a = 0.5f * (p + q);
Vector2f b = 0.5f * (q + r);
Vector2f u = pq.PerpendicularCCW;
float d = Vector2f.Dot(u, qr);
float t = Vector2f.Dot(b - a, qr) / d;
CircumCenter = a + t * u;
CircumRadius = (CircumCenter - p).Magnitude;
}
public static Vector2f Projection(this Vector2f obj, Vector2f vector)
{
float vectorLen = vector.Len();
float len = obj.Dot(vector) / vectorLen;
return(vector / vectorLen * len);
}
public static List <VertexArray> GenerateLineWithThickness(List <Vector2f> points, Color color, float thickness, bool[] lines)
{
List <VertexArray> result = new List <VertexArray>( );
VertexArray array = new VertexArray(PrimitiveType.TrianglesStrip);
for (int i = 0; i < points.Count; i++)
{
Vector2f v0 = (i == 0 ? 2 * points[0] - points[1] : points[i - 1]);
Vector2f v1 = points[i];
Vector2f v2 = (i == points.Count - 1 ? 2 * points[i] - points[i - 1] : points[i + 1]);
Vector2f v01 = (v1 - v0).Normalized( );
Vector2f v12 = (v2 - v1).Normalized( );
Vector2f d = (v01 + v12).GetNormal( );
float dot = d.Dot(v01.GetNormal( ));
d *= thickness / 2f / dot; //< TODO: Add flat miter joint in extreme cases
if (lines[i % lines.Length])
{
array.Append(new Vertex(v1 + d, color));
array.Append(new Vertex(v1 - d, color));
}
else
{
if (array.VertexCount > 0)
{
result.Add(array);
array = new VertexArray(PrimitiveType.TrianglesStrip);
}
}
}
if (array.VertexCount > 0)
{
result.Add(array);
}
return(result);
}
private void CollidePlanes()
{
for (int i = 0; i < Particles.Count; ++i)
{
for (int p = 0; p < Planes.Count; ++p)
{
Vector2f n = Planes[p].xy;
float d = Vector2f.Dot(Particles[i].p, n) + Planes[p].z;
if (d < 0.0f)
{
// push out of halfspace
Particles[i].p -= d * n;
// make relative velocity separating
float rv = Vector2f.Dot(Particles[i].v, n);
if (rv < 0.0f)
{
// zero normal velocity, material simulation will take care of restitution
Vector2f nv = -rv * n;
// friction
Vector2f tv = (Particles[i].v + nv) * Friction;
// update velocity
Particles[i].v = tv;
}
}
}
}
}
VertexArray GenerateTrianglesStrip(List <Vector2f> points, Color color, float thickness, bool open)
{
var array = new VertexArray(PrimitiveType.TrianglesStrip);
for (int i = 1; i < points.Count + 1 + (open ? 0 : 1); i++)
{
Vector2f v0 = points[(i - 1) % points.Count];
Vector2f v1 = points[i % points.Count];
Vector2f v2 = points[(i + 1) % points.Count];
Vector2f v01 = (v1 - v0).Normalized();
Vector2f v12 = (v2 - v1).Normalized();
Vector2f d = (v01 + v12).GetNormal();
float dot = d.Dot(v01.GetNormal());
d *= thickness / 2f / dot; //< TODO: Add flat miter joint in extreme cases // d *= thickness / 2f / (float)Math.Max(.8, dot);
if (points.Count == i)
{
array.Append(new Vertex(v0 - d / 4, color));
array.Append(new Vertex(v0 + d / 4, color));
}
else
{
array.Append(new Vertex(v0 + d, color));
array.Append(new Vertex(v0 - d, color));
}
}
return(array);
}
private PPolygonPolygonCollider.PWContactedVertex [] GetEdgeOfPotentialCollision(
PConvexPolygonShape p1, PConvexPolygonShape p2, int r1edge,
bool flip)
{
PPolygonPolygonCollider.PWContactedVertex [] line = new PPolygonPolygonCollider.PWContactedVertex [2];
Vector2f normal = p1.nors[r1edge];
float dist = 1.0F;
int ver = -1;
int nextVer = -1;
for (int i = 0; i < p2.numVertices; i++)
{
float dot = normal.Dot(p2.nors[i]);
if (dot < dist || dist == 1.0F)
{
dist = dot;
ver = i;
nextVer = (i + 1) % p2.numVertices;
}
}
line[0] = new PPolygonPolygonCollider.PWContactedVertex();
line[0].v.Set(p2.vers[ver].x, p2.vers[ver].y);
line[0].data.Set(r1edge + ver * 2 + ver * 4, false);
line[1] = new PPolygonPolygonCollider.PWContactedVertex();
line[1].v.Set(p2.vers[nextVer].x, p2.vers[nextVer].y);
line[1].data.Set(r1edge + ver * 2 + nextVer * 4, false);
return(line);
}
public Projection(Vector2f axis, List<Vector2f> vertices)
{
this.axis = axis;
min = axis.Dot(vertices[0]);
max = min;
for(int i = 1; i < vertices.Count; ++i)
{
float p = axis.Dot(vertices[i]);
if(p < min)
min = p;
else if(p > max)
max = p;
}
}
/// <summary>
/// If the segment ab intersects the curve.
/// </summary>
public bool Intersects(Vector2f a, Vector2f b)
{
//coefficients of quadratic
Vector2f c2 = C0 + C1 * -2.0f + C2;
Vector2f c1 = C0 * -2.0f + C1 * 2.0f;
//Convert line to normal form: ax + by + c = 0
//Find normal to line: negative inverse of original line's slope
Vector2f n = new Vector2f(a.y - b.y, b.x - a.x);
//c coefficient for normal form of line
float c = a.x * b.y - b.x * a.y;
//Transform coefficients to line's coordinate system and find roots of cubic
var roots = Polynomial3d.Solve(1, Vector2f.Dot(n, c2), Vector2f.Dot(n, c1), Vector2f.Dot(n, C0) + c);
Vector2f min, max;
min.x = Math.Min(a.x, b.x);
min.y = Math.Min(a.y, b.y);
max.x = Math.Max(a.x, b.x);
max.y = Math.Max(a.y, b.y);
for (int i = 0; i < roots.real; i++)
{
float t = (float)roots[i];
if (t < 0.0f || t > 1.0f)
{
continue;
}
Vector2f v0 = Position(t);
if (a.x == b.x)
{
if (min.y <= v0.y && v0.y <= max.y)
{
return(true);
}
}
else if (a.y == b.y)
{
if (min.x <= v0.x && v0.x <= max.x)
{
return(true);
}
}
else if (min.x <= v0.x && v0.x <= max.x && min.y <= v0.y && v0.y <= max.y)
{
return(true);
}
}
return(false);
}
internal override void SolvePosition()
{
float rvn = normal.Dot(PTransformer.CalcRelativeCorrectVelocity(b1, b2,
relAnchor1, relAnchor2));
float impulse = -mass * ((dist - length) + rvn) * 0.2F;
if (impulse > 0.0F)
{
impulse = Max(impulse - 0.002F, 0.0F);
}
else if (impulse < 0.0F)
{
impulse = Min(impulse + 0.002F, 0.0F);
}
float forceX = normal.x * impulse;
float forceY = normal.y * impulse;
b1.PositionCorrection(forceX, forceY, anchor1.x, anchor1.y);
b2.PositionCorrection(-forceX, -forceY, anchor2.x, anchor2.y);
}
private PPolygonPolygonCollider.PWContactedVertex [] ClipEdge(PPolygonPolygonCollider.PWContactedVertex [] clips,
Vector2f normal, float dist)
{
PPolygonPolygonCollider.PWContactedVertex [] line = new PPolygonPolygonCollider.PWContactedVertex [2];
int numClips = 0;
float dist0 = normal.Dot(clips[0].v) - dist;
float dist1 = normal.Dot(clips[1].v) - dist;
if (dist0 < 0.0F)
{
line[numClips] = clips[0];
numClips++;
}
if (dist1 < 0.0F)
{
line[numClips] = clips[1];
numClips++;
}
if (numClips == 0)
{
return(null);
}
if (numClips == 2)
{
return(line);
}
int c = 0;
if (dist0 < 0.0F && dist1 > 0.0F)
{
c = 1;
}
float d = dist0 / (dist0 - dist1);
line[1] = new PPolygonPolygonCollider.PWContactedVertex();
line[1].v = clips[1].v.Sub(clips[0].v).Clone();
line[1].v.MulLocal(d);
line[1].v.AddLocal(clips[0].v);
line[1].data = clips[c].data;
return(line);
}
void MoveByMouseDrag(float dt)
{
float dampStrength = 10;
if (MouseIndex != -1)
{
Vector2f pq = MousePos - Scene.Particles[MouseIndex].p;
Vector2f damp = -dampStrength *Vector2f.Dot(pq.Normalized, Scene.Particles[MouseIndex].v) * pq.Normalized;
Vector2f stretch = mouseStrength * pq;
Scene.Particles[MouseIndex].f += stretch + damp;
}
}
protected bool IsLightedByPlayer()
{
if (world.avatar.body.position.DistanceTo(entity.body.position) < LIGHTABLE_DISTANCE)
{
float avatarLookAngle = world.avatar.sprite.rotation;
Vector2f avatarLook = new Vector2f(
Mathf.Cos(avatarLookAngle * Mathf.DEG2RAD),
Mathf.Sin(avatarLookAngle * Mathf.DEG2RAD)
);
Vector2f myLook = (world.avatar.body.position - entity.body.position).Normalized();
float dot = myLook.Dot(avatarLook);
return(dot < -0.5f);
}
else
{
return(false);
}
}
public static float SqrDistanceFromSegment(Segment2f seg, Vector2f p)
{
Vector2f ab = seg.B - seg.A;
Vector2f ac = p - seg.A;
Vector2f bc = p - seg.B;
float e = Vector2f.Dot(ac, ab);
// Handle cases where c projects outside ab
if (e <= 0.0)
{
return(Vector2f.Dot(ac, ac));
}
float f = Vector2f.Dot(ab, ab);
if (e >= f)
{
return(Vector2f.Dot(bc, bc));
}
// Handle case where p projects onto ab
return(Vector2f.Dot(ac, ac) - e * e / f);
}
/// <summary>
/// Gets the intersection between the convex shape and the ray.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="transform">Transform of the convex shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
/// <param name="hit">Ray hit data, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
//Put the ray into local space.
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3f.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Vector3f.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
Vector3f.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
//Check for containment in the cylindrical portion of the capsule.
if (localRay.Position.Y >= -halfLength && localRay.Position.Y <= halfLength && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= collisionMargin * collisionMargin)
{
//It's inside!
hit.T = 0;
hit.Location = localRay.Position;
hit.Normal = new Vector3f(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared;
if (normalLengthSquared > 1e-9f)
{
Vector3f.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3f();
}
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//Project the ray direction onto the plane where the cylinder is a circle.
//The projected ray is then tested against the circle to compute the time of impact.
//That time of impact is used to compute the 3d hit location.
Vector2f planeDirection = new Vector2f(localRay.Direction.X, localRay.Direction.Z);
float planeDirectionLengthSquared = planeDirection.LengthSquared;
if (planeDirectionLengthSquared < MathHelper.Epsilon)
{
//The ray is nearly parallel with the axis.
//Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside
//and won't hit the sides.
if (localRay.Position.Y > halfLength)
{
goto upperSphereTest;
}
if (localRay.Position.Y < -halfLength)
{
goto lowerSphereTest;
}
hit = new RayHit();
return(false);
}
Vector2f planeOrigin = new Vector2f(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2f.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
Vector2f closestPoint;
Vector2f.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2f.Add(ref planeOrigin, ref closestPoint, out closestPoint);
//How close does the ray come to the circle?
float squaredDistance = closestPoint.LengthSquared;
if (squaredDistance > collisionMargin * collisionMargin)
{
//It's too far! The ray cannot possibly hit the capsule.
hit = new RayHit();
return(false);
}
//With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis.
float backwardsDistance = collisionMargin * (float)Math.Sqrt(1 - squaredDistance / (collisionMargin * collisionMargin));
float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared);
hit.T = closestToCenterT - tOffset;
//Compute the impact point on the infinite cylinder in 3d local space.
Vector3f.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3f.Add(ref hit.Location, ref localRay.Position, out hit.Location);
//Is it intersecting the cylindrical portion of the capsule?
if (hit.Location.Y <= halfLength && hit.Location.Y >= -halfLength && hit.T < maximumLength)
{
//Yup!
hit.Normal = new Vector3f(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared;
if (normalLengthSquared > 1e-9f)
{
Vector3f.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3f();
}
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
if (hit.Location.Y < halfLength)
{
goto lowerSphereTest;
}
upperSphereTest:
//Nope! It may be intersecting the ends of the capsule though.
//We're above the capsule, so cast a ray against the upper sphere.
//We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first.
var spherePosition = new Vector3f(0, halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
lowerSphereTest:
//Okay, what about the bottom sphere?
//We're above the capsule, so cast a ray against the upper sphere.
//We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first.
spherePosition = new Vector3f(0, -halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
Vector3f.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
}
private bool Solve(Game0 game, DynamicEntity entity, out Vector2f position, out Vector2f velocity)
{
position = entity.Position;
velocity = entity.Velocity;
// limit falling velocity
velocity.Y = Math.Max(velocity.Y, TerminalFallingVelocity);
// no velocity ? no collision!
if (velocity == Vector2f.Zero)
{
return(false);
}
var obstacles = game.Entities.Where(e => (e as Obstacle) != null).Select(o => o as Obstacle).ToArray();
var remainingVelocity = velocity;
var collisionFound = false;
while (true)
{
var distance = velocity.Length();
var direction = velocity.Normalize();
var entityBox = entity.BoundingBox + position;
var t = Collisions.Trace(entityBox, velocity, obstacles.Select(o => o.BoundingBox + o.Position).ToArray(), out Vector2f normal);
if (t.LeEq(distance))
{
collisionFound = true;
var hitPosition = position + direction * t;
// set new position & velocity
velocity = (position + velocity)
+ normal * Vector2f.Dot(direction.Inv(), normal) * (distance - t)
- hitPosition;
position = hitPosition;
if (normal.X == 0)
{
remainingVelocity.Y = 0;
}
else
{
remainingVelocity.X = 0;
}
if (velocity == Vector2f.Zero) // or better just close to zero ?
{
break; // solved - nowhere to move
}
}
else
{
break; // solved - nothing else stands in the way
}
}
position += velocity;
velocity = remainingVelocity;
return(collisionFound);
}
private void UpdateForces(float dt, bool performFracture)
{
for (int i = 0; i < Particles.Count; ++i)
{
Particles[i].max = 0;
if (Particles[i].invMass > 0.0f)
{
Particles[i].f += Gravity / Particles[i].invMass;
}
else
{
Particles[i].f += Vector2f.Zero - Drag * Particles[i].v;
}
}
Vector2f[] x = new Vector2f[3];
Vector2f[] v = new Vector2f[3];
for (int i = 0; i < Triangles.Count; ++i)
{
Triangle tri = Triangles[i];
FEMElement elem = Elements[i];
x[0] = Particles[tri.i].p;
x[1] = Particles[tri.j].p;
x[2] = Particles[tri.k].p;
v[0] = Particles[tri.i].v;
v[1] = Particles[tri.j].v;
v[2] = Particles[tri.k].v;
if (performFracture)
{
Matrix2x2f f = CalcDeformation(x, elem.mInvDm);
Matrix2x2f q = Decomposition2x2f.QRDecomposition(f);
// strain
Matrix2x2f e = CalcCauchyStrainTensor(q.Transpose * f);
// update plastic strain
float ef = FrobeniusNorm(e);
if (ef > Yield)
{
elem.mEp += e * dt * Creep;
}
const float epmax = 0.6f;
if (ef > epmax)
{
elem.mEp *= epmax / ef;
}
// adjust strain
e -= elem.mEp;
Matrix2x2f s = CalcStressTensor(e, LameLambda, LameMu);
// damping forces
Matrix2x2f dfdt = CalcDeformation(v, elem.mInvDm);
Matrix2x2f dedt = CalcCauchyStrainTensorDt(q.Transpose * dfdt);
Matrix2x2f dsdt = CalcStressTensor(dedt, Damping, Damping);
Matrix2x2f p = s + dsdt;
float e1, e2;
Decomposition2x2f.EigenDecomposition(p, out e1, out e2);
float me = Mathf.Max(e1, e2);
if (me > Toughness)
{
// calculate Eigenvector corresponding to max Eigenvalue
Vector2f ev = q * (new Vector2f(p.m01, me - p.m00)).Normalized;
// pick a random vertex to split on
int splitNode = Rnd.Next(0, 2);
// don't fracture immovable nodes
if (Particles[GetVertex(tri, splitNode)].invMass == 0.0f)
{
break;
}
// fracture plane perpendicular to ev
Vector3f plane = new Vector3f(ev.x, ev.y, -Vector2f.Dot(ev, Particles[GetVertex(tri, splitNode)].p));
FEMFractureEvent fracture = new FEMFractureEvent();
fracture.Tri = i;
fracture.Node = splitNode;
fracture.Plane = plane;
//Fracture not implemented so these fracture planes are not used.
Fractures.Add(fracture);
}
// calculate force on each edge due to stress and distribute to the nodes
Vector2f f1 = q * p * elem.mB[0];
Vector2f f2 = q * p * elem.mB[1];
Vector2f f3 = q * p * elem.mB[2];
Particles[tri.i].f -= f1 / 3.0f;
Particles[tri.j].f -= f2 / 3.0f;
Particles[tri.k].f -= f3 / 3.0f;
}
else
{
//This was the code used when fracturing was disabled
//in the original. It seems very unstable for me. maybe
//a bug or precision issue. Not used atm.
Matrix2x2f f = CalcDeformation(x, elem.mInvDm);
// elastic forces
Matrix2x2f e = CalcGreenStrainTensor(f);
Matrix2x2f s = CalcStressTensor(e, LameLambda, LameMu);
// damping forces
Matrix2x2f dfdt = CalcDeformation(v, elem.mInvDm);
Matrix2x2f dedt = CalcGreenStrainTensorDt(f, dfdt);
Matrix2x2f dsdt = CalcStressTensor(dedt, Damping, Damping);
Matrix2x2f p = s + dsdt;
float e1, e2;
Decomposition2x2f.EigenDecomposition(p, out e1, out e2);
float me = Mathf.Max(e1, e2);
Matrix2x2f finv = f.Transpose.Inverse;
Vector2f f1 = p * (finv * elem.mB[0]);
Vector2f f2 = p * (finv * elem.mB[1]);
Vector2f f3 = p * (finv * elem.mB[2]);
Particles[tri.i].f -= f1 / 3.0f;
Particles[tri.j].f -= f2 / 3.0f;
Particles[tri.k].f -= f3 / 3.0f;
Particles[tri.i].max += me / 3.0f;
Particles[tri.j].max += me / 3.0f;
Particles[tri.k].max += me / 3.0f;
}
}
}
private bool IsPointInsideDomain(Vector2f coordinate)
{
bool result = false;
Vector2f origin = new Vector2f(-100000, -100000);
float minDist = int.MaxValue;
int nbIntersect = 0;
for (int i = 0; i < this.domainPoints.Count; i++)
{
Vector2f point1 = this.domainPoints[i];
Vector2f point2;
if (i == this.domainPoints.Count - 1)
{
point2 = this.domainPoints[0];
}
else
{
point2 = this.domainPoints[i + 1];
}
float num1 = point1.X * point2.Y - point1.Y * point2.X;
float num2 = origin.X * coordinate.Y - origin.Y * coordinate.X;
float denum = (point1.X - point2.X) * (origin.Y - coordinate.Y) - (point1.Y - point2.Y) * (origin.X - coordinate.X);
if (denum != 0)
{
float intersecX = (num1 * (origin.X - coordinate.X) - num2 * (point1.X - point2.X)) / denum;
float intersecY = (num1 * (origin.Y - coordinate.Y) - num2 * (point1.Y - point2.Y)) / denum;
Vector2f intersect = new Vector2f(intersecX, intersecY);
if (intersect != point2 &&
(intersect - point1).Dot(intersect - point2) < 0 &&
(intersect - origin).Dot(intersect - coordinate) < 0)
{
nbIntersect++;
}
}
Vector2f firstVector = coordinate - point1;
Vector2f normalizedEdge = (point2 - point1).Normalize();
//vec2 vector = vector - normalizedEdge * dot(normalizedEdge, vector);
//vec3 crossVector = cross(vec3(firstVector, 0), vec3(normalizedEdge, 0));
//vec3 crossVector2 = cross(vec3(vector2, 0), vec3(-normalizedEdge, 0));
Vector2f secondVector = coordinate - point2;
if (normalizedEdge.Dot(firstVector) * normalizedEdge.Dot(secondVector) < 0)
{
float crossLen = Math.Abs(firstVector.CrossZ(normalizedEdge));
if (crossLen < minDist)
{
minDist = crossLen;
}
}
float lenToPoint = firstVector.Len();
if (lenToPoint < minDist)
{
minDist = lenToPoint;
}
}
if (this.isFilled)
{
if (nbIntersect % 2 == 1)
{
result = true;
}
}
if (minDist < MARGIN_DOMAIN / 2)
{
result = true;
}
return(result);
}
public static Vector2f ClosestPointOnTriangle(Triangle2f triangle, Vector2f p)
{
Vector2f ab = triangle.B - triangle.A;
Vector2f ac = triangle.C - triangle.A;
Vector2f ap = p - triangle.A;
// Check if P in vertex region outside A
float d1 = Vector2f.Dot(ab, ap);
float d2 = Vector2f.Dot(ac, ap);
if (d1 <= 0.0 && d2 <= 0.0)
{
// barycentric coordinates (1,0,0)
return(triangle.A);
}
float v, w;
// Check if P in vertex region outside B
Vector2f bp = p - triangle.B;
float d3 = Vector2f.Dot(ab, bp);
float d4 = Vector2f.Dot(ac, bp);
if (d3 >= 0.0 && d4 <= d3)
{
// barycentric coordinates (0,1,0)
return(triangle.B);
}
// Check if P in edge region of AB, if so return projection of P onto AB
float vc = d1 * d4 - d3 * d2;
if (vc <= 0.0 && d1 >= 0.0f && d3 <= 0.0)
{
v = d1 / (d1 - d3);
// barycentric coordinates (1-v,v,0)
return(triangle.A + v * ab);
}
// Check if P in vertex region outside C
Vector2f cp = p - triangle.C;
float d5 = Vector2f.Dot(ab, cp);
float d6 = Vector2f.Dot(ac, cp);
if (d6 >= 0.0 && d5 <= d6)
{
// barycentric coordinates (0,0,1)
return(triangle.C);
}
// Check if P in edge region of AC, if so return projection of P onto AC
float vb = d5 * d2 - d1 * d6;
if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0)
{
w = d2 / (d2 - d6);
// barycentric coordinates (1-w,0,w)
return(triangle.A + w * ac);
}
// Check if P in edge region of BC, if so return projection of P onto BC
float va = d3 * d6 - d5 * d4;
if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0)
{
w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
// barycentric coordinates (0,1-w,w)
return(triangle.B + w * (triangle.C - triangle.B));
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
float denom = 1.0f / (va + vb + vc);
v = vb * denom;
w = vc * denom;
// = u*a + v*b + w*c, u = va * denom = 1.0f - v - w
return(triangle.A + ab * v + ac * w);
}
public static float Angle(Vector2f a, Vector2f b)
{
return(Math.Acos(Math.Clamp(Vector2f.Dot(Normalize(a), Normalize(b)), -1f, 1f)));
}
// apply a force at 'fromCenter' relative to center of the shape
public void ApplyTorque(Vector2f force, Vector2f fromCenter)
{
var radius = fromCenter.Magnitude();
// check if 'fromCenter' is outside of the shape.
if(radius > GetRadiusOn(fromCenter))
return;
// torque is the amount of force in the perpendicular direction
var torque = force.Cross(fromCenter);
// linear force is the amount of force in the parallel direction
var linearForce = force.Dot(fromCenter) / (radius != 0 ? radius : 1f);
// moment of inertia of the mass at this radius
var inertia = mass * radius * radius;
AngularAcceleration += -torque / (inertia != 0 ? inertia : 1);
ApplyForce(linearForce * force.Unit());
}
public void Dot()
{
Assert.AreEqual(Vector2f.Dot(Vector2f.One, Vector2f.One), 2.0f);
}
public static void ClosestSegmentToSegments(Segment2f seg0, Segment2f seg1, out float s, out float t)
{
Vector2f ab0 = seg0.B - seg0.A;
Vector2f ab1 = seg1.B - seg1.A;
Vector2f a01 = seg0.A - seg1.A;
float d00 = Vector2f.Dot(ab0, ab0);
float d11 = Vector2f.Dot(ab1, ab1);
float d1 = Vector2f.Dot(ab1, a01);
s = 0;
t = 0;
//Check if either or both segments degenerate into points.
if (d00 < FMath.EPS && d11 < FMath.EPS)
{
return;
}
if (d00 < FMath.EPS)
{
//First segment degenerates into a point.
s = 0;
t = FMath.Clamp01(d1 / d11);
}
else
{
float c = Vector2f.Dot(ab0, a01);
if (d11 < FMath.EPS)
{
//Second segment degenerates into a point.
s = FMath.Clamp01(-c / d00);
t = 0;
}
else
{
//The generate non degenerate case starts here.
float d2 = Vector2f.Dot(ab0, ab1);
float denom = d00 * d11 - d2 * d2;
//if segments not parallel compute closest point and clamp to segment.
if (!FMath.IsZero(denom))
{
s = FMath.Clamp01((d2 * d1 - c * d11) / denom);
}
else
{
s = 0;
}
t = (d2 * s + d1) / d11;
if (t < 0.0f)
{
t = 0.0f;
s = FMath.Clamp01(-c / d00);
}
else if (t > 1.0f)
{
t = 1.0f;
s = FMath.Clamp01((d2 - c) / d00);
}
}
}
}
public VertexArray GetVisMesh()
{
if (_boundsNeedUpdate)
{
ComputeBoundaries();
_boundsNeedUpdate = false;
}
// if segments state is clean, no need to calculate mesh again
if (!_visMeshNeedsUpdate)
return _visMesh;
// add all segments to a temp list
_visibleSegments = new List<Segment>(_segments);
// get all unique VISIBLE segments' endpoints + bound endpoints
var points = new HashSet<Vector2f>();
for (var i = _visibleSegments.Count - 1; i >= 0; i--)
{
var s = _visibleSegments[i];
var edge = s.Start - s.End;
var normal = new Vector2f(edge.Y, -edge.X);
// is the segment visible from the ray origin?
var distVector = s.End - _center;
var dot = normal.Dot(distVector);
if (dot > 0 && distVector.Length() <= _radius)
{
points.Add(s.Start);
points.Add(s.End);
}
else
{
_visibleSegments.RemoveAt(i);
}
}
// add bounding points
foreach (var s in _bounds)
{
points.Add(s.Start);
points.Add(s.End);
}
// add bounding segments to visible segments
_visibleSegments.AddRange(_bounds);
// get all angles
var angles = new List<float>();
foreach (
var angle in
points.Select(p => Math.Atan2(p.Y - _center.Y, p.X - _center.X)).Select(angle => (float) angle))
{
angles.Add(angle - 0.0001f);
angles.Add(angle);
angles.Add(angle + 0.0001f);
}
// rays to all visible endpoints
var intersections = new List<Ray>();
foreach (var angle in angles)
{
var dx = Math.Cos(angle);
var dy = Math.Sin(angle);
var rayStart = _center;
var rayEnd = new Vector2f(_center.X + (float) dx, _center.Y + (float) dy);
// find closest intersection
Ray closestIntersect = null;
foreach (
var intersect in
_visibleSegments
.Select(s => MathUtils.GetIntersection(rayStart, rayEnd, s.Start, s.End))
.Where(intersect => intersect != null)
.Where(intersect => closestIntersect == null || intersect.Length < closestIntersect.Length))
closestIntersect = intersect;
if (closestIntersect == null) continue;
closestIntersect.Angle = angle;
intersections.Add(closestIntersect);
}
// sort intersects by angle
intersections.Sort((a, b) => a.Angle.CompareTo(b.Angle));
// make visibility mesh
_visMesh.Clear();
_visMesh.Append(new Vertex(_center, _center));
foreach (var hit in intersections)
_visMesh.Append(new Vertex(hit.Position, hit.Position));
_visMesh.Append(new Vertex(intersections[0].Position, intersections[0].Position));
_visMeshNeedsUpdate = false;
Console.WriteLine("mesh updated with " + intersections.Count + " hits");
return _visMesh;
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs)
{
if (s1._type != PShapeType.CONVEX_SHAPE &&
s1._type != PShapeType.BOX_SHAPE ||
s2._type != PShapeType.CONVEX_SHAPE &&
s2._type != PShapeType.BOX_SHAPE)
{
return(0);
}
PConvexPolygonShape p1 = (PConvexPolygonShape)s1;
PConvexPolygonShape p2 = (PConvexPolygonShape)s2;
PPolygonPolygonCollider.PWDistanceData dis1 = GetDistance(p1, p2);
if (dis1.dist > 0.0F)
{
return(0);
}
PPolygonPolygonCollider.PWDistanceData dis2 = GetDistance(p2, p1);
if (dis2.dist > 0.0F)
{
return(0);
}
float error = 0.008F;
int edgeA;
PConvexPolygonShape pa;
PConvexPolygonShape pb;
bool flip;
if (dis1.dist > dis2.dist + error)
{
pa = p1;
pb = p2;
edgeA = dis1.edge;
flip = false;
}
else
{
pa = p2;
pb = p1;
edgeA = dis2.edge;
flip = true;
}
Vector2f normal = pa.nors[edgeA];
Vector2f tangent = new Vector2f(-normal.y, normal.x);
Vector2f[] paVers = pa.vers;
PPolygonPolygonCollider.PWContactedVertex [] cv = GetEdgeOfPotentialCollision(pa, pb, edgeA,
flip);
cv = ClipEdge(cv, tangent.Negate(), -tangent.Dot(paVers[edgeA]));
if (cv == null)
{
return(0);
}
cv = ClipEdge(cv, tangent,
tangent.Dot(paVers[(edgeA + 1) % pa.numVertices]));
if (cv == null)
{
return(0);
}
Vector2f contactNormal = (flip) ? normal : normal.Negate();
int numContacts = 0;
for (int i = 0; i < 2; i++)
{
float dist = normal.Dot(cv[i].v) - normal.Dot(paVers[edgeA]);
if (dist < 0.0F)
{
PContact c = new PContact();
c.normal.Set(contactNormal.x, contactNormal.y);
c.pos.Set(cv[i].v.x, cv[i].v.y);
c.overlap = dist;
c.data = cv[i].data;
c.data.flip = flip;
cs[numContacts] = c;
numContacts++;
}
}
return(numContacts);
}
public static float Dot(Vector2f a, Vector2f b)
{
return(Vector2f.Dot(a, b));
}
