encounter/Petari
0
0
Fork 0
mirror of https://github.com/encounter/Petari.git synced 2026-03-30 11:34:15 -07:00
Code Issues Packages Projects Releases Wiki Activity
Files
master
Petari/source/Game/Gravity/CubeGravity.cpp
T
Headpenguin 0eeb7a6d64 Fix minor formatting issues
2024-06-27 07:49:16 -04:00

408 lines
11 KiB
C++
Raw Permalink Blame History

#include "Game/Gravity.hpp"
#include "Game/Util/MathUtil.hpp"
#include "JSystem/JMath/JMath.hpp"
#define REGION_NEGATIVE_X 0
#define REGION_BETWEEN_X 1
#define REGION_POSITIVE_X 2
#define REGION_NEGATIVE_Y 0
#define REGION_BETWEEN_Y 3
#define REGION_POSITIVE_Y 6
#define REGION_NEGATIVE_Z 0
#define REGION_BETWEEN_Z 9
#define REGION_POSITIVE_Z 18
#define ENCODE_REGION(x, y, z) \
(x + y + z)
#define REGION_X_FACE_POSITIVE ENCODE_REGION(REGION_POSITIVE_X, REGION_BETWEEN_Y, REGION_BETWEEN_Z)
#define REGION_X_FACE_NEGATIVE ENCODE_REGION(REGION_NEGATIVE_X, REGION_BETWEEN_Y, REGION_BETWEEN_Z)
#define REGION_Y_FACE_POSITIVE ENCODE_REGION(REGION_BETWEEN_X, REGION_POSITIVE_Y, REGION_BETWEEN_Z)
#define REGION_Y_FACE_NEGATIVE ENCODE_REGION(REGION_BETWEEN_X, REGION_NEGATIVE_Y, REGION_BETWEEN_Z)
#define REGION_Z_FACE_POSITIVE ENCODE_REGION(REGION_BETWEEN_X, REGION_BETWEEN_Y, REGION_POSITIVE_Z)
#define REGION_Z_FACE_NEGATIVE ENCODE_REGION(REGION_BETWEEN_X, REGION_BETWEEN_Y, REGION_NEGATIVE_Z)
#define AXIS_X 1
#define AXIS_Y 3
#define AXIS_Z 9
#define ENCODE_EDGE(axis, half1, half2) \
(axis + half1 + half2)
#define ENCODE_CORNER(signumx, signumy, signumz) \
((signumx + 1) + (signumy + 1) * 3 + (signumz + 1) * 9)
template <>
bool TVec3f::isZero() const
{
register const TVec3f *rSrc = this;
register f32 sum;
__asm {
psq_l f1, 0(rSrc), 0, 0
lfs sum, 8(rSrc)
ps_mul f1, f1, f1
ps_madd sum, sum, sum, f1
ps_sum0 sum, sum, f1, f1
};
return sum <= 0.0000038146973f;
}
template <>
float TVec3f::normalize(const TVec3f &rSrc)
{
x = rSrc.x;
y = rSrc.y;
z = rSrc.z;
float magnitude = PSVECMag(toCVec());
PSVECNormalize(toCVec(), toVec());
return magnitude;
}
CubeGravity::CubeGravity() : PlanetGravity()
{
lenX = 1.0;
lenY = 1.0;
lenZ = 1.0;
mActiveFaces = 63;
mCube.identity();
mPosition.identity();
}
void CubeGravity::setCube(const TPos3f &rCube)
{
mCube = rCube;
updateIdentityMtx();
}
void CubeGravity::updateMtx(const TPos3f &rMtx)
{
mPosition.concat(rMtx, mCube);
TVec3f dir;
mPosition.getXDir(dir);
lenX = PSVECMag(dir.toCVec());
mPosition.getYDir(dir);
lenY = PSVECMag(dir.toCVec());
mPosition.getZDir(dir);
lenZ = PSVECMag(dir.toCVec());
}
bool CubeGravity::calcOwnGravityVector(TVec3f *pDest, f32 *pScalar, const TVec3f &rPosition) const
{
int area = calcGravityArea(rPosition);
if (area < 0) {
return false;
}
TVec3f gravityForce;
float scalar;
if (!calcFaceGravity(rPosition, area, &gravityForce, &scalar) && !calcCornerGravity(rPosition, area, &gravityForce, &scalar) && !calcEdgeGravity(rPosition, area, &gravityForce, &scalar)) {
return false;
}
if (isInRangeDistance(scalar)) {
return false;
}
if (pDest != NULL) {
*pDest = gravityForce;
}
if (pScalar != NULL) {
*pScalar = scalar;
}
return true;
}
int CubeGravity::calcGravityArea(const TVec3f &rPosition) const
{
TVec3f dirX, dirY, dirZ, trans;
mPosition.getXDir(dirX);
mPosition.getYDir(dirY);
mPosition.getZDir(dirZ);
mPosition.getTrans(trans);
TVec3f relativePosition = rPosition - trans;
int area; // Region of the cube
float xDirDistance = relativePosition.dot(dirX) / lenX, yDirDistance = relativePosition.dot(dirY) / lenY, zDirDistance = relativePosition.dot(dirZ) / lenZ;
if (xDirDistance < -lenX) {
if ((mActiveFaces & 2) != 2) {
return -1;
}
area = REGION_NEGATIVE_X;
}
else {
if (xDirDistance <= lenX) {
area = REGION_BETWEEN_X;
}
else {
if ((mActiveFaces & 1) != 1) {
return -1;
}
area = REGION_POSITIVE_X;
}
}
if (yDirDistance < -lenY) {
if ((mActiveFaces & 8) != 8) {
return -1;
}
area += REGION_NEGATIVE_Y;
}
else {
if (yDirDistance <= lenY) {
area += REGION_BETWEEN_Y;
}
else {
if ((mActiveFaces & 4) != 4) {
return -1;
}
area += REGION_POSITIVE_Y;
}
}
if (zDirDistance < -lenZ) {
if ((mActiveFaces & 32) != 32) {
return -1;
}
area += REGION_NEGATIVE_Z;
}
else {
if (zDirDistance <= lenZ) {
area += REGION_BETWEEN_Z;
}
else {
if ((mActiveFaces & 16) != 16) {
return -1;
}
area += REGION_POSITIVE_Z;
}
}
return area;
}
bool CubeGravity::calcFaceGravity(const TVec3f &rPosition, s32 area, TVec3f *pDest, f32 *pScalar) const
{
TVec3f antiFaceDir; // Negative of the normal vector of the face an object is on
switch (area) {
case REGION_Z_FACE_NEGATIVE:
mPosition.getZDir(antiFaceDir);
break;
case REGION_Y_FACE_NEGATIVE:
mPosition.getYDir(antiFaceDir);
break;
case REGION_X_FACE_NEGATIVE:
mPosition.getXDir(antiFaceDir);
break;
case REGION_X_FACE_POSITIVE:
mPosition.getXDir(antiFaceDir);
JGeometry::negateInternal(&antiFaceDir.x, &antiFaceDir.x);
break;
case REGION_Y_FACE_POSITIVE:
mPosition.getYDir(antiFaceDir);
JGeometry::negateInternal(&antiFaceDir.x, &antiFaceDir.x);
break;
case REGION_Z_FACE_POSITIVE:
mPosition.getZDir(antiFaceDir);
JGeometry::negateInternal(&antiFaceDir.x, &antiFaceDir.x);
break;
default:
return false;
}
TVec3f trans;
f32 length;
mPosition.getTrans(trans);
MR::separateScalarAndDirection(&length, &antiFaceDir, antiFaceDir);
float height = antiFaceDir.dot(trans - rPosition) - length;
if (height < 0.0f) {
height = 0.0f;
}
*pDest = antiFaceDir;
*pScalar = height;
return true;
}
TVec3f negate(const TVec3f &in)
{
TVec3f tmp;
JGeometry::negateInternal(&in.x, &tmp.x);
return tmp;
}
bool CubeGravity::calcEdgeGravity(const TVec3f &rPosition, s32 area, TVec3f *pDest, f32 *pScalar) const
{
// There is a mistake here: so long as area is not both even and negative, the function will not
// return here. The intent is that area should be neither even nor negative, since all edges
// are odd and positive. However, this mistake does not really matter since the switch will
// return if this does not.
if (!(((area & 1) ^ ((area & 0x80000000) >> 31)) - ((area & 0x80000000) >> 31))
|| area == ENCODE_REGION(REGION_BETWEEN_X, REGION_BETWEEN_Y, REGION_BETWEEN_Z)) {
return false;
}
TVec3f edgeVector, edgeTranslation, xDir, yDir, zDir, trans, positionOppositeInOrthogonalPlane;
mPosition.getXDir(xDir);
mPosition.getYDir(yDir);
mPosition.getZDir(zDir);
switch (area) {
case ENCODE_EDGE(AXIS_X, REGION_NEGATIVE_Y, REGION_NEGATIVE_Z):
edgeVector = xDir;
edgeTranslation = negate(yDir) - zDir;
break;
case ENCODE_EDGE(AXIS_Y, REGION_NEGATIVE_X, REGION_NEGATIVE_Z):
edgeVector = yDir;
edgeTranslation = negate(xDir) - zDir;
break;
case ENCODE_EDGE(AXIS_Y, REGION_POSITIVE_X, REGION_NEGATIVE_Z):
edgeVector = yDir;
edgeTranslation = xDir - zDir;
break;
case ENCODE_EDGE(AXIS_X, REGION_POSITIVE_Y, REGION_NEGATIVE_Z):
edgeVector = xDir;
edgeTranslation = yDir - zDir;
break;
case ENCODE_EDGE(AXIS_Z, REGION_NEGATIVE_X, REGION_NEGATIVE_Y):
edgeVector = zDir;
edgeTranslation = negate(xDir) - yDir;
break;
case ENCODE_EDGE(AXIS_Z, REGION_POSITIVE_X, REGION_NEGATIVE_Y):
edgeVector = zDir;
edgeTranslation = xDir - yDir;
break;
case ENCODE_EDGE(AXIS_Z, REGION_NEGATIVE_X, REGION_POSITIVE_Y):
edgeVector = zDir;
edgeTranslation = negate(xDir).translate(yDir);
break;
case ENCODE_EDGE(AXIS_Z, REGION_POSITIVE_X, REGION_POSITIVE_Y):
edgeVector = zDir;
edgeTranslation = xDir.translate(yDir);
break;
case ENCODE_EDGE(AXIS_X, REGION_NEGATIVE_Y, REGION_POSITIVE_Z):
edgeVector = xDir;
edgeTranslation = negate(yDir).translate(zDir);
break;
case ENCODE_EDGE(AXIS_Y, REGION_NEGATIVE_X, REGION_POSITIVE_Z):
edgeVector = yDir;
edgeTranslation = negate(xDir).translate(zDir);
break;
case ENCODE_EDGE(AXIS_Y, REGION_POSITIVE_X, REGION_POSITIVE_Z):
edgeVector = yDir;
edgeTranslation = xDir.translate(zDir);
break;
case ENCODE_EDGE(AXIS_X, REGION_POSITIVE_Y, REGION_POSITIVE_Z):
edgeVector = xDir;
edgeTranslation = yDir.translate(zDir);
break;
default:
return false;
}
mPosition.getTrans(trans);
edgeTranslation += trans;
MR::normalizeOrZero(&edgeVector);
positionOppositeInOrthogonalPlane.rejection(edgeTranslation - rPosition, edgeVector);
if (positionOppositeInOrthogonalPlane.isZero()) {
pDest->normalize(edgeTranslation - trans);
*pScalar = 0.0;
}
else {
*pScalar = pDest->normalize(positionOppositeInOrthogonalPlane);
}
return true;
}
bool CubeGravity::calcCornerGravity(const TVec3f &rPosition, s32 area, TVec3f *pDest, f32 *pScalar) const
{
TVec3f vertex, xDir, yDir, zDir, trans;
mPosition.getXDir(xDir);
mPosition.getYDir(yDir);
mPosition.getZDir(zDir);
switch (area) {
case ENCODE_CORNER(-1, -1, -1):
vertex = negate(xDir) - yDir - zDir;
break;
case ENCODE_CORNER(1, -1, -1):
vertex = xDir - yDir - zDir;
break;
case ENCODE_CORNER(-1, 1, -1):
vertex = negate(xDir).translate(yDir) - zDir;
break;
case ENCODE_CORNER(1, 1, -1):
vertex = xDir.translate(yDir) - zDir;
break;
case ENCODE_CORNER(-1, -1, 1):
vertex = (negate(xDir) - yDir).translate(zDir);
break;
case ENCODE_CORNER(1, -1, 1):
vertex = (xDir - yDir).translate(zDir);
break;
case ENCODE_CORNER(-1, 1, 1):
vertex = negate(xDir).translate(yDir).translate(zDir);
break;
case ENCODE_CORNER(1, 1, 1):
vertex = xDir.translate(yDir).translate(zDir);
break;
default:
return false;
}
mPosition.getTrans(trans);
vertex += trans;
TVec3f gravity = vertex - rPosition;
if (gravity.isZero()) {
*pScalar = 0.0;
pDest->normalize(vertex - trans);
}
else {
*pScalar = pDest->normalize(gravity);
}
return true;
}
Reference in New Issue View Git Blame Copy Permalink