Untitled
Loading...
Searching...
No Matches
physics.c
Go to the documentation of this file.
1
6
7#include "core/camera.h"
8#include "core/engine_types.h"
9#include "utilities/types.h"
10#include "load/init.h"
11#include <float.h>
12#include <math.h>
13#include <stdio.h>
14#include "utilities/mathf.h"
16#include "core/mesh.h"
17#include "physics/GJK.h"
18#include "utilities/utilities.h"
19#include "render/render.h"
20
21
22
23// // Assume you already ran your Hull code to find the Hull's reference face and normal.
24// // hull_normal = the collision normal pointing FROM the hull TO the capsule.
25// // hull_plane_dist = the dot product of a point on the hull face and the normal.
26
32static void
34 (
36 float dt
37 );
38
43static int generate_capsule_hull_manifold(CapsuleData* cap, mat4 cap_transform, vec3 hull_normal, float hull_plane_dist, Manifold * manifold, PhysicsComponent * pcA, PhysicsComponent * pcB);
44
54static void clip_manifold(Manifold * manifold, CollisionPoints * p, CollisionData a_cd, mat4 modelA, CollisionData b_cd, mat4 modelB);
55
62static Manifold * find_manifold_for_pair(int mn0, int mn1);
63
72static Manifold * create_new_manifold(PhysicsComponent * a, int mn0, PhysicsComponent * b, int mn1);
73
89static bool PC_process_collision_node(MeshNode * mn, PhysicsComponent * pc, Mesh * m0, MeshNode * mn0, BVHNode * node, CollisionData cd, mat4 mat, mat4 mat0, mat4 mat_inv, mat4 mat0_inv, Manifold * manifold);
90
107static bool PC_traverse_collision_bvh(MeshNode * mn, PhysicsComponent * pc, Mesh * m0, MeshNode * mn0, BVHNode * nodes, CollisionData cd, mat4 mat, mat4 mat0, mat4 mat_inv, mat4 mat0_inv, AABB tr_aabb, Manifold * manifold);
108
119static void PC_traverse_scene_bvh(MeshNode * mn, PhysicsComponent * pc, CollisionData cd, SceneData * sd, mat4 mat, mat4 mat_inv, AABB aabb);
120
127
134
140static void PC_find_collision(MeshNode * mn, PhysicsComponent * pc);
141
147static void
149 (
150 PhysicsComponent * pc,
151 float dt
152 )
153
154{
155
156 static const float MAX_SPEED = 0.02f;
157 static const float ACCEL = 0.01f;
158 static const float GRAVITY = 9.8f;
159 float accel_step = ACCEL * dt;
160
161 pc->vel.z -= GRAVITY * dt;
162
163 float nlen = sqrtf(pc->vel.x * pc->vel.x + pc->vel.y * pc->vel.y + pc->vel.z * pc->vel.z);
164
165 pc->vel = vec3_scale(pc->vel, 0.999f);
167
168
169}
170
171
172static int generate_capsule_hull_manifold(CapsuleData* cap, mat4 cap_transform, vec3 hull_normal, float hull_plane_dist, Manifold * manifold, PhysicsComponent * pcA, PhysicsComponent * pcB)
173{
174 int contact_count = 0;
175
176 vec3 world_A = mat4_mul_point(cap_transform, cap->a);
177 vec3 world_B = mat4_mul_point(cap_transform, cap->b);
178 // vec3 world_A = cap->a;
179 // vec3 world_B = cap->b;
180
181 float dist_A = vec3_dot(world_A, hull_normal) - hull_plane_dist;
182 float dist_B = vec3_dot(world_B, hull_normal) - hull_plane_dist;
183
184 float pen_A = dist_A - cap->radius;
185 // float pen_A = -hull_plane_dist;
186 float pen_B = dist_B - cap->radius;
187
188 if (pen_A <= 0.0f) {
189 manifold->contacts[contact_count].a = vec3_sub(cap->a, vec3_scale(hull_normal, hull_plane_dist));
190 manifold->contacts[contact_count].pcA = pcA;
191 manifold->contacts[contact_count].pcB = pcB;
192 manifold->contacts[contact_count].b = cap->a;
193 manifold->contacts[contact_count].d = -pen_A;
194 manifold->contacts[contact_count].normal = hull_normal;
195 contact_count++;
196 }
197
198 if (pen_B <= 0.0f) {
199 manifold->contacts[contact_count].a = vec3_sub(cap->b, vec3_scale(hull_normal, hull_plane_dist));
200 manifold->contacts[contact_count].pcA = pcA;
201 manifold->contacts[contact_count].pcB = pcB;
202
203 manifold->contacts[contact_count].b = cap->b;
204 manifold->contacts[contact_count].d = -pen_B;
205 manifold->contacts[contact_count].normal= hull_normal;
206 contact_count++;
207 }
208 manifold->count = contact_count;
209
210 return contact_count; // Will be 0, 1, or 2
211}
212
213static void clip_manifold(Manifold * manifold, CollisionPoints * p, CollisionData a_cd, mat4 modelA, CollisionData b_cd, mat4 modelB)
214{
215
216
217 vec3 * vA = a_cd.data.hull.pts;
218 size_t vAcnt = a_cd.data.hull.count;
219 HullFace * fA = a_cd.data.hull.mfs;
220 size_t fAcnt = a_cd.data.hull.face_count;
221 vec3 * vB = b_cd.data.hull.pts;
222 size_t vBcnt = b_cd.data.hull.count;
223 HullFace * fB = b_cd.data.hull.mfs;
224 size_t fBcnt = b_cd.data.hull.face_count;
225 // if (manifold->processed) return;
226 manifold->processed = true;
227 vec3 n = vec3_scale(p->normal, -1);
228
229 // find furthest along collision normal
230 float maximum_dotA = -INFINITY;
231 size_t largest_indexA = 0;
232 for (int i = 0; i < vAcnt; i++)
233 {
234 float dot = vec3_dot(mat4_mul_point(modelA, vA[i]), n);
235 if (dot > maximum_dotA)
236 {
237 maximum_dotA = dot;
238 largest_indexA = i;
239 }
240 }
241
242 float maximum_face_dotA = -INFINITY;
243 size_t largest_face_indexA = 0;
244 // find most parallel face that includes that point
245 for (int i = 0; i < fAcnt; i++)
246 {
247 HullFace * f = &fA[i];
248 bool found = false;
249 for (int j = 0; j < f->count; j++)
250 {
251 if (f->indices[j] == largest_indexA)
252 {
253
254 found = true;
255 break;
256 }
257 }
258 if (!found) continue;
259
260 float dot = vec3_dot(mat4_mul_direction(modelA, f->normal), n);
261 if (dot > maximum_face_dotA)
262 {
263 maximum_face_dotA = dot;
264 largest_face_indexA = i;
265 }
266 }
267
268 float maximum_dotB = -INFINITY;
269 size_t largest_indexB = 0;
270 for (int i = 0; i < vBcnt; i++)
271 {
272 float dot = vec3_dot(mat4_mul_point(modelB, vB[i]), vec3_scale(n, -1));
273 if (dot > maximum_dotB)
274 {
275 maximum_dotB = dot;
276 largest_indexB = i;
277 }
278 }
279
280 float maximum_face_dotB = -INFINITY;
281 size_t largest_face_indexB = 0;
282 // find most parallel face that includes that point
283 for (int i = 0; i < fBcnt; i++)
284 {
285 HullFace * f = &fB[i];
286 bool found = false;
287 for (int j = 0; j < f->count; j++)
288 {
289 if (f->indices[j] == largest_indexB)
290 {
291
292 found = true;
293 break;
294 }
295 }
296 if (!found) continue;
297
298 float dot = vec3_dot(mat4_mul_direction(modelB, f->normal), vec3_scale(n, -1));
299 if (dot > maximum_face_dotB)
300 {
301 maximum_face_dotB = dot;
302 largest_face_indexB = i;
303 }
304 }
305
306 HullFace * reference_face = &fA[largest_face_indexA];
307 vec3 * reference_points = vA;
308 HullFace * incident_face = &fB[largest_face_indexB];
309 int reference_idx = largest_face_indexA;
310 int incident_idx = largest_face_indexB;
311 vec3 * incident_points = vB;
312 mat4 * reference_mat = &modelA;
313 mat4 * incident_mat = &modelB;
314 bool flipped = false;
315 if (vec3_dot(mat4_mul_direction(modelA, fA[largest_face_indexA].normal), n) < vec3_dot(mat4_mul_direction(modelB, fB[largest_face_indexB].normal), n))
316 {
317 flipped = true;
318 HullFace * temp = reference_face;
319 vec3 * tempP = reference_points;
320 mat4 * tempMat = reference_mat;
321 reference_face = incident_face;
322 reference_points = incident_points;
323 reference_mat = incident_mat;
324 incident_face = temp;
325 incident_points = tempP;
326 incident_mat = tempMat;
327 incident_idx = largest_face_indexA;
328 reference_idx = largest_face_indexB;
329 }
330 // for (int i = 0; i < incident_face->count; i++)
331 // {
332
333 // DEBUG_LINES[DEBUG_LINE_COUNT].pos = mat4_mul_point(*incident_mat, incident_points[incident_face->indices[i]]);
334 // DEBUG_LINES[DEBUG_LINE_COUNT].color = (vec3){0, 1, 0};
335 // DEBUG_LINE_COUNT++;
336 // DEBUG_LINES[DEBUG_LINE_COUNT].pos = mat4_mul_point(*incident_mat, incident_points[incident_face->indices[(i+1) % incident_face->count]]);
337 // DEBUG_LINES[DEBUG_LINE_COUNT].color = (vec3){0, 1, 0};
338 // DEBUG_LINE_COUNT++;
339 // }
340 // for (int i = 0; i < reference_face->count; i++)
341 // {
342
343 // DEBUG_LINES[DEBUG_LINE_COUNT].pos = mat4_mul_point(*reference_mat, reference_points[reference_face->indices[i]]);
344 // DEBUG_LINES[DEBUG_LINE_COUNT].color = (vec3){0, 1, 1};
345 // DEBUG_LINE_COUNT++;
346 // DEBUG_LINES[DEBUG_LINE_COUNT].pos = mat4_mul_point(*reference_mat, reference_points[reference_face->indices[(i+1) % reference_face->count]]);
347 // DEBUG_LINES[DEBUG_LINE_COUNT].color = (vec3){0, 1, 1};
348 // DEBUG_LINE_COUNT++;
349 // }
350
351 const int MAX_CLIP_PT = 64;
352 ClipVertex ipA[MAX_CLIP_PT];
353 for (int i = 0; i < incident_face->count; i++)
354 {
355 ipA[i].p = mat4_mul_point(*incident_mat, incident_points[incident_face->indices[i]]);
356 ipA[i].edge0 = i;
357 ipA[i].edge1 = (i+1) % incident_face->count;
358 }
359 ClipVertex ipB[MAX_CLIP_PT];
360 int incident_count = incident_face->count;
361 ClipVertex * ping = ipA;
362 ClipVertex * pong = ipB;
363
364 vec3 reference_origin = mat4_mul_point(*reference_mat, reference_points[reference_face->indices[0]]);
365 vec3 reference_normal = mat4_mul_direction(*reference_mat, reference_face->normal);
366 // clip against each plane
367
368 for (int i = 0; i < reference_face->count; i++)
369 {
370 vec3 A = mat4_mul_point(*reference_mat, reference_points[reference_face->indices[i]]);
371 vec3 B = mat4_mul_point(*reference_mat, reference_points[reference_face->indices[(i+1)%reference_face->count]]);
372 vec3 edge = vec3_sub(B, A);
373 // vec3 plane_normal = vec3_cross(edge, reference_face->normal);
374 vec3 plane_normal = vec3_cross(reference_normal, edge);
375
376 incident_count = clip_pts_against_plane(ping, incident_count, pong, A, plane_normal);
377 ClipVertex * temp = ping;
378 ping = pong;
379 pong = temp;
380 }
381 if (incident_count >= MAX_CLIP_PT)
382 {
383 printf("INCIDENT CLIP OOB\n");
384 return;
385 }
386
387
388 const int MAX_CLIP_AND_CONTACT = 68;
389 float depths[MAX_CLIP_PT];
390 CollisionPoints clip_contact[MAX_CLIP_AND_CONTACT];
391 vec3 temp_arr[MAX_CLIP_AND_CONTACT];
392 size_t clip_contact_cnt = 0;
393
394 mat4 inverse_incident = mat4_inverse(*incident_mat);
395 mat4 inverse_a;
396 m4_inverse((float *)&inverse_a, (float*)&modelA);
397 mat4 inverse_reference = mat4_inverse(*reference_mat);
398 // mat4 inverse_b;
399 mat4 inverse_b;
400 m4_inverse((float *)&inverse_b, (float*)&modelB);
401
402
403 for (int i = 0; i < incident_count; i++)
404 {
405 if (i >= incident_count) break;
406
407 float depth = vec3_dot(vec3_sub(ping[i].p, reference_origin), reference_normal);
408 if (depth > 0.01f)
409 {
410 for (int j = i; j < incident_count-1; j++)
411 {
412 ping[j] = ping[j+1];
413 }
414 incident_count--;
415 i--;
416 }
417 else
418 {
419 if (!flipped)
420 {
421 clip_contact[clip_contact_cnt].b = mat4_mul_point(inverse_b, ping[i].p);
422 clip_contact[clip_contact_cnt].a = mat4_mul_point(inverse_a, vec3_sub(ping[i].p, vec3_scale(reference_normal, depth)));
423 clip_contact[clip_contact_cnt].reference_face = reference_idx;
424 clip_contact[clip_contact_cnt].incident_face = incident_idx;
425 clip_contact[clip_contact_cnt].edge_a = ping[i].edge0;
426 clip_contact[clip_contact_cnt].edge_b = ping[i].edge1;
427
428 clip_contact[clip_contact_cnt].normal = p->normal;
429 clip_contact[clip_contact_cnt].d = -depth;
430 clip_contact[clip_contact_cnt].pcA = p->pcA;
431 clip_contact[clip_contact_cnt].pcB = p->pcB;
432 clip_contact[clip_contact_cnt].mnA = p->mnA;
433 clip_contact[clip_contact_cnt].mnB = p->mnB;
434 clip_contact[clip_contact_cnt].normal_impulse = 0;
435 clip_contact[clip_contact_cnt].tangent_impulse = 0;
436 clip_contact[clip_contact_cnt].bias_impulse = 0;
437 clip_contact[clip_contact_cnt].tangent = (vec3){0,0,0};
438 clip_contact_cnt++;
439
440 }
441 else
442 {
443
444 clip_contact[clip_contact_cnt].a = mat4_mul_point(inverse_a, ping[i].p);
445 clip_contact[clip_contact_cnt].b = mat4_mul_point(inverse_b, vec3_sub(ping[i].p, vec3_scale(reference_normal, depth)));
446 clip_contact[clip_contact_cnt].normal = p->normal;
447 clip_contact[clip_contact_cnt].reference_face = reference_idx;
448 clip_contact[clip_contact_cnt].incident_face = incident_idx;
449 clip_contact[clip_contact_cnt].edge_a = ping[i].edge0;
450 clip_contact[clip_contact_cnt].edge_b = ping[i].edge1;
451 clip_contact[clip_contact_cnt].d = -depth;
452 clip_contact[clip_contact_cnt].pcA = p->pcA;
453 clip_contact[clip_contact_cnt].pcB = p->pcB;
454 clip_contact[clip_contact_cnt].mnA = p->mnA;
455 clip_contact[clip_contact_cnt].mnB = p->mnB;
456 clip_contact[clip_contact_cnt].normal_impulse = 0;
457 clip_contact[clip_contact_cnt].tangent_impulse = 0;
458 clip_contact[clip_contact_cnt].bias_impulse = 0;
459 clip_contact[clip_contact_cnt].tangent = (vec3){0,0,0};
460 clip_contact_cnt++;
461 }
462
463 }
464 }
465
466 for (int i = 0; i < clip_contact_cnt; i++)
467 {
468
469 DEBUG_LINES[DEBUG_LINE_COUNT].pos = mat4_mul_point(modelA, clip_contact[i].a);
472 DEBUG_LINES[DEBUG_LINE_COUNT].pos = mat4_mul_point(modelA, clip_contact[(i+1)%clip_contact_cnt].a);
475 }
476 bool not_found = false;
477
478 for (int i = 0; i < manifold->count; i++)
479 {
480 bool found = false;
481
482 for (int j = 0; j < clip_contact_cnt; j++)
483 {
484 if (manifold->contacts[i].incident_face == clip_contact[j].incident_face &&
485 manifold->contacts[i].reference_face == clip_contact[j].reference_face &&
486 manifold->contacts[i].edge_a == clip_contact[j].edge_a &&
487 manifold->contacts[i].edge_b == clip_contact[j].edge_b)
488 {
489 // clip_contact[j] = manifold->contacts[i];
490
491 // if (clip_contact[j].mnA == manifold->contacts[i].mnA && clip_contact[j].mnB == manifold->contacts[i].mnB)
492 // {
493 if ((vec3_equal(clip_contact[j].a, manifold->contacts[i].a, 0.01f) && vec3_equal(clip_contact[j].b, manifold->contacts[i].b, 0.01f)) || (vec3_equal(clip_contact[j].a, manifold->contacts[i].b, 0.01f) && vec3_equal(clip_contact[j].b, manifold->contacts[i].a, 0.01f)))
494 {
495
496 clip_contact[j].normal_impulse = manifold->contacts[i].normal_impulse;
497 clip_contact[j].tangent_impulse = manifold->contacts[i].tangent_impulse;
498 clip_contact[j].bias_impulse = manifold->contacts[i].bias_impulse;
499 clip_contact[j].tangent = manifold->contacts[i].tangent;
500 found = true;
501 break;
502 }
503
504 }
505
506 }
507
508 }
509
510
511 size_t deepest_index = 0;
512 if (clip_contact_cnt >= 1)
513 {
514 float deepest = -INFINITY;
515 for (int i = 0; i < clip_contact_cnt; i++)
516 {
517 if (clip_contact[i].d > deepest)
518 {
519 deepest_index = i;
520 deepest = clip_contact[i].d;
521 }
522 }
523
524
525 manifold->contacts[0] = clip_contact[deepest_index];
526 }
527 size_t furthest_index_pt = 0;
528 if (clip_contact_cnt >= 2)
529 {
530 float furthest = -INFINITY;
531
532 for (int i = 0; i < clip_contact_cnt; i++)
533 {
534 if (deepest_index == i) continue;
535 vec3 d = vec3_sub(manifold->contacts[0].a, clip_contact[i].a);
536 float dist = vec3_dot(d, d);
537 if (dist > furthest)
538 {
539 furthest_index_pt = i;
540 furthest = dist;
541 }
542 }
543 manifold->contacts[1] = clip_contact[furthest_index_pt];
544 }
545 size_t furthest_index_seg = 0;
546 if (clip_contact_cnt >= 3)
547 {
548
549 float furthest = -INFINITY;
550
551 for (int i = 0; i < clip_contact_cnt; i++)
552 {
553 if (deepest_index == i || furthest_index_pt == i) continue;
554 vec3 pt = closest_point_segment(clip_contact[i].a, manifold->contacts[0].a, manifold->contacts[1].a);
555 vec3 d = vec3_sub(pt, clip_contact[i].a);
556 float dist = vec3_dot(d, d);
557 if (dist > furthest)
558 {
559 furthest_index_seg = i;
560 furthest = dist;
561 }
562 }
563 manifold->contacts[2] = clip_contact[furthest_index_seg];
564 }
565 size_t furthest_index_tri = 0;
566 if (clip_contact_cnt >= 4)
567 {
568
569 float furthest = -INFINITY;
570
571 for (int i = 0; i < clip_contact_cnt; i++)
572 {
573 if (deepest_index == i || furthest_index_pt == i || furthest_index_seg == i) continue;
574 vec3 pt = closest_point_triangle(clip_contact[i].a, manifold->contacts[0].a, manifold->contacts[1].a, manifold->contacts[2].a);
575 vec3 d = vec3_sub(pt, clip_contact[i].a);
576 float dist = vec3_dot(d, d);
577 if (dist > furthest)
578 {
579 furthest_index_tri = i;
580 furthest = dist;
581 }
582 }
583 manifold->contacts[3] = clip_contact[furthest_index_tri];
584 }
585
586
587 manifold->count = MIN(clip_contact_cnt, 4);
588 manifold->processed = true;
589
590}
591
592
593
594static Manifold * find_manifold_for_pair(int mn0, int mn1)
595{
596 for (int i = 0; i < MAX_MANIFOLDS; i++)
597 {
598 if (MANIFOLDS[i].free) continue;
599
600 Manifold * m = &MANIFOLDS[i];
601 if ((m->mnA == mn0 && m->mnB == mn1) || (m->mnB == mn0 && m->mnA == mn1))
602 {
603 return m;
604 }
605 }
606
607 return NULL;
608}
609
610
612{
613
614 for (int i = 0; i < MAX_MANIFOLDS; i++)
615 {
616 if (MANIFOLDS[i].free)
617 {
618 MANIFOLDS[i].a = a;
619 MANIFOLDS[i].b = b;
620 MANIFOLDS[i].mnA = mn0;
621 MANIFOLDS[i].mnB = mn1;
622 MANIFOLDS[i].count = 0;
623 MANIFOLDS[i].free = false;
624 MANIFOLDS[i].processed = false;
625 MANIFOLDS[i].deepest = -INFINITY;
626 if (a) a->at_rest = false;
627 if (b) b->at_rest = false;
628 return &MANIFOLDS[i];
629 }
630 }
631 return NULL;
632}
633
634
635
636
637
638static bool PC_process_collision_node(MeshNode * mn, PhysicsComponent * pc, Mesh * m0, MeshNode * mn0, BVHNode * node, CollisionData cd, mat4 mat, mat4 mat0, mat4 mat_inv, mat4 mat0_inv, Manifold * manifold)
639{
640
641 MeshFace * fs = CAST_VEC(m0->tris, MeshFace);
642 Point * v = CAST_VEC(m0->vertices, Point);
643 size_t * li = m0->bvh_indices;
644 int tris = 0;
645 for (int k = 0; k < node->count; k++)
646 {
647
648 MeshFace * f = &fs[li[k + node->first_index]];
649 vec3 A = v[f->indices[0]].pos;
650 vec3 B = v[f->indices[1]].pos;
651 vec3 C = v[f->indices[2]].pos;
652
653 tris++;
654 vec3 vertices[3];
655 vertices[0] = A;
656 vertices[1] = B;
657 vertices[2] = C;
658 HullFace face;
659 face.count = 3;
660 face.normal = f->normal;
661 face.indices[0] = 0;
662 face.indices[1] = 1;
663 face.indices[2] = 2;
664
665
667 p.collision = false;
668 p.d = -INFINITY;
669 CollisionData cd0;
670 cd0.type = COL_TRIANGLE;
671 cd0.data.hull.pts = vertices;
672 cd0.data.hull.count = 3;
673 cd0.data.hull.mfs = &face;
674 cd0.data.hull.face_count = 1;
675
676 if (GJK(cd, cd0, mat, mat0, mat_inv, mat0_inv, &p, EW[0]))
677 {
678 p.pcA = pc;
679 p.pcB = NULL;
680
681 p.mnA = mn->id;
682 p.mnB = mn0->id;
683 p.normal_impulse = 0;
684 p.tangent_impulse = 0;
685 manifold = find_manifold_for_pair(mn->id, mn0->id);
686 if (!manifold)
687 {
688 manifold = create_new_manifold(pc, mn->id, NULL, mn0->id);
689 }
690 if (manifold->deepest < p.d)
691 {
692
693 if (cd.type != COL_CAPSULE)
694 {
695
696 clip_manifold(manifold, &p, cd, mn->global_matrix, cd0, mn0->global_matrix);
697 }
698 else
699 {
700 // int cap_count = generate_capsule_hull_manifold(&cd.data.capsule, mn->global_matrix, f->normal, p.d, manifold, pc, NULL);
701 // manifold->count = cap_count;
702 manifold->count = 1;
703 manifold->contacts[0] = p;
704 manifold->contacts[0].normal = f->normal;
705
706 }
707 manifold->processed = true;
708 manifold->deepest = p.d;
709 }
710 }
711
712
713 }
714 return false;
715}
716
717static bool PC_traverse_collision_bvh(MeshNode * mn, PhysicsComponent * pc, Mesh * m0, MeshNode * mn0, BVHNode * nodes, CollisionData cd, mat4 mat, mat4 mat0, mat4 mat_inv, mat4 mat0_inv, AABB tr_aabb, Manifold * manifold)
718{
719
720
721
722 int ni = 0;
723
724
725 while (ni >= 0)
726 {
727 BVHNode * node = &nodes[ni];
728 if (!aabbs_intersect(&tr_aabb, &node->aabb))
729 {
730 ni = node->next;
731 }
732 if (node->left == -1 && node->right == -1)
733 {
734 if (PC_process_collision_node(mn, pc, m0, mn0, node, cd, mat, mat0, mat_inv, mat0_inv, manifold))
735 {
736
737 }
738 ni = node->next;
739 }
740 else
741 {
742 ni = node->left;
743 }
744 }
745
746 return false;
747
748}
749
750
751
752static void PC_traverse_scene_bvh(MeshNode * mn, PhysicsComponent * pc, CollisionData cd, SceneData * sd, mat4 mat, mat4 mat_inv, AABB aabb)
753{
754
755
756 Mesh * meshes = CAST_VEC(GLOBALS.meshes, Mesh);
758
759 int ni = 0;
760 BVHNode * nodes = CAST_VEC(sd->bvh_nodes, BVHNode);
761
762 while (ni >= 0)
763 {
764
765 BVHNode * node = &nodes[ni];
766
767 if (!aabbs_intersect(&node->aabb, &aabb))
768 {
769 ni = node->next;
770 continue;
771 }
772 if (node->left == -1 && node->right == -1)
773 {
774
775 for (int i = 0; i < node->count; i++)
776 {
777
778 MeshNode * mn0 = &mns[sd->bvh_indices[node->first_index + i]];
779 mn0 = get_root_node(mns, mn0);
780 if ((mn0->mesh == -1 && mn0->parent != -1) || mn0->free) continue;
781
782 // if (mn_inside_of_root_tree(mns, sd->bvh_indices[node->first_index + i], mn->id))
783 // {
784 // printf("LOCAL: %zu %zu\n", mn0->local_id, mn->local_id);
785 // continue;
786 // }
787 if (mn0->local_id == mn->local_id)
788 {
789 continue;
790 }
791 Manifold * manifold = find_manifold_for_pair(mn->id, mn0->id);
792 if (manifold)
793 {
794 if (manifold->processed)
795 {
796 continue;
797 }
798 }
799
800
801 if (mn0->pc)
802 {
803 switch(mn0->pc->cd.type)
804 {
805 case COL_TRIANGLE:
806 if (mn0->mesh == -1) continue;
807 {
808
809 Mesh * m0 = &meshes[mn0->mesh];
810 BVHNode * mesh_nodes = CAST_VEC(m0->nodes, BVHNode);
811
812
813 mat4 mat0 = mn0->global_matrix;
814 mat4 mat0_inv;
815 m4_inverse((float*)&mat0_inv, (float*)&mat0);
816 AABB tr = transform_aabb_by_model_matrix(aabb, mat0_inv);
817
818 bool col = PC_traverse_collision_bvh(mn, pc, m0, mn0, mesh_nodes, cd, mat, mat0, mat_inv, mat0_inv, tr, manifold);
819 }
820 break;
821 case COL_HULL:
822 {
823 if (mn0->mesh == -1) continue;
824 Mesh * m0 = &meshes[mn0->mesh];
825 ConvexHull * hulls0 = CAST_VEC(m0->hulls, ConvexHull);
826 for (int k = 0; k < m0->hulls.count; k++)
827 {
828
829
831 p.collision = false;
832 p.d = -INFINITY;
833 CollisionData cd0;
834 cd0.type = COL_HULL;
835 cd0.data.hull.pts = hulls0[k].vertices.buffer;
836 cd0.data.hull.count = hulls0[k].vertices.count;
837 cd0.data.hull.mfs = hulls0[k].hull_faces.buffer;
838 cd0.data.hull.face_count = hulls0[k].hull_faces.count;
839
840 mat4 mat0 = mn0->global_matrix;
841 mat4 mat0_inv;
842 m4_inverse((float*)&mat0_inv, (float*)&mat0);
843 if (GJK(cd, cd0, mat, mat0, mat_inv, mat0_inv, &p, EW[0]))
844 {
845 p.pcA = pc;
846 p.pcB = mn0->pc;
847
848 p.mnA = mn->id;
849 p.mnB = mn0->id;
850 p.normal_impulse = 0;
851 p.tangent_impulse = 0;
852 p.bias_impulse = 0;
853 manifold = find_manifold_for_pair(mn->id, mn0->id);
854 if (!manifold)
855 {
856 manifold = create_new_manifold(pc, mn->id, mn0->pc, mn0->id);
857 }
858 if (cd.type != COL_CAPSULE)
859 {
860
861 clip_manifold(manifold, &p, cd, mn->global_matrix, cd0, mn0->global_matrix);
862 }
863 else
864 {
865
866 manifold->count = 1;
867 manifold->contacts[0] = p;
868
869 }
870 manifold->processed = true;
871 }
872 }
873
874 }
875 break;
876 case COL_CAPSULE:
877 {
878 CollisionData cd0 = mn0->pc->cd;
880 p.collision = false;
881 p.d = -INFINITY;
882 mat4 mat0 = mn0->global_matrix;
883 mat4 mat0_inv;
884 m4_inverse((float*)&mat0_inv, (float*)&mat0);
885 if (GJK(cd, cd0, mat, mat0, mat_inv, mat0_inv, &p, EW[0]))
886 {
887 p.pcA = pc;
888 p.pcB = mn0->pc;
889
890 p.mnA = mn->id;
891 p.mnB = mn0->id;
892 p.normal_impulse = 0;
893 p.tangent_impulse = 0;
894 p.bias_impulse = 0;
895 manifold = find_manifold_for_pair(mn->id, mn0->id);
896 if (!manifold)
897 {
898 manifold = create_new_manifold(pc, mn->id, mn0->pc, mn0->id);
899 }
900 if (cd.type != COL_CAPSULE && cd0.type != COL_CAPSULE)
901 {
902
903 clip_manifold(manifold, &p, cd, mn->global_matrix, cd0, mn0->global_matrix);
904 }
905 else
906 {
907
908 manifold->count = 1;
909 manifold->contacts[0] = p;
910
911 }
912 manifold->processed = true;
913 }
914
915 }
916 break;
917 }
918 }
919 else
920 {
921 Mesh * m0 = &meshes[mn0->mesh];
922
923 ConvexHull * hulls0 = CAST_VEC(m0->hulls, ConvexHull);
924 if (m0->static_mesh)
925 {
926 BVHNode * mesh_nodes = CAST_VEC(m0->nodes, BVHNode);
927
928
929 mat4 mat0 = mn0->global_matrix;
930 mat4 mat0_inv;
931 m4_inverse((float*)&mat0_inv, (float*)&mat0);
932 AABB tr = transform_aabb_by_model_matrix(aabb, mat0_inv);
933
934 bool col = PC_traverse_collision_bvh(mn, pc, m0, mn0, mesh_nodes, cd, mat, mat0, mat_inv, mat0_inv, tr, manifold);
935 }
936 else
937 {
938 for (int k = 0; k < m0->hulls.count; k++)
939 {
940
941
943 p.collision = false;
944 p.d = -INFINITY;
945 CollisionData cd0;
946 cd0.type = COL_HULL;
947 cd0.data.hull.pts = hulls0[k].vertices.buffer;
948 cd0.data.hull.count = hulls0[k].vertices.count;
949 cd0.data.hull.mfs = hulls0[k].hull_faces.buffer;
950 cd0.data.hull.face_count = hulls0[k].hull_faces.count;
951
952 mat4 mat0 = mn0->global_matrix;
953 mat4 mat0_inv;
954 m4_inverse((float*)&mat0_inv, (float*)&mat0);
955 if (GJK(cd, cd0, mat, mat0, mat_inv, mat0_inv, &p, EW[0]))
956 {
957 p.pcA = pc;
958 p.pcB = mn0->pc;
959
960 p.mnA = mn->id;
961 p.mnB = mn0->id;
962 p.normal_impulse = 0;
963 p.tangent_impulse = 0;
964 p.bias_impulse = 0;
965 manifold = find_manifold_for_pair(mn->id, mn0->id);
966 if (!manifold)
967 {
968 manifold = create_new_manifold(pc, mn->id, mn0->pc, mn0->id);
969 }
970 if (cd.type != COL_CAPSULE)
971 {
972
973 clip_manifold(manifold, &p, cd, mn->global_matrix, cd0, mn0->global_matrix);
974 }
975 else
976 {
977 // generate_capsule_hull_manifold(&cd.data.capsule, mn->global_matrix, p.normal, p.d, manifold, pc, NULL);
978
979 manifold->count = 1;
980 manifold->contacts[0] = p;
981
982 }
983 manifold->processed = true;
984 }
985 }
986
987 }
988 }
989
990
991 }
992
993 ni = node->next;
994
995 }
996 else
997 {
998 ni = node->left;
999 }
1000
1001 }
1002
1003
1004}
1006{
1007 CollisionData cd = pc->cd;
1008 Mesh * meshes = CAST_VEC(GLOBALS.meshes, Mesh);
1010 mat4 mat = mn->global_matrix;
1011
1012 mat4 mat_inv;
1013 m4_inverse((float*)&mat_inv, (float*)&mat);
1015 PC_traverse_scene_bvh(mn, pc, cd, &GLOBALS.scene_data, mat, mat_inv, aabb);
1016}
1017
1018
1019
1020
1022{
1023 Mesh * meshes = CAST_VEC(GLOBALS.meshes, Mesh);
1025 Mesh * m = &meshes[mn->mesh];
1026 if (m->static_mesh) return;
1027 CollisionData cd = pc->cd;
1028
1029 // mat4 prop = compose_trs_from_vectors(proposed, mn->rotation, mn->scale);
1030 mat4 mat = mn->global_matrix;
1031
1032 mat4 mat_inv;
1033 m4_inverse((float*)&mat_inv, (float*)&mat);
1034
1036
1037
1038 ConvexHull * hulls = CAST_VEC(m->hulls, ConvexHull);
1039
1040
1041
1042 for (int i = 0; i < m->hulls.count; i++)
1043 {
1044 ConvexHull * hull = &hulls[i];
1045
1046 switch(cd.type)
1047 {
1048 case COL_TRIANGLE:
1049 break;
1050
1051 case COL_HULL:
1052 cd.data.hull.pts = hull->vertices.buffer;
1053 cd.data.hull.count = hull->vertices.count;
1054 cd.data.hull.mfs = hull->hull_faces.buffer;
1055 cd.data.hull.face_count = hull->hull_faces.count;
1056 break;
1057 case COL_CAPSULE:
1058 break;
1059 }
1061 PC_traverse_scene_bvh(mn, pc, cd, &GLOBALS.scene_data, mat, mat_inv, aabb);
1062 }
1063
1064}
1065
1067{
1068
1069 Mesh * meshes = CAST_VEC(GLOBALS.meshes, Mesh);
1071 CollisionData cd = pc->cd;
1072 if (mn->mesh == -1)
1073 {
1074
1075
1077
1078
1079 }
1080 else
1081 {
1083
1084 }
1085
1086}
1087
1088float normal_impulse0 = 0.0f;
1089float normal_impulse1 = 0.0f;
1090float normal_impulse2 = 0.0f;
1091float normal_impulse3 = 0.0f;
1096
1098{
1099
1101 Entity * entA = NULL;
1102 Entity * entB = NULL;
1103 MeshNode * mnA = NULL;
1104 MeshNode * mnB = NULL;
1105
1106
1107 vec3 vA = pcA->vel;
1108 vec3 vB = (vec3){0,0,0};
1109 float IMA = 1.0f / pcA->mass;
1110 float IMB = 0;
1111 mat3 IIA = pcA->inverse_world;
1112 vec3 IIB = (vec3){0,0,0};
1113
1114 vec3 comA;
1115 vec3 pA;
1118
1119
1120
1121 vec3 rA = vec3_sub(pA, comA);
1122
1123 vec3 CVA = vA;
1124
1125 vec3 relative = vec3_sub(CVA, (vec3){0,0,0});
1126 float vn = vec3_dot(relative, p->normal);
1127 float restitution = pcA->restitution;
1128 float angularA, angularB;
1129 if (vn < 0)
1130 {
1131
1132
1133 vec3 rnA = vec3_cross(rA, p->normal);
1134 vec3 IA = mat3_mul_vec3(IIA, rnA);
1135 vec3 tA = vec3_cross(IA, rA);
1136 angularA = vec3_dot(p->normal, tA);
1137
1138
1139 const float slop = 0.005f;
1140 const float beta = 0.2f;
1141
1142 float bias = fmaxf(p->d - slop, 0) * beta / dt;
1143
1144 float K = IMA + angularA;
1145 float j_vel = (-(1 + restitution) * vn) / K;
1146 float j_bias = bias / K;
1147
1148 float old_vel = p->normal_impulse;
1149 p->normal_impulse = fmaxf(old_vel + j_vel, 0.0f);
1150
1151 // float old_bias = p->bias_impulse;
1152 // p->bias_impulse = fmaxf(old_bias + j_bias, 0.0f);
1153
1154
1155 float j = (p->normal_impulse - old_vel);
1156
1157
1158 vec3 impulse = vec3_scale(p->normal, j);
1159
1160 pcA->vel.x += impulse.x * IMA;
1161 pcA->vel.y += impulse.y * IMA;
1162 pcA->vel.z += impulse.z * IMA;
1163
1164 }
1165}
1166
1168{
1169
1171 Entity * entA = NULL;
1172 Entity * entB = NULL;
1173 MeshNode * mnA = NULL;
1174 MeshNode * mnB = NULL;
1175
1176 if (!pcB->player_pc)
1177 {
1178 entB = pcB->ent;
1179 if (!entB) return;
1180 if (entB->free) return;
1181 if (entB->mesh_node < 0) return;
1182 mnB = &mns[entB->mesh_node];
1183 }
1184
1185
1186 vec3 vA = pcA->vel;
1187 vec3 vB = pcB->vel;
1188 float IMA = 1.0f / pcA->mass;
1189 float IMB = 1.0f / pcB->mass;
1190 mat3 IIA = pcA->inverse_world;
1191 mat3 IIB = pcB->inverse_world;
1192
1193 vec3 comA = pcA->center_of_mass;
1194 vec3 pA, pB;
1197
1198
1199 vec3 comB = pcB->center_of_mass;
1200 if (pcB->player_pc)
1201 {
1204
1205 }
1206 else
1207 {
1208 comB = mat4_mul_point(mnB->global_matrix, comB);
1209 pB = mat4_mul_point(mnB->global_matrix, p->b);
1210 }
1211
1212
1213 vec3 rA = vec3_sub(pA, comA);
1214 vec3 rB = vec3_sub(pB, comB);
1215
1216 // PRINT_VEC3N(rB);
1217 // PRINT_VEC3N(comB);
1218 // printf("NODE: %d\n", mnB->id);
1219 // A
1220 vec3 CVA = vA;
1221 vec3 CVB = vec3_add(vB, vec3_cross(pcB->angular_velocity, rB));
1222
1223
1224 vec3 relative = vec3_sub(CVA, CVB);
1225 float vn = vec3_dot(relative, p->normal);
1226 float angularA, angularB;
1227 if (vn < 0)
1228 {
1229
1230
1231 vec3 rnA = vec3_cross(rA, p->normal);
1232 vec3 IA = mat3_mul_vec3(IIA, rnA);
1233 vec3 tA = vec3_cross(IA, rA);
1234 angularA = vec3_dot(p->normal, tA);
1235
1236
1237 vec3 rnB = vec3_cross(rB, p->normal);
1238 vec3 IB = mat3_mul_vec3(IIB, rnB);
1239 vec3 tB = vec3_cross(IB, rB);
1240 angularB = vec3_dot(p->normal, tB);
1241 float K = IMA + IMB + angularA + angularB;
1242
1243 const float slop = 0.005f;
1244 const float beta = 0.05f;
1245
1246 float bias = fmaxf(p->d - slop, 0) * beta / dt;
1247
1248 float j_vel = (-(1 + pcA->restitution) * vn) / K;
1249 float j_bias = bias / K;
1250
1251 float old_vel = p->normal_impulse;
1252 p->normal_impulse = fmaxf(old_vel + j_vel, 0.0f);
1253
1254 float old_bias = p->bias_impulse;
1255 p->bias_impulse = fmaxf(old_bias + j_bias, 0.0f);
1256
1257
1258 float j = (p->normal_impulse - old_vel) + j_bias;
1259 // float j = (p->normal_impulse - old_vel);
1260
1261
1262 vec3 impulse = vec3_scale(p->normal, j);
1263
1264
1265 pcA->vel.x += impulse.x * IMA;
1266 pcA->vel.y += impulse.y * IMA;
1267 pcA->vel.z += impulse.z * IMA;
1268
1269 pcB->vel.x -= impulse.x * IMB;
1270 pcB->vel.y -= impulse.y * IMB;
1271 pcB->vel.z -= impulse.z * IMB;
1272 vec3 torqueA = vec3_cross(rA, impulse);
1273
1274 vec3 angA_impulse = mat3_mul_vec3(IIA, torqueA);
1275 // pcA->angular_velocity.x += angA_impulse.x;
1276 // pcA->angular_velocity.y += angA_impulse.y;
1277 // pcA->angular_velocity.z += angA_impulse.z;
1278
1279
1280 vec3 torqueB = vec3_cross(rB, vec3_scale(impulse, -1));
1281
1282 vec3 angB_impulse = mat3_mul_vec3(IIB, torqueB);
1283 pcB->angular_velocity.x += angB_impulse.x;
1284 pcB->angular_velocity.y += angB_impulse.y;
1285 pcB->angular_velocity.z += angB_impulse.z;
1286
1287 CVA = vec3_add(pcA->vel, vec3_cross(pcA->angular_velocity, rA));
1288 CVB = vec3_add(pcB->vel, vec3_cross(pcB->angular_velocity, rB));
1289
1290 relative = vec3_sub(CVA, CVB);
1291 vn = vec3_dot(relative, p->normal);
1292 // if (vn - j_bias < 0)
1293 // {
1294 vec3 vt = vec3_sub(relative, vec3_scale(p->normal, vn));
1295
1296 float vt_len = vec3_length(vt);
1297
1298 if (vt_len > 1e-6f)
1299 {
1300 vec3 tangent = vec3_scale(vt, 1.0f/vt_len);
1301
1302 p->tangent = tangent;
1303 vec3 rtA = vec3_cross(rA, tangent);
1304 vec3 ITA = mat3_mul_vec3(IIA, rtA);
1305 vec3 ttA = vec3_cross(ITA, rA);
1306 float angulartA = vec3_dot(tangent, ttA);
1307
1308
1309 vec3 rtB = vec3_cross(rB, tangent);
1310 vec3 ITB = mat3_mul_vec3(IIB, rtB);
1311 vec3 ttB = vec3_cross(ITB, rB);
1312 float angulartB = vec3_dot(tangent, ttB);
1313
1314 float jt = -vec3_dot(relative, tangent) / (IMA + IMB + angulartA + angulartB);
1315
1316 float max_frictionA = pcA->mu * p->normal_impulse;
1317
1318 float old = p->tangent_impulse;
1319
1320 p->tangent_impulse = clamp(old + jt, -max_frictionA, max_frictionA);
1321
1322 jt = p->tangent_impulse - old;
1323
1324
1325
1326 vec3 FIA = vec3_scale(tangent, jt);
1327
1328
1329 pcB->vel.x -= FIA.x * IMB;
1330 pcB->vel.y -= FIA.y * IMB;
1331 pcB->vel.z -= FIA.z * IMB;
1332
1333 vec3 FTA = vec3_cross(rA, FIA);
1334
1335 vec3 angFA_impulse = mat3_mul_vec3(IIA, FTA);
1336 // pcA->angular_velocity.x += angFA_impulse.x;
1337 // pcA->angular_velocity.y += angFA_impulse.y;
1338 // pcA->angular_velocity.z += angFA_impulse.z;
1339
1340 vec3 FTB = vec3_cross(rB, vec3_scale(FIA, -1));
1341 vec3 angFB_impulse = mat3_mul_vec3(IIB, FTB);
1342
1343 pcB->angular_velocity.x += angFB_impulse.x;
1344 pcB->angular_velocity.y += angFB_impulse.y;
1345 pcB->angular_velocity.z += angFB_impulse.z;
1346
1347
1348 }
1349
1350 // }
1351
1352
1353 }
1354
1355}
1356
1358{
1359
1361 Entity * entA = NULL;
1362 Entity * entB = NULL;
1363 MeshNode * mnA = NULL;
1364 MeshNode * mnB = NULL;
1365 if (!pcA->player_pc)
1366 {
1367 entA = pcA->ent;
1368 if (!entA) return;
1369 if (entA->free) return;
1370 if (entA->mesh_node < 0) return;
1371 mnA = &mns[entA->mesh_node];
1372 }
1373
1374
1375
1376 vec3 vA = pcA->vel;
1377 vec3 vB = (vec3){0,0,0};
1378 float IMA = 1.0f / pcA->mass;
1379 float IMB = 0;
1380 mat3 IIA = pcA->inverse_world;
1381 vec3 IIB = (vec3){0,0,0};
1382
1383 vec3 comA;
1384 vec3 pA;
1385 if (pcA->player_pc)
1386 {
1389
1390 }
1391 else
1392 {
1393
1394 comA = mat4_mul_point(mnA->global_matrix, pcA->center_of_mass);
1395 pA = mat4_mul_point(mnA->global_matrix, p->a);
1396 }
1397
1398
1399 vec3 rA = vec3_sub(pA, comA);
1400 // PRINT_VEC3N(rA);
1401 vec3 CVA = vec3_add(vA, vec3_cross(pcA->angular_velocity, rA));
1402
1403 vec3 relative = vec3_sub(CVA, (vec3){0,0,0});
1404 float vn = vec3_dot(relative, p->normal);
1405 // printf("VN: %f\n", vn);
1406 float restitution = pcA->restitution;
1407 // if (vn < 1.0f) restitution = 0;
1408 float angularA, angularB;
1409 if (vn < 0)
1410 {
1411
1412
1413 vec3 rnA = vec3_cross(rA, p->normal);
1414 vec3 IA = mat3_mul_vec3(IIA, rnA);
1415 vec3 tA = vec3_cross(IA, rA);
1416 angularA = vec3_dot(p->normal, tA);
1417
1418
1419 const float slop = 0.005f;
1420 const float beta = 0.05f;
1421
1422 float bias = fmaxf(p->d - slop, 0) * beta / dt;
1423
1424 float K = IMA + angularA;
1425 float j_vel = (-(1 + restitution) * vn) / K;
1426 float j_bias = bias / K;
1427
1428 float old_vel = p->normal_impulse;
1429 p->normal_impulse = fmaxf(old_vel + j_vel, 0.0f);
1430
1431 float old_bias = p->bias_impulse;
1432 p->bias_impulse = fmaxf(old_bias + j_bias, 0.0f);
1433
1434
1435 float j = (p->normal_impulse - old_vel) /* + j_bias */;
1436
1437 // printf("J: %f\n", j);
1438
1439 vec3 impulse = vec3_scale(p->normal, j);
1440
1441 pcA->vel.x += impulse.x * IMA;
1442 pcA->vel.y += impulse.y * IMA;
1443 pcA->vel.z += impulse.z * IMA;
1444
1445
1446 vec3 torqueA = vec3_cross(rA, impulse);
1447 vec3 ang_impulse = mat3_mul_vec3(IIA, torqueA);
1449
1450 pcA->angular_velocity.x += ang_impulse.x;
1451 pcA->angular_velocity.y += ang_impulse.y;
1452 pcA->angular_velocity.z += ang_impulse.z;
1453
1454 CVA = vec3_add(pcA->vel, vec3_cross(pcA->angular_velocity, rA));
1455 // CVA =pcA->vel;
1456 relative = CVA;
1457 vn = vec3_dot(relative, p->normal);
1458 // if (vn< 0)
1459 // {
1460 vec3 vt = vec3_sub(relative, vec3_scale(p->normal, vn));
1461
1462 float vt_len = vec3_length(vt);
1463 if (vt_len > 1e-3f)
1464 {
1465
1466
1467 vec3 tangent = vec3_scale(vt, 1.0f/vt_len);
1468
1469 p->tangent = tangent;
1470 // p->tangent = vec3_normalize(vec3_add(p->tangent, tangent));
1471
1472 vec3 rtA = vec3_cross(rA, tangent);
1473 vec3 ITA = mat3_mul_vec3(IIA, rtA);
1474 vec3 ttA = vec3_cross(ITA, rA);
1475 float angulartA = vec3_dot(tangent, ttA);
1476
1477 float jt = -vec3_dot(relative, tangent) / (IMA + angulartA);
1478
1479
1480 float max_frictionA = pcA->mu * p->normal_impulse;
1481
1482 float old = p->tangent_impulse;
1483
1484 p->tangent_impulse = clamp(old + jt, -max_frictionA, max_frictionA);
1485
1486 jt = p->tangent_impulse - old;
1487
1488 float jtA = clamp(jt, -max_frictionA, max_frictionA);
1489
1490 vec3 FIA = vec3_scale(tangent, jt);
1491
1492 pcA->vel.x += FIA.x * IMA;
1493 pcA->vel.y += FIA.y * IMA;
1494 pcA->vel.z += FIA.z * IMA;
1495
1496 vec3 FTA = vec3_cross(rA, FIA);
1497
1498 vec3 angF_impulse = mat3_mul_vec3(IIA, FTA);
1499
1500 pcA->angular_velocity.x += angF_impulse.x;
1501 pcA->angular_velocity.y += angF_impulse.y;
1502 pcA->angular_velocity.z += angF_impulse.z;
1503
1504
1505
1506 }
1507
1508 // }
1509
1510
1511 }
1512}
1513
1514
1515
1517{
1518
1520 Entity * entA = NULL;
1521 Entity * entB = NULL;
1522 MeshNode * mnA = NULL;
1523 MeshNode * mnB = NULL;
1524 if (!pcA->player_pc)
1525 {
1526 entA = pcA->ent;
1527 if (!entA) return;
1528 if (entA->free) return;
1529 if (entA->mesh_node < 0) return;
1530 mnA = &mns[entA->mesh_node];
1531 }
1532
1533 if (!pcB->player_pc)
1534 {
1535 entB = pcB->ent;
1536 if (!entB) return;
1537 if (entB->free) return;
1538 if (entB->mesh_node < 0) return;
1539 mnB = &mns[entB->mesh_node];
1540 }
1541
1542
1543 vec3 vA = pcA->vel;
1544 vec3 vB = pcB->vel;
1545 float IMA = 1.0f / pcA->mass;
1546 float IMB = 1.0f / pcB->mass;
1547 mat3 IIA = pcA->inverse_world;
1548 mat3 IIB = pcB->inverse_world;
1549
1550 vec3 comA = pcA->center_of_mass;
1551 vec3 pA, pB;
1552 if (pcA->player_pc)
1553 {
1556
1557 }
1558 else
1559 {
1560
1561 comA = mat4_mul_point(mnA->global_matrix, comA);
1562 pA = mat4_mul_point(mnA->global_matrix, p->a);
1563 }
1564
1565 vec3 comB = pcB->center_of_mass;
1566 if (pcB->player_pc)
1567 {
1570
1571 }
1572 else
1573 {
1574 comB = mat4_mul_point(mnB->global_matrix, comB);
1575 pB = mat4_mul_point(mnB->global_matrix, p->b);
1576 }
1577
1578
1579 vec3 rA = vec3_sub(pA, comA);
1580 vec3 rB = vec3_sub(pB, comB);
1581
1582 // A
1583 vec3 CVA = vec3_add(vA, vec3_cross(pcA->angular_velocity, rA));
1584 vec3 CVB = vec3_add(vB, vec3_cross(pcB->angular_velocity, rB));
1585
1586
1587 vec3 relative = vec3_sub(CVA, CVB);
1588 float vn = vec3_dot(relative, p->normal);
1589 float angularA, angularB;
1590 if (vn < 0)
1591 {
1592
1593
1594 vec3 rnA = vec3_cross(rA, p->normal);
1595 vec3 IA = mat3_mul_vec3(IIA, rnA);
1596 vec3 tA = vec3_cross(IA, rA);
1597 angularA = vec3_dot(p->normal, tA);
1598
1599
1600 vec3 rnB = vec3_cross(rB, p->normal);
1601 vec3 IB = mat3_mul_vec3(IIB, rnB);
1602 vec3 tB = vec3_cross(IB, rB);
1603 angularB = vec3_dot(p->normal, tB);
1604 float K = IMA + IMB + angularA + angularB;
1605
1606 const float slop = 0.005f;
1607 const float beta = 0.05f;
1608
1609 float bias = fmaxf(p->d - slop, 0) * beta / dt;
1610
1611 float j_vel = (-(1 + pcA->restitution) * vn) / K;
1612 float j_bias = bias / K;
1613
1614 float old_vel = p->normal_impulse;
1615 p->normal_impulse = fmaxf(old_vel + j_vel, 0.0f);
1616
1617 float old_bias = p->bias_impulse;
1618 p->bias_impulse = fmaxf(old_bias + j_bias, 0.0f);
1619
1620
1621 float j = (p->normal_impulse - old_vel) /* + j_bias */;
1622
1623
1624 vec3 impulse = vec3_scale(p->normal, j);
1625
1626
1627 pcA->vel.x += impulse.x * IMA;
1628 pcA->vel.y += impulse.y * IMA;
1629 pcA->vel.z += impulse.z * IMA;
1630
1631 pcB->vel.x -= impulse.x * IMB;
1632 pcB->vel.y -= impulse.y * IMB;
1633 pcB->vel.z -= impulse.z * IMB;
1634 vec3 torqueA = vec3_cross(rA, impulse);
1635
1636 vec3 angA_impulse = mat3_mul_vec3(IIA, torqueA);
1637
1638 pcA->angular_velocity.x += angA_impulse.x;
1639 pcA->angular_velocity.y += angA_impulse.y;
1640 pcA->angular_velocity.z += angA_impulse.z;
1641
1642 vec3 torqueB = vec3_cross(rB, vec3_scale(impulse, -1));
1643
1644 vec3 angB_impulse = mat3_mul_vec3(IIB, torqueB);
1645 pcB->angular_velocity.x += angB_impulse.x;
1646 pcB->angular_velocity.y += angB_impulse.y;
1647 pcB->angular_velocity.z += angB_impulse.z;
1648
1649 CVA = vec3_add(pcA->vel, vec3_cross(pcA->angular_velocity, rA));
1650 CVB = vec3_add(pcB->vel, vec3_cross(pcB->angular_velocity, rB));
1651
1652 relative = vec3_sub(CVA, CVB);
1653 vn = vec3_dot(relative, p->normal);
1654 // if (vn - j_bias < 0)
1655 // {
1656 vec3 vt = vec3_sub(relative, vec3_scale(p->normal, vn));
1657
1658 float vt_len = vec3_length(vt);
1659
1660 if (vt_len > 1e-6f)
1661 {
1662 vec3 tangent = vec3_scale(vt, 1.0f/vt_len);
1663
1664 p->tangent = tangent;
1665 vec3 rtA = vec3_cross(rA, tangent);
1666 vec3 ITA = mat3_mul_vec3(IIA, rtA);
1667 vec3 ttA = vec3_cross(ITA, rA);
1668 float angulartA = vec3_dot(tangent, ttA);
1669
1670
1671 vec3 rtB = vec3_cross(rB, tangent);
1672 vec3 ITB = mat3_mul_vec3(IIB, rtB);
1673 vec3 ttB = vec3_cross(ITB, rB);
1674 float angulartB = vec3_dot(tangent, ttB);
1675
1676 float jt = -vec3_dot(relative, tangent) / (IMA + IMB + angulartA + angulartB);
1677
1678 float max_frictionA = pcA->mu * p->normal_impulse;
1679
1680 float old = p->tangent_impulse;
1681
1682 p->tangent_impulse = clamp(old + jt, -max_frictionA, max_frictionA);
1683
1684 jt = p->tangent_impulse - old;
1685
1686
1687
1688 vec3 FIA = vec3_scale(tangent, jt);
1689
1690 pcA->vel.x += FIA.x * IMA;
1691 pcA->vel.y += FIA.y * IMA;
1692 pcA->vel.z += FIA.z * IMA;
1693
1694 pcB->vel.x -= FIA.x * IMB;
1695 pcB->vel.y -= FIA.y * IMB;
1696 pcB->vel.z -= FIA.z * IMB;
1697
1698 vec3 FTA = vec3_cross(rA, FIA);
1699
1700 vec3 angFA_impulse = mat3_mul_vec3(IIA, FTA);
1701
1702 pcA->angular_velocity.x += angFA_impulse.x;
1703 pcA->angular_velocity.y += angFA_impulse.y;
1704 pcA->angular_velocity.z += angFA_impulse.z;
1705
1706 vec3 FTB = vec3_cross(rB, vec3_scale(FIA, -1));
1707 vec3 angFB_impulse = mat3_mul_vec3(IIB, FTB);
1708
1709 pcB->angular_velocity.x += angFB_impulse.x;
1710 pcB->angular_velocity.y += angFB_impulse.y;
1711 pcB->angular_velocity.z += angFB_impulse.z;
1712
1713
1714 }
1715
1716 // }
1717
1718
1719 }
1720
1721}
1722
1723
1725{
1726
1727 for (int i = 0; i < MAX_PHYSICS_COMPONENTS; i++)
1728 {
1729 if (PHYSICS_COMPONENTS[i].free || PHYSICS_COMPONENTS[i].at_rest || PHYSICS_COMPONENTS[i].player_pc) continue;
1731 pc->old_vel = pc->vel;
1732 pc->old_angular = pc->angular_velocity;
1733 PC_apply_velocity(pc, dt);
1734 }
1735 for (int i = 0; i < MAX_PHYSICS_COMPONENTS; i++)
1736 {
1737 if (PHYSICS_COMPONENTS[i].free || PHYSICS_COMPONENTS[i].player_pc) continue;
1739
1740 Entity * ent = pc->ent;
1741
1742 if (!ent) continue;
1743 if (ent->free) continue;
1744 if (ent->mesh_node < 0) continue;
1746 MeshNode * mn = &mns[ent->mesh_node];
1747 if (pc->cd.type == COL_CAPSULE)
1748 {
1755
1758 for (int j = 0; j < 24; j++)
1759 {
1760 DEBUG_LINES[DEBUG_LINE_COUNT + j].color = (vec3){0,1,0};
1761 }
1762 DEBUG_LINE_COUNT += 24;
1763
1765
1766 }
1767
1768 if (pc->at_rest) continue;
1769
1770 vec3 proposed = vec3_add(mn->translation, vec3_scale(pc->vel, dt));
1771
1772
1773 float angle = vec3_length(pc->angular_velocity) * dt;
1774
1775 vec4 rotation = mn->rotation;
1776 if(angle > 0 && pc->physics_type == PHYSICS_FULL)
1777 {
1779
1780 vec4 dq = quat_from_axis_angle(axis, angle);
1781
1782 rotation = vec4_normalize(quat_mul(dq, mn->rotation));
1783 }
1784
1785
1786 mn->global_matrix = compose_trs_from_vectors(proposed, rotation, mn->scale);
1789 // print_aabb(&pc->aabb);
1790
1791
1792
1793 }
1794 for (int i = 0; i < MAX_PHYSICS_COMPONENTS; i++)
1795 {
1796 if (PHYSICS_COMPONENTS[i].free || PHYSICS_COMPONENTS[i].at_rest || PHYSICS_COMPONENTS[i].player_pc) continue;
1798
1799 Entity * ent = pc->ent;
1800
1801 if (!ent) continue;
1802 if (ent->free) continue;
1803 if (ent->mesh_node < 0) continue;
1805 MeshNode * mn = &mns[ent->mesh_node];
1806 // printf("CHECKING\n");
1807 // Timer PC_timer;
1808 // start_timer(&PC_timer);
1809 PC_find_collision(mn, pc);
1810 // double PC_time = end_timer(&PC_timer);
1811 // printf("TIME: %f\n", PC_time);
1812 }
1813 for (int i = 0; i < MAX_MANIFOLDS; i++)
1814 {
1815 if (MANIFOLDS[i].free) continue;
1816 // if this manifold did not detect a collision this frame, remove it
1817 if (!MANIFOLDS[i].processed)
1818 {
1819 MANIFOLDS[i].free = true;
1820 MANIFOLDS[i].a = NULL;
1821 MANIFOLDS[i].b = NULL;
1822 MANIFOLDS[i].count = 0;
1823 MANIFOLDS[i].processed = false;
1824 continue;
1825 }
1826 Manifold * m = &MANIFOLDS[i];
1827 m->deepest = -INFINITY;
1828 for (int k = 0; k < m->count; k++)
1829 {
1830 CollisionPoints * p = &m->contacts[k];
1831
1833 MeshNode * mnA = &mns[p->mnA];
1834 MeshNode * mnB = &mns[p->mnB];
1835
1836 vec3 worldA = mat4_mul_point(mnA->global_matrix, p->a);
1837 vec3 worldB = mat4_mul_point(mnB->global_matrix, p->b);
1838 float separation = vec3_dot(vec3_sub(worldA, worldB), p->normal);
1839 p->d = -separation;
1840
1841
1842 }
1843 }
1844 normal_impulse0 = 0.0f;
1845 normal_impulse1 = 0.0f;
1846 normal_impulse2 = 0.0f;
1847 normal_impulse3 = 0.0f;
1848 torque_sum = (vec3){0,0,0};
1849 impulse_sum = (vec3){0,0,0};
1850 friction_sum = (vec3){0,0,0};
1851 ang_impulse_sum = (vec3){0,0,0};
1852 for (int j = 0; j < 4; j++)
1853 {
1854 for (int i = 0; i < MAX_MANIFOLDS; i++)
1855 {
1856 MANIFOLDS[i].processed = false;
1857 if (MANIFOLDS[i].free) continue;
1858 Manifold * m = &MANIFOLDS[i];
1859 if(m->count == 0) continue;
1860 int seed = rand() % m->count;
1861 for (int k = 0; k < m->count; k++)
1862 {
1863 int idx = (seed + k) % m->count;
1864 // int idx = k;
1865 CollisionPoints * p = &m->contacts[idx];
1866 // PRINT_VEC3N(m->contacts[k].a);
1867 // PRINT_VEC3N(m->contacts[k].b);
1868 // PRINT_VEC3N(m->contacts[k].normal);
1869 // printf("DFINAL: %f\n", p->d);
1870 if (p->d < 0.0) continue;
1871 if (p->pcB && p->pcA)
1872 {
1873 PC_resolve_impulse(p->pcA, p->pcB, p);
1874
1875 }
1876 else if (p->pcA)
1877 {
1878
1880 }
1881 else
1882 {
1884 }
1885 if (p->pcA)
1886 {
1887 if (p->pcA->physics_type != PHYSICS_FULL)
1888 {
1889 p->pcA->angular_velocity = (vec3){0,0,0};
1890 }
1891 }
1892 if (p->pcB)
1893 {
1894 if (p->pcB->physics_type != PHYSICS_FULL)
1895 {
1896 p->pcB->angular_velocity = (vec3){0,0,0};
1897 }
1898
1899 }
1900
1901 }
1902 }
1903 }
1904 // for (int i = 0; i < MAX_MANIFOLDS; i++)
1905 // {
1906 // if (MANIFOLDS[i].free) continue;
1907 // Manifold * m = &MANIFOLDS[i];
1908 // if (m->count == 0) continue;
1909
1910
1911
1912 // float max_depth = 0.0f;
1913 // vec3 normal = m->contacts[0].normal;
1914 // // vec3 normal = (vec3){0,0,0};
1915
1916 // for (int k = 0; k < m->count; ++k)
1917 // {
1918 // max_depth = fmaxf(max_depth, m->contacts[k].d);
1919 // // normal = vec3_add(normal, m->contacts[k].normal);
1920 // }
1921 // // normal = vec3_normalize(normal);
1922 // CollisionPoints * p = &m->contacts[0];
1923 // // if (max_depth < 0) continue;
1924
1925 // float slop = 0.005f;
1926
1927 // float correction = fmaxf(max_depth - slop, 0) * 0.0005f;
1928
1929
1930 // // if (p->pcB && p->pcA)
1931 // // {
1932
1933 // // float IMA = 1.0f/p->pcA->mass;
1934 // // float IMB = 1.0f/p->pcB->mass;
1935 // // float total_inv = IMA + IMB;
1936 // // p->pcA->corrected_position = vec3_add(p->pcA->corrected_position, vec3_scale(vec3_scale(vec3_scale(p->normal, correction), IMA), 1.0f/total_inv));
1937 // // p->pcB->corrected_position = vec3_sub(p->pcB->corrected_position, vec3_scale(vec3_scale(vec3_scale(p->normal, correction), IMB), 1.0f/total_inv));
1938
1939 // // }
1940 // // else if (p->pcA)
1941 // // {
1942
1943 // // p->pcA->corrected_position = vec3_add(p->pcA->corrected_position, vec3_scale(p->normal, correction));
1944
1945 // // }
1946 // // else
1947 // // {
1948 // // p->pcB->corrected_position = vec3_add(p->pcB->corrected_position, vec3_scale(p->normal, correction));
1949
1950 // // }
1951
1952
1953 // MANIFOLDS[i].processed = false;
1954
1955 // }
1956 for (int i = 0; i < MAX_PHYSICS_COMPONENTS; i++)
1957 {
1958 if (PHYSICS_COMPONENTS[i].free || PHYSICS_COMPONENTS[i].at_rest || PHYSICS_COMPONENTS[i].player_pc) continue;
1960
1961 Entity * ent = pc->ent;
1962
1963 if (!ent) continue;
1964 if (ent->free) continue;
1965 if (ent->mesh_node < 0) continue;
1967 Mesh * meshes = CAST_VEC(GLOBALS.meshes, Mesh);
1968 MeshNode * mn = &mns[ent->mesh_node];
1969 vec3 proposed = vec3_add(mn->translation, vec3_scale(pc->vel, dt));
1970 float diff = vec3_length(vec3_sub(proposed, mn->translation));
1971 Light * lights = CAST_VEC(GLOBALS.lights, Light);
1972 if (mn->mesh != -1)
1973 {
1974 Mesh * m = &meshes[mn->mesh];
1975 int lights_in_range[MAX_LIGHTS];
1976 int light_count = 0;
1977 for (int j = 0; j < GLOBALS.lights.count; j++)
1978 {
1979 Light * l = &lights[j];
1980
1981 if (l->light_type != POINT_LIGHT || l->sm_dirty) continue;
1982
1983 vec3 min = mat4_mul_point(mn->global_matrix, (vec3){m->aabb.min_x, m->aabb.min_y, m->aabb.min_z});
1984 vec3 max = mat4_mul_point(mn->global_matrix, (vec3){m->aabb.max_x, m->aabb.max_y, m->aabb.max_z});
1985 if (test_sphere_aabb(l->pos, l->radius, min, max))
1986 {
1987 lights_in_range[light_count++] = j;
1988 }
1989
1990 }
1991 float lowest_dist = INFINITY;
1992 int closest = -1;
1993 for (int j = 0; j < light_count; j++)
1994 {
1995 Light * l = &lights[lights_in_range[j]];
1996 float dist = vec3_length(vec3_sub(l->pos, proposed));
1997 if (dist < lowest_dist)
1998 {
1999 closest = lights_in_range[j];
2000 lowest_dist = dist;
2001 }
2002 }
2003 if (closest >= 0)
2004 {
2005 lights[closest].sm_dirty = true;
2006 }
2007 }
2008 mn->translation.x = proposed.x;
2009 mn->translation.y = proposed.y;
2010 mn->translation.z = proposed.z;
2011
2012 float angle = vec3_length(pc->angular_velocity) * dt;
2013
2014 if(angle > 0 && pc->physics_type == PHYSICS_FULL)
2015 {
2017
2018 vec4 dq = quat_from_axis_angle(axis, angle);
2019
2020 mn->rotation = vec4_normalize(quat_mul(dq, mn->rotation));
2021 }
2022
2023
2025
2028
2029
2030 pc->accel = vec3_add(vec3_scale(pc->accel, 1.0f - ACCEL_BETA), vec3_sub(pc->vel, pc->old_vel));
2031
2032 }
2033
2034 for (int i = 0; i < MAX_MANIFOLDS; i++)
2035 {
2036 if (MANIFOLDS[i].free) continue;
2037 Manifold * m = &MANIFOLDS[i];
2038 for (int k = 0; k < m->count; k++)
2039 {
2040 CollisionPoints * p = &m->contacts[k];
2041
2043 MeshNode * mnA = &mns[p->mnA];
2044 MeshNode * mnB = &mns[p->mnB];
2045
2046 vec3 worldA = mat4_mul_point(mnA->global_matrix, p->a);
2047 vec3 worldB = mat4_mul_point(mnB->global_matrix, p->b);
2048 vec3 dist = vec3_sub(worldA, worldB);
2049 float d = vec3_dot(dist, dist);
2050 float separation = vec3_dot(dist, p->normal);
2051 vec3 up;
2052
2053 if (fabsf(p->normal.z) < 0.9f)
2054 up = (vec3){0,0,1};
2055 else
2056 up = (vec3){1,0,0};
2057
2058 vec3 T = vec3_normalize(vec3_cross(up, p->normal));
2059 float td = vec3_dot(dist, T);
2060
2061
2062 if (separation > 0.01f || td > 0.01f)
2063 {
2064 for (int j = k; j < m->count-1; j++)
2065 {
2066 m->contacts[j] = m->contacts[j+1];
2067 }
2068 m->count--;
2069 k--;
2070 }
2071
2072 }
2073 }
2074 for (int i = 0; i < MAX_PHYSICS_COMPONENTS; i++)
2075 {
2076 if (PHYSICS_COMPONENTS[i].free || PHYSICS_COMPONENTS[i].at_rest || PHYSICS_COMPONENTS[i].player_pc) continue;
2078
2079 float vel_diff = vec3_length(vec3_sub(pc->old_vel, pc->vel));
2080 float avel_diff = vec3_length(vec3_sub(pc->old_angular, pc->angular_velocity));
2081 // PRINT_VEC3N(pc->old_vel);
2082 // PRINT_VEC3N(pc->vel);
2083 // printf("VEL DIFF: %f\n", vel_diff);
2084 // PRINT_VEC3N(pc->old_angular);
2085 // PRINT_VEC3N(pc->angular_velocity);
2086 // printf("AVEL DIFF: %f\n", avel_diff);
2087
2088 float vel_len = vec3_length(pc->vel);
2089 float angular_len = vec3_length(pc->angular_velocity);
2090 const float rest_eps = 0.1f;
2091 const float rest_ceil = 0.2f;
2092 // if (vel_diff < rest_eps && vel_len < rest_ceil && avel_diff < rest_eps && angular_len < rest_ceil)
2093 if (vel_len < rest_ceil && angular_len < rest_ceil)
2094 {
2095 pc->rest_counter++;
2096 }
2097 else
2098 {
2099
2100 pc->rest_counter = 0;
2101 pc->at_rest = false;
2102 }
2103
2104 const int REST_MARGIN = 50;
2105 if (pc->rest_counter >= REST_MARGIN)
2106 {
2107 pc->at_rest = true;
2108 pc->rest_counter = 0;
2109 }
2110
2111 }
2112
2113}
2114
2116{
2117 AABB aabb = pc->aabb;
2118 float w = aabb.extents.x * 2;
2119 float d = aabb.extents.y * 2;
2120 float h = aabb.extents.z * 2;
2121 float w2 = w*w;
2122 float d2 = d*d;
2123 float h2 = h*h;
2124
2125 float mass = pc->mass;
2126 mat3 iia;
2127 iia.a0 = 12.0f / (mass * (h2 + d2));
2128 iia.a1 = 0;
2129 iia.a2 = 0;
2130 iia.b0 = 0;
2131 iia.b1 = 12.0f / (mass * (w2 + d2));
2132 iia.b2 = 0;
2133 iia.c0 = 0;
2134 iia.c1 = 0;
2135 iia.c2 = 12.0f / (mass * (w2 + h2));
2136
2137 return iia;
2138
2139}
2140
2141void init_PC(int mesh_node, PhysicsComponent * pc, AABB ai_aabb, CollisionType collision_type, PhysicsType physics_type)
2142{
2144 Mesh * meshes = CAST_VEC(GLOBALS.meshes, Mesh);
2145 MeshNode * mn = &mns[mesh_node];
2146 // if (mn->mesh == -1) return;
2147 AABB aabb;
2148 if (mn->mesh != -1)
2149 {
2150 Mesh * m = &meshes[mn->mesh];
2151 aabb = m->aabb;
2152
2153 // aabb = mn->aabb;
2154 // aabb = ai_aabb;
2155 }
2156 else
2157 {
2158 aabb = mn->aabb;
2159 // aabb = ai_aabb;
2160 }
2161
2162 pc->center_of_mass = aabb.center;
2163 pc->aabb = aabb;
2164 pc->vel = (vec3){0,0,0};
2165 pc->angular_velocity = (vec3){0,0,0};
2166 float mass = 200.0f;
2167 pc->mass = mass;
2168 pc->restitution = 0.2f;
2169 pc->mu = 0.5f;
2170 pc->player_pc = false;
2171 pc->physics_type = physics_type;
2172
2173 pc->cd.type = collision_type;
2174 switch(collision_type)
2175 {
2176 case COL_TRIANGLE:
2177 break;
2178 case COL_HULL:
2179 break;
2180 case COL_CAPSULE:
2181 print_aabb(&aabb);
2182 pc->cd.data.capsule.a = (vec3){aabb.center.x,aabb.center.y,aabb.min_z};
2183 pc->cd.data.capsule.b = (vec3){aabb.center.x,aabb.center.y,aabb.max_z};
2184 // pc->cd.data.capsule.a = (vec3){aabb.center.x,aabb.center.y,-aabb.extents.z};
2185 // pc->cd.data.capsule.b = (vec3){aabb.center.x,aabb.center.y,aabb.extents.z};
2186 // pc->cd.data.capsule.a = (vec3){aabb.center.x,aabb.center.y,0};
2187 // pc->cd.data.capsule.b = (vec3){aabb.center.x,aabb.center.y,1};
2188 // pc->cd.data.capsule.radius = MAX(aabb.max_y - aabb.min_y, aabb.max_x - aabb.min_x) / 2;
2189 pc->cd.data.capsule.radius = 0.2;
2190 break;
2191 }
2192 // pc->cd.type = COL_HULL;
2193 // pc->cd.data.capsule.a = (vec3){aabb.center.x,aabb.center.y,0};
2194 // pc->cd.data.capsule.b = (vec3){aabb.center.x,aabb.center.y,1};
2195 // // causes a bug with epa
2196 // pc->cd.data.capsule.radius = 0.6;
2197 // printf("W: %f %f\n", pc->cd.data.capsule.height, pc->cd.data.capsule.radius);
2198 // PRINT_VEC3N(pc->cd.data.capsule.a);
2199 // PRINT_VEC3N(pc->cd.data.capsule.b);
2200
2201
2202 // try 6 after
2203
2204
2205 // float I = 2.0f/5.0f * mass * m->aabb.extents.x * m->aabb.extents.x;
2206
2207 // pc->inverse_inertia = (vec3)
2208 // {
2209 // 1/I,
2210 // 1/I,
2211 // 1/I
2212 // };
2213
2215
2216}
2217
GJK and EPA collision resolution.
float rand()
vec3 rot(vec3 M[3], vec3 a)
Refer to Ken Perlin's 2 joint IK implmentation.
Definition camera.c:1004
Functions for camera.
Camera cam
This is currently the player cam.
Definition main.c:322
bool test_sphere_aabb(vec3 light_pos, float light_radius, vec4 min, vec4 max)
AABBBounds transform_aabb_by_model_matrix(MeshNode mn, mat4 mat)
uniform uint n
shared uint temp[1024]
Engine specific types and defines.
Vector mesh_nodes
MeshNodes.
@ POINT_LIGHT
SceneData scene_data
SceneData that is more-so scene specific.
Vector bvh_nodes
int mesh_node
The top-most MeshNode that belongs to this Entity.
Vector meshes
Meshes.
#define MAX_LIGHTS
Maximum amount of Lights to be pushed per frame (used for SSBO sizing).
Globals GLOBALS
Engine Globals.
Definition main.c:310
Vector lights
Lights.
bool free
If the Entity is empty.
#define ACCEL_BETA
Friction / air accel decay constant.
Camera * cam
Player Camera.
CamData cam_data
size_t * bvh_indices
vec4 cam_pos
The position of the Camera (padded).
Entity structure that holds components and references to the data it owns.
The data for the current scene.
MeshNode * get_root_node(MeshNode *mns, MeshNode *mn)
mat4 compose_trs_from_mesh_node(MeshNode *mn)
Vector hull_faces
HullFaces that are generated after incremental_hull() that combine contiguous triangles into singular...
CollisionData cd
Tagged union for collision options.
vec3 old_vel
Last step's velocity.
Vector vertices
Points that initially copy the Mesh vertices but then are deduplicated and modified in incremental_hu...
mat3 inverse_world
PhysicsComponent.inverse_inertia transformed by rotation into world space for the current step.
int rest_counter
The counter that decreases until at rest.
int count
The amount of valid contacts.
bool processed
Tracks if a Manifold has been processed this frame yet.
bool player_pc
If this is the Player PhysicsComponent.
vec3 vel
Velocity.
mat3 inverse_inertia
IIA (currently just a calculated box).
bool free
Tracks if a Manifold can be replaced.
PhysicsComponent * a
vec3 accel
Acceleration from this step + acceleration from last step * ACCEL_BETA.
vec3 tangent
The tangent used for frictional impulse resolution.
float mass
Mass of the component.
PhysicsType physics_type
Impulse resolution options.
vec3 a
The bottom of the capsule (based on AABB).
AABB aabb
Bounding box of the PC, determined based on Mesh or MeshNode.
PhysicsComponent * b
PhysicsComponent * pcA
vec3 angular_velocity
Rotational velocity.
float deepest
Current deepest contact for a search.
union CollisionData::@341211070342303276310035141141142214214320110321 data
PhysicsComponent * pcB
CollisionPoints contacts[4]
The collision contacts.
bool at_rest
If this component is at rest it does not update.
float d
Length of B-A.
float mu
The friction constant for the component.
float radius
The radius of the capsule.
Entity * ent
Which Entity this component belongs to.
vec3 old_angular
Last step's angular velocity.
vec3 b
The top of the capsule (based on AABB).
vec3 a
Furthest point of A into B.
CollisionType type
vec3 center_of_mass
COM based on the AABB.
vec3 normal
The collision normal from B into A.
vec3 b
Furthest point of B into A.
float restitution
The restitution (bounce).
bool PC_traverse_collision_bvh(MeshNode *mn, PhysicsComponent *pc, Mesh *m0, MeshNode *mn0, BVHNode *nodes, CollisionData cd, mat4 mat, mat4 mat0, mat4 mat_inv, mat4 mat0_inv, AABB tr_aabb, Manifold *manifold)
Traverses the BVH for a static MeshNode with a PhysicsComponent to detect collisions.
Definition physics.c:717
void PC_find_collision(MeshNode *mn, PhysicsComponent *pc)
Searches for collisions from a PhysicsComponent and determines how based on if its corresponding Mesh...
Definition physics.c:1066
void search_collisions_from_pc(MeshNode *mn, PhysicsComponent *pc)
Searches for collisions from a PhysicsComponent that has no corresponding Mesh.
Definition physics.c:1005
vec3 torque_sum
Definition physics.c:1092
float normal_impulse3
Definition physics.c:1091
void PC_resolve_impulse_static(PhysicsComponent *pcA, CollisionPoints *p)
Solves an impulse for a PhysicsComponent against a static Mesh.
Definition physics.c:1357
void update_physics_components()
The main PhysicsComponent loop that determines velocity, solves collisions, then integrates position.
Definition physics.c:1724
void player_resolve_impulse_static(PhysicsComponent *pcA, CollisionPoints *p)
Solves an impulse for a Player PhysicsComponent against a static Mesh.
Definition physics.c:1097
void PC_apply_velocity(PhysicsComponent *pc, float dt)
Applies gravity to a PhysicsComponent.
Definition physics.c:149
float normal_impulse0
Definition physics.c:1088
Manifold * create_new_manifold(PhysicsComponent *a, int mn0, PhysicsComponent *b, int mn1)
Finds a free Manifold and initializes it between two MeshNodes.
Definition physics.c:611
vec3 ang_impulse_sum
Definition physics.c:1095
void PC_traverse_scene_bvh(MeshNode *mn, PhysicsComponent *pc, CollisionData cd, SceneData *sd, mat4 mat, mat4 mat_inv, AABB aabb)
Traverses the scene BVH to detect collisions from a PhysicsComponent.
Definition physics.c:752
Manifold * find_manifold_for_pair(int mn0, int mn1)
Finds a current Manifold if there is one from MANIFOLDS for a given pair of MeshNodes.
Definition physics.c:594
bool GJK(CollisionData a_cd, CollisionData b_cd, mat4 a_mat, mat4 b_mat, mat4 a_inv, mat4 b_inv, CollisionPoints *collision_points, EPAWorkspace *EW)
GJK polytope construction based on A and B (which can be any CollisionType), calls EPA internally.
Definition GJK.c:1052
void init_PC(int mesh_node, PhysicsComponent *pc, AABB ai_aabb, CollisionType collision_type, PhysicsType physics_type)
Initializes a PhysicsComponent.
Definition physics.c:2141
void clip_manifold(Manifold *manifold, CollisionPoints *p, CollisionData a_cd, mat4 modelA, CollisionData b_cd, mat4 modelB)
Clips a manifold based on two different sets of points and faces with separate transforms.
Definition physics.c:213
int generate_capsule_hull_manifold(CapsuleData *cap, mat4 cap_transform, vec3 hull_normal, float hull_plane_dist, Manifold *manifold, PhysicsComponent *pcA, PhysicsComponent *pcB)
Generates a manifold between a capsule and a hull plane.
Definition physics.c:172
float normal_impulse1
Definition physics.c:1089
vec3 impulse_sum
Definition physics.c:1094
CollisionType
Defines collision types with different behaviors and mesh setup and validation.
float normal_impulse2
Definition physics.c:1090
mat3 calc_inverse_inertia(PhysicsComponent *pc)
Calculates the mat3 IIA for a PhysicsComponent, using AABB bounds and mass.
Definition physics.c:2115
void PC_resolve_impulse(PhysicsComponent *pcA, PhysicsComponent *pcB, CollisionPoints *p)
Solves an impulse for a PhysicsComponent against another.
Definition physics.c:1516
bool PC_process_collision_node(MeshNode *mn, PhysicsComponent *pc, Mesh *m0, MeshNode *mn0, BVHNode *node, CollisionData cd, mat4 mat, mat4 mat0, mat4 mat_inv, mat4 mat0_inv, Manifold *manifold)
Processes a collision against one BVHNode inside of a COL_TRIANGLE static MeshNode.
Definition physics.c:638
void search_collisions_from_hull(MeshNode *mn, PhysicsComponent *pc)
Searches for collisions from a PhysicsComponent that has a corresponding Mesh.
Definition physics.c:1021
PhysicsType
Defines physics types with different behaviors in impulse resolution.
void player_resolve_impulse(PhysicsComponent *pcA, PhysicsComponent *pcB, CollisionPoints *p)
Solves an impulse for a Player PhysicsComponent against another PhysicsComponent.
Definition physics.c:1167
vec3 friction_sum
Definition physics.c:1093
@ COL_CAPSULE
A capsule is constructed via AABB dimensions and is used for collision tests.
@ COL_HULL
Incremental hull generates a ConvexHull which is used for collision tests.
@ COL_TRIANGLE
The triangle BVH is generated for this mesh and triangle tests are performed on it.
@ PHYSICS_FULL
All impulse resolution options applied.
The capsule data for collisions.
The tagged union for collision type, stored in PhysicsComponent.
Points that track collision data such as direction, penetration depth, and more to generate Manifolds...
Convex Hulls that are generated via incremental hull for non-static meshes.
Persistent Manifolds that track collisions and how to resolve them.
Entity Component used for physics resolution.
size_t id
The ID inside of Globals.mesh_nodes.
vec3 pos
Position of light.
vec3 translation
LightType light_type
The type of Light.
PhysicsComponent * pc
The physics controller initialized based on data from this MeshNode.
size_t local_id
The ID local to the original file structure.
bool sm_dirty
If a point light needs a new shadowmap.
bool free
If the MeshNode can be replaced.
int mesh
The Mesh it references.
int parent
The parent to this node, a global index into Globals.mesh_nodes.
float radius
The radius of the Light.
AABB aabb
The AABB of the MeshNode and its children.
The CPU side structure for Lights.
MeshNodes that describe the position of Meshes, GLTF nodes for GLTFAnimations, or hold references to ...
Important globals for the entire engine and initialization functions that are mainly called on engine...
#define MAX_PHYSICS_COMPONENTS
Definition init.h:445
#define MAX_MANIFOLDS
The maximum amount of Manifolds in MANIFOLDS.
Definition init.h:448
Vector hulls
The ConvexHulls generated if this Mesh is dynamic.
bool static_mesh
This Mesh is static and must create a BVH or if not it must perform incremental_hull.
size_t * bvh_indices
Indices for the BVH, sorted as the BVH is generated.
AABB aabb
The AABB of the mesh.
Vector nodes
BVHNodes.
Vector tris
The MeshFaces.
Vector vertices
The Points.
A Mesh with geometry data that is either initialized as static or dynamic and split into SubMeshes.
EPAWorkspace * EW[8]
Definition main.c:291
PhysicsComponent PHYSICS_COMPONENTS[MAX_PHYSICS_COMPONENTS]
Definition main.c:314
float dt
The 1.0f/60 simulation timestep.
Definition main.c:319
int DEBUG_LINE_COUNT
Definition main.c:282
Manifold MANIFOLDS[MAX_MANIFOLDS]
Definition main.c:270
DebugVertex DEBUG_LINES[DEBUG_LINES_MAX]
Definition main.c:281
Player player
The Player data.
Definition main.c:287
vec3 vec3_cross(vec3 a, vec3 b)
Definition mathf.h:80
float vec3_dot(vec3 a, vec3 b)
Definition mathf.h:322
#define MIN(a, b)
Definition mathf.h:435
vec3 vec3_normalize(vec3 v)
Definition mathf.h:46
vec3 vec3_scale(vec3 a, float b)
Definition mathf.h:248
vec3 vec3_sub(vec3 a, vec3 b)
Definition mathf.h:266
vec3 vec3_add(vec3 a, vec3 b)
Definition mathf.h:286
vec4 vec4_normalize(vec4 v)
Definition mathf.h:55
float clamp(float a, float b, float c)
Definition mathf.h:379
bool m4_inverse(float out[16], const float m[16])
Definition mathf.h:164
mat4 mat4_inverse(mat4 in)
Definition mathf.h:340
float vec3_length(vec3 v)
Definition mathf.h:328
Mesh functions that modify and initialize Mesh data.
int next
The pointer to the next node (either right child or parent's right child) for recursion-less traversa...
int right
Right child.
AABB aabb
The bounding box of all of this node's children.
int left
Left child.
Nodes for the acceleration structures of BVHs.
void render_mesh_aabbs_from_root(MeshNode *mn)
Adds Mesh AABBs for each MeshNode child recursively to DEBUG_LINES.
Definition render.c:517
void aabb_to_vertices(DebugVertex *out, AABB *aabb, vec3 offset)
Transfers an AABB to DebufVertex for a wireframe (GL_LINES).
Definition render.c:475
The main render pass, data GPU uploads, and SSBO management.
float c2
Definition types.h:20
float a0
Definition types.h:20
vec3 normal
Definition types.h:284
float a1
Definition types.h:20
float c0
Definition types.h:20
@ B
Definition types.h:26
@ A
Definition types.h:26
@ C
Definition types.h:26
float x
Definition types.h:12
float b0
Definition types.h:20
float max_z
Definition types.h:39
int edge1
Definition types.h:331
float b1
Definition types.h:20
size_t indices[3]
Definition types.h:283
vec3 p
Definition types.h:330
float y
Definition types.h:12
int edge0
Definition types.h:331
vec3 pos
Definition types.h:220
float z
Definition types.h:12
size_t count
Definition types.h:209
float min_z
Definition types.h:39
vec3 normal
Definition types.h:324
float b2
Definition types.h:20
size_t count
Definition types.h:323
void * buffer
Definition types.h:210
float a2
Definition types.h:20
float c1
Definition types.h:20
size_t indices[64]
Definition types.h:322
Definition types.h:218
Definition types.h:20
Definition types.h:21
Definition types.h:12
Definition types.h:13
bool vec3_equal(vec3 a, vec3 b, float eps)
Definition utilities.c:541
vec3 closest_point_segment(vec3 p, vec3 a, vec3 b)
Definition utilities.c:1244
bool aabbs_intersect(AABB *a, AABB *b)
Definition utilities.c:276
int clip_pts_against_plane(ClipVertex *in, int in_count, ClipVertex *out, vec3 plane_point, vec3 plane_normal)
Definition utilities.c:838
vec3 closest_point_triangle(vec3 p, vec3 a, vec3 b, vec3 c)
Definition utilities.c:1327
vec3 mat3_mul_vec3(mat3 m, vec3 v)
Definition utilities.h:498
void print_aabb(AABB *aabb)
Definition utilities.h:168
mat3 quat_to_mat3(const vec4 q)
Definition utilities.h:329
#define CAST_VEC(vec, type)
Definition utilities.h:134
vec4 quat_mul(vec4 a, vec4 b)
Definition utilities.h:384
mat4 compose_trs_from_vectors(vec3 pos, vec4 rot, vec3 scale)
Definition utilities.h:481
mat3 mat3_mul(mat3 M, mat3 N)
Definition utilities.h:559
vec3 mat4_mul_direction(const mat4 m, vec3 v)
Definition utilities.h:323
mat3 mat3_transpose(mat3 in)
Definition utilities.h:545
vec3 mat4_mul_point(mat4 m, vec3 v)
Definition utilities.h:313
vec3 vec4_to_vec3(vec4 v)
Definition utilities.h:165
vec4 quat_from_axis_angle(vec3 axis, float angle)
Definition utilities.h:361