public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, GGame.Math.Fix64 amount)
{
return(new Vector3(
MathHelper.SmoothStep(value1.X, value2.X, amount),
MathHelper.SmoothStep(value1.Y, value2.Y, amount),
MathHelper.SmoothStep(value1.Z, value2.Z, amount)));
}
public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result)
{
GGame.Math.Fix64 dot = Dot(vector, normal);
result.X = vector.X - ((2f * dot) * normal.X);
result.Y = vector.Y - ((2f * dot) * normal.Y);
result.Z = vector.Z - ((2f * dot) * normal.Z);
}
public static Vector3 Multiply(Vector3 value1, GGame.Math.Fix64 scaleFactor)
{
value1.X *= scaleFactor;
value1.Y *= scaleFactor;
value1.Z *= scaleFactor;
return(value1);
}
public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2,
GGame.Math.Fix64 amount, out Vector3 result)
{
result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
}
public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, GGame.Math.Fix64 amount, out Vector3 result)
{
result = new Vector3(
MathHelper.SmoothStep(value1.X, value2.X, amount),
MathHelper.SmoothStep(value1.Y, value2.Y, amount),
MathHelper.SmoothStep(value1.Z, value2.Z, amount));
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.Velocities[_indexA].V;
GGame.Math.Fix64 wA = data.Velocities[_indexA].W;
// Cdot = v + cross(w, r)
Vector2 Cdot = vA + MathUtils.Cross(wA, _rA);
Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse));
Vector2 oldImpulse = _impulse;
_impulse += impulse;
GGame.Math.Fix64 maxImpulse = data.Step.dt * MaxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
vA += _invMassA * impulse;
wA += _invIA * MathUtils.Cross(_rA, impulse);
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
}
public static void ComputeCircleAABB(ref Vector2 pos, GGame.Math.Fix64 radius, ref Transform transform, out AABB aabb)
{
Vector2 p = transform.p + MathUtils.Mul(transform.q, pos);
aabb.LowerBound = new Vector2(p.X - radius, p.Y - radius);
aabb.UpperBound = new Vector2(p.X + radius, p.Y + radius);
}
public Point(GGame.Math.Fix64 x, GGame.Math.Fix64 y)
{
X = x;
Y = y;
Next = null;
Prev = null;
}
public static Fixture AttachLineArc(GGame.Math.Fix64 radians, int sides, GGame.Math.Fix64 radius, bool closed, Body body)
{
Vertices arc = PolygonUtils.CreateArc(radians, sides, radius);
arc.Rotate((MathHelper.Pi - radians) / 2);
return(closed ? AttachLoopShape(arc, body) : AttachChainShape(arc, body));
}
private void ComputeMass()
{
//Velcro: We calculate area for later consumption
GGame.Math.Fix64 area = Settings.Pi * _2radius;
MassData.Area = area;
MassData.Mass = Density * area;
}
internal Vector2 GetSearchDirection()
{
switch (Count)
{
case 1:
return(-V[0].W);
case 2:
{
Vector2 e12 = V[1].W - V[0].W;
GGame.Math.Fix64 sgn = MathUtils.Cross(e12, -V[0].W);
if (sgn > 0.0f)
{
// Origin is left of e12.
return(MathUtils.Cross(1.0f, e12));
}
else
{
// Origin is right of e12.
return(MathUtils.Cross(e12, 1.0f));
}
}
default:
Debug.Assert(false);
return(Vector2.Zero);
}
}
/// <summary>
/// Creates a new terrain
/// </summary>
/// <param name="world">The World</param>
/// <param name="position">The position (center) of the terrain.</param>
/// <param name="width">The width of the terrain.</param>
/// <param name="height">The height of the terrain.</param>
public Terrain(World world, Vector2 position, GGame.Math.Fix64 width, GGame.Math.Fix64 height)
{
World = world;
Width = width;
Height = height;
Center = position;
}
/// <summary>
/// Compute the collision manifold between two circles.
/// </summary>
public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB)
{
manifold.PointCount = 0;
Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position);
Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position);
Vector2 d = pB - pA;
GGame.Math.Fix64 distSqr = Vector2.Dot(d, d);
GGame.Math.Fix64 rA = circleA.Radius, rB = circleB.Radius;
GGame.Math.Fix64 radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circleA.Position;
manifold.LocalNormal = Vector2.Zero;
manifold.PointCount = 1;
ManifoldPoint p0 = manifold.Points[0];
p0.LocalPoint = circleB.Position;
p0.Id.Key = 0;
manifold.Points[0] = p0;
}
private GGame.Math.Fix64 GetCurvePosition(GGame.Math.Fix64 position)
{
//only for position in curve
CurveKey prev = keys[0];
CurveKey next;
for (int i = 1; i < keys.Count; i++)
{
next = Keys[i];
if (next.Position >= position)
{
if (prev.Continuity == CurveContinuity.Step)
{
if (position >= 1f)
{
return(next.Value);
}
return(prev.Value);
}
GGame.Math.Fix64 t = (position - prev.Position) / (next.Position - prev.Position); //to have t in [0,1]
GGame.Math.Fix64 ts = t * t;
GGame.Math.Fix64 tss = ts * t;
//After a lot of search on internet I have found all about spline function
// and bezier (phi'sss ancien) but finaly use hermite curve
//http://en.wikipedia.org/wiki/Cubic_Hermite_spline
//P(t) = (2*t^3 - 3t^2 + 1)*P0 + (t^3 - 2t^2 + t)m0 + (-2t^3 + 3t^2)P1 + (t^3-t^2)m1
//with P0.value = prev.value , m0 = prev.tangentOut, P1= next.value, m1 = next.TangentIn
return((2 * tss - 3 * ts + 1f) * prev.Value + (tss - 2 * ts + t) * prev.TangentOut + (3 * ts - 2 * tss) * next.Value +
(tss - ts) * next.TangentIn);
}
prev = next;
}
return(0f);
}
/// <summary>
/// Resets the dynamics of this body.
/// Sets torque, force and linear/angular velocity to 0
/// </summary>
public void ResetDynamics()
{
_torque = 0;
_angularVelocity = 0;
_force = Vector2.Zero;
_linearVelocity = Vector2.Zero;
}
private AdvancingFrontNode LocateNode(GGame.Math.Fix64 x)
{
AdvancingFrontNode node = FindSearchNode(x);
if (x < node.Value)
{
while ((node = node.Prev) != null)
{
if (x >= node.Value)
{
Search = node;
return(node);
}
}
}
else
{
while ((node = node.Next) != null)
{
if (x < node.Value)
{
Search = node.Prev;
return(node.Prev);
}
}
}
return(null);
}
/// <summary>
/// Creates a new terrain.
/// </summary>
/// <param name="world">The World</param>
/// <param name="area">The area of the terrain.</param>
public Terrain(World world, AABB area)
{
World = world;
Width = area.Width;
Height = area.Height;
Center = area.Center;
}
public static bool TestPointCircle(ref Vector2 pos, GGame.Math.Fix64 radius, ref Vector2 point, ref Transform transform)
{
Vector2 center = transform.p + MathUtils.Mul(transform.q, pos);
Vector2 d = point - center;
return(Vector2.Dot(d, d) <= radius * radius);
}
public TrapezoidalMap()
{
Map = new HashSet <Trapezoid>();
_margin = 50.0f;
_bCross = null;
_cross = null;
}
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
internal void Solve2()
{
Vector2 w1 = V[0].W;
Vector2 w2 = V[1].W;
Vector2 e12 = w2 - w1;
// w1 region
GGame.Math.Fix64 d12_2 = -Vector2.Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
V.Value0.A = 1.0f;
Count = 1;
return;
}
// w2 region
GGame.Math.Fix64 d12_1 = Vector2.Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
V.Value1.A = 1.0f;
Count = 1;
V.Value0 = V.Value1;
return;
}
// Must be in e12 region.
GGame.Math.Fix64 inv_d12 = 1.0f / (d12_1 + d12_2);
V.Value0.A = d12_1 * inv_d12;
V.Value1.A = d12_2 * inv_d12;
Count = 2;
}
/// <summary>
/// Initialize the terrain for use.
/// </summary>
public void Initialize()
{
// find top left of terrain in world space
_topLeft = new Vector2(Center.X - (Width * 0.5f), Center.Y - (-Height * 0.5f));
// convert the terrains size to a point cloud size
_localWidth = Width * PointsPerUnit;
_localHeight = Height * PointsPerUnit;
_terrainMap = new sbyte[(int)_localWidth + 1, (int)_localHeight + 1];
for (int x = 0; x < _localWidth; x++)
{
for (int y = 0; y < _localHeight; y++)
{
_terrainMap[x, y] = 1;
}
}
_xnum = (int)(_localWidth / CellSize);
_ynum = (int)(_localHeight / CellSize);
_bodyMap = new List <Body> [_xnum, _ynum];
// make sure to mark the dirty area to an infinitely small box
_dirtyArea = new AABB(new Vector2(GGame.Math.Fix64.MaxValue, GGame.Math.Fix64.MaxValue), new Vector2(GGame.Math.Fix64.MinValue, GGame.Math.Fix64.MinValue));
}
/// <summary>
/// Decompose the polygon into several smaller non-concave polygons.
/// </summary>
/// <param name="vertices">The polygon to decompose.</param>
/// <param name="sheer">The sheer to use if you get bad results, try using a higher value. 默认值 0.001</param>
/// <returns>A list of trapezoids</returns>
public static List <Vertices> ConvexPartitionTrapezoid(Vertices vertices, GGame.Math.Fix64 sheer)
{
List <Point> compatList = new List <Point>(vertices.Count);
foreach (Vector2 vertex in vertices)
{
compatList.Add(new Point(vertex.X, vertex.Y));
}
Triangulator t = new Triangulator(compatList, sheer);
List <Vertices> list = new List <Vertices>();
foreach (Trapezoid trapezoid in t.Trapezoids)
{
Vertices verts = new Vertices();
List <Point> points = trapezoid.GetVertices();
foreach (Point point in points)
{
verts.Add(new Vector2(point.X, point.Y));
}
list.Add(verts);
}
return(list);
}
/// <summary>
/// Decompose the polygon into several smaller non-concave polygons.
/// </summary>
/// <param name="vertices">The polygon to decompose.</param>
/// <param name="sheer">The sheer to use if you get bad results, try using a higher value 默认值 0.001.</param>
/// <returns>A list of triangles</returns>
public static List <Vertices> ConvexPartition(Vertices vertices, GGame.Math.Fix64 sheer)
{
Debug.Assert(vertices.Count > 3);
List <Point> compatList = new List <Point>(vertices.Count);
foreach (Vector2 vertex in vertices)
{
compatList.Add(new Point(vertex.X, vertex.Y));
}
Triangulator t = new Triangulator(compatList, sheer);
List <Vertices> list = new List <Vertices>();
foreach (List <Point> triangle in t.Triangles)
{
Vertices outTriangles = new Vertices(triangle.Count);
foreach (Point outTriangle in triangle)
{
outTriangles.Add(new Vector2(outTriangle.X, outTriangle.Y));
}
list.Add(outTriangles);
}
return(list);
}
public static void Divide(ref Vector3 value1, GGame.Math.Fix64 divisor, out Vector3 result)
{
GGame.Math.Fix64 factor = 1 / divisor;
result.X = value1.X * factor;
result.Y = value1.Y * factor;
result.Z = value1.Z * factor;
}
/// <summary>
/// Set the position of the body's origin and rotation.
/// This breaks any contacts and wakes the other bodies.
/// Manipulating a body's transform may cause non-physical behavior.
/// </summary>
/// <param name="position">The world position of the body's local origin.</param>
/// <param name="rotation">The world rotation in radians.</param>
public void SetTransform(ref Vector2 position, GGame.Math.Fix64 rotation)
{
SetTransformIgnoreContacts(ref position, rotation);
//Velcro: We check for new contacts after a body has been moved.
_world.ContactManager.FindNewContacts();
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.Velocities[_indexA].V;
GGame.Math.Fix64 wA = data.Velocities[_indexA].W;
Vector2 vB = data.Velocities[_indexB].V;
GGame.Math.Fix64 wB = data.Velocities[_indexB].W;
GGame.Math.Fix64 mA = _invMassA, mB = _invMassB;
GGame.Math.Fix64 iA = _invIA, iB = _invIB;
if (FrequencyHz > 0.0f)
{
GGame.Math.Fix64 Cdot2 = wB - wA;
GGame.Math.Fix64 impulse2 = -_mass.ez.Z * (Cdot2 + _bias + _gamma * _impulse.Z);
_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1);
_impulse.X += impulse1.X;
_impulse.Y += impulse1.Y;
Vector2 P = impulse1;
vA -= mA * P;
wA -= iA * MathUtils.Cross(_rA, P);
vB += mB * P;
wB += iB * MathUtils.Cross(_rB, P);
}
else
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
GGame.Math.Fix64 Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = -MathUtils.Mul(_mass, Cdot);
_impulse += impulse;
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.Velocities[_indexA].V;
GGame.Math.Fix64 wA = data.Velocities[_indexA].W;
Vector2 vB = data.Velocities[_indexB].V;
GGame.Math.Fix64 wB = data.Velocities[_indexB].W;
GGame.Math.Fix64 mA = _invMassA, mB = _invMassB;
GGame.Math.Fix64 iA = _invIA, iB = _invIB;
GGame.Math.Fix64 h = data.Step.dt;
GGame.Math.Fix64 inv_h = data.Step.inv_dt;
// Solve angular friction
{
GGame.Math.Fix64 Cdot = wB - wA + inv_h * CorrectionFactor * _angularError;
GGame.Math.Fix64 impulse = -_angularMass * Cdot;
GGame.Math.Fix64 oldImpulse = _angularImpulse;
GGame.Math.Fix64 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, _rB) - vA - MathUtils.Cross(wA, _rA) + inv_h * CorrectionFactor * _linearError;
Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
GGame.Math.Fix64 maxImpulse = h * _maxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
/// <summary>
/// Advance the sweep forward, yielding a new initial state.
/// </summary>
/// <param name="alpha">new initial time</param>
public void Advance(GGame.Math.Fix64 alpha)
{
Debug.Assert(Alpha0 < 1.0f);
GGame.Math.Fix64 beta = (alpha - Alpha0) / (1.0f - Alpha0);
C0 += beta * (C - C0);
A0 += beta * (A - A0);
Alpha0 = alpha;
}
/// <summary>
/// Moves the given body along the defined path.
/// </summary>
/// <param name="path">The path.</param>
/// <param name="body">The body.</param>
/// <param name="time">The time.</param>
/// <param name="strength">The strength.</param>
/// <param name="timeStep">The time step.</param>
public static void MoveBodyOnPath(Path path, Body body, GGame.Math.Fix64 time, GGame.Math.Fix64 strength, GGame.Math.Fix64 timeStep)
{
Vector2 destination = path.GetPosition(time);
Vector2 positionDelta = body.Position - destination;
Vector2 velocity = (positionDelta / timeStep) * strength;
body.LinearVelocity = -velocity;
}
public CurveKey(GGame.Math.Fix64 position, GGame.Math.Fix64 value, GGame.Math.Fix64 tangentIn, GGame.Math.Fix64 tangentOut, CurveContinuity continuity)
{
this.position = position;
this.value = value;
this.tangentIn = tangentIn;
this.tangentOut = tangentOut;
this.continuity = continuity;
}
