protected override void Update(GameTime gameTime)
{
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed || Keyboard.GetState().IsKeyDown(Keys.Escape))
{
Exit();
}
if (Keyboard.GetState().IsKeyDown(Keys.Up))
{
MovePolygon(Square, new Vector2(0, -1));
}
if (Keyboard.GetState().IsKeyDown(Keys.Down))
{
MovePolygon(Square, new Vector2(0, 1));
}
if (Keyboard.GetState().IsKeyDown(Keys.Left))
{
MovePolygon(Square, new Vector2(-1, 0));
}
if (Keyboard.GetState().IsKeyDown(Keys.Right))
{
MovePolygon(Square, new Vector2(1, 0));
}
LastCollision = GetCollisionResponse(Square, Triangle);
if (LastCollision.Collides)
{
MovePolygon(Square, LastCollision.MTV * 0.5f);
MovePolygon(Triangle, -LastCollision.MTV * 0.5f);
}
base.Update(gameTime);
}
public CollisionResponse CheckSimplified(CollisionSimplifiedScenario scenario)
{
CollisionResponse test = null;
/**
* k: x/a = y/b = z/c
* for a, b, c != 0
*/
if (scenario.Linear.Velocity.X == 0 && scenario.Linear.StartingPosition.X != 0 ||
scenario.Linear.Velocity.Y == 0 && scenario.Linear.StartingPosition.Y != 0 ||
scenario.Linear.Velocity.Z == 0 && scenario.Linear.StartingPosition.Z != 0)
{
return(test);
}
double k;
if (scenario.Linear.Velocity.X != 0)
{
k = scenario.Linear.StartingPosition.X / scenario.Linear.Velocity.X;
}
else
if (scenario.Linear.Velocity.Y != 0)
{
k = scenario.Linear.StartingPosition.Y / scenario.Linear.Velocity.Y;
}
else
if (scenario.Linear.Velocity.Z != 0)
{
k = scenario.Linear.StartingPosition.Z / scenario.Linear.Velocity.Z;
}
else
{
k = 0;
}
// collision cannot occur right on start on just after collision
if (k == 0)
{
return(test);
}
if (scenario.Linear.StartingPosition.X == k * scenario.Linear.Velocity.X ||
scenario.Linear.StartingPosition.Y == k * scenario.Linear.Velocity.Y ||
scenario.Linear.StartingPosition.Z == k * scenario.Linear.Velocity.Z
)
{
// there is collision on this time
if (Math.Abs(k) < scenario.Linear.DeltaTime)
{
test = new CollisionResponse(
(float)Math.Abs(k),
scenario.Linear.StartingPosition + scenario.Linear.Velocity * k
);
}
}
return(test);
}
public Movement Move(Vector2 destination, CollisionResponse filter)
{
IsActive = true;
var movement = _world.Simulate(this, destination, filter);
_bounds = movement.Destination;
_world.Update(this, movement.Origin);
return(movement);
}
public override CollisionResponse CanCollide(WorldBody otherBody)
{
CollisionResponse response = new CollisionResponse();
if (otherBody is ShipWeaponBullet)
{
response.CanCollide = true;
}
return(response);
}
public IMovement Simulate(float x, float y, Func <ICollision, CollisionResponses> filter)
{
return(world.Simulate(this, x, y, (col) =>
{
if (col.Hit == null)
{
return null;
}
return CollisionResponse.Create(col, filter(col));
}));
}
public Movement Simulate(Box box, Vector2 destination, CollisionResponse response)
{
var origin = box.Bounds;
var goal = new Rect(destination, box.Bounds.Size);
var hits = new List <Hit>();
var result = new Movement
{
Origin = origin,
Goal = goal,
Destination = SimulateDestination(hits, new List <Box>
{
box
}, box, origin, goal, response),
Hits = hits
};
return(result);
}
private static bool FruitPolygonCollision(Fruit fruit, Polygon polygon, CCPoint polygonVelocity)
{
// Test whether the fruit collides
bool didCollide = polygon.CollideAgainst(fruit.Collision);
if (didCollide)
{
var circle = fruit.Collision;
// Get the separation vector to reposition the fruit so it doesn't overlap the polygon
var separation = CollisionResponse.GetSeparationVector(circle, polygon);
fruit.Position += separation;
// Adjust the fruit's Velocity to make it bounce:
var normal = separation;
normal.Normalize();
fruit.Velocity = CollisionResponse.ApplyBounce(
fruit.Velocity,
polygonVelocity,
normal,
GameCoefficients.FruitCollisionElasticity);
}
return(didCollide);
}
private extern static void _SetCollisionResponseToChannel(IntPtr handler, CollisionChannel Channel, CollisionResponse NewResponse);
public void SetCollisionResponseToAllChannels(CollisionResponse NewResponse)
{
_SetAngularDamping1(NativeHandler, NewResponse);
}
public void SetCollisionResponseToChannel(CollisionChannel Channel, CollisionResponse NewResponse)
{
_SetCollisionResponseToChannel(NativeHandler, Channel, NewResponse);
}
public Collider()
{
response = Respond;
}
private void Tick(int milliseconds)
{
double dt = milliseconds / 1000.0; // Convert to fractional seconds.
// Integrate forces for each body.
foreach (RigidBodyBase body in doc.Bodies)
{
// Don't move background-anchored bodies.
if (body.anchored)
{
continue;
}
// First, apply gravitational force (if defined).
body.totalForce = new Vector(0, 0);
body.totalAngularForce = 0.0;
if (doc.Gravity != null)
{
Point fp; // force point, body-local coordinates (ink-space)
Vector fv; // force vector, g·isu/s²
doc.Gravity.GetForceForBody(body, out fp, out fv);
// Gravity acts proportional to bodies' mass.
// The fudgefactor variable makes forces match expected perceptions.
double fudgefactor = 3.0;
fv = (fv * body.Mass) / fudgefactor;
body.totalForce += fv;
}
// Integrate the forces and moments from mechanisms.
MechanismBase[] mechs = doc.GetMechanismsForBody(body);
foreach (MechanismBase mech in mechs)
{
// Rods require the delta time to calculate force properly.
if (mech is RodMech)
{
((RodMech)mech).dt = dt;
}
Point fp; // force point, body-local coords (ink-space)
Vector fv; // force vector, g·isu/s²
mech.GetForceForBody(body, out fp, out fv);
body.totalForce += fv;
body.totalAngularForce +=
Vector.Cross(Vector.FromPoints(Point.Empty, fp), fv) / 1e6;
}
// Integrate forces and moments from any pending collisions.
foreach (CollisionResponse cr in this.collisions)
{
if (!Object.ReferenceEquals(cr.body, body))
{
continue;
}
body.totalForce += cr.impulseforce;
body.totalAngularForce += Vector.Cross(Vector.FromPoints(
cr.body.CG, cr.contactpoint), cr.impulseforce) / 1e6;
// If collision is between body not at rest and body at rest,
// then the body at rest is no longer at rest.
if (cr.body.initiallyAtRest &&
!cr.collidingBody.anchored && !cr.collidingBody.initiallyAtRest)
{
cr.body.initiallyAtRest = false;
}
}
}
// Apply forces, to move each body.
foreach (RigidBodyBase body in doc.Bodies)
{
// If body still at rest, don't move it.
if (body.initiallyAtRest)
{
continue;
}
// Add it up, and move the bodies. Note, we're just doing simple
// Euler integration, for now. If integration error proves to be a
// problem, Runge-Kutta or Improved Euler could be implemented here,
// for better results (at the expense of a little performance).
body.Vx += dt * body.totalForce.DX / body.Mass; // isu/sec
body.Vy += dt * body.totalForce.DY / body.Mass; // isu/sec
body.Va += dt * body.totalAngularForce / body.I; // radians/sec
int dx = 0, dy = 0; float da = 0f;
dx = MathEx.Round(dt * body.Vx);
dy = MathEx.Round(dt * body.Vy);
da = (float)Geometry.Rad2Deg(dt * body.Va);
body.Move(dx, dy, da);
// Move force mechanisms
foreach (MechanismBase mech in doc.GetMechanismsForBody(body))
{
if (mech is ForceMechanismBase)
{
mech.Move(dx, dy, da);
}
}
}
// Apply a method to stabilize rods, pin joints, and ropes.
StabilizeBindingMechanisms();
// Everything works better when there are no overlaps, so physically
// separate any objects that are intersecting.
SeparateIntersectingObjects();
// Look for collisions, apply impulses.
collisions.Clear();
foreach (RigidBodyBase body1 in doc.Bodies)
{
foreach (RigidBodyBase body2 in doc.Bodies)
{
if (Object.ReferenceEquals(body1, body2))
{
continue;
}
if (body1.anchored && body2.anchored)
{
continue;
}
Point contactPoint;
PointF normal;
if (!FindIntersection(body1, body2, out contactPoint, out normal))
{
continue;
}
using (Graphics g = wnd.CreateGraphics())
using (Region contactrgn = RigidBodyBase.GetOverlap(body1, body2))
{
if (contactrgn.IsEmpty(g))
{
continue;
}
// We've got a hit; but make sure it's waxing not waning.
int dx1 = 0, dy1 = 0, dx2 = 0, dy2 = 0; float da1 = 0f, da2 = 0f;
dx1 = MathEx.Round(dt * body1.Vx);
dy1 = MathEx.Round(dt * body1.Vy);
da1 = (float)Geometry.Rad2Deg(dt * body1.Va);
dx2 = MathEx.Round(dt * body2.Vx);
dy2 = MathEx.Round(dt * body2.Vy);
da2 = (float)Geometry.Rad2Deg(dt * body2.Va);
using (Matrix m1 = new Matrix())
using (Matrix m2 = new Matrix())
using (Region rgn = new Region())
{
m1.Translate(dx1, dy1);
m1.RotateAt(da1, body1.CG);
m2.Translate(dx2, dy2);
m2.RotateAt(da2, body2.CG);
Region contactrgn1 = body1.rgncache.Clone();
Region contactrgn2 = body2.rgncache.Clone();
contactrgn1.Transform(m1);
contactrgn2.Transform(m2);
rgn.Intersect(contactrgn1);
rgn.Intersect(contactrgn2);
float newarea = Geometry.CalculateArea(rgn);
float oldarea = Geometry.CalculateArea(contactrgn);
if (newarea < oldarea)
{
continue;
}
}
// Calculate contact point, and relative velocities.
// Make a 1000 unit normal so that we can use ints.
Vector collNormal = new Vector(MathEx.Round(normal.X * 1000),
MathEx.Round(normal.Y * 1000));
double collNormalLength = collNormal.Length;
if (collNormalLength == 0)
{
continue;
}
// Find the relative velocity at the collision point.
Vector rvBodies = new Vector(MathEx.Round(body1.Vx - body2.Vx),
MathEx.Round(body1.Vy - body2.Vy));
// Add in the velocity due to rotation of the bodies.
Vector cgToContact1 = Vector.FromPoints(body1.CG, contactPoint);
Vector orthoToCG1 = new Vector(-cgToContact1.DY, cgToContact1.DX);
Vector cgToContact2 = Vector.FromPoints(body2.CG, contactPoint);
Vector orthoToCG2 = new Vector(-cgToContact2.DY, cgToContact2.DX);
Vector rv = rvBodies + orthoToCG1 * body1.Va - orthoToCG2 * body2.Va;
CollisionResponse cr = new CollisionResponse();
cr.body = body2;
cr.collidingBody = body1;
cr.contactpoint = contactPoint;
// Take the smaller of the two elasticities for the collision.
double elasticity = Math.Min(body1.elasticity, body2.elasticity);
// Calculate the change in velocity times mass.
// These formulas come from _Physics for Game Developers_ by Bourg, p 98.
double impulseTimesMass = 0;
if (body1.anchored && collNormalLength > 0)
{
impulseTimesMass = (1 + elasticity) * rv.Dot(collNormal) /
(1 / body2.Mass +
Math.Abs(collNormal.Dot(orthoToCG2) * Vector.Cross(cgToContact2, collNormal)) /
body2.I / 1e6 / collNormal.LengthSq) /
collNormalLength;
}
else if (collNormalLength > 0)
{
impulseTimesMass = (1 + elasticity) * rv.Dot(collNormal) /
(1 / body1.Mass + 1 / body2.Mass +
Math.Abs(collNormal.Dot(orthoToCG1) * Vector.Cross(cgToContact1, collNormal)) /
body1.I / 1e6 / collNormal.LengthSq +
Math.Abs(collNormal.Dot(orthoToCG2) * Vector.Cross(cgToContact2, collNormal)) /
body2.I / 1e6 / collNormal.LengthSq) /
collNormalLength;
}
// Force that will result in that change in velocity.
cr.impulseforce = collNormal * (impulseTimesMass / dt / collNormalLength);
// Add sliding friction for ellipses colliding with polygons.
if (!body2.anchored && body2 is EllipticalBody && body1 is PolygonalBody)
{
// Figure out the velocity parallel to the normal.
double velocityNormal = rvBodies.Dot(collNormal) / collNormalLength;
// The frictional force is proportional to that.
// Note: For some reason, a coefficient of friction of 1.0
// sometimes creates a singularity.
double cfriction = Math.Min(body2.cfriction, .99);
double frictionForceMagnitude =
Math.Abs(velocityNormal) * cfriction * body2.Mass / dt;
// Figure out the velocity orthogonal to the normal.
Vector orthoNormal = new Vector(collNormal.DY, -collNormal.DX);
double velocityOrtho = -rv.Dot(orthoNormal) / collNormalLength;
// You can't have a frictional force that actually reverses the velocity.
double maximumForce = Math.Abs(velocityOrtho * body2.Mass / dt);
try
{
// The frictional force will be along the orthogonal to the normal,
// in the opposite direction of the velocity.
Vector frictionForce = orthoNormal * (-Math.Sign(velocityOrtho) *
Math.Min(frictionForceMagnitude, maximumForce)) / collNormalLength;
// Add friction.
cr.impulseforce += frictionForce;
}
catch (ArithmeticException ex)
{
Console.WriteLine("Silding friction exception: " + ex.ToString());
}
}
collisions.Add(cr);
}
}
}
}
private Rect SimulateDestination(List <Hit> hits, List <Box> ignoring, Box box, Rect origin, Rect destination, CollisionResponse response)
{
var nearest = Hit(origin, destination, ignoring);
if (nearest.IsHit)
{
hits.Add(nearest);
var impact = new Rect(nearest.Position, origin.Size);
var collision = new Collision
{
Box = box, Hit = nearest, Goal = destination, Origin = origin
};
var dest = response(collision);
if (dest != null && destination != dest)
{
ignoring.Add(nearest.Box);
if (ignoring.Count > 2)
{
return(dest.Value);
}
else
{
return(SimulateDestination(hits, ignoring, box, impact, dest.Value, response));
}
}
}
return(destination);
}
private void Tick(int milliseconds)
{
double dt = milliseconds/1000.0; // Convert to fractional seconds.
// Integrate forces for each body.
foreach (RigidBodyBase body in doc.Bodies)
{
// Don't move background-anchored bodies.
if (body.anchored) continue;
// First, apply gravitational force (if defined).
body.totalForce = new Vector(0,0);
body.totalAngularForce = 0.0;
if (doc.Gravity != null)
{
Point fp; // force point, body-local coordinates (ink-space)
Vector fv; // force vector, g·isu/s²
doc.Gravity.GetForceForBody(body, out fp, out fv);
// Gravity acts proportional to bodies' mass.
// The fudgefactor variable makes forces match expected perceptions.
double fudgefactor = 3.0;
fv = (fv*body.Mass)/fudgefactor;
body.totalForce += fv;
}
// Integrate the forces and moments from mechanisms.
MechanismBase[] mechs = doc.GetMechanismsForBody(body);
foreach (MechanismBase mech in mechs)
{
// Rods require the delta time to calculate force properly.
if (mech is RodMech)
{
((RodMech)mech).dt = dt;
}
Point fp; // force point, body-local coords (ink-space)
Vector fv; // force vector, g·isu/s²
mech.GetForceForBody(body, out fp, out fv);
body.totalForce += fv;
body.totalAngularForce +=
Vector.Cross(Vector.FromPoints(Point.Empty,fp), fv)/1e6;
}
// Integrate forces and moments from any pending collisions.
foreach (CollisionResponse cr in this.collisions)
{
if (!Object.ReferenceEquals(cr.body,body)) continue;
body.totalForce += cr.impulseforce;
body.totalAngularForce += Vector.Cross(Vector.FromPoints(
cr.body.CG,cr.contactpoint), cr.impulseforce)/1e6;
// If collision is between body not at rest and body at rest,
// then the body at rest is no longer at rest.
if (cr.body.initiallyAtRest &&
!cr.collidingBody.anchored && !cr.collidingBody.initiallyAtRest)
{
cr.body.initiallyAtRest = false;
}
}
}
// Apply forces, to move each body.
foreach (RigidBodyBase body in doc.Bodies)
{
// If body still at rest, don't move it.
if (body.initiallyAtRest)
continue;
// Add it up, and move the bodies. Note, we're just doing simple
// Euler integration, for now. If integration error proves to be a
// problem, Runge-Kutta or Improved Euler could be implemented here,
// for better results (at the expense of a little performance).
body.Vx += dt * body.totalForce.DX/body.Mass; // isu/sec
body.Vy += dt * body.totalForce.DY/body.Mass; // isu/sec
body.Va += dt * body.totalAngularForce/body.I; // radians/sec
int dx=0,dy=0; float da=0f;
dx = MathEx.Round(dt * body.Vx);
dy = MathEx.Round(dt * body.Vy);
da = (float)Geometry.Rad2Deg(dt * body.Va);
body.Move(dx,dy,da);
// Move force mechanisms
foreach (MechanismBase mech in doc.GetMechanismsForBody(body))
{
if (mech is ForceMechanismBase)
mech.Move(dx,dy,da);
}
}
// Apply a method to stabilize rods, pin joints, and ropes.
StabilizeBindingMechanisms();
// Everything works better when there are no overlaps, so physically
// separate any objects that are intersecting.
SeparateIntersectingObjects();
// Look for collisions, apply impulses.
collisions.Clear();
foreach (RigidBodyBase body1 in doc.Bodies)
foreach (RigidBodyBase body2 in doc.Bodies)
{
if (Object.ReferenceEquals(body1,body2)) continue;
if (body1.anchored && body2.anchored) continue;
Point contactPoint;
PointF normal;
if (!FindIntersection(body1, body2, out contactPoint, out normal)) continue;
using (Graphics g = wnd.CreateGraphics())
using (Region contactrgn = RigidBodyBase.GetOverlap(body1,body2))
{
if (contactrgn.IsEmpty(g)) continue;
// We've got a hit; but make sure it's waxing not waning.
int dx1=0,dy1=0,dx2=0,dy2=0; float da1=0f,da2=0f;
dx1 = MathEx.Round(dt * body1.Vx);
dy1 = MathEx.Round(dt * body1.Vy);
da1 = (float)Geometry.Rad2Deg(dt * body1.Va);
dx2 = MathEx.Round(dt * body2.Vx);
dy2 = MathEx.Round(dt * body2.Vy);
da2 = (float)Geometry.Rad2Deg(dt * body2.Va);
using (Matrix m1 = new Matrix())
using (Matrix m2 = new Matrix())
using (Region rgn = new Region())
{
m1.Translate(dx1,dy1);
m1.RotateAt(da1,body1.CG);
m2.Translate(dx2,dy2);
m2.RotateAt(da2,body2.CG);
Region contactrgn1 = body1.rgncache.Clone();
Region contactrgn2 = body2.rgncache.Clone();
contactrgn1.Transform(m1);
contactrgn2.Transform(m2);
rgn.Intersect(contactrgn1);
rgn.Intersect(contactrgn2);
float newarea = Geometry.CalculateArea(rgn);
float oldarea = Geometry.CalculateArea(contactrgn);
if (newarea < oldarea)
continue;
}
// Calculate contact point, and relative velocities.
// Make a 1000 unit normal so that we can use ints.
Vector collNormal = new Vector(MathEx.Round(normal.X * 1000),
MathEx.Round(normal.Y * 1000));
double collNormalLength = collNormal.Length;
if (collNormalLength == 0)
continue;
// Find the relative velocity at the collision point.
Vector rvBodies = new Vector(MathEx.Round(body1.Vx - body2.Vx),
MathEx.Round(body1.Vy - body2.Vy));
// Add in the velocity due to rotation of the bodies.
Vector cgToContact1 = Vector.FromPoints(body1.CG, contactPoint);
Vector orthoToCG1 = new Vector(-cgToContact1.DY, cgToContact1.DX);
Vector cgToContact2 = Vector.FromPoints(body2.CG, contactPoint);
Vector orthoToCG2 = new Vector(-cgToContact2.DY, cgToContact2.DX);
Vector rv = rvBodies + orthoToCG1 * body1.Va - orthoToCG2 * body2.Va;
CollisionResponse cr = new CollisionResponse();
cr.body = body2;
cr.collidingBody = body1;
cr.contactpoint = contactPoint;
// Take the smaller of the two elasticities for the collision.
double elasticity = Math.Min(body1.elasticity, body2.elasticity);
// Calculate the change in velocity times mass.
// These formulas come from _Physics for Game Developers_ by Bourg, p 98.
double impulseTimesMass = 0;
if (body1.anchored && collNormalLength > 0)
{
impulseTimesMass = (1 + elasticity) * rv.Dot(collNormal) /
(1/ body2.Mass +
Math.Abs(collNormal.Dot(orthoToCG2) * Vector.Cross(cgToContact2, collNormal)) /
body2.I / 1e6 / collNormal.LengthSq) /
collNormalLength;
}
else if (collNormalLength > 0)
{
impulseTimesMass = (1 + elasticity) * rv.Dot(collNormal) /
(1/body1.Mass + 1/body2.Mass +
Math.Abs(collNormal.Dot(orthoToCG1) * Vector.Cross(cgToContact1, collNormal)) /
body1.I / 1e6 / collNormal.LengthSq +
Math.Abs(collNormal.Dot(orthoToCG2) * Vector.Cross(cgToContact2, collNormal)) /
body2.I / 1e6 / collNormal.LengthSq) /
collNormalLength;
}
// Force that will result in that change in velocity.
cr.impulseforce = collNormal * (impulseTimesMass / dt / collNormalLength);
// Add sliding friction for ellipses colliding with polygons.
if (!body2.anchored && body2 is EllipticalBody && body1 is PolygonalBody)
{
// Figure out the velocity parallel to the normal.
double velocityNormal = rvBodies.Dot(collNormal) / collNormalLength;
// The frictional force is proportional to that.
// Note: For some reason, a coefficient of friction of 1.0
// sometimes creates a singularity.
double cfriction = Math.Min(body2.cfriction, .99);
double frictionForceMagnitude =
Math.Abs(velocityNormal) * cfriction * body2.Mass / dt;
// Figure out the velocity orthogonal to the normal.
Vector orthoNormal = new Vector(collNormal.DY, -collNormal.DX);
double velocityOrtho = -rv.Dot(orthoNormal) / collNormalLength;
// You can't have a frictional force that actually reverses the velocity.
double maximumForce = Math.Abs(velocityOrtho * body2.Mass / dt);
try
{
// The frictional force will be along the orthogonal to the normal,
// in the opposite direction of the velocity.
Vector frictionForce = orthoNormal * (-Math.Sign(velocityOrtho) *
Math.Min(frictionForceMagnitude, maximumForce)) / collNormalLength;
// Add friction.
cr.impulseforce += frictionForce;
}
catch (ArithmeticException ex)
{
Console.WriteLine("Silding friction exception: " + ex.ToString());
}
}
collisions.Add(cr);
}
}
}
private extern static void _SetAngularDamping1(IntPtr handler, CollisionResponse NewResponse);
public void Hit(CollisionResponse collisionResponse)
{
OnCollision(
new CollisionEventArgs(collisionResponse)
);
}
//public bool HasCollision { get { return (LastCollisionNormal != null && LastCollisionNormal.LengthSquared > 0); } }
public void HandleCollision(CollisionResponse collisionResponse)
{
throw new NotImplementedException();
}
public CollisionEventArgs(CollisionResponse collisionResponse)
{
CollisionResponse = collisionResponse;
}
/**
* <summary>
* Sphere must be the first, not moving object in center
* just the second object can move
* </summary>
*/
public CollisionResponse CheckSimplified(CollisionSimplifiedScenario scenario)
{
CollisionResponse test = null;
// 1. first escape scenario -point is too far to collide with sphere
// it may happen if (|point center| - R) ^2 <= movement ^2
double R = scenario.ColliderShape1.CalculateMaximumRadius;
Vector3D totalDisplacement;
totalDisplacement = scenario.Linear.Velocity * scenario.Linear.DeltaTime;
Vector3D minimumDistanceToSphere = new Point3D(0, 0, 0) - scenario.Linear.StartingPosition;
Vector3D absoluteminimumDistanceToSphere = minimumDistanceToSphere.Abs();
absoluteminimumDistanceToSphere.X = Math.Max(0, absoluteminimumDistanceToSphere.X - R);
absoluteminimumDistanceToSphere.Y = Math.Max(0, absoluteminimumDistanceToSphere.Y - R);
absoluteminimumDistanceToSphere.Z = Math.Max(0, absoluteminimumDistanceToSphere.Z - R);
if (absoluteminimumDistanceToSphere.LengthSquared > totalDisplacement.LengthSquared)
{
return(test);
}
// 2. check direction of displacement. If point does not move towards the center
// escape, as can never collide with sphere
Vector3D PointTowardsSphereCenter = minimumDistanceToSphere;
double displacementDirection = Vector3D.DotProduct(PointTowardsSphereCenter, totalDisplacement);
if (displacementDirection < 0)
{
return(test);
}
// 3. Check calculation
Vector3D VelocityDirectionNormalized = new Vector3D(totalDisplacement.X, totalDisplacement.Y, totalDisplacement.Z);
VelocityDirectionNormalized.Normalize();
double lo = Vector3D.DotProduct(VelocityDirectionNormalized, PointTowardsSphereCenter);
Vector3D TowardCenterAbsolute = PointTowardsSphereCenter.Abs();
double sqareRootValue = lo * lo - TowardCenterAbsolute.LengthSquared + R * R;
// 3a. escape condition
if (sqareRootValue < 0)
{
return(test);
}
double squareCalculatedValue = Math.Sqrt(sqareRootValue);
double d1 = Math.Abs(-lo + squareCalculatedValue);
double d2 = Math.Abs(-lo - squareCalculatedValue);
double d = Math.Min(d1, d2);
Vector3D FastestCollisionLength = d * VelocityDirectionNormalized;
// the last escape, collision would happen, but in next frames
if (FastestCollisionLength.LengthSquared > totalDisplacement.LengthSquared)
{
return(test);
}
test = new CollisionResponse(
(float)d,
scenario.Linear.StartingPosition + FastestCollisionLength
);
return(test);
}
public void Update(GameTime time)
{
var state = Keyboard.GetState();
if (previous.IsKeyUp(Keys.N) && state.IsKeyDown(Keys.N))
{
moveDestination = !moveDestination;
this.selected.Location = (moveDestination ? goal.Location : origin.Location) - this.selected.Size / 2;
}
if (previous.IsKeyUp(Keys.R) && state.IsKeyDown(Keys.R))
{
response = (response + 1) % values.Length;
}
previous = state;
this.isMoving = state.IsKeyDown(Keys.Space);
var m = Mouse.GetState().Position;
var pos = new Base.Vector2(m.X, m.Y);
var size = isMoving ? 18 : 6;
this.cursor = new RectangleF(m.X - size / 2, m.Y - size / 2, size, size);
if (isMoving)
{
this.selected.Location = pos - this.selected.Size / 2;
if (moveDestination)
{
this.goal.Location = pos;
}
else
{
this.origin.Location = pos;
}
}
// Calculate collision
var hit = Hit.Resolve(origin, goal, other);
var r = this.values[response];
if (hit != null && r != CollisionResponses.None)
{
this.collision = new RectangleF(hit.Position, origin.Size);
this.normal = new RectangleF(this.collision.Center + hit.Normal * 50, new Base.Vector2(5, 5));
// Destination
var collisionPoint = new Collision()
{
Origin = origin,
Goal = goal,
Hit = hit,
};
this.destination = CollisionResponse.Create(collisionPoint, r)?.Destination ?? goal;
}
else
{
this.collision = new RectangleF();
this.normal = new RectangleF();
this.destination = this.goal;
}
}
/**
* <summary>
* Sphere must be the first, not moving object in center
* just the second object can move
* </summary>
*/
public CollisionResponse CheckSimplified(CollisionSimplifiedScenario scenario)
{
CollisionResponse test = null;
// 1. first escape scenario -point is too far to collide with sphere
// it may happen if (|point center| - R) ^2 <= movement ^2
double R1 = scenario.ColliderShape1.CalculateMaximumRadius;
double R2 = scenario.ColliderShape2.CalculateMaximumRadius;
Vector3D totalDisplacement;
totalDisplacement = scenario.Linear.Velocity * scenario.Linear.DeltaTime;
Vector3D DistanceToSphereCenter = new Point3D(0, 0, 0) - scenario.Linear.StartingPosition;
Vector3D absoluteminimumDistanceToSphere = DistanceToSphereCenter.Abs();
absoluteminimumDistanceToSphere.X = Math.Max(0, absoluteminimumDistanceToSphere.X - R1 - R2);
absoluteminimumDistanceToSphere.Y = Math.Max(0, absoluteminimumDistanceToSphere.Y - R1 - R2);
absoluteminimumDistanceToSphere.Z = Math.Max(0, absoluteminimumDistanceToSphere.Z - R1 - R2);
if (absoluteminimumDistanceToSphere.LengthSquared > totalDisplacement.LengthSquared)
{
return(test);
}
// 2. check direction of displacement. If point does not move towards the center
// escape, as can never collide with sphere
Vector3D VelocityDirectionNormalized = new Vector3D(totalDisplacement.X, totalDisplacement.Y, totalDisplacement.Z);
VelocityDirectionNormalized.Normalize();
Vector3D PointTowardsSphereCenter = DistanceToSphereCenter;
double displacementDirection = Vector3D.DotProduct(VelocityDirectionNormalized, PointTowardsSphereCenter);
// does not move towards the sphere center
if (displacementDirection < 0)
{
return(test);
}
// will be close but too far to sphere
double RadiusSum = (R1 + R2);
double RadiusSumSquared = RadiusSum * RadiusSum;
double SquaredMinimumDistanceEverOnVelocityVector = DistanceToSphereCenter.LengthSquared - (displacementDirection * displacementDirection);
if (SquaredMinimumDistanceEverOnVelocityVector > RadiusSumSquared)
{
return(test);
}
//double MinimumDistanceEverOnVelocityVector = Math.Sqrt(SquaredMinimumDistanceEverOnVelocityVector);
double MinimumCollisionLength = Math.Sqrt(RadiusSumSquared - SquaredMinimumDistanceEverOnVelocityVector);
Vector3D MinimumCollisionVector = VelocityDirectionNormalized * (displacementDirection - MinimumCollisionLength);
// the last escape, collision would happen, but in next frames
if (MinimumCollisionVector.LengthSquared > totalDisplacement.LengthSquared)
{
return(test);
}
double k;
if (scenario.Linear.Velocity.X != 0)
{
k = MinimumCollisionVector.X / totalDisplacement.X;
}
else
if (scenario.Linear.Velocity.Y != 0)
{
k = MinimumCollisionVector.Y / totalDisplacement.Y;
}
else
if (scenario.Linear.Velocity.Z != 0)
{
k = MinimumCollisionVector.Z / totalDisplacement.Z;
}
else
{
k = 0;
}
// spheres were intersecting before test
if (k <= 0)
{
return(test);
}
double deltaTime = k * scenario.Linear.DeltaTime;
test = new CollisionResponse(
(float)deltaTime,
scenario.Linear.StartingPosition + MinimumCollisionVector
);
return(test);
}