|
@@ -0,0 +1,378 @@
|
|
|
+module graphics.collision;
|
|
|
+
|
|
|
+import raylib;
|
|
|
+import raylib.raymath;
|
|
|
+import std.math;
|
|
|
+import variables;
|
|
|
+
|
|
|
+struct OBB
|
|
|
+{
|
|
|
+ Quaternion rotation;
|
|
|
+ Vector3 center;
|
|
|
+ Vector3 halfExtents;
|
|
|
+}
|
|
|
+
|
|
|
+void getAxes(ref const OBB obb, out Vector3 right, out Vector3 up, out Vector3 forward)
|
|
|
+{
|
|
|
+ Matrix rot = QuaternionToMatrix(obb.rotation);
|
|
|
+
|
|
|
+ right = Vector3(rot.m0, rot.m1, rot.m2);
|
|
|
+ up = Vector3(rot.m4, rot.m5, rot.m6);
|
|
|
+ forward = Vector3(rot.m8, rot.m9, rot.m10);
|
|
|
+}
|
|
|
+
|
|
|
+void getCorners(ref const OBB obb, out Vector3[8] corners)
|
|
|
+{
|
|
|
+ Vector3 right, up, forward;
|
|
|
+ getAxes(obb, right, up, forward);
|
|
|
+
|
|
|
+ right = Vector3Scale(right, obb.halfExtents.x);
|
|
|
+ up = Vector3Scale(up, obb.halfExtents.y);
|
|
|
+ forward = Vector3Scale(forward, obb.halfExtents.z);
|
|
|
+
|
|
|
+ corners[0] = Vector3Add(Vector3Add(Vector3Add(obb.center, right), up), forward);
|
|
|
+ corners[1] = Vector3Add(Vector3Add(Vector3Subtract(obb.center, right), up), forward);
|
|
|
+ corners[2] = Vector3Add(Vector3Add(Vector3Subtract(obb.center, right), up), Vector3Negate(
|
|
|
+ forward));
|
|
|
+ corners[3] = Vector3Add(Vector3Add(Vector3Add(obb.center, right), up), Vector3Negate(forward));
|
|
|
+
|
|
|
+ corners[4] = Vector3Add(Vector3Add(Vector3Add(obb.center, right), Vector3Negate(up)), forward);
|
|
|
+ corners[5] = Vector3Add(Vector3Add(Vector3Subtract(obb.center, right), Vector3Negate(up)), forward);
|
|
|
+ corners[6] = Vector3Add(Vector3Add(Vector3Subtract(obb.center, right), Vector3Negate(up)), Vector3Negate(
|
|
|
+ forward));
|
|
|
+ corners[7] = Vector3Add(Vector3Add(Vector3Add(obb.center, right), Vector3Negate(up)), Vector3Negate(
|
|
|
+ forward));
|
|
|
+}
|
|
|
+
|
|
|
+void drawWireframe(ref const OBB obb, Color color)
|
|
|
+{
|
|
|
+ Vector3[8] c;
|
|
|
+ getCorners(obb, c);
|
|
|
+
|
|
|
+ DrawLine3D(c[0], c[1], color);
|
|
|
+ DrawLine3D(c[1], c[2], color);
|
|
|
+ DrawLine3D(c[2], c[3], color);
|
|
|
+ DrawLine3D(c[3], c[0], color);
|
|
|
+
|
|
|
+ DrawLine3D(c[4], c[5], color);
|
|
|
+ DrawLine3D(c[5], c[6], color);
|
|
|
+ DrawLine3D(c[6], c[7], color);
|
|
|
+ DrawLine3D(c[7], c[4], color);
|
|
|
+
|
|
|
+ DrawLine3D(c[0], c[4], color);
|
|
|
+ DrawLine3D(c[1], c[5], color);
|
|
|
+ DrawLine3D(c[2], c[6], color);
|
|
|
+ DrawLine3D(c[3], c[7], color);
|
|
|
+}
|
|
|
+
|
|
|
+bool containsPoint(ref const OBB obb, Vector3 point)
|
|
|
+{
|
|
|
+ Vector3 local = Vector3Subtract(point, obb.center);
|
|
|
+
|
|
|
+ Quaternion inverseRot = QuaternionInvert(obb.rotation);
|
|
|
+ local = Vector3RotateByQuaternion(local, inverseRot);
|
|
|
+
|
|
|
+ return fabs(local.x) <= obb.halfExtents.x &&
|
|
|
+ fabs(local.y) <= obb.halfExtents.y &&
|
|
|
+ fabs(local.z) <= obb.halfExtents.z;
|
|
|
+}
|
|
|
+
|
|
|
+void projectBoundingBoxOntoAxis(ref const BoundingBox box, Vector3 axis, out float outMin, out float outMax)
|
|
|
+{
|
|
|
+ Vector3[8] corners = [
|
|
|
+ Vector3(box.min.x, box.min.y, box.min.z),
|
|
|
+ Vector3(box.max.x, box.min.y, box.min.z),
|
|
|
+ Vector3(box.max.x, box.max.y, box.min.z),
|
|
|
+ Vector3(box.min.x, box.max.y, box.min.z),
|
|
|
+ Vector3(box.min.x, box.min.y, box.max.z),
|
|
|
+ Vector3(box.max.x, box.min.y, box.max.z),
|
|
|
+ Vector3(box.max.x, box.max.y, box.max.z),
|
|
|
+ Vector3(box.min.x, box.max.y, box.max.z)
|
|
|
+ ];
|
|
|
+
|
|
|
+ float min = Vector3DotProduct(corners[0], axis);
|
|
|
+ float max = min;
|
|
|
+
|
|
|
+ for (int i = 1; i < 8; ++i)
|
|
|
+ {
|
|
|
+ float projection = Vector3DotProduct(corners[i], axis);
|
|
|
+ if (projection < min)
|
|
|
+ min = projection;
|
|
|
+ if (projection > max)
|
|
|
+ max = projection;
|
|
|
+ }
|
|
|
+
|
|
|
+ outMin = min;
|
|
|
+ outMax = max;
|
|
|
+}
|
|
|
+
|
|
|
+void projectOBBOntoAxis(ref const OBB obb, Vector3 axis, out float outMin, out float outMax)
|
|
|
+{
|
|
|
+ Vector3 right, up, forward;
|
|
|
+ getAxes(obb, right, up, forward);
|
|
|
+
|
|
|
+ float r =
|
|
|
+ fabs(Vector3DotProduct(right, axis)) * obb.halfExtents.x +
|
|
|
+ fabs(Vector3DotProduct(up, axis)) * obb.halfExtents.y +
|
|
|
+ fabs(
|
|
|
+ Vector3DotProduct(forward, axis)) * obb.halfExtents.z;
|
|
|
+
|
|
|
+ float centerProj = Vector3DotProduct(obb.center, axis);
|
|
|
+ outMin = centerProj - r;
|
|
|
+ outMax = centerProj + r;
|
|
|
+}
|
|
|
+
|
|
|
+bool checkCollisionBoundingBoxVsOBB(ref const BoundingBox box, ref const OBB obb)
|
|
|
+{
|
|
|
+ Vector3[3] aabbAxes = [
|
|
|
+ Vector3(1, 0, 0),
|
|
|
+ Vector3(0, 1, 0),
|
|
|
+ Vector3(0, 0, 1)
|
|
|
+ ];
|
|
|
+
|
|
|
+ Vector3[3] obbAxes;
|
|
|
+ getAxes(obb, obbAxes[0], obbAxes[1], obbAxes[2]);
|
|
|
+
|
|
|
+ Vector3[15] testAxes;
|
|
|
+ int axisCount = 0;
|
|
|
+
|
|
|
+ for (int i = 0; i < 3; i++)
|
|
|
+ testAxes[axisCount++] = aabbAxes[i];
|
|
|
+
|
|
|
+ for (int i = 0; i < 3; i++)
|
|
|
+ testAxes[axisCount++] = obbAxes[i];
|
|
|
+
|
|
|
+ for (int i = 0; i < 3; i++)
|
|
|
+ {
|
|
|
+ for (int j = 0; j < 3; j++)
|
|
|
+ {
|
|
|
+ Vector3 cross = Vector3CrossProduct(aabbAxes[i], obbAxes[j]);
|
|
|
+ if (Vector3LengthSqr(cross) > 0.000001f)
|
|
|
+ {
|
|
|
+ testAxes[axisCount++] = Vector3Normalize(cross);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = 0; i < axisCount; ++i)
|
|
|
+ {
|
|
|
+ Vector3 axis = testAxes[i];
|
|
|
+ float minA, maxA, minB, maxB;
|
|
|
+
|
|
|
+ projectBoundingBoxOntoAxis(box, axis, minA, maxA);
|
|
|
+ projectOBBOntoAxis(obb, axis, minB, maxB);
|
|
|
+
|
|
|
+ if (maxA < minB || maxB < minA)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+bool checkCollisionOBBvsOBB(ref const OBB a, ref const OBB b)
|
|
|
+{
|
|
|
+ Vector3[3] axesA, axesB;
|
|
|
+ getAxes(a, axesA[0], axesA[1], axesA[2]);
|
|
|
+ getAxes(b, axesB[0], axesB[1], axesB[2]);
|
|
|
+
|
|
|
+ Vector3[15] testAxes;
|
|
|
+ int axisCount = 0;
|
|
|
+
|
|
|
+ for (int i = 0; i < 3; ++i)
|
|
|
+ testAxes[axisCount++] = axesA[i];
|
|
|
+
|
|
|
+ for (int i = 0; i < 3; ++i)
|
|
|
+ testAxes[axisCount++] = axesB[i];
|
|
|
+
|
|
|
+ for (int i = 0; i < 3; ++i)
|
|
|
+ {
|
|
|
+ for (int j = 0; j < 3; ++j)
|
|
|
+ {
|
|
|
+ Vector3 cross = Vector3CrossProduct(axesA[i], axesB[j]);
|
|
|
+ float len = Vector3Length(cross);
|
|
|
+ if (len > 0.0001f)
|
|
|
+ {
|
|
|
+ testAxes[axisCount++] = Vector3Scale(cross, 1.0f / len);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = 0; i < axisCount; ++i)
|
|
|
+ {
|
|
|
+ Vector3 axis = testAxes[i];
|
|
|
+
|
|
|
+ float minA, maxA, minB, maxB;
|
|
|
+ projectOBBOntoAxis(a, axis, minA, maxA);
|
|
|
+ projectOBBOntoAxis(b, axis, minB, maxB);
|
|
|
+
|
|
|
+ if (maxA < minB || maxB < minA)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+RayCollision getRayCollisionOBB(Ray ray, ref const OBB obb)
|
|
|
+{
|
|
|
+ RayCollision result;
|
|
|
+ result.hit = false;
|
|
|
+ result.distance = 0;
|
|
|
+ result.normal = Vector3(0.0f, 0.0f, 0.0f);
|
|
|
+ result.point = Vector3(0.0f, 0.0f, 0.0f);
|
|
|
+
|
|
|
+ // Move ray into OBB's local space
|
|
|
+ Vector3 localOrigin = Vector3Subtract(ray.position, obb.center);
|
|
|
+ Quaternion inverseRot = QuaternionInvert(obb.rotation);
|
|
|
+ Vector3 localRayOrigin = Vector3RotateByQuaternion(localOrigin, inverseRot);
|
|
|
+ Vector3 localRayDir = Vector3RotateByQuaternion(ray.direction, inverseRot);
|
|
|
+
|
|
|
+ Vector3 boxMin = Vector3Negate(obb.halfExtents);
|
|
|
+ Vector3 boxMax = obb.halfExtents;
|
|
|
+
|
|
|
+ // Ray vs AABB in OBB-local space
|
|
|
+ float tmin = -float.infinity;
|
|
|
+ float tmax = float.infinity;
|
|
|
+ Vector3 normal = Vector3(0);
|
|
|
+
|
|
|
+ for (int i = 0; i < 3; ++i)
|
|
|
+ {
|
|
|
+ float origin;
|
|
|
+ float dir;
|
|
|
+ float min;
|
|
|
+ float max;
|
|
|
+
|
|
|
+ switch (i)
|
|
|
+ {
|
|
|
+ case 0:
|
|
|
+ origin = localRayOrigin.x;
|
|
|
+ dir = localRayDir.x;
|
|
|
+ min = boxMin.x;
|
|
|
+ max = boxMax.x;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ origin = localRayOrigin.y;
|
|
|
+ dir = localRayDir.y;
|
|
|
+ min = boxMin.y;
|
|
|
+ max = boxMax.y;
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ origin = localRayOrigin.z;
|
|
|
+ dir = localRayDir.z;
|
|
|
+ min = boxMin.z;
|
|
|
+ max = boxMax.z;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ assert(false);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (fabs(dir) < 0.0001f)
|
|
|
+ {
|
|
|
+ if (origin < min || origin > max)
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ float ood = 1.0f / dir;
|
|
|
+ float t1 = (min - origin) * ood;
|
|
|
+ float t2 = (max - origin) * ood;
|
|
|
+ int axis = i;
|
|
|
+
|
|
|
+ if (t1 > t2)
|
|
|
+ {
|
|
|
+ float temp = t1;
|
|
|
+ t1 = t2;
|
|
|
+ t2 = temp;
|
|
|
+ axis = -axis;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (t1 > tmin)
|
|
|
+ {
|
|
|
+ tmin = t1;
|
|
|
+ normal = Vector3(0);
|
|
|
+ switch (abs(axis))
|
|
|
+ {
|
|
|
+ case 0:
|
|
|
+ normal.x = axis >= 0 ? -1.0f : 1.0f;
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ normal.y = axis >= 0 ? -1.0f : 1.0f;
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ normal.z = axis >= 0 ? -1.0f : 1.0f;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (t2 < tmax)
|
|
|
+ {
|
|
|
+ tmax = t2;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (tmin > tmax)
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // Convert result to world space
|
|
|
+ result.hit = true;
|
|
|
+ result.distance = tmin;
|
|
|
+ result.point = Vector3Add(ray.position, Vector3Scale(ray.direction, tmin));
|
|
|
+ result.normal = Vector3RotateByQuaternion(normal, obb.rotation);
|
|
|
+
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+bool checkCollisionSphereVsOBB(Vector3 sphereCenter, float radius, ref const OBB obb)
|
|
|
+{
|
|
|
+ Vector3 localCenter = Vector3Subtract(sphereCenter, obb.center);
|
|
|
+ Quaternion invRot = QuaternionInvert(obb.rotation);
|
|
|
+ localCenter = Vector3RotateByQuaternion(localCenter, invRot);
|
|
|
+
|
|
|
+ Vector3 Clamped = Vector3(
|
|
|
+ Clamp(localCenter.x, -obb.halfExtents.x, obb.halfExtents.x),
|
|
|
+ Clamp(localCenter.y, -obb.halfExtents.y, obb.halfExtents.y),
|
|
|
+ Clamp(localCenter.z, -obb.halfExtents.z, obb.halfExtents.z)
|
|
|
+ );
|
|
|
+
|
|
|
+ Vector3 worldClamped = Vector3RotateByQuaternion(Clamped, obb.rotation);
|
|
|
+ worldClamped = Vector3Add(worldClamped, obb.center);
|
|
|
+
|
|
|
+ float distSq = Vector3DistanceSqr(sphereCenter, worldClamped);
|
|
|
+ return distSq <= radius * radius;
|
|
|
+}
|
|
|
+
|
|
|
+void placeOBB(int index, Vector3 position, Vector3 size, Vector3 rotation) {
|
|
|
+ OBB newOBB;
|
|
|
+ newOBB.center = position;
|
|
|
+ newOBB.halfExtents = size;
|
|
|
+ newOBB.rotation = QuaternionFromEuler(
|
|
|
+ rotation.x * raylib.DEG2RAD,
|
|
|
+ rotation.y * raylib.DEG2RAD,
|
|
|
+ rotation.z * raylib.DEG2RAD
|
|
|
+ );
|
|
|
+ if (collisions.length <= index) {
|
|
|
+ collisions.length += 1;
|
|
|
+ }
|
|
|
+ collisions[index] = newOBB;
|
|
|
+}
|
|
|
+
|
|
|
+void removeOBB(int index) {
|
|
|
+ collisions = collisions[0 .. index] ~ collisions[index + 1 .. $];
|
|
|
+}
|
|
|
+
|
|
|
+void updatePlayerOBB(ref OBB playerOBB, Vector3 playerPosition, Vector3 modelCharacterSize, float playerRotation)
|
|
|
+{
|
|
|
+ playerOBB.center = Vector3(playerPosition.x,
|
|
|
+ playerPosition.y + modelCharacterSize.y / 2,
|
|
|
+ playerPosition.z);
|
|
|
+ playerOBB.halfExtents = Vector3(modelCharacterSize.x / 2,
|
|
|
+ modelCharacterSize.y / 2,
|
|
|
+ modelCharacterSize.z / 2);
|
|
|
+ playerOBB.rotation = QuaternionFromAxisAngle(Vector3(0, 1, 0), playerRotation * raylib.raymath.DEG2RAD);
|
|
|
+}
|