Work on collision quad
This commit is contained in:
James Lambert
parent
1e2b8b82a8
commit
619eadb643
|
|
@ -2,10 +2,6 @@
|
|||
|
||||
#include <math.h>
|
||||
|
||||
#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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -1,2 +1,384 @@
|
|||
|
||||
#include "collision_quad.h"
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -7,9 +7,9 @@
|
|||
|
||||
#include <string.h>
|
||||
|
||||
#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
|
||||
|
||||
|
|
|
|||
|
|
@ -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 ];
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue