/// <summary> Create a new event describing a contact
///
/// </summary>
/// <param name="time">The time of the collision
/// </param>
/// <param name="body1">The first body in the collision
/// </param>
/// <param name="body2">The second body in the collision
/// </param>
/// <param name="point">The point of collision (not always perfect - accepts penetration)
/// </param>
/// <param name="normal">The normal of collision
/// </param>
/// <param name="depth">The penetration of of the contact
/// </param>
public CollisionEvent(float time, Body body1, Body body2, ROVector2f point, ROVector2f normal, float depth)
{
this.time = time;
this.body1 = body1;
this.body2 = body2;
this.point = point;
this.normal = normal;
this.depth = depth;
}
/// <summary>Construct the polygon with a list of vertices
/// sorted in counterclockwise order.
/// Note that all the vector values will be copied.
///
/// Throws an exception when too few vertices (<3) are supplied.
/// TODO: throw an exception when the vertices arent counterclockwise?
///
/// </summary>
/// <param name="vertices">Vertices sorted in counterclockwise order
/// </param>
public Polygon(ROVector2f[] vertices)
{
if (vertices.Length < 3)
throw new System.ArgumentException("A polygon can not have fewer than 3 edges!");
this.vertices = new Vector2f[vertices.Length];
for (int i = 0; i < vertices.Length; i++)
{
this.vertices[i] = new Vector2f(vertices[i]);
}
float r = ComputeBoundingCircleRadius();
this.bounds = new AABox(r * 2, r * 2);
this.area = ComputeArea();
this.centroid = ComputeCentroid();
}
/// <summary> Get the closest point on the line to a given point
///
/// </summary>
/// <param name="point">The point which we want to project
/// </param>
/// <param name="result">The point on the line closest to the given point
/// </param>
public virtual void getClosestPoint(ROVector2f point, Vector2f result)
{
loc.Reconfigure(point);
loc.Sub(start);
v.Reconfigure(vec);
v2.Reconfigure(vec);
v2.Scale(- 1);
v.Normalise();
loc.ProjectOntoUnit(v, proj);
if (proj.LengthSquared() > vec.LengthSquared())
{
result.Reconfigure(end);
return ;
}
proj.Add(start);
other.Reconfigure(proj);
other.Sub(end);
if (other.LengthSquared() > vec.LengthSquared())
{
result.Reconfigure(start);
return ;
}
result.Reconfigure(proj);
return ;
}
/// <summary> Get the shortest Distance squared from a point to this line
///
/// </summary>
/// <param name="point">The point from which we want the Distance
/// </param>
/// <returns> The Distance squared from the line to the point
/// </returns>
public virtual float distanceSquared(ROVector2f point)
{
getClosestPoint(point, closest);
closest.Sub(point);
float result = closest.LengthSquared();
return result;
}
/// <summary> Get the shortest Distance from a point to this line
///
/// </summary>
/// <param name="point">The point from which we want the Distance
/// </param>
/// <returns> The Distance from the line to the point
/// </returns>
public virtual float distance(ROVector2f point)
{
return (float) System.Math.Sqrt(distanceSquared(point));
}
/// <summary> Create a new line based on two points
///
/// </summary>
/// <param name="start">The start point
/// </param>
/// <param name="end">The end point
/// </param>
public Line(ROVector2f start, ROVector2f end)
: base()
{
// float width = Math.Abs(end.getX()-start.getX());
// float height = Math.Abs(end.getY()-start.getY());
// float xoffset = width/2;
// float yoffset = height/2;
// if (width < 10) {
// width = 10;
// }
// if (height < 10) {
// height = 50;
// }
// if (end.getY() < start.getY()) {
// yoffset = -yoffset;
// }
// if (end.getX() < start.getX()) {
// xoffset = -xoffset;
// }
//TODO: do this properly!
float radius = System.Math.Max(start.Length(), end.Length());
bounds = new AABox(0, 0, radius * 2, radius * 2);
Reconfigure(start, end);
}
/// <summary> Reconfigure this joint
///
/// </summary>
/// <param name="b1">The first body attached to this joint
/// </param>
/// <param name="b2">The second body attached to this joint
/// </param>
/// <param name="anchor1">The location of the attachment to the first body, in absolute coordinates.
/// </param>
/// <param name="anchor2">The location of the attachment to the second body, in absolute coordinates.
/// </param>
public virtual void Reconfigure(Body b1, Body b2, ROVector2f anchor1, ROVector2f anchor2)
{
body1 = b1;
body2 = b2;
Matrix2f rot1 = new Matrix2f(body1.Rotation);
Matrix2f rot1T = rot1.Transpose();
Vector2f a1 = new Vector2f(anchor1);
a1.Sub(body1.GetPosition());
localAnchor1 = MathUtil.Mul(rot1T, a1);
Matrix2f rot2 = new Matrix2f(body2.Rotation);
Matrix2f rot2T = rot2.Transpose();
Vector2f a2 = new Vector2f(anchor2);
a2.Sub(body2.GetPosition());
localAnchor2 = MathUtil.Mul(rot2T, a2);
}
/// <summary> Adjust the position of this body.
/// The previous position will be set to the position before
/// this function was called.
///
/// </summary>
/// <param name="delta">The amount to change the position by
/// </param>
/// <param name="Scale">The amount to Scale the delta by
/// </param>
public virtual void AdjustPosition(ROVector2f delta, float scale)
{
lastPosition.Reconfigure(position);
position.x += delta.X * scale;
position.y += delta.Y * scale;
}
/// <summary> Move this line a certain amount
///
/// </summary>
/// <param name="v">The amount to Move the line
/// </param>
public virtual void move(ROVector2f v)
{
Vector2f temp = new Vector2f(start);
temp.Add(v);
start = temp;
temp = new Vector2f(end);
temp.Add(v);
end = temp;
}
/// <summary> Returns a translated and rotated copy of this poly's vertices.
/// The vertices are rotated before they are translated, i.e. they
/// are rotated around the origin (0,0).
/// The vertices are sorted counterclockwise.
///
/// This function is typically used to get the vertices for a
/// specific body, for example to Collide it with another body
/// or draw it.
///
/// </summary>
/// <param name="displacement">The displacement with wich all the
/// </param>
/// <param name="rotation">
/// </param>
/// <returns> this polygon's vertices translated and rotated
/// </returns>
public virtual Vector2f[] GetVertices(ROVector2f displacement, float rotation)
{
Vector2f[] retVertices = new Vector2f[vertices.Length];
float cos = (float) System.Math.Cos(rotation);
float sin = (float) System.Math.Sin(rotation);
for (int i = 0; i < vertices.Length; i++)
{
float x = vertices[i].x * cos - vertices[i].y * sin;
float y = vertices[i].y * cos + vertices[i].x * sin;
x += displacement.X;
y += displacement.Y;
retVertices[i] = new Vector2f(x, y);
}
return retVertices;
}
/// <summary> Returns a copy of the list of vertices. The vertices are sorted
/// counterclockwise.
///
/// </summary>
/// <returns> this polygons vertices
/// </returns>
public virtual ROVector2f[] GetVertices()
{
ROVector2f[] roVertices = new ROVector2f[vertices.Length];
for (int i = 0; i < vertices.Length; i++)
roVertices[i] = vertices[i];
return roVertices;
}
/// <summary> Get point on this polygon's hull that is closest to p.
///
/// TODO: make this thing return a negative value when it is contained in the polygon
///
/// </summary>
/// <param name="p">The point to search the closest point for
/// </param>
/// <returns> the nearest point on this vertex' hull
/// </returns>
public virtual ROVector2f GetNearestPoint(ROVector2f p)
{
// TODO: implement this
return null;
}
/// <summary> Returns a translated and rotated copy of this poly's centroid.
/// The centroid is rotated before it is translated, i.e. it
/// is rotated around the origin (0,0).
///
/// </summary>
/// <param name="displacement">The displacement with wich all the
/// </param>
/// <param name="rotation">
/// </param>
/// <returns> this polygon's vertices translated and rotated
/// </returns>
public virtual Vector2f GetCentroid(ROVector2f displacement, float rotation)
{
float cos = (float) System.Math.Cos(rotation);
float sin = (float) System.Math.Sin(rotation);
return new Vector2f(centroid.x * cos - centroid.y * sin + displacement.X, centroid.y * cos + centroid.x * sin + displacement.Y);
}
/// <summary> Test whether or not the point p is in this polygon in O(n),
/// where n is the number of vertices in this polygon.
///
/// </summary>
/// <param name="p">The point to be tested for inclusion in this polygon
/// </param>
/// <returns> true iff the p is in this polygon (not on a border)
/// </returns>
public virtual bool Contains(ROVector2f p)
{
// TODO: implement this
return false;
}
/// <summary> Get the current positon of a set of points
///
/// </summary>
/// <param name="pos">The centre of the box
/// </param>
/// <param name="rotation">The rotation of the box
/// </param>
/// <returns> The points building up a box at this position and rotation
/// </returns>
public virtual Vector2f[] GetPoints(ROVector2f pos, float rotation)
{
Matrix2f R = new Matrix2f(rotation);
Vector2f[] pts = new Vector2f[4];
ROVector2f h = MathUtil.Scale(Size, 0.5f);
pts[0] = MathUtil.Mul(R, new Vector2f(- h.X, - h.Y));
pts[0].Add(pos);
pts[1] = MathUtil.Mul(R, new Vector2f(h.X, - h.Y));
pts[1].Add(pos);
pts[2] = MathUtil.Mul(R, new Vector2f(h.X, h.Y));
pts[2].Add(pos);
pts[3] = MathUtil.Mul(R, new Vector2f(- h.X, h.Y));
pts[3].Add(pos);
return pts;
}
/// <summary> Get a line starting a x,y and ending offset from the current
/// end point. Curious huh?
///
/// </summary>
/// <param name="displacement">The displacement of the line
/// </param>
/// <param name="rotation">The rotation of the line in radians
/// </param>
/// <returns> The newly created line
/// </returns>
public virtual Line getPositionedLine(ROVector2f displacement, float rotation)
{
Vector2f[] verts = getVertices(displacement, rotation);
Line line = new Line(verts[0], verts[1]);
return line;
}
/// <summary> Return a translated and rotated line.
///
/// </summary>
/// <param name="displacement">The displacement of the line
/// </param>
/// <param name="rotation">The rotation of the line in radians
/// </param>
/// <returns> The two endpoints of this line
/// </returns>
public virtual Vector2f[] getVertices(ROVector2f displacement, float rotation)
{
float cos = (float) System.Math.Cos(rotation);
float sin = (float) System.Math.Sin(rotation);
Vector2f[] endPoints = new Vector2f[2];
endPoints[0] = new Vector2f(X1 * cos - Y1 * sin, Y1 * cos + X1 * sin);
endPoints[0].Add(displacement);
endPoints[1] = new Vector2f(X2 * cos - Y2 * sin, Y2 * cos + X2 * sin);
endPoints[1].Add(displacement);
return endPoints;
}
/// <summary> Get the edges from a list of vertices that can Collide with the given circle.
/// This uses a sweepline algorithm which is only efficient if some assumptions
/// are indeed true. See CPolygonCPolygonCollider for more information.
///
/// </summary>
/// <param name="vertsA">The vertices of a polygon that is Collided with a circle
/// </param>
/// <param name="centroid">The center of the polygon
/// </param>
/// <param name="radius">The radius of the circle
/// </param>
/// <param name="circlePos">The position (center) of the circle
/// </param>
/// <returns> The list of edges that can Collide with the circle
/// </returns>
protected internal virtual int[][] GetCollisionCandidates(Vector2f[] vertsA, ROVector2f centroid, float radius, ROVector2f circlePos)
{
Vector2f sweepDir = new Vector2f(centroid);
sweepDir.Sub(circlePos);
sweepDir.Normalise(); //TODO: this normalization might not be necessary
EdgeSweep sweep = new EdgeSweep(sweepDir); //vertsA[0], true, true, dist);
sweep.AddVerticesToSweep(true, vertsA);
float circProj = circlePos.Dot(sweepDir);
sweep.Insert(0, false, - radius + circProj);
sweep.Insert(0, false, radius + circProj);
return sweep.OverlappingEdges;
}
/// <summary> Configure the line
///
/// </summary>
/// <param name="start">The start point of the line
/// </param>
/// <param name="end">The end point of the line
/// </param>
public virtual void Reconfigure(ROVector2f start, ROVector2f end)
{
this.start = start;
this.end = end;
vec = new Vector2f(end);
vec.Sub(start);
lenSquared = vec.Length();
lenSquared *= lenSquared;
}
/// <summary> Get point on this polygon's hull that is closest to p.
///
/// TODO: make this thing return a negative value when it is contained in the polygon
///
/// </summary>
/// <param name="p">The point to search the closest point for
/// </param>
/// <returns> the nearest point on this vertex' hull
/// </returns>
public override ROVector2f GetNearestPoint(ROVector2f p)
{
// TODO: this can be done with a kind of binary search
float r = System.Single.MaxValue;
float l;
Vector2f v;
int m = - 1;
for (int i = 0; i < vertices.Length; i++)
{
v = new Vector2f(vertices[i]);
v.Sub(p);
l = v.x * v.x + v.y * v.y;
if (l < r)
{
r = l;
m = i;
}
}
// the closest point could be on one of the closest point's edges
// this happens when the angle between v[m-1]-v[m] and p-v[m] is
// smaller than 90 degrees, same for v[m+1]-v[m]
int length = vertices.Length;
Vector2f pm = new Vector2f(p);
pm.Sub(vertices[m]);
Vector2f l1 = new Vector2f(vertices[(m - 1 + length) % length]);
l1.Sub(vertices[m]);
Vector2f l2 = new Vector2f(vertices[(m + 1) % length]);
l2.Sub(vertices[m]);
Vector2f normal;
if (pm.Dot(l1) > 0)
{
normal = MathUtil.GetNormal(vertices[(m - 1 + length) % length], vertices[m]);
}
else if (pm.Dot(l2) > 0)
{
normal = MathUtil.GetNormal(vertices[m], vertices[(m + 1) % length]);
}
else
{
return vertices[m];
}
normal.Scale(- pm.Dot(normal));
normal.Add(p);
return normal;
}
/// <summary> Notify listeners of a collision
///
/// </summary>
/// <param name="body1">The first body in the collision
/// </param>
/// <param name="body2">The second body in the collision
/// </param>
/// <param name="point">The point of collision (not always perfect - accepts penetration)
/// </param>
/// <param name="normal">The normal of collision
/// </param>
/// <param name="depth">The penetration of of the contact
/// </param>
private void NotifyCollision(Body body1, Body body2, ROVector2f point, ROVector2f normal, float depth)
{
if (listeners.Count == 0)
{
return ;
}
CollisionEvent newEvent = new CollisionEvent(totalTime, body1, body2, point, normal, depth);
for (int i = 0; i < listeners.Count; i++)
{
((CollisionListener) listeners[i]).CollisionOccured(newEvent);
}
}
/// <summary> Create a joint holding two bodies together
///
/// </summary>
/// <param name="b1">The first body attached to the joint
/// </param>
/// <param name="b2">The second body attached to the joint
/// </param>
/// <param name="anchor1">The location of the attachment to the first body, in absolute coordinates.
/// </param>
/// <param name="anchor2">The location of the attachment to the second body, in absolute coordinates.
/// </param>
public SpringJoint(Body b1, Body b2, ROVector2f anchor1, ROVector2f anchor2)
{
id = NEXT_ID++;
stretchedSpringConst = 100;
compressedSpringConst = 100;
brokenSpringConst = 100;
Vector2f spring = new Vector2f(anchor1);
spring.Sub(anchor2);
springSize = spring.Length();
minSpringSize = 0;
maxSpringSize = 2 * springSize;
Reconfigure(b1, b2, anchor1, anchor2);
}
