/// <summary>
/// Creates a new instance of the TransformedShape struct.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="orientation">The orientation this shape should have.</param>
/// <param name="position">The position this shape should have.</param>
public TransformedShape(Shape shape, JMatrix orientation, JVector position)
{
this.position = position;
this.orientation = orientation;
JMatrix.Transpose(ref orientation, out invOrientation);
this.shape = shape;
}
/// <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);
}
}
}
/// <summary>
/// Creates a new instance of the TransformedShape struct.
/// </summary>
/// <param name="shape">The shape.</param>
/// <param name="orientation">The orientation this shape should have.</param>
/// <param name="position">The position this shape should have.</param>
public TransformedShape(Shape shape, JMatrix orientation, JVector position)
{
this.position = position;
this.orientation = orientation;
JMatrix.Transpose(ref orientation, out invOrientation);
this.shape = shape;
this.boundingBox = new JBBox();
UpdateBoundingBox();
}
protected override void fireDown()
{
RigidBody body; JVector normal; float frac;
bool result = Scene.world.CollisionSystem.Raycast(GenericMethods.FromOpenTKVector(Position), GenericMethods.FromOpenTKVector(PointingDirection),
raycastCallback, out body, out normal, out frac);
Vector4 gpos = new Vector4(Position + PointingDirection * frac, 1);
JVector hitCoords = GenericMethods.FromOpenTKVector(gpos.Xyz);
weaponLocalHitCoords = GenericMethods.Mult(gpos, Matrix4.Invert(weaponModel.ModelMatrix)).Xyz;
arcModel.Orientation2 = Matrix4.CreateTranslation(weaponLocalHitCoords - new Vector3(0, 0, -5));
muzzleModel.isVisible = true;
if (result && body.Tag != null)
{
PhysModel curMod = (PhysModel)body.Tag;
if (curMod.grabable)
{
arcModel.isVisible = true;
grabDist = frac;
selectedBody = body;
selectedMod = (PhysModel)body.Tag;
selectedMod.selected = 1;
selectedMod.Forceupdate = true;
Matrix4 localMaker = Matrix4.Invert(Matrix4.Mult(selectedMod.Orientation, selectedMod.ModelMatrix));
modelLocalHitCoords = GenericMethods.Mult(gpos, localMaker);
if (body.IsStatic)
{
selModRelPos = body.Position - hitCoords;
}
else
{
JVector lanchor = hitCoords - body.Position;
lanchor = JVector.Transform(lanchor, JMatrix.Transpose(body.Orientation));
body.IsActive = true;
//body.SetMassProperties(JMatrix.Identity, 0.1f, false);
//body.AffectedByGravity = false;
mConst = new Jitter.Dynamics.Constraints.SingleBody.PointOnPoint(body, lanchor);
mConst.Softness = 0.02f;
mConst.BiasFactor = 0.1f;
Scene.world.AddConstraint(mConst);
}
}
}
}
/// <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);
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
effectiveMass = body1.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
JMatrix.Inverse(ref effectiveMass, out effectiveMass);
JMatrix q = JMatrix.Transpose(orientation) * body1.orientation;
JVector axis;
float x = q.M32 - q.M23;
float y = q.M13 - q.M31;
float z = q.M21 - q.M12;
float r = JMath.Sqrt(x * x + y * y + z * z);
float t = q.M11 + q.M22 + q.M33;
float angle = (float)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);
}
}
/// <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);
}
protected override void Update(GameTime gameTime)
{
padState = GamePad.GetState(PlayerIndex.One);
keyState = Keyboard.GetState();
mouseState = Mouse.GetState();
if (PressedOnce(Keys.Escape, Buttons.Back))
{
Exit();
}
if (PressedOnce(Keys.M, Buttons.A))
{
multithread = !multithread;
}
if (PressedOnce(Keys.P, Buttons.A))
{
var e = World.RigidBodies.GetEnumerator();
e.MoveNext(); e.MoveNext(); e.MoveNext();
e.MoveNext(); e.MoveNext(); e.MoveNext();
e.MoveNext(); e.MoveNext(); e.MoveNext();
(e.Current as RigidBody).IsStatic = true;
e.MoveNext();
(e.Current as RigidBody).IsStatic = true;
}
if ((mouseState.LeftButton == ButtonState.Pressed &&
mousePreviousState.LeftButton == ButtonState.Released) ||
(padState.IsButtonDown(Buttons.RightThumbstickDown) &&
gamePadPreviousState.IsButtonUp(Buttons.RightThumbstickUp)))
{
var ray = Conversion.ToJitterVector(RayTo(mouseState.X, mouseState.Y));
var camp = Conversion.ToJitterVector(Camera.Position);
ray = JVector.Normalize(ray) * 100;
bool result = World.CollisionSystem.Raycast(camp, ray, RaycastCallback, out grabBody, out hitNormal, out float fraction);
if (result)
{
hitPoint = camp + (fraction * ray);
if (grabConstraint != null)
{
World.RemoveConstraint(grabConstraint);
}
var lanchor = hitPoint - grabBody.Position;
lanchor = JVector.Transform(lanchor, JMatrix.Transpose(grabBody.Orientation));
grabConstraint = new SingleBodyConstraints.PointOnPoint(grabBody, lanchor)
{
Softness = 0.01f,
BiasFactor = 0.1f
};
World.AddConstraint(grabConstraint);
hitDistance = (Conversion.ToXNAVector(hitPoint) - Camera.Position).Length();
scrollWheel = mouseState.ScrollWheelValue;
grabConstraint.Anchor = hitPoint;
}
}
if (mouseState.LeftButton == ButtonState.Pressed || padState.IsButtonDown(Buttons.RightThumbstickDown))
{
hitDistance += (mouseState.ScrollWheelValue - scrollWheel) * 0.01f;
scrollWheel = mouseState.ScrollWheelValue;
if (grabBody != null)
{
var ray = RayTo(mouseState.X, mouseState.Y); ray.Normalize();
grabConstraint.Anchor = Conversion.ToJitterVector(Camera.Position + (ray * hitDistance));
grabBody.IsActive = true;
if (!grabBody.IsStatic)
{
grabBody.LinearVelocity *= 0.98f;
grabBody.AngularVelocity *= 0.98f;
}
}
}
else
{
if (grabConstraint != null)
{
World.RemoveConstraint(grabConstraint);
}
grabBody = null;
grabConstraint = null;
}
if (PressedOnce(Keys.Space, Buttons.B))
{
SpawnRandomPrimitive(Conversion.ToJitterVector(Camera.Position),
Conversion.ToJitterVector((Camera.Target - Camera.Position) * 40.0f));
}
if (PressedOnce(Keys.Add, Buttons.X))
{
DestroyCurrentScene();
currentScene++;
currentScene = currentScene % PhysicScenes.Count;
PhysicScenes[currentScene].Build();
}
if (PressedOnce(Keys.Subtract, Buttons.Y))
{
DestroyCurrentScene();
currentScene += PhysicScenes.Count - 1;
currentScene = currentScene % PhysicScenes.Count;
PhysicScenes[currentScene].Build();
}
UpdateDisplayText(gameTime);
float step = (float)gameTime.ElapsedGameTime.TotalSeconds;
if (step > 1.0f / 100.0f)
{
step = 1.0f / 100.0f;
}
World.Step(step, multithread);
gamePadPreviousState = padState;
keyboardPreviousState = keyState;
mousePreviousState = mouseState;
base.Update(gameTime);
}
protected override void Update(GameTime gameTime)
{
if (codeFormVisible)
{
return;
}
KeyboardState keyState = Keyboard.GetState();
MouseState mouseState = Mouse.GetState();
if (keyState.IsKeyDown(Keys.Escape))
{
this.Exit();
}
bool leftHold = (mouseState.LeftButton == ButtonState.Pressed);
bool spaceHold = (keyState.IsKeyDown(Keys.Space));
bool enterHold = (keyState.IsKeyDown(Keys.Enter));
bool mHold = (keyState.IsKeyDown(Keys.M));
#region Turn multithreading on/off
if (mHold)
{
if (!mClicked)
{
multithread = !multithread; mClicked = true;
}
}
#endregion
#region Object drag & drop
if (leftHold && !leftClicked)
{
JVector ray = Conversion.ToJitterVector(RayTo(mouseState.X, mouseState.Y)); ray.Normalize();
JVector camp = Conversion.ToJitterVector(Camera.Position);
float fraction;
bool result = world.CollisionSystem.Raycast(camp, ray * 100, RaycastCallback, out resBody, out hitNormal, out fraction);
if (result)
{
hitPoint = camp + fraction * ray * 100;
if (wp != null)
{
world.RemoveConstraint(wp);
}
JVector lanchor = hitPoint - resBody.Position;
lanchor = JVector.Transform(lanchor, JMatrix.Transpose(resBody.Orientation));
wp = new WorldPointConstraint(resBody, lanchor);
world.AddConstraint(wp);
hitDistance = (Conversion.ToXNAVector(hitPoint) - Camera.Position).Length();
scrollWheel = mouseState.ScrollWheelValue;
wp.Anchor = hitPoint;
}
leftClicked = true;
}
if (mouseState.LeftButton == ButtonState.Pressed)
{
hitDistance += (mouseState.ScrollWheelValue - scrollWheel) * 0.001f;
scrollWheel = mouseState.ScrollWheelValue;
if (resBody != null)
{
Vector3 ray = RayTo(mouseState.X, mouseState.Y); ray.Normalize();
wp.Anchor = Conversion.ToJitterVector(Camera.Position + ray * hitDistance);
}
}
else
{
resBody = null;
if (wp != null)
{
world.RemoveConstraint(wp);
}
}
#endregion
#region Show code form
if (enterHold && !enterClicked && gameTime.TotalGameTime.TotalSeconds > 0.1f)
{
display.DisplayText[5] = string.Empty;
System.Windows.Forms.Form form =
(System.Windows.Forms.Form)System.Windows.Forms.Control.FromHandle(this.Window.Handle);
codeFormVisible = true;
System.Windows.Forms.DialogResult result = codeForm.ShowDialog(form);
codeFormVisible = false;
enterClicked = true;
}
#endregion
#region Spawn random primitive
if (spaceHold && !spaceClicked)
{
int rndn = random.Next(6);
RigidBody body;
if (rndn == 0)
{
body = new RigidBody(new ConeShape((float)random.Next(5, 50) / 20.0f, (float)random.Next(10, 20) / 20.0f));
}
else if (rndn == 1)
{
body = new RigidBody(new BoxShape(new JVector(
(float)random.Next(10, 30) / 20.0f,
(float)random.Next(10, 30) / 20.0f, (float)random.Next(10, 30) / 20.0f)));
}
else if (rndn == 2)
{
body = new RigidBody(new SphereShape((float)random.Next(30, 100) / 100.0f));
}
else if (rndn == 3)
{
body = new RigidBody(new CylinderShape(1.0f, 0.5f));
}
else if (rndn == 4)
{
body = new RigidBody(new CapsuleShape(1.0f, 0.5f));
}
else
{
Shape b1 = new BoxShape(new JVector(3, 1, 1));
Shape b2 = new BoxShape(new JVector(1, 1, 3));
Shape b3 = new CylinderShape(2.0f, 0.5f);
CompoundShape.TransformedShape t1 = new CompoundShape.TransformedShape(b1, JMatrix.Identity, JVector.Zero);
CompoundShape.TransformedShape t2 = new CompoundShape.TransformedShape(b2, JMatrix.Identity, JVector.Zero);
CompoundShape.TransformedShape t3 = new CompoundShape.TransformedShape(b3, JMatrix.Identity, new JVector(0, 0, 0));
CompoundShape ms = new CompoundShape(new CompoundShape.TransformedShape[3] {
t1, t2, t3
});
body = new RigidBody(ms);
}
world.AddBody(body);
body.Position = Conversion.ToJitterVector(Camera.Position);
body.LinearVelocity = Conversion.ToJitterVector((Camera.Target - Camera.Position) * 40.0f);
body.Update();
spaceClicked = true;
}
#endregion
spaceClicked = spaceHold;
leftClicked = leftHold;
enterClicked = enterHold;
mClicked = mHold;
int contactCount = 0;
foreach (Arbiter ar in world.ArbiterMap.Values)
{
contactCount += ar.ContactList.Count;
}
display.DisplayText[0] = "Arbitercount: " + world.ArbiterMap.Values.Count.ToString() + ";" + " Contactcount: " + contactCount.ToString();
display.DisplayText[2] = "Bodycount: " + world.RigidBodies.Count;
display.DisplayText[3] = (multithread) ? "Multithreaded" : "Single Threaded";
float elapsedTime = (float)gameTime.ElapsedGameTime.TotalSeconds;
if (elapsedTime > 1.0f / 100.0f)
{
elapsedTime = 1.0f / 100.0f;
}
world.Step(elapsedTime, multithread);
base.Update(gameTime);
}