public static int[] Build(List <TSVector> pointCloud, TSConvexHull.Approximation factor)
{
List <int> list = new List <int>();
for (int i = 0; i < (int)factor; i++)
{
FP x = TSMath.Pi / (factor - (TSConvexHull.Approximation) 1) * i;
FP x2 = FP.Sin(x);
FP y = FP.Cos(x);
for (int j = 0; j < (int)factor; j++)
{
FP x3 = 2 * FP.One * TSMath.Pi / (int)factor * j - TSMath.Pi;
FP y2 = FP.Sin(x3);
FP y3 = FP.Cos(x3);
TSVector tSVector = new TSVector(x2 * y3, y, x2 * y2);
int item = TSConvexHull.FindExtremePoint(pointCloud, ref tSVector);
list.Add(item);
}
}
list.Sort();
for (int k = 1; k < list.Count; k++)
{
bool flag = list[k - 1] == list[k];
if (flag)
{
list.RemoveAt(k - 1);
k--;
}
}
return(list.ToArray());
}
public static TSQuaternion AngleAxis(FP angle, TSVector axis)
{
axis *= FP.Deg2Rad;
axis.Normalize();
FP fP = angle * FP.Deg2Rad * FP.Half;
FP fP2 = FP.Sin(fP);
TSQuaternion result;
result.x = axis.x * fP2;
result.y = axis.y * fP2;
result.z = axis.z * fP2;
result.w = FP.Cos(fP);
return(result);
}
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 void CreateRotationX(FP radians, out TSMatrix result)
{
FP fP = FP.Cos(radians);
FP fP2 = FP.Sin(radians);
result.M11 = FP.One;
result.M12 = FP.Zero;
result.M13 = FP.Zero;
result.M21 = FP.Zero;
result.M22 = fP;
result.M23 = fP2;
result.M31 = FP.Zero;
result.M32 = -fP2;
result.M33 = fP;
}
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 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 CreateRotationY(FP radians)
{
FP fP = FP.Cos(radians);
FP fP2 = FP.Sin(radians);
TSMatrix result;
result.M11 = fP;
result.M12 = FP.Zero;
result.M13 = -fP2;
result.M21 = FP.Zero;
result.M22 = FP.One;
result.M23 = FP.Zero;
result.M31 = fP2;
result.M32 = FP.Zero;
result.M33 = fP;
return(result);
}
public static void CreateFromYawPitchRoll(FP yaw, FP pitch, FP roll, out TSQuaternion result)
{
FP fP = roll * FP.Half;
FP fP2 = FP.Sin(fP);
FP fP3 = FP.Cos(fP);
FP fP4 = pitch * FP.Half;
FP fP5 = FP.Sin(fP4);
FP fP6 = FP.Cos(fP4);
FP fP7 = yaw * FP.Half;
FP fP8 = FP.Sin(fP7);
FP fP9 = FP.Cos(fP7);
result.x = fP9 * fP5 * fP3 + fP8 * fP6 * fP2;
result.y = fP8 * fP6 * fP3 - fP9 * fP5 * fP2;
result.z = fP9 * fP6 * fP2 - fP8 * fP5 * fP3;
result.w = fP9 * fP6 * fP3 + fP8 * fP5 * fP2;
}
public static void CreateFromAxisAngle(ref TSVector3 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));
}
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 x2 = FP.Sin(angle);
FP x3 = FP.Cos(angle);
FP fP = x * x;
FP fP2 = y * y;
FP fP3 = z * z;
FP fP4 = x * y;
FP fP5 = x * z;
FP fP6 = y * z;
result.M11 = fP + x3 * (FP.One - fP);
result.M12 = fP4 - x3 * fP4 + x2 * z;
result.M13 = fP5 - x3 * fP5 - x2 * y;
result.M21 = fP4 - x3 * fP4 - x2 * z;
result.M22 = fP2 + x3 * (FP.One - fP2);
result.M23 = fP6 - x3 * fP6 + x2 * x;
result.M31 = fP5 - x3 * fP5 + x2 * y;
result.M32 = fP6 - x3 * fP6 - x2 * x;
result.M33 = fP3 + x3 * (FP.One - fP3);
}
/// <summary>
/// Returns the cosine of value.
/// </summary>
public static FP Cos(FP value)
{
return(FP.Cos(value));
}
private void IntegrateCallback(object obj)
{
RigidBody rigidBody = obj as RigidBody;
TSVector tSVector;
TSVector.Multiply(ref rigidBody.linearVelocity, this.timestep, out tSVector);
TSVector.Add(ref tSVector, ref rigidBody.position, out rigidBody.position);
bool flag = !rigidBody.isParticle;
if (flag)
{
FP magnitude = rigidBody.angularVelocity.magnitude;
bool flag2 = magnitude < FP.EN3;
TSVector tSVector2;
if (flag2)
{
TSVector.Multiply(ref rigidBody.angularVelocity, FP.Half * this.timestep - this.timestep * this.timestep * this.timestep * (2082 * FP.EN6) * magnitude * magnitude, out tSVector2);
}
else
{
TSVector.Multiply(ref rigidBody.angularVelocity, FP.Sin(FP.Half * magnitude * this.timestep) / magnitude, out tSVector2);
}
TSQuaternion tSQuaternion = new TSQuaternion(tSVector2.x, tSVector2.y, tSVector2.z, FP.Cos(magnitude * this.timestep * FP.Half));
TSQuaternion tSQuaternion2;
TSQuaternion.CreateFromMatrix(ref rigidBody.orientation, out tSQuaternion2);
TSQuaternion.Multiply(ref tSQuaternion, ref tSQuaternion2, out tSQuaternion);
tSQuaternion.Normalize();
TSMatrix.CreateFromQuaternion(ref tSQuaternion, out rigidBody.orientation);
}
bool flag3 = (rigidBody.Damping & RigidBody.DampingType.Linear) > RigidBody.DampingType.None;
if (flag3)
{
TSVector.Multiply(ref rigidBody.linearVelocity, this.currentLinearDampFactor, out rigidBody.linearVelocity);
}
bool flag4 = (rigidBody.Damping & RigidBody.DampingType.Angular) > RigidBody.DampingType.None;
if (flag4)
{
TSVector.Multiply(ref rigidBody.angularVelocity, this.currentAngularDampFactor, out rigidBody.angularVelocity);
}
rigidBody.Update();
bool flag5 = this.CollisionSystem.EnableSpeculativeContacts || rigidBody.EnableSpeculativeContacts;
if (flag5)
{
rigidBody.SweptExpandBoundingBox(this.timestep);
}
}
