public virtual int Collide(PShape s1, PShape s2, PContact[] cs)
{
if (s1._type != Physics.PShapeType.CIRCLE_SHAPE ||
s2._type != Physics.PShapeType.CIRCLE_SHAPE)
{
return(0);
}
PCircleShape c1 = (PCircleShape)s1;
PCircleShape c2 = (PCircleShape)s2;
Vector2f normal = c2._pos.Sub(c1._pos);
float rad = c1.rad + c2.rad;
float length = normal.Length();
if (length < rad)
{
PContact c = new PContact();
c.overlap = length - rad;
normal.Normalize();
c.pos.Set(c1._pos.x + normal.x * c1.rad, c1._pos.y + normal.y
* c1.rad);
c.normal.Set(-normal.x, -normal.y);
cs[0] = c;
return(1);
}
else
{
return(0);
}
}
public static PolarVector CartesianToPolarCoordinateRadian(this Vector2f coordinate)
{
PolarVector polarVector = new PolarVector();
polarVector.r = coordinate.Length();
polarVector.theta = (float)Math.Atan(coordinate.Y / coordinate.X);
return(polarVector);
}
/// <summary>
/// Anchors the object to a position
/// </summary>
/// <param name="objX">X of the object to anchor</param>
/// <param name="objY">Y of the object to anchor</param>
/// <param name="anchorX">X of the anchor</param>
/// <param name="anchorY">Y of the anchor</param>
/// <param name="distance">The max distance that the object can be from the anchor</param>
public static void AnchorTo(ref float objX, ref float objY, float anchorX, float anchorY, float distance = 0, float?minDistance = null)
{
var point = new Vector2f(objX - anchorX, objY - anchorY);
if (point.Length() > distance)
{
point = point.Normalized(distance);
}
if (minDistance.HasValue && point.Length() < minDistance.Value)
{
point = point.Normalized(minDistance.Value);
}
objX = anchorX + point.X;
objY = anchorY + point.Y;
}
public static Vector2f Normalize(Vector2f vec)
{
float length = vec.Length();
vec.X /= length;
vec.Y /= length;
return(new Vector2f(vec.X, vec.Y));
}
public static Vector2f Rotate(this Vector2f vector, double radians)
{
var vectorAngle = vector.Angle();
var length = vector.Length();
float x = (float)(length * Math.Cos(radians + vectorAngle));
float y = (float)(length * Math.Sin(radians + vectorAngle));
return(new Vector2f(x, y));
}
public static Vector2f Normalize(this Vector2f vector)
{
var length = vector.Length();
return(new Vector2f
{
X = (float)(vector.X / length),
Y = (float)(vector.Y / length)
});
}
public static Vector2f Normalize(this Vector2f vector)
{
var length = vector.Length();
if (length == 0)
{
return(vector);
}
return(vector / length);
}
public static Vector2f Normalized(this Vector2f v)
{
float l = v.Length();
if (l == 0f)
{
return(new Vector2f(0, 0));
}
return(v / l);
}
private void Stop(IGameObject self)
{
var rotationDirection = GetRotateSign(self.Body.Rotation, -velocity);
if (rotationDirection == 0)
{
if (velocity.Length() < 1)
{
velocity = new Vector2f();
}
else
{
velocity += new Vector2f(Math.Min(throttle, velocity.Length()), 0).Rotate(self.Body.Rotation.Angle());
}
}
else
{
self.Body.Rotate(rotationDirection * turnSpeed);
}
}
private Direction DetermineDirection(Vector2f from, Vector2f to)
{
Vector2f delta = to - from;
if (delta.Length() < 0.1)
{
return(_lastDeterminedDirection);
}
_lastDeterminedDirection = from.DirectionTo(to, fallback: _lastDeterminedDirection);
return(_lastDeterminedDirection);
}
public static Vector2f Normalize(this Vector2f vector)
{
var length = vector.Length();
if (length != 0)
{
return(new Vector2f(vector.X / length, vector.Y / length));
}
else
{
return(new Vector2f(0, 0));
}
}
public PhysicalModel(Vector2f position, Vector2f size, double weight, bool isSolid, bool isStatic)
{
Collider = new AABB(position, size);
Incircle = new RoundShape(position + size / 2, Math.Min(Size.X, Size.Y) / 2);
Circumcircle = new RoundShape(position + size / 2, size.Length() / 2);
Position = position;
Velocity = new Vector2f();
Rotation = new Vector2f(1, 0);
IsSolid = isSolid;
IsStatic = isStatic;
ColliderOffset = new Vector2f();
Weight = weight;
}
public PhysicalModel(Vector2f position, Vector2f size, double weight, bool isSolid, bool isStatic)
{
Collider = new AABB(position, size);
Incircle = new RoundShape(position + size / 2, Math.Min(Size.X, Size.Y) / 2);
Circumcircle = new RoundShape(position + size / 2, size.Length() / 2);
Position = position;
Velocity = new Vector2f();
Rotation = new Vector2f(1, 0);
IsSolid = isSolid;
IsStatic = isStatic;
ColliderOffset = new Vector2f();
Weight = weight;
}
internal override void PreSolve(float dt)
{
relAnchor1 = b1.mAng.Mul(localAnchor1);
relAnchor2 = b2.mAng.Mul(localAnchor2);
anchor1.Set(relAnchor1.x + b1.pos.x, relAnchor1.y + b1.pos.y);
anchor2.Set(relAnchor2.x + b2.pos.x, relAnchor2.y + b2.pos.y);
normal = anchor2.Sub(anchor1);
length = normal.Length();
normal.Normalize();
mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
normal);
b1.ApplyImpulse(normal.x * norI, normal.y * norI, anchor1.x, anchor1.y);
b2.ApplyImpulse(normal.x * -norI, normal.y * -norI, anchor2.x,
anchor2.y);
}
public void Update(Vector2f acceleration, TimeSpan time)
{
Velocity += new Vector2f
{
X = acceleration.X * (float)time.TotalSeconds,
Y = acceleration.Y * (float)time.TotalSeconds
};
if (Velocity.Length() > MaxVelocity)
{
var normalizedVelocity = Velocity.Normalize();
Velocity = normalizedVelocity.Scale(MaxVelocity);
}
Position += Velocity;
}
/// <summary>
/// Steps X and Y coordinates towards a point.
/// </summary>
/// <param name="x">The X value to move</param>
/// <param name="y">The Y value to move</param>
/// <param name="toX">X position to step towards.</param>
/// <param name="toY">Y position to step towards.</param>
/// <param name="distance">The distance to step (will not overshoot target).</param>
public static void StepTowards(ref float x, ref float y, float toX, float toY, float distance)
{
var point = new Vector2f(toX - x, toY - y);
if (point.Length() <= distance)
{
x = toX;
y = toY;
return;
}
point = point.Normalized(distance);
x += point.X;
y += point.Y;
}
private Direction DetermineDirection(Vector2f from, Vector2f to)
{
Vector2f delta = to - from;
if (delta.Length() < 0.1f)
{
return(_lastDeterminedDirection);
}
if (delta.X > 0 && delta.Y > 0)
{
_lastDeterminedDirection = Direction.SouthEast;
}
if (delta.X > 0 && delta.Y < 0)
{
_lastDeterminedDirection = Direction.NorthEast;
}
if (delta.X < 0 && delta.Y > 0)
{
_lastDeterminedDirection = Direction.SouthWest;
}
if (delta.X < 0 && delta.Y < 0)
{
_lastDeterminedDirection = Direction.NorthWest;
}
if (delta.X > 0 && Math.Abs(0 - delta.Y) < 0.05f)
{
_lastDeterminedDirection = Direction.East;
}
if (delta.X < 0 && Math.Abs(0 - delta.Y) < 0.05f)
{
_lastDeterminedDirection = Direction.West;
}
if (delta.Y > 0 && Math.Abs(0 - delta.X) < 0.05f)
{
_lastDeterminedDirection = Direction.South;
}
if (delta.Y < 0 && Math.Abs(0 - delta.X) < 0.05f)
{
_lastDeterminedDirection = Direction.North;
}
return(_lastDeterminedDirection);
}
/// <summary>
/// Scales the line segment between (0,0) and the current point to a set length.
/// </summary>
/// <param name="vec">Self</param>
/// <param name="amount">The length to scale the vector to</param>
/// <returns>The normalized vector</returns>
public static Vector2f Normalized(this Vector2f vec, float amount = 1)
{
if (vec.IsZero())
{
return(vec);
}
double val = 1.0 / vec.Length();
var x = vec.X * val;
var y = vec.Y * val;
x *= amount;
y *= amount;
vec.X = (float)x;
vec.Y = (float)y;
return(vec);
}
internal override void SolvePosition()
{
if (enableLimit && limitState != 0)
{
float over = b2.ang - b1.ang - localAngle;
if (over < minAngle)
{
over += 0.008F;
over = ((over - minAngle) + b2.correctAngVel)
- b1.correctAngVel;
float torque = over * 0.2F * angM;
float subAngleImpulse = angI;
angI = MathUtils.Min(angI + torque, 0.0F);
torque = angI - subAngleImpulse;
b1.PositionCorrection(torque);
b2.PositionCorrection(-torque);
}
if (over > maxAngle)
{
over -= 0.008F;
over = ((over - maxAngle) + b2.correctAngVel)
- b1.correctAngVel;
float torque_0 = over * 0.2F * angM;
float subAngleImpulse_1 = angI;
angI = MathUtils.Max(angI + torque_0, 0.0F);
torque_0 = angI - subAngleImpulse_1;
b1.PositionCorrection(torque_0);
b2.PositionCorrection(-torque_0);
}
}
Vector2f force = anchor2.Sub(anchor1);
force.SubLocal(PTransformer.CalcRelativeCorrectVelocity(b1, b2,
relAnchor1, relAnchor2));
float length = force.Length();
force.Normalize();
force.MulLocal(System.Math.Max(length * 0.2F - 0.002F, 0.0F));
mass.MulEqual(force);
b1.PositionCorrection(force.x, force.y, anchor1.x, anchor1.y);
b2.PositionCorrection(-force.x, -force.y, anchor2.x, anchor2.y);
}
public static PolarVector2f ToPolarCoordinates(this Vector2f vector)
{
if (vector == Zero)
{
return(new PolarVector2f(0, 0));
}
var quadrant = vector.Quadrant();
var quadrantAddition = quadrant switch {
2 => 180,
3 => 180,
4 => 360,
_ => 0
};
var degrees = (float)(Math.Atan(vector.Y / vector.X) * (180 / Math.PI));
var polar = new PolarVector2f(
vector.Length(),
degrees + quadrantAddition + 90
);
return(polar);
}
internal override void PreSolve(float i_0)
{
relAnchor1 = b1.mAng.Mul(localAnchor1);
relAnchor2 = b2.mAng.Mul(localAnchor2);
anchor1.Set(relAnchor1.x + b1.pos.x, relAnchor1.y + b1.pos.y);
anchor2.Set(relAnchor2.x + b2.pos.x, relAnchor2.y + b2.pos.y);
normal = anchor2.Sub(anchor1);
float over = dist - normal.Length();
normal.Normalize();
mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
normal);
float k = mass * 1000F * str;
force = -over * k;
force += PTransformer.CalcRelativeVelocity(b1, b2, relAnchor1,
relAnchor2).Dot(normal)
* damp * -(float)System.Math.Sqrt(k * mass) * 2.0F;
force *= i_0;
b1.ApplyImpulse(normal.x * force, normal.y * force, anchor1.x,
anchor1.y);
b2.ApplyImpulse(normal.x * -force, normal.y * -force, anchor2.x,
anchor2.y);
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs)
{
if (s1._type != PShapeType.CIRCLE_SHAPE ||
s2._type != PShapeType.CONVEX_SHAPE &&
s2._type != PShapeType.BOX_SHAPE)
{
return(0);
}
PCircleShape c1 = (PCircleShape)s1;
PConvexPolygonShape p1 = (PConvexPolygonShape)s2;
float distance = -1F;
int edgeNumber = -1;
Vector2f[] vers = p1.vers;
int numVers = p1.numVertices;
Vector2f normal = new Vector2f();
Vector2f edgeNormal = new Vector2f();
Vector2f a = new Vector2f();
Vector2f b = new Vector2f();
int num = 0;
for (int i = 0; i < numVers; i++)
{
a.Set(c1._pos.x - vers[i].x, c1._pos.y - vers[i].y);
distance = a.Length();
distance -= c1.rad;
if (distance <= 0.0F)
{
PContact c = new PContact();
c.overlap = distance;
a.Normalize();
c.normal.Set(a.x, a.y);
c.pos.Set(vers[i].x, vers[i].y);
cs[num] = c;
if (++num == 2)
{
return(num);
}
}
}
if (num > 0)
{
return(num);
}
for (int i_0 = 0; i_0 < numVers; i_0++)
{
Vector2f ver = vers[i_0];
Vector2f nextVer = vers[(i_0 + 1) % numVers];
float edgeX = nextVer.x - ver.x;
float edgeY = nextVer.y - ver.y;
edgeNormal.Set(edgeY, -edgeX);
edgeNormal.Normalize();
a.Set(c1._pos.x - ver.x, c1._pos.y - ver.y);
b.Set(c1._pos.x - nextVer.x, c1._pos.y - nextVer.y);
if ((a.x * edgeX + a.y * edgeY) * (b.x * edgeX + b.y * edgeY) <= 0.0F)
{
float edgeLen = (float)System.Math.Sqrt(edgeX * edgeX + edgeY * edgeY);
float distanceToEdge = System.Math.Abs(a.x * edgeY - a.y * edgeX)
/ edgeLen;
if (distanceToEdge <= c1.rad)
{
distanceToEdge -= c1.rad;
if (distance > distanceToEdge || distance == -1F)
{
edgeNumber = i_0;
distance = distanceToEdge;
normal.Set(edgeNormal.x, edgeNormal.y);
}
}
}
}
if (edgeNumber > -1)
{
PContact c_1 = new PContact();
c_1.overlap = distance;
c_1.normal = normal;
c_1.pos = c1._pos.Sub(normal.Mul(c1.rad));
cs[0] = c_1;
return(1);
}
bool hit = true;
for (int i_2 = 0; i_2 < numVers; i_2++)
{
Vector2f ver = vers[i_2];
Vector2f nextVer = vers[(i_2 + 1) % numVers];
float v1x = nextVer.x - ver.x;
float v1y = nextVer.y - ver.y;
float v2x = c1._pos.x - ver.x;
float v2y = c1._pos.y - ver.y;
if (v1x * v2y - v1y * v2x >= 0.0F)
{
continue;
}
hit = false;
break;
}
if (hit)
{
distance = 1.0F;
normal = new Vector2f();
for (int i = 0; i < numVers; i++)
{
Vector2f ver = vers[i];
Vector2f nextVer = vers[(i + 1) % numVers];
a.Set(nextVer.x - ver.x, nextVer.y - ver.y);
a.Normalize();
float d = c1._pos.Sub(ver).Cross(a);
if (d < 0.0F && (distance == 1.0F || distance < d))
{
distance = d;
normal.Set(a.y, -a.x);
}
}
if (distance != 1.0F)
{
PContact c = new PContact();
c.normal.Set(normal.x, normal.y);
c.pos.Set(c1._pos.x, c1._pos.y);
c.overlap = distance;
cs[0] = c;
return(1);
}
}
return(0);
}
public static Vector2f ProectionTo(this Vector2f v1, Vector2f v2)
{
return(v2.Normalize() * v1.ScalarMultiplicate(v2) / v2.Length());
}
public static double Trace(AABB aabb, Vector2f move, AABB[] obstacles, out Vector2f normal)
{
normal = Vector2f.Zero;
var tMin = double.PositiveInfinity;
// no movement, nothing to trace
if (move.X.Eq(0) && move.Y.Eq(0))
{
return(tMin);
}
var movedAabb = aabb + move;
if (move.X.Eq(0) || move.Y.Eq(0))
{
var path = new AABB(
new Vector2f(Math.Min(aabb.Right, movedAabb.Left), Math.Min(aabb.Top, movedAabb.Bottom)),
new Vector2f(Math.Max(aabb.Left, movedAabb.Right), Math.Max(aabb.Bottom, movedAabb.Top)));
var obstaclesInPath = obstacles.Where(o => o.Overlap(path)).ToArray();
if (obstaclesInPath.Length != 0)
{
normal = move.Inv().Normalize();
// find closest obstacle in path
if (move.X > 0)
{
tMin = Math.Abs(obstaclesInPath.Min(o => o.Left) - aabb.Right);
}
else if (move.X < 0)
{
tMin = Math.Abs(obstaclesInPath.Max(o => o.Right) - aabb.Left);
}
else if (move.Y > 0)
{
tMin = Math.Abs(obstaclesInPath.Min(o => o.Bottom) - aabb.Top);
}
else if (move.Y < 0)
{
tMin = Math.Abs(obstaclesInPath.Max(o => o.Top) - aabb.Bottom);
}
}
}
else
{
var v1 = aabb.Min;
var v2 = aabb.Max;
var v3 = movedAabb.Min;
var v4 = movedAabb.Max;
if (move.X * move.Y > 0)
{
v1 = new Vector2f(aabb.Right, aabb.Bottom);
v2 = new Vector2f(aabb.Left, aabb.Top);
v3 = new Vector2f(movedAabb.Right, movedAabb.Bottom);
v4 = new Vector2f(movedAabb.Left, movedAabb.Top);
}
var t1 = new Triangle(v1, v3, v4);
var t2 = new Triangle(v1, v2, v4);
var obstaclesInPath = obstacles.Where(o => movedAabb.Overlap(o) || t1.Overlap(o) || t2.Overlap(o))
.ToArray();
if (obstaclesInPath.Length != 0)
{
tMin = double.NegativeInfinity;
if (move.X > 0)
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Left.GrEq(aabb.Right))
.Select(o => (o.Left - aabb.Right) / (movedAabb.Right - aabb.Right)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Left;
}
}
else
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Right.LeEq(aabb.Left))
.Select(o => (o.Right - aabb.Left) / (movedAabb.Right - aabb.Right)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Right;
}
}
if (move.Y > 0)
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Bottom.GrEq(aabb.Top))
.Select(o => (o.Bottom - aabb.Top) / (movedAabb.Top - aabb.Top)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Down;
}
}
else
{
var tBefore = tMin;
tMin = GetTMin(tMin,
obstaclesInPath.Where(o => o.Top.LeEq(aabb.Bottom))
.Select(o => (o.Top - aabb.Bottom) / (movedAabb.Top - aabb.Top)),
aabb, move, obstaclesInPath);
if (tMin > tBefore)
{
normal = Vector2f.Up;
}
}
if (tMin < 0)
{
// this should never happen
tMin = double.PositiveInfinity;
}
else
{
tMin *= move.Length();
}
}
}
return(tMin);
}
public static Vector2f ProectionTo(this Vector2f v1, Vector2f v2)
{
return v2.Normalize() * v1.ScalarMultiplicate(v2) / v2.Length();
}
private bool Solve(Game0 game, DynamicEntity entity, out Vector2f position, out Vector2f velocity)
{
position = entity.Position;
velocity = entity.Velocity;
// limit falling velocity
velocity.Y = Math.Max(velocity.Y, TerminalFallingVelocity);
// no velocity ? no collision!
if (velocity == Vector2f.Zero)
{
return(false);
}
var obstacles = game.Entities.Where(e => (e as Obstacle) != null).Select(o => o as Obstacle).ToArray();
var remainingVelocity = velocity;
var collisionFound = false;
while (true)
{
var distance = velocity.Length();
var direction = velocity.Normalize();
var entityBox = entity.BoundingBox + position;
var t = Collisions.Trace(entityBox, velocity, obstacles.Select(o => o.BoundingBox + o.Position).ToArray(), out Vector2f normal);
if (t.LeEq(distance))
{
collisionFound = true;
var hitPosition = position + direction * t;
// set new position & velocity
velocity = (position + velocity)
+ normal * Vector2f.Dot(direction.Inv(), normal) * (distance - t)
- hitPosition;
position = hitPosition;
if (normal.X == 0)
{
remainingVelocity.Y = 0;
}
else
{
remainingVelocity.X = 0;
}
if (velocity == Vector2f.Zero) // or better just close to zero ?
{
break; // solved - nowhere to move
}
}
else
{
break; // solved - nothing else stands in the way
}
}
position += velocity;
velocity = remainingVelocity;
return(collisionFound);
}
public static Vector2f Normalize(this Vector2f vector)
{
float length = vector.Length();
return(new Vector2f(vector.X / length, vector.Y / length));
}
public static Vector2f Normalize(this Vector2f vec)
{
var length = vec.Length();
return(new Vector2f(vec.X / length, vec.Y / length));
}
public static double Project(this Vector2f a, Vector2f b)
{
return(Dot(a, b) / a.Length());
}
