395 lines
10 KiB
C++
395 lines
10 KiB
C++
#include "pch.h"
|
|
#include "maths.h"
|
|
|
|
#include "TBall.h"
|
|
#include "TFlipperEdge.h"
|
|
|
|
|
|
// 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)
|
|
{
|
|
auto xPos = rect1.XPosition, width = rect1.Width;
|
|
if (rect2.XPosition < rect1.XPosition)
|
|
{
|
|
xPos = rect2.XPosition;
|
|
width += rect1.XPosition - rect2.XPosition;
|
|
}
|
|
|
|
auto yPos = rect1.YPosition, height = rect1.Height;
|
|
if (rect2.YPosition < rect1.YPosition)
|
|
{
|
|
yPos = rect2.YPosition;
|
|
height += rect1.YPosition - rect2.YPosition;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
auto xEnd2 = rect2.XPosition + rect2.Width;
|
|
if (rect2.XPosition >= rect1.XPosition + rect1.Width || rect1.XPosition >= xEnd2)
|
|
return 0;
|
|
|
|
auto yEnd2 = rect2.YPosition + rect2.Height;
|
|
if (rect2.YPosition >= rect1.YPosition + rect1.Height || rect1.YPosition >= yEnd2)
|
|
return 0;
|
|
|
|
auto xPos = rect1.XPosition, width = rect1.Width;
|
|
if (rect1.XPosition < rect2.XPosition)
|
|
{
|
|
xPos = rect2.XPosition;
|
|
width += rect1.XPosition - rect2.XPosition;
|
|
}
|
|
|
|
auto yPos = rect1.YPosition, height = rect1.Height;
|
|
if (rect1.YPosition < rect2.YPosition)
|
|
{
|
|
yPos = rect2.YPosition;
|
|
height += rect1.YPosition - rect2.YPosition;
|
|
}
|
|
|
|
if (xPos + width > xEnd2)
|
|
width = xEnd2 - xPos;
|
|
if (yPos + height > yEnd2)
|
|
height = yEnd2 - yPos;
|
|
|
|
if (width == 0 || height == 0)
|
|
return false;
|
|
|
|
if (dstRect)
|
|
{
|
|
dstRect->XPosition = xPos;
|
|
dstRect->YPosition = yPos;
|
|
dstRect->Width = width;
|
|
dstRect->Height = height;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// Returns the distance from ray origin to the first ray-circle intersection point.
|
|
float maths::ray_intersect_circle(const ray_type& ray, const circle_type& circle)
|
|
{
|
|
// O - ray origin
|
|
// D - ray direction
|
|
// C - circle center
|
|
// R - circle radius
|
|
// L, C - O, vector between O and C
|
|
auto L = vector_sub(circle.Center, ray.Origin);
|
|
|
|
// Tca, L dot D, projection of L on D
|
|
float Tca = DotProduct(L, ray.Direction);
|
|
if (Tca < 0.0f) // No intersection if Tca is negative
|
|
return 1000000000.0f;
|
|
|
|
// L dot L, distance from ray origin to circle center
|
|
float LMagSq = DotProduct(L, L);
|
|
|
|
// Thc^2 = rad^2 - d^2; d = sqrt(L dot L - Tca * Tca)
|
|
float ThcSq = circle.RadiusSq - LMagSq + Tca * Tca;
|
|
|
|
// 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)
|
|
return 1000000000.0f;
|
|
|
|
// T0 should be positive and less that max ray distance
|
|
float T0 = Tca - sqrt(ThcSq);
|
|
if (T0 < 0.0f || T0 > ray.MaxDistance)
|
|
return 1000000000.0f;
|
|
return T0;
|
|
}
|
|
|
|
float maths::normalize_2d(vector2& vec)
|
|
{
|
|
float mag = sqrt(vec.X * vec.X + vec.Y * vec.Y);
|
|
if (mag != 0.0f)
|
|
{
|
|
vec.X /= mag;
|
|
vec.Y /= mag;
|
|
}
|
|
return mag;
|
|
}
|
|
|
|
|
|
void maths::line_init(line_type* line, float x0, float y0, float x1, float y1)
|
|
{
|
|
line->Origin = { x0, y0 };
|
|
line->Direction.X = x1 - x0;
|
|
line->Direction.Y = y1 - y0;
|
|
normalize_2d(line->Direction);
|
|
|
|
// Clockwise perpendicular to the line direction vector
|
|
line->PerpendicularC = { line->Direction.Y, -line->Direction.X };
|
|
|
|
auto lineStart = x0, lineEnd = x1;
|
|
if (std::abs(line->Direction.X) < 0.000000001f)
|
|
{
|
|
line->Direction.X = 0.0;
|
|
lineStart = y0;
|
|
lineEnd = y1;
|
|
}
|
|
|
|
line->MinCoord = std::min(lineStart, lineEnd);
|
|
line->MaxCoord = std::max(lineStart, lineEnd);
|
|
}
|
|
|
|
// Returns the distance from ray origin to the ray-line segment intersection point.
|
|
float maths::ray_intersect_line(ray_type* ray, line_type* line)
|
|
{
|
|
// V1 vector between ray origin and line origin
|
|
// V2 ray direction
|
|
// V3 line perpendicular clockwise
|
|
auto v1 = vector_sub(ray->Origin, line->Origin);
|
|
auto v2 = line->Direction;
|
|
auto v3 = vector2{ -ray->Direction.Y, ray->Direction.X };
|
|
|
|
// Project line on ray perpendicular, no intersection if ray is pointing away from the line
|
|
auto v2DotV3 = DotProduct(v2, v3);
|
|
if (v2DotV3 < 0.0f)
|
|
{
|
|
// Distance to the intersect point: (V2 X V1) / (V2 dot V3)
|
|
auto distance = cross(v2, v1) / v2DotV3;
|
|
if (distance >= -ray->MinDistance && distance <= ray->MaxDistance)
|
|
{
|
|
line->RayIntersect.X = distance * ray->Direction.X + ray->Origin.X;
|
|
line->RayIntersect.Y = distance * ray->Direction.Y + ray->Origin.Y;
|
|
|
|
// Check if intersection point is inside line segment
|
|
auto testPoint = line->Direction.X != 0.0f ? line->RayIntersect.X : line->RayIntersect.Y;
|
|
if (testPoint >= line->MinCoord && testPoint <= line->MaxCoord)
|
|
{
|
|
return distance;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 1000000000.0;
|
|
}
|
|
|
|
void maths::cross(const vector3& vec1, const vector3& vec2, vector3& dstVec)
|
|
{
|
|
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;
|
|
}
|
|
|
|
float maths::cross(const vector2& vec1, const vector2& vec2)
|
|
{
|
|
return vec1.X * vec2.Y - vec1.Y * vec2.X;
|
|
}
|
|
|
|
float maths::magnitude(const vector3& vec)
|
|
{
|
|
float result;
|
|
auto magSq = vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z;
|
|
if (magSq == 0.0f)
|
|
result = 0.0;
|
|
else
|
|
result = sqrt(magSq);
|
|
return result;
|
|
}
|
|
|
|
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)
|
|
{
|
|
return { vec1.X - vec2.X, vec1.Y - vec2.Y };
|
|
}
|
|
|
|
float maths::basic_collision(TBall* ball, vector2* nextPosition, vector2* direction, float elasticity, float smoothness,
|
|
float threshold, float boost)
|
|
{
|
|
ball->Position.X = nextPosition->X;
|
|
ball->Position.Y = nextPosition->Y;
|
|
float proj = -(direction->Y * ball->Acceleration.Y + direction->X * ball->Acceleration.X);
|
|
if (proj < 0)
|
|
{
|
|
proj = -proj;
|
|
}
|
|
else
|
|
{
|
|
float dx1 = proj * direction->X;
|
|
float dy1 = proj * direction->Y;
|
|
ball->Acceleration.X = (dx1 + ball->Acceleration.X) * smoothness + dx1 * elasticity;
|
|
ball->Acceleration.Y = (dy1 + ball->Acceleration.Y) * smoothness + dy1 * elasticity;
|
|
normalize_2d(ball->Acceleration);
|
|
}
|
|
float projSpeed = proj * ball->Speed;
|
|
float newSpeed = ball->Speed - (1.0f - elasticity) * projSpeed;
|
|
ball->Speed = newSpeed;
|
|
if (projSpeed >= threshold)
|
|
{
|
|
ball->Acceleration.X = newSpeed * ball->Acceleration.X + direction->X * boost;
|
|
ball->Acceleration.Y = newSpeed * ball->Acceleration.Y + direction->Y * boost;
|
|
ball->Speed = normalize_2d(ball->Acceleration);
|
|
}
|
|
return projSpeed;
|
|
}
|
|
|
|
float maths::Distance_Squared(const vector2& vec1, const vector2& vec2)
|
|
{
|
|
auto dx = vec1.X - vec2.X;
|
|
auto dy = vec1.Y - vec2.Y;
|
|
return dy * dy + dx * dx;
|
|
}
|
|
|
|
float maths::DotProduct(const vector2& vec1, const vector2& vec2)
|
|
{
|
|
return vec1.X * vec2.X + vec1.Y * vec2.Y;
|
|
}
|
|
|
|
float maths::Distance(const vector2& vec1, const vector2& vec2)
|
|
{
|
|
return sqrt(Distance_Squared(vec1, vec2));
|
|
}
|
|
|
|
void maths::SinCos(float angle, float* sinOut, float* cosOut)
|
|
{
|
|
*sinOut = sin(angle);
|
|
*cosOut = cos(angle);
|
|
}
|
|
|
|
void maths::RotatePt(vector2& point, float sin, float cos, const vector2& origin)
|
|
{
|
|
auto dirX = point.X - origin.X;
|
|
auto dirY = point.Y - origin.Y;
|
|
point.X = dirX * cos - dirY * sin + origin.X;
|
|
point.Y = dirX * sin + dirY * cos + origin.Y;
|
|
}
|
|
|
|
float maths::distance_to_flipper(ray_type* ray1, ray_type* ray2)
|
|
{
|
|
auto distance = 1000000000.0f;
|
|
auto distanceType = -1;
|
|
auto newDistance = ray_intersect_line(ray1, &TFlipperEdge::lineA);
|
|
if (newDistance < 1000000000.0f)
|
|
{
|
|
distance = newDistance;
|
|
distanceType = 0;
|
|
}
|
|
newDistance = ray_intersect_circle(*ray1, TFlipperEdge::circlebase);
|
|
if (newDistance < distance)
|
|
{
|
|
distance = newDistance;
|
|
distanceType = 2;
|
|
}
|
|
newDistance = ray_intersect_circle(*ray1, TFlipperEdge::circleT1);
|
|
if (newDistance < distance)
|
|
{
|
|
distance = newDistance;
|
|
distanceType = 3;
|
|
}
|
|
newDistance = ray_intersect_line(ray1, &TFlipperEdge::lineB);
|
|
if (newDistance < distance)
|
|
{
|
|
distance = newDistance;
|
|
distanceType = 1;
|
|
}
|
|
if (!ray2 || distance >= 1000000000.0f)
|
|
return distance;
|
|
|
|
if (distanceType != -1)
|
|
{
|
|
vector2* nextOrigin;
|
|
if (distanceType)
|
|
{
|
|
if (distanceType != 1)
|
|
{
|
|
float dirY;
|
|
ray2->Origin.X = distance * ray1->Direction.X + ray1->Origin.X;
|
|
ray2->Origin.Y = distance * ray1->Direction.Y + ray1->Origin.Y;
|
|
if (distanceType == 2)
|
|
{
|
|
ray2->Direction.X = ray2->Origin.X - TFlipperEdge::circlebase.Center.X;
|
|
dirY = ray2->Origin.Y - TFlipperEdge::circlebase.Center.Y;
|
|
}
|
|
else
|
|
{
|
|
ray2->Direction.X = ray2->Origin.X - TFlipperEdge::circleT1.Center.X;
|
|
dirY = ray2->Origin.Y - TFlipperEdge::circleT1.Center.Y;
|
|
}
|
|
ray2->Direction.Y = dirY;
|
|
normalize_2d(ray2->Direction);
|
|
return distance;
|
|
}
|
|
ray2->Direction = TFlipperEdge::lineB.PerpendicularC;
|
|
nextOrigin = &TFlipperEdge::lineB.RayIntersect;
|
|
}
|
|
else
|
|
{
|
|
ray2->Direction = TFlipperEdge::lineA.PerpendicularC;
|
|
nextOrigin = &TFlipperEdge::lineA.RayIntersect;
|
|
}
|
|
ray2->Origin = *nextOrigin;
|
|
return distance;
|
|
}
|
|
return 1000000000.0;
|
|
}
|
|
|
|
void maths::RotateVector(vector2& vec, float angle)
|
|
{
|
|
float s = sin(angle), c = cos(angle);
|
|
vec.X = c * vec.X - s * vec.Y;
|
|
vec.Y = s * vec.X + c * vec.Y;
|
|
/* Error in the original, should be:
|
|
* auto newX = c * vec.X - s * vec.Y;
|
|
* vec.Y = s * vec.X + c * vec.Y;
|
|
* vec.X = newX;
|
|
*/
|
|
// Original code rotates the point on a figure eight curve.
|
|
// Luckily, it is never used with angle always set to 0.
|
|
}
|
|
|
|
void maths::find_closest_edge(ramp_plane_type* planes, int planeCount, wall_point_type* wall, vector2& lineEnd,
|
|
vector2& lineStart)
|
|
{
|
|
vector2 wallEnd{}, wallStart{};
|
|
|
|
wallStart.X = wall->X0;
|
|
wallStart.Y = wall->Y0;
|
|
wallEnd.Y = wall->Y1;
|
|
wallEnd.X = wall->X1;
|
|
|
|
float maxDistance = 1000000000.0f;
|
|
for (auto index = 0; index < planeCount; index++)
|
|
{
|
|
auto& plane = planes[index];
|
|
vector2* pointOrder[4] = { &plane.V1, &plane.V2, &plane.V3, &plane.V1 };
|
|
|
|
for (auto pt = 0; pt < 3; pt++)
|
|
{
|
|
auto& point1 = *pointOrder[pt], point2 = *pointOrder[pt + 1];
|
|
|
|
auto distance = Distance(wallStart, point1) + Distance(wallEnd, point2);
|
|
if (distance < maxDistance)
|
|
{
|
|
maxDistance = distance;
|
|
lineEnd = point1;
|
|
lineStart = point2;
|
|
}
|
|
}
|
|
}
|
|
}
|