From a6f5f4634c63a515196a0f650682953356cb8e18 Mon Sep 17 00:00:00 2001 From: Sergeanur Date: Sat, 14 Nov 2020 22:13:32 +0200 Subject: Make collision code placement more like original (+ small fixes) --- src/collision/VuCollision.cpp | 282 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 282 insertions(+) create mode 100644 src/collision/VuCollision.cpp (limited to 'src/collision/VuCollision.cpp') diff --git a/src/collision/VuCollision.cpp b/src/collision/VuCollision.cpp new file mode 100644 index 00000000..8828d2e1 --- /dev/null +++ b/src/collision/VuCollision.cpp @@ -0,0 +1,282 @@ +#include "common.h" +#ifdef VU_COLLISION +#include "VuVector.h" +#include "VuCollision.h" + +#ifndef GTA_PS2 +int16 vi01; +CVuVector vf01; +CVuVector vf02; +CVuVector vf03; + +CVuVector +DistanceBetweenSphereAndLine(const CVuVector ¢er, const CVuVector &p0, const CVuVector &line) +{ + // center VF12 + // p0 VF14 + // line VF15 + CVuVector ret; // VF16 + CVuVector p1 = p0+line; + CVuVector dist0 = center - p0; // VF20 + CVuVector dist1 = center - p1; // VF25 + float lenSq = line.MagnitudeSqr(); // VF21 + float distSq0 = dist0.MagnitudeSqr(); // VF22 + float distSq1 = dist1.MagnitudeSqr(); + float dot = DotProduct(dist0, line); // VF23 + if(dot < 0.0f){ + // not above line, closest to p0 + ret = p0; + ret.w = distSq0; + return ret; + } + float t = dot/lenSq; // param of nearest point on infinite line + if(t > 1.0f){ + // not above line, closest to p1 + ret = p1; + ret.w = distSq1; + return ret; + } + // closest to line + ret = p0 + line*t; + ret.w = (ret - center).MagnitudeSqr(); + return ret; +} +inline int SignFlags(const CVector &v) +{ + int f = 0; + if(v.x < 0.0f) f |= 1; + if(v.y < 0.0f) f |= 2; + if(v.z < 0.0f) f |= 4; + return f; +} +#endif + +extern "C" void +LineToTriangleCollision(const CVuVector &p0, const CVuVector &p1, + const CVuVector &v0, const CVuVector &v1, const CVuVector &v2, + const CVuVector &plane) +{ +#ifdef GTA_PS2 + __asm__ volatile ( + ".set noreorder\n" + "lqc2 vf12, 0x0(%0)\n" + "lqc2 vf13, 0x0(%1)\n" + "lqc2 vf14, 0x0(%2)\n" + "lqc2 vf15, 0x0(%3)\n" + "lqc2 vf16, 0x0(%4)\n" + "lqc2 vf17, 0x0(%5)\n" + "vcallms Vu0LineToTriangleCollisionStart\n" + ".set reorder\n" + : + : "r" (&p0), "r" (&p1), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane) + ); +#else + float dot0 = DotProduct(plane, p0); + float dot1 = DotProduct(plane, p1); + float dist0 = plane.w - dot0; + float dist1 = plane.w - dot1; + + // if points are on the same side, no collision + if(dist0 * dist1 > 0.0f){ + vi01 = 0; + return; + } + + CVuVector diff = p1 - p0; + float t = dist0/(dot1 - dot0); + CVuVector p = p0 + diff*t; + p.w = 0.0f; + vf01 = p; + vf03.x = t; + + // Check if point is inside + CVector cross1 = CrossProduct(p-v0, v1-v0); + CVector cross2 = CrossProduct(p-v1, v2-v1); + CVector cross3 = CrossProduct(p-v2, v0-v2); + // Only check relevant directions + int flagmask = 0; + if(Abs(plane.x) > 0.5f) flagmask |= 1; + if(Abs(plane.y) > 0.5f) flagmask |= 2; + if(Abs(plane.z) > 0.5f) flagmask |= 4; + int flags1 = SignFlags(cross1) & flagmask; + int flags2 = SignFlags(cross2) & flagmask; + int flags3 = SignFlags(cross3) & flagmask; + // inside if on the same side of all edges + if(flags1 != flags2 || flags1 != flags3){ + vi01 = 0; + return; + } + vi01 = 1; + vf02 = plane; + return; +#endif +} + +extern "C" void +LineToTriangleCollisionCompressed(const CVuVector &p0, const CVuVector &p1, VuTriangle &tri) +{ +#ifdef GTA_PS2 + __asm__ volatile ( + ".set noreorder\n" + "lqc2 vf12, 0x0(%0)\n" + "lqc2 vf13, 0x0(%1)\n" + "lqc2 vf14, 0x0(%2)\n" + "lqc2 vf15, 0x10(%2)\n" + "lqc2 vf16, 0x20(%2)\n" + "lqc2 vf17, 0x30(%2)\n" + "vcallms Vu0LineToTriangleCollisionCompressedStart\n" + ".set reorder\n" + : + : "r" (&p0), "r" (&p1), "r" (&tri) + ); +#else + CVuVector v0, v1, v2, plane; + v0.x = tri.v0[0]/128.0f; + v0.y = tri.v0[1]/128.0f; + v0.z = tri.v0[2]/128.0f; + v0.w = tri.v0[3]/128.0f; + v1.x = tri.v1[0]/128.0f; + v1.y = tri.v1[1]/128.0f; + v1.z = tri.v1[2]/128.0f; + v1.w = tri.v1[3]/128.0f; + v2.x = tri.v2[0]/128.0f; + v2.y = tri.v2[1]/128.0f; + v2.z = tri.v2[2]/128.0f; + v2.w = tri.v2[3]/128.0f; + plane.x = tri.plane[0]/4096.0f; + plane.y = tri.plane[1]/4096.0f; + plane.z = tri.plane[2]/4096.0f; + plane.w = tri.plane[3]/128.0f; + LineToTriangleCollision(p0, p1, v0, v1, v2, plane); +#endif +} + +extern "C" void +SphereToTriangleCollision(const CVuVector &sph, + const CVuVector &v0, const CVuVector &v1, const CVuVector &v2, + const CVuVector &plane) +{ +#ifdef GTA_PS2 + __asm__ volatile ( + ".set noreorder\n" + "lqc2 vf12, 0x0(%0)\n" + "lqc2 vf14, 0x0(%1)\n" + "lqc2 vf15, 0x0(%2)\n" + "lqc2 vf16, 0x0(%3)\n" + "lqc2 vf17, 0x0(%4)\n" + "vcallms Vu0SphereToTriangleCollisionStart\n" + ".set reorder\n" + : + : "r" (&sph), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane) + ); +#else + float planedist = DotProduct(plane, sph) - plane.w; // VF02 + if(Abs(planedist) > sph.w){ + vi01 = 0; + return; + } + // point on plane + CVuVector p = sph - planedist*plane; + p.w = 0.0f; + vf01 = p; + planedist = Abs(planedist); + // edges + CVuVector v01 = v1 - v0; + CVuVector v12 = v2 - v1; + CVuVector v20 = v0 - v2; + // VU code calculates normal again for some weird reason... + // Check sides of point + CVector cross1 = CrossProduct(p-v0, v01); + CVector cross2 = CrossProduct(p-v1, v12); + CVector cross3 = CrossProduct(p-v2, v20); + // Only check relevant directions + int flagmask = 0; + if(Abs(plane.x) > 0.1f) flagmask |= 1; + if(Abs(plane.y) > 0.1f) flagmask |= 2; + if(Abs(plane.z) > 0.1f) flagmask |= 4; + int nflags = SignFlags(plane) & flagmask; + int flags1 = SignFlags(cross1) & flagmask; + int flags2 = SignFlags(cross2) & flagmask; + int flags3 = SignFlags(cross3) & flagmask; + int testcase = 0; + CVuVector closest(0.0f, 0.0f, 0.0f); // VF04 + if(flags1 == nflags){ + closest += v2; + testcase++; + } + if(flags2 == nflags){ + closest += v0; + testcase++; + } + if(flags3 == nflags){ + closest += v1; + testcase++; + } + if(testcase == 3){ + // inside triangle - dist to plane already checked + vf02 = plane; + vf02.w = vf03.x = planedist; + vi01 = 1; + }else if(testcase == 1){ + // outside two sides - closest to point opposide inside edge + vf01 = closest; + vf02 = sph - closest; + float distSq = vf02.MagnitudeSqr(); + vi01 = sph.w*sph.w > distSq; + vf03.x = Sqrt(distSq); + vf02 *= 1.0f/vf03.x; + }else{ + // inside two sides - closest to third edge + if(flags1 != nflags) + closest = DistanceBetweenSphereAndLine(sph, v0, v01); + else if(flags2 != nflags) + closest = DistanceBetweenSphereAndLine(sph, v1, v12); + else + closest = DistanceBetweenSphereAndLine(sph, v2, v20); + vi01 = sph.w*sph.w > closest.w; + vf01 = closest; + vf02 = sph - closest; + vf03.x = Sqrt(closest.w); + vf02 *= 1.0f/vf03.x; + } +#endif +} + +extern "C" void +SphereToTriangleCollisionCompressed(const CVuVector &sph, VuTriangle &tri) +{ +#ifdef GTA_PS2 + __asm__ volatile ( + ".set noreorder\n" + "lqc2 vf12, 0x0(%0)\n" + "lqc2 vf14, 0x0(%1)\n" + "lqc2 vf15, 0x10(%1)\n" + "lqc2 vf16, 0x20(%1)\n" + "lqc2 vf17, 0x30(%1)\n" + "vcallms Vu0SphereToTriangleCollisionCompressedStart\n" + ".set reorder\n" + : + : "r" (&sph), "r" (&tri) + ); +#else + CVuVector v0, v1, v2, plane; + v0.x = tri.v0[0]/128.0f; + v0.y = tri.v0[1]/128.0f; + v0.z = tri.v0[2]/128.0f; + v0.w = tri.v0[3]/128.0f; + v1.x = tri.v1[0]/128.0f; + v1.y = tri.v1[1]/128.0f; + v1.z = tri.v1[2]/128.0f; + v1.w = tri.v1[3]/128.0f; + v2.x = tri.v2[0]/128.0f; + v2.y = tri.v2[1]/128.0f; + v2.z = tri.v2[2]/128.0f; + v2.w = tri.v2[3]/128.0f; + plane.x = tri.plane[0]/4096.0f; + plane.y = tri.plane[1]/4096.0f; + plane.z = tri.plane[2]/4096.0f; + plane.w = tri.plane[3]/128.0f; + SphereToTriangleCollision(sph, v0, v1, v2, plane); +#endif +} +#endif \ No newline at end of file -- cgit v1.2.3