public static Polygon RandomCircleSweep(FP scale, int vertexCount)
{
FP fP = scale / 4;
PolygonPoint[] array = new PolygonPoint[vertexCount];
for (int i = 0; i < vertexCount; i++)
{
do
{
bool flag = i % 250 == 0;
if (flag)
{
fP += scale / 2 * (0.5 - PolygonGenerator.RNG.NextFP());
}
else
{
bool flag2 = i % 50 == 0;
if (flag2)
{
fP += scale / 5 * (0.5 - PolygonGenerator.RNG.NextFP());
}
else
{
fP += 25 * scale / vertexCount * (0.5 - PolygonGenerator.RNG.NextFP());
}
}
fP = ((fP > scale / 2) ? (scale / 2) : fP);
fP = ((fP < scale / 10) ? (scale / 10) : fP);
}while (fP < scale / 10 || fP > scale / 2);
PolygonPoint polygonPoint = new PolygonPoint(fP * FP.Cos(PolygonGenerator.PI_2 * i / vertexCount), fP * FP.Sin(PolygonGenerator.PI_2 * i / vertexCount));
array[i] = polygonPoint;
}
return(new Polygon(array));
}
public static Vertices CreateRoundedRectangle(FP width, FP height, FP xRadius, FP yRadius, int segments)
{
bool flag = yRadius > height / 2 || xRadius > width / 2;
if (flag)
{
throw new Exception("Rounding amount can't be more than half the height and width respectively.");
}
bool flag2 = segments < 0;
if (flag2)
{
throw new Exception("Segments must be zero or more.");
}
Debug.Assert(Settings.MaxPolygonVertices >= 8);
Vertices vertices = new Vertices();
bool flag3 = segments == 0;
if (flag3)
{
vertices.Add(new TSVector2(width * 0.5f - xRadius, -height * 0.5f));
vertices.Add(new TSVector2(width * 0.5f, -height * 0.5f + yRadius));
vertices.Add(new TSVector2(width * 0.5f, height * 0.5f - yRadius));
vertices.Add(new TSVector2(width * 0.5f - xRadius, height * 0.5f));
vertices.Add(new TSVector2(-width * 0.5f + xRadius, height * 0.5f));
vertices.Add(new TSVector2(-width * 0.5f, height * 0.5f - yRadius));
vertices.Add(new TSVector2(-width * 0.5f, -height * 0.5f + yRadius));
vertices.Add(new TSVector2(-width * 0.5f + xRadius, -height * 0.5f));
}
else
{
int num = segments * 4 + 8;
FP x = MathHelper.TwoPi / (num - 4);
int num2 = num / 4;
TSVector2 value = new TSVector2(width / 2 - xRadius, height / 2 - yRadius);
vertices.Add(value + new TSVector2(xRadius, -yRadius + yRadius));
short num3 = 0;
for (int i = 1; i < num; i++)
{
bool flag4 = i - num2 == 0 || i - num2 * 3 == 0;
if (flag4)
{
value.x *= -1;
num3 -= 1;
}
else
{
bool flag5 = i - num2 * 2 == 0;
if (flag5)
{
value.y *= -1;
num3 -= 1;
}
}
vertices.Add(value + new TSVector2(xRadius * FP.Cos(x * -(i + (int)num3)), -yRadius * FP.Sin(x * -(i + (int)num3))));
}
}
return(vertices);
}
public static void CreateRotationX(FP radians, out Matrix result)
{
result = Matrix.Identity;
result.M22 = FP.Cos(radians);
result.M23 = FP.Sin(radians);
result.M32 = -result.M23;
result.M33 = result.M22;
}
public static void CreateRotationY(FP radians, out Matrix result)
{
result = Matrix.Identity;
result.M11 = FP.Cos(radians);
result.M13 = FP.Sin(radians);
result.M31 = -result.M13;
result.M33 = result.M11;
}
public static void CreateRotationZ(FP radians, out Matrix result)
{
result = Matrix.Identity;
result.M11 = FP.Cos(radians);
result.M12 = FP.Sin(radians);
result.M21 = -result.M12;
result.M22 = result.M11;
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
TSVector temp;
TSVector.Multiply(ref body.linearVelocity, timestep, out temp);
TSVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
FP halfTimeStep = FP.Half * timestep;
FP halfTimeStepAngle = halfTimeStep * angle;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
//TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle));
TSQuaternion ornA;
TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity /= (1 + timestep * body.linearDrag);
body.angularVelocity /= (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
public static Matrix CreateRotationX(FP radians)
{
Matrix matrix = Matrix.Identity;
matrix.M22 = FP.Cos(radians);
matrix.M23 = FP.Sin(radians);
matrix.M32 = -matrix.M23;
matrix.M33 = matrix.M22;
return(matrix);
}
public static Matrix CreateRotationY(FP radians)
{
Matrix matrix = Matrix.Identity;
matrix.M11 = FP.Cos(radians);
matrix.M13 = FP.Sin(radians);
matrix.M31 = -matrix.M13;
matrix.M33 = matrix.M11;
return(matrix);
}
public static Matrix CreateRotationZ(FP radians)
{
Matrix matrix = Matrix.Identity;
matrix.M11 = FP.Cos(radians);
matrix.M12 = FP.Sin(radians);
matrix.M21 = -matrix.M12;
matrix.M22 = matrix.M11;
return(matrix);
}
private void Integrate()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
var body = rigidBodies[index];
if (body.isStatic || !body.IsActive)
{
continue;
}
body.position += body.linearVelocity * timestep;
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
TSVector.Multiply(body.angularVelocity, (FP.Half * timestep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(body.angularVelocity, (FP.Sin(FP.Half * angle * timestep) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(angle * timestep * FP.Half));
TSQuaternion ornA; TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity *= 1 / (1 + timestep * body.linearDrag);
body.angularVelocity *= 1 / (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
}
public static TSVector2 Rotate(this TSVector2 v, FP degrees)
{
FP sin = FP.Sin(degrees * FP.Deg2Rad);
FP cos = FP.Cos(degrees * FP.Deg2Rad);
FP tx = v.x;
FP ty = v.y;
v.x = (cos * tx) - (sin * ty);
v.y = (sin * tx) + (cos * ty);
return(v);
}
public static Vertices CreateEllipse(FP xRadius, FP yRadius, int numberOfEdges)
{
Vertices vertices = new Vertices();
FP x = MathHelper.TwoPi / numberOfEdges;
vertices.Add(new TSVector2(xRadius, 0));
for (int i = numberOfEdges - 1; i > 0; i--)
{
vertices.Add(new TSVector2(xRadius * FP.Cos(x * i), -yRadius * FP.Sin(x * i)));
}
return(vertices);
}
public static Vertices CreateArc(FP radians, int sides, FP 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();
FP x = radians / sides;
for (int i = sides - 1; i > 0; i--)
{
vertices.Add(new TSVector2(radius * FP.Cos(x * i), radius * FP.Sin(x * i)));
}
return(vertices);
}
/// <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(FP xRadius, FP yRadius, int numberOfEdges)
{
Vertices vertices = new Vertices();
FP stepSize = FP.PiTimes2 / numberOfEdges;
vertices.Add(new FPVector2(xRadius, 0));
for (int i = numberOfEdges - 1; i > 0; --i)
{
vertices.Add(new FPVector2(xRadius * FP.Cos(stepSize * i),
-yRadius * FP.Sin(stepSize * i)));
}
return(vertices);
}
public static void CreateRotationZ(FP radians, out TSMatrix result)
{
FP num2 = FP.Cos(radians);
FP num = FP.Sin(radians);
result.M11 = num2;
result.M12 = num;
result.M13 = FP.Zero;
result.M21 = -num;
result.M22 = num2;
result.M23 = FP.Zero;
result.M31 = FP.Zero;
result.M32 = FP.Zero;
result.M33 = FP.One;
}
public static TSQuaternion AngleAxis(FP angle, TSVector axis)
{
axis = axis * FP.Deg2Rad;
axis.Normalize();
FP halfAngle = angle * FP.Deg2Rad * FP.Half;
TSQuaternion rotation;
FP sin = FP.Sin(halfAngle);
rotation.x = axis.x * sin;
rotation.y = axis.y * sin;
rotation.z = axis.z * sin;
rotation.w = FP.Cos(halfAngle);
return(rotation);
}
public static TSMatrix CreateRotationZ(FP radians)
{
TSMatrix matrix;
FP num2 = FP.Cos(radians);
FP num = FP.Sin(radians);
matrix.M11 = num2;
matrix.M12 = num;
matrix.M13 = FP.Zero;
matrix.M21 = -num;
matrix.M22 = num2;
matrix.M23 = FP.Zero;
matrix.M31 = FP.Zero;
matrix.M32 = FP.Zero;
matrix.M33 = FP.One;
return(matrix);
}
public static void CreateFromYawPitchRoll(FP yaw, FP pitch, FP roll, out TSQuaternion result)
{
FP num9 = roll * FP.Half;
FP num6 = FP.Sin(num9);
FP num5 = FP.Cos(num9);
FP num8 = pitch * FP.Half;
FP num4 = FP.Sin(num8);
FP num3 = FP.Cos(num8);
FP num7 = yaw * FP.Half;
FP num2 = FP.Sin(num7);
FP num = FP.Cos(num7);
result.x = ((num * num4) * num5) + ((num2 * num3) * num6);
result.y = ((num2 * num3) * num5) - ((num * num4) * num6);
result.z = ((num * num3) * num6) - ((num2 * num4) * num5);
result.w = ((num * num3) * num5) + ((num2 * num4) * num6);
}
public static int[] Build(List <FPVector> pointCloud, Approximation factor)
{
List <int> allIndices = new List <int>();
int steps = (int)factor;
for (int thetaIndex = 0; thetaIndex < steps; thetaIndex++)
{
// [0,PI]
FP theta = FPMath.Pi / (steps - 1) * thetaIndex;
FP sinTheta = FP.Sin(theta);
FP cosTheta = FP.Cos(theta);
for (int phiIndex = 0; phiIndex < steps; phiIndex++)
{
// [-PI,PI]
FP phi = ((2 * FP.One) * FPMath.Pi) / (steps - 0) * phiIndex - FPMath.Pi;
FP sinPhi = FP.Sin(phi);
FP cosPhi = FP.Cos(phi);
FPVector dir = new FPVector(sinTheta * cosPhi, cosTheta, sinTheta * sinPhi);
int index = FindExtremePoint(pointCloud, ref dir);
allIndices.Add(index);
}
}
allIndices.Sort();
for (int i = 1; i < allIndices.Count; i++)
{
if (allIndices[i - 1] == allIndices[i])
{
allIndices.RemoveAt(i - 1); i--;
}
}
return(allIndices.ToArray());
// or using 3.5 extensions
// return allIndices.Distinct().ToArray();
}
/// <summary>
/// Rotate the vertices with the defined value in radians.
///
/// Warning: Using this method on an active set of vertices of a Body,
/// will cause problems with collisions. Use Body.Rotation instead.
/// </summary>
/// <param name="value">The amount to rotate by in radians.</param>
public void Rotate(FP value)
{
Debug.Assert(!AttachedToBody, "Rotating vertices that are used by a Body can result in unstable behavior.");
FP num1 = FP.Cos(value);
FP num2 = FP.Sin(value);
for (int i = 0; i < Count; i++)
{
TSVector2 position = this[i];
this[i] = new TSVector2((position.x * num1 + position.y * -num2), (position.x * num2 + position.y * num1));
}
if (Holes != null && Holes.Count > 0)
{
foreach (Vertices hole in Holes)
{
hole.Rotate(value);
}
}
}
public void Rotate(FP value)
{
Debug.Assert(!this.AttachedToBody, "Rotating vertices that are used by a Body can result in unstable behavior.");
FP y = FP.Cos(value);
FP fP = FP.Sin(value);
for (int i = 0; i < base.Count; i++)
{
TSVector2 tSVector = base[i];
base[i] = new TSVector2(tSVector.x * y + tSVector.y * -fP, tSVector.x * fP + tSVector.y * y);
}
bool flag = this.Holes != null && this.Holes.Count > 0;
if (flag)
{
foreach (Vertices current in this.Holes)
{
current.Rotate(value);
}
}
}
/// <summary>
/// Creates a gear shape with the specified radius and number of teeth.
/// </summary>
/// <param name="radius">The radius.</param>
/// <param name="numberOfTeeth">The number of teeth.</param>
/// <param name="tipPercentage">The tip percentage.</param>
/// <param name="toothHeight">Height of the tooth.</param>
/// <returns></returns>
public static Vertices CreateGear(FP radius, int numberOfTeeth, FP tipPercentage, FP toothHeight)
{
Vertices vertices = new Vertices();
FP stepSize = FP.PiTimes2 / numberOfTeeth;
tipPercentage /= 100f;
FPMath.Clamp(tipPercentage, 0f, 1f);
FP toothTipStepSize = (stepSize / 2f) * tipPercentage;
FP toothAngleStepSize = (stepSize - (toothTipStepSize * 2f)) / 2f;
for (int i = numberOfTeeth - 1; i >= 0; --i)
{
if (toothTipStepSize > 0f)
{
vertices.Add(
new FPVector2(radius *
FP.Cos(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize),
-radius *
FP.Sin(stepSize * i + toothAngleStepSize * 2f + toothTipStepSize)));
vertices.Add(
new FPVector2((radius + toothHeight) *
FP.Cos(stepSize * i + toothAngleStepSize + toothTipStepSize),
-(radius + toothHeight) *
FP.Sin(stepSize * i + toothAngleStepSize + toothTipStepSize)));
}
vertices.Add(new FPVector2((radius + toothHeight) *
FP.Cos(stepSize * i + toothAngleStepSize),
-(radius + toothHeight) *
FP.Sin(stepSize * i + toothAngleStepSize)));
vertices.Add(new FPVector2(radius * FP.Cos(stepSize * i),
-radius * FP.Sin(stepSize * i)));
}
return(vertices);
}
public static Vertices CreateGear(FP radius, int numberOfTeeth, FP tipPercentage, FP toothHeight)
{
Vertices vertices = new Vertices();
FP x = MathHelper.TwoPi / numberOfTeeth;
tipPercentage /= 100f;
MathHelper.Clamp(tipPercentage, 0f, 1f);
FP fP = x / 2f * tipPercentage;
FP fP2 = (x - fP * 2f) / 2f;
for (int i = numberOfTeeth - 1; i >= 0; i--)
{
bool flag = fP > 0f;
if (flag)
{
vertices.Add(new TSVector2(radius * FP.Cos(x * i + fP2 * 2f + fP), -radius * FP.Sin(x * i + fP2 * 2f + fP)));
vertices.Add(new TSVector2((radius + toothHeight) * FP.Cos(x * i + fP2 + fP), -(radius + toothHeight) * FP.Sin(x * i + fP2 + fP)));
}
vertices.Add(new TSVector2((radius + toothHeight) * FP.Cos(x * i + fP2), -(radius + toothHeight) * FP.Sin(x * i + fP2)));
vertices.Add(new TSVector2(radius * FP.Cos(x * i), -radius * FP.Sin(x * i)));
}
return(vertices);
}
/// <summary>
/// Creates a matrix which rotates around the given axis by the given angle.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="angle">The angle.</param>
/// <param name="result">The resulting rotation matrix</param>
#region public static void CreateFromAxisAngle(ref JVector axis, FP angle, out JMatrix result)
public static void CreateFromAxisAngle(ref TSVector axis, FP angle, out TSMatrix result)
{
FP x = axis.x;
FP y = axis.y;
FP z = axis.z;
FP num2 = FP.Sin(angle);
FP num = FP.Cos(angle);
FP num11 = x * x;
FP num10 = y * y;
FP num9 = z * z;
FP num8 = x * y;
FP num7 = x * z;
FP num6 = y * z;
result.M11 = num11 + (num * (FP.One - num11));
result.M12 = (num8 - (num * num8)) + (num2 * z);
result.M13 = (num7 - (num * num7)) - (num2 * y);
result.M21 = (num8 - (num * num8)) - (num2 * z);
result.M22 = num10 + (num * (FP.One - num10));
result.M23 = (num6 - (num * num6)) + (num2 * x);
result.M31 = (num7 - (num * num7)) + (num2 * y);
result.M32 = (num6 - (num * num6)) - (num2 * x);
result.M33 = num9 + (num * (FP.One - num9));
}
/// <summary>
/// Creates an capsule with the specified height, radius and number of edges.
/// A capsule has the same form as a pill capsule.
/// </summary>
/// <param name="height">Height (inner height + radii) of the capsule.</param>
/// <param name="topRadius">Radius of the top.</param>
/// <param name="topEdges">The number of edges of the top. The more edges, the more it resembles an capsule</param>
/// <param name="bottomRadius">Radius of bottom.</param>
/// <param name="bottomEdges">The number of edges of the bottom. The more edges, the more it resembles an capsule</param>
/// <returns></returns>
public static Vertices CreateCapsule(FP height, FP topRadius, int topEdges, FP bottomRadius,
int bottomEdges)
{
if (height <= 0)
{
throw new ArgumentException("Height must be longer than 0", "height");
}
if (topRadius <= 0)
{
throw new ArgumentException("The top radius must be more than 0", "topRadius");
}
if (topEdges <= 0)
{
throw new ArgumentException("Top edges must be more than 0", "topEdges");
}
if (bottomRadius <= 0)
{
throw new ArgumentException("The bottom radius must be more than 0", "bottomRadius");
}
if (bottomEdges <= 0)
{
throw new ArgumentException("Bottom edges must be more than 0", "bottomEdges");
}
if (topRadius >= height / 2)
{
throw new ArgumentException(
"The top radius must be lower than height / 2. Higher values of top radius would create a circle, and not a half circle.",
"topRadius");
}
if (bottomRadius >= height / 2)
{
throw new ArgumentException(
"The bottom radius must be lower than height / 2. Higher values of bottom radius would create a circle, and not a half circle.",
"bottomRadius");
}
Vertices vertices = new Vertices();
FP newHeight = (height - topRadius - bottomRadius) * 0.5f;
// top
vertices.Add(new FPVector2(topRadius, newHeight));
FP stepSize = FP.Pi / topEdges;
for (int i = 1; i < topEdges; i++)
{
vertices.Add(new FPVector2(topRadius * FP.Cos(stepSize * i),
topRadius * FP.Sin(stepSize * i) + newHeight));
}
vertices.Add(new FPVector2(-topRadius, newHeight));
// bottom
vertices.Add(new FPVector2(-bottomRadius, -newHeight));
stepSize = FP.Pi / bottomEdges;
for (int i = 1; i < bottomEdges; i++)
{
vertices.Add(new FPVector2(-bottomRadius * FP.Cos(stepSize * i),
-bottomRadius * FP.Sin(stepSize * i) - newHeight));
}
vertices.Add(new FPVector2(bottomRadius, -newHeight));
return(vertices);
}
public Dictionary <Fixture, TSVector2> Activate(TSVector2 pos, FP radius, FP maxForce)
{
AABB aABB;
aABB.LowerBound = pos + new TSVector2(-radius, -radius);
aABB.UpperBound = pos + new TSVector2(radius, radius);
Fixture[] shapes = new Fixture[this.MaxShapes];
Fixture[] containedShapes = new Fixture[5];
bool exit = false;
int shapeCount = 0;
int containedShapeCount = 0;
this.World.QueryAABB(delegate(Fixture fixture)
{
bool flag22 = fixture.TestPoint(ref pos);
bool result2;
if (flag22)
{
bool ignoreWhenInsideShape = this.IgnoreWhenInsideShape;
if (ignoreWhenInsideShape)
{
exit = true;
result2 = false;
return(result2);
}
Fixture[] arg_45_0 = containedShapes;
int num4 = containedShapeCount;
containedShapeCount = num4 + 1;
arg_45_0[num4] = fixture;
}
else
{
Fixture[] arg_62_0 = shapes;
int num4 = shapeCount;
shapeCount = num4 + 1;
arg_62_0[num4] = fixture;
}
result2 = true;
return(result2);
}, ref aABB);
bool exit2 = exit;
Dictionary <Fixture, TSVector2> result;
if (exit2)
{
result = new Dictionary <Fixture, TSVector2>();
}
else
{
Dictionary <Fixture, TSVector2> dictionary = new Dictionary <Fixture, TSVector2>(shapeCount + containedShapeCount);
FP[] array = new FP[shapeCount * 2];
int num = 0;
for (int i = 0; i < shapeCount; i++)
{
CircleShape circleShape = shapes[i].Shape as CircleShape;
bool flag = circleShape != null;
PolygonShape polygonShape;
if (flag)
{
Vertices vertices = new Vertices();
TSVector2 item = TSVector2.zero + new TSVector2(circleShape.Radius, 0);
vertices.Add(item);
item = TSVector2.zero + new TSVector2(0, circleShape.Radius);
vertices.Add(item);
item = TSVector2.zero + new TSVector2(-circleShape.Radius, circleShape.Radius);
vertices.Add(item);
item = TSVector2.zero + new TSVector2(0, -circleShape.Radius);
vertices.Add(item);
polygonShape = new PolygonShape(vertices, 0);
}
else
{
polygonShape = (shapes[i].Shape as PolygonShape);
}
bool flag2 = shapes[i].Body.BodyType == BodyType.Dynamic && polygonShape != null;
if (flag2)
{
TSVector2 tSVector = shapes[i].Body.GetWorldPoint(polygonShape.MassData.Centroid) - pos;
FP y = FP.Atan2(tSVector.y, tSVector.x);
FP y2 = FP.MaxValue;
FP y3 = FP.MinValue;
FP fP = 0f;
FP fP2 = 0f;
for (int j = 0; j < polygonShape.Vertices.Count; j++)
{
TSVector2 tSVector2 = shapes[i].Body.GetWorldPoint(polygonShape.Vertices[j]) - pos;
FP fP3 = FP.Atan2(tSVector2.y, tSVector2.x);
FP fP4 = fP3 - y;
fP4 = (fP4 - MathHelper.Pi) % (2 * MathHelper.Pi);
bool flag3 = fP4 < 0f;
if (flag3)
{
fP4 += 2 * MathHelper.Pi;
}
fP4 -= MathHelper.Pi;
bool flag4 = FP.Abs(fP4) > MathHelper.Pi;
if (!flag4)
{
bool flag5 = fP4 > y3;
if (flag5)
{
y3 = fP4;
fP2 = fP3;
}
bool flag6 = fP4 < y2;
if (flag6)
{
y2 = fP4;
fP = fP3;
}
}
}
array[num] = fP;
num++;
array[num] = fP2;
num++;
}
}
Array.Sort <FP>(array, 0, num, this._rdc);
this._data.Clear();
bool flag7 = true;
for (int k = 0; k < num; k++)
{
Fixture fixture = null;
int num2 = (k == num - 1) ? 0 : (k + 1);
bool flag8 = array[k] == array[num2];
if (!flag8)
{
bool flag9 = k == num - 1;
FP x;
if (flag9)
{
x = array[0] + MathHelper.Pi * 2 + array[k];
}
else
{
x = array[k + 1] + array[k];
}
x /= 2;
TSVector2 pos2 = pos;
TSVector2 point = radius * new TSVector2(FP.Cos(x), FP.Sin(x)) + pos;
bool hitClosest = false;
this.World.RayCast(delegate(Fixture f, TSVector2 p, TSVector2 n, FP fr)
{
Body body = f.Body;
bool flag22 = !this.IsActiveOn(body);
FP result2;
if (flag22)
{
result2 = 0;
}
else
{
hitClosest = true;
fixture = f;
result2 = fr;
}
return(result2);
}, pos2, point);
bool flag10 = hitClosest && fixture.Body.BodyType == BodyType.Dynamic;
if (flag10)
{
bool flag11 = this._data.Any <ShapeData>() && this._data.Last <ShapeData>().Body == fixture.Body && !flag7;
if (flag11)
{
int index = this._data.Count - 1;
ShapeData value = this._data[index];
value.Max = array[num2];
this._data[index] = value;
}
else
{
ShapeData item2;
item2.Body = fixture.Body;
item2.Min = array[k];
item2.Max = array[num2];
this._data.Add(item2);
}
bool flag12 = this._data.Count > 1 && k == num - 1 && this._data.Last <ShapeData>().Body == this._data.First <ShapeData>().Body&& this._data.Last <ShapeData>().Max == this._data.First <ShapeData>().Min;
if (flag12)
{
ShapeData value2 = this._data[0];
value2.Min = this._data.Last <ShapeData>().Min;
this._data.RemoveAt(this._data.Count - 1);
this._data[0] = value2;
while (this._data.First <ShapeData>().Min >= this._data.First <ShapeData>().Max)
{
value2.Min -= MathHelper.Pi * 2;
this._data[0] = value2;
}
}
int index2 = this._data.Count - 1;
ShapeData value3 = this._data[index2];
while (this._data.Count > 0 && this._data.Last <ShapeData>().Min >= this._data.Last <ShapeData>().Max)
{
value3.Min = this._data.Last <ShapeData>().Min - 2 * MathHelper.Pi;
this._data[index2] = value3;
}
flag7 = false;
}
else
{
flag7 = true;
}
}
}
for (int l = 0; l < this._data.Count; l++)
{
bool flag13 = !this.IsActiveOn(this._data[l].Body);
if (!flag13)
{
FP fP5 = this._data[l].Max - this._data[l].Min;
FP fP6 = MathHelper.Min(RealExplosion.MaxEdgeOffset, this.EdgeRatio * fP5);
int num3 = FP.Ceiling((fP5 - 2f * fP6 - (this.MinRays - 1) * this.MaxAngle) / this.MaxAngle).AsInt();
bool flag14 = num3 < 0;
if (flag14)
{
num3 = 0;
}
FP y4 = (fP5 - fP6 * 2f) / (this.MinRays + num3 - 1);
FP fP7 = this._data[l].Min + fP6;
while (fP7 < this._data[l].Max || MathUtils.FPEquals(fP7, this._data[l].Max, 0.0001f))
{
TSVector2 pos3 = pos;
TSVector2 tSVector3 = pos + radius * new TSVector2(FP.Cos(fP7), FP.Sin(fP7));
TSVector2 tSVector4 = TSVector2.zero;
FP fP8 = FP.MaxValue;
List <Fixture> fixtureList = this._data[l].Body.FixtureList;
for (int m = 0; m < fixtureList.Count; m++)
{
Fixture fixture3 = fixtureList[m];
RayCastInput rayCastInput;
rayCastInput.Point1 = pos3;
rayCastInput.Point2 = tSVector3;
rayCastInput.MaxFraction = 50f;
RayCastOutput rayCastOutput;
bool flag15 = fixture3.RayCast(out rayCastOutput, ref rayCastInput, 0);
if (flag15)
{
bool flag16 = fP8 > rayCastOutput.Fraction;
if (flag16)
{
fP8 = rayCastOutput.Fraction;
tSVector4 = rayCastOutput.Fraction * tSVector3 + (1 - rayCastOutput.Fraction) * pos3;
}
}
FP scaleFactor = fP5 / (this.MinRays + num3) * maxForce * 180f / MathHelper.Pi * (1f - TSMath.Min(FP.One, fP8));
TSVector2 tSVector5 = TSVector2.Dot(scaleFactor * new TSVector2(FP.Cos(fP7), FP.Sin(fP7)), -rayCastOutput.Normal) * new TSVector2(FP.Cos(fP7), FP.Sin(fP7));
this._data[l].Body.ApplyLinearImpulse(ref tSVector5, ref tSVector4);
bool flag17 = dictionary.ContainsKey(fixture3);
if (flag17)
{
Dictionary <Fixture, TSVector2> dictionary2 = dictionary;
Fixture key = fixture3;
dictionary2[key] += tSVector5;
}
else
{
dictionary.Add(fixture3, tSVector5);
}
bool flag18 = fP8 > 1f;
if (flag18)
{
tSVector4 = tSVector3;
}
}
fP7 += y4;
}
}
}
for (int n2 = 0; n2 < containedShapeCount; n2++)
{
Fixture fixture2 = containedShapes[n2];
bool flag19 = !this.IsActiveOn(fixture2.Body);
if (!flag19)
{
FP scaleFactor2 = this.MinRays * maxForce * 180f / MathHelper.Pi;
CircleShape circleShape2 = fixture2.Shape as CircleShape;
bool flag20 = circleShape2 != null;
TSVector2 worldPoint;
if (flag20)
{
worldPoint = fixture2.Body.GetWorldPoint(circleShape2.Position);
}
else
{
PolygonShape polygonShape2 = fixture2.Shape as PolygonShape;
worldPoint = fixture2.Body.GetWorldPoint(polygonShape2.MassData.Centroid);
}
TSVector2 value4 = scaleFactor2 * (worldPoint - pos);
fixture2.Body.ApplyLinearImpulse(ref value4, ref worldPoint);
bool flag21 = !dictionary.ContainsKey(fixture2);
if (flag21)
{
dictionary.Add(fixture2, value4);
}
}
}
result = dictionary;
}
return(result);
}
/// <summary>
/// Set using an angle in radians.
/// </summary>
/// <param name="angle"></param>
public void Set(FP angle)
{
// TODO_ERIN optimize
s = FP.Sin(angle);
c = FP.Cos(angle);
}
/// <summary>
/// Returns the cosine of value.
/// </summary>
public static FP Cos(FP value)
{
return(FP.Cos(value));
}
//Rounded rectangle contributed by Jonathan Smars - [email protected] /// <summary> /// Creates a rounded rectangle with the specified width and height. /// </summary> /// <param name="width">The width.</param> /// <param name="height">The height.</param> /// <param name="xRadius">The rounding X radius.</param> /// <param name="yRadius">The rounding Y radius.</param> /// <param name="segments">The number of segments to subdivide the edges.</param> /// <returns></returns> public static Vertices CreateRoundedRectangle(FP width, FP height, FP xRadius, FP yRadius, int segments) { if (yRadius > height / 2 || xRadius > width / 2) { throw new Exception("Rounding amount can't be more than half the height and width respectively."); } if (segments < 0) { throw new Exception("Segments must be zero or more."); } //We need at least 8 vertices to create a rounded rectangle Debug.Assert(Settings.MaxPolygonVertices >= 8); Vertices vertices = new Vertices(); if (segments == 0) { vertices.Add(new FPVector2(width * .5f - xRadius, -height * .5f)); vertices.Add(new FPVector2(width * .5f, -height * .5f + yRadius)); vertices.Add(new FPVector2(width * .5f, height * .5f - yRadius)); vertices.Add(new FPVector2(width * .5f - xRadius, height * .5f)); vertices.Add(new FPVector2(-width * .5f + xRadius, height * .5f)); vertices.Add(new FPVector2(-width * .5f, height * .5f - yRadius)); vertices.Add(new FPVector2(-width * .5f, -height * .5f + yRadius)); vertices.Add(new FPVector2(-width * .5f + xRadius, -height * .5f)); } else { int numberOfEdges = (segments * 4 + 8); FP stepSize = FP.PiTimes2 / (numberOfEdges - 4); int perPhase = numberOfEdges / 4; FPVector2 posOffset = new FPVector2(width / 2 - xRadius, height / 2 - yRadius); vertices.Add(posOffset + new FPVector2(xRadius, -yRadius + yRadius)); short phase = 0; for (int i = 1; i < numberOfEdges; i++) { if (i - perPhase == 0 || i - perPhase * 3 == 0) { posOffset.x *= -1; phase--; } else if (i - perPhase * 2 == 0) { posOffset.y *= -1; phase--; } vertices.Add(posOffset + new FPVector2(xRadius * FP.Cos(stepSize * -(i + phase)), -yRadius * FP.Sin(stepSize * -(i + phase)))); } } return(vertices); }
/// <summary>
/// Activate the explosion at the specified position.
/// </summary>
/// <param name="pos">The position where the explosion happens </param>
/// <param name="radius">The explosion radius </param>
/// <param name="maxForce">The explosion force at the explosion point (then is inversely proportional to the square of the distance)</param>
/// <returns>A list of bodies and the amount of force that was applied to them.</returns>
public Dictionary <Fixture, TSVector2> Activate(TSVector2 pos, FP radius, FP maxForce)
{
AABB aabb;
aabb.LowerBound = pos + new TSVector2(-radius, -radius);
aabb.UpperBound = pos + new TSVector2(radius, radius);
Fixture[] shapes = new Fixture[MaxShapes];
// More than 5 shapes in an explosion could be possible, but still strange.
Fixture[] containedShapes = new Fixture[5];
bool exit = false;
int shapeCount = 0;
int containedShapeCount = 0;
// Query the world for overlapping shapes.
World.QueryAABB(
fixture =>
{
if (fixture.TestPoint(ref pos))
{
if (IgnoreWhenInsideShape)
{
exit = true;
return(false);
}
containedShapes[containedShapeCount++] = fixture;
}
else
{
shapes[shapeCount++] = fixture;
}
// Continue the query.
return(true);
}, ref aabb);
if (exit)
{
return(new Dictionary <Fixture, TSVector2>());
}
Dictionary <Fixture, TSVector2> exploded = new Dictionary <Fixture, TSVector2>(shapeCount + containedShapeCount);
// Per shape max/min angles for now.
FP[] vals = new FP[shapeCount * 2];
int valIndex = 0;
for (int i = 0; i < shapeCount; ++i)
{
PolygonShape ps;
CircleShape cs = shapes[i].Shape as CircleShape;
if (cs != null)
{
// We create a "diamond" approximation of the circle
Vertices v = new Vertices();
TSVector2 vec = TSVector2.zero + new TSVector2(cs.Radius, 0);
v.Add(vec);
vec = TSVector2.zero + new TSVector2(0, cs.Radius);
v.Add(vec);
vec = TSVector2.zero + new TSVector2(-cs.Radius, cs.Radius);
v.Add(vec);
vec = TSVector2.zero + new TSVector2(0, -cs.Radius);
v.Add(vec);
ps = new PolygonShape(v, 0);
}
else
{
ps = shapes[i].Shape as PolygonShape;
}
if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
{
TSVector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos;
FP angleToCentroid = FP.Atan2(toCentroid.y, toCentroid.x);
FP min = FP.MaxValue;
FP max = FP.MinValue;
FP minAbsolute = 0.0f;
FP maxAbsolute = 0.0f;
for (int j = 0; j < ps.Vertices.Count; ++j)
{
TSVector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
FP newAngle = FP.Atan2(toVertex.y, toVertex.x);
FP diff = (newAngle - angleToCentroid);
diff = (diff - FP.Pi) % (2 * FP.Pi);
// the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
if (diff < 0.0f)
{
diff += 2 * FP.Pi; // correction for not handling negs
}
diff -= FP.Pi;
if (FP.Abs(diff) > FP.Pi)
{
continue; // Something's wrong, point not in shape but exists angle diff > 180
}
if (diff > max)
{
max = diff;
maxAbsolute = newAngle;
}
if (diff < min)
{
min = diff;
minAbsolute = newAngle;
}
}
vals[valIndex] = minAbsolute;
++valIndex;
vals[valIndex] = maxAbsolute;
++valIndex;
}
}
Array.Sort(vals, 0, valIndex, _rdc);
_data.Clear();
bool rayMissed = true;
for (int i = 0; i < valIndex; ++i)
{
Fixture fixture = null;
FP midpt;
int iplus = (i == valIndex - 1 ? 0 : i + 1);
if (vals[i] == vals[iplus])
{
continue;
}
if (i == valIndex - 1)
{
// the single edgecase
midpt = (vals[0] + FP.PiTimes2 + vals[i]);
}
else
{
midpt = (vals[i + 1] + vals[i]);
}
midpt = midpt / 2;
TSVector2 p1 = pos;
TSVector2 p2 = radius * new TSVector2(FP.Cos(midpt), FP.Sin(midpt)) + pos;
// RaycastOne
bool hitClosest = false;
World.RayCast((f, p, n, fr) =>
{
Body body = f.Body;
if (!IsActiveOn(body))
{
return(0);
}
hitClosest = true;
fixture = f;
return(fr);
}, p1, p2);
//draws radius points
if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic))
{
if ((_data.Any()) && (_data.Last().Body == fixture.Body) && (!rayMissed))
{
int laPos = _data.Count - 1;
ShapeData la = _data[laPos];
la.Max = vals[iplus];
_data[laPos] = la;
}
else
{
// make new
ShapeData d;
d.Body = fixture.Body;
d.Min = vals[i];
d.Max = vals[iplus];
_data.Add(d);
}
if ((_data.Count > 1) &&
(i == valIndex - 1) &&
(_data.Last().Body == _data.First().Body) &&
(_data.Last().Max == _data.First().Min))
{
ShapeData fi = _data[0];
fi.Min = _data.Last().Min;
_data.RemoveAt(_data.Count - 1);
_data[0] = fi;
while (_data.First().Min >= _data.First().Max)
{
fi.Min -= FP.PiTimes2;
_data[0] = fi;
}
}
int lastPos = _data.Count - 1;
ShapeData last = _data[lastPos];
while ((_data.Count > 0) &&
(_data.Last().Min >= _data.Last().Max)) // just making sure min<max
{
last.Min = _data.Last().Min - FP.PiTimes2;
_data[lastPos] = last;
}
rayMissed = false;
}
else
{
rayMissed = true; // raycast did not find a shape
}
}
for (int i = 0; i < _data.Count; ++i)
{
if (!IsActiveOn(_data[i].Body))
{
continue;
}
FP arclen = _data[i].Max - _data[i].Min;
FP first = TSMath.Min(MaxEdgeOffset, EdgeRatio * arclen);
int insertedRays = FP.Ceiling((((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle)).AsInt();
if (insertedRays < 0)
{
insertedRays = 0;
}
FP offset = (arclen - first * 2.0f) / ((FP)MinRays + insertedRays - 1);
//Note: This loop can go into infinite as it operates on FPs.
//Added FPEquals with a large epsilon.
for (FP j = _data[i].Min + first;
j < _data[i].Max || MathUtils.FPEquals(j, _data[i].Max, 0.0001f);
j += offset)
{
TSVector2 p1 = pos;
TSVector2 p2 = pos + radius * new TSVector2(FP.Cos(j), FP.Sin(j));
TSVector2 hitpoint = TSVector2.zero;
FP minlambda = FP.MaxValue;
List <Fixture> fl = _data[i].Body.FixtureList;
for (int x = 0; x < fl.Count; x++)
{
Fixture f = fl[x];
RayCastInput ri;
ri.Point1 = p1;
ri.Point2 = p2;
ri.MaxFraction = 50f;
RayCastOutput ro;
if (f.RayCast(out ro, ref ri, 0))
{
if (minlambda > ro.Fraction)
{
minlambda = ro.Fraction;
hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
}
}
// the force that is to be applied for this particular ray.
// offset is angular coverage. lambda*length of segment is distance.
FP impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / FP.Pi * (1.0f - SyncFrame.TSMath.Min(FP.One, minlambda));
// We Apply the impulse!!!
TSVector2 vectImp = TSVector2.Dot(impulse * new TSVector2(FP.Cos(j), FP.Sin(j)), -ro.Normal) * new TSVector2(FP.Cos(j), FP.Sin(j));
_data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);
// We gather the fixtures for returning them
if (exploded.ContainsKey(f))
{
exploded[f] += vectImp;
}
else
{
exploded.Add(f, vectImp);
}
if (minlambda > 1.0f)
{
hitpoint = p2;
}
}
}
}
// We check contained shapes
for (int i = 0; i < containedShapeCount; ++i)
{
Fixture fix = containedShapes[i];
if (!IsActiveOn(fix.Body))
{
continue;
}
FP impulse = MinRays * maxForce * 180.0f / FP.Pi;
TSVector2 hitPoint;
CircleShape circShape = fix.Shape as CircleShape;
if (circShape != null)
{
hitPoint = fix.Body.GetWorldPoint(circShape.Position);
}
else
{
PolygonShape shape = fix.Shape as PolygonShape;
hitPoint = fix.Body.GetWorldPoint(shape.MassData.Centroid);
}
TSVector2 vectImp = impulse * (hitPoint - pos);
fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
if (!exploded.ContainsKey(fix))
{
exploded.Add(fix, vectImp);
}
}
return(exploded);
}
