/// <summary>
///
/// </summary>
/// <param name="point"></param>
/// <param name="AgentHeading"></param>
/// <param name="AgentSide"></param>
/// <param name="AgentPosition"></param>
/// <returns></returns>
public static Vector2D PointToLocalSpace(Vector2D point,
Vector2D AgentHeading,
Vector2D AgentSide,
Vector2D AgentPosition)
{
//make a copy of the point
Vector2D TransPoint = new Vector2D(point);
//create a transformation matrix
Matrix2D matTransform = new Matrix2D();
double Tx = -AgentPosition.GetDotProduct(AgentHeading);
double Ty = -AgentPosition.GetDotProduct(AgentSide);
//create the transformation matrix
matTransform._11 = AgentHeading.X; matTransform._12 = AgentSide.X;
matTransform._21 = AgentHeading.Y; matTransform._22 = AgentSide.Y;
matTransform._31 = Tx; matTransform._32 = Ty;
//matTransform._11( AgentHeading.X ); matTransform._12( AgentSide.X );
//matTransform._21( AgentHeading.Y ); matTransform._22( AgentSide.Y );
//matTransform._31( Tx ); matTransform._32( Ty );
////now transform the vertices
matTransform.TransformVector2Ds(TransPoint);
return(TransPoint);
}
/// <summary>
/// Calculate distance to the intersection point of a ray and a plane
/// </summary>
/// <param name="RayOrigin"></param>
/// <param name="RayHeading"></param>
/// <param name="PlanePoint"></param>
/// <param name="PlaneNormal"></param>
/// <returns></returns>
public static double DistanceToRayPlaneIntersection(Vector2D RayOrigin,
Vector2D RayHeading,
Vector2D PlanePoint, //any point on the plane
Vector2D PlaneNormal)
{
double d = -PlaneNormal.GetDotProduct(PlanePoint);
double numer = PlaneNormal.GetDotProduct(RayOrigin) + d;
double denom = PlaneNormal.GetDotProduct(RayHeading);
// normal is parallel to vector
if ((denom < Epsilon) && (denom > -Epsilon))
{
return(-1.0);
}
return(-(numer / denom));
}
//------------------ isSecondInFOVOfFirst -------------------------------------
//
// returns true if the target position is in the field of view of the entity
// positioned at posFirst facing in facingFirst
//-----------------------------------------------------------------------------
public static bool isSecondInFOVOfFirst(Vector2D posFirst,
Vector2D facingFirst,
Vector2D posSecond,
double fov)
{
Vector2D toTarget = Vec2DNormalize(posSecond - posFirst);
return(facingFirst.GetDotProduct(toTarget) >= Math.Cos(fov / 2.0));
}
/// <summary>
/// Check if a there is an intersection in a circle
/// </summary>
/// <param name="RayOrigin"></param>
/// <param name="RayHeading"></param>
/// <param name="CircleOrigin"></param>
/// <param name="radius"></param>
/// <returns></returns>
public static bool DoRayCircleIntersect(Vector2D RayOrigin,
Vector2D RayHeading,
Vector2D CircleOrigin,
double radius)
{
Vector2D ToCircle = CircleOrigin - RayOrigin;
double length = ToCircle.Length;
double v = ToCircle.GetDotProduct(RayHeading);
double d = radius * radius - (length * length - v * v);
// If there was no intersection, return -1
return(d < 0.0);
}
//------------------------- WithinFieldOfView ---------------------------
//
// returns true if subject is within field of view of this player
//-----------------------------------------------------------------------
bool IsPositionInFrontOfPlayer(Vector2D position)
{
Vector2D ToSubject = position - Position;
if (ToSubject.GetDotProduct(Heading) > 0)
{
return(true);
}
else
{
return(false);
}
}
public bool RotateHeadingToFacePosition(Vector2D target)
{
Vector2D delta = target - Position;
Vector2D toTarget = Vector2D.Vec2DNormalize(delta);
double dot = _heading.GetDotProduct(toTarget);
//some compilers lose acurracy so the value is clamped to ensure it
//remains valid for the acos
if (dot < -1)
{
dot = -1;
}
else if (dot > 1)
{
dot = 1;
}
//first determine the angle between the heading vector and the target
double angle = Math.Acos(dot);
//return true if the player is facing the target
if (angle < .00001)
{
return(true);
}
//clamp the amount to turn to the max turn rate
if (angle > _maxTurnRate)
{
angle = _maxTurnRate;
}
//The next few lines use a rotation matrix to rotate the player's heading
//vector accordingly
Matrix2D rotationMatrix = new Matrix2D();
//notice how the direction of rotation has to be determined when creating
//the rotation matrix
rotationMatrix.Rotate(angle * _heading.Sign(toTarget));
rotationMatrix.TransformVector2Ds(_heading);
rotationMatrix.TransformVector2Ds(_velocity);
//finally recreate m_vSide
_side = _heading.Perp;
return(false);
}
private const double pi = Math.PI; // 3.14159;
/// <summary>
/// Calculates the distince to the first interecting point in a circle
/// </summary>
/// <param name="RayOrigin"></param>
/// <param name="RayHeading"></param>
/// <param name="CircleOrigin"></param>
/// <param name="radius"></param>
/// <returns></returns>
public static double GetRayCircleIntersect(Vector2D RayOrigin,
Vector2D RayHeading,
Vector2D CircleOrigin,
double radius)
{
Vector2D ToCircle = CircleOrigin - RayOrigin;
double length = ToCircle.Length;
double v = ToCircle.GetDotProduct(RayHeading);
double d = radius * radius - (length * length - v * v);
// If there was no intersection, return -1
if (d < 0.0)
{
return(-1.0);
}
// Return the distance to the [first] intersecting point
return(v - Math.Sqrt(d));
}
/// <summary>
/// Determine where on a plane a point lies
/// </summary>
/// <param name="point"></param>
/// <param name="PointOnPlane"></param>
/// <param name="PlaneNormal"></param>
/// <returns></returns>
public static PlaneLocation WhereIsPoint(Vector2D point,
Vector2D PointOnPlane, //any point on the plane
Vector2D PlaneNormal)
{
Vector2D dir = PointOnPlane - point;
double d = dir.GetDotProduct(PlaneNormal);
if (d < -MinPrecision)
{
return(PlaneLocation.InFront);
}
else if (d > MinPrecision)
{
return(PlaneLocation.Behind);
}
return(PlaneLocation.On);
}
//------------------ isSecondInFOVOfFirst -------------------------------------
//
// returns true if the target position is in the field of view of the entity
// positioned at posFirst facing in facingFirst
//-----------------------------------------------------------------------------
public static bool isSecondInFOVOfFirst(Vector2D posFirst,
Vector2D facingFirst,
Vector2D posSecond,
double fov)
{
Vector2D toTarget = Vec2DNormalize(posSecond - posFirst);
return facingFirst.GetDotProduct(toTarget) >= Math.Cos(fov / 2.0);
}
/// <summary>
/// Tests to see if the ball has collided with a ball and reflects
/// the ball's velocity accordingly
/// </summary>
/// <param name="walls"></param>
public void TestCollisionWithWalls(List <Wall2D> walls)
{
//test ball against each wall, find out which is closest
int closestIndex = -1;
Vector2D normalVector = Vector2D.Vec2DNormalize(Velocity);
Vector2D intersectionPoint = new Vector2D(), collisionPoint = new Vector2D();
double distToIntersection = double.MaxValue;
////iterate through each wall and calculate if the ball intersects.
////If it does then store the index into the closest intersecting wall
for (int wallIndex = 0; wallIndex < walls.Count; wallIndex++)
{
//assuming a collision if the ball continued on its current heading
//calculate the point on the ball that would hit the wall. This is
//simply the wall's normal(inversed) multiplied by the ball's radius
//and added to the balls center (its position)
Vector2D thisCollisionPoint = Position - (walls[wallIndex].VectorNormal * BoundingRadius);
// //calculate exactly where the collision point will hit the plane
if (Geometry.WhereIsPoint(thisCollisionPoint,
walls[wallIndex].VectorFrom,
walls[wallIndex].VectorNormal) == Geometry.PlaneLocation.Behind)
{
double distToWall = Geometry.DistanceToRayPlaneIntersection(thisCollisionPoint, walls[wallIndex].VectorNormal, walls[wallIndex].VectorFrom, walls[wallIndex].VectorNormal);
intersectionPoint = thisCollisionPoint + (distToWall * walls[wallIndex].VectorNormal);
}
else
{
double distToWall = Geometry.DistanceToRayPlaneIntersection(thisCollisionPoint,
normalVector,
walls[wallIndex].VectorFrom,
walls[wallIndex].VectorNormal);
intersectionPoint = thisCollisionPoint + (distToWall * normalVector);
}
//check to make sure the intersection point is actually on the line
//segment
bool onLineSegment = false;
if (Geometry.LineIntersection2D(walls[wallIndex].VectorFrom,
walls[wallIndex].VectorTo,
thisCollisionPoint - walls[wallIndex].VectorNormal * 20.0,
thisCollisionPoint + walls[wallIndex].VectorNormal * 20.0))
{
onLineSegment = true;
}
//Note, there is no test for collision with the end of a line segment
//now check to see if the collision point is within range of the
//velocity vector. [work in distance squared to avoid sqrt] and if it
//is the closest hit found so far.
//If it is that means the ball will collide with the wall sometime
//between this time step and the next one.
double distSq = Vector2D.Vec2DDistanceSq(thisCollisionPoint, intersectionPoint);
if ((distSq <= Velocity.LengthSquared) && (distSq < distToIntersection) && onLineSegment)
{
distToIntersection = distSq;
closestIndex = wallIndex;
collisionPoint = intersectionPoint;
}
} // next wall
//to prevent having to calculate the exact time of collision we
//can just check if the velocity is opposite to the wall normal
//before reflecting it. This prevents the case where there is overshoot
//and the ball gets reflected back over the line before it has completely
//reentered the playing area.
if ((closestIndex >= 0) && normalVector.GetDotProduct(walls[closestIndex].VectorNormal) < 0)
{
Velocity.Reflect(walls[closestIndex].VectorNormal);
}
}
/// <summary>
///
/// </summary>
/// <param name="point"></param>
/// <param name="AgentHeading"></param>
/// <param name="AgentSide"></param>
/// <param name="AgentPosition"></param>
/// <returns></returns>
public static Vector2D PointToLocalSpace(Vector2D point,
Vector2D AgentHeading,
Vector2D AgentSide,
Vector2D AgentPosition)
{
//make a copy of the point
Vector2D TransPoint = new Vector2D(point);
//create a transformation matrix
Matrix2D matTransform = new Matrix2D();
double Tx = -AgentPosition.GetDotProduct(AgentHeading);
double Ty = -AgentPosition.GetDotProduct(AgentSide);
//create the transformation matrix
matTransform._11 = AgentHeading.X; matTransform._12 = AgentSide.X;
matTransform._21 = AgentHeading.Y; matTransform._22 = AgentSide.Y;
matTransform._31 = Tx; matTransform._32 = Ty;
//matTransform._11( AgentHeading.X ); matTransform._12( AgentSide.X );
//matTransform._21( AgentHeading.Y ); matTransform._22( AgentSide.Y );
//matTransform._31( Tx ); matTransform._32( Ty );
////now transform the vertices
matTransform.TransformVector2Ds(TransPoint);
return TransPoint;
}