SpaceCadetPinballEmscripten/SpaceCadetPinball/maths.cpp

464 lines
12 KiB
C++

#include "pch.h"
#include "maths.h"
#include "TBall.h"
#include "TFlipperEdge.h"
void maths::enclosing_box(rectangle_type* rect1, rectangle_type* rect2, rectangle_type* dstRect)
{
int xPos1 = rect1->XPosition;
int yPos1 = rect1->YPosition;
int width1 = rect1->Width;
int height1 = rect1->Height;
int xPos2 = rect2->XPosition;
bool rect2XPosLessRect1 = rect2->XPosition < rect1->XPosition;
int yPos2 = rect2->YPosition;
int width2 = rect2->Width;
int height2 = rect2->Height;
int xPos2_2 = rect2->XPosition;
if (rect2XPosLessRect1)
{
width1 += xPos1 - xPos2;
xPos1 = xPos2;
}
if (yPos2 < yPos1)
{
height1 += yPos1 - yPos2;
yPos1 = yPos2;
}
if (width2 + xPos2 > xPos1 + width1)
width1 = xPos2_2 + width2 - xPos1;
int height1_2 = height1;
if (height2 + yPos2 > height1 + yPos1)
height1_2 = yPos2 + height2 - yPos1;
dstRect->YPosition = yPos1;
dstRect->Height = height1_2;
dstRect->XPosition = xPos1;
dstRect->Width = width1;
}
int maths::rectangle_clip(rectangle_type* rect1, rectangle_type* rect2, rectangle_type* dstRect)
{
int xPos1 = rect1->XPosition;
int yPos1 = rect1->YPosition;
int height1 = rect1->Height;
int xRight2 = rect2->XPosition + rect2->Width;
int width1 = rect1->Width;
int yRight2 = rect2->YPosition + rect2->Height;
if (xPos1 + width1 < rect2->XPosition)
return 0;
if (xPos1 >= xRight2)
return 0;
int yPos2 = yPos1;
if (yPos1 + height1 < rect2->YPosition || yPos1 >= yRight2)
return 0;
if (xPos1 < rect2->XPosition)
{
width1 += xPos1 - rect2->XPosition;
xPos1 = rect2->XPosition;
}
if (xPos1 + width1 > xRight2)
width1 = xRight2 - xPos1;
int height2 = height1;
if (yPos1 < rect2->YPosition)
{
height2 = yPos1 - rect2->YPosition + height1;
yPos2 = rect2->YPosition;
}
if (height2 + yPos2 > yRight2)
height2 = yRight2 - yPos2;
if (!width1 || !height2)
return 0;
if (dstRect)
{
dstRect->XPosition = xPos1;
dstRect->YPosition = yPos2;
dstRect->Width = width1;
dstRect->Height = height2;
}
return 1;
}
int maths::overlapping_box(rectangle_type* rect1, rectangle_type* rect2, rectangle_type* dstRect)
{
int v3;
int v4;
int v6;
int v7;
if (rect1->XPosition >= rect2->XPosition)
{
dstRect->XPosition = rect2->XPosition;
v3 = rect1->Width - rect2->XPosition;
v4 = rect1->XPosition;
}
else
{
dstRect->XPosition = rect1->XPosition;
v3 = rect2->Width - rect1->XPosition;
v4 = rect2->XPosition;
}
dstRect->Width = v3 + v4 + 1;
int v5 = rect1->YPosition;
if (v5 >= rect2->YPosition)
{
dstRect->YPosition = rect2->YPosition;
v6 = rect1->Height - rect2->YPosition;
v7 = rect1->YPosition;
}
else
{
dstRect->YPosition = v5;
v6 = rect2->Height - rect1->YPosition;
v7 = rect2->YPosition;
}
dstRect->Height = v6 + v7 + 1;
return dstRect->Width <= rect2->Width + rect1->Width && dstRect->Height <= rect2->Height + rect1->Height;
}
float maths::ray_intersect_circle(ray_type* ray, circle_type* circle)
{
// O - ray origin
// D - ray direction
// C - circle center
// R - circle radius
// L, C - O, vector between O and C
float Lx = circle->Center.X - ray->Origin.X;
float Ly = circle->Center.Y - ray->Origin.Y;
// Tca, L dot D, projection of L on D
float Tca = Ly * ray->Direction.Y + Lx * ray->Direction.X;
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 = Ly * Ly + Lx * Lx;
// If ray origin is inside of the circle
// T0 = Tca - Sqrt(rad^2 - d^2). d = sqrt(L dot L - Tca dot Tca)
if (LMagSq < circle->RadiusSq)
return Tca - sqrt(circle->RadiusSq - LMagSq + Tca * Tca);
// Thc^2 = rad^2 - d = rad^2 - L dot L + Tca dot Tca
float ThcSq = circle->RadiusSq - LMagSq + Tca * Tca;
if (ThcSq < 0.0f) // No intersection if Thc is negative
return 1000000000.0f;
// T0 = Tca - Thc, distance from origin to first intersection
float T0 = Tca - sqrt(ThcSq);
if (T0 < 0.0f || T0 > ray->MaxDistance)
return 1000000000.0f;
return T0;
}
float maths::normalize_2d(vector_type* vec)
{
float mag = sqrt(vec->X * vec->X + vec->Y * vec->Y);
if (mag != 0.0f)
{
vec->X = 1.0f / mag * vec->X;
vec->Y = 1.0f / mag * vec->Y;
}
return mag;
}
void maths::line_init(line_type* line, float x0, float y0, float x1, float y1)
{
float v9;
bool lineDirection;
float v11;
line->Direction.X = x1 - x0;
line->Direction.Y = y1 - y0;
normalize_2d(&line->Direction);
line->PerpendicularL.X = line->Direction.Y;
line->PerpendicularL.Y = -line->Direction.X;
line->PreComp1 = -(line->Direction.Y * x0) + line->Direction.X * y0;
if (line->Direction.X >= 0.000000001f || line->Direction.X <= -0.000000001f)
{
v9 = x1;
lineDirection = x0 >= x1;
v11 = x0;
}
else
{
line->Direction.X = 0.0;
v9 = y1;
lineDirection = y0 >= y1;
v11 = y0;
}
if (lineDirection)
{
line->OriginX = v9;
line->OriginY = v11;
}
else
{
line->OriginY = v9;
line->OriginX = v11;
}
}
float maths::ray_intersect_line(ray_type* ray, line_type* line)
{
bool v5;
bool v6;
float perpDot = line->PerpendicularL.Y * ray->Direction.Y + ray->Direction.X * line->PerpendicularL.X;
if (perpDot < 0.0f)
{
float result = -((ray->Origin.X * line->PerpendicularL.X + ray->Origin.Y * line->PerpendicularL.Y + line->
PreComp1)
/ perpDot);
if (result >= -ray->MinDistance && result <= ray->MaxDistance)
{
line->RayIntersect.X = result * ray->Direction.X + ray->Origin.X;
float v4 = result * ray->Direction.Y + ray->Origin.Y;
line->RayIntersect.Y = v4;
if (line->Direction.X == 0.0f)
{
if (v4 >= line->OriginX)
{
v5 = v4 < line->OriginY;
v6 = v4 == line->OriginY;
if (v5 || v6)
return result;
return 1000000000.0;
}
}
else if (line->OriginX <= line->RayIntersect.X)
{
float v7 = line->RayIntersect.X;
v5 = v7 < line->OriginY;
v6 = v7 == line->OriginY;
if (v5 || v6)
return result;
return 1000000000.0;
}
}
}
return 1000000000.0;
}
void maths::cross(vector_type* vec1, vector_type* vec2, vector_type* 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::magnitude(vector_type* 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(vector_type* vec1Dst, vector_type* vec2)
{
vec1Dst->X += vec2->X;
vec1Dst->Y += vec2->Y;
}
float maths::basic_collision(TBall* ball, vector_type* nextPosition, vector_type* 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(vector_type vec1, vector_type vec2)
{
return (vec1.Y - vec2.Y) * (vec1.Y - vec2.Y) + (vec1.X - vec2.X) * (vec1.X - vec2.X);
}
float maths::DotProduct(vector_type* vec1, vector_type* vec2)
{
return vec1->Y * vec2->Y + vec1->X * vec2->X;
}
void maths::vswap(vector_type* vec1, vector_type* vec2)
{
vector_type tmp = *vec1;
*vec1 = *vec2;
*vec2 = tmp;
}
float maths::Distance(vector_type* vec1, vector_type* vec2)
{
auto dx = vec1->X - vec2->X;
auto dy = vec1->Y - vec2->Y;
return sqrt(dy * dy + dx * dx);
}
void maths::SinCos(float angle, float* sinOut, float* cosOut)
{
*sinOut = sin(angle);
*cosOut = cos(angle);
}
void maths::RotatePt(vector_type* point, float sin, float cos, vector_type* 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)
{
vector_type* 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.PerpendicularL;
nextOrigin = &TFlipperEdge::lineB.RayIntersect;
}
else
{
ray2->Direction = TFlipperEdge::lineA.PerpendicularL;
nextOrigin = &TFlipperEdge::lineA.RayIntersect;
}
ray2->Origin = *nextOrigin;
return distance;
}
return 1000000000.0;
}
void maths::RotateVector(vector_type* 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:
* tmp = c * vec->X - s * vec->Y;
* vec->Y = s * vec->X + c * vec->Y;
* vec->X = tmp
*/
}
void maths::find_closest_edge(ramp_plane_type* plane, int planeCount, wall_point_type* wall, vector_type** lineEnd,
vector_type** lineStart)
{
vector_type wallEnd{}, wallStart{};
wallStart.X = wall->X0;
wallStart.Y = wall->Y0;
wallEnd.Y = wall->Y1;
wallEnd.X = wall->X1;
float maxDistance = 1000000000.0f;
ramp_plane_type* planePtr = plane;
for (auto index = 0; index < planeCount; index++)
{
auto vec1 = reinterpret_cast<vector_type*>(&planePtr->V1),
vec2 = reinterpret_cast<vector_type*>(&planePtr->V2),
vec3 = reinterpret_cast<vector_type*>(&planePtr->V3);
auto distance = Distance(&wallStart, vec1) + Distance(&wallEnd, vec2);
if (distance < maxDistance)
{
maxDistance = distance;
*lineEnd = vec1;
*lineStart = vec2;
}
distance = Distance(&wallStart, vec2) + Distance(&wallEnd, vec3);
if (distance < maxDistance)
{
maxDistance = distance;
*lineEnd = vec2;
*lineStart = vec3;
}
distance = Distance(&wallStart, vec3) + Distance(&wallEnd, vec1);
if (distance < maxDistance)
{
maxDistance = distance;
*lineEnd = vec3;
*lineStart = vec1;
}
++planePtr;
}
}