/// <summary>
/// The points in world space gets recalculated by transforming the
/// local coordinates. Also new penetration depth is estimated.
/// </summary>
public void UpdatePosition()
{
if (body1IsParticle)
{
JVector.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
JMatrix xForm = JMatrix.CreateRotationZ(body1.invOrientation);
JVector.Transform(ref realRelPos1, ref xForm, out p1);
JVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsParticle)
{
JVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
JMatrix xForm = JMatrix.CreateRotationZ(body2.invOrientation);
JVector.Transform(ref realRelPos2, ref xForm, out p2);
JVector.Add(ref p2, ref body2.position, out p2);
}
JVector dist; JVector.Subtract(ref p1, ref p2, out dist);
penetration = JVector.Dot(ref dist, ref normal);
}
/// <summary>
/// The points in world space gets recalculated by transforming the
/// local coordinates. Also new penetration depth is estimated.
/// </summary>
public void UpdatePosition()
{
if (body1IsMassPoint)
{
JVector.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
JMatrix o1 = JMatrix.CreateRotationZ(body1.orientation);
JVector.Transform(ref realRelPos1, ref o1, out p1);
JVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsMassPoint)
{
JVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
JMatrix o2 = JMatrix.CreateRotationZ(body2.orientation);
JVector.Transform(ref realRelPos2, ref o2, out p2);
JVector.Add(ref p2, ref body2.position, out p2);
}
JVector dist; JVector.Subtract(ref p1, ref p2, out dist);
penetration = JVector.Dot(ref dist, ref normal);
}
/// <summary>
/// Initializes a contact.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="point1">The collision point in worldspace</param>
/// <param name="point2">The collision point in worldspace</param>
/// <param name="n">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void Initialize(RigidBody body1, RigidBody body2, ref JVector point1, ref JVector point2, ref JVector n,
float penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1; this.body2 = body2;
this.normal = n; normal.Normalize();
this.p1 = point1; this.p2 = point2;
this.newContact = newContact;
JVector.Subtract(ref p1, ref body1.position, out relativePos1);
JVector.Subtract(ref p2, ref body2.position, out relativePos2);
JMatrix o1 = JMatrix.CreateRotationZ(body1.invOrientation);
JMatrix o2 = JMatrix.CreateRotationZ(body2.invOrientation);
JVector.Transform(ref relativePos1, ref o1, out realRelPos1);
JVector.Transform(ref relativePos2, ref o2, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
accumulatedNormalImpulse = 0.0f;
accumulatedTangentImpulse = 0.0f;
lostSpeculativeBounce = 0.0f;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = JMath.Max(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Max(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Max(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = JMath.Min(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Min(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Min(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / 2.0f;
dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / 2.0f;
restitution = (body1.material.restitution + body2.material.restitution) / 2.0f;
break;
}
}
this.settings = settings;
}
private static void SupportMapping(RigidBody body, Shape workingShape, ref JVector direction, out JVector result)
{
JMatrix xform = JMatrix.CreateRotationZ(-body.orientation);
JVector.Transform(ref direction, ref xform, out result);
workingShape.SupportMapping(ref result, out result);
xform = JMatrix.CreateRotationZ(body.orientation);
JVector.Transform(ref result, ref xform, out result);
JVector.Add(ref result, ref body.position, out result);
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref float orientation, out JBBox box)
{
JMatrix xForm = JMatrix.CreateRotationZ(-orientation);
JMatrix abs; JMath.Absolute(ref xForm, out abs);
JVector temp;
JVector.Transform(ref halfSize, ref abs, out temp);
box.Max = temp;
JVector.Negate(ref temp, out box.Min);
}
void DrawBox(BoxShape box, JVector pos, float orientation, Color color)
{
List <JVector> poly = new List <JVector>
{
new JVector(-box.Size.X * 0.5f, -box.Size.Y * 0.5f),
new JVector(box.Size.X * 0.5f, -box.Size.Y * 0.5f),
new JVector(box.Size.X * 0.5f, box.Size.Y * 0.5f),
new JVector(-box.Size.X * 0.5f, box.Size.Y * 0.5f)
};
DrawPoly(poly, pos, JMatrix.CreateRotationZ(orientation), color);
}
private JMatrix GetOrientation()
{
switch (Axis)
{
case AxisAlignment.PositiveX:
case AxisAlignment.NegativeX:
return(JMatrix.CreateRotationZ(JMath.PiOver2));
case AxisAlignment.PositiveY:
case AxisAlignment.NegativeY:
return(JMatrix.Identity);
case AxisAlignment.PositiveZ:
case AxisAlignment.NegativeZ:
return(JMatrix.CreateRotationX(JMath.PiOver2));
default:
throw new ArgumentOutOfRangeException();
}
}
/// <summary>
/// Uses the supportMapping to calculate the bounding box. Should be overidden
/// to make this faster.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public virtual void GetBoundingBox(ref float rotation, out JBBox box)
{
JVector vec = JVector.Zero;
JMatrix orientation = JMatrix.CreateRotationZ(rotation);
vec.Set(orientation.M11, orientation.M21);
SupportMapping(ref vec, out vec);
box.Max.X = orientation.M11 * vec.X + orientation.M21 * vec.Y;
vec.Set(orientation.M12, orientation.M22);
SupportMapping(ref vec, out vec);
box.Max.Y = orientation.M12 * vec.X + orientation.M22 * vec.Y;
vec.Set(-orientation.M11, -orientation.M21);
SupportMapping(ref vec, out vec);
box.Min.X = orientation.M11 * vec.X + orientation.M21 * vec.Y;
vec.Set(-orientation.M12, -orientation.M22);
SupportMapping(ref vec, out vec);
box.Min.Y = orientation.M12 * vec.X + orientation.M22 * vec.Y;
}
public void InitBody(ref RigidBody body, TransformComponent transform, int entity)
{
body.Shape = new BoxShape(0.2f, 0.2f, 0.2f);
body.Position = new JVector(transform.X, transform.Y, transform.Z);
body.Orientation = JMatrix.CreateRotationX(transform.RotationX) *
JMatrix.CreateRotationY(transform.RotationY) *
JMatrix.CreateRotationZ(transform.RotationZ);
body.SetMassProperties();
body.Update();
body.IsActive = true; // TODO
body.IsStatic = !OwnerApp.EntityManager.HasComponent <MovableComponent>(entity);
body.Mass = 1.0f; // TODO
body.Material = new Material()
{
Restitution = 0.2f,
StaticFriction = 0.8f,
KineticFriction = 0.8f
}; // TODO
_world.AddBody(body);
}
// this should only be called if this shape is colliding
public override void UpdateAxes(float orientation)
{
this.xAxis = JVector.Transform(JVector.Left, JMatrix.CreateRotationZ(orientation));
this.yAxis = JVector.Transform(JVector.Up, JMatrix.CreateRotationZ(orientation));
}
protected override void Update(GameTime gameTime)
{
padState = GamePad.GetState(PlayerIndex.One);
keyState = Keyboard.GetState();
mouseState = Mouse.GetState();
// let the user escape the demo
if (PressedOnce(Keys.Escape, Buttons.Back))
{
this.Exit();
}
// change threading mode
if (PressedOnce(Keys.M, Buttons.A))
{
multithread = !multithread;
}
#region drag and drop physical objects with the mouse
JVector mouseLocation = Camera.ScreenToWorldSpace(new JVector(mouseState.X, mouseState.Y));
// MouseDown
if (mouseState.LeftButton == ButtonState.Pressed &&
mousePreviousState.LeftButton == ButtonState.Released)
{
JBBox mouseBox = new JBBox(mouseLocation - new JVector(0.01f), mouseLocation + new JVector(0.01f));
World.CollisionSystem.Query((foundItem) =>
{
grabBody = foundItem as RigidBody;
// don't continue
return(false);
}, ref mouseBox);
if (grabBody != null)
{
if (grabSpring != null)
{
World.RemoveSpring(grabSpring);
}
// convert mouse coordinates to foundBody's local space
var localMouseLocation = JVector.Transform(mouseLocation - grabBody.Position, JMatrix.CreateRotationZ(-grabBody.Orientation));
grabBody.IsActive = true;
grabSpring = new FixedLinearSpring(grabBody, localMouseLocation, mouseLocation, 50 * grabBody.Mass, 15 * grabBody.Mass);
grabSpring.IsOnlyPull = true;
grabSpring.Length = 1;
World.AddSpring(grabSpring);
}
}
// MouseMove
if (mouseState.LeftButton == ButtonState.Pressed)
{
if (grabBody != null)
{
grabSpring.WorldAnchor = mouseLocation;
}
}
// MouseUp
else if (mouseState.LeftButton == ButtonState.Released && mousePreviousState.LeftButton == ButtonState.Pressed)
{
if (grabSpring != null)
{
World.RemoveSpring(grabSpring);
}
grabSpring = null;
grabBody = null;
}
#endregion
#region create random primitives
Random r = new Random();
if (PressedOnce(Keys.Space, Buttons.B))
{
Random rand = new Random();
RigidBody body = new RigidBody(new BoxShape(5.5f, 5.5f))
{
EnableDebugDraw = true,
//Position = new JVector((float)rand.NextDouble(), 0),
AngularVelocity = 0,
LinearVelocity = new JVector(0, -10),
Orientation = 0.0f,//0.001f + (float)rand.NextDouble(),
Material = new Material()
{
DynamicFriction = 1f,
StaticFriction = 1f,
Restitution = 0f,
},
Position = new JVector(0, 15),
};
World.AddBody(body);
}
#endregion
#region switch through physic scenes
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();
}
#endregion
UpdateDisplayText(gameTime);
float step = (float)gameTime.ElapsedGameTime.TotalSeconds;
if (step > 1.0f / 60.0f)
{
step = 1.0f / 60.0f;
}
World.Step(step, multithread);
// TODO - add options for variable timestep vs fixed
//World.Step((float)gameTime.ElapsedGameTime.TotalSeconds, multithread, 1f/ 100f, 1);
gamePadPreviousState = padState;
keyboardPreviousState = keyState;
mousePreviousState = mouseState;
base.Update(gameTime);
}
/*
* private ResourcePool<List<int>> potentialTriangleLists = new ResourcePool<List<int>>();
*
* private void DetectSoftSoft(SoftBody body1, SoftBody body2)
* {
* List<int> my = potentialTriangleLists.GetNew();
* List<int> other = potentialTriangleLists.GetNew();
*
* body1.dynamicTree.Query(other, my, body2.dynamicTree);
*
* for (int i = 0; i < other.Count; i++)
* {
* SoftBody.Triangle myTriangle = body1.dynamicTree.GetUserData(my[i]);
* SoftBody.Triangle otherTriangle = body2.dynamicTree.GetUserData(other[i]);
*
* JVector point, normal;
* float penetration;
* bool result;
*
* result = XenoCollide.Detect(myTriangle, otherTriangle, ref JMatrix.InternalIdentity, ref JMatrix.InternalIdentity,
* ref JVector.InternalZero, ref JVector.InternalZero, out point, out normal, out penetration);
*
* if (result)
* {
* int minIndexMy = FindNearestTrianglePoint(body1, my[i], ref point);
* int minIndexOther = FindNearestTrianglePoint(body2, other[i], ref point);
*
* if (this.RaisePassedNarrowphase(body1.points[minIndexMy], body2.points[minIndexOther],
* ref point, ref normal, penetration))
* {
* RaiseCollisionDetected(body1.points[minIndexMy],
* body2.points[minIndexOther], ref point, ref point, ref normal, penetration);
* }
* }
* }
*
* my.Clear(); other.Clear();
* potentialTriangleLists.GiveBack(my);
* potentialTriangleLists.GiveBack(other);
* }
*/
private void DetectRigidRigid(RigidBody body1, RigidBody body2)
{
// we don't support multi shapes yet!
bool b1IsMulti = false; // (body1.Shape is Multishape);
bool b2IsMulti = false; // (body2.Shape is Multishape);
bool speculative = speculativeContacts ||
(body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts);
JVector point = JVector.Zero;
JVector normal = JVector.Zero;
float penetration = 0;
if (!b1IsMulti && !b2IsMulti)
{
JVector point1 = JVector.Zero;
JVector point2 = JVector.Zero;
JMatrix OA = JMatrix.CreateRotationZ(body1.orientation);
JMatrix OB = JMatrix.CreateRotationZ(body2.orientation);
JVector zero = JVector.Zero;
float t = 0.0f;
if (body1.Shape.type == ShapeType.Box && body2.Shape.type == ShapeType.Box)
{
var A = body1.Shape as BoxShape;
var B = body2.Shape as BoxShape;
A.UpdateAxes(body1.orientation);
B.UpdateAxes(body2.orientation);
JVector[] CA = new JVector[2], CB = new JVector[2];
int Cnum = 0;
if (Collision.BoxBoxTestContact(ref A, ref body1.position, ref OA,
ref B, ref body2.position, ref OB,
out normal, out t, out CA, out CB, out Cnum))
{
normal.Negate();
RaiseCollisionDetected(body1, body2, ref CA[0], ref CB[0], ref normal, -t);
}
}
else if (body1.Shape.type == ShapeType.Circle && body2.Shape.type == ShapeType.Circle)
{
var A = body1.Shape as CircleShape;
var B = body2.Shape as CircleShape;
Collision.CircleCircleTest(body1.position, A.Radius, body2.position, B.Radius, out point1, out point2, out normal, out penetration);
if (penetration < 0)
{
RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, -penetration);
}
}
// all shapes (GJK)
else if (speculative)
{
//JVector hit1, hit2;
//if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref OA, ref OB,
// ref body1.position, ref body2.position, out hit1, out hit2, out normal))
//{
// JVector delta = hit2 - hit1;
// if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared())
// {
// //normal.Negate();
// penetration = delta * normal;
// if (penetration < 0.0f)
// {
// RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
// }
// }
//}
}
}
else if (b1IsMulti && b2IsMulti)
{
/*
* Multishape ms1 = (body1.Shape as Multishape);
* Multishape ms2 = (body2.Shape as Multishape);
*
* ms1 = ms1.RequestWorkingClone();
* ms2 = ms2.RequestWorkingClone();
*
* JBBox transformedBoundingBox = body2.boundingBox;
* transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation);
*
* int ms1Length = ms1.Prepare(ref transformedBoundingBox);
*
* transformedBoundingBox = body1.boundingBox;
* transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation);
*
* int ms2Length = ms2.Prepare(ref transformedBoundingBox);
*
* if (ms1Length == 0 || ms2Length == 0)
* {
* ms1.ReturnWorkingClone();
* ms2.ReturnWorkingClone();
* return;
* }
*
* for (int i = 0; i < ms1Length; i++)
* {
* ms1.SetCurrentShape(i);
*
* for (int e = 0; e < ms2Length; e++)
* {
* ms2.SetCurrentShape(e);
*
* if (XenoCollide.Detect(ms1, ms2, ref body1.orientation,
* ref body2.orientation, ref body1.position, ref body2.position,
* out point, out normal, out penetration))
* {
* if (this.RaisePassedNarrowphase(body1, body2, ref point, ref normal, penetration))
* {
* JVector point1, point2;
* FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2);
*
* RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
* }
* }
* }
* }
*
* ms1.ReturnWorkingClone();
* ms2.ReturnWorkingClone();
* */
}
else
{
/*
* RigidBody b1, b2;
*
* if (body2.Shape is Multishape) { b1 = body2; b2 = body1; }
* else { b2 = body2; b1 = body1; }
*
* Multishape ms = (b1.Shape as Multishape);
*
* ms = ms.RequestWorkingClone();
*
* JBBox transformedBoundingBox = b2.boundingBox;
* transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation);
*
* int msLength = ms.Prepare(ref transformedBoundingBox);
*
* if (msLength == 0)
* {
* ms.ReturnWorkingClone();
* return;
* }
*
* for (int i = 0; i < msLength; i++)
* {
* ms.SetCurrentShape(i);
*
* if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
* ref b2.orientation, ref b1.position, ref b2.position,
* out point, out normal, out penetration))
* {
* if (this.RaisePassedNarrowphase(b1, b2, ref point, ref normal, penetration))
* {
* JVector point1, point2;
* FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2);
*
* if (useTerrainNormal && ms is TerrainShape)
* {
* (ms as TerrainShape).CollisionNormal(out normal);
* JVector.Transform(ref normal, ref b1.orientation, out normal);
* }
* else if (useTriangleMeshNormal && ms is TriangleMeshShape)
* {
* (ms as TriangleMeshShape).CollisionNormal(out normal);
* JVector.Transform(ref normal, ref b1.orientation, out normal);
* }
*
* RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
* }
* }
* }
*
* ms.ReturnWorkingClone();
* */
}
}
public override void BuildGeometry(GraphicsDevice device)
{
float dh = Room.Door.DoorHeight; // door height
float dw2 = Room.Door.DoorWidth * .5f; // door width * .5
float ww = .2f; // wall width
float rw = .1f; // roof thickness
float roofSlope = .3f; // slope of line that makes roof
float rsx = 0; // offset of line that makes roof
float roofAng = (float)Math.Atan(roofSlope);
#region geometry generation
Color pillarColor = new Color(.47f, .36f, .21f);
Color wallColor = new Color(.57f, .45f, .26f);
Color roofColor = new Color(.47f, .36f, .21f);
for (int q = 0; q < Rooms.Length; q++)
{
Room r = Rooms[q];
Vector3 p = r.Position;
JVector jp = new JVector(p.X, p.Y, p.Z);
float w = r.Width * .5f, l = r.Length * .5f;
Vector3 c0 = p + new Vector3(-w, 0, -l),
c1 = p + new Vector3(w, 0, -l),
c2 = p + new Vector3(-w, 0, l),
c3 = p + new Vector3(w, 0, l);
rsx = -w;
// floor
addFace(wallColor, c0 + new Vector3(0, .1f, 0), c1 + new Vector3(0, .1f, 0), c3 + new Vector3(0, .1f, 0), c2 + new Vector3(0, .1f, 0));
RigidBody floorBody = new RigidBody(new BoxShape(r.Width, 2, r.Length));
floorBody.Position = new JVector(p.X, p.Y - .9f, p.Z);
RigidBodies.Add(floorBody);
// make walls without doors
byte doors = 0;
Vector3 low = new Vector3(0, -2, 0);
Vector3 high = new Vector3(0, r.Height, 0);
#region doors
for (int d = 0; d < r.doors.Length; d++)
{
Vector3 p1 = c0, p2 = c1;
Vector3 dp = Vector3.Zero, ds = Vector3.Zero;
Vector3 dsy = new Vector3(0, dh, 0);
Vector3 wo = Vector3.Zero;
JVector o;
RigidBody b1 = null, b2 = null, b3 = null;
float f, f2;
switch (r.doors[d].Wall)
{
case 0: // z-
doors |= 1;
p1 = c0;
p2 = c1;
dp = Vector3.Lerp(p1, p2, r.doors[d].Location);
ds = new Vector3(dw2, 0, 0);
wo = new Vector3(0, 0, ww);
o = new JVector(dp.X, dp.Y, dp.Z) + new JVector(0, r.Height * .5f, ww * .5f);
f = r.Width * r.doors[d].Location - dw2;
f2 = r.Width * (1 - r.doors[d].Location) - dw2;
b1 = new RigidBody(new BoxShape(f, r.Height, ww)); b1.Position = o + new JVector(-f / 2 - dw2, 0, 0);
b2 = new RigidBody(new BoxShape(dw2 * 2, r.Height - dh, ww)); b2.Position = o + new JVector(0, dh * .5f, 0);
b3 = new RigidBody(new BoxShape(f2, r.Height, ww)); b3.Position = o + new JVector(f2 / 2 + dw2, 0, 0);
entrancebbox = new BoundingBox(dp - ds + new Vector3(0, -1, -2), dp + ds + new Vector3(0, dh, 2));
break;
case 1: // z+
doors |= 2;
p1 = c2;
p2 = c3;
dp = Vector3.Lerp(p1, p2, r.doors[d].Location);
ds = new Vector3(dw2, 0, 0);
wo = new Vector3(0, 0, -ww);
o = new JVector(dp.X, dp.Y, dp.Z) + new JVector(0, r.Height * .5f, -ww * .5f);
f = r.Width * r.doors[d].Location - dw2;
f2 = r.Width * (1 - r.doors[d].Location) - dw2;
b1 = new RigidBody(new BoxShape(f, r.Height, ww)); b1.Position = o + new JVector(-f / 2 - dw2, 0, 0);
b2 = new RigidBody(new BoxShape(dw2 * 2, r.Height - dh, ww)); b2.Position = o + new JVector(0, dh * .5f, 0);
b3 = new RigidBody(new BoxShape(f2, r.Height, ww)); b3.Position = o + new JVector(f2 / 2 + dw2, 0, 0);
entrancebbox = new BoundingBox(dp - ds + new Vector3(0, -1, -2), dp + ds + new Vector3(0, dh, 2));
break;
case 2: // x-
doors |= 4;
p1 = c0;
p2 = c2;
dp = Vector3.Lerp(p1, p2, r.doors[d].Location);
ds = new Vector3(0, 0, dw2);
wo = new Vector3(ww, 0, 0);
o = new JVector(dp.X, dp.Y, dp.Z) + new JVector(ww * .5f, r.Height * .5f, 0);
f = r.Length * r.doors[d].Location - dw2;
f2 = r.Length * (1 - r.doors[d].Location) - dw2;
b1 = new RigidBody(new BoxShape(ww, r.Height, f)); b1.Position = o + new JVector(0, 0, -f / 2 - dw2);
b2 = new RigidBody(new BoxShape(ww, r.Height - dh, dw2 * 22)); b2.Position = o + new JVector(0, dh * .5f, 0);
b3 = new RigidBody(new BoxShape(ww, r.Height, f2)); b3.Position = o + new JVector(0, 0, f2 / 2 + dw2);
entrancebbox = new BoundingBox(dp - ds + new Vector3(-2, -1, 0), dp + ds + new Vector3(2, dh, 0));
break;
case 3: // x+
doors |= 8;
p1 = c1;
p2 = c3;
dp = Vector3.Lerp(p1, p2, r.doors[d].Location);
ds = new Vector3(0, 0, dw2);
wo = new Vector3(-ww, 0, 0);
o = new JVector(dp.X, dp.Y, dp.Z) + new JVector(-ww * .5f, r.Height * .5f, 0);
f = r.Length * r.doors[d].Location - dw2;
f2 = r.Length * (1 - r.doors[d].Location) - dw2;
b1 = new RigidBody(new BoxShape(ww, r.Height, f)); b1.Position = o + new JVector(0, 0, -f / 2 - dw2);
b2 = new RigidBody(new BoxShape(ww, r.Height - dh, dw2 * 22)); b2.Position = o + new JVector(0, dh * .5f, 0);
b3 = new RigidBody(new BoxShape(ww, r.Height, f2)); b3.Position = o + new JVector(0, 0, f2 / 2 + dw2);
entrancebbox = new BoundingBox(dp - ds + new Vector3(-2, -1, 0), dp + ds + new Vector3(2, dh, 0));
break;
}
entrancebbox.Min += Position + r.Position;
entrancebbox.Max += Position + r.Position;
RigidBodies.Add(b1);
RigidBodies.Add(b2);
RigidBodies.Add(b3);
#region wall polygons
// outside wall face
addFace(wallColor,
new int[] {
0, 1, 4,
0, 4, 5,
0, 5, 6,
0, 6, 7,
1, 2, 3,
1, 3, 4,
8, 3, 6,
9, 3, 8
},
p2 + high + new Vector3(0, (p2.X - rsx) * roofSlope, 0),
p1 + high + new Vector3(0, (p1.X - rsx) * roofSlope, 0),
p1 + low,
dp - ds + low,
dp - ds + dsy,
dp + ds + dsy,
dp + ds + low,
p2 + low,
dp + ds + new Vector3(0, .1f, 0),
dp - ds + new Vector3(0, .1f, 0)
);
// inside wall face
addFace(wallColor,
new int[] {
0, 4, 1,
0, 5, 4,
0, 6, 5,
0, 7, 6,
1, 3, 2,
1, 4, 3
},
p2 + high + wo + new Vector3(0, (p2.X - rsx) * roofSlope, 0),
p1 + high + wo + new Vector3(0, (p1.X - rsx) * roofSlope, 0),
p1 + low + wo,
dp - ds + low + wo,
dp - ds + dsy + wo,
dp + ds + dsy + wo,
dp + ds + low + wo,
p2 + low + wo
);
// door insides
addFace(wallColor,
dp + ds,
dp + ds + dsy,
dp + ds + wo + dsy,
dp + ds + wo);
addFace(wallColor,
dp - ds + wo,
dp - ds + wo + dsy,
dp - ds + dsy,
dp - ds);
addFace(wallColor,
dp - ds + dsy + wo,
dp + ds + dsy + wo,
dp + ds + dsy,
dp - ds + dsy);
#endregion
}
#endregion
#region walls
if ((doors & 1) == 0) // no door on z- side, make a wall
{
addFace(wallColor,
c0 + high + new Vector3(0, (c0.X - rsx) * roofSlope, 0),
c1 + high + new Vector3(0, (c1.X - rsx) * roofSlope, 0),
c1 + low,
c0 + low);
addFace(wallColor,
c1 + high + new Vector3(ww, 0, ww) + new Vector3(0, (c1.X + ww - rsx) * roofSlope, 0),
c0 + high + new Vector3(-ww, 0, ww) + new Vector3(0, (c0.X - ww - rsx) * roofSlope, 0),
c0 + new Vector3(-ww, 0, ww),
c1 + new Vector3(ww, 0, ww));
RigidBody wallBody = new RigidBody(new BoxShape(r.Width, r.Height, ww));
wallBody.Position = jp + new JVector(0, r.Height * .5f, -(r.Length - ww) * .5f);
RigidBodies.Add(wallBody);
}
if ((doors & 2) == 0) // no door on z+ side, make a wall
{
addFace(wallColor,
c2 + high + new Vector3(0, (c2.X - rsx) * roofSlope, 0),
c3 + high + new Vector3(0, (c3.X - rsx) * roofSlope, 0),
c3 + low,
c2 + low);
addFace(wallColor,
c3 + high + new Vector3(-ww, 0, -ww) + new Vector3(0, (c3.X - ww - rsx) * roofSlope, 0),
c2 + high + new Vector3(ww, 0, -ww) + new Vector3(0, (c2.X + ww - rsx) * roofSlope, 0),
c2 + new Vector3(ww, 0, -ww),
c3 + new Vector3(-ww, 0, -ww));
RigidBody wallBody = new RigidBody(new BoxShape(r.Width, r.Height, ww));
wallBody.Position = jp + new JVector(0, r.Height * .5f, (r.Length - ww) * .5f);
RigidBodies.Add(wallBody);
}
if ((doors & 4) == 0) // no door on x- side, make a wall
{
addFace(wallColor,
c0 + high + new Vector3(0, (c0.X - rsx) * roofSlope, 0),
c2 + high + new Vector3(0, (c2.X - rsx) * roofSlope, 0),
c2 + low,
c0 + low);
addFace(wallColor,
c2 + high + new Vector3(ww, 0, -ww) + new Vector3(0, (c2.X + ww - rsx) * roofSlope, 0),
c0 + high + new Vector3(ww, 0, ww) + new Vector3(0, (c0.X + ww - rsx) * roofSlope, 0),
c0 + new Vector3(ww, 0, ww),
c2 + new Vector3(ww, 0, -ww));
RigidBody wallBody = new RigidBody(new BoxShape(ww, r.Height, r.Length));
wallBody.Position = jp + new JVector(-(r.Width - ww) * .5f, r.Height * .5f, 0);
RigidBodies.Add(wallBody);
}
if ((doors & 8) == 0) // no door on x+ side, make a wall
{
addFace(wallColor,
c3 + high + new Vector3(0, (c3.X - rsx) * roofSlope, 0),
c1 + high + new Vector3(0, (c1.X - rsx) * roofSlope, 0),
c1 + low,
c3 + low);
addFace(wallColor,
c1 + high + new Vector3(-ww, 0, ww) + new Vector3(0, (c1.X - ww - rsx) * roofSlope, 0),
c3 + high + new Vector3(-ww, 0, -ww) + new Vector3(0, (c3.X - ww - rsx) * roofSlope, 0),
c3 + new Vector3(-ww, 0, -ww),
c1 + new Vector3(-ww, 0, ww));
RigidBody wallBody = new RigidBody(new BoxShape(ww, r.Height, r.Length));
wallBody.Position = jp + new JVector((r.Width - ww) * .5f, r.Height * .5f, 0);
RigidBodies.Add(wallBody);
}
#endregion
#region roof
{
Vector3 c0r = c0 + high + new Vector3(-.5f, (c0.X - .5f - rsx) * roofSlope + rw, -.5f),
c1r = c1 + high + new Vector3(.5f, (c1.X + .5f - rsx) * roofSlope + rw, -.5f),
c2r = c2 + high + new Vector3(-.5f, (c2.X - .5f - rsx) * roofSlope + rw, .5f),
c3r = c3 + high + new Vector3(.5f, (c3.X + .5f - rsx) * roofSlope + rw, .5f);
Vector3 c0rd = c0 + high + new Vector3(-.5f, (c0.X - .5f - rsx) * roofSlope, -.5f),
c1rd = c1 + high + new Vector3(.5f, (c1.X + .5f - rsx) * roofSlope, -.5f),
c2rd = c2 + high + new Vector3(-.5f, (c2.X - .5f - rsx) * roofSlope, .5f),
c3rd = c3 + high + new Vector3(.5f, (c3.X + .5f - rsx) * roofSlope, .5f);
// top face
addFace(roofColor,
c0r,
c1r,
c3r,
c2r);
// bottom (inside) face
addFace(roofColor,
c2rd,
c3rd,
c1rd,
c0rd);
// front edge face
addFace(roofColor,
c0rd,
c1rd,
c1r,
c0r);
// back edge face
addFace(roofColor,
c3rd,
c2rd,
c2r,
c3r);
// right edge face
addFace(roofColor,
c1rd,
c3rd,
c3r,
c1r);
// left edge face
addFace(roofColor,
c2rd,
c0rd,
c0r,
c2r);
}
float a = roofSlope * (r.Width + 1 - rsx);
RigidBody roofBody = new RigidBody(new BoxShape(
(float)Math.Sqrt((r.Width + 1) * (r.Width + 1) + a * a) // pythagorean theorem
, rw, r.Length + 1));
roofBody.Orientation = JMatrix.CreateRotationZ(roofAng);
roofBody.Position = new JVector(0, r.Height + (roofSlope * r.Width + rw) * .5f, 0);
RigidBodies.Add(roofBody);
#endregion
}
#endregion
base.BuildGeometry(device);
}
protected override void Update(GameTime gameTime)
{
// Allows the game to exit
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
this.Exit();
}
KeyboardState keys = Keyboard.GetState();
JVector moveVector = JVector.Zero;
float amountOfMovement = 0.05f;
if (keys.IsKeyDown(Keys.Right))
{
moveVector.X += amountOfMovement;
}
if (keys.IsKeyDown(Keys.Left))
{
moveVector.X -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Down))
{
moveVector.Y -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Up))
{
moveVector.Y += amountOfMovement;
}
PB += moveVector;
OA = (float)gameTime.TotalGameTime.TotalSeconds * 0.1f;
OB = (float)gameTime.TotalGameTime.TotalSeconds * -0.1f;
DrawBox(A, PA, OA, Color.Blue * 0.25f);
DrawBox(B, PB, OB, Color.Green * 0.25f);
float t = 0.0f;
JVector[] CA = new JVector[2], CB = new JVector[2];
int NumContacts = 0;
sw.Start();
JMatrix OAM = JMatrix.CreateRotationZ(OA);
JMatrix OBM = JMatrix.CreateRotationZ(OB);
for (int i = 0; i < 1; i++)
{
A.UpdateAxes(OA);
B.UpdateAxes(OB);
//hit = Collision.BoxBoxTest(ref A, ref PA, ref B, ref PB);
//AV = new List<JVector> { A.GetCorner(0), A.GetCorner(1), A.GetCorner(2), A.GetCorner(3) };
//BV = new List<JVector> { B.GetCorner(0), B.GetCorner(1), B.GetCorner(2), B.GetCorner(3) };
//hit = SAT.Collide(ref AV, ref PA, ref VA, ref OAM,
// ref BV, ref PB, ref VB, ref OBM,
// ref normal, ref t);
//if (hit)
//{
// SAT.FindContacts(ref AV, ref PA, ref VA, ref OAM,
// ref BV, ref PB, ref VB, ref OBM,
// ref normal, t, out CA, out CB, out NumContacts);
// normal.Normalize();
//}
hit = Collision.BoxBoxTestContact(ref A, ref PA, ref OAM, ref B, ref PB, ref OBM,
out normal, out t, out CA, out CB, out NumContacts);
penetration = t;
iterations = NumContacts;
}
sw.Stop();
ticks = sw.ElapsedTicks / 1;
sw.Reset();
if (hit)
{
//DrawBox(A, PA + normal * (t * 0.5f), OA, Color.Blue);
//DrawBox(B, PB - normal * (t * 0.5f), OB, Color.Green);
for (int i = 0; i < NumContacts; i++)
{
DebugDrawer.DrawPoint(CA[i]); // + normal * (t * 0.5f));
DebugDrawer.DrawPoint(CB[i]); // - normal * (t * 0.5f));
}
}
base.Update(gameTime);
}
/// <summary>
/// Randomizes the orientation of a die
/// </summary>
/// <param name="die">Die body</param>
public void RandomizeDieOrientation(RigidBody die)
{
float xRot = -MathHelper.Pi + ((float)random.NextDouble() * MathHelper.TwoPi);
float yRot = -MathHelper.Pi + ((float)random.NextDouble() * MathHelper.TwoPi);
float zRot = -MathHelper.Pi + ((float)random.NextDouble() * MathHelper.TwoPi);
die.Orientation = JMatrix.CreateRotationX(xRot) * JMatrix.CreateRotationY(yRot) * JMatrix.CreateRotationZ(zRot);
}
protected override void Update(GameTime gameTime)
{
// Allows the game to exit
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
this.Exit();
}
KeyboardState keys = Keyboard.GetState();
JVector moveVector = JVector.Zero;
float amountOfMovement = 0.05f;
if (keys.IsKeyDown(Keys.Right))
{
moveVector.X += amountOfMovement;
}
if (keys.IsKeyDown(Keys.Left))
{
moveVector.X -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Down))
{
moveVector.Y -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Up))
{
moveVector.Y += amountOfMovement;
}
body1.Position += moveVector;
body1.Orientation += 0.001f;
body2.Orientation -= 0.001f;
JMatrix o1 = JMatrix.CreateRotationZ(body1.Orientation);
JMatrix o2 = JMatrix.CreateRotationZ(body2.Orientation);
JVector pos1 = body1.Position;
JVector pos2 = body2.Position;
JVector point2;
sw.Start();
hit = XenoCollide.Detect(body1.Shape, body2.Shape, ref o1, ref o2, ref pos1, ref pos2, out point, out normal, out penetration);
sw.Stop();
ticks = sw.ElapsedTicks;
sw.Reset();
DebugDrawer.DrawLine(point, point + normal);
//DebugDrawer.DrawPoint(point2);
DebugDrawer.DrawPoint(point);
DebugDrawer.Color = Color.Red;
DebugDrawer.Color = Color.Black;
DebugDrawer.DrawLine(JVector.Up, JVector.Down);
DebugDrawer.DrawLine(JVector.Left, JVector.Right);
body1.DebugDraw(DebugDrawer);
body2.DebugDraw(DebugDrawer);
if (hit)
{
var oldPosition = body1.Position;
body1.Position += normal;
body1.DebugDraw(DebugDrawer);
body1.Position = oldPosition;
}
base.Update(gameTime);
}
public JVector LocalToWorld(JVector localPosition)
{
return(JVector.Transform(localPosition, JMatrix.CreateRotationZ(Orientation)) + Position);
}
// this method is extremely brute force, only use for debugging!
public void DebugDraw(IDebugDrawer drawer)
{
if (this.shape.type == ShapeType.Box)
{
BoxShape box = this.shape as BoxShape;
// get corners
JVector a = box.GetCorner(0);
JVector b = box.GetCorner(1);
JVector c = box.GetCorner(2);
JVector d = box.GetCorner(3);
// transform points
JMatrix xform = JMatrix.CreateRotationZ(this.orientation);
JVector.Transform(ref a, ref xform, out a);
JVector.Transform(ref b, ref xform, out b);
JVector.Transform(ref c, ref xform, out c);
JVector.Transform(ref d, ref xform, out d);
a += this.position;
b += this.position;
c += this.position;
d += this.position;
if (isStatic)
{
drawer.SetColor(0.25f, 0.85f, 0.25f, 1);
}
else if (isActive)
{
drawer.SetColor(0.95f, 0.95f, 0.95f, 1);
}
else
{
drawer.SetColor(0.65f, 0.65f, 0.65f, 1);
}
drawer.DrawTriangle(a, c, b);
drawer.DrawTriangle(c, a, d);
// draw outline
drawer.SetColor(0, 0, 0, 1);
drawer.DrawLine(a, b);
drawer.DrawLine(b, c);
drawer.DrawLine(c, d);
drawer.DrawLine(d, a);
}
else if (this.shape.type == ShapeType.Circle)
{
JMatrix o1 = JMatrix.CreateRotationZ(orientation);
JVector dir = JVector.Up;
JVector u = JVector.Zero;
JVector a;
for (int i = -1; i <= 36; i++)
{
JVector.TransposedTransform(ref dir, ref o1, out a);
// get the support in the given direction
JVector s; this.shape.SupportMapping(ref a, out s);
// transform the support into world space
a = JVector.Transform(s, o1) + position;
dir = JVector.Transform(dir, JMatrix.CreateRotationZ(0.0174532925f * 10f));
if (i >= 0)
{
if (isStatic)
{
drawer.SetColor(0.25f, 0.85f, 0.25f, 1);
}
else if (isActive)
{
drawer.SetColor(0.95f, 0.95f, 0.95f, 1);
}
else
{
drawer.SetColor(0.65f, 0.65f, 0.65f, 1);
}
drawer.DrawTriangle(a, u, this.position);
drawer.SetColor(0, 0, 0, 1);
drawer.DrawLine(a, u);
}
u = a;
}
}
//JMatrix o1 = JMatrix.CreateRotationZ(orientation);
//JVector dir = JVector.Up;
//JVector u = JVector.Zero;
//JVector a;
//for (int i = -1; i <= 36; i++)
//{
// JVector.TransposedTransform(ref dir, ref o1, out a);
// // get the support in the given direction
// JVector s; this.shape.SupportMapping(ref a, out s);
// // transform the support into world space
// a = JVector.Transform(s, o1) + position;
// dir = JVector.Transform(dir, JMatrix.CreateRotationZ(0.0174532925f * 10f));
// if (i >= 0)
// {
// if (isStatic)
// drawer.SetColor(0.25f, 0.85f, 0.25f, 1);
// else if (isActive)
// drawer.SetColor(0.95f, 0.95f, 0.95f, 1);
// else
// drawer.SetColor(0.65f, 0.65f, 0.65f, 1);
// drawer.DrawTriangle(a, u, this.position);
// drawer.SetColor(0,0,0, 1);
// drawer.DrawLine(a, u);
// }
// u = a;
//}
//JMatrix xForm = JMatrix.CreateRotationZ(orientation);
//drawer.SetColor(1, 0, 0, 1);
//drawer.DrawLine(position + JVector.Transform(JVector.Left * 0.25f, xForm), position + JVector.Transform(JVector.Zero, xForm));
//drawer.SetColor(0, 1, 0, 1);
//drawer.DrawLine(position + JVector.Transform(JVector.Up * 0.25f, xForm), position + JVector.Transform(JVector.Zero, xForm));
}
protected override void Update(GameTime gameTime)
{
// Allows the game to exit
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
this.Exit();
}
KeyboardState keys = Keyboard.GetState();
JVector moveVector = JVector.Zero;
float amountOfMovement = 0.05f;
if (keys.IsKeyDown(Keys.Right))
{
moveVector.X += amountOfMovement;
}
if (keys.IsKeyDown(Keys.Left))
{
moveVector.X -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Down))
{
moveVector.Y -= amountOfMovement;
}
if (keys.IsKeyDown(Keys.Up))
{
moveVector.Y += amountOfMovement;
}
body1.Position += moveVector;
if (keys.IsKeyDown(Keys.OemPlus) && oldState.IsKeyUp(Keys.OemPlus))
{
GJKCollide.MaxIterations++;
}
if (keys.IsKeyDown(Keys.OemMinus) && oldState.IsKeyUp(Keys.OemMinus))
{
GJKCollide.MaxIterations--;
}
GJKCollide.MaxIterations = (int)JMath.Clamp(GJKCollide.MaxIterations, 0, 25);
bool changeShape = false;
if (keys.IsKeyDown(Keys.D1) && oldState.IsKeyUp(Keys.D1))
{
shapeType++;
changeShape = true;
}
if (keys.IsKeyDown(Keys.D2) && oldState.IsKeyUp(Keys.D2))
{
shapeType--;
changeShape = true;
}
shapeType = (int)JMath.Clamp(shapeType, 0, 2);
if (changeShape)
{
Random r = new Random();
switch (shapeType)
{
case 0: // circle
body1 = new RigidBody(new CircleShape((float)r.NextDouble() * 3f));
break;
case 1: // capsule
body1 = new RigidBody(new CapsuleShape((float)r.NextDouble() * 3f, (float)r.NextDouble() * 1f));
break;
case 2: // box
body1 = new RigidBody(new BoxShape((float)r.NextDouble() * 3f, (float)r.NextDouble() * 3f));
break;
}
}
body1.Orientation += 0.005f;
body2.Orientation -= 0.005f;
JMatrix o1 = JMatrix.CreateRotationZ(body1.Orientation);
JMatrix o2 = JMatrix.CreateRotationZ(body2.Orientation);
JVector pos1 = body1.Position;
JVector pos2 = body2.Position;
JVector point2;
sw.Start();
hit = GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref o1, ref o2, ref pos1, ref pos2, out point, out point2, out normal);
sw.Stop();
penetration = JVector.Distance(point, point2);
ticks = sw.ElapsedTicks;
sw.Reset();
DebugDrawer.DrawPoint(point2);
DebugDrawer.DrawPoint(point);
DebugDrawer.Color = Color.Red;
DebugDrawer.DrawLine(JVector.Zero, normal);
DebugDrawer.Color = Color.Black;
DebugDrawer.DrawLine(JVector.Up, JVector.Down);
DebugDrawer.DrawLine(JVector.Left, JVector.Right);
body1.DebugDraw(DebugDrawer);
body2.DebugDraw(DebugDrawer);
oldState = keys;
base.Update(gameTime);
}