private void AddBodyToDrawList(RigidBody rb)
{
if (rb.Tag is BodyTag && ((BodyTag)rb.Tag) == BodyTag.DontDrawMe)
{
return;
}
bool isCompoundShape = rb.Shape is CompoundShape;
if (!isCompoundShape)
{
AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position);
}
else
{
var cShape = rb.Shape as CompoundShape;
var orientation = rb.Orientation;
var position = rb.Position;
foreach (var ts in cShape.Shapes)
{
var pos = ts.Position;
var ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
AddShapeToDrawList(ts.Shape, ori, pos);
}
}
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (isParticle)
{
this.inertia = JMatrix.Zero;
this.invInertia = this.invInertiaWorld = JMatrix.Zero;
this.invOrientation = this.orientation = JMatrix.Identity;
this.boundingBox = shape.boundingBox;
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
angularVelocity.MakeZero();
}
else
{
// Given: Orientation, Inertia
JMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
if (!isStatic)
{
JMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
JMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
}
private void DrawBody(RigidBody rb)
{
if (rb.Tag is bool && ((bool)rb.Tag) == true)
{
return;
}
bool isCompoundShape = (rb.Shape is CompoundShape);
if (!isCompoundShape)
{
DrawShape(rb.Shape, rb.Orientation, rb.Position);
}
else
{
CompoundShape cShape = rb.Shape as CompoundShape;
JMatrix orientation = rb.Orientation;
JVector position = rb.Position;
foreach (var ts in cShape.Shapes)
{
JVector pos = ts.Position;
JMatrix ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
DrawShape(ts.Shape, ori, pos);
}
}
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
// Given: Orientation, Inertia
JMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
if (!isStatic)
{
JMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
JMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(double timestep)
{
effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
JMatrix.Inverse(ref effectiveMass, out effectiveMass);
JMatrix orientationDifference;
JMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference);
JMatrix.Transpose(ref orientationDifference, out orientationDifference);
JMatrix q = orientationDifference * body2.invOrientation * body1.orientation;
JVector axis;
double x = q.M32 - q.M23;
double y = q.M13 - q.M31;
double z = q.M21 - q.M12;
double r = JMath.Sqrt(x * x + y * y + z * z);
double t = q.M11 + q.M22 + q.M33;
double angle = (double)Math.Atan2(r, t - 1);
axis = new JVector(x, y, z) * angle;
if (r != 0.0f)
{
axis = axis * (1.0f / r);
}
bias = axis * biasFactor * (-1.0f / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += JVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.angularVelocity += JVector.Transform(-1.0f * accumulatedImpulse, body2.invInertiaWorld);
}
}
protected override void rotate()
{
if (gameInput.keyboard[Key.Q])
{
if (selectedMod != null && mConst == null)
{
JMatrix rotMatA = JMatrix.CreateRotationY(gameInput.move.X * cameraRotSpeed);
JMatrix rotMatB = GenericMethods.FromOpenTKMatrix(Matrix4.CreateFromAxisAngle(-Parent.rightVec, gameInput.move.Y * cameraRotSpeed));
JMatrix rotMatFinal = JMatrix.Multiply(rotMatA, rotMatB);
selectedBody.Orientation = JMatrix.Multiply(selectedBody.Orientation, rotMatFinal);
}
}
else
{
base.rotate();
}
}
public CompoundShape(List <TransformedShape> shapes)
{
this.shapes = shapes.ToArray();
var jshapes = new List <Jitter.Collision.Shapes.CompoundShape.TransformedShape>();
// Remove original reference model from scene graph
foreach (var shape in shapes)
{
// TODO: Decompose - for now we only have rotations and translations so this should work
JMatrix orientation = Util.ToJitter(shape.Transform);
Vector3 p = shape.Transform.GetColumn3(3);
JVector position = Util.ToJitter(p);
Jitter.Collision.Shapes.Shape oldShape = shape.Shape.shape;
Jitter.Collision.Shapes.CompoundShape compound = oldShape as Jitter.Collision.Shapes.CompoundShape;
if (compound != null)
{
foreach (var part in compound.Shapes)
{
var newShape = new Jitter.Collision.Shapes.CompoundShape.TransformedShape(
part.Shape,
JMatrix.Multiply(orientation, part.Orientation),
position + JVector.Transform(part.Position, orientation)
);
jshapes.Add(newShape);
}
}
else
{
var newShape = new Jitter.Collision.Shapes.CompoundShape.TransformedShape(
oldShape,
orientation,
position
);
jshapes.Add(newShape);
}
}
compoundShape = new Jitter.Collision.Shapes.CompoundShape(jshapes);
Matrix4 vertexOffset = Matrix4.CreateTranslation(compoundShape.Shift.X, compoundShape.Shift.Y, compoundShape.Shift.Z);
Matrix4 modelOffset = Matrix4.CreateTranslation(-compoundShape.Shift.X, -compoundShape.Shift.Y, -compoundShape.Shift.Z);
}
/// <summary>
/// Add rigid-body to the draw-list to draw on the screen.
/// Only rigid-bodies with "Tag" = "DrawMe" are drawn.
/// </summary>
/// <param name="rb">Rigidbody</param>
private void AddBodyToDrawList(RigidBody rb)
{
if (rb.Tag is BodyTag && (BodyTag)rb.Tag == BodyTag.DontDrawMe)
{
return;
}
Collider c = rb as Collider;
if (c?.GameObject.tag == ObjectTag.Ground)
{
return;
}
bool isCompoundShape = (rb.Shape is CompoundShape);
if (!isCompoundShape)
{
AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position);
}
else
{
CompoundShape cShape = rb.Shape as CompoundShape;
JMatrix orientation = rb.Orientation;
JVector position = rb.Position;
foreach (var ts in cShape.Shapes)
{
JVector pos = ts.Position;
JMatrix ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
AddShapeToDrawList(ts.Shape, ori, pos);
}
}
}
private void AddBodyToDrawList(RigidBody rb)
{
if (rb.Tag is BodyTag && ((BodyTag)rb.Tag) == BodyTag.DontDrawMe)
{
return;
}
bool isCompoundShape = (rb.Shape is CompoundShape);
if (!isCompoundShape)
{
//GraphicsDevice.BlendState = BlendState.Opaque;
//GraphicsDevice.DepthStencilState = DepthStencilState.Default;
AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position);
}
else
{
//GraphicsDevice.BlendState = BlendState.Opaque;
//GraphicsDevice.DepthStencilState = DepthStencilState.None;
CompoundShape cShape = rb.Shape as CompoundShape;
JMatrix orientation = rb.Orientation;
JVector position = rb.Position;
foreach (var ts in cShape.Shapes)
{
JVector pos = ts.Position;
JMatrix ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
AddShapeToDrawList(ts.Shape, ori, pos);
}
}
}
/// <summary>
/// Calculates the inertia of the shape relative to the center of mass.
/// </summary>
/// <param name="shape"></param>
/// <param name="centerOfMass"></param>
/// <param name="inertia">Returns the inertia relative to the center of mass, not to the origin</param>
/// <returns></returns>
#region public static float CalculateMassInertia(Shape shape, out JVector centerOfMass, out JMatrix inertia)
public static float CalculateMassInertia(Shape shape, out JVector centerOfMass,
out JMatrix inertia)
{
float mass = 0.0f;
centerOfMass = JVector.Zero; inertia = JMatrix.Zero;
if (shape is Multishape)
{
throw new ArgumentException("Can't calculate inertia of multishapes.", "shape");
}
// build a triangle hull around the shape
List <JVector> hullTriangles = new List <JVector>();
shape.MakeHull(ref hullTriangles, 3);
// create inertia of tetrahedron with vertices at
// (0,0,0) (1,0,0) (0,1,0) (0,0,1)
float a = 1.0f / 60.0f, b = 1.0f / 120.0f;
JMatrix C = new JMatrix(a, b, b, b, a, b, b, b, a);
for (int i = 0; i < hullTriangles.Count; i += 3)
{
JVector column0 = hullTriangles[i + 0];
JVector column1 = hullTriangles[i + 1];
JVector column2 = hullTriangles[i + 2];
JMatrix A = new JMatrix(column0.X, column1.X, column2.X,
column0.Y, column1.Y, column2.Y,
column0.Z, column1.Z, column2.Z);
float detA = A.Determinant();
// now transform this canonical tetrahedron to the target tetrahedron
// inertia by a linear transformation A
JMatrix tetrahedronInertia = JMatrix.Multiply(A * C * JMatrix.Transpose(A), detA);
JVector tetrahedronCOM = (1.0f / 4.0f) * (hullTriangles[i + 0] + hullTriangles[i + 1] + hullTriangles[i + 2]);
float tetrahedronMass = (1.0f / 6.0f) * detA;
inertia += tetrahedronInertia;
centerOfMass += tetrahedronMass * tetrahedronCOM;
mass += tetrahedronMass;
}
inertia = JMatrix.Multiply(JMatrix.Identity, inertia.Trace()) - inertia;
centerOfMass = centerOfMass * (1.0f / mass);
float x = centerOfMass.X;
float y = centerOfMass.Y;
float z = centerOfMass.Z;
// now translate the inertia by the center of mass
JMatrix t = new JMatrix(
-mass * (y * y + z * z), mass * x * y, mass * x * z,
mass * y * x, -mass * (z * z + x * x), mass * y * z,
mass * z * x, mass * z * y, -mass * (x * x + y * y));
JMatrix.Add(ref inertia, ref t, out inertia);
return(mass);
}