diff --git a/src/graphics/debug_render.c b/src/graphics/debug_render.c new file mode 100644 index 0000000..17ac619 --- /dev/null +++ b/src/graphics/debug_render.c @@ -0,0 +1,46 @@ +#include "debug_render.h" + +#include + +#include "defs.h" +#include "../math/vector3.h" +#include "../math/matrix.h" + +Vtx vtx_quad[] = { + {{{0, 0, 0}, 0, {0, 0}, {200, 0, 0, 255}}}, + {{{SCENE_SCALE, 0, 0}, 0, {0, 0}, {200, 0, 0, 255}}}, + {{{SCENE_SCALE, SCENE_SCALE, 0}, 0, {0, 0}, {200, 0, 0, 255}}}, + {{{0, SCENE_SCALE, 0}, 0, {0, 0}, {200, 0, 0, 255}}}, +}; + +#define SOLID_SHADE_COLOR 0, 0, 0, SHADE, 0, 0, 0, SHADE + +Gfx mat_quad[] = { + gsDPSetRenderMode(G_RM_AA_ZB_OPA_SURF, G_RM_AA_ZB_OPA_SURF2), + gsSPGeometryMode(G_LIGHTING | G_CULL_BOTH, G_ZBUFFER | G_SHADE), + gsDPSetCombineMode(SOLID_SHADE_COLOR, SOLID_SHADE_COLOR), + gsSPEndDisplayList(), +}; + +Gfx gfx_quad[] = { + gsSPVertex(vtx_quad, 4, 0), + gsSP2Triangles(0, 1, 2, 0, 0, 2, 3, 0), + gsSPEndDisplayList(), +}; + +void debugRenderQuad(struct Vector3* origin, struct Vector3* edgeA, struct Vector3* edgeB, float edgeLengthA, float edgeLengthB, struct RenderState* renderState) { + Mtx* mtx = renderStateRequestMatrices(renderState, 1); + + struct Vector3 normal; + struct Vector3 x; + struct Vector3 y; + vector3Scale(edgeA, &x, edgeLengthA); + vector3Scale(edgeB, &y, edgeLengthB); + vector3Cross(edgeA, edgeB, &normal); + matrixFromBasisL(mtx, origin, &x, &y, &normal); + + gSPMatrix(renderState->dl++, mtx, G_MTX_MUL | G_MTX_PUSH | G_MTX_MODELVIEW); + gSPDisplayList(renderState->dl++, mat_quad); + gSPDisplayList(renderState->dl++, gfx_quad); + gSPPopMatrix(renderState->dl++, G_MTX_MODELVIEW); +} \ No newline at end of file diff --git a/src/graphics/debug_render.h b/src/graphics/debug_render.h new file mode 100644 index 0000000..8fd84c4 --- /dev/null +++ b/src/graphics/debug_render.h @@ -0,0 +1,9 @@ +#ifndef __DEBUG_RENDER_H__ +#define __DEBUG_RENDER_H__ + +#include "../math/vector3.h" +#include "renderstate.h" + +void debugRenderQuad(struct Vector3* origin, struct Vector3* edgeA, struct Vector3* edgeB, float edgeLengthA, float edgeLengthB, struct RenderState* renderState); + +#endif \ No newline at end of file diff --git a/src/math/matrix.c b/src/math/matrix.c index cc4c7e2..907b3d0 100644 --- a/src/math/matrix.c +++ b/src/math/matrix.c @@ -1,6 +1,7 @@ #include "matrix.h" #include +#include "defs.h" void matrixPerspective(float matrix[4][4], unsigned short* perspNorm, float l, float r, float t, float b, float near, float far) { guMtxIdentF(matrix); @@ -31,4 +32,33 @@ void matrixVec3Mul(float matrix[4][4], struct Vector3* input, struct Vector4* ou output->y = matrix[0][1] * input->x + matrix[1][1] * input->y + matrix[2][1] * input->z + matrix[3][1]; output->z = matrix[0][2] * input->x + matrix[1][2] * input->y + matrix[2][2] * input->z + matrix[3][2]; output->w = matrix[0][3] * input->x + matrix[1][3] * input->y + matrix[2][3] * input->z + matrix[3][3]; +} + +void matrixFromBasis(float matrix[4][4], struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z) { + matrix[0][0] = x->x; + matrix[0][1] = x->y; + matrix[0][2] = x->z; + matrix[0][3] = 0.0f; + + matrix[1][0] = y->x; + matrix[1][1] = y->y; + matrix[1][2] = y->z; + matrix[1][3] = 0.0f; + + matrix[2][0] = z->x; + matrix[2][1] = z->y; + matrix[2][2] = z->z; + matrix[2][3] = 0.0f; + + matrix[3][0] = origin->x * SCENE_SCALE; + matrix[3][1] = origin->y * SCENE_SCALE; + matrix[3][2] = origin->z * SCENE_SCALE; + matrix[3][3] = 1.0f; +} + + +void matrixFromBasisL(Mtx* matrix, struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z) { + float fmtx[4][4]; + matrixFromBasis(fmtx, origin, x, y, z); + guMtxF2L(fmtx, matrix); } \ No newline at end of file diff --git a/src/math/matrix.h b/src/math/matrix.h index 061a21e..9ac91cc 100644 --- a/src/math/matrix.h +++ b/src/math/matrix.h @@ -3,9 +3,13 @@ #include "vector4.h" #include "vector3.h" +#include void matrixPerspective(float matrix[4][4], unsigned short* perspNorm, float l, float r, float top, float b, float near, float far); void matrixVec3Mul(float matrix[4][4], struct Vector3* input, struct Vector4* output); +void matrixFromBasis(float matrix[4][4], struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z); +void matrixFromBasisL(Mtx* matrix, struct Vector3* origin, struct Vector3* x, struct Vector3* y, struct Vector3* z); + #endif \ No newline at end of file diff --git a/src/physics/collision.h b/src/physics/collision.h index 822aa79..9ee18e2 100644 --- a/src/physics/collision.h +++ b/src/physics/collision.h @@ -5,6 +5,7 @@ #include "../math/transform.h" #include "../math/plane.h" #include "contact_solver.h" +#include "collision_quad.h" enum CollisionShapeType { CollisionShapeTypeBox, @@ -22,9 +23,11 @@ struct ColliderTypeData; typedef float (*MomentOfInertiaCalculator)(struct ColliderTypeData* typeData, float mass); typedef int (*CollideWithPlane)(void* data, struct Transform* transform, struct Plane* plane, struct ContactConstraintState* contact); +typedef int (*CollideWithQuad)(void* data, struct Transform* transform, struct CollisionQuad* quad, struct ContactConstraintState* contact); struct ColliderCallbacks { CollideWithPlane collideWithPlane; + CollideWithQuad collideWithQuad; MomentOfInertiaCalculator mofICalculator; }; diff --git a/src/physics/collision_box.c b/src/physics/collision_box.c index db5dbef..2f337bb 100644 --- a/src/physics/collision_box.c +++ b/src/physics/collision_box.c @@ -4,6 +4,7 @@ struct ColliderCallbacks gCollisionBoxCallbacks = { collisionBoxCollidePlane, + collisionBoxCollideQuad, collisionBoxSolidMofI, }; diff --git a/src/physics/collision_object.c b/src/physics/collision_object.c index 49034a0..6a51a47 100644 --- a/src/physics/collision_object.c +++ b/src/physics/collision_object.c @@ -25,6 +25,30 @@ void collisionObjectCollideWithPlane(struct CollisionObject* object, struct Coll return; } + contactSolverAssign(contact, &localContact); + } else if (contact) { + contactSolverRemoveContact(contactSolver, contact); + } +} + +void collisionObjectCollideWithQuad(struct CollisionObject* object, struct CollisionObject* quad, struct ContactSolver* contactSolver) { + CollideWithQuad quadCollider = object->collider->callbacks->collideWithQuad; + + if (!quadCollider) { + return; + } + + struct ContactConstraintState localContact; + localContact.contactCount = 0; + + struct ContactConstraintState* contact = contactSolverPeekContact(contactSolver, quad, object); + + if (quadCollider(object->collider->data, &object->body->transform, quad->collider->data, &localContact)) { + + if (!contact) { + return; + } + contactSolverAssign(contact, &localContact); } else if (contact) { contactSolverRemoveContact(contactSolver, contact); diff --git a/src/physics/collision_object.h b/src/physics/collision_object.h index fe59cb4..af68119 100644 --- a/src/physics/collision_object.h +++ b/src/physics/collision_object.h @@ -12,5 +12,6 @@ struct CollisionObject { void collisionObjectInit(struct CollisionObject* object, struct ColliderTypeData *collider, struct RigidBody* body, float mass); void collisionObjectCollideWithPlane(struct CollisionObject* object, struct CollisionObject* plane, struct ContactSolver* contactSolver); +void collisionObjectCollideWithQuad(struct CollisionObject* object, struct CollisionObject* quad, struct ContactSolver* contactSolver); #endif \ No newline at end of file diff --git a/src/physics/collision_quad.c b/src/physics/collision_quad.c index b9fe993..86d0922 100644 --- a/src/physics/collision_quad.c +++ b/src/physics/collision_quad.c @@ -90,8 +90,8 @@ char secondOtherAxis[] = { 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) +#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)) struct CollisionEdge { struct Vector3 origin; @@ -162,7 +162,7 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra cubeEdge.length = VECTOR3_AS_ARRAY(&box->sideLength)[axis] * 2.0f; int firstAxis = otherAxis[axis]; - int secondAxis = otherAxis[axis]; + int secondAxis = secondOtherAxis[axis]; VECTOR3_AS_ARRAY(&cubeEdge.origin)[axis] = -VECTOR3_AS_ARRAY(&box->sideLength)[axis]; @@ -171,7 +171,7 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra 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))); + int cornerIDMask = (1 << cornerID) | (1 << (cornerID | ID_SEGEMENT_FROM_AXIS(axis, 1))); if ((cornerIDMask & cornerIds) == 0) { continue; } @@ -201,10 +201,11 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra } quatMultVector(&boxTransform->rotation, &output->normal, &output->normal); + output->tangentVectors[0] = edge->direction; + vector3Cross(&output->normal, &edge->direction, &output->tangentVectors[1]); } - // TODO transform contact back into world space - + // rotate from cube space to world space quatMultVector(&boxTransform->rotation, &contact->ra, &contact->ra); quatMultVector(&boxTransform->rotation, &contact->rb, &contact->rb); @@ -213,7 +214,8 @@ void _collisionBoxCollideEdge(struct CollisionBox* box, struct Transform* boxTra return; } - vector3Add(&boxTransform->position, &contact->rb, &contact->rb); + // move edge contact to world space + vector3Add(&boxTransform->position, &contact->ra, &contact->ra); ++output->contactCount; contact->id = 8 + axis + corner * 4; @@ -266,8 +268,8 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C output->contactCount = 0; output->normal = quad->plane.normal; // TODO actually calculate tangent - output->tangentVectors[0] = gRight; - output->tangentVectors[1] = gForward; + output->tangentVectors[0] = quad->edgeA; + output->tangentVectors[1] = quad->edgeB; output->restitution = 0.0f; output->friction = 1.0f; @@ -312,25 +314,25 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C 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); + 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); - edges |= pointEdges; if (pointEdges > 0) { - cornerIds |= (1 << id); + 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); - edges |= pointEdges; if (pointEdges > 0) { - cornerIds |= (1 << id); + edges |= pointEdges; + cornerIds |= (1 << nextId); } edges &= quad->enabledEdges; @@ -346,7 +348,7 @@ int collisionBoxCollideQuad(void* data, struct Transform* boxTransform, struct C quatMultVector(&inverseBoxRotation, &quad->edgeA, &localEdgeA); quatMultVector(&inverseBoxRotation, &quad->edgeB, &localEdgeB); - vector3Sub(&edge.origin, &boxTransform->position, &localOrigin); + vector3Sub(&quad->corner, &boxTransform->position, &localOrigin); quatMultVector(&inverseBoxRotation, &localOrigin, &localOrigin); if (edges & (1 << 0)) { diff --git a/src/scene/cube.c b/src/scene/cube.c index 15cc321..764523c 100644 --- a/src/scene/cube.c +++ b/src/scene/cube.c @@ -2,6 +2,7 @@ #include "../physics/collision_box.h" #include "../models/models.h" #include "defs.h" +#include "../graphics/debug_render.h" struct CollisionBox gCubeCollisionBox = { {0.3165f, 0.3165f, 0.3165f} @@ -22,6 +23,30 @@ struct CollisionObject gFloorObject = { NULL, }; +struct CollisionQuad gFloatingQuad = { + {-1.0f, 0.5f, -1.0f}, + {1.0f, 0.0f, 0.0f}, + 2.0f, + {0.0f, 0.0f, 1.0f}, + 2.0f, + {{0.0f, 1.0f, 0.0}, -0.5f}, + 0xF, +}; + +struct ColliderTypeData gFloatingQuadCollider = { + CollisionShapeTypeQuad, + &gFloatingQuad, + 0.0f, + 1.0f, + NULL, +} ; + + +struct CollisionObject gFloatingQuadObject = { + &gFloatingQuadCollider, + NULL, +}; + struct ColliderTypeData gCubeCollider = { CollisionShapeTypeBox, &gCubeCollisionBox, @@ -36,6 +61,7 @@ void cubeInit(struct Cube* cube) { void cubeUpdate(struct Cube* cube) { collisionObjectCollideWithPlane(&cube->collisionObject, &gFloorObject, &gContactSolver); + collisionObjectCollideWithQuad(&cube->collisionObject, &gFloatingQuadObject, &gContactSolver); contactSolverSolve(&gContactSolver); rigidBodyUpdate(&cube->rigidBody); } @@ -47,4 +73,6 @@ void cubeRender(struct Cube* cube, struct RenderState* renderState) { gSPMatrix(renderState->dl++, matrix, G_MTX_MODELVIEW | G_MTX_PUSH | G_MTX_MUL); gSPDisplayList(renderState->dl++, cube_CubeSimpleBevel_mesh); gSPPopMatrix(renderState->dl++, G_MTX_MODELVIEW); + + debugRenderQuad(&gFloatingQuad.corner, &gFloatingQuad.edgeA, &gFloatingQuad.edgeB, gFloatingQuad.edgeALength, gFloatingQuad.edgeBLength, renderState); } \ No newline at end of file diff --git a/src/scene/scene.c b/src/scene/scene.c index 1834e76..f8cc7c2 100644 --- a/src/scene/scene.c +++ b/src/scene/scene.c @@ -75,14 +75,19 @@ void sceneRender(struct Scene* scene, struct RenderState* renderState, struct Gr sceneRenderWithProperties(scene, &renderProperties, renderState); gDPPipeSync(renderState->dl++); + + gDPSetRenderMode(renderState->dl++, G_RM_OPA_SURF, G_RM_OPA_SURF2); + gSPGeometryMode(renderState->dl++, G_ZBUFFER | G_LIGHTING | G_CULL_BOTH, G_SHADE); + gDPSetCycleType(renderState->dl++, G_CYC_1CYCLE); gDPSetFillColor(renderState->dl++, (GPACK_RGBA5551(0, 0, 0, 1) << 16 | GPACK_RGBA5551(0, 0, 0, 1))); gDPSetCombineMode(renderState->dl++, SOLID_COLOR, SOLID_COLOR); - // gDPSetEnvColor(renderState->dl++, 32, 32, 32, 255); - // gSPTextureRectangle(renderState->dl++, 32 << 2, 32 << 2, (32 + 256) << 2, (32 + 16) << 2, 0, 0, 0, 1, 1); + gDPSetEnvColor(renderState->dl++, 32, 32, 32, 255); + gSPTextureRectangle(renderState->dl++, 32 << 2, 32 << 2, (32 + 256) << 2, (32 + 16) << 2, 0, 0, 0, 1, 1); gDPPipeSync(renderState->dl++); gDPSetEnvColor(renderState->dl++, 32, 255, 32, 255); gSPTextureRectangle(renderState->dl++, 33 << 2, 33 << 2, (32 + 254 * scene->cpuTime / scene->lastFrameTime) << 2, (32 + 14) << 2, 0, 0, 0, 1, 1); + gDPPipeSync(renderState->dl++); contactSolverDebugDraw(&gContactSolver, renderState); }