internal override void InitVelocityConstraints(ref SolverData data)
{
int indexA = BodyA.IslandIndex;
int indexB = BodyB.IslandIndex;
GGame.Math.Fix64 aW = data.Positions[indexA].A;
GGame.Math.Fix64 bW = data.Positions[indexB].A;
_jointError = (bW - aW - TargetAngle);
_bias = -BiasFactor * data.Step.inv_dt * _jointError;
_massFactor = (1 - Softness) / (BodyA._invI + BodyB._invI);
}
/// <summary>
/// Set the joint limits, usually in meters.
/// </summary>
/// <param name="lower">The lower limit</param>
/// <param name="upper">The upper limit</param>
public void SetLimits(GGame.Math.Fix64 lower, GGame.Math.Fix64 upper)
{
if (upper == _upperTranslation && lower == _lowerTranslation)
{
return;
}
WakeBodies();
_upperTranslation = upper;
_lowerTranslation = lower;
_impulse.Z = 0.0f;
}
private GGame.Math.Fix64 GetPercent(GGame.Math.Fix64 distance, GGame.Math.Fix64 radius)
{
//(1-(distance/radius))^power-1
GGame.Math.Fix64 percent = (GGame.Math.Fix64)Math.Pow((float)(1 - ((distance - radius) / radius)), (float)Power) - 1;
if (GGame.Math.Fix64.IsNaN(percent))
{
return(0f);
}
return(MathHelper.Clamp(percent, 0f, 1f));
}
/// <summary>
/// Set the joint limits, usually in meters.
/// </summary>
/// <param name="lower">The lower limit</param>
/// <param name="upper">The upper limit</param>
public void SetLimits(GGame.Math.Fix64 lower, GGame.Math.Fix64 upper)
{
if (lower == _lowerAngle && upper == _upperAngle)
{
return;
}
WakeBodies();
_upperAngle = upper;
_lowerAngle = lower;
_impulse.Z = 0.0f;
}
/// <summary>
/// This method detects if two line segments (or lines) intersect,
/// and, if so, the point of intersection. Use the <paramref name="firstIsSegment" /> and
/// <paramref name="secondIsSegment" /> parameters to set whether the intersection point
/// must be on the first and second line segments. Setting these
/// both to true means you are doing a line-segment to line-segment
/// intersection. Setting one of them to true means you are doing a
/// line to line-segment intersection test, and so on.
/// Note: If two line segments are coincident, then
/// no intersection is detected (there are actually
/// infinite intersection points).
/// Author: Jeremy Bell
/// </summary>
/// <param name="point1">The first point of the first line segment.</param>
/// <param name="point2">The second point of the first line segment.</param>
/// <param name="point3">The first point of the second line segment.</param>
/// <param name="point4">The second point of the second line segment.</param>
/// <param name="point">
/// This is set to the intersection
/// point if an intersection is detected.
/// </param>
/// <param name="firstIsSegment">
/// Set this to true to require that the
/// intersection point be on the first line segment.
/// </param>
/// <param name="secondIsSegment">
/// Set this to true to require that the
/// intersection point be on the second line segment.
/// </param>
/// <returns>True if an intersection is detected, false otherwise.</returns>
public static bool LineIntersect(ref Vector2 point1, ref Vector2 point2, ref Vector2 point3, ref Vector2 point4, bool firstIsSegment, bool secondIsSegment, out Vector2 point)
{
point = new Vector2();
// these are reused later.
// each lettered sub-calculation is used twice, except
// for b and d, which are used 3 times
GGame.Math.Fix64 a = point4.Y - point3.Y;
GGame.Math.Fix64 b = point2.X - point1.X;
GGame.Math.Fix64 c = point4.X - point3.X;
GGame.Math.Fix64 d = point2.Y - point1.Y;
// denominator to solution of linear system
GGame.Math.Fix64 denom = (a * b) - (c * d);
// if denominator is 0, then lines are parallel
if (!(denom >= -Settings.Epsilon && denom <= Settings.Epsilon))
{
GGame.Math.Fix64 e = point1.Y - point3.Y;
GGame.Math.Fix64 f = point1.X - point3.X;
GGame.Math.Fix64 oneOverDenom = 1.0f / denom;
// numerator of first equation
GGame.Math.Fix64 ua = (c * e) - (a * f);
ua *= oneOverDenom;
// check if intersection point of the two lines is on line segment 1
if (!firstIsSegment || ua >= 0.0f && ua <= 1.0f)
{
// numerator of second equation
GGame.Math.Fix64 ub = (b * e) - (d * f);
ub *= oneOverDenom;
// check if intersection point of the two lines is on line segment 2
// means the line segments intersect, since we know it is on
// segment 1 as well.
if (!secondIsSegment || ub >= 0.0f && ub <= 1.0f)
{
// check if they are coincident (no collision in this case)
if (ua != 0f || ub != 0f)
{
//There is an intersection
point.X = point1.X + ua * b;
point.Y = point1.Y + ua * d;
return(true);
}
}
}
}
return(false);
}
/// <summary>
/// Returns a set of points defining the
/// curve with the specifed number of divisions
/// between each control point.
/// </summary>
/// <param name="divisions">Number of divisions between each control point.</param>
/// <returns></returns>
public Vertices GetVertices(int divisions)
{
Vertices verts = new Vertices();
GGame.Math.Fix64 timeStep = 1f / divisions;
for (GGame.Math.Fix64 i = 0; i < 1f; i += timeStep)
{
verts.Add(GetPosition(i));
}
return(verts);
}
public static Body CreateRoundedRectangle(World world, GGame.Math.Fix64 width, GGame.Math.Fix64 height, GGame.Math.Fix64 xRadius, GGame.Math.Fix64 yRadius, int segments, GGame.Math.Fix64 density, Vector2 position = new Vector2(), GGame.Math.Fix64 rotation = new Fix64(), BodyType bodyType = BodyType.Static, object userData = null)
{
Vertices verts = PolygonUtils.CreateRoundedRectangle(width, height, xRadius, yRadius, segments);
//There are too many vertices in the capsule. We decompose it.
if (verts.Count >= Settings.MaxPolygonVertices)
{
List <Vertices> vertList = Triangulate.ConvexPartition(verts, TriangulationAlgorithm.Earclip, true, 0.001f);
return(CreateCompoundPolygon(world, vertList, density, position, rotation, bodyType, userData));
}
return(CreatePolygon(world, verts, density, position, rotation, bodyType, userData));
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.Positions[_indexA].C;
GGame.Math.Fix64 aA = data.Positions[_indexA].A;
Vector2 cB = data.Positions[_indexB].C;
GGame.Math.Fix64 aB = data.Positions[_indexB].A;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
Vector2 ay = MathUtils.Mul(qA, _localYAxis);
GGame.Math.Fix64 sAy = MathUtils.Cross(d + rA, ay);
GGame.Math.Fix64 sBy = MathUtils.Cross(rB, ay);
GGame.Math.Fix64 C = Vector2.Dot(d, ay);
GGame.Math.Fix64 k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy;
GGame.Math.Fix64 impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
Vector2 P = impulse * ay;
GGame.Math.Fix64 LA = impulse * sAy;
GGame.Math.Fix64 LB = impulse * sBy;
cA -= _invMassA * P;
aA -= _invIA * LA;
cB += _invMassB * P;
aB += _invIB * LB;
data.Positions[_indexA].C = cA;
data.Positions[_indexA].A = aA;
data.Positions[_indexB].C = cB;
data.Positions[_indexB].A = aB;
return(Fix64.Abs(C) <= Settings.LinearSlop);
}
public static GGame.Math.Fix64 WrapAngle(GGame.Math.Fix64 angle)
{
angle = (GGame.Math.Fix64)Math.IEEERemainder((float)angle, 6.2831854820251465); //2xPi precission is GGame.Math.Fix64
if (angle <= -3.141593f)
{
angle += 6.283185f;
return(angle);
}
if (angle > 3.141593f)
{
angle -= 6.283185f;
}
return(angle);
}
// From Eric Jordan's convex decomposition library
/// <summary>
/// Check if the lines a0->a1 and b0->b1 cross.
/// If they do, intersectionPoint will be filled
/// with the point of crossing.
/// Grazing lines should not return true.
/// </summary>
public static bool LineIntersect2(ref Vector2 a0, ref Vector2 a1, ref Vector2 b0, ref Vector2 b1, out Vector2 intersectionPoint)
{
intersectionPoint = Vector2.Zero;
if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1)
{
return(false);
}
GGame.Math.Fix64 x1 = a0.X;
GGame.Math.Fix64 y1 = a0.Y;
GGame.Math.Fix64 x2 = a1.X;
GGame.Math.Fix64 y2 = a1.Y;
GGame.Math.Fix64 x3 = b0.X;
GGame.Math.Fix64 y3 = b0.Y;
GGame.Math.Fix64 x4 = b1.X;
GGame.Math.Fix64 y4 = b1.Y;
//AABB early exit
if (Math.Max((float)x1, (float)x2) < Math.Min((float)x3, (float)x4) || Math.Max((float)x3, (float)x4) < Math.Min((float)x1, (float)x2))
{
return(false);
}
if (Math.Max((float)y1, (float)y2) < Math.Min((float)y3, (float)y4) || Math.Max((float)y3, (float)y4) < Math.Min((float)y1, (float)y2))
{
return(false);
}
GGame.Math.Fix64 ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
GGame.Math.Fix64 ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
GGame.Math.Fix64 denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
if (Fix64.Abs(denom) < Settings.Epsilon)
{
//Lines are too close to parallel to call
return(false);
}
ua /= denom;
ub /= denom;
if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1))
{
intersectionPoint.X = (x1 + ua * (x2 - x1));
intersectionPoint.Y = (y1 + ua * (y2 - y1));
return(true);
}
return(false);
}
public BreakableBody(World world, IEnumerable <Vertices> vertices, GGame.Math.Fix64 density, Vector2 position = new Vector2(), GGame.Math.Fix64 rotation = new GGame.Math.Fix64())
{
_world = world;
_world.ContactManager.PostSolve += PostSolve;
Parts = new List <Fixture>(8);
MainBody = BodyFactory.CreateBody(_world, position, rotation, BodyType.Dynamic);
Strength = 500.0f;
foreach (Vertices part in vertices)
{
PolygonShape polygonShape = new PolygonShape(part, density);
Fixture fixture = MainBody.CreateFixture(polygonShape);
Parts.Add(fixture);
}
}
public static Body CreateGear(World world, GGame.Math.Fix64 radius, int numberOfTeeth, GGame.Math.Fix64 tipPercentage, GGame.Math.Fix64 toothHeight, GGame.Math.Fix64 density, Vector2 position = new Vector2(), GGame.Math.Fix64 rotation = new Fix64(), BodyType bodyType = BodyType.Static, object userData = null)
{
Vertices gearPolygon = PolygonUtils.CreateGear(radius, numberOfTeeth, tipPercentage, toothHeight);
//Gears can in some cases be convex
if (!gearPolygon.IsConvex())
{
//Decompose the gear:
List <Vertices> list = Triangulate.ConvexPartition(gearPolygon, TriangulationAlgorithm.Earclip, true, 0.001f);
return(CreateCompoundPolygon(world, list, density, position, rotation, bodyType, userData));
}
return(CreatePolygon(world, gearPolygon, density, position, rotation, bodyType, userData));
}
/// <summary>
/// Creates a ellipse with the specified width, height and number of edges.
/// </summary>
/// <param name="xRadius">Width of the ellipse.</param>
/// <param name="yRadius">Height of the ellipse.</param>
/// <param name="numberOfEdges">The number of edges. The more edges, the more it resembles an ellipse</param>
/// <returns></returns>
public static Vertices CreateEllipse(GGame.Math.Fix64 xRadius, GGame.Math.Fix64 yRadius, int numberOfEdges)
{
Vertices vertices = new Vertices();
GGame.Math.Fix64 stepSize = MathHelper.TwoPi / numberOfEdges;
vertices.Add(new Vector2(xRadius, 0));
for (int i = numberOfEdges - 1; i > 0; --i)
{
vertices.Add(new Vector2(xRadius * GGame.Math.Fix64.Cos(stepSize * i),
-yRadius * GGame.Math.Fix64.Sin(stepSize * i)));
}
return(vertices);
}
/// <summary>
/// Inherited from Controller
/// Depending on the TimingMode perform timing logic and call ApplyForce()
/// </summary>
/// <param name="dt"></param>
public override void Update(GGame.Math.Fix64 dt)
{
switch (TimingMode)
{
case TimingModes.Switched:
{
if (Enabled)
{
ApplyForce(dt, Strength);
}
break;
}
case TimingModes.Triggered:
{
if (Enabled && Triggered)
{
if (ImpulseTime < ImpulseLength)
{
ApplyForce(dt, Strength);
ImpulseTime += dt;
}
else
{
Triggered = false;
}
}
break;
}
case TimingModes.Curve:
{
if (Enabled && Triggered)
{
if (ImpulseTime < ImpulseLength)
{
ApplyForce(dt, Strength * StrengthCurve.Evaluate(ImpulseTime));
ImpulseTime += dt;
}
else
{
Triggered = false;
}
}
break;
}
}
}
/// <summary>
/// Return the angle between two vectors on a plane
/// The angle is from vector 1 to vector 2, positive anticlockwise
/// The result is between -pi -> pi
/// </summary>
public static GGame.Math.Fix64 VectorAngle(ref Vector2 p1, ref Vector2 p2)
{
GGame.Math.Fix64 theta1 = Fix64.Atan2(p1.Y, p1.X);
GGame.Math.Fix64 theta2 = Fix64.Atan2(p2.Y, p2.X);
GGame.Math.Fix64 dtheta = theta2 - theta1;
while (dtheta > (Fix64)Math.PI)
{
dtheta -= (2 * Math.PI);
}
while (dtheta < -(Fix64)Math.PI)
{
dtheta += (2 * Math.PI);
}
return(dtheta);
}
/// <summary>
/// Initializes a new instance of the <see cref="VelocityLimitController" /> class.
/// Pass in 0 or GGame.Math.Fix64.MaxValue to disable the limit.
/// maxAngularVelocity = 0 will disable the angular velocity limit.
/// </summary>
/// <param name="maxLinearVelocity">The max linear velocity.</param>
/// <param name="maxAngularVelocity">The max angular velocity.</param>
public VelocityLimitController(GGame.Math.Fix64 maxLinearVelocity, GGame.Math.Fix64 maxAngularVelocity)
: base(ControllerType.VelocityLimitController)
{
if (maxLinearVelocity == 0 || maxLinearVelocity == GGame.Math.Fix64.MaxValue)
{
LimitLinearVelocity = false;
}
if (maxAngularVelocity == 0 || maxAngularVelocity == GGame.Math.Fix64.MaxValue)
{
LimitAngularVelocity = false;
}
MaxLinearVelocity = maxLinearVelocity;
MaxAngularVelocity = maxAngularVelocity;
}
public static void Invert(ref Matrix matrix, out Matrix result)
{
//
// Use Laplace expansion theorem to calculate the inverse of a 4x4 matrix
//
// 1. Calculate the 2x2 determinants needed and the 4x4 determinant based on the 2x2 determinants
// 2. Create the adjugate matrix, which satisfies: A * adj(A) = det(A) * I
// 3. Divide adjugate matrix with the determinant to find the inverse
GGame.Math.Fix64 det1 = matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21;
GGame.Math.Fix64 det2 = matrix.M11 * matrix.M23 - matrix.M13 * matrix.M21;
GGame.Math.Fix64 det3 = matrix.M11 * matrix.M24 - matrix.M14 * matrix.M21;
GGame.Math.Fix64 det4 = matrix.M12 * matrix.M23 - matrix.M13 * matrix.M22;
GGame.Math.Fix64 det5 = matrix.M12 * matrix.M24 - matrix.M14 * matrix.M22;
GGame.Math.Fix64 det6 = matrix.M13 * matrix.M24 - matrix.M14 * matrix.M23;
GGame.Math.Fix64 det7 = matrix.M31 * matrix.M42 - matrix.M32 * matrix.M41;
GGame.Math.Fix64 det8 = matrix.M31 * matrix.M43 - matrix.M33 * matrix.M41;
GGame.Math.Fix64 det9 = matrix.M31 * matrix.M44 - matrix.M34 * matrix.M41;
GGame.Math.Fix64 det10 = matrix.M32 * matrix.M43 - matrix.M33 * matrix.M42;
GGame.Math.Fix64 det11 = matrix.M32 * matrix.M44 - matrix.M34 * matrix.M42;
GGame.Math.Fix64 det12 = matrix.M33 * matrix.M44 - matrix.M34 * matrix.M43;
GGame.Math.Fix64 detMatrix = (GGame.Math.Fix64)(det1 * det12 - det2 * det11 + det3 * det10 + det4 * det9 - det5 * det8 + det6 * det7);
GGame.Math.Fix64 invDetMatrix = 1f / detMatrix;
Matrix ret; // Allow for matrix and result to point to the same structure
ret.M11 = (matrix.M22 * det12 - matrix.M23 * det11 + matrix.M24 * det10) * invDetMatrix;
ret.M12 = (-matrix.M12 * det12 + matrix.M13 * det11 - matrix.M14 * det10) * invDetMatrix;
ret.M13 = (matrix.M42 * det6 - matrix.M43 * det5 + matrix.M44 * det4) * invDetMatrix;
ret.M14 = (-matrix.M32 * det6 + matrix.M33 * det5 - matrix.M34 * det4) * invDetMatrix;
ret.M21 = (-matrix.M21 * det12 + matrix.M23 * det9 - matrix.M24 * det8) * invDetMatrix;
ret.M22 = (matrix.M11 * det12 - matrix.M13 * det9 + matrix.M14 * det8) * invDetMatrix;
ret.M23 = (-matrix.M41 * det6 + matrix.M43 * det3 - matrix.M44 * det2) * invDetMatrix;
ret.M24 = (matrix.M31 * det6 - matrix.M33 * det3 + matrix.M34 * det2) * invDetMatrix;
ret.M31 = (matrix.M21 * det11 - matrix.M22 * det9 + matrix.M24 * det7) * invDetMatrix;
ret.M32 = (-matrix.M11 * det11 + matrix.M12 * det9 - matrix.M14 * det7) * invDetMatrix;
ret.M33 = (matrix.M41 * det5 - matrix.M42 * det3 + matrix.M44 * det1) * invDetMatrix;
ret.M34 = (-matrix.M31 * det5 + matrix.M32 * det3 - matrix.M34 * det1) * invDetMatrix;
ret.M41 = (-matrix.M21 * det10 + matrix.M22 * det8 - matrix.M23 * det7) * invDetMatrix;
ret.M42 = (matrix.M11 * det10 - matrix.M12 * det8 + matrix.M13 * det7) * invDetMatrix;
ret.M43 = (-matrix.M41 * det4 + matrix.M42 * det2 - matrix.M43 * det1) * invDetMatrix;
ret.M44 = (matrix.M31 * det4 - matrix.M32 * det2 + matrix.M33 * det1) * invDetMatrix;
result = ret;
}
public static Vertices CreateArc(GGame.Math.Fix64 radians, int sides, GGame.Math.Fix64 radius)
{
Debug.Assert(radians > 0, "The arc needs to be larger than 0");
Debug.Assert(sides > 1, "The arc needs to have more than 1 sides");
Debug.Assert(radius > 0, "The arc needs to have a radius larger than 0");
Vertices vertices = new Vertices();
GGame.Math.Fix64 stepSize = radians / sides;
for (int i = sides - 1; i > 0; i--)
{
vertices.Add(new Vector2(radius * GGame.Math.Fix64.Cos(stepSize * i),
radius * GGame.Math.Fix64.Sin(stepSize * i)));
}
return(vertices);
}
/// <summary>
/// Build vertices to represent an oriented box.
/// </summary>
/// <param name="hx">the half-width.</param>
/// <param name="hy">the half-height.</param>
/// <param name="center">the center of the box in local coordinates.</param>
/// <param name="angle">the rotation of the box in local coordinates.</param>
public static Vertices CreateRectangle(GGame.Math.Fix64 hx, GGame.Math.Fix64 hy, Vector2 center, GGame.Math.Fix64 angle)
{
Vertices vertices = CreateRectangle(hx, hy);
Transform xf = new Transform();
xf.p = center;
xf.q.Set(angle);
// Transform vertices
for (int i = 0; i < 4; ++i)
{
vertices[i] = MathUtils.Mul(ref xf, vertices[i]);
}
return(vertices);
}
public GGame.Math.Fix64 GetLength()
{
List <Vector2> verts = GetVertices(ControlPoints.Count * 25);
GGame.Math.Fix64 length = 0;
for (int i = 1; i < verts.Count; i++)
{
length += Vector2.Distance(verts[i - 1], verts[i]);
}
if (Closed)
{
length += Vector2.Distance(verts[ControlPoints.Count - 1], verts[0]);
}
return(length);
}
internal void Validate(GGame.Math.Fix64 invDt)
{
if (!Enabled)
{
return;
}
GGame.Math.Fix64 jointErrorSquared = GetReactionForce(invDt).LengthSquared();
if (Fix64.Abs(jointErrorSquared) <= _breakpointSquared)
{
return;
}
Enabled = false;
Broke?.Invoke(this, (GGame.Math.Fix64)Fix64.Sqrt(jointErrorSquared));
}
public static List <TriangulationPoint> UniformGrid(int n, GGame.Math.Fix64 scale)
{
GGame.Math.Fix64 x = 0;
GGame.Math.Fix64 size = scale / n;
GGame.Math.Fix64 halfScale = 0.5 * scale;
List <TriangulationPoint> points = new List <TriangulationPoint>();
for (int i = 0; i < n + 1; i++)
{
x = halfScale - i * size;
for (int j = 0; j < n + 1; j++)
{
points.Add(new TriangulationPoint(x, halfScale - j * size));
}
}
return(points);
}
/// <summary>
/// Simplify the polygon by removing all points that in pairs of 3 have an area less than the tolerance.
/// Pass in 0 as tolerance, and it will only remove collinear points.
/// </summary>
/// <param name="vertices"></param>
/// <param name="areaTolerance"></param>
/// <returns></returns>
public static Vertices ReduceByArea(Vertices vertices, GGame.Math.Fix64 areaTolerance)
{
//From physics2d.net
if (vertices.Count <= 3)
{
return(vertices);
}
if (areaTolerance < 0)
{
throw new ArgumentOutOfRangeException(nameof(areaTolerance), "must be equal to or greater than zero.");
}
Vertices simplified = new Vertices(vertices.Count);
Vector2 v3;
Vector2 v1 = vertices[vertices.Count - 2];
Vector2 v2 = vertices[vertices.Count - 1];
areaTolerance *= 2;
for (int i = 0; i < vertices.Count; ++i, v2 = v3)
{
v3 = i == vertices.Count - 1 ? simplified[0] : vertices[i];
GGame.Math.Fix64 old1;
MathUtils.Cross(ref v1, ref v2, out old1);
GGame.Math.Fix64 old2;
MathUtils.Cross(ref v2, ref v3, out old2);
GGame.Math.Fix64 new1;
MathUtils.Cross(ref v1, ref v3, out new1);
if (Fix64.Abs(new1 - (old1 + old2)) > areaTolerance)
{
simplified.Add(v2);
v1 = v2;
}
}
return(simplified);
}
public static void Multiply(ref Matrix matrix1, GGame.Math.Fix64 factor, out Matrix result)
{
result.M11 = matrix1.M11 * factor;
result.M12 = matrix1.M12 * factor;
result.M13 = matrix1.M13 * factor;
result.M14 = matrix1.M14 * factor;
result.M21 = matrix1.M21 * factor;
result.M22 = matrix1.M22 * factor;
result.M23 = matrix1.M23 * factor;
result.M24 = matrix1.M24 * factor;
result.M31 = matrix1.M31 * factor;
result.M32 = matrix1.M32 * factor;
result.M33 = matrix1.M33 * factor;
result.M34 = matrix1.M34 * factor;
result.M41 = matrix1.M41 * factor;
result.M42 = matrix1.M42 * factor;
result.M43 = matrix1.M43 * factor;
result.M44 = matrix1.M44 * factor;
}
public static bool RayCastCircle(ref Vector2 pos, GGame.Math.Fix64 radius, ref RayCastInput input, ref Transform transform, out RayCastOutput output)
{
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
output = new RayCastOutput();
Vector2 position = transform.p + MathUtils.Mul(transform.q, pos);
Vector2 s = input.Point1 - position;
GGame.Math.Fix64 b = Vector2.Dot(s, s) - radius * radius;
// Solve quadratic equation.
Vector2 r = input.Point2 - input.Point1;
GGame.Math.Fix64 c = Vector2.Dot(s, r);
GGame.Math.Fix64 rr = Vector2.Dot(r, r);
GGame.Math.Fix64 sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.Epsilon)
{
return(false);
}
// Find the point of intersection of the line with the circle.
GGame.Math.Fix64 a = -(c + (GGame.Math.Fix64)Fix64.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Fraction = a;
output.Normal = s + a * r;
output.Normal.Normalize();
return(true);
}
return(false);
}
public static void CreateOrthographicOffCenter(GGame.Math.Fix64 left, GGame.Math.Fix64 right, GGame.Math.Fix64 bottom, GGame.Math.Fix64 top,
GGame.Math.Fix64 zNearPlane, GGame.Math.Fix64 zFarPlane, out Matrix result)
{
result.M11 = 2 / (right - left);
result.M12 = 0;
result.M13 = 0;
result.M14 = 0;
result.M21 = 0;
result.M22 = 2 / (top - bottom);
result.M23 = 0;
result.M24 = 0;
result.M31 = 0;
result.M32 = 0;
result.M33 = 1 / (zNearPlane - zFarPlane);
result.M34 = 0;
result.M41 = (left + right) / (left - right);
result.M42 = (bottom + top) / (bottom - top);
result.M43 = zNearPlane / (zNearPlane - zFarPlane);
result.M44 = 1;
}
private void Initialize(Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis, bool useWorldCoordinates)
{
JointType = JointType.Prismatic;
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(localAnchorA);
LocalAnchorB = BodyB.GetLocalPoint(localAnchorB);
}
else
{
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
}
Axis = axis; //Velcro only: store the orignal value for use in Serialization
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
/// <summary>
/// Constructor for 4x4 Matrix
/// </summary>
/// <param name="m11">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m12">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m13">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m14">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m21">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m22">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m23">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m24">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m31">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m32">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m33">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m34">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m41">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m42">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m43">
/// A <see cref="System.Single"/>
/// </param>
/// <param name="m44">
/// A <see cref="System.Single"/>
/// </param>
public Matrix(GGame.Math.Fix64 m11, GGame.Math.Fix64 m12, GGame.Math.Fix64 m13, GGame.Math.Fix64 m14, GGame.Math.Fix64 m21, GGame.Math.Fix64 m22, GGame.Math.Fix64 m23, GGame.Math.Fix64 m24,
GGame.Math.Fix64 m31, GGame.Math.Fix64 m32, GGame.Math.Fix64 m33, GGame.Math.Fix64 m34, GGame.Math.Fix64 m41, GGame.Math.Fix64 m42, GGame.Math.Fix64 m43, GGame.Math.Fix64 m44)
{
M11 = m11;
M12 = m12;
M13 = m13;
M14 = m14;
M21 = m21;
M22 = m22;
M23 = m23;
M24 = m24;
M31 = m31;
M32 = m32;
M33 = m33;
M34 = m34;
M41 = m41;
M42 = m42;
M43 = m43;
M44 = m44;
}
public static Matrix Multiply(Matrix matrix1, GGame.Math.Fix64 factor)
{
matrix1.M11 *= factor;
matrix1.M12 *= factor;
matrix1.M13 *= factor;
matrix1.M14 *= factor;
matrix1.M21 *= factor;
matrix1.M22 *= factor;
matrix1.M23 *= factor;
matrix1.M24 *= factor;
matrix1.M31 *= factor;
matrix1.M32 *= factor;
matrix1.M33 *= factor;
matrix1.M34 *= factor;
matrix1.M41 *= factor;
matrix1.M42 *= factor;
matrix1.M43 *= factor;
matrix1.M44 *= factor;
return(matrix1);
}
public static void CreateOrthographic(GGame.Math.Fix64 width, GGame.Math.Fix64 height, GGame.Math.Fix64 zNearPlane, GGame.Math.Fix64 zFarPlane,
out Matrix result)
{
result.M11 = 2 / width;
result.M12 = 0;
result.M13 = 0;
result.M14 = 0;
result.M21 = 0;
result.M22 = 2 / height;
result.M23 = 0;
result.M24 = 0;
result.M31 = 0;
result.M32 = 0;
result.M33 = 1 / (zNearPlane - zFarPlane);
result.M34 = 0;
result.M41 = 0;
result.M42 = 0;
result.M43 = zNearPlane / (zNearPlane - zFarPlane);
result.M44 = 1;
}
