Start work on collision cylinder

README.md

@@ -57,7 +57,6 @@ where `/home/james/Blender/blender-2.93.1-linux-x64` is the folder where blender
 
 ## Current TODO list
 
-refactor dynamic rendering
 cylinder collider type
 cube dispenser
 logic and trigger wiring

src/physics/collision_cylinder.c

@@ -0,0 +1,108 @@
+#include "collision_cylinder.h"
+
+#include "../math/mathf.h"
+#include "contact_solver.h"
+#include "collision_quad.h"
+
+int _collisionCylinderParallel(struct CollisionCylinder* cylinder, struct Transform* cylinderTransform, struct Vector3* centerAxis, struct Vector3* crossAxis, float normalDotProduct, struct CollisionQuad* quad, struct ContactConstraintState* output) {
+    struct Vector3 edgeEndpoint;
+    vector3AddScaled(&cylinderTransform->position, centerAxis, normalDotProduct > 0.0f ? -cylinder->halfHeight : cylinder->halfHeight, &edgeEndpoint);
+    vector3Add(&edgeEndpoint, crossAxis, &edgeEndpoint);
+
+    float edgeDistance = planePointDistance(&quad->plane, &edgeEndpoint);
+
+    if (edgeDistance > NEGATIVE_PENETRATION_BIAS) {
+        return 0;
+    }
+
+    int edgesToCheck = collisionQuadDetermineEdges(&edgeEndpoint, quad);
+
+    output->contactCount = 0;
+
+    if (!edgesToCheck) {
+        collisionQuadInitializeNormalContact(quad, output);
+
+        struct ContactState* contact = &output->contacts[output->contactCount];
+
+        ++output->contactCount;
+
+        vector3Sub(&edgeEndpoint, &cylinderTransform->position, &contact->rb);
+        vector3AddScaled(&edgeEndpoint, &quad->plane.normal, -edgeDistance, &contact->ra);
+
+        contact->id = 0;
+        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;
+    }
+
+    vector3AddScaled(&edgeEndpoint, centerAxis,  (normalDotProduct > 0.0f ? 2.0f : -2.0f) * cylinder->halfHeight, &edgeEndpoint);
+    edgeDistance = planePointDistance(&quad->plane, &edgeEndpoint);
+
+    if (edgeDistance > NEGATIVE_PENETRATION_BIAS) {
+        return edgesToCheck;
+    }
+
+    int otherEdgesToCheck = collisionQuadDetermineEdges(&edgeEndpoint, quad);
+
+    if (!otherEdgesToCheck) {
+        if (output->contactCount == 0) {
+            collisionQuadInitializeNormalContact(quad, output);
+        }
+        
+        struct ContactState* contact = &output->contacts[output->contactCount];
+
+        ++output->contactCount;
+
+        vector3Sub(&edgeEndpoint, &cylinderTransform->position, &contact->rb);
+        vector3AddScaled(&edgeEndpoint, &quad->plane.normal, -edgeDistance, &contact->ra);
+
+        contact->id = 1;
+        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 collisionCylinderCollideQuad(void* data, struct Transform* cylinderTransform, struct CollisionQuad* quad, struct ContactConstraintState* 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);
+
+    float magSqrd = vector3MagSqrd(&capCenterTowardsPlane);
+
+    int edgesToCheck;
+
+    struct CollisionCylinder* cylinder = (struct CollisionCylinder*)data;
+
+    if (fabsf(magSqrd) > 0.7f) {
+        // TODO treat as upright
+        edgesToCheck = 0;
+    } else {
+        vector3Scale(&capCenterTowardsPlane, &capCenterTowardsPlane, 1.0f / sqrtf(magSqrd));
+        edgesToCheck = _collisionCylinderParallel(cylinder, cylinderTransform, &centerAxis, &capCenterTowardsPlane, normalDotProduct, quad, output);
+    }
+
+    if (!edgesToCheck) {
+        return output->contactCount > 0;
+    }
+
+    // TODO check edges and points of quad
+
+    return 0;
+}

src/physics/collision_cylinder.h

@@ -0,0 +1,14 @@
+#ifndef __COLLISION_CYLINDER_H__
+#define __COLLISION_CYLINDER_H__
+
+#include "collision_quad.h"
+
+struct CollisionCylinder {
+    float radius;
+    float halfHeight;
+};
+
+
+int collisionCylinderCollideQuad(void* data, struct Transform* cylinderTransform, struct CollisionQuad* quad, struct ContactConstraintState* output);
+
+#endif

src/physics/collision_quad.c

@@ -8,6 +8,15 @@
 
 #define POINT_NO_OVERLAP    -1
 
+void collisionQuadInitializeNormalContact(struct CollisionQuad* quad, struct ContactConstraintState* output) {
+    output->normal = quad->plane.normal;
+    output->tangentVectors[0] = quad->edgeA;
+    output->tangentVectors[1] = quad->edgeB;
+
+    output->restitution = 0.0f;
+    output->friction = 1.0f;
+}
+
 /**
  *        -------
  *        |  3  |
@@ -351,7 +360,7 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C
 
     output->contactCount = 0;
     output->normal = quad->plane.normal;
-    // TODO actually calculate tangent 
+    // TODO handle non orthogonal edges
     output->tangentVectors[0] = quad->edgeA;
     output->tangentVectors[1] = quad->edgeB;
 

src/physics/collision_quad.h

@@ -19,6 +19,7 @@ struct CollisionQuad {
     u8 enabledEdges;
 };
 
+void collisionQuadInitializeNormalContact(struct CollisionQuad* quad, struct ContactConstraintState* output);
 int collisionQuadDetermineEdges(struct Vector3* worldPoint, struct CollisionQuad* quad);
 int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct CollisionQuad* quad, struct ContactConstraintState* output);