internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
Vector2 u = cB + rB - cA - rA;
float length = u.Length(); u.Normalize();
float C = length - MaxLength;
C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection);
float impulse = -_mass * C;
Vector2 P = impulse * u;
cA -= _invMassA * P;
aA -= _invIA * MathUtils.Cross(ref rA, ref P);
cB += _invMassB * P;
aB += _invIB * MathUtils.Cross(ref rB, ref P);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(length - MaxLength < Settings.LinearSlop);
}
public void Update(float timeStep, float kSpring, float influenceRadius)
{
if (!Active)
{
return;
}
Vector2 dir = P1.Position - P0.Position;
float distance = dir.Length();
dir.Normalize();
// This is to avoid imploding simulation with really springy fluids
if (distance < 0.5f * influenceRadius)
{
Active = false;
return;
}
if (RestLength > influenceRadius)
{
Active = false;
return;
}
//Algorithm 3
float displacement = timeStep * timeStep * kSpring * (1.0f - RestLength / influenceRadius) * (RestLength - distance) * 0.5f;
dir *= displacement;
P0.Position -= dir;
P1.Position += dir;
}
public void InterpretGoal(Tantric.Logic.Unit u, Tantric.Logic.UnitGoal goal, int elapsedMilliseconds)
{
switch (goal.Name.ToLower())
{
case "moveto": // Move To is exclusive
Microsoft.Xna.Framework.Vector2 dir = (Microsoft.Xna.Framework.Vector2)goal.GetArgument(1) - u.Position;
Microsoft.Xna.Framework.Vector2 norm = dir;
norm.Normalize();
norm *= elapsedMilliseconds;
norm /= 1000;
norm *= World.Objects.Human.HumanStatistics.GetStatistic("Human_Assembler", "Speed");
if (dir.LengthSquared() == 0)
{
goal.Satisfy();
break;
}
if (norm.LengthSquared() > dir.LengthSquared())
{
u.Translate(dir);
goal.Satisfy();
}
else
{
u.Translate(norm);
}
break;
default:
break;
}
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
float h = data.step.dt;
float inv_h = data.step.inv_dt;
// Solve angular friction
{
float Cdot = wB - wA + inv_h * CorrectionFactor * _angularError;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = h * _maxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, ref _rB) - vA - MathUtils.Cross(wA, ref _rA) + inv_h * CorrectionFactor * _linearError;
Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = h * _maxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(ref _rA, ref impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(ref _rB, ref impulse);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public static PhysicalVector2 NextPhysicalVector2(this Random random)
{
var result = new PhysicalVector2(
(float)random.NextDouble(-1.0, 1.0),
(float)random.NextDouble(-1.0, 1.0)
);
result.Normalize();
return(result);
}
private static void DrawLine(Core.CustomSpriteBatch g, Microsoft.Xna.Framework.Graphics.Texture2D sprPixel, Microsoft.Xna.Framework.Vector2 StartPos, Microsoft.Xna.Framework.Vector2 EndPos, Microsoft.Xna.Framework.Color DrawColor)
{
Microsoft.Xna.Framework.Vector2 ScaleFactor = new Microsoft.Xna.Framework.Vector2(EndPos.X - StartPos.X, EndPos.Y - StartPos.Y);
ScaleFactor.Normalize();
while ((int)StartPos.X != EndPos.X)
{
g.Draw(sprPixel, StartPos, DrawColor);
StartPos += ScaleFactor;
}
}
public static void Initialize(ContactPositionConstraint pc, ref Transform xfA, ref Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation)
{
Debug.Assert(pc.pointCount > 0);
switch (pc.type)
{
case ManifoldType.Circles:
{
Vector2 pointA = Transform.Multiply(ref pc.localPoint, ref xfA);
Vector2 pointB = Transform.Multiply(pc.localPoints[0], ref xfB);
normal = pointB - pointA;
// Handle zero normalization
if (normal != Vector2.Zero)
{
normal.Normalize();
}
point = 0.5f * (pointA + pointB);
separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
}
break;
case ManifoldType.FaceA:
{
Complex.Multiply(ref pc.localNormal, ref xfA.q, out normal);
Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfA);
Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfB);
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
}
break;
case ManifoldType.FaceB:
{
Complex.Multiply(ref pc.localNormal, ref xfB.q, out normal);
Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfB);
Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfA);
separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
default:
normal = Vector2.Zero;
point = Vector2.Zero;
separation = 0;
break;
}
}
/// <summary>
/// tests if ray intersects AABB
/// </summary>
/// <param name="aabb"></param>
/// <returns></returns>
public static bool RayCastAABB(AABB aabb, Vector2 p1, Vector2 p2)
{
AABB segmentAABB = new AABB();
{
Vector2.Min(ref p1, ref p2, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref p2, out segmentAABB.UpperBound);
}
if (!AABB.TestOverlap(ref aabb, ref segmentAABB))
{
return(false);
}
Vector2 rayDir = p2 - p1;
Vector2 rayPos = p1;
Vector2 norm = new Vector2(-rayDir.Y, rayDir.X); //normal to ray
if (norm.Length() == 0.0f)
{
return(true); //if ray is just a point, return true (iff point is within aabb, as tested earlier)
}
norm.Normalize();
float dPos = Vector2.Dot(rayPos, norm);
var verts = aabb.Vertices;
float d0 = Vector2.Dot(verts[0], norm) - dPos;
for (int i = 1; i < 4; i++)
{
float d = Vector2.Dot(verts[i], norm) - dPos;
if (Math.Sign(d) != Math.Sign(d0))
{
//return true if the ray splits the vertices (ie: sign of dot products with normal are not all same)
return(true);
}
}
return(false);
}
public void ProjectParentVelocityOnLastMoveCollisionTangent(float minimumVectorLengthSquared)
{
#if FRB_MDX
if (LastMoveCollisionReposition.LengthSq() > minimumVectorLengthSquared &&
#else
if (LastMoveCollisionReposition.LengthSquared() > minimumVectorLengthSquared &&
#endif
Vector2.Dot(
new Vector2(TopParent.Velocity.X, TopParent.Velocity.Y), LastMoveCollisionReposition) < 0)
{
Vector2 collisionAdjustmentNormalized = LastMoveCollisionReposition;
collisionAdjustmentNormalized.Normalize();
float temporaryFloat = collisionAdjustmentNormalized.X;
collisionAdjustmentNormalized.X = -collisionAdjustmentNormalized.Y;
collisionAdjustmentNormalized.Y = temporaryFloat;
float length = Vector2.Dot(
new Vector2(TopParent.Velocity.X, TopParent.Velocity.Y), collisionAdjustmentNormalized);
TopParent.Velocity.X = collisionAdjustmentNormalized.X * length;
TopParent.Velocity.Y = collisionAdjustmentNormalized.Y * length;
}
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="xfA">The transform for A.</param>
/// <param name="radiusA">The radius for A.</param>
/// <param name="xfB">The transform for B.</param>
/// <param name="radiusB">The radius for B.</param>
/// <param name="normal">World vector pointing from A to B</param>
/// <param name="points">Torld contact point (point of intersection).</param>
public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 <Vector2> points)
{
normal = Vector2.Zero;
points = new FixedArray2 <Vector2>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new Vector2(1.0f, 0.0f);
Vector2 pointA = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
Vector2 pointB = Transform.Multiply(manifold.Points[0].LocalPoint, ref xfB);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
normal = Complex.Multiply(ref manifold.LocalNormal, ref xfA.q);
Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfB);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
normal = Complex.Multiply(ref manifold.LocalNormal, ref xfB.q);
Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfB);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfA);
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
public void GenVoronoi(IEnumerable<Vector> points)
{
VoronoiGraph graph = Fortune.ComputeVoronoiGraph(points);
foreach (VoronoiEdge vedge in graph.Edges)
{
if (double.IsNaN(vedge.VVertexB[0]))
{
Vector2 vectLeft = Tools.Functions.VectorToVector2(vedge.LeftData);
Vector2 vectRight = Tools.Functions.VectorToVector2(vedge.RightData);
Vector2 start = Tools.Functions.VectorToVector2(vedge.VVertexA);
Vector2 dir = new Vector2(-vectRight.Y + vectLeft.Y, vectRight.X - vectLeft.X);
dir.Normalize();
dir /= 2;
Vector2 end = dir + start;
if (end.X <= 1 && end.X >= 0 && end.Y >= 0 && end.Y <= 1)
end = start - dir;
vedge.VVertexB = Tools.Functions.Vector2ToVector(end);
}
if (!((vedge.VVertexA[0] < 0 || vedge.VVertexA[1] < 0 || vedge.VVertexA[0] > 1 || vedge.VVertexA[1] > 1)
&& (vedge.VVertexB[0] < 0 || vedge.VVertexB[1] < 0 || vedge.VVertexB[0] > 1 || vedge.VVertexB[1] > 1))
&& !double.IsNaN(vedge.VVertexB[0])
)
{
Edge e = new Edge();
Center cent1, cent2;
Corner corn1, corn2;
if (!Centers.TryGetValue(Tools.Functions.VectorToVector2(vedge.LeftData), out cent1))
{
cent1 = new Center(Tools.Functions.VectorToVector2(vedge.LeftData));
Centers.Add(cent1.position, cent1);
}
if (!Centers.TryGetValue(Tools.Functions.VectorToVector2(vedge.RightData), out cent2))
{
cent2 = new Center(Tools.Functions.VectorToVector2(vedge.RightData));
Centers.Add(cent2.position, cent2);
}
if (!Corners.TryGetValue(Tools.Functions.VectorToVector2(vedge.VVertexA), out corn1))
{
corn1 = new Corner(Tools.Functions.VectorToVector2(vedge.VVertexA));
Corners.Add(corn1.position, corn1);
}
if (!Corners.TryGetValue(Tools.Functions.VectorToVector2(vedge.VVertexB), out corn2))
{
corn2 = new Corner(Tools.Functions.VectorToVector2(vedge.VVertexB));
Corners.Add(corn2.position, corn2);
}
if (corn1.position != corn2.position)
{
fill(corn1, corn2, cent1, cent2, e);
Edges.Add(e.ID, e);
}
}
}
}
public bool CollideAgainstMove(Circle circle, float thisMass, float otherMass)
{
#if DEBUG
if (thisMass == 0 && otherMass == 0)
{
throw new ArgumentException("Both masses cannot be 0. For equal masses pick a non-zero value");
}
#endif
if (circle.CollideAgainst(this))
{
Point circleCenter = new Point(circle.X, circle.Y);
if (IsPointOnOrInside(ref circleCenter))
{
double xDistanceToMoveCircle = 0;
double yDistanceToMoveCircle = 0;
float smallestDistance = float.PositiveInfinity;
if ((this.RepositionDirections & Geometry.RepositionDirections.Right) == Geometry.RepositionDirections.Right)
{
smallestDistance = Right - circle.X;
xDistanceToMoveCircle = smallestDistance + circle.Radius;
}
if ((this.RepositionDirections & Geometry.RepositionDirections.Left) == Geometry.RepositionDirections.Left &&
circle.X - Left < smallestDistance)
{
smallestDistance = circle.X - Left;
xDistanceToMoveCircle = -smallestDistance - circle.Radius;
}
if ((this.RepositionDirections & Geometry.RepositionDirections.Up) == Geometry.RepositionDirections.Up &&
Top - circle.Y < smallestDistance)
{
smallestDistance = Top - circle.Y;
xDistanceToMoveCircle = 0;
yDistanceToMoveCircle = smallestDistance + circle.Radius;
}
if ((this.RepositionDirections & Geometry.RepositionDirections.Down) == Geometry.RepositionDirections.Down &&
circle.Y - Bottom < smallestDistance)
{
smallestDistance = circle.Y - Bottom;
xDistanceToMoveCircle = 0;
yDistanceToMoveCircle = -smallestDistance - circle.Radius;
}
float amountToMoveThis = otherMass / (thisMass + otherMass);
mLastMoveCollisionReposition.X = (float)(-xDistanceToMoveCircle * amountToMoveThis);
mLastMoveCollisionReposition.Y = (float)(-yDistanceToMoveCircle * amountToMoveThis);
TopParent.X += mLastMoveCollisionReposition.X;
TopParent.Y += mLastMoveCollisionReposition.Y;
circle.LastMoveCollisionReposition.X = (float)(xDistanceToMoveCircle * (1 - amountToMoveThis));
circle.LastMoveCollisionReposition.Y = (float)(yDistanceToMoveCircle * (1 - amountToMoveThis));
circle.mLastCollisionTangent = new Point(circle.LastMoveCollisionReposition.Y,
-circle.LastMoveCollisionReposition.X);
circle.TopParent.Position.X += circle.LastMoveCollisionReposition.X;
circle.TopParent.Position.Y += circle.LastMoveCollisionReposition.Y;
ForceUpdateDependencies();
circle.ForceUpdateDependencies();
return(true);
}
else
{
Segment collisionSegment = new Segment();
// top
Segment edge = new Segment();
float smallestDistance = float.PositiveInfinity;
#if FRB_MDX
Vector2 amountToMove = Vector2.Empty;
#else
Vector2 amountToMove = Vector2.Zero;
#endif
bool isAmountToMoveSet = false;
if ((this.RepositionDirections & Geometry.RepositionDirections.Up) == Geometry.RepositionDirections.Up)
{
bool shouldUseInfiniteSegment = (circleCenter.X < Position.X &&
(this.RepositionDirections & Geometry.RepositionDirections.Left) != Geometry.RepositionDirections.Left) ||
(circleCenter.X > Position.X &&
(this.RepositionDirections & Geometry.RepositionDirections.Right) != Geometry.RepositionDirections.Right);
if (shouldUseInfiniteSegment)
{
smallestDistance = Top + circle.Radius - circle.Position.Y;
isAmountToMoveSet = true;
amountToMove.X = 0;
amountToMove.Y = -smallestDistance;
}
else
{
// Maybe we can save by not calling "new"
edge = new Segment(
new Point(this.Left, this.Top),
new Point(this.Right, this.Top));
smallestDistance = edge.DistanceTo(circleCenter, out collisionSegment);
}
}
if ((this.RepositionDirections & Geometry.RepositionDirections.Down) == Geometry.RepositionDirections.Down)
{
bool shouldUseInfiniteSegment = (circleCenter.X < Position.X &&
(this.RepositionDirections & Geometry.RepositionDirections.Left) != Geometry.RepositionDirections.Left) ||
(circleCenter.X > Position.X &&
(this.RepositionDirections & Geometry.RepositionDirections.Right) != Geometry.RepositionDirections.Right);
if (shouldUseInfiniteSegment)
{
float candidate = Bottom - circle.Radius - circle.Position.Y;
if (System.Math.Abs(candidate) < System.Math.Abs(smallestDistance))
{
smallestDistance = candidate;
isAmountToMoveSet = true;
amountToMove.X = 0;
amountToMove.Y = -smallestDistance;
}
}
else
{
// bottom
edge = new Segment(
new Point(Left, Bottom),
new Point(Right, Bottom));
if (edge.DistanceTo(circleCenter) < smallestDistance)
{
smallestDistance = (float)edge.DistanceTo(circleCenter, out collisionSegment);
}
}
}
if ((this.RepositionDirections & Geometry.RepositionDirections.Left) == Geometry.RepositionDirections.Left)
{
bool shouldUseInfiniteSegment = (circleCenter.Y < Position.Y &&
(this.RepositionDirections & Geometry.RepositionDirections.Down) != Geometry.RepositionDirections.Down) ||
(circleCenter.Y > Position.Y &&
(this.RepositionDirections & Geometry.RepositionDirections.Up) != Geometry.RepositionDirections.Up);
if (shouldUseInfiniteSegment)
{
float candidate = Left - circle.Radius - circle.Position.X;
if (System.Math.Abs(candidate) < System.Math.Abs(smallestDistance))
{
smallestDistance = candidate;
isAmountToMoveSet = true;
amountToMove.Y = 0;
amountToMove.X = -smallestDistance;
}
}
else
{
// left
edge = new Segment(
new Point(Left, Top),
new Point(Left, Bottom));
if (edge.DistanceTo(circleCenter) < smallestDistance)
{
smallestDistance = (float)edge.DistanceTo(circleCenter, out collisionSegment);
}
}
}
if ((this.RepositionDirections & Geometry.RepositionDirections.Right) == Geometry.RepositionDirections.Right)
{
bool shouldUseInfiniteSegment = (circleCenter.Y < Position.Y &&
(this.RepositionDirections & Geometry.RepositionDirections.Down) != Geometry.RepositionDirections.Down) ||
(circleCenter.Y > Position.Y &&
(this.RepositionDirections & Geometry.RepositionDirections.Up) != Geometry.RepositionDirections.Up);
if (shouldUseInfiniteSegment)
{
float candidate = Right + circle.Radius - circle.Position.X;
if (System.Math.Abs(candidate) < System.Math.Abs(smallestDistance))
{
smallestDistance = candidate;
isAmountToMoveSet = true;
amountToMove.Y = 0;
amountToMove.X = -smallestDistance;
}
}
else
{
// right
edge = new Segment(
new Point(Right, Top),
new Point(Right, Bottom));
if (edge.DistanceTo(circleCenter) < smallestDistance)
{
smallestDistance = (float)edge.DistanceTo(circleCenter, out collisionSegment);
// edgeClosestTo = "right";
}
}
}
if (smallestDistance <= circle.Radius)
{
float remainingDistance = (float)circle.Radius - smallestDistance;
if (!isAmountToMoveSet)
{
amountToMove = new Vector2((float)(collisionSegment.Point2.X - collisionSegment.Point1.X),
(float)(collisionSegment.Point2.Y - collisionSegment.Point1.Y));
amountToMove.Normalize();
amountToMove = amountToMove * remainingDistance;
}
float amountToMoveThis = otherMass / (thisMass + otherMass);
mLastMoveCollisionReposition.X = amountToMove.X * amountToMoveThis;
mLastMoveCollisionReposition.Y = amountToMove.Y * amountToMoveThis;
TopParent.X += mLastMoveCollisionReposition.X;
TopParent.Y += mLastMoveCollisionReposition.Y;
circle.LastMoveCollisionReposition.X = -amountToMove.X * (1 - amountToMoveThis);
circle.LastMoveCollisionReposition.Y = -amountToMove.Y * (1 - amountToMoveThis);
circle.TopParent.Position.X += circle.LastMoveCollisionReposition.X;
circle.TopParent.Position.Y += circle.LastMoveCollisionReposition.Y;
ForceUpdateDependencies();
circle.ForceUpdateDependencies();
return(true);
}
else
{
return(false);
}
}
}
return(false);
}
/// <summary>
/// Triangulates a polygon using simple ear-clipping algorithm. Returns
/// size of Triangle array unless the polygon can't be triangulated.
/// This should only happen if the polygon self-intersects,
/// though it will not _always_ return null for a bad polygon - it is the
/// caller's responsibility to check for self-intersection, and if it
/// doesn't, it should at least check that the return value is non-null
/// before using. You're warned!
///
/// Triangles may be degenerate, especially if you have identical points
/// in the input to the algorithm. Check this before you use them.
///
/// This is totally unoptimized, so for large polygons it should not be part
/// of the simulation loop.
/// </summary>
/// <remarks>
/// Only works on simple polygons.
/// </remarks>
private static List <Vertices> TriangulatePolygon(Vertices vertices, float tolerance)
{
//FPE note: Check is needed as invalid triangles can be returned in recursive calls.
if (vertices.Count < 3)
{
return(new List <Vertices>());
}
List <Vertices> results = new List <Vertices>();
//Recurse and split on pinch points
Vertices pA, pB;
Vertices pin = new Vertices(vertices);
if (ResolvePinchPoint(pin, out pA, out pB, tolerance))
{
List <Vertices> mergeA = TriangulatePolygon(pA, tolerance);
List <Vertices> mergeB = TriangulatePolygon(pB, tolerance);
if (mergeA.Count == -1 || mergeB.Count == -1)
{
throw new Exception("Can't triangulate your polygon.");
}
for (int i = 0; i < mergeA.Count; ++i)
{
results.Add(new Vertices(mergeA[i]));
}
for (int i = 0; i < mergeB.Count; ++i)
{
results.Add(new Vertices(mergeB[i]));
}
return(results);
}
Vertices[] buffer = new Vertices[vertices.Count - 2];
int bufferSize = 0;
float[] xrem = new float[vertices.Count];
float[] yrem = new float[vertices.Count];
for (int i = 0; i < vertices.Count; ++i)
{
xrem[i] = vertices[i].X;
yrem[i] = vertices[i].Y;
}
int vNum = vertices.Count;
while (vNum > 3)
{
// Find an ear
int earIndex = -1;
float earMaxMinCross = -10.0f;
for (int i = 0; i < vNum; ++i)
{
if (IsEar(i, xrem, yrem, vNum))
{
int lower = Remainder(i - 1, vNum);
int upper = Remainder(i + 1, vNum);
Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]);
Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]);
Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]);
d1.Normalize();
d2.Normalize();
d3.Normalize();
float cross12;
MathUtils.Cross(ref d1, ref d2, out cross12);
cross12 = Math.Abs(cross12);
float cross23;
MathUtils.Cross(ref d2, ref d3, out cross23);
cross23 = Math.Abs(cross23);
float cross31;
MathUtils.Cross(ref d3, ref d1, out cross31);
cross31 = Math.Abs(cross31);
//Find the maximum minimum angle
float minCross = Math.Min(cross12, Math.Min(cross23, cross31));
if (minCross > earMaxMinCross)
{
earIndex = i;
earMaxMinCross = minCross;
}
}
}
// If we still haven't found an ear, we're screwed.
// Note: sometimes this is happening because the
// remaining points are collinear. Really these
// should just be thrown out without halting triangulation.
if (earIndex == -1)
{
for (int i = 0; i < bufferSize; i++)
{
results.Add(buffer[i]);
}
return(results);
}
// Clip off the ear:
// - remove the ear tip from the list
--vNum;
float[] newx = new float[vNum];
float[] newy = new float[vNum];
int currDest = 0;
for (int i = 0; i < vNum; ++i)
{
if (currDest == earIndex)
{
++currDest;
}
newx[i] = xrem[currDest];
newy[i] = yrem[currDest];
++currDest;
}
// - add the clipped triangle to the triangle list
int under = (earIndex == 0) ? (vNum) : (earIndex - 1);
int over = (earIndex == vNum) ? 0 : (earIndex + 1);
Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under],
yrem[under]);
buffer[bufferSize] = toAdd;
++bufferSize;
// - replace the old list with the new one
xrem = newx;
yrem = newy;
}
Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]);
buffer[bufferSize] = tooAdd;
++bufferSize;
for (int i = 0; i < bufferSize; i++)
{
results.Add(new Vertices(buffer[i]));
}
return(results);
}
public static void Set(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1)
{
_localPoint = Vector2.Zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
Vector2 a = localPointB2 - localPointB1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
Vector2 a = localPointA2 - localPointA1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
}
public static Vector2 Normalize(Vector2 vectorToNormalize)
{
var normalizedVector = MonoGameVector2.Normalize(vectorToNormalize.MonoGameVector);
return(new Vector2(normalizedVector));
}
public void Normalize()
{
_vector.Normalize();
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = Complex.Divide(input.Point1 - transform.p, ref transform.q);
Vector2 p2 = Complex.Divide(input.Point2 - transform.p, ref transform.q);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 normal = new Vector2(e.Y, -e.X); //TODO: Could possibly cache the normal.
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.MaxFraction < t)
{
return(false);
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.Fraction = t;
if (numerator > 0.0f)
{
output.Normal = -normal;
}
else
{
output.Normal = normal;
}
return(true);
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(BroadPhaseRayCastCallback callback, ref RayCastInput input)
{
Vector2 p1 = input.Point1;
Vector2 p2 = input.Point2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_raycastStack.Clear();
_raycastStack.Push(_root);
while (_raycastStack.Count > 0)
{
int nodeId = _raycastStack.Pop();
if (nodeId == NullNode)
{
continue;
}
//TreeNode<T>* node = &_nodes[nodeId];
if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = _nodes[nodeId].AABB.Center;
Vector2 h = _nodes[nodeId].AABB.Extents;
float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (_nodes[nodeId].IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(ref subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
Vector2 t = p1 + maxFraction * (p2 - p1);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
}
else
{
_raycastStack.Push(_nodes[nodeId].Child1);
_raycastStack.Push(_nodes[nodeId].Child2);
}
}
}
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = new SimplexCache();
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKCalls;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, ref input.ProxyA, ref input.TransformA, ref input.ProxyB, ref input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3<int> saveA = new FixedArray3<int>();
FixedArray3<int> saveB = new FixedArray3<int>();
//float distanceSqr1 = Settings.MaxFloat;
// Main iteration loop.
int iter = 0;
while (iter < Settings.MaxGJKIterations)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = simplex.V[i].IndexA;
saveB[i] = simplex.V[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//float distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.Epsilon * Settings.Epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = simplex.V[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(-Complex.Divide(ref d, ref input.TransformA.q));
vertex.WA = Transform.Multiply(input.ProxyA.Vertices[vertex.IndexA], ref input.TransformA);
vertex.IndexB = input.ProxyB.GetSupport( Complex.Divide(ref d, ref input.TransformB.q));
vertex.WB = Transform.Multiply(input.ProxyB.Vertices[vertex.IndexB], ref input.TransformB);
vertex.W = vertex.WB - vertex.WA;
simplex.V[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++GJKIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
GJKMaxIters = Math.Max(GJKMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length();
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float rB = input.ProxyB.Radius;
if (output.Distance > rA + rB && output.Distance > Settings.Epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
public override void ProcessInput(string Pressed, Microsoft.Xna.Framework.Vector2 Mouse)
{
// TODO: Clean up formation algorithm
// TODO; Redesign input, it's ugly.
#region Formation
if (m_Selected != null && Pressed == "Mouse_Right" && m_Camera != null)
{
Microsoft.Xna.Framework.Vector2 offset = new Microsoft.Xna.Framework.Vector2();
Microsoft.Xna.Framework.Vector2 formationOrientation = new Microsoft.Xna.Framework.Vector2();
Microsoft.Xna.Framework.Vector2 flippedOrientation = new Microsoft.Xna.Framework.Vector2();
Microsoft.Xna.Framework.Vector2 clickedLocation = (m_Camera.Position + Mouse);
float averageX = 0;
float averageY = 0;
int numberUnits = m_Selected.Count;
int rows = 4;
int spacing = 100;
float counterX = -.5f * (Math.Min(numberUnits, rows) - 1);
if (counterX < rows * -.5f)
{
counterX = rows * -.5f;
}
float counterY = 0;
foreach (Tantric.Logic.Unit unit in m_Selected)
{
averageX += unit.Position.X;
averageY += unit.Position.Y;
}
averageX /= numberUnits;
averageY /= numberUnits;
formationOrientation = clickedLocation - new Microsoft.Xna.Framework.Vector2(averageX, averageY);
formationOrientation.Normalize();
flippedOrientation.X = -formationOrientation.Y;
flippedOrientation.Y = formationOrientation.X;
foreach (Tantric.Logic.Unit unit in m_Selected)
{
offset = (flippedOrientation * counterX) * spacing;
offset += -formationOrientation * counterY * spacing;
unit.AddGoal(new Tantric.Logic.UnitGoal("MoveTo", true, -1, clickedLocation + offset));
counterX += 1;
if (counterX > (rows - 1) * .5)
{
numberUnits -= rows;
counterX = -.5f * (Math.Min(numberUnits, rows) - 1);
if (counterX < rows * -.5f)
{
counterX = rows * -.5f;
}
counterY += 1;
}
}
}
#endregion
if (Pressed == "Mouse_Left" && m_SelectionState == SelectingState.Idle)
{
m_SelectionBox.X = Mouse.X;
m_SelectionBox.Y = Mouse.Y;
m_SelectionState = SelectingState.Started;
m_Selected.Clear();
}
else if (Pressed == "Mouse_Left" && m_SelectionState == SelectingState.Started)
{
m_SelectionBox.Z = Mouse.X;
m_SelectionBox.W = Mouse.Y;
}
else
{
m_SelectionState = SelectingState.Finished;
}
if (m_Camera != null && Pressed == "Camera_Left")
{
m_Camera.Translate(new Microsoft.Xna.Framework.Vector2(-1, 0) * World.Objects.Human.HumanStatistics.GetStatistic("Camera", "Speed"));
}
if (m_Camera != null && Pressed == "Camera_Right")
{
m_Camera.Translate(new Microsoft.Xna.Framework.Vector2(1, 0) * World.Objects.Human.HumanStatistics.GetStatistic("Camera", "Speed"));
}
if (m_Camera != null && Pressed == "Camera_Up")
{
m_Camera.Translate(new Microsoft.Xna.Framework.Vector2(0, -1) * World.Objects.Human.HumanStatistics.GetStatistic("Camera", "Speed"));
}
if (m_Camera != null && Pressed == "Camera_Down")
{
m_Camera.Translate(new Microsoft.Xna.Framework.Vector2(0, 1) * World.Objects.Human.HumanStatistics.GetStatistic("Camera", "Speed"));
}
}