/// <summary>
/// Construct a new world point constraint.
/// </summary>
/// <param name="body">The body to constrain.</param>
/// <param name="worldPoint">The point in world coordinates to constrain the point to. The body is expected to be occupying this
/// point at the time the constraint is created.</param>
public WorldPointConstraint(RigidBody body, Vector3 worldPoint)
: base(body, body)
{
_worldPoint = worldPoint;
Vector3.Transform(ref worldPoint, ref BodyA.WorldInverse.Combined, out _bodyPoint);
orientation = body.Orientation;
}
/// <summary>
/// Construct a new constraint.
/// </summary>
/// <param name="bodyA">The first constrained body.</param>
/// <param name="bodyB">The second constrained body.</param>
public Constraint(RigidBody bodyA, RigidBody bodyB)
{
_bodyA = bodyA;
_bodyB = bodyB;
this.ConstraintId = Interlocked.Increment(ref CurrentConstraintId);
}
/// <summary>
/// Construct a new distance constraint.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="worldPointA">The world-space point at which the constraint is applied on the first body.</param>
/// <param name="worldPointB">The world-space point at which the constraint is applied on the second body.</param>
/// <param name="minDistance">The minimum allowed distance.</param>
/// <param name="maxDistance">The maximum allowed distance.</param>
public DistanceConstraint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPointA, Vector3 worldPointB, float minDistance, float maxDistance)
: base(bodyA, bodyB)
{
// store body points
Vector3.Transform(ref worldPointA, ref bodyA.WorldInverse.Combined, out _bodyPointA);
Vector3.Transform(ref worldPointB, ref bodyB.WorldInverse.Combined, out _bodyPointB);
_minDistance = minDistance;
_maxDistance = maxDistance;
}
protected bool onCollide(RigidBody r1, RigidBody r2)
{
if (r2 == room.TrackedObject)
{
room.TrackedObject = r1 as ITrackable;
this.OnCollision = null;
}
return false;
}
/// <summary>
/// Construct a new spring force.
/// </summary>
/// <param name="bodyA">The first body on which force is applied.</param>
/// <param name="bodyB">The second body on which force is applied.</param>
/// <param name="bodyPointA">The point in local-space on the first body on which to apply force.</param>
/// <param name="bodyPointB">The point in local-space on the second body on which to apply force.</param>
/// <param name="length">The resting length of the spring.</param>
/// <param name="stiffness">The stiffness factor of the spring.</param>
/// <param name="damping">The damping coefficient.</param>
public SpringForce(RigidBody bodyA, RigidBody bodyB, Vector3 bodyPointA, Vector3 bodyPointB, float length, float stiffness, float damping)
{
_bodyA = bodyA;
_bodyB = bodyB;
_bodyPointA = bodyPointA;
_bodyPointB = bodyPointB;
_length = length;
_k = stiffness;
_c = damping;
}
/// <summary>
/// Construct a new generic constraint.
/// </summary>
/// <param name="bodyA">The first constrained body.</param>
/// <param name="bodyB">The second constrained body.</param>
/// <param name="basis">The world-space frame of the second body against which the first body will be constrained. This value
/// must contain the initial position and orientation of the second body at the time the constraint was created.</param>
/// <param name="limitedPosAxes">Specifies which axes around which to constrain position.</param>
/// <param name="minPos">The minimum distances between the two bodies along each axis.</param>
/// <param name="maxPos">The maximum distances between the two bodies along each axis.</param>
/// <param name="limitedRotAxes">Specifies which axes around which to constrain rotation.</param>
/// <param name="minAngles">The minimum relative angles, in radians, around each axis.</param>
/// <param name="maxAngles">The maximum relative angles, in radians, around each axis.</param>
public GenericConstraint(RigidBody bodyA, RigidBody bodyB, Frame basis, Axes limitedPosAxes,
Vector3 minPos, Vector3 maxPos, Axes limitedRotAxes, Vector3 minAngles, Vector3 maxAngles)
: base(bodyA, bodyB)
{
Frame.Transform(ref basis, ref bodyA.WorldInverse, out _bodyBasisA);
Frame.Transform(ref basis, ref bodyB.WorldInverse, out _bodyBasisB);
_limitedPosAxes = limitedPosAxes;
_limitedRotAxes = limitedRotAxes;
_minPos = minPos;
_maxPos = maxPos;
_minAngles = minAngles;
_maxAngles = maxAngles;
}
/// <summary>
/// Construct a new universal joint.
/// </summary>
/// <param name="bodyA">The first body to constrain.</param>
/// <param name="bodyB">The second body to constrain.</param>
/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
/// <param name="axisA">The axis fixed to the first body which the second body can rotate around.</param>
/// <param name="axisB">The axis fixed to the second body which the first body can rotate around.</param>
/// <param name="aMin">The minimum angle that the second body must maintain relative to the first around the first body's axis.
/// This number should be zero or negative.</param>
/// <param name="aMax">The maximum angle that the second body must maintain relative to the first around the first body's axis.
/// This number should be zero or positive.</param>
/// <param name="bMin">The minimum angle that the first body must maintain relative to the second around the second body's axis.
/// This number should be zero or negative.</param>
/// <param name="bMax">The maximum angle that the first body must maintain relative to the second around the second body's axis.
/// This number should be zero or positive.</param>
public UniversalJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axisA, Vector3 axisB,
float aMin, float aMax, float bMin, float bMax)
: base(bodyA, bodyB)
{
aMin = MathHelper.Clamp(aMin, -MathHelper.PiOver2 + Constants.Epsilon, 0f);
aMax = MathHelper.Clamp(aMax, 0f, MathHelper.PiOver2 - Constants.Epsilon);
bMin = MathHelper.Clamp(bMin, -MathHelper.PiOver2 + Constants.Epsilon, 0f);
bMax = MathHelper.Clamp(bMax, 0f, MathHelper.PiOver2 - Constants.Epsilon);
var frame = new Frame(Vector3.Zero, axisA, axisB, worldPoint);
this.Constraints.Add(new PointConstraint(bodyA, bodyB, worldPoint));
this.Constraints.Add(new GenericConstraint(bodyA, bodyB, frame,
Axes.None, Vector3.Zero, Vector3.Zero,
Axes.All, new Vector3(0f, aMin, bMin), new Vector3(0f, aMax, bMax)));
}
/// <summary>
/// Construct a new revolute joint.
/// </summary>
/// <param name="bodyA">The first constrained body.</param>
/// <param name="bodyB">The second constrained body.</param>
/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
/// <param name="axis">The axis around which to allow relative rotation. Rotation around the other axes will be prevented.</param>
/// <param name="minAngle">The minimum angle specifies (in radians) the most that the first body can rotate clockwise relative
/// to the second body around the axis. This number should be zero or negative.</param>
/// <param name="maxAngle">The maximum angle specifies (in radians) the most that the first body can rotate counter-clockwise relative
/// to the second body around the axis. This number should be zero or positive.</param>
public RevoluteJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axis, float minAngle, float maxAngle)
: base(bodyA, bodyB)
{
var frame = new Frame(worldPoint);
// create an arbitrary basis with the axis as X
Vector3 n = Vector3.UnitZ;
Vector3.Cross(ref n, ref axis, out n);
frame.X = axis;
if (n.LengthSquared() < Constants.Epsilon)
{
n = Vector3.UnitY;
Vector3.Cross(ref axis, ref n, out n);
frame.Z = n;
}
else
frame.Y = n;
frame.Normalize();
this.Constraints.Add(new GenericConstraint(bodyA, bodyB, frame,
Axes.All, Vector3.Zero, Vector3.Zero,
Axes.All, new Vector3(minAngle, 0f, 0f), new Vector3(maxAngle, 0f, 0f)));
}
public void processTouchPoints(ReadOnlyTouchPointCollection touches)
{
lastTouchPosition = touchPosition;
int tagID = -1;
if (touches.Count >= 1)
{
for (int i = 0; i < touches.Count; i++)
{
if (touches[i].IsTagRecognized)
{
tagID = (int)touches[i].Tag.Value;
break;
}
}
if (tagID != -1)
{
touchPosition = touches[0];
//First time touch
if (lastTouchPosition == null)
{
Segment s;
s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 0f),
viewManager.Projection, viewManager.View, Matrix.Identity);
s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 1f),
viewManager.Projection, viewManager.View, Matrix.Identity);
float scalar;
Vector3 point;
var c = physics.BroadPhase.Intersect(ref s, out scalar, out point);
if (c != null && c is BodySkin)
{
pickedObject = ((BodySkin)c).Owner;
pickedForce = new WorldPointConstraint(pickedObject, point);
physics.Add(pickedForce);
pickedDistance = scalar;
pickedObject.IsActive = true;
}
lastOrientation = touches.Count == 1 ? touches[0].Orientation : touches[1].Orientation;
}
else if (pickedObject != null)
{
Segment s;
s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 0f),
viewManager.Projection, viewManager.View, Matrix.Identity);
s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.CenterX, touchPosition.CenterY, 1f),
viewManager.Projection, viewManager.View, Matrix.Identity);
Vector3 diff, point;
Vector3.Subtract(ref s.P2, ref s.P1, out diff);
Vector3.Multiply(ref diff, pickedDistance, out diff);
Vector3.Add(ref s.P1, ref diff, out point);
pickedForce.WorldPoint = point;
pickedObject.IsActive = true;
//SolidThing po = (SolidThing)pickedObject;
//Console.Out.WriteLine("UNFREEEEEEEEEEEEZE");
switch (tagID)
{
//Pin a block
case 0:
pickedObject.Freeze();
break;
//unPin a block
case 1:
pickedObject.Unfreeze();
break;
//Rotate a block
case 2:
pickedForce.orientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, -1.0f), touchPosition.Orientation);
break;
//Move a block towards or away from camera
case 3:
TouchPoint tagPoint = touches[0];
float deltaRotation = MathHelper.ToDegrees(lastOrientation) - MathHelper.ToDegrees(tagPoint.Orientation);
Vector3 direction = new Vector3(0, 0, 1.0f);
direction.Normalize();
pickedForce.WorldPoint = Vector3.Add(pickedForce.WorldPoint, Vector3.Multiply(direction, deltaRotation * 0.01f));
break;
//Rotate stack onto top view
/*case 4:
viewManager.rotateToSide(4, touch);
break;
//Corkscrew closer or further away
case 5:
viewManager.rotateToSide(5);
break;
case 6:
viewManager.rotateToSide(6);
break;
case 7:
viewManager.rotateToSide(7);
break;
case 8:
viewManager.rotateToSide(8);
break;
*/
}
}
else if (pickedObject != null)
{
physics.Remove(pickedForce);
pickedObject = null;
}
}
}
else if (pickedObject != null)
{
physics.Remove(pickedForce);
pickedObject = null;
touchPosition = null;
lastTouchPosition = null;
}
else
{
touchPosition = null;
}
}
protected override void Update(GameTime gameTime)
{
_inputManager.CaptureMouse = this.IsActive && _inputManager.MouseState.RightButton == ButtonState.Pressed;
#if WINDOWS
_physics.Enabled = !Program.Proxy.IsPaused;
#endif
#if WINDOWS || WINDOWS_PHONE
foreach (Keys k in _inputManager.KeysPressed)
{
switch (k)
{
case Keys.Space:
SpawnSelectedObject();
break;
case Keys.R:
SpawnRagdoll();
break;
case Keys.N:
if (!_physics.Enabled)
{
_physics.Integrate((float)gameTime.ElapsedGameTime.TotalSeconds);
}
break;
case Keys.D0:
case Keys.D1:
case Keys.D2:
case Keys.D3:
case Keys.D4:
case Keys.D5:
case Keys.D6:
case Keys.D7:
case Keys.D8:
case Keys.D9:
CreateScene(k - Keys.D0);
break;
case Keys.Escape:
case Keys.Q:
this.Exit();
break;
}
}
// object picking
if (_inputManager.WasPressed(MouseButton.MiddleButton))
{
Segment s;
s.P1 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 0f),
_viewManager.Projection, _viewManager.View, Matrix.Identity);
s.P2 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 1f),
_viewManager.Projection, _viewManager.View, Matrix.Identity);
float scalar;
Vector3 point;
var c = _physics.BroadPhase.Intersect(ref s, out scalar, out point);
if (c != null && c is BodySkin)
{
_pickedObject = ((BodySkin)c).Owner;
_pickedForce = new GrabConstraint(_pickedObject, point);
_physics.Add(_pickedForce);
_pickedDistance = scalar;
_pickedObject.IsActive = true;
}
}
else if (_inputManager.MouseState.MiddleButton == ButtonState.Pressed && _pickedObject != null)
{
Segment s;
s.P1 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 0f),
_viewManager.Projection, _viewManager.View, Matrix.Identity);
s.P2 = GraphicsDevice.Viewport.Unproject(new Vector3(_inputManager.MouseState.X, _inputManager.MouseState.Y, 1f),
_viewManager.Projection, _viewManager.View, Matrix.Identity);
Vector3 diff, point;
Vector3.Subtract(ref s.P2, ref s.P1, out diff);
Vector3.Multiply(ref diff, _pickedDistance, out diff);
Vector3.Add(ref s.P1, ref diff, out point);
_pickedForce.WorldPoint = point;
_pickedObject.IsActive = true;
}
else if (_pickedObject != null)
{
_physics.Remove(_pickedForce);
_pickedObject = null;
}
#else
if (_inputManager.WasPressed(Buttons.RightShoulder)) CreateScene(++curScene);
if (_inputManager.WasPressed(Buttons.LeftShoulder)) CreateScene(curScene == 0 ? 0 : --curScene);
if (_inputManager.WasPressed(Buttons.A)) SpawnSelectedObject();
if (_inputManager.WasPressed(Buttons.B)) SpawnRagdoll();
if (_inputManager.WasPressed(Buttons.X))
_physics.Enabled = !_physics.Enabled;
if (_inputManager.WasPressed(Buttons.Y)) _physics.Integrate((float)gameTime.ElapsedGameTime.TotalSeconds);
if (_inputManager.WasPressed(Buttons.Back)) this.Exit();
#endif
UpdateFps(gameTime);
base.Update(gameTime);
}
/// <summary>
/// Construct a new revolute joint.
/// </summary>
/// <param name="bodyA">The first constrained body.</param>
/// <param name="bodyB">The second constrained body.</param>
/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
/// <param name="axis">The axis around which to allow relative rotation. Rotation around the other axes will be prevented.</param>
public RevoluteJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axis)
: this(bodyA, bodyB, worldPoint, axis, -MathHelper.PiOver2, MathHelper.PiOver2)
{
}
private void Activate(RigidBody body)
{
body.IsActive = true;
for (int i = 0; i < body.Constraints.Count; i++)
{
var c = body.Constraints[i];
if (!c.BodyA.IsActive && c.BodyA.IsMovable) Activate(body.Constraints[i].BodyA);
if (!c.BodyB.IsActive && c.BodyB.IsMovable) Activate(body.Constraints[i].BodyB);
}
for (int i = 0; i < body.Contacts.Count; i++)
{
var c = body.Contacts[i];
if (!c.BodyA.IsActive && c.BodyA.IsMovable) Activate(c.BodyA);
if (!c.BodyB.IsActive && c.BodyB.IsMovable) Activate(c.BodyB);
}
}
private bool OnCollide(RigidBody b1, RigidBody b2)
{
Body body1 = b1 as Body;
Body body2 = b2 as Body;
if (body1.Tag.Equals("cup_bottom") && body2.Tag.Equals("sphere")) {
Console.WriteLine("CONTACT");
physicsManager.Remove(body2);
bodies.Remove(body2);
} else if (body1.Tag.Equals("sphere") && body2.Tag.Equals("cup_bottom")) {
Console.WriteLine("CONTACT");
physicsManager.Remove(body1);
bodies.Remove(body1);
}
return false;
}
/// <summary>
/// Remove a body from the physics world.
/// </summary>
/// <param name="body">The body to remove.</param>
/// <returns>Returns a value indicating whether the body was successfully removed.</returns>
public bool Remove(RigidBody body)
{
var fg = body as IForceGenerator;
if (fg != null)
{
_generators.Remove(fg);
}
body.Manager = null;
_broadPhase.Remove(body.Skin);
return _bodies.Remove(body);
}
/// <summary>
/// Construct a new point constraint.
/// </summary>
/// <param name="bodyA">The first constrained body.</param>
/// <param name="bodyB">The second constrained body.</param>
/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
public PointConstraint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint)
: base(bodyA, bodyB)
{
Vector3.Transform(ref worldPoint, ref BodyA.WorldInverse.Combined, out _bodyPointA);
Vector3.Transform(ref worldPoint, ref BodyB.WorldInverse.Combined, out _bodyPointB);
}
/// <summary>
/// Reset the constraint to default values.
/// </summary>
public virtual void Recycle()
{
_island = null;
_bodyA = _bodyB = null;
}
/// <summary>
/// Construct a new universal joint.
/// </summary>
/// <param name="bodyA">The first body to constrain.</param>
/// <param name="bodyB">The second body to constrain.</param>
/// <param name="worldPoint">The point in world coordinates shared by both bodies at the time the constraint is created.</param>
/// <param name="axisA">The axis fixed to the first body which the second body can rotate around.</param>
/// <param name="axisB">The axis fixed to the second body which the first body can rotate around.</param>
public UniversalJoint(RigidBody bodyA, RigidBody bodyB, Vector3 worldPoint, Vector3 axisA, Vector3 axisB)
: this(bodyA, bodyB, worldPoint, axisA, axisB,
-MathHelper.PiOver2 + 0.01f, MathHelper.PiOver2 - 0.01f,
-MathHelper.PiOver2 + 0.01f, MathHelper.PiOver2 - 0.01f)
{
}
/// <summary>
/// Construct a new grab constraint.
/// </summary>
/// <param name="body">The body to constrain.</param>
/// <param name="worldPoint">The initial world point to which the body should be constrained.</param>
/// <param name="bodyPoint">The initial body point, in world coordinates, that the constraint is applied to.</param>
public GrabConstraint(RigidBody body, Vector3 worldPoint, Vector3 bodyPoint)
: base(body, body)
{
_worldPoint = worldPoint;
_bodyPoint = bodyPoint;
}
/// <summary>
/// Construct a new grab constraint.
/// </summary>
/// <param name="body">The body to constrain.</param>
/// <param name="worldPoint">The initial world point to which the body should be constrained. It is expected that the body
/// is occupying this point when the constraint is created.</param>
public GrabConstraint(RigidBody body, Vector3 worldPoint)
: this(body, worldPoint, Vector3.Transform(worldPoint, body.WorldInverse.Combined))
{
}
/// <summary>
/// Allows the game to perform any initialization it needs to before starting to run.
/// This is where it can query for any required services and load any non-graphic
/// related content. Calling base.Initialize will enumerate through any components
/// and initialize them as well.
/// </summary>
protected override void Initialize()
{
// TODO: Add your initialization logic here
base.Initialize();
physicsManager.LinearErrorTolerance = .2f;
Model cupBottomModel = Content.Load<Model>("cup_bottom");
Model cupTopModel = Content.Load<Model>("cup_top");
Body cupBottom = new Body(this, cupBottomModel, "cup_bottom");
Body cupTop = new Body(this, cupTopModel, "cup_top");
cupBottom.OnCollision += OnCollide;
for (int i = 0; i < cupBottom.Skin.Count; i++) {
cupBottom.Skin.SetMaterial(cupBottom.Skin[i], new Material(1, .5f));
}
cupBottom.MassProperties = MassProperties.Immovable;
cupBottom.SetWorld(10, Vector3.Zero, Quaternion.Identity);
bodies.Add(cupBottom);
for (int i = 0; i < cupTop.Skin.Count; i++) {
cupTop.Skin.SetMaterial(cupTop.Skin[i], new Material(1, .5f));
}
cupTop.MassProperties = MassProperties.Immovable;
cupTop.SetWorld(10, Vector3.Zero, Quaternion.Identity);
bodies.Add(cupTop);
BodySkin skin = new BodySkin();
skin.DefaultMaterial = new Material(.4f, .5f);
skin.Add(
new PlanePart(new Vector3(0, 0, -1f), Vector3.UnitZ),
new PlanePart(new Vector3(0, 1, 0), -Vector3.UnitY),
new PlanePart(new Vector3(0, 0, 0), Vector3.UnitY),
new PlanePart(new Vector3(1, 0, 0), -Vector3.UnitX),
new PlanePart(new Vector3(-1, 0, 0), Vector3.UnitX)
);
wall = new RigidBody(skin);
wall.SetWorld(10, Vector3.Zero, Quaternion.Identity);
physicsManager.Gravity = new Vector3(0,-9.8f, 0);
physicsManager.Add(cupBottom);
physicsManager.Add(cupTop);
physicsManager.Add(wall);
prevState = Keyboard.GetState();
}
/// <summary>
/// Add a body to the physics world.
/// </summary>
/// <param name="body">The body to add. It must not already be managed by a physics implementation.</param>
public void Add(RigidBody body)
{
if (body.Manager != null)
throw new ArgumentException("Body is already managed by a physics implementation.");
_bodies.Add(body);
_broadPhase.Add(body.Skin);
body.Manager = this;
var fg = body as IForceGenerator;
if (fg != null)
{
_generators.Add(fg);
}
}
private bool UpdateContactStates(RigidBody a, RigidBody b)
{
if (b.OnCollision != null || b.OnSeparation != null)
{
if (b.ContactStates == null)
{
b.ContactStates = new Dictionary<RigidBody, ContactStateFlags>();
}
if (!b.ContactStates.ContainsKey(a))
{
b.ContactStates.Add(a, 0);
}
b.ContactStates[a] |= ContactStateFlags.IsInContact;
if ((b.ContactStates[a] & ContactStateFlags.WasInContact) == 0)
{
if (b.OnCollision != null && b.OnCollision(b, a))
{
b.ContactStates[a] |= ContactStateFlags.IsSuppressed;
return true;
}
}
}
return false;
}
public void processTouchPoints(ReadOnlyTouchPointCollection touches, List<BlobPair> blobPairs, GameTime gameTime)
{
lastTouchPosition = touchPosition;
int tagID = -1;
int tagValue = -1;
Boolean manipulatorControl = false;
if (touches.Count == 2 && touches[0].IsFingerRecognized && touches[1].IsFingerRecognized)
{
manipulatorControl = true;
Manipulator2D[] manipulators;
manipulators = new Manipulator2D[] {
new Manipulator2D(1, touches[1].X, touches[1].Y),
new Manipulator2D(3, touches[0].X, touches[0].Y)
};
manipulationProcessor.Pivot.X = touches[0].X;
manipulationProcessor.Pivot.Y = touches[0].Y;
manipulationProcessor.ProcessManipulators(Timestamp, manipulators);
}
else
{
manipulationProcessor.CompleteManipulation(Timestamp);
}
if (touches.Count >= 1)
{
for (int i = 0; i < touches.Count; i++)
{
if (touches[i].IsTagRecognized)
{
tagID = touches[i].Id;
tagValue = (int)touches[i].Tag.Value;
break;
}
}
/*switch (tagValue)
{
case 4:
viewManager.rotateToSide(4,t);
break;
case 5:
viewManager.rotateToSide(5,t);
break;
case 6:
viewManager.rotateToSide(6,t);
break;
case 7:
viewManager.rotateToSide(7,t);
break;
case 8:
viewManager.rotateToSide(8,t);
break;
}
*/
touchPosition = touches[0];
//First time touch
if (lastTouchPosition == null)
{
Segment s;
s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 0f),
viewManager.Projection, viewManager.View, Matrix.Identity);
s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 1f),
viewManager.Projection, viewManager.View, Matrix.Identity);
float scalar;
Vector3 point;
var c = physics.BroadPhase.Intersect(ref s, out scalar, out point);
if (c != null && c is BodySkin && !(((SolidThing)((BodySkin)c).Owner).getThingType() == 1) && !(((SolidThing)((BodySkin)c).Owner).getThingType() == 2))
{
pickedObject = ((BodySkin)c).Owner;
orientation = pickedObject.Orientation;
pickedDistance = scalar;
pickedObject.IsActive = true;
pickedObjectOffset = pickedObject.Position - point;
pickedObject.IsWeightless = true;
}
//lastOrientation = touches.Count == 1 ? touches[0].Orientation : touches[1].Orientation;
lastOrientation = touches[0].Orientation;
}
else if (pickedObject != null)
{
Segment s;
s.P1 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 0f),
viewManager.Projection, viewManager.View, Matrix.Identity);
s.P2 = game.GraphicsDevice.Viewport.Unproject(new Vector3(touchPosition.X, touchPosition.Y, 1f),
viewManager.Projection, viewManager.View, Matrix.Identity);
Vector3 diff, point;
Vector3.Subtract(ref s.P2, ref s.P1, out diff);
Vector3.Multiply(ref diff, pickedDistance, out diff);
Vector3.Add(ref s.P1, ref diff, out point);
pickedObject.SetVelocity(Vector3.Zero, Vector3.Zero);
if (!manipulatorControl)
{
Vector3 position = Vector3.Add(point, pickedObjectOffset);
pickedObject.SetWorld(position, orientation);
}
pickedObject.IsActive = true;
SolidThing pickedObjectST = (SolidThing)pickedObject;
switch (tagValue)
{
//Pin a block
case 0:
pickedObject.Unfreeze();
pickedObject.Freeze();
break;
//unPin a block
case 1:
if (pickedObjectST.getThingType() == 1)
{
break;
}
else
{
pickedObject.Unfreeze();
break;
}
//Rotate a block
case 2:
pickedObject.SetWorld(pickedObject.Position, Quaternion.CreateFromAxisAngle(new Vector3(0, 0, -1.0f), touchPosition.Orientation));
break;
//Move a block towards or away from camera
case 3:
TouchPoint tagPoint = touches.GetTouchPointFromId(tagID);
float deltaRotation = MathHelper.ToDegrees(lastOrientation) - MathHelper.ToDegrees(tagPoint.Orientation);
Vector3 direction = new Vector3(0, 0, 1.0f);
direction.Normalize();
pickedObject.SetWorld(Vector3.Add(pickedObject.Position, Vector3.Multiply(direction, deltaRotation * 0.03f)));
lastOrientation = tagPoint.Orientation;
break;
}
}
else if (pickedObject != null)
{
pickedObject.IsWeightless = false;
pickedObject = null;
}
}
else if (pickedObject != null)
{
pickedObject.IsWeightless = false;
pickedObject = null;
touchPosition = null;
lastTouchPosition = null;
}
else
{
touchPosition = null;
}
}
/// <summary>
/// Prepare the contact constraint for iterative processing within a single frame. Computes the desired target velocities
/// to attempt to prevent inter-penetration.
/// </summary>
public override void PreProcess()
{
if (this.IsCollisionSuppressed)
{
return;
}
RigidBody a = BodyA, b = BodyB;
var cached = b.Manager.ContactCache.Get(a, b);
bool isWarmStarted = cached != null && cached.Count == _count;
// calculate relative force applied during this frame
Vector3 va = Vector3.Zero, vb = Vector3.Zero;
float forceMag;
if (a.IsMovable)
{
Vector3.Multiply(ref a.Force, this.Manager.TimeStep * a.Mass.MassInverse, out va);
}
if (b.IsMovable)
{
Vector3.Multiply(ref b.Force, this.Manager.TimeStep * b.Mass.MassInverse, out vb);
}
Vector3.Add(ref va, ref vb, out va);
Vector3.Dot(ref _normal, ref va, out forceMag);
forceMag = MathHelper.Min(forceMag, 0f);
for (int i = 0; i < _count; i++)
{
var p = _points[i];
// calculate movement along normal and tangent
float normalDelta;
a.GetVelocityAtPoint(ref p.OffsetA, out va);
b.GetVelocityAtPoint(ref p.OffsetB, out vb);
Vector3.Subtract(ref va, ref vb, out va);
Vector3.Dot(ref _normal, ref va, out normalDelta);
float tangentDelta;
Vector3.Multiply(ref _normal, normalDelta, out p.Tangent);
Vector3.Subtract(ref va, ref p.Tangent, out p.Tangent);
Vector3.Dot(ref p.Tangent, ref va, out tangentDelta);
if (p.Tangent.LengthSquared() >= Constants.Epsilon)
{
p.Tangent.Normalize();
Vector3.Negate(ref p.Tangent, out p.Tangent);
}
else
{
p.Tangent = Vector3.Zero;
}
// calculate effective mass along tangent and normal
p.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref _normal);
p.TangentMass = p.Tangent != Vector3.Zero ?
MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref p.Tangent) : 0f;
// calculate target velocity
float restitution = Math.Max(_restitution * -(normalDelta - forceMag), 0f);
float penetration = (p.Depth - this.Manager.LinearErrorTolerance);
if (restitution < this.Manager.MinRestitution)
{
if (penetration > 0f)
{
p.Target = penetration * this.Manager.PenetrationBias;
}
else
{
float scale = MathHelper.Clamp(-0.1f * penetration / this.Manager.LinearErrorTolerance, Constants.Epsilon, 1f);
p.Target = scale * (p.Depth - this.Manager.LinearErrorTolerance) * this.Manager.TimeStepInverse;
}
}
else
{
p.Target = Math.Max(Math.Max(penetration * this.Manager.PenetrationBias, 0f), restitution);
}
p.Impulse = 0f;
p.TangentImpulse = 0f;
p.PositionImpulse = 0f;
if (isWarmStarted)
{
Vector3 impulse, tangentImpulse;
// find the best cached point
var bestPoint = new CachedContactPoint();
float bestDistance = float.MaxValue;
for (int j = 0; j < cached.Points.Length; j++)
{
float d1, d2;
Vector3.DistanceSquared(ref cached.Points[j].OffsetA, ref p.OffsetA, out d1);
Vector3.DistanceSquared(ref cached.Points[j].OffsetB, ref p.OffsetB, out d2);
if (d1 + d2 < bestDistance)
{
bestDistance = d1 + d2;
bestPoint = cached.Points[j];
}
}
p.Impulse = bestPoint.NormalImpulse;
float tangentImpulseMag = MathHelper.Clamp(-tangentDelta * p.TangentMass, 0f, _friction * p.Impulse);
p.TangentImpulse = tangentImpulseMag * p.NormalMass;
if (Math.Abs(p.Impulse) >= Constants.Epsilon)
{
Vector3.Multiply(ref _normal, p.Impulse, out impulse);
Vector3.Multiply(ref p.Tangent, p.TangentImpulse, out tangentImpulse);
Vector3.Add(ref impulse, ref tangentImpulse, out impulse);
a.ApplyImpulse(ref impulse, ref p.OffsetA);
Vector3.Negate(ref impulse, out impulse);
b.ApplyImpulse(ref impulse, ref p.OffsetB);
}
}
_points[i] = p;
}
// calculate an averaged contact point for help with stabilization during position correction
if (_count > 2 && _count < _points.Length)
{
var ap = _points[_count];
ap.Depth = float.MaxValue;
for (int i = 0; i < _count; i++)
{
var p = _points[i];
float depth;
Vector3 pa, pb;
Vector3.Add(ref a.World.Position, ref p.OffsetA, out pa);
Vector3.Add(ref b.World.Position, ref p.OffsetB, out pb);
Vector3.Add(ref ap.OffsetA, ref pa, out ap.OffsetA);
Vector3.Add(ref ap.OffsetB, ref pb, out ap.OffsetB);
Vector3.Subtract(ref pb, ref pa, out pa);
Vector3.Dot(ref _normal, ref pa, out depth);
if (depth < ap.Depth)
{
ap.Depth = depth;
}
}
Vector3.Divide(ref ap.OffsetA, _count, out ap.OffsetA);
Vector3.Divide(ref ap.OffsetB, _count, out ap.OffsetB);
ap.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref ap.OffsetA, ref ap.OffsetB, ref _normal);
ap.PositionImpulse = 0f;
_points[_count] = ap;
}
}
/// <summary>
/// Construct a new joint.
/// </summary>
/// <param name="bodyA">The first body to constrain.</param>
/// <param name="bodyB">The second body to constrain.</param>
public Joint(RigidBody bodyA, RigidBody bodyB)
: base(bodyA, bodyB)
{
_constraints = new List<Constraint>();
}
/// <summary>
/// Construct a new motor force.
/// </summary>
/// <param name="body">The body on which to apply torque.</param>
/// <param name="worldTorque">The axis and magnitude of the torque in world-space.</param>
public MotorForce(RigidBody body, Vector3 worldTorque)
{
_body = body;
Vector3.Transform(ref worldTorque, ref _body.WorldInverse.Orientation, out _torque);
}