Archive/portal64-still-alive
1
0
Fork 0
mirror of https://github.com/mwpenny/portal64-still-alive.git synced 2024-10-20 10:37:37 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity

Remove old collision system and try to get new one to work

Browse source
This commit is contained in:
James Lambert 2022-06-01 22:47:51 -06:00
parent c1c9f25adc
commit 0747b078eb
15 changed files with 158 additions and 959 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/physics/collision_box.c
View file

@ -6,7 +6,7 @@
#include <math.h>
struct ColliderCallbacks gCollisionBoxCallbacks = {
collisionBoxCollideQuad,
NULL,
raycastBox,
collisionBoxSolidMofI,
collisionBoxBoundingBox,

329
src/physics/collision_cylinder.c
View file

@ -7,7 +7,7 @@
#include "line.h"
struct ColliderCallbacks gCollisionCylinderCallbacks = {
collisionCylinderCollideQuad,
NULL,
raycastCylinder,
collisionCylinderSolidMofI,
collisionCylinderBoundingBox,
@ -34,331 +34,4 @@ void collisionCylinderBoundingBox(struct ColliderTypeData* typeData, struct Tran
quatRotatedBoundingBoxSize(&transform->rotation, &radius, &halfSize);
vector3Sub(&transform->position, &halfSize, &box->min);
vector3Add(&transform->position, &halfSize, &box->max);
}
int _collisionPointCheckOverlapWithQuad(struct Vector3* pointToCheck, struct Vector3* colliderCenter, struct CollisionQuad* quad, struct ContactManifold* output, int id) {
float edgeDistance = planePointDistance(&quad->plane, pointToCheck);
if (edgeDistance > NEGATIVE_PENETRATION_BIAS) {
return POINT_NO_OVERLAP;
}
int edgesToCheck = collisionQuadDetermineEdges(pointToCheck, quad);
if (!edgesToCheck) {
if (output->contactCount == 0) {
collisionQuadInitializeNormalContact(quad, output);
}
struct ContactPoint* contact = &output->contacts[output->contactCount];
++output->contactCount;
vector3Sub(pointToCheck, colliderCenter, &contact->rb);
vector3AddScaled(pointToCheck, &quad->plane.normal, -edgeDistance, &contact->ra);
contact->id = id;
contact->penetration = edgeDistance;
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 edgesToCheck;
}
int _collisionCylinderParallel(struct CollisionCylinder* cylinder, struct Transform* cylinderTransform, struct Vector3* centerAxis, struct Vector3* crossAxis, float normalDotProduct, struct CollisionQuad* quad, struct ContactManifold* output) {
struct Vector3 edgeEndpoint;
vector3AddScaled(&cylinderTransform->position, centerAxis, normalDotProduct > 0.0f ? -cylinder->halfHeight : cylinder->halfHeight, &edgeEndpoint);
vector3Add(&edgeEndpoint, crossAxis, &edgeEndpoint);
int edgesToCheck = _collisionPointCheckOverlapWithQuad(&edgeEndpoint, &cylinderTransform->position, quad, output, 0);
if (edgesToCheck == POINT_NO_OVERLAP) {
return 0;
}
vector3AddScaled(&edgeEndpoint, centerAxis, (normalDotProduct > 0.0f ? 2.0f : -2.0f) * cylinder->halfHeight, &edgeEndpoint);
int otherEdgesToCheck = _collisionPointCheckOverlapWithQuad(&edgeEndpoint, &cylinderTransform->position, quad, output, 1);
if (otherEdgesToCheck != POINT_NO_OVERLAP) {
edgesToCheck |= otherEdgesToCheck;
}
return edgesToCheck;
}
int _collisionCylinderPerpendicular(struct CollisionCylinder* cylinder, struct Transform* cylinderTransform, struct Vector3* centerAxis, struct Vector3* crossAxis, float normalDotProduct, struct CollisionQuad* quad, struct ContactManifold* output) {
float centerDistance = planePointDistance(&quad->plane, &cylinderTransform->position);
if (centerDistance < -cylinder->radius) {
return 0;
}
struct Vector3 edgeEndpoint;
struct Vector3 centerPoint;
vector3AddScaled(&cylinderTransform->position, centerAxis, normalDotProduct > 0.0f ? -cylinder->halfHeight : cylinder->halfHeight, &centerPoint);
vector3Add(&centerPoint, crossAxis, &edgeEndpoint);
int edgesToCheck = _collisionPointCheckOverlapWithQuad(&edgeEndpoint, &cylinderTransform->position, quad, output, 0);
if (edgesToCheck == POINT_NO_OVERLAP) {
return 0;
}
struct Vector3 rotatedCrossAxis;
vector3Cross(centerAxis, crossAxis, &rotatedCrossAxis);
vector3Add(&centerPoint, &rotatedCrossAxis, &edgeEndpoint);
int otherEdgesToCheck = _collisionPointCheckOverlapWithQuad(&edgeEndpoint, &cylinderTransform->position, quad, output, 2);
if (otherEdgesToCheck == POINT_NO_OVERLAP) {
return edgesToCheck;
}
edgesToCheck |= otherEdgesToCheck;
vector3Negate(&rotatedCrossAxis, &rotatedCrossAxis);
vector3Add(&centerPoint, &rotatedCrossAxis, &edgeEndpoint);
otherEdgesToCheck = _collisionPointCheckOverlapWithQuad(&edgeEndpoint, &cylinderTransform->position, quad, output, 3);
if (otherEdgesToCheck == POINT_NO_OVERLAP) {
return edgesToCheck;
}
edgesToCheck |= otherEdgesToCheck;
vector3Negate(crossAxis, crossAxis);
vector3Add(&centerPoint, crossAxis, &edgeEndpoint);
otherEdgesToCheck = _collisionPointCheckOverlapWithQuad(&edgeEndpoint, &cylinderTransform->position, quad, output, 4);
if (otherEdgesToCheck != POINT_NO_OVERLAP) {
edgesToCheck |= otherEdgesToCheck;
}
return edgesToCheck;
}
#define EDGE_LERP_BIAS 0.00001f
void collisionCylinderSingleCap(struct CollisionCylinder* cylinder, struct Transform* cylinderTransform, struct Vector3* centerAxis, float capDistance, float invDot, int idOffset, struct CollisionEdge* edge, struct Vector3* edgeDirection, struct ContactManifold* output) {
float dOffset = vector3Dot(&cylinderTransform->position, centerAxis);
float lineDot = vector3Dot(&edge->origin, centerAxis);
float distance = -(lineDot - (dOffset + capDistance)) * invDot;
if (distance < EDGE_LERP_BIAS || distance > edge->length + EDGE_LERP_BIAS) {
return;
}
struct ContactPoint* contact = &output->contacts[output->contactCount];
vector3AddScaled(&cylinderTransform->position, centerAxis, capDistance, &contact->rb);
vector3AddScaled(&edge->origin, &edge->direction, distance, &contact->ra);
struct Vector3 offset;
vector3Sub(&contact->rb, &contact->ra, &offset);
float offsetSqrd = vector3MagSqrd(&offset);
if (offsetSqrd > cylinder->radius * cylinder->radius) {
return;
}
if (vector3Dot(&offset, edgeDirection) < 0.0f) {
return;
}
if (output->contactCount == 0) {
if (offsetSqrd < 0.00001f) {
vector3Cross(centerAxis, edgeDirection, &output->normal);
} else {
vector3Sub(&contact->ra, &contact->ra, &output->normal);
}
vector3Normalize(&output->normal, &output->normal);
output->tangentVectors[0] = *centerAxis;
vector3Cross(&output->normal, &output->tangentVectors[0], &output->tangentVectors[1]);
output->restitution = 0.0f;
output->friction = 1.0f;
}
vector3AddScaled(&contact->rb, &output->normal, -cylinder->radius, &contact->rb);
contact->penetration = vector3Dot(&contact->ra, &output->normal) - vector3Dot(&contact->rb, &output-> normal);
if (contact->penetration >= NEGATIVE_PENETRATION_BIAS) {
return;
}
contact->id = idOffset;
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;
++output->contactCount;
}
int collisionCylinderCap(struct CollisionCylinder* cylinder, struct Transform* cylinderTransform, struct Vector3* centerAxis, float normalDotProduct, int idOffset, struct CollisionEdge* edge, struct Vector3* edgeDirection, struct ContactManifold* output) {
if (fabsf(normalDotProduct) < 0.00001f) {
return 0;
}
float invDot = 1.0f / normalDotProduct;
collisionCylinderSingleCap(cylinder, cylinderTransform, centerAxis, cylinder->halfHeight, invDot, idOffset + 1, edge, edgeDirection, output);
collisionCylinderSingleCap(cylinder, cylinderTransform, centerAxis, -cylinder->halfHeight, invDot, idOffset + 2, edge, edgeDirection, output);
return output->contactCount > 0;
}
int collisionCylinderEdge(struct CollisionCylinder* cylinder, struct Transform* cylinderTransform, struct Vector3* centerAxis, float normalDotProduct, int idOffset, struct CollisionEdge* edge, struct Vector3* edgeDirection, struct ContactManifold* output) {
float cylinderLerp;
float edgeLerp;
if (!lineNearestApproach(&cylinderTransform->position, centerAxis, &edge->origin, &edge->direction, &cylinderLerp, &edgeLerp)) {
return collisionCylinderCap(cylinder, cylinderTransform, centerAxis, normalDotProduct, idOffset, edge, edgeDirection, output);
}
struct ContactPoint* contact = &output->contacts[output->contactCount];
vector3AddScaled(&cylinderTransform->position, centerAxis, cylinderLerp, &contact->rb);
vector3AddScaled(&edge->origin, &edge->direction, edgeLerp, &contact->ra);
struct Vector3 offset;
vector3Sub(&contact->rb, &contact->ra, &offset);
float offsetSqrd = vector3MagSqrd(&offset);
if (offsetSqrd > cylinder->radius * cylinder->radius) {
return 0;
}
if (cylinderLerp > -cylinder->halfHeight - EDGE_LERP_BIAS && cylinderLerp < cylinder->halfHeight + EDGE_LERP_BIAS &&
edgeLerp > -EDGE_LERP_BIAS && edgeLerp < edge->length + EDGE_LERP_BIAS) {
if (vector3Dot(&offset, edgeDirection) < 0.0f) {
return 0;
}
struct Vector3 offsetNormalized;
if (offsetSqrd < 0.00001f) {
vector3Cross(centerAxis, &edge->direction, &offsetNormalized);
vector3Normalize(&offsetNormalized, &offsetNormalized);
} else {
vector3Scale(&offset, &offsetNormalized, 1.0f / sqrtf(offsetSqrd));
}
if (output->contactCount == 0) {
output->normal = offsetNormalized;
output->tangentVectors[0] = *centerAxis;
vector3Cross(&output->normal, &output->tangentVectors[0], &output->tangentVectors[1]);
output->restitution = 0.0f;
output->friction = 1.0f;
}
vector3AddScaled(&contact->rb, &offsetNormalized, -cylinder->radius, &contact->rb);
contact->penetration = vector3Dot(&contact->ra, &output->normal) - vector3Dot(&contact->rb, &output-> normal);
if (contact->penetration >= NEGATIVE_PENETRATION_BIAS) {
return 0;
}
contact->id = idOffset;
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;
++output->contactCount;
return 1;
}
return collisionCylinderCap(cylinder, cylinderTransform, centerAxis, normalDotProduct, idOffset, edge, edgeDirection, output);
}
int collisionCylinderCollideQuad(void* data, struct Transform* cylinderTransform, struct CollisionQuad* quad, struct ContactManifold* output) {
struct Vector3 centerAxis;
quatMultVector(&cylinderTransform->rotation, &gUp, &centerAxis);
float normalDotProduct = vector3Dot(&centerAxis, &quad->plane.normal);
struct Vector3 capCenterTowardsPlane;
vector3AddScaled(&quad->plane.normal, &centerAxis, -normalDotProduct, &capCenterTowardsPlane);
vector3Negate(&capCenterTowardsPlane, &capCenterTowardsPlane);
float magSqrd = vector3MagSqrd(&capCenterTowardsPlane);
int edgesToCheck;
struct CollisionCylinder* cylinder = (struct CollisionCylinder*)data;
output->contactCount = 0;
if (fabsf(magSqrd) < 0.7f) {
if (magSqrd < 0.0001f) {
// if the two shapes are too well aligned then pick
// an arbitrary axis to use to construct face points
if (fabsf(centerAxis.x) > fabsf(centerAxis.z)) {
vector3Cross(&gForward, &centerAxis, &capCenterTowardsPlane);
} else {
vector3Cross(&gRight, &centerAxis, &capCenterTowardsPlane);
}
vector3Scale(&capCenterTowardsPlane, &capCenterTowardsPlane, cylinder->radius / sqrtf(vector3MagSqrd(&capCenterTowardsPlane)));
} else {
vector3Scale(&capCenterTowardsPlane, &capCenterTowardsPlane, cylinder->radius / sqrtf(magSqrd));
}
edgesToCheck = _collisionCylinderPerpendicular(cylinder, cylinderTransform, &centerAxis, &capCenterTowardsPlane, normalDotProduct, quad, output);
} else {
vector3Scale(&capCenterTowardsPlane, &capCenterTowardsPlane, cylinder->radius / sqrtf(magSqrd));
edgesToCheck = _collisionCylinderParallel(cylinder, cylinderTransform, &centerAxis, &capCenterTowardsPlane, normalDotProduct, quad, output);
}
if (!edgesToCheck) {
return output->contactCount > 0;
}
struct CollisionEdge edge;
struct Vector3 edgeDirection;
if (edgesToCheck & (1 << 0)) {
edge.origin = quad->corner;
edge.direction = quad->edgeB;
edge.length = quad->edgeBLength;
vector3Negate(&quad->edgeA, &edgeDirection);
collisionCylinderEdge(cylinder, cylinderTransform, &centerAxis, normalDotProduct, 5, &edge, &edgeDirection, output);
}
if (edgesToCheck & (1 << 1)) {
edge.origin = quad->corner;
edge.direction = quad->edgeA;
edge.length = quad->edgeALength;
vector3Negate(&quad->edgeB, &edgeDirection);
collisionCylinderEdge(cylinder, cylinderTransform, &centerAxis, normalDotProduct, 8, &edge, &edgeDirection, output);
}
if (edgesToCheck & (1 << 2)) {
vector3AddScaled(&quad->corner, &quad->edgeA, quad->edgeALength, &edge.origin);
edge.direction = quad->edgeB;
edge.length = quad->edgeBLength;
collisionCylinderEdge(cylinder, cylinderTransform, &centerAxis, normalDotProduct, 11, &edge, &edgeDirection, output);
}
if (edgesToCheck & (1 << 3)) {
vector3AddScaled(&quad->corner, &quad->edgeB, quad->edgeBLength, &edge.origin);
edge.direction = quad->edgeA;
edge.length = quad->edgeALength;
collisionCylinderEdge(cylinder, cylinderTransform, &centerAxis, normalDotProduct, 14, &edge, &edgeDirection, output);
}
// TODO check edges and points of quad
return 0;
}

1
src/physics/collision_cylinder.h
View file

@ -11,7 +11,6 @@ struct CollisionCylinder {
extern struct ColliderCallbacks gCollisionCylinderCallbacks;
int collisionCylinderCollideQuad(void* data, struct Transform* cylinderTransform, struct CollisionQuad* quad, struct ContactManifold* output);
float collisionCylinderSolidMofI(struct ColliderTypeData* typeData, float mass);
void collisionCylinderBoundingBox(struct ColliderTypeData* typeData, struct Transform* transform, struct Box3D* box);

52
src/physics/collision_object.c
View file

@ -1,4 +1,8 @@
#include "collision_object.h"
#include "epa.h"
#include "gjk.h"
#include "contact_insertion.h"
#include "collision_scene.h"
void collisionObjectInit(struct CollisionObject* object, struct ColliderTypeData *collider, struct RigidBody* body, float mass) {
object->collider = collider;
@ -7,33 +11,43 @@ void collisionObjectInit(struct CollisionObject* object, struct ColliderTypeData
collisionObjectUpdateBB(object);
}
void collisionObjectCollideWithQuad(struct CollisionObject* object, struct CollisionObject* quad, struct ContactSolver* contactSolver) {
CollideWithQuad quadCollider = object->collider->callbacks->collideWithQuad;
if (!quadCollider) {
void collisionObjectCollideWithQuad(struct CollisionObject* object, struct CollisionObject* quadObject, struct ContactSolver* contactSolver) {
if (!box3DHasOverlap(&object->boundingBox, &quadObject->boundingBox)) {
return;
}
if (!box3DHasOverlap(&object->boundingBox, &quad->boundingBox)) {
struct Simplex simplex;
struct CollisionQuad* quad = (struct CollisionQuad*)quadObject->collider->data;
if (!gjkCheckForOverlap(&simplex,
quad, minkowsiSumAgainstQuad,
object, minkowsiSumAgainstObject,
&quad->plane.normal)) {
return;
}
struct ContactManifold localContact;
localContact.contactCount = 0;
struct EpaResult result;
epaSolve(
&simplex,
quad, minkowsiSumAgainstQuad,
object, minkowsiSumAgainstObject,
&result
);
struct ContactManifold* contact = contactSolverPeekContact(contactSolver, quad, object);
if (quadCollider(object->collider->data, &object->body->transform, quad->collider->data, &localContact)) {
if (!contact) {
return;
}
if (contactSolverAssign(contact, &localContact, 1) && object->body) {
object->body->flags |= RigidBodyIsTouchingPortal;
}
} else if (contact) {
contactSolverRemoveContact(contactSolver, contact);
if (collisionSceneIsTouchingPortal(&result.contactA, &result.normal)) {
object->body->flags |= RigidBodyIsTouchingPortal;
return;
}
transformPointInverse(&object->body->transform, &result.contactB, &result.contactB);
struct ContactManifold* contact = contactSolverGetContactManifold(contactSolver, quadObject, object);
if (!contact) {
return;
}
contactInsert(contact, &result);
}
void collisionObjectUpdateBB(struct CollisionObject* object) {

464
src/physics/collision_quad.c
View file

@ -51,468 +51,4 @@ int collisionQuadDetermineEdges(struct Vector3* worldPoint, struct CollisionQuad
}
return edgeMask;
}
int _collsionBuildQuadContact(struct Transform* boxTransform, struct CollisionQuad* quad, struct Vector3* point, struct ContactManifold* output, int id) {
struct Vector3 worldPoint;
struct ContactPoint* 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;
}
int edgeMask = collisionQuadDetermineEdges(&worldPoint, quad);
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) ? 1 : 0) | ((positiveY) ? 2 : 0) | ((positiveZ) ? 4 : 0))
#define ID_SEGEMENT_FROM_AXIS(axisNumber, isPositive) ((isPositive) ? 0 : (1 << axisNumber))
// 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 ContactPoint* _collisionEdgeEdge(struct CollisionEdge* quadEdge, struct CollisionEdge* cubeEdge, struct ContactManifold* 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;
// float cubeLerp = (edgesDot * edgeDot - cubeDot) * denomInv;
float quadLerp;
float cubeLerp;
if (!lineNearestApproach(
&quadEdge->origin, &quadEdge->direction,
&cubeEdge->origin, &cubeEdge->direction,
&quadLerp, &cubeLerp
)) {
return NULL;
}
if (quadLerp < -EDGE_ZERO_BIAS || quadLerp > quadEdge->length + EDGE_ZERO_BIAS) {
return NULL;
}
if (cubeLerp < -EDGE_ZERO_BIAS || cubeLerp > cubeEdge->length + EDGE_ZERO_BIAS) {
return NULL;
}
if (output->contactCount == MAX_CONTACTS_PER_MANIFOLD) {
return NULL;
}
struct ContactPoint* 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 ContactManifold* 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 = secondOtherAxis[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 ContactPoint* contact = _collisionEdgeEdge(edge, &cubeEdge, output);
if (!contact) {
continue;
}
// check if contact point is inside of box
if (fabsf(contact->ra.x) > box->sideLength.x + NEGATIVE_PENETRATION_BIAS ||
fabsf(contact->ra.y) > box->sideLength.y + NEGATIVE_PENETRATION_BIAS ||
fabsf(contact->ra.z) > box->sideLength.z + NEGATIVE_PENETRATION_BIAS) {
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);
output->tangentVectors[0] = edge->direction;
vector3Cross(&output->normal, &edge->direction, &output->tangentVectors[1]);
}
if (vector3Dot(&contact->ra, edgeDirection) - vector3Dot(&contact->rb, edgeDirection) > 0) {
continue;
}
// rotate from cube space to 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) {
continue;
}
// move edge contact to world space
vector3Add(&boxTransform->position, &contact->ra, &contact->ra);
++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;
}
void _collisionBoxCollidePoint(struct CollisionBox* box, struct Transform* boxTransform, struct Vector3* localPoint, int id, struct ContactManifold* output) {
float minDepth = localPoint->x > 0.0f ? localPoint->x - box->sideLength.x : (-box->sideLength.x - localPoint->x);
int minAxis = 0;
if (minDepth > NEGATIVE_PENETRATION_BIAS) {
return;
}
float check = localPoint->y > 0.0f ? localPoint->y - box->sideLength.y : (-box->sideLength.y - localPoint->y);
if (check > NEGATIVE_PENETRATION_BIAS) {
return;
}
if (check > minDepth) {
minDepth = check;
minAxis = 1;
}
check = localPoint->z > 0.0f ? localPoint->z - box->sideLength.z : (-box->sideLength.z - localPoint->z);
if (check > NEGATIVE_PENETRATION_BIAS) {
return;
}
if (check > minDepth) {
minDepth = check;
minAxis = 2;
}
if (output->contactCount == MAX_CONTACTS_PER_MANIFOLD) {
return;
}
struct ContactPoint* contact = &output->contacts[output->contactCount];
int positiveAxis = VECTOR3_AS_ARRAY(localPoint)[minAxis] > 0.0f;
contact->rb = *localPoint;
contact->ra = *localPoint;
VECTOR3_AS_ARRAY(&contact->ra)[minAxis] = positiveAxis ? VECTOR3_AS_ARRAY(&box->sideLength)[minAxis] : -VECTOR3_AS_ARRAY(&box->sideLength)[minAxis];
if (output->contactCount == 0) {
output->normal = gZeroVec;
VECTOR3_AS_ARRAY(&output->normal)[minAxis] = positiveAxis ? 1.0f : -1.0f;
quatMultVector(&boxTransform->rotation, &output->normal, &output->normal);
}
// rotate from cube space to 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;
}
// move edge contact to world space
vector3Add(&boxTransform->position, &contact->ra, &contact->ra);
++output->contactCount;
contact->id = 24 + minAxis + positiveAxis * 3 + id * 6;
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;
}
int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactManifold* 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 handle non orthogonal edges
output->tangentVectors[0] = quad->edgeA;
output->tangentVectors[1] = quad->edgeB;
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;
// TODO roll up into a loop
int nextId = id ^ (1 << minAxis);
VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis];
int pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId);
if (pointEdges > 0) {
edges |= pointEdges;
cornerIds |= (1 << nextId);
}
nextId = nextId ^ (1 << midAxis);
VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[midAxis];
pointEdges =_collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId);
if (pointEdges > 0) {
edges |= pointEdges;
cornerIds |= (1 << nextId);
}
nextId = nextId ^ (1 << minAxis);
VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis] = -VECTOR3_AS_ARRAY(&nextFurthestPoint)[minAxis];
pointEdges = _collsionBuildQuadContact(boxTransform, quad, &nextFurthestPoint, output, nextId);
if (pointEdges > 0) {
edges |= pointEdges;
cornerIds |= (1 << nextId);
}
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(&quad->corner, &boxTransform->position, &localOrigin);
quatMultVector(&inverseBoxRotation, &localOrigin, &localOrigin);
/**
* -------
* | 3 |
* ^ |0 2|
* | | 1 |
* edge b -------
* edge a -->
*/
// TODO roll up into a loop
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 << 3)) {
vector3AddScaled(&localOrigin, &localEdgeB, quad->edgeBLength, &edge.origin);
edge.direction = localEdgeA;
edge.length = quad->edgeALength;
_collisionBoxCollideEdge(box, boxTransform, cornerIds, &edge, &localEdgeB, output);
}
if (edges & ((1 << 0) | (1 << 1))) {
_collisionBoxCollidePoint(box, boxTransform, &localOrigin, 0, output);
}
if (edges & ((1 << 1) | (1 << 2))) {
struct Vector3 origin;
vector3AddScaled(&localOrigin, &localEdgeA, quad->edgeALength, &origin);
_collisionBoxCollidePoint(box, boxTransform, &origin, 1, output);
}
if (edges & ((1 << 2) | (1 << 3))) {
struct Vector3 origin;
vector3AddScaled(&localOrigin, &localEdgeA, quad->edgeALength, &origin);
vector3AddScaled(&origin, &localEdgeB, quad->edgeBLength, &origin);
_collisionBoxCollidePoint(box, boxTransform, &origin, 2, output);
}
if (edges & ((1 << 3) | (1 << 0))) {
struct Vector3 origin;
vector3AddScaled(&localOrigin, &localEdgeB, quad->edgeBLength, &origin);
_collisionBoxCollidePoint(box, boxTransform, &origin, 3, output);
}
}
return output->contactCount > 0;
}

1
src/physics/collision_quad.h
View file

@ -29,6 +29,5 @@ struct CollisionQuad {
void collisionQuadInitializeNormalContact(struct CollisionQuad* quad, struct ContactManifold* output);
int collisionQuadDetermineEdges(struct Vector3* worldPoint, struct CollisionQuad* quad);
int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactManifold* output);
#endif

7
src/physics/collision_scene.c
View file

@ -3,6 +3,7 @@
#include "math/mathf.h"
#include "gjk.h"
#include "epa.h"
#include "contact_solver.h"
struct CollisionScene gCollisionScene;
@ -112,8 +113,8 @@ int collisionSceneFilterPortalContacts(struct ContactManifold* contact) {
int writeIndex = 0;
for (int readIndex = 0; readIndex < contact->contactCount; ++readIndex) {
if (collisionSceneIsTouchingPortal(&contact->contacts[readIndex].ra, &contact->normal)) {
continue;;
if (collisionSceneIsTouchingPortal(&contact->contacts[readIndex].contactALocal, &contact->normal)) {
continue;
}
if (readIndex != writeIndex) {
@ -374,6 +375,8 @@ void collisionSceneRemoveDynamicObject(struct CollisionObject* object) {
}
void collisionSceneUpdateDynamics() {
contactSolverRemoveUnusedContacts(&gContactSolver);
for (unsigned i = 0; i < gCollisionScene.dynamicObjectCount; ++i) {
collisionObjectCollideWithScene(gCollisionScene.dynamicObjects[i], &gCollisionScene, &gContactSolver);
}

8
src/physics/collision_sphere.c
View file

@ -21,10 +21,10 @@ int collisionSphereCollideQuad(void* data, struct Transform* boxTransform, struc
struct ContactPoint* contact = &output->contacts[output->contactCount];
vector3AddScaled(&quad->corner, &quad->edgeA, aLerp, &contact->ra);
vector3AddScaled(&contact->ra, &quad->edgeB, bLerp, &contact->ra);
vector3AddScaled(&quad->corner, &quad->edgeA, aLerp, &contact->contactAWorld);
vector3AddScaled(&contact->contactAWorld, &quad->edgeB, bLerp, &contact->contactAWorld);
vector3Sub(&boxTransform->position, &contact->ra, &output->normal);
vector3Sub(&boxTransform->position, &contact->contactAWorld, &output->normal);
float outputLength = vector3MagSqrd(&output->normal);
@ -36,7 +36,7 @@ int collisionSphereCollideQuad(void* data, struct Transform* boxTransform, struc
vector3Scale(&output->normal, &output->normal, 1.0f / sqrtf(outputLength));
vector3AddScaled(&boxTransform->position, &output->normal, -sphere->radius, &contact->rb);
vector3AddScaled(&boxTransform->position, &output->normal, -sphere->radius, &contact->contactBWorld);
output->restitution = 0.1f;
output->friction = 0.5f;

20
src/physics/contact_insertion.c
View file

@ -1,5 +1,7 @@
#include "contact_insertion.h"
#define CONTACT_MOVE_TOLERNACE 0.1f
void contactInsert(struct ContactManifold* contactState, struct EpaResult* epaResult) {
int shouldReplace = 1;
int replacementIndex = 0;
@ -7,10 +9,15 @@ void contactInsert(struct ContactManifold* contactState, struct EpaResult* epaRe
int insertIndex;
for (insertIndex = 0; insertIndex < contactState->contactCount; ++insertIndex) {
for (insertIndex = 0; insertIndex < contactState->contactCount && insertIndex < MAX_CONTACT_COUNT; ++insertIndex) {
struct ContactPoint* contactPoint = &contactState->contacts[insertIndex];
if (contactPoint->id == epaResult->id) {
// if the existing contact is close enough then keep it
// if (vector3DistSqrd(&contactPoint->contactALocal, &epaResult->contactA) < CONTACT_MOVE_TOLERNACE &&
// vector3DistSqrd(&contactPoint->contactBLocal, &epaResult->contactB) < CONTACT_MOVE_TOLERNACE) {
// return;
// }
break;
}
@ -27,7 +34,12 @@ void contactInsert(struct ContactManifold* contactState, struct EpaResult* epaRe
}
}
contactState->normal = epaResult->normal;
if (contactState->contactCount == 0) {
contactState->normal = epaResult->normal;
vector3Perp(&contactState->normal, &contactState->tangentVectors[0]);
vector3Normalize(&contactState->tangentVectors[0], &contactState->tangentVectors[0]);
vector3Cross(&contactState->normal, &contactState->tangentVectors[0], &contactState->tangentVectors[1]);
}
if (insertIndex == MAX_CONTACT_COUNT) {
if (!shouldReplace) {
@ -42,8 +54,8 @@ void contactInsert(struct ContactManifold* contactState, struct EpaResult* epaRe
struct ContactPoint* contactPoint = &contactState->contacts[insertIndex];
contactPoint->id = epaResult->id;
contactPoint->ra = epaResult->contactA;
contactPoint->rb = epaResult->contactB;
contactPoint->contactALocal = epaResult->contactA;
contactPoint->contactBLocal = epaResult->contactB;
contactPoint->penetration = epaResult->penetration;
if (insertIndex == contactState->contactCount) {

197
src/physics/contact_solver.c
View file

@ -12,17 +12,81 @@
#define Q3_PENETRATION_SLOP 0.001f
#define SLIDE_TOLERANCE 0.1f
#define SEPERATION_TOLERANCE 0.2f
#define ENABLE_FRICTION 1
struct ContactSolver gContactSolver;
void contactSolverCleanupManifold(struct ContactManifold* manifold) {
int writeIndex = 0;
for (int readIndex = 0; readIndex < manifold->contactCount; ++readIndex) {
struct ContactPoint* contactPoint = &manifold->contacts[readIndex];
struct Vector3 offset;
struct Vector3 worldPosA;
struct Vector3 worldPosB;
if (manifold->shapeA->body) {
transformPoint(&manifold->shapeA->body->transform, &contactPoint->contactALocal, &worldPosA);
} else {
worldPosA = contactPoint->contactALocal;
}
if (manifold->shapeB->body) {
transformPoint(&manifold->shapeB->body->transform, &contactPoint->contactBLocal, &worldPosB);
} else {
worldPosB = contactPoint->contactBLocal;
}
vector3Sub(&worldPosB, &worldPosA, &offset);
contactPoint->penetration = vector3Dot(&offset, &manifold->normal);
// skip this point to remove it
if (contactPoint->penetration > SEPERATION_TOLERANCE) {
continue;
}
vector3AddScaled(&offset, &manifold->normal, -contactPoint->penetration, &offset);
if (vector3MagSqrd(&offset) > SLIDE_TOLERANCE * SLIDE_TOLERANCE) {
continue;
}
// update the world radius for the contact solver
if (manifold->shapeA->body) {
vector3Sub(&worldPosA, &manifold->shapeA->body->transform.position, &worldPosA);
}
if (manifold->shapeB->body) {
vector3Sub(&worldPosB, &manifold->shapeB->body->transform.position, &worldPosB);
}
contactPoint->contactAWorld = worldPosA;
contactPoint->contactBWorld = worldPosA;
if (readIndex != writeIndex) {
manifold->contacts[writeIndex] = *contactPoint;
}
++writeIndex;
}
manifold->contactCount = writeIndex;
}
void contactSolverRemoveUnusedContacts(struct ContactSolver* contactSolver) {
struct ContactManifold* curr = contactSolver->activeContacts;
struct ContactManifold* prev = NULL;
while (curr) {
contactSolverCleanupManifold(curr);
if (!curr->didContactLastFrame) {
if (curr->contactCount == 0) {
if (prev) {
prev->next = curr->next;
} else {
@ -34,7 +98,6 @@ void contactSolverRemoveUnusedContacts(struct ContactSolver* contactSolver) {
contactSolver->unusedContacts = curr;
curr = next;
} else {
curr->didContactLastFrame = 0;
prev = curr;
curr = curr->next;
}
@ -88,9 +151,9 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
// Precalculate JM^-1JT for contact and friction constraints
struct Vector3 raCn;
vector3Cross(&c->ra, &cs->normal, &raCn);
vector3Cross(&c->contactAWorld, &cs->normal, &raCn);
struct Vector3 rbCn;
vector3Cross(&c->rb, &cs->normal, &rbCn);
vector3Cross(&c->contactBWorld, &cs->normal, &rbCn);
float nm = 0;
if (bodyA) {
@ -118,9 +181,9 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
for (int i = 0; i < 2; ++i )
{
struct Vector3 raCt;
vector3Cross(&cs->tangentVectors[ i ], &c->ra, &raCt);
vector3Cross(&cs->tangentVectors[ i ], &c->contactAWorld, &raCt);
struct Vector3 rbCt;
vector3Cross(&cs->tangentVectors[ i ], &c->rb, &rbCt);
vector3Cross(&cs->tangentVectors[ i ], &c->contactBWorld, &rbCt);
if (bodyA) {
tm[ i ] += bodyA->momentOfInertiaInv * vector3MagSqrd(&raCt);
@ -149,13 +212,13 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
if (bodyA) {
vector3AddScaled(vA, &P, -bodyA->massInv, vA);
vector3Cross(&c->ra, &P, &w);
vector3Cross(&c->contactAWorld, &P, &w);
vector3AddScaled(wA, &w, -bodyA->momentOfInertiaInv, wA);
}
if (bodyB) {
vector3AddScaled(vB, &P, bodyB->massInv, vB);
vector3Cross(&c->rb, &P, &w);
vector3Cross(&c->contactBWorld, &P, &w);
vector3AddScaled(wB, &w, bodyB->momentOfInertiaInv, wB);
}
@ -163,14 +226,14 @@ void contactSolverPreSolve(struct ContactSolver* contactSolver) {
struct Vector3 angularVelocity;
if (vB) {
vector3Cross(wB, &c->rb, &angularVelocity);
vector3Cross(wB, &c->contactBWorld, &angularVelocity);
vector3Add(&angularVelocity, vB, &velocity);
} else {
velocity = gZeroVec;
}
if (vA) {
vector3Cross(wA, &c->ra, &angularVelocity);
vector3Cross(wA, &c->contactAWorld, &angularVelocity);
vector3Sub(&velocity, &angularVelocity, &velocity);
vector3Sub(&velocity, vA, &velocity);
}
@ -223,14 +286,14 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
struct Vector3 angularVelocity;
if (wB) {
vector3Cross(wB, &c->rb, &angularVelocity);
vector3Cross(wB, &c->contactBWorld, &angularVelocity);
vector3Add(&angularVelocity, vB, &dv);
} else {
dv = gZeroVec;
}
if (wA) {
vector3Cross(wA, &c->ra, &angularVelocity);
vector3Cross(wA, &c->contactAWorld, &angularVelocity);
vector3Sub(&dv, &angularVelocity, &dv);
vector3Sub(&dv, vA, &dv);
}
@ -257,13 +320,13 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
struct Vector3 w;
if (vA) {
vector3AddScaled(vA, &impulse, -bodyA->massInv, vA);
vector3Cross(&c->ra, &impulse, &w);
vector3Cross(&c->contactAWorld, &impulse, &w);
vector3AddScaled(wA, &w, -bodyA->momentOfInertiaInv, wA);
}
if (vB) {
vector3AddScaled(vB, &impulse, bodyB->massInv, vB);
vector3Cross(&c->rb, &impulse, &w);
vector3Cross(&c->contactBWorld, &impulse, &w);
vector3AddScaled(wB, &w, bodyB->momentOfInertiaInv, wB);
}
}
@ -272,14 +335,14 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
// Normal
{
if (wB) {
vector3Cross(wB, &c->rb, &angularVelocity);
vector3Cross(wB, &c->contactBWorld, &angularVelocity);
vector3Add(&angularVelocity, vB, &dv);
} else {
dv = gZeroVec;
}
if (wA) {
vector3Cross(wA, &c->ra, &angularVelocity);
vector3Cross(wA, &c->contactAWorld, &angularVelocity);
vector3Sub(&dv, &angularVelocity, &dv);
vector3Sub(&dv, vA, &dv);
}
@ -302,13 +365,13 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
struct Vector3 w;
if (vA) {
vector3AddScaled(vA, &impulse, -bodyA->massInv, vA);
vector3Cross(&c->ra, &impulse, &w);
vector3Cross(&c->contactAWorld, &impulse, &w);
vector3AddScaled(wA, &w, -bodyA->momentOfInertiaInv, wA);
}
if (vB) {
vector3AddScaled(vB, &impulse, bodyB->massInv, vB);
vector3Cross(&c->rb, &impulse, &w);
vector3Cross(&c->contactBWorld, &impulse, &w);
vector3AddScaled(wB, &w, bodyB->momentOfInertiaInv, wB);
}
}
@ -320,14 +383,13 @@ void contactSolverIterate(struct ContactSolver* contactSolver) {
void contactSolverSolve(struct ContactSolver* solver) {
contactSolverRemoveUnusedContacts(solver);
contactSolverPreSolve(solver);
contactSolverIterate(solver);
contactSolverIterate(solver);
contactSolverIterate(solver);
}
struct ContactManifold* contactSolverPeekContact(struct ContactSolver* solver, struct CollisionObject* shapeA, struct CollisionObject* shapeB) {
struct ContactManifold* contactSolverGetContactManifold(struct ContactSolver* solver, struct CollisionObject* shapeA, struct CollisionObject* shapeB) {
struct ContactManifold* curr = solver->activeContacts;
while (curr) {
@ -350,100 +412,5 @@ struct ContactManifold* contactSolverPeekContact(struct ContactSolver* solver, s
solver->activeContacts = result;
}
return result;
}
void contactSolverRemoveContact(struct ContactSolver* solver, struct ContactManifold* toRemove) {
struct ContactManifold* curr = solver->activeContacts;
struct ContactManifold* prev = NULL;
while (curr) {
if (curr == toRemove) {
break;
}
prev = curr;
curr = curr->next;
}
if (!curr) {
return;
}
if (prev) {
prev->next = curr->next;
} else {
solver->activeContacts = curr->next;
}
curr->next = solver->unusedContacts;
solver->unusedContacts = curr;
}
struct ContactPoint* contactSolverGetContact(struct ContactManifold* contact, int id) {
int i;
for (i = 0; i < contact->contactCount; ++i) {
if (contact->contacts[i].id == id) {
return &contact->contacts[i];
}
}
if (i == MAX_CONTACTS_PER_MANIFOLD) {
return NULL;
}
struct ContactPoint* result = &contact->contacts[i];
result->normalImpulse = 0.0f;
result->tangentImpulse[0] = 0.0f;
result->tangentImpulse[1] = 0.0f;
result->id = id;
++contact->contactCount;
return result;
}
int contactSolverAssign(struct ContactManifold* into, struct ContactManifold* from, int filterPortalContacts) {
for (int sourceIndex = 0; sourceIndex < from->contactCount; ++sourceIndex) {
int targetIndex;
struct ContactPoint* sourceContact = &from->contacts[sourceIndex];
for (targetIndex = 0; targetIndex < into->contactCount; ++targetIndex) {
struct ContactPoint* targetContact = &into->contacts[targetIndex];
if (sourceContact->id == targetContact->id) {
sourceContact->normalImpulse = targetContact->normalImpulse;
// TODO reproject tangents
sourceContact->tangentImpulse[0] = targetContact->tangentImpulse[0];
sourceContact->tangentImpulse[1] = targetContact->tangentImpulse[1];
break;
}
}
}
int copiedCount = 0;
int result = 0;
for (int sourceIndex = 0; sourceIndex < from->contactCount; ++sourceIndex) {
if (filterPortalContacts && collisionSceneIsTouchingPortal(&from->contacts[sourceIndex].ra, &from->normal)) {
result = 1;
continue;
}
into->contacts[copiedCount] = from->contacts[sourceIndex];
++copiedCount;
}
into->contactCount = copiedCount;
into->didContactLastFrame = copiedCount > 0;
into->tangentVectors[0] = from->tangentVectors[0];
into->tangentVectors[1] = from->tangentVectors[1];
into->normal = from->normal;
into->restitution = from->restitution;
into->friction = from->friction;
return result;
}

20
src/physics/contact_solver.h
View file

@ -14,8 +14,10 @@ struct VelocityState
struct ContactPoint
{
int id;
struct Vector3 ra; // Vector from C.O.M to contact position
struct Vector3 rb; // Vector from C.O.M to contact position
struct Vector3 contactALocal; // Vector from C.O.M to contact position
struct Vector3 contactBLocal; // Vector from C.O.M to contact position
struct Vector3 contactAWorld; // Vector from C.O.M to contact position
struct Vector3 contactBWorld; // Vector from C.O.M to contact position
float penetration; // Depth of penetration from collision
float normalImpulse; // Accumulated normal impulse
float tangentImpulse[ 2 ]; // Accumulated friction impulse
@ -24,15 +26,13 @@ struct ContactPoint
float tangentMass[ 2 ]; // Tangent constraint mass
};
#define MAX_CONTACTS_PER_MANIFOLD 8
#define MAX_CONTACTS_PER_MANIFOLD 4
#define NEGATIVE_PENETRATION_BIAS 0.00001f
struct ContactManifold
{
struct ContactManifold {
struct ContactPoint contacts[ MAX_CONTACTS_PER_MANIFOLD ];
short contactCount;
short didContactLastFrame;
struct Vector3 tangentVectors[ 2 ]; // Tangent vectors
struct Vector3 normal; // From A to B
float restitution;
@ -58,13 +58,9 @@ void contactSolverInit(struct ContactSolver* contactSolver);
void contactSolverSolve(struct ContactSolver* solver);
struct ContactManifold* contactSolverPeekContact(struct ContactSolver* solver, struct CollisionObject* shapeA, struct CollisionObject* shapeB);
struct ContactManifold* contactSolverGetContactManifold(struct ContactSolver* solver, struct CollisionObject* shapeA, struct CollisionObject* shapeB);
void contactSolverRemoveContact(struct ContactSolver* solver, struct ContactManifold* toRemove);
struct ContactPoint* contactSolverGetContact(struct ContactManifold* contact, int id);
int contactSolverAssign(struct ContactManifold* into, struct ContactManifold* from, int filterPortalContacts);
void contactSolverRemoveUnusedContacts(struct ContactSolver* contactSolver);
#endif

6
src/physics/debug_renderer.c
View file

@ -48,10 +48,10 @@ void contactConstraintStateDebugDraw(struct ContactManifold* constraintState, st
mat[2][2] = constraintState->tangentVectors[1].z;
mat[2][3] = 0.0f;
struct Vector3 pos = contact->ra;
struct Vector3 pos = contact->contactBLocal;
if (constraintState->shapeA->body) {
vector3Add(&pos, &constraintState->shapeB->body->transform.position, &pos);
if (constraintState->shapeB->body) {
transformPoint(&constraintState->shapeB->body->transform, &pos, &pos);
}
mat[3][0] = pos.x * SCENE_SCALE;

4
src/physics/epa.c
View file

@ -376,7 +376,7 @@ void epaCalculateContact(struct ExpandingSimplex* simplex, struct SimplexTriangl
&result->contactA
);
vector3AddScaled(&result->contactA, &result->normal, -result->penetration, &result->contactB);
vector3AddScaled(&result->contactA, &result->normal, result->penetration, &result->contactB);
result->id = simplex->ids[closestFace->indexData.indices[0]] & simplex->ids[closestFace->indexData.indices[1]] & simplex->ids[closestFace->indexData.indices[2]];
}
@ -417,7 +417,7 @@ void epaSolve(struct Simplex* startingSimplex, void* objectA, MinkowsiSum object
if (closestFace) {
result->normal = closestFace->normal;
result->penetration = projection;
result->penetration = -projection;
epaCalculateContact(simplex, closestFace, result);
}

2
src/scene/cube.c
View file

@ -63,7 +63,7 @@ void cubeRender(void* data, struct RenderScene* renderScene) {
Mtx* matrix = renderStateRequestMatrices(renderScene->renderState, 1);
transformToMatrixL(&cube->rigidBody.transform, matrix, SCENE_SCALE);
renderSceneAdd(renderScene, cube_gfx, matrix, (cube->rigidBody.flags & RigidBodyDebugFlag) ? button_material_index : cube_material_index, &cube->rigidBody.transform.position);
renderSceneAdd(renderScene, cube_gfx, matrix, cube_material_index, &cube->rigidBody.transform.position);
}
void cubeInit(struct Cube* cube) {

4
src/scene/scene.c
View file

@ -45,7 +45,7 @@ void sceneInit(struct Scene* scene) {
cubeInit(&scene->cubes[i]);
scene->cubes[i].rigidBody.transform.position.x = 0.0f;
scene->cubes[i].rigidBody.transform.position.y = 1.0f;
scene->cubes[i].rigidBody.transform.position.y = 0.1f;
scene->cubes[i].rigidBody.transform.position.z = 6.0f + i;
scene->cubes[i].rigidBody.currentRoom = 1;
@ -176,7 +176,7 @@ void sceneRender(struct Scene* scene, struct RenderState* renderState, struct Gr
sceneRenderPerformanceMetrics(scene, renderState, task);
// contactSolverDebugDraw(&gContactSolver, renderState);
contactSolverDebugDraw(&gContactSolver, renderState);
}
void sceneCheckPortals(struct Scene* scene) {