20201108 16:37:59 +01:00



#include "pch.h"




#include "maths.h"





20210106 15:06:13 +01:00



#include "TBall.h"

20210110 13:22:06 +01:00



#include "TFlipperEdge.h"

20210106 15:06:13 +01:00




20201108 16:37:59 +01:00




20221228 06:47:44 +01:00



void RectF::Merge(RectF aabb)




{




XMax = std::max(XMax, aabb.XMax);




YMax = std::max(YMax, aabb.YMax);




XMin = std::min(XMin, aabb.XMin);




YMin = std::min(YMin, aabb.YMin);




}





20220513 10:15:30 +02:00



// Performs AABB merge, creating rect that is just large enough to contain both source rects.




void maths::enclosing_box(const rectangle_type& rect1, const rectangle_type& rect2, rectangle_type& dstRect)

20201108 16:37:59 +01:00



{

20220513 10:15:30 +02:00



auto xPos = rect1.XPosition, width = rect1.Width;




if (rect2.XPosition < rect1.XPosition)

20201115 15:39:00 +01:00



{

20220513 10:15:30 +02:00



xPos = rect2.XPosition;




width += rect1.XPosition  rect2.XPosition;

20201115 15:39:00 +01:00



}

20220513 10:15:30 +02:00







auto yPos = rect1.YPosition, height = rect1.Height;




if (rect2.YPosition < rect1.YPosition)

20201115 15:39:00 +01:00



{

20220513 10:15:30 +02:00



yPos = rect2.YPosition;




height += rect1.YPosition  rect2.YPosition;

20201115 15:39:00 +01:00



}





20220513 10:15:30 +02:00



auto xEnd2 = rect2.XPosition + rect2.Width;




if (xEnd2 > xPos + width)




width = xEnd2  xPos;








auto yEnd2 = rect2.YPosition + rect2.Height;




if (yEnd2 > yPos + height)




height = yEnd2  yPos;








dstRect.XPosition = xPos;




dstRect.YPosition = yPos;




dstRect.Width = width;




dstRect.Height = height;




}

20201115 15:39:00 +01:00




20220513 10:15:30 +02:00



// Creates rect that represents an intersection of rect1 and rect2.




// Return true when intersection exists.




bool maths::rectangle_clip(const rectangle_type& rect1, const rectangle_type& rect2, rectangle_type* dstRect)

20201115 15:39:00 +01:00



{

20220513 10:15:30 +02:00



auto xEnd2 = rect2.XPosition + rect2.Width;




if (rect2.XPosition >= rect1.XPosition + rect1.Width  rect1.XPosition >= xEnd2)

20201115 15:39:00 +01:00



return 0;

20220513 10:15:30 +02:00







auto yEnd2 = rect2.YPosition + rect2.Height;




if (rect2.YPosition >= rect1.YPosition + rect1.Height  rect1.YPosition >= yEnd2)

20201115 15:39:00 +01:00



return 0;

20220513 10:15:30 +02:00







auto xPos = rect1.XPosition, width = rect1.Width;




if (rect1.XPosition < rect2.XPosition)

20201115 15:39:00 +01:00



{

20220513 10:15:30 +02:00



xPos = rect2.XPosition;




width += rect1.XPosition  rect2.XPosition;

20201115 15:39:00 +01:00



}

20220513 10:15:30 +02:00







auto yPos = rect1.YPosition, height = rect1.Height;




if (rect1.YPosition < rect2.YPosition)

20201115 15:39:00 +01:00



{

20220513 10:15:30 +02:00



yPos = rect2.YPosition;




height += rect1.YPosition  rect2.YPosition;

20201115 15:39:00 +01:00



}





20220513 10:15:30 +02:00



if (xPos + width > xEnd2)




width = xEnd2  xPos;




if (yPos + height > yEnd2)




height = yEnd2  yPos;

20201115 15:39:00 +01:00




20220513 10:15:30 +02:00



if (width == 0  height == 0)




return false;

20201115 15:39:00 +01:00




20220513 10:15:30 +02:00



if (dstRect)

20201115 15:39:00 +01:00



{

20220513 10:15:30 +02:00



dstRect>XPosition = xPos;




dstRect>YPosition = yPos;




dstRect>Width = width;




dstRect>Height = height;

20201115 15:39:00 +01:00



}

20220513 10:15:30 +02:00



return true;

20201115 15:39:00 +01:00



}

20201121 16:14:40 +01:00




20220513 10:15:30 +02:00



// Returns the distance from ray origin to the first raycircle intersection point.




float maths::ray_intersect_circle(const ray_type& ray, const circle_type& circle)

20201121 16:14:40 +01:00



{




// O  ray origin




// D  ray direction




// C  circle center




// R  circle radius




// L, C  O, vector between O and C

20220513 10:15:30 +02:00



auto L = vector_sub(circle.Center, ray.Origin);

20201121 16:14:40 +01:00







// Tca, L dot D, projection of L on D

20220513 10:15:30 +02:00



float Tca = DotProduct(L, ray.Direction);

20210218 10:53:25 +01:00



if (Tca < 0.0f) // No intersection if Tca is negative

20201121 16:14:40 +01:00



return 1000000000.0f;








// L dot L, distance from ray origin to circle center

20220513 10:15:30 +02:00



float LMagSq = DotProduct(L, L);

20201121 16:14:40 +01:00




20220513 10:15:30 +02:00



// Thc^2 = rad^2  d^2; d = sqrt(L dot L  Tca * Tca)




float ThcSq = circle.RadiusSq  LMagSq + Tca * Tca;

20201121 16:14:40 +01:00




20220513 10:15:30 +02:00



// T0 = Tca  Thc, distance from origin to first intersection




// If ray origin is inside of the circle, then T0 is negative




if (LMagSq < circle.RadiusSq)




return Tca  sqrt(ThcSq);








// No intersection if ThcSq is negative, that is if d > rad




if (ThcSq < 0.0f)

20201121 16:14:40 +01:00



return 1000000000.0f;





20220513 10:15:30 +02:00



// T0 should be positive and less that max ray distance

20201121 16:14:40 +01:00



float T0 = Tca  sqrt(ThcSq);

20220513 10:15:30 +02:00



if (T0 < 0.0f  T0 > ray.MaxDistance)

20201121 16:14:40 +01:00



return 1000000000.0f;




return T0;




}





20220513 10:15:30 +02:00



float maths::normalize_2d(vector2& vec)

20201121 16:14:40 +01:00



{

20220513 10:15:30 +02:00



float mag = sqrt(vec.X * vec.X + vec.Y * vec.Y);

20210218 10:53:25 +01:00



if (mag != 0.0f)

20201121 16:14:40 +01:00



{

20220513 10:15:30 +02:00



vec.X /= mag;




vec.Y /= mag;

20201121 16:14:40 +01:00



}




return mag;




}









20220516 08:28:35 +02:00



void maths::line_init(line_type& line, float x0, float y0, float x1, float y1)

20201121 16:14:40 +01:00



{

20220516 08:28:35 +02:00



line.Origin = { x0, y0 };

20220519 13:17:31 +02:00



line.End = { x1, y1 };

20220516 08:28:35 +02:00



line.Direction.X = x1  x0;




line.Direction.Y = y1  y0;




normalize_2d(line.Direction);

20220513 10:15:30 +02:00







// Clockwise perpendicular to the line direction vector

20220516 08:28:35 +02:00



line.PerpendicularC = { line.Direction.Y, line.Direction.X };

20220513 10:15:30 +02:00







auto lineStart = x0, lineEnd = x1;

20220516 08:28:35 +02:00



if (std::abs(line.Direction.X) < 0.000000001f)

20201121 16:14:40 +01:00



{

20220516 08:28:35 +02:00



line.Direction.X = 0.0;

20220513 10:15:30 +02:00



lineStart = y0;




lineEnd = y1;

20201121 16:14:40 +01:00



}

20220513 10:15:30 +02:00




20220516 08:28:35 +02:00



line.MinCoord = std::min(lineStart, lineEnd);




line.MaxCoord = std::max(lineStart, lineEnd);

20201121 16:14:40 +01:00



}





20220513 10:15:30 +02:00



// Returns the distance from ray origin to the rayline segment intersection point.

20220516 08:28:35 +02:00



// Stores rayline intersection point in line.RayIntersect




float maths::ray_intersect_line(const ray_type& ray, line_type& line)

20201121 16:14:40 +01:00



{

20220513 10:15:30 +02:00



// V1 vector between ray origin and line origin




// V2 ray direction




// V3 line perpendicular clockwise

20220516 08:28:35 +02:00



auto v1 = vector_sub(ray.Origin, line.Origin);




auto v2 = line.Direction;




auto v3 = vector2{ ray.Direction.Y, ray.Direction.X };

20220513 10:15:30 +02:00







// Project line on ray perpendicular, no intersection if ray is pointing away from the line




auto v2DotV3 = DotProduct(v2, v3);




if (v2DotV3 < 0.0f)

20201121 16:14:40 +01:00



{

20220513 10:15:30 +02:00



// Distance to the intersect point: (V2 X V1) / (V2 dot V3)




auto distance = cross(v2, v1) / v2DotV3;

20220516 08:28:35 +02:00



if (distance >= ray.MinDistance && distance <= ray.MaxDistance)

20201121 16:14:40 +01:00



{

20220516 08:28:35 +02:00



line.RayIntersect.X = distance * ray.Direction.X + ray.Origin.X;




line.RayIntersect.Y = distance * ray.Direction.Y + ray.Origin.Y;

20220513 10:15:30 +02:00







// Check if intersection point is inside line segment

20220516 08:28:35 +02:00



auto testPoint = line.Direction.X != 0.0f ? line.RayIntersect.X : line.RayIntersect.Y;




if (testPoint >= line.MinCoord && testPoint <= line.MaxCoord)

20201121 16:14:40 +01:00



{

20220513 10:15:30 +02:00



return distance;

20201121 16:14:40 +01:00



}




}




}

20220513 10:15:30 +02:00




20201121 16:14:40 +01:00



return 1000000000.0;




}

20201128 12:39:12 +01:00




20220513 10:15:30 +02:00



void maths::cross(const vector3& vec1, const vector3& vec2, vector3& dstVec)

20201128 12:39:12 +01:00



{

20220513 10:15:30 +02:00



dstVec.X = vec2.Z * vec1.Y  vec2.Y * vec1.Z;




dstVec.Y = vec2.X * vec1.Z  vec1.X * vec2.Z;




dstVec.Z = vec1.X * vec2.Y  vec2.X * vec1.Y;

20201128 12:39:12 +01:00



}

20201129 16:50:49 +01:00




20220513 10:15:30 +02:00



float maths::cross(const vector2& vec1, const vector2& vec2)




{




return vec1.X * vec2.Y  vec1.Y * vec2.X;




}








float maths::magnitude(const vector3& vec)

20201129 16:50:49 +01:00



{




float result;

20220513 10:15:30 +02:00



auto magSq = vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z;

20210218 10:53:25 +01:00



if (magSq == 0.0f)

20201129 16:50:49 +01:00



result = 0.0;




else




result = sqrt(magSq);




return result;

20201227 16:19:36 +01:00



}





20230304 15:31:23 +01:00



float maths::magnitudeSq(const vector2& vec)




{




return vec.X * vec.X + vec.Y * vec.Y;




}





20230305 12:16:07 +01:00



int maths::magnitudeSq(const vector2i& vec)




{




return vec.X * vec.X + vec.Y * vec.Y;




}





20220513 10:15:30 +02:00



void maths::vector_add(vector2& vec1Dst, const vector2& vec2)




{




vec1Dst.X += vec2.X;




vec1Dst.Y += vec2.Y;




}








vector2 maths::vector_sub(const vector2& vec1, const vector2& vec2)

20201227 16:19:36 +01:00



{

20220513 10:15:30 +02:00



return { vec1.X  vec2.X, vec1.Y  vec2.Y };

20201227 16:19:36 +01:00



}

20210106 15:06:13 +01:00




20220601 15:19:27 +02:00



vector3 maths::vector_sub(const vector3& vec1, const vector3& vec2)




{




return { vec1.X  vec2.X, vec1.Y  vec2.Y, vec1.Z  vec2.Z };




}





20220519 13:17:31 +02:00



vector2 maths::vector_mul(const vector2& vec1, float val)




{




return { vec1.X * val, vec1.Y * val };




}





20220511 15:47:13 +02:00



float maths::basic_collision(TBall* ball, vector2* nextPosition, vector2* direction, float elasticity, float smoothness,

20210123 11:33:30 +01:00



float threshold, float boost)

20210106 15:06:13 +01:00



{

20230313 08:54:33 +01:00



ball>Position.X = nextPosition>X + direction>X * 0.0005f;




ball>Position.Y = nextPosition>Y + direction>Y * 0.0005f;

20220516 08:28:35 +02:00




20220520 10:51:00 +02:00



// Project ball direction on collision rebound direction




auto reboundProj = DotProduct(*direction, ball>Direction);




if (reboundProj < 0)

20210106 15:06:13 +01:00



{

20220520 10:51:00 +02:00



// Negative projection means no rebound, both direction vectors point the same way.




reboundProj = reboundProj;

20210106 15:06:13 +01:00



}




else




{

20220520 10:51:00 +02:00



// Apply rebound to ball direction




float dx1 = reboundProj * direction>X;




float dy1 = reboundProj * direction>Y;




ball>Direction.X = (dx1 + ball>Direction.X) * smoothness + dx1 * elasticity;




ball>Direction.Y = (dy1 + ball>Direction.Y) * smoothness + dy1 * elasticity;




normalize_2d(ball>Direction);

20210106 15:06:13 +01:00



}

20220516 08:28:35 +02:00




20220520 10:51:00 +02:00



// Apply rebound to ball speed




float reboundSpeed = reboundProj * ball>Speed;




ball>Speed = (1.0f  elasticity) * reboundSpeed;








if (reboundSpeed >= threshold)

20210106 15:06:13 +01:00



{

20220520 10:51:00 +02:00



// Change ball direction if rebound speed is above threshold




ball>Direction.X = ball>Speed * ball>Direction.X + direction>X * boost;




ball>Direction.Y = ball>Speed * ball>Direction.Y + direction>Y * boost;




ball>Speed = normalize_2d(ball>Direction);

20210106 15:06:13 +01:00



}

20220520 10:51:00 +02:00



return reboundSpeed;

20210106 15:06:13 +01:00



}

20210108 16:50:12 +01:00




20220513 10:15:30 +02:00



float maths::Distance_Squared(const vector2& vec1, const vector2& vec2)

20210108 16:50:12 +01:00



{

20220513 10:15:30 +02:00



auto dx = vec1.X  vec2.X;




auto dy = vec1.Y  vec2.Y;




return dy * dy + dx * dx;

20210108 16:50:12 +01:00



}

20210109 17:11:03 +01:00




20220513 10:15:30 +02:00



float maths::DotProduct(const vector2& vec1, const vector2& vec2)

20210109 17:11:03 +01:00



{

20220513 10:15:30 +02:00



return vec1.X * vec2.X + vec1.Y * vec2.Y;

20210109 17:11:03 +01:00



}





20220513 10:15:30 +02:00



float maths::Distance(const vector2& vec1, const vector2& vec2)

20210109 17:11:03 +01:00



{

20220513 10:15:30 +02:00



return sqrt(Distance_Squared(vec1, vec2));

20210109 17:11:03 +01:00



}





20220516 08:28:35 +02:00



void maths::SinCos(float angle, float& sinOut, float& cosOut)

20210109 17:11:03 +01:00



{

20220516 08:28:35 +02:00



sinOut = sin(angle);




cosOut = cos(angle);

20210109 17:11:03 +01:00



}





20220513 10:15:30 +02:00



void maths::RotatePt(vector2& point, float sin, float cos, const vector2& origin)

20210109 17:11:03 +01:00



{

20220516 08:28:35 +02:00



auto xOffset = point.X  origin.X;




auto yOffset = point.Y  origin.Y;




point.X = xOffset * cos  yOffset * sin + origin.X;




point.Y = xOffset * sin + yOffset * cos + origin.Y;

20210109 17:11:03 +01:00



}





20220516 08:28:35 +02:00



// Return the distance from ray1 origin to the intersection point with the closest flipper feature.




// Sets ray2 origin to intersection point, direction to collision direction

20220823 07:14:28 +02:00



float maths::distance_to_flipper(TFlipperEdge* flipper, const ray_type& ray1, ray_type& ray2)

20210109 17:11:03 +01:00



{

20210110 13:22:06 +01:00



auto distance = 1000000000.0f;

20220516 08:28:35 +02:00



auto distanceType = FlipperIntersect::none;

20230304 15:31:23 +01:00



auto newDistance = ray_intersect_line(ray1, flipper>LineA);

20220516 08:28:35 +02:00



if (newDistance < distance)

20210110 13:22:06 +01:00



{




distance = newDistance;

20220516 08:28:35 +02:00



distanceType = FlipperIntersect::lineA;

20210110 13:22:06 +01:00



}

20220823 07:14:28 +02:00



newDistance = ray_intersect_circle(ray1, flipper>circlebase);

20210110 13:22:06 +01:00



if (newDistance < distance)




{




distance = newDistance;

20220516 08:28:35 +02:00



distanceType = FlipperIntersect::circlebase;

20210110 13:22:06 +01:00



}

20220823 07:14:28 +02:00



newDistance = ray_intersect_circle(ray1, flipper>circleT1);

20210110 13:22:06 +01:00



if (newDistance < distance)




{




distance = newDistance;

20220516 08:28:35 +02:00



distanceType = FlipperIntersect::circleT1;

20210110 13:22:06 +01:00



}

20230304 15:31:23 +01:00



newDistance = ray_intersect_line(ray1, flipper>LineB);

20210110 13:22:06 +01:00



if (newDistance < distance)




{




distance = newDistance;

20220516 08:28:35 +02:00



distanceType = FlipperIntersect::lineB;

20210110 13:22:06 +01:00



}





20220516 08:28:35 +02:00



switch (distanceType)

20210110 13:22:06 +01:00



{

20220516 08:28:35 +02:00



case FlipperIntersect::lineA:

20230304 15:31:23 +01:00



ray2.Direction = flipper>LineA.PerpendicularC;




ray2.Origin = flipper>LineA.RayIntersect;

20220516 08:28:35 +02:00



break;




case FlipperIntersect::lineB:

20230304 15:31:23 +01:00



ray2.Direction = flipper>LineB.PerpendicularC;




ray2.Origin = flipper>LineB.RayIntersect;

20220516 08:28:35 +02:00



break;




case FlipperIntersect::circlebase:




case FlipperIntersect::circleT1:




ray2.Origin.X = distance * ray1.Direction.X + ray1.Origin.X;




ray2.Origin.Y = distance * ray1.Direction.Y + ray1.Origin.Y;




ray2.Direction = vector_sub(ray2.Origin, distanceType == FlipperIntersect::circlebase ?

20220823 07:14:28 +02:00



flipper>circlebase.Center : flipper>circleT1.Center);

20220516 08:28:35 +02:00



normalize_2d(ray2.Direction);




break;




case FlipperIntersect::none:




default:




break;

20210110 13:22:06 +01:00



}

20220516 08:28:35 +02:00







return distance;

20210109 17:11:03 +01:00



}

20210117 16:26:03 +01:00




20220513 10:15:30 +02:00



void maths::RotateVector(vector2& vec, float angle)

20210117 16:26:03 +01:00



{




float s = sin(angle), c = cos(angle);

20220513 10:15:30 +02:00



vec.X = c * vec.X  s * vec.Y;




vec.Y = s * vec.X + c * vec.Y;

20210117 16:26:03 +01:00



/* Error in the original, should be:

20220513 10:15:30 +02:00



* auto newX = c * vec.X  s * vec.Y;




* vec.Y = s * vec.X + c * vec.Y;




* vec.X = newX;

20210117 16:26:03 +01:00



*/

20220513 10:15:30 +02:00



// Original code rotates the point on a figure eight curve.




// Luckily, it is never used with angle always set to 0.

20210117 16:26:03 +01:00



}

20210118 16:30:19 +01:00




20220513 10:15:30 +02:00



void maths::find_closest_edge(ramp_plane_type* planes, int planeCount, wall_point_type* wall, vector2& lineEnd,




vector2& lineStart)

20210118 16:30:19 +01:00



{

20220516 08:28:35 +02:00



float distance = 1000000000.0f;

20210118 16:30:19 +01:00



for (auto index = 0; index < planeCount; index++)




{

20220513 10:15:30 +02:00



auto& plane = planes[index];




vector2* pointOrder[4] = { &plane.V1, &plane.V2, &plane.V3, &plane.V1 };

20210118 16:30:19 +01:00




20220513 10:15:30 +02:00



for (auto pt = 0; pt < 3; pt++)

20210118 16:30:19 +01:00



{

20220513 10:15:30 +02:00



auto& point1 = *pointOrder[pt], point2 = *pointOrder[pt + 1];

20210118 16:30:19 +01:00




20220516 08:28:35 +02:00



auto newDistance = Distance(wall>Pt0, point1) + Distance(wall>Pt1, point2);




if (newDistance < distance)

20220513 10:15:30 +02:00



{

20220516 08:28:35 +02:00



distance = newDistance;

20220513 10:15:30 +02:00



lineEnd = point1;




lineStart = point2;




}

20210118 16:30:19 +01:00



}




}




}
