From 619eadb6437930ba711610bc74f4bf3882a4f1a1 Mon Sep 17 00:00:00 2001 From: James Lambert Date: Sun, 3 Apr 2022 15:20:53 -0600 Subject: [PATCH] Work on collision quad --- src/physics/collision_box.c | 18 +- src/physics/collision_box.h | 1 - src/physics/collision_quad.c | 384 ++++++++++++++++++++++++++++++++++- src/physics/collision_quad.h | 11 +- src/physics/contact_solver.c | 4 +- src/physics/contact_solver.h | 2 + src/scene/cube.c | 4 - src/util/time.h | 2 +- 8 files changed, 400 insertions(+), 26 deletions(-) diff --git a/src/physics/collision_box.c b/src/physics/collision_box.c index 274daf4..db5dbef 100644 --- a/src/physics/collision_box.c +++ b/src/physics/collision_box.c @@ -2,10 +2,6 @@ #include -#define NORMAL_ZERO_BIAS 0.001f - -#define NEGATIVE_PENETRATION_BIAS 0.1f - struct ColliderCallbacks gCollisionBoxCallbacks = { collisionBoxCollidePlane, collisionBoxSolidMofI, @@ -78,8 +74,8 @@ int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct output->tangentVectors[0] = gRight; output->tangentVectors[1] = gForward; - output->restitution = 0.0f; - output->friction = 1.0f; + output->restitution = 0.1f; + output->friction = 0.5f; if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, output, id)) { return 0; @@ -128,16 +124,6 @@ int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct return 1; } -void collisionBoxCollideQuad(struct CollisionBox* box, struct Transform* boxTransform, struct CollisionQuad* quad) { - struct Quaternion inverseBoxRotation; - - quatConjugate(&boxTransform->rotation, &inverseBoxRotation); - - struct Vector3 normalInBoxSpace; - - quatMultVector(&inverseBoxRotation, &quad->normal, &normalInBoxSpace); -} - float collisionBoxSolidMofI(struct ColliderTypeData* typeData, float mass) { float singleSide = sqrtf(vector3MagSqrd(&((struct CollisionBox*)typeData->data)->sideLength)); return mass * singleSide * singleSide * (1.0f / 6.0f); diff --git a/src/physics/collision_box.h b/src/physics/collision_box.h index 330f960..4482b8d 100644 --- a/src/physics/collision_box.h +++ b/src/physics/collision_box.h @@ -15,7 +15,6 @@ struct CollisionBox { extern struct ColliderCallbacks gCollisionBoxCallbacks; int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct Plane* plane, struct ContactConstraintState* contact); -void collisionBoxCollideQuad(struct CollisionBox* box, struct Transform* boxTransform, struct CollisionQuad* quad); float collisionBoxSolidMofI(struct ColliderTypeData* typeData, float mass); diff --git a/src/physics/collision_quad.c b/src/physics/collision_quad.c index 356fcb7..b9fe993 100644 --- a/src/physics/collision_quad.c +++ b/src/physics/collision_quad.c @@ -1,2 +1,384 @@ -#include "collision_quad.h" \ No newline at end of file +#include "collision_quad.h" + +#include "collision_box.h" +#include "../math/mathf.h" + +#define EDGE_ZERO_BIAS 0.001f + +#define POINT_NO_OVERLAP -1 + +/** + * ------- + * | 3 | + * ^ |0 2| + * | | 1 | + * edge b ------- + * edge a --> + */ + +int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQuad* quad, struct Vector3* point, struct ContactConstraintState* output, int id) { + struct Vector3 worldPoint; + struct ContactState* contact = &output->contacts[output->contactCount]; + + quatMultVector(&boxTransform->rotation, point, &contact->rb); + vector3Add(&contact->rb, &boxTransform->position, &worldPoint); + float penetration = planePointDistance(&quad->plane, &worldPoint); + + if (penetration >= NEGATIVE_PENETRATION_BIAS) { + return POINT_NO_OVERLAP; + } + + struct Vector3 relative; + vector3Sub(&worldPoint, &quad->corner, &relative); + float edgeDistance = vector3Dot(&relative, &quad->edgeA); + + int edgeMask = 0; + + if (edgeDistance < -EDGE_ZERO_BIAS) { + edgeMask |= 1 << 0; + } + + if (edgeDistance > quad->edgeALength + EDGE_ZERO_BIAS) { + edgeMask |= 1 << 2; + } + + edgeDistance = vector3Dot(&relative, &quad->edgeB); + + if (edgeDistance < -EDGE_ZERO_BIAS) { + edgeMask |= 1 << 1; + } + + if (edgeDistance > quad->edgeBLength + EDGE_ZERO_BIAS) { + edgeMask |= 1 << 3; + } + + if (edgeMask) { + return edgeMask; + } + + vector3AddScaled(&worldPoint, &quad->plane.normal, -penetration, &contact->ra); + + ++output->contactCount; + contact->id = id; + contact->penetration = penetration; + contact->bias = 0; + contact->normalMass = 0; + contact->tangentMass[0] = 0.0f; + contact->tangentMass[1] = 0.0f; + contact->normalImpulse = 0.0f; + contact->tangentImpulse[0] = 0.0f; + contact->tangentImpulse[1] = 0.0f; + + return 0; +} + + +struct CubeEdge { + char corneredge; +}; + +char otherAxis[] = { + 1, + 0, + 0, +}; + +char secondOtherAxis[] = { + 2, + 2, + 1, +}; + +#define ID_FROM_AXIS_DIRS(positiveX, positiveY, positiveZ) (((positiveX) ? 0 : 1) | ((positiveY) ? 0 : 2) | ((positiveZ) ? 0 : 4)) +#define ID_SEGEMENT_FROM_AXIS(axisNumber, isPositive) ((isPositive) ? (1 << axisNumber) : 0) + +struct CollisionEdge { + struct Vector3 origin; + struct Vector3 direction; + float length; +}; + +// contactPoint(Pa, Da, Pb, Db) +// offset = Pa - Pb +// Dota = Da * offset +// Dotb = Db * offset +// edgesDot = Da * Db +// denomInv = 1.0f / (1 - edgesDot * edgesDot) +// a = (edgesDot * Dotb - Dota) * denomInv +// b = (Dotb - edgesDot * Dota) * denomInv +// nearestPointA = Pa + Da * a +// nearestPointB = Pb + Db * b + +struct ContactState* _collisionEdgeEdge(struct CollisionEdge* quadEdge, struct CollisionEdge* cubeEdge, struct ContactConstraintState* output) { + float edgesDot = vector3Dot(&quadEdge->direction, &cubeEdge->direction); + + float denomInv = 1.0f - edgesDot * edgesDot; + + if (denomInv < 0.0001f) { + return NULL; + } + + denomInv = 1.0f / denomInv; + + struct Vector3 offset; + vector3Sub(&cubeEdge->origin, &quadEdge->origin, &offset); + + float cubeDot = vector3Dot(&cubeEdge->direction, &offset); + float edgeDot = vector3Dot(&quadEdge->direction, &offset); + + float quadLerp = (edgeDot - edgesDot * cubeDot) * denomInv; + + if (quadLerp < -EDGE_ZERO_BIAS || quadLerp > quadEdge->length + EDGE_ZERO_BIAS) { + return NULL; + } + + float cubeLerp = (edgesDot * edgeDot - cubeDot) * denomInv; + + if (cubeLerp < -EDGE_ZERO_BIAS || cubeLerp > cubeEdge->length + EDGE_ZERO_BIAS) { + return NULL; + } + + if (output->contactCount == MAX_CONTACTS_PER_MANIFOLD) { + return NULL; + } + + struct ContactState* result = &output->contacts[output->contactCount]; + + vector3AddScaled(&quadEdge->origin, &quadEdge->direction, quadLerp, &result->rb); + vector3AddScaled(&cubeEdge->origin, &cubeEdge->direction, cubeLerp, &result->ra); + + return result; +} + +void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTransform, int cornerIds, struct CollisionEdge* edge, struct Vector3* edgeDirection, struct ContactConstraintState* output) { + struct Quaternion inverseBoxRotation; + quatConjugate(&boxTransform->rotation, &inverseBoxRotation); + + for (int axis = 0; axis < 3; ++axis) { + struct CollisionEdge cubeEdge; + cubeEdge.direction = gZeroVec; + VECTOR3_AS_ARRAY(&cubeEdge.direction)[axis] = 1.0f; + cubeEdge.length = VECTOR3_AS_ARRAY(&box->sideLength)[axis] * 2.0f; + + int firstAxis = otherAxis[axis]; + int secondAxis = otherAxis[axis]; + + VECTOR3_AS_ARRAY(&cubeEdge.origin)[axis] = -VECTOR3_AS_ARRAY(&box->sideLength)[axis]; + + for (int corner = 0; corner < 4; ++corner) { + int isFirstPositive = corner & 1; + int isSecondPositive = corner & 2; + int cornerID = ID_SEGEMENT_FROM_AXIS(firstAxis, isFirstPositive) | ID_SEGEMENT_FROM_AXIS(secondAxis, isSecondPositive); + + int cornerIDMask = (1 << cornerID) | (1 << (cornerID | ID_SEGEMENT_FROM_AXIS(axis, 0))); + if ((cornerIDMask & cornerIds) == 0) { + continue; + } + + VECTOR3_AS_ARRAY(&cubeEdge.origin)[firstAxis] = isFirstPositive ? VECTOR3_AS_ARRAY(&box->sideLength)[firstAxis] : -VECTOR3_AS_ARRAY(&box->sideLength)[firstAxis]; + VECTOR3_AS_ARRAY(&cubeEdge.origin)[secondAxis] = isSecondPositive ? VECTOR3_AS_ARRAY(&box->sideLength)[secondAxis] : -VECTOR3_AS_ARRAY(&box->sideLength)[secondAxis]; + + struct ContactState* contact = _collisionEdgeEdge(edge, &cubeEdge, output); + + if (!contact) { + continue; + } + + // check if contact point is inside of box + if (fabsf(contact->ra.x) > box->sideLength.x || + fabsf(contact->ra.y) > box->sideLength.y || + fabsf(contact->ra.z) > box->sideLength.z) { + continue; + } + + if (output->contactCount == 0) { + vector3Cross(&edge->direction, &cubeEdge.direction, &output->normal); + vector3Normalize(&output->normal, &output->normal); + + if (vector3Dot(edgeDirection, &output->normal) < 0.0f) { + vector3Negate(&output->normal, &output->normal); + } + + quatMultVector(&boxTransform->rotation, &output->normal, &output->normal); + } + + // TODO transform contact back into world space + + quatMultVector(&boxTransform->rotation, &contact->ra, &contact->ra); + quatMultVector(&boxTransform->rotation, &contact->rb, &contact->rb); + + contact->penetration = vector3Dot(&contact->ra, &output->normal) - vector3Dot(&contact->rb, &output-> normal); + if (contact->penetration >= NEGATIVE_PENETRATION_BIAS) { + return; + } + + vector3Add(&boxTransform->position, &contact->rb, &contact->rb); + + ++output->contactCount; + contact->id = 8 + axis + corner * 4; + contact->bias = 0; + contact->normalMass = 0; + contact->tangentMass[0] = 0.0f; + contact->tangentMass[1] = 0.0f; + contact->normalImpulse = 0.0f; + contact->tangentImpulse[0] = 0.0f; + contact->tangentImpulse[1] = 0.0f; + + } + } + + return; +} + +int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output) { + struct CollisionBox* box = (struct CollisionBox*)data; + + float boxDistance = planePointDistance(&quad->plane, &boxTransform->position); + float maxBoxReach = vector3MagSqrd(&box->sideLength); + + if (boxDistance > maxBoxReach || boxDistance < 0.0f) { + return 0; + } + + struct Quaternion inverseBoxRotation; + + quatConjugate(&boxTransform->rotation, &inverseBoxRotation); + + struct Vector3 normalInBoxSpace; + + quatMultVector(&inverseBoxRotation, &quad->plane.normal, &normalInBoxSpace); + + struct Vector3 deepestCorner; + + int id = 0; + + for (int axis = 0; axis < 3; ++axis) { + float normalValue = VECTOR3_AS_ARRAY(&normalInBoxSpace)[axis]; + if (normalValue < 0) { + VECTOR3_AS_ARRAY(&deepestCorner)[axis] = VECTOR3_AS_ARRAY(&box->sideLength)[axis]; + } else { + VECTOR3_AS_ARRAY(&deepestCorner)[axis] = -VECTOR3_AS_ARRAY(&box->sideLength)[axis]; + id |= 1 << axis; + } + } + + output->contactCount = 0; + output->normal = quad->plane.normal; + // TODO actually calculate tangent + output->tangentVectors[0] = gRight; + output->tangentVectors[1] = gForward; + + output->restitution = 0.0f; + output->friction = 1.0f; + + int edges = _collsionBuildQuadContact(boxTransform, quad, &deepestCorner, output, id); + int cornerIds = 0; + + if (edges == POINT_NO_OVERLAP) { + return 0; + } + + if (edges > 0) { + cornerIds |= (1 << id); + } + + struct Vector3 sideLengthProjected; + vector3Multiply(&box->sideLength, &normalInBoxSpace, &sideLengthProjected); + + int minAxis = 0; + float minAxisDistnace = fabsf(sideLengthProjected.x); + int maxAxis = 0; + float maxAxisDistnace = minAxisDistnace; + + for (int axis = 1; axis < 3; ++axis) { + float length = fabsf(VECTOR3_AS_ARRAY(&sideLengthProjected)[axis]); + + if (length < minAxisDistnace) { + minAxisDistnace = length; + minAxis = axis; + } + + if (length > maxAxisDistnace) { + maxAxisDistnace = length; + maxAxis = axis; + } + } + + int midAxis = 3 - maxAxis - minAxis; + + struct Vector3 nextFurthestPoint = deepestCorner; + + int nextId = id ^ (1 << minAxis); + VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis]; + int pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId); + edges |= pointEdges; + if (pointEdges > 0) { + cornerIds |= (1 << id); + } + + nextId = nextId ^ (1 << midAxis); + VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis]; + pointEdges =_collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId); + edges |= pointEdges; + if (pointEdges > 0) { + cornerIds |= (1 << id); + } + + nextId = nextId ^ (1 << minAxis); + VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis]; + pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId); + edges |= pointEdges; + if (pointEdges > 0) { + cornerIds |= (1 << id); + } + + edges &= quad->enabledEdges; + + if (edges && cornerIds) { + struct CollisionEdge edge; + struct Vector3 edgeDirection; + + struct Vector3 localOrigin; + struct Vector3 localEdgeA; + struct Vector3 localEdgeB; + + quatMultVector(&inverseBoxRotation, &quad->edgeA, &localEdgeA); + quatMultVector(&inverseBoxRotation, &quad->edgeB, &localEdgeB); + + vector3Sub(&edge.origin, &boxTransform->position, &localOrigin); + quatMultVector(&inverseBoxRotation, &localOrigin, &localOrigin); + + if (edges & (1 << 0)) { + edge.origin = localOrigin; + edge.direction = localEdgeB; + edge.length = quad->edgeBLength; + vector3Negate(&localEdgeA, &edgeDirection); + _collisionBoxCollideEdge(box, boxTransform, cornerIds, &edge, &edgeDirection, output); + } + + if (edges & (1 << 1)) { + edge.origin = localOrigin; + edge.direction = localEdgeA; + edge.length = quad->edgeALength; + vector3Negate(&localEdgeB, &edgeDirection); + _collisionBoxCollideEdge(box, boxTransform, cornerIds, &edge, &edgeDirection, output); + } + + if (edges & (1 << 2)) { + vector3AddScaled(&localOrigin, &localEdgeA, quad->edgeALength, &edge.origin); + edge.direction = localEdgeB; + edge.length = quad->edgeBLength; + _collisionBoxCollideEdge(box, boxTransform, cornerIds, &edge, &localEdgeA, output); + } + + if (edges & (1 << 2)) { + vector3AddScaled(&localOrigin, &localEdgeB, quad->edgeBLength, &edge.origin); + edge.direction = localEdgeA; + edge.length = quad->edgeALength; + _collisionBoxCollideEdge(box, boxTransform, cornerIds, &edge, &localEdgeB, output); + } + } + + return output->contactCount > 0; +} \ No newline at end of file diff --git a/src/physics/collision_quad.h b/src/physics/collision_quad.h index 0c4c96c..ff9c60b 100644 --- a/src/physics/collision_quad.h +++ b/src/physics/collision_quad.h @@ -2,6 +2,9 @@ #define __COLLISION_QUAD_H__ #include "../math/vector3.h" +#include "../math/plane.h" +#include "../math/transform.h" +#include "contact_solver.h" struct CollisionQuad { struct Vector3 corner; @@ -9,7 +12,13 @@ struct CollisionQuad { float edgeALength; struct Vector3 edgeB; float edgeBLength; - struct Vector3 normal; + struct Plane plane; + // used to filter out concave edges or + // reduce duplicate checks for faces + // that share edges + u8 enabledEdges; }; +int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output); + #endif \ No newline at end of file diff --git a/src/physics/contact_solver.c b/src/physics/contact_solver.c index 0bde078..861cd2c 100644 --- a/src/physics/contact_solver.c +++ b/src/physics/contact_solver.c @@ -7,9 +7,9 @@ #include -#define Q3_BAUMGARTE 0.03f +#define Q3_BAUMGARTE 0.05f -#define Q3_PENETRATION_SLOP 0.01f +#define Q3_PENETRATION_SLOP 0.001f #define ENABLE_FRICTION 1 diff --git a/src/physics/contact_solver.h b/src/physics/contact_solver.h index 5a3970f..ff9aba8 100644 --- a/src/physics/contact_solver.h +++ b/src/physics/contact_solver.h @@ -26,6 +26,8 @@ struct ContactState #define MAX_CONTACTS_PER_MANIFOLD 8 +#define NEGATIVE_PENETRATION_BIAS 0.00001f + struct ContactConstraintState { struct ContactState contacts[ MAX_CONTACTS_PER_MANIFOLD ]; diff --git a/src/scene/cube.c b/src/scene/cube.c index 0211fcb..15cc321 100644 --- a/src/scene/cube.c +++ b/src/scene/cube.c @@ -38,10 +38,6 @@ void cubeUpdate(struct Cube* cube) { collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver); contactSolverSolve(&gContactSolver); rigidBodyUpdate(&cube->rigidBody); - - collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver); - contactSolverSolve(&gContactSolver); - rigidBodyUpdate(&cube->rigidBody); } void cubeRender(struct Cube* cube, struct RenderState* renderState) { diff --git a/src/util/time.h b/src/util/time.h index ac551e5..c6c42f1 100644 --- a/src/util/time.h +++ b/src/util/time.h @@ -6,7 +6,7 @@ extern float gTimePassed; extern OSTime gLastTime; -#define FIXED_DELTA_TIME (1.0f / 120.0f) +#define FIXED_DELTA_TIME (1.0f / 60.0f) void timeUpdateDelta();