├── .gitignore ├── README.md ├── miniphysics.h └── tests └── bin └── DO_NOT_DELETE /.gitignore: -------------------------------------------------------------------------------- 1 | examples/out/ 2 | docs/private/ 3 | tests/x64/ 4 | tests/Debug/ 5 | tests/Release/ 6 | tests/bin/ 7 | !/tests/bin/DO_NOT_DELETE 8 | tests/res/private/ 9 | *.vcxproj.user 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | An experimental dynamics and collision detection library. Nothing interesting here yet, but these are the goals: 2 | 3 | * Single file 4 | * C89 5 | * Support for both fixed and floating point math 6 | * Collision detection independent of dynamics 7 | * Dynamics simulation 8 | - Rigid body to begin with, maybe soft body later -------------------------------------------------------------------------------- /miniphysics.h: -------------------------------------------------------------------------------- 1 | #ifndef miniphysics_h 2 | #define miniphysics_h 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include /* For size_t. */ 9 | 10 | /* Sized types. Prefer built-in types. Fall back to stdint. */ 11 | #ifdef _MSC_VER 12 | #if defined(__clang__) 13 | #pragma GCC diagnostic push 14 | #pragma GCC diagnostic ignored "-Wlanguage-extension-token" 15 | #pragma GCC diagnostic ignored "-Wlong-long" 16 | #pragma GCC diagnostic ignored "-Wc++11-long-long" 17 | #endif 18 | typedef signed __int8 mp_int8; 19 | typedef unsigned __int8 mp_uint8; 20 | typedef signed __int16 mp_int16; 21 | typedef unsigned __int16 mp_uint16; 22 | typedef signed __int32 mp_int32; 23 | typedef unsigned __int32 mp_uint32; 24 | typedef signed __int64 mp_int64; 25 | typedef unsigned __int64 mp_uint64; 26 | #if defined(__clang__) 27 | #pragma GCC diagnostic pop 28 | #endif 29 | #else 30 | #define MP_HAS_STDINT 31 | #include 32 | typedef int8_t mp_int8; 33 | typedef uint8_t mp_uint8; 34 | typedef int16_t mp_int16; 35 | typedef uint16_t mp_uint16; 36 | typedef int32_t mp_int32; 37 | typedef uint32_t mp_uint32; 38 | typedef int64_t mp_int64; 39 | typedef uint64_t mp_uint64; 40 | #endif 41 | 42 | #ifdef MP_HAS_STDINT 43 | typedef uintptr_t mp_uintptr; 44 | #else 45 | #if defined(_WIN32) 46 | #if defined(_WIN64) 47 | typedef mp_uint64 mp_uintptr; 48 | #else 49 | typedef mp_uint32 mp_uintptr; 50 | #endif 51 | #elif defined(__GNUC__) 52 | #if defined(__LP64__) 53 | typedef mp_uint64 mp_uintptr; 54 | #else 55 | typedef mp_uint32 mp_uintptr; 56 | #endif 57 | #else 58 | typedef mp_uint64 mp_uintptr; /* Fallback. */ 59 | #endif 60 | #endif 61 | 62 | typedef mp_uint8 mp_bool8; 63 | typedef mp_uint32 mp_bool32; 64 | #define MP_TRUE 1 65 | #define MP_FALSE 0 66 | 67 | typedef float mp_float32; 68 | typedef double mp_float64; 69 | typedef mp_uint32 mp_fixed32; /* 16.16 fixed point. */ 70 | typedef mp_uint64 mp_fixed64; /* 32.32 fixed point. */ 71 | 72 | typedef void* mp_handle; 73 | typedef void* mp_ptr; 74 | typedef void (* mp_proc)(void); 75 | 76 | /* Define NULL for some compilers. */ 77 | #ifndef NULL 78 | #define NULL 0 79 | #endif 80 | 81 | #if defined(SIZE_MAX) 82 | #define MP_SIZE_MAX SIZE_MAX 83 | #else 84 | #define MP_SIZE_MAX 0xFFFFFFFF /* When SIZE_MAX is not defined by the standard library just default to the maximum 32-bit unsigned integer. */ 85 | #endif 86 | 87 | 88 | #ifdef _MSC_VER 89 | #define MP_INLINE static __forceinline 90 | #elif defined(__GNUC__) 91 | /* 92 | I've had a bug report where GCC is emitting warnings about functions possibly not being inlineable. This warning happens when 93 | the __attribute__((always_inline)) attribute is defined without an "inline" statement. I think therefore there must be some 94 | case where "__inline__" is not always defined, thus the compiler emitting these warnings. When using -std=c89 or -ansi on the 95 | command line, we cannot use the "inline" keyword and instead need to use "__inline__". In an attempt to work around this issue 96 | I am using "__inline__" only when we're compiling in strict ANSI mode. 97 | */ 98 | #if defined(__STRICT_ANSI__) 99 | #define MP_INLINE static __inline__ __attribute__((always_inline)) 100 | #else 101 | #define MP_INLINE static inline __attribute__((always_inline)) 102 | #endif 103 | #else 104 | #define MP_INLINE static 105 | #endif 106 | 107 | 108 | typedef int mp_result; 109 | #define MP_SUCCESS 0 110 | #define MP_ERROR -1 /* A generic error. */ 111 | #define MP_INVALID_ARGS -2 112 | #define MP_INVALID_OPERATION -3 113 | #define MP_OUT_OF_MEMORY -4 114 | #define MP_OUT_OF_RANGE -5 115 | #define MP_ACCESS_DENIED -6 116 | #define MP_DOES_NOT_EXIST -7 117 | #define MP_ALREADY_EXISTS -8 118 | #define MP_TOO_MANY_OPEN_FILES -9 119 | #define MP_INVALID_FILE -10 120 | #define MP_TOO_BIG -11 121 | #define MP_PATH_TOO_LONG -12 122 | #define MP_NAME_TOO_LONG -13 123 | #define MP_NOT_DIRECTORY -14 124 | #define MP_IS_DIRECTORY -15 125 | #define MP_DIRECTORY_NOT_EMPTY -16 126 | #define MP_END_OF_FILE -17 127 | #define MP_NO_SPACE -18 128 | #define MP_BUSY -19 129 | #define MP_IO_ERROR -20 130 | #define MP_INTERRUPT -21 131 | #define MP_UNAVAILABLE -22 132 | #define MP_ALREADY_IN_USE -23 133 | #define MP_BAD_ADDRESS -24 134 | #define MP_BAD_SEEK -25 135 | #define MP_BAD_PIPE -26 136 | #define MP_DEADLOCK -27 137 | #define MP_TOO_MANY_LINKS -28 138 | #define MP_NOT_IMPLEMENTED -29 139 | #define MP_NO_MESSAGE -30 140 | #define MP_BAD_MESSAGE -31 141 | #define MP_NO_DATA_AVAILABLE -32 142 | #define MP_INVALID_DATA -33 143 | #define MP_TIMEOUT -34 144 | #define MP_NO_NETWORK -35 145 | #define MP_NOT_UNIQUE -36 146 | #define MP_NOT_SOCKET -37 147 | #define MP_NO_ADDRESS -38 148 | #define MP_BAD_PROTOCOL -39 149 | #define MP_PROTOCOL_UNAVAILABLE -40 150 | #define MP_PROTOCOL_NOT_SUPPORTED -41 151 | #define MP_PROTOCOL_FAMILY_NOT_SUPPORTED -42 152 | #define MP_ADDRESS_FAMILY_NOT_SUPPORTED -43 153 | #define MP_SOCKET_NOT_SUPPORTED -44 154 | #define MP_CONNECTION_RESET -45 155 | #define MP_ALREADY_CONNECTED -46 156 | #define MP_NOT_CONNECTED -47 157 | #define MP_CONNECTION_REFUSED -48 158 | #define MP_NO_HOST -49 159 | #define MP_IN_PROGRESS -50 160 | #define MP_CANCELLED -51 161 | #define MP_MEMORY_ALREADY_MAPPED -52 162 | #define MP_AT_END -53 163 | 164 | 165 | /********************************************************************************************************************** 166 | 167 | Math 168 | ==== 169 | 170 | **********************************************************************************************************************/ 171 | #include 172 | 173 | #define MP_FIXED32_SHIFT 16 174 | #define MP_FIXED64_SHIFT 32 175 | #define MP_FIXED32_ONE ((mp_fixed32)1 << MP_FIXED32_SHIFT) 176 | #define MP_FIXED64_ONE ((mp_fixed64)1 << MP_FIXED64_SHIFT) 177 | 178 | MP_INLINE mp_float32 mp_float32_from_fixed32(mp_fixed32 x) 179 | { 180 | return x / (mp_float32)MP_FIXED32_ONE; 181 | } 182 | MP_INLINE mp_float64 mp_float64_from_fixed64(mp_fixed64 x) 183 | { 184 | return x / (mp_float64)MP_FIXED64_ONE; 185 | } 186 | MP_INLINE mp_fixed32 mp_fixed32_from_float32(mp_float32 x) 187 | { 188 | return (mp_fixed32)(x * MP_FIXED32_ONE); 189 | } 190 | MP_INLINE mp_fixed64 mp_fixed64_from_float64(mp_float64 x) 191 | { 192 | return (mp_fixed64)(x * MP_FIXED64_ONE); 193 | } 194 | 195 | 196 | MP_INLINE mp_float32 mp_sqrtf32(mp_float32 x) 197 | { 198 | return sqrtf(x); 199 | } 200 | MP_INLINE mp_float64 mp_sqrtf64(mp_float64 x) 201 | { 202 | return sqrt(x); 203 | } 204 | 205 | MP_INLINE mp_float32 mp_sinf32(mp_float32 x) 206 | { 207 | return sinf(x); 208 | } 209 | MP_INLINE mp_float64 mp_sinf64(mp_float64 x) 210 | { 211 | return sin(x); 212 | } 213 | 214 | MP_INLINE mp_float32 mp_cosf32(mp_float32 x) 215 | { 216 | return cosf(x); 217 | } 218 | MP_INLINE mp_float64 mp_cosf64(mp_float64 x) 219 | { 220 | return cos(x); 221 | } 222 | 223 | MP_INLINE mp_float32 mp_tanf32(mp_float32 x) 224 | { 225 | return tanf(x); 226 | } 227 | MP_INLINE mp_float64 mp_tanf64(mp_float64 x) 228 | { 229 | return tan(x); 230 | } 231 | 232 | MP_INLINE mp_float32 mp_atanf32(mp_float32 x) 233 | { 234 | return atanf(x); 235 | } 236 | MP_INLINE mp_float64 mp_atanf64(mp_float64 x) 237 | { 238 | return atan(x); 239 | } 240 | 241 | MP_INLINE mp_float32 mp_fabsf(mp_float32 x) 242 | { 243 | return fabsf(x); 244 | } 245 | MP_INLINE mp_float64 mp_absf(mp_float64 x) 246 | { 247 | return fabs(x); 248 | } 249 | 250 | 251 | MP_INLINE mp_float32 mp_float32_sin(mp_float32 x) 252 | { 253 | return mp_sinf32(x); 254 | } 255 | MP_INLINE mp_float64 mp_float64_sin(mp_float64 x) 256 | { 257 | return mp_sinf64(x); 258 | } 259 | MP_INLINE mp_fixed32 mp_fixed32_sin(mp_fixed32 x) 260 | { 261 | /* TODO: Need a proper deterministic fixed point sin(). */ 262 | return mp_fixed32_from_float32(mp_float32_sin(mp_float32_from_fixed32(x))); 263 | } 264 | MP_INLINE mp_fixed64 mp_fixed64_sin(mp_fixed64 x) 265 | { 266 | /* TODO: Need a proper deterministic fixed point sin(). */ 267 | return mp_fixed64_from_float64(mp_float64_sin(mp_float64_from_fixed64(x))); 268 | } 269 | 270 | 271 | MP_INLINE mp_float32 mp_float32_cos(mp_float32 x) 272 | { 273 | return mp_cosf32(x); 274 | } 275 | MP_INLINE mp_float64 mp_float64_cos(mp_float64 x) 276 | { 277 | return mp_cosf64(x); 278 | } 279 | MP_INLINE mp_fixed32 mp_fixed32_cos(mp_fixed32 x) 280 | { 281 | /* TODO: Need a proper deterministic fixed point cos(). */ 282 | return mp_fixed32_from_float32(mp_float32_cos(mp_float32_from_fixed32(x))); 283 | } 284 | MP_INLINE mp_fixed64 mp_fixed64_cos(mp_fixed64 x) 285 | { 286 | /* TODO: Need a proper deterministic fixed point cos(). */ 287 | return mp_fixed64_from_float64(mp_float64_cos(mp_float64_from_fixed64(x))); 288 | } 289 | 290 | 291 | MP_INLINE mp_float32 mp_float32_from_int32(mp_int32 x) 292 | { 293 | return (mp_float32)x; 294 | } 295 | MP_INLINE mp_float64 mp_float64_from_int32(mp_int32 x) 296 | { 297 | return (mp_float64)x; 298 | } 299 | MP_INLINE mp_fixed32 mp_fixed32_from_int32(mp_int32 x) 300 | { 301 | return (mp_fixed32)x << MP_FIXED32_SHIFT; 302 | } 303 | MP_INLINE mp_fixed64 mp_fixed64_from_int32(mp_int32 x) 304 | { 305 | return (mp_fixed64)x << MP_FIXED64_SHIFT; 306 | } 307 | 308 | 309 | #define DEFINE_FUNCTION_add(type) MP_INLINE type type##_add(type x, type y) { return x + y; } 310 | DEFINE_FUNCTION_add(mp_float32) 311 | DEFINE_FUNCTION_add(mp_float64) 312 | DEFINE_FUNCTION_add(mp_fixed32) 313 | DEFINE_FUNCTION_add(mp_fixed64) 314 | 315 | #define DEFINE_FUNCTION_sub(type) MP_INLINE type type##_sub(type x, type y) { return x - y; } 316 | DEFINE_FUNCTION_sub(mp_float32) 317 | DEFINE_FUNCTION_sub(mp_float64) 318 | DEFINE_FUNCTION_sub(mp_fixed32) 319 | DEFINE_FUNCTION_sub(mp_fixed64) 320 | 321 | #define DEFINE_FUNCTION_div(type) MP_INLINE type type##_div(type x, type y) { return x / y; } 322 | DEFINE_FUNCTION_div(mp_float32) 323 | DEFINE_FUNCTION_div(mp_float64) 324 | DEFINE_FUNCTION_div(mp_fixed32) 325 | DEFINE_FUNCTION_div(mp_fixed64) 326 | 327 | MP_INLINE mp_float32 mp_float32_mul(mp_float32 x, mp_float32 y) 328 | { 329 | return x * y; 330 | } 331 | MP_INLINE mp_float64 mp_float64_mul(mp_float64 x, mp_float64 y) 332 | { 333 | return x * y; 334 | } 335 | MP_INLINE mp_fixed32 mp_fixed32_mul(mp_fixed32 x, mp_fixed32 y) 336 | { 337 | return (mp_fixed32)(((mp_int64)x * (mp_int64)y) >> MP_FIXED32_SHIFT); 338 | } 339 | MP_INLINE mp_fixed64 mp_fixed64_mul(mp_fixed64 x, mp_fixed64 y) 340 | { 341 | /* TODO: We need to do this manually since 128-bit integers are not standard. */ 342 | return (x * y) >> MP_FIXED64_SHIFT; 343 | } 344 | 345 | 346 | #define DECLARE_STRUCT_x2(type) \ 347 | typedef union \ 348 | { \ 349 | struct \ 350 | { \ 351 | type x; \ 352 | type y; \ 353 | }; \ 354 | type v[2]; \ 355 | } type##x2; 356 | 357 | DECLARE_STRUCT_x2(mp_float32) 358 | DECLARE_STRUCT_x2(mp_float64) 359 | DECLARE_STRUCT_x2(mp_fixed32) 360 | DECLARE_STRUCT_x2(mp_fixed64) 361 | 362 | 363 | #define DECLARE_STRUCT_x3(type) \ 364 | typedef union \ 365 | { \ 366 | struct \ 367 | { \ 368 | type x; \ 369 | type y; \ 370 | type z; \ 371 | }; \ 372 | type v[3]; \ 373 | } type##x3; 374 | 375 | DECLARE_STRUCT_x3(mp_float32) 376 | DECLARE_STRUCT_x3(mp_float64) 377 | DECLARE_STRUCT_x3(mp_fixed32) 378 | DECLARE_STRUCT_x3(mp_fixed64) 379 | 380 | 381 | #define DECLARE_STRUCT_x4(type) \ 382 | typedef union \ 383 | { \ 384 | struct \ 385 | { \ 386 | type x; \ 387 | type y; \ 388 | type z; \ 389 | type w; \ 390 | }; \ 391 | type v[4]; \ 392 | } type##x4; 393 | 394 | DECLARE_STRUCT_x4(mp_float32) 395 | DECLARE_STRUCT_x4(mp_float64) 396 | DECLARE_STRUCT_x4(mp_fixed32) 397 | DECLARE_STRUCT_x4(mp_fixed64) 398 | 399 | 400 | /* mp_float32x2 mp_float32x2f(mp_float32 x, mp_float32 y); */ 401 | #define DEFINE_FUNCTION_x2f(type) \ 402 | MP_INLINE type##x2 type##x2f(type x, type y) \ 403 | { \ 404 | type##x2 r; \ 405 | r.x = x; \ 406 | r.y = y; \ 407 | return r; \ 408 | } 409 | 410 | DEFINE_FUNCTION_x2f(mp_float32) 411 | DEFINE_FUNCTION_x2f(mp_float64) 412 | DEFINE_FUNCTION_x2f(mp_fixed32) 413 | DEFINE_FUNCTION_x2f(mp_fixed64) 414 | 415 | /* mp_float32x3 mp_float32x3f(mp_float32 x, mp_float32 y, mp_float32 z); */ 416 | #define DEFINE_FUNCTION_x3f(type) \ 417 | MP_INLINE type##x3 type##x3f(type x, type y, type z) \ 418 | { \ 419 | type##x3 r; \ 420 | r.x = x; \ 421 | r.y = y; \ 422 | r.z = z; \ 423 | return r; \ 424 | } 425 | 426 | DEFINE_FUNCTION_x3f(mp_float32) 427 | DEFINE_FUNCTION_x3f(mp_float64) 428 | DEFINE_FUNCTION_x3f(mp_fixed32) 429 | DEFINE_FUNCTION_x3f(mp_fixed64) 430 | 431 | /* mp_float32x4 mp_float32x4f(mp_float32 x, mp_float32 y, mp_float32 z, mp_float32 w); */ 432 | #define DEFINE_FUNCTION_x4f(type) \ 433 | MP_INLINE type##x4 type##x4f(type x, type y, type z, type w) \ 434 | { \ 435 | type##x4 r; \ 436 | r.x = x; \ 437 | r.y = y; \ 438 | r.z = z; \ 439 | r.w = w; \ 440 | return r; \ 441 | } 442 | 443 | DEFINE_FUNCTION_x4f(mp_float32) 444 | DEFINE_FUNCTION_x4f(mp_float64) 445 | DEFINE_FUNCTION_x4f(mp_fixed32) 446 | DEFINE_FUNCTION_x4f(mp_fixed64) 447 | 448 | 449 | #define DEFINE_FUNCTION_x2fv(type) MP_INLINE type##x2 type##x2fv(type* v) { return type##x2f(v[0], v[1]); } 450 | DEFINE_FUNCTION_x2fv(mp_float32) 451 | DEFINE_FUNCTION_x2fv(mp_float64) 452 | DEFINE_FUNCTION_x2fv(mp_fixed32) 453 | DEFINE_FUNCTION_x2fv(mp_fixed64) 454 | 455 | #define DEFINE_FUNCTION_x3fv(type) MP_INLINE type##x3 type##x3fv(type* v) { return type##x3f(v[0], v[1], v[2]); } 456 | DEFINE_FUNCTION_x3fv(mp_float32) 457 | DEFINE_FUNCTION_x3fv(mp_float64) 458 | DEFINE_FUNCTION_x3fv(mp_fixed32) 459 | DEFINE_FUNCTION_x3fv(mp_fixed64) 460 | 461 | #define DEFINE_FUNCTION_x4fv(type) MP_INLINE type##x4 type##x4fv(type* v) { return type##x4f(v[0], v[1], v[2], v[3]); } 462 | DEFINE_FUNCTION_x4fv(mp_float32) 463 | DEFINE_FUNCTION_x4fv(mp_float64) 464 | DEFINE_FUNCTION_x4fv(mp_fixed32) 465 | DEFINE_FUNCTION_x4fv(mp_fixed64) 466 | 467 | 468 | /* 469 | Returns a unit vector (1, 0) rotated by the specified angle in radians. 470 | */ 471 | #define DEFINE_FUNCTION_x2_from_angle(type) \ 472 | MP_INLINE type##x2 type##x2_from_angle(type angleInRadians) \ 473 | { \ 474 | return type##x2f(type##_cos(angleInRadians), type##_sin(angleInRadians)); \ 475 | } 476 | 477 | DEFINE_FUNCTION_x2_from_angle(mp_float32) 478 | DEFINE_FUNCTION_x2_from_angle(mp_float64) 479 | DEFINE_FUNCTION_x2_from_angle(mp_fixed32) 480 | DEFINE_FUNCTION_x2_from_angle(mp_fixed64) 481 | 482 | 483 | 484 | #define DEFINE_FUNCTION_x2_add(type) \ 485 | MP_INLINE type##x2 type##x2_add(type##x2 v0, type##x2 v1) \ 486 | { \ 487 | return type##x2f(type##_add(v0.x, v1.x), type##_add(v0.y, v1.y)); \ 488 | } 489 | 490 | DEFINE_FUNCTION_x2_add(mp_float32) 491 | DEFINE_FUNCTION_x2_add(mp_float64) 492 | DEFINE_FUNCTION_x2_add(mp_fixed32) 493 | DEFINE_FUNCTION_x2_add(mp_fixed64) 494 | 495 | #define DEFINE_FUNCTION_x3_add(type) \ 496 | MP_INLINE type##x3 type##x3_add(type##x3 v0, type##x3 v1) \ 497 | { \ 498 | return type##x3f(type##_add(v0.x, v1.x), type##_add(v0.y, v1.y), type##_add(v0.z, v1.z)); \ 499 | } 500 | 501 | DEFINE_FUNCTION_x3_add(mp_float32) 502 | DEFINE_FUNCTION_x3_add(mp_float64) 503 | DEFINE_FUNCTION_x3_add(mp_fixed32) 504 | DEFINE_FUNCTION_x3_add(mp_fixed64) 505 | 506 | #define DEFINE_FUNCTION_x4_add(type) \ 507 | MP_INLINE type##x4 type##x4_add(type##x4 v0, type##x4 v1) \ 508 | { \ 509 | return type##x4f(type##_add(v0.x, v1.x), type##_add(v0.y, v1.y), type##_add(v0.z, v1.z), type##_add(v0.w, v1.w)); \ 510 | } 511 | 512 | DEFINE_FUNCTION_x4_add(mp_float32) 513 | DEFINE_FUNCTION_x4_add(mp_float64) 514 | DEFINE_FUNCTION_x4_add(mp_fixed32) 515 | DEFINE_FUNCTION_x4_add(mp_fixed64) 516 | 517 | 518 | 519 | #define DEFINE_FUNCTION_x2_sub(type) \ 520 | MP_INLINE type##x2 type##x2_sub(type##x2 v0, type##x2 v1) \ 521 | { \ 522 | return type##x2f(type##_sub(v0.x, v1.x), type##_sub(v0.y, v1.y)); \ 523 | } 524 | 525 | DEFINE_FUNCTION_x2_sub(mp_float32) 526 | DEFINE_FUNCTION_x2_sub(mp_float64) 527 | DEFINE_FUNCTION_x2_sub(mp_fixed32) 528 | DEFINE_FUNCTION_x2_sub(mp_fixed64) 529 | 530 | #define DEFINE_FUNCTION_x3_sub(type) \ 531 | MP_INLINE type##x3 type##x3_sub(type##x3 v0, type##x3 v1) \ 532 | { \ 533 | return type##x3f(type##_sub(v0.x, v1.x), type##_sub(v0.y, v1.y), type##_sub(v0.z, v1.z)); \ 534 | } 535 | 536 | DEFINE_FUNCTION_x3_sub(mp_float32) 537 | DEFINE_FUNCTION_x3_sub(mp_float64) 538 | DEFINE_FUNCTION_x3_sub(mp_fixed32) 539 | DEFINE_FUNCTION_x3_sub(mp_fixed64) 540 | 541 | #define DEFINE_FUNCTION_x4_sub(type) \ 542 | MP_INLINE type##x4 type##x4_sub(type##x4 v0, type##x4 v1) \ 543 | { \ 544 | return type##x4f(type##_sub(v0.x, v1.x), type##_sub(v0.y, v1.y), type##_sub(v0.z, v1.z), type##_sub(v0.w, v1.w)); \ 545 | } 546 | 547 | DEFINE_FUNCTION_x4_sub(mp_float32) 548 | DEFINE_FUNCTION_x4_sub(mp_float64) 549 | DEFINE_FUNCTION_x4_sub(mp_fixed32) 550 | DEFINE_FUNCTION_x4_sub(mp_fixed64) 551 | 552 | 553 | 554 | #define DEFINE_FUNCTION_x2_mul(type) \ 555 | MP_INLINE type##x2 type##x2_mul(type##x2 v0, type##x2 v1) \ 556 | { \ 557 | return type##x2f(type##_mul(v0.x, v1.x), type##_mul(v0.y, v1.y)); \ 558 | } 559 | 560 | DEFINE_FUNCTION_x2_mul(mp_float32) 561 | DEFINE_FUNCTION_x2_mul(mp_float64) 562 | DEFINE_FUNCTION_x2_mul(mp_fixed32) 563 | DEFINE_FUNCTION_x2_mul(mp_fixed64) 564 | 565 | #define DEFINE_FUNCTION_x3_mul(type) \ 566 | MP_INLINE type##x3 type##x3_mul(type##x3 v0, type##x3 v1) \ 567 | { \ 568 | return type##x3f(type##_mul(v0.x, v1.x), type##_mul(v0.y, v1.y), type##_mul(v0.z, v1.z)); \ 569 | } 570 | 571 | DEFINE_FUNCTION_x3_mul(mp_float32) 572 | DEFINE_FUNCTION_x3_mul(mp_float64) 573 | DEFINE_FUNCTION_x3_mul(mp_fixed32) 574 | DEFINE_FUNCTION_x3_mul(mp_fixed64) 575 | 576 | #define DEFINE_FUNCTION_x4_mul(type) \ 577 | MP_INLINE type##x4 type##x4_mul(type##x4 v0, type##x4 v1) \ 578 | { \ 579 | return type##x4f(type##_mul(v0.x, v1.x), type##_mul(v0.y, v1.y), type##_mul(v0.z, v1.z), type##_mul(v0.w, v1.w)); \ 580 | } 581 | 582 | DEFINE_FUNCTION_x4_mul(mp_float32) 583 | DEFINE_FUNCTION_x4_mul(mp_float64) 584 | DEFINE_FUNCTION_x4_mul(mp_fixed32) 585 | DEFINE_FUNCTION_x4_mul(mp_fixed64) 586 | 587 | 588 | #define DEFINE_FUNCTION_x2_mul1(type) \ 589 | MP_INLINE type##x2 type##x2_mul1(type##x2 v, type a) \ 590 | { \ 591 | return type##x2f(type##_mul(v.x, a), type##_mul(v.y, a)); \ 592 | } 593 | 594 | DEFINE_FUNCTION_x2_mul1(mp_float32) 595 | DEFINE_FUNCTION_x2_mul1(mp_float64) 596 | DEFINE_FUNCTION_x2_mul1(mp_fixed32) 597 | DEFINE_FUNCTION_x2_mul1(mp_fixed64) 598 | 599 | #define DEFINE_FUNCTION_x3_mul1(type) \ 600 | MP_INLINE type##x3 type##x3_mul1(type##x3 v, type a) \ 601 | { \ 602 | return type##x3f(type##_mul(v.x, a), type##_mul(v.y, a), type##_mul(v.z, a)); \ 603 | } 604 | 605 | DEFINE_FUNCTION_x3_mul1(mp_float32) 606 | DEFINE_FUNCTION_x3_mul1(mp_float64) 607 | DEFINE_FUNCTION_x3_mul1(mp_fixed32) 608 | DEFINE_FUNCTION_x3_mul1(mp_fixed64) 609 | 610 | #define DEFINE_FUNCTION_x4_mul1(type) \ 611 | MP_INLINE type##x4 type##x4_mul1(type##x4 v, type a) \ 612 | { \ 613 | return type##x4f(type##_mul(v.x, a), type##_mul(v.y, a), type##_mul(v.z, a), type##_mul(v.z, a)); \ 614 | } 615 | 616 | DEFINE_FUNCTION_x4_mul1(mp_float32) 617 | DEFINE_FUNCTION_x4_mul1(mp_float64) 618 | DEFINE_FUNCTION_x4_mul1(mp_fixed32) 619 | DEFINE_FUNCTION_x4_mul1(mp_fixed64) 620 | 621 | 622 | 623 | #define DEFINE_FUNCTION_x2_div(type) \ 624 | MP_INLINE type##x2 type##x2_div(type##x2 v0, type##x2 v1) \ 625 | { \ 626 | return type##x2f(type##_div(v0.x, v1.x), type##_div(v0.y, v1.y)); \ 627 | } 628 | 629 | DEFINE_FUNCTION_x2_div(mp_float32) 630 | DEFINE_FUNCTION_x2_div(mp_float64) 631 | DEFINE_FUNCTION_x2_div(mp_fixed32) 632 | DEFINE_FUNCTION_x2_div(mp_fixed64) 633 | 634 | #define DEFINE_FUNCTION_x3_div(type) \ 635 | MP_INLINE type##x3 type##x3_div(type##x3 v0, type##x3 v1) \ 636 | { \ 637 | return type##x3f(type##_div(v0.x, v1.x), type##_div(v0.y, v1.y), type##_div(v0.z, v1.z)); \ 638 | } 639 | 640 | DEFINE_FUNCTION_x3_div(mp_float32) 641 | DEFINE_FUNCTION_x3_div(mp_float64) 642 | DEFINE_FUNCTION_x3_div(mp_fixed32) 643 | DEFINE_FUNCTION_x3_div(mp_fixed64) 644 | 645 | #define DEFINE_FUNCTION_x4_div(type) \ 646 | MP_INLINE type##x4 type##x4_div(type##x4 v0, type##x4 v1) \ 647 | { \ 648 | return type##x4f(type##_div(v0.x, v1.x), type##_div(v0.y, v1.y), type##_div(v0.z, v1.z), type##_div(v0.w, v1.w)); \ 649 | } 650 | 651 | DEFINE_FUNCTION_x4_div(mp_float32) 652 | DEFINE_FUNCTION_x4_div(mp_float64) 653 | DEFINE_FUNCTION_x4_div(mp_fixed32) 654 | DEFINE_FUNCTION_x4_div(mp_fixed64) 655 | 656 | 657 | 658 | 659 | MP_INLINE mp_float32 mp_float32x2_dot(mp_float32x2 v0, mp_float32x2 v1) 660 | { 661 | return v0.x*v1.x + v0.y*v1.y; 662 | } 663 | 664 | MP_INLINE mp_float32 mp_float32x3_dot(mp_float32x3 v0, mp_float32x3 v1) 665 | { 666 | return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z; 667 | } 668 | 669 | MP_INLINE mp_float32 mp_float32x4_dot(mp_float32x4 v0, mp_float32x4 v1) 670 | { 671 | return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z + v0.w*v1.w; 672 | } 673 | 674 | 675 | MP_INLINE mp_float32 mp_float32x2_length2(mp_float32x2 v) 676 | { 677 | return mp_float32x2_dot(v, v); 678 | } 679 | 680 | MP_INLINE mp_float32 mp_float32x3_length2(mp_float32x3 v) 681 | { 682 | return mp_float32x3_dot(v, v); 683 | } 684 | 685 | MP_INLINE mp_float32 mp_float32x4_length2(mp_float32x4 v) 686 | { 687 | return mp_float32x4_dot(v, v); 688 | } 689 | 690 | 691 | MP_INLINE mp_float32 mp_float32x2_length(mp_float32x2 v) 692 | { 693 | return mp_sqrtf32(mp_float32x2_length2(v)); 694 | } 695 | 696 | MP_INLINE mp_float32 mp_float32x3_length(mp_float32x3 v) 697 | { 698 | return mp_sqrtf32(mp_float32x3_length2(v)); 699 | } 700 | 701 | MP_INLINE mp_float32 mp_float32x4_length(mp_float32x4 v) 702 | { 703 | return mp_sqrtf32(mp_float32x4_length2(v)); 704 | } 705 | 706 | 707 | MP_INLINE mp_float32 mp_float32x2_distance2(mp_float32x2 v0, mp_float32x2 v1) 708 | { 709 | return mp_float32x2_length2(mp_float32x2_sub(v0, v1)); 710 | } 711 | 712 | MP_INLINE mp_float32 mp_float32x3_distance2(mp_float32x3 v0, mp_float32x3 v1) 713 | { 714 | return mp_float32x3_length2(mp_float32x3_sub(v0, v1)); 715 | } 716 | 717 | MP_INLINE mp_float32 mp_float32x4_distance2(mp_float32x4 v0, mp_float32x4 v1) 718 | { 719 | return mp_float32x4_length2(mp_float32x4_sub(v0, v1)); 720 | } 721 | 722 | 723 | MP_INLINE mp_float32 mp_float32x2_distance(mp_float32x2 v0, mp_float32x2 v1) 724 | { 725 | return mp_sqrtf32(mp_float32x2_distance2(v0, v1)); 726 | } 727 | 728 | MP_INLINE mp_float32 mp_float32x3_distance(mp_float32x3 v0, mp_float32x3 v1) 729 | { 730 | return mp_sqrtf32(mp_float32x3_distance2(v0, v1)); 731 | } 732 | 733 | MP_INLINE mp_float32 mp_float32x4_distance(mp_float32x4 v0, mp_float32x4 v1) 734 | { 735 | return mp_sqrtf32(mp_float32x4_distance2(v0, v1)); 736 | } 737 | 738 | 739 | MP_INLINE mp_float32x2 mp_float32x2_normalize(mp_float32x2 v) 740 | { 741 | return mp_float32x2_mul1(v, 1.0f / mp_float32x2_length(v)); 742 | } 743 | 744 | MP_INLINE mp_float32x3 mp_float32x3_normalize(mp_float32x3 v) 745 | { 746 | return mp_float32x3_mul1(v, 1.0f / mp_float32x3_length(v)); 747 | } 748 | 749 | MP_INLINE mp_float32x4 mp_float32x4_normalize(mp_float32x4 v) 750 | { 751 | return mp_float32x4_mul1(v, 1.0f / mp_float32x4_length(v)); 752 | } 753 | 754 | 755 | MP_INLINE mp_float32x2 mp_float32x2_rotate(mp_float32x2 v, const mp_float32 angleInRadians) 756 | { 757 | mp_float32 s = mp_sinf32(angleInRadians); 758 | mp_float32 c = mp_cosf32(angleInRadians); 759 | return mp_float32x2f( 760 | v.x*c - v.y*s, 761 | v.x*s + v.y*c 762 | ); 763 | } 764 | 765 | MP_INLINE mp_float32x2 mp_float32x2_perpendicular(mp_float32x2 v) 766 | { 767 | return mp_float32x2f(-v.y, v.x); 768 | } 769 | MP_INLINE mp_float32x2 mp_float32x2_perpendicular_rev(mp_float32x2 v) 770 | { 771 | return mp_float32x2f(v.y, -v.x); 772 | } 773 | 774 | MP_INLINE mp_float32 mp_float32x2_angle(mp_float32x2 v0, mp_float32x2 v1) 775 | { 776 | return mp_atanf32(v0.y / v0.x) - mp_atanf32(v1.y / v1.x); 777 | } 778 | 779 | 780 | 781 | #define DECLARE_STRUCT_x3x3(type) \ 782 | typedef union \ 783 | { \ 784 | type##x3 col[3]; \ 785 | } type##x3x3; 786 | 787 | DECLARE_STRUCT_x3x3(mp_float32) 788 | DECLARE_STRUCT_x3x3(mp_float64) 789 | DECLARE_STRUCT_x3x3(mp_fixed32) 790 | DECLARE_STRUCT_x3x3(mp_fixed64) 791 | 792 | #define DECLARE_STRUCT_x4x4(type) \ 793 | typedef union \ 794 | { \ 795 | type##x4 col[4]; \ 796 | } type##x4x4; 797 | 798 | DECLARE_STRUCT_x4x4(mp_float32) 799 | DECLARE_STRUCT_x4x4(mp_float64) 800 | DECLARE_STRUCT_x4x4(mp_fixed32) 801 | DECLARE_STRUCT_x4x4(mp_fixed64) 802 | 803 | 804 | 805 | /* 806 | Performs a ray/plane intersection test. 807 | 808 | Parameters 809 | ---------- 810 | `rayO` (in) 811 | The ray's origin. 812 | 813 | `rayD` (in) 814 | The ray's direction. 815 | 816 | `lineA` (in) 817 | The A in the plane equation. 818 | 819 | `lineB` (in) 820 | The B in the plane equation. 821 | 822 | `lineC` (in) 823 | The C in the plane equation. 824 | 825 | `pIntersectionPoint` (out) 826 | A pointer to the vector that will receive the intersection point, if any. 827 | 828 | Return Value 829 | ------------ 830 | True if the ray intersects with the plane; false otherwise. 831 | 832 | Remarks 833 | ------- 834 | A line is just a plane in 2D. It's equation is the following: 835 | 836 | `Ax + Bx + C = 0` 837 | 838 | Where `A` and `B` are the x and y coordinates of the line's normal, and `C` is the distance from the origin. Below are the mappings for each 839 | variable and their parameters for this function: 840 | 841 | `A` = `lineA` 842 | `B` = `lineB` 843 | `C` = `lineC` 844 | 845 | You do not need to pass in values for the `x` and `y` variables in the line equation above. This function will determine the values to use for 846 | these variables which are calculated based on the ray equation below: 847 | 848 | `R(t) = O + Dt` 849 | 850 | For each coordiate: 851 | 852 | `Rx(t) = O.x + D.x*t` 853 | `Ry(t) = O.y + D.y*t` 854 | 855 | Inserting the ray equation into the line equation: 856 | 857 | `A(Rx(t)) + B(Rx(t)) + C = 0` 858 | 859 | We can then solve this for `t`: 860 | 861 | ` 0 = A(O.x + D.x*t) + B(O.y + D.y*t) + C` 862 | ` -C = A(O.x + D.x*t) + B(O.y + D.y*t)` 863 | ` -C = AO.x + AD.x*t + BO.y + BD.y*t` 864 | ` -C - AO.x - BO.y = AD.x*t + BD.y*t` 865 | ` -(C + AO.x + BO.y) = t(AD.x + BD.y)` 866 | `-(C + AO.x + BO.y)/(AD.x + BD.y) = t` 867 | 868 | If `t` is < 0 then it means the point is behind the ray's origin and there is no intersection. 869 | */ 870 | MP_INLINE mp_bool32 mp_ray_line_intersection_float32x2(mp_float32x2 rayO, mp_float32x2 rayD, mp_float32 lineA, mp_float32 lineB, mp_float32 lineC, mp_float32x2* pIntersectionPoint) 871 | { 872 | mp_float32 t; 873 | 874 | t = lineA*rayD.x + lineB*rayD.y; 875 | if (t != 0) { 876 | t = -(lineC + lineA*rayO.x + lineB*rayO.y) / t; 877 | } else { 878 | return MP_FALSE; 879 | } 880 | 881 | if (t < 0) { 882 | return MP_FALSE; /* The line is behind the ray. */ 883 | } 884 | 885 | /* Intersects. */ 886 | if (pIntersectionPoint != NULL) { 887 | *pIntersectionPoint = mp_float32x2_add(rayO, mp_float32x2_mul1(rayD, t)); 888 | } 889 | 890 | return MP_TRUE; 891 | } 892 | 893 | 894 | /* 895 | Performs a ray/line segment intersection test. 896 | 897 | Return Value 898 | ------------ 899 | True if the ray intersects with the line segment; false otherwise. 900 | 901 | Remarks 902 | ------- 903 | To check the intersection of a ray and a line segment, all we need to do is get the intersection point of the ray and the line 904 | defined by the line segment and then compare the intersection point against the segment points. 905 | 906 | Below is how the line parameters are calculated: 907 | 908 | `N = ` 909 | 910 | 911 | If `pIntersectionPoint` is not null, this will receive the intersection point if the ray intersects the line. If the ray does 912 | not intersect with the line segment this is left unmodified. 913 | 914 | The distance between the ray's origin and the intersection point can be calculated using mp_float32x2_distance(). 915 | */ 916 | MP_INLINE mp_bool32 mp_ray_line_segment_intersection_float32x2(mp_float32x2 rayO, mp_float32x2 rayD, mp_float32x2 p0, mp_float32x2 p1, mp_float32x2* pIntersectionPoint) 917 | { 918 | mp_float32x2 lineIntersection; 919 | mp_float32x2 d; 920 | mp_float32x2 n; 921 | mp_float32 c; 922 | 923 | d = mp_float32x2_sub(p1, p0); 924 | if (d.x == 0 && d.y == 0) { 925 | return MP_FALSE; /* Line segment is of 0 length. */ 926 | } 927 | 928 | n = mp_float32x2f(-d.y, d.x); 929 | c = d.y*p0.x - d.x*p0.y; 930 | 931 | if (mp_ray_line_intersection_float32x2(rayO, rayD, n.x, n.y, c, &lineIntersection)) { 932 | mp_float32 t; 933 | 934 | if (p0.x != p1.x) { 935 | t = (lineIntersection.x - p0.x) / (p1.x - p0.x); 936 | } else { 937 | t = (lineIntersection.y - p0.y) / (p1.y - p0.y); 938 | } 939 | 940 | if (t < 0 || t > 1) { 941 | return MP_FALSE; 942 | } 943 | 944 | if (pIntersectionPoint != NULL) { 945 | *pIntersectionPoint = mp_float32x2_add(p0, mp_float32x2_mul1(mp_float32x2_sub(p1, p0), t)); 946 | } 947 | 948 | return MP_TRUE; 949 | } else { 950 | return MP_FALSE; 951 | } 952 | } 953 | 954 | /* 955 | Retrieves the closest point on a 2D plane to another point. 956 | */ 957 | MP_INLINE mp_float32x2 mp_closest_point_on_plane_to_point(mp_float32x2 p, mp_float32 planeA, mp_float32 planeB, mp_float32 planeC) 958 | { 959 | mp_float32x2 r; 960 | mp_float32 denom; 961 | 962 | denom = planeA*planeA + planeB*planeB; 963 | if (denom == 0) { 964 | return p; 965 | } 966 | 967 | r.x = (planeB * (planeB*p.x - planeA*p.y) - planeA*planeC) / denom; 968 | r.y = (planeA * (planeA*p.y - planeB*p.x) - planeB*planeC) / denom; 969 | 970 | return r; 971 | } 972 | 973 | /* 974 | Retrieves the distance of a point to a 2D plane. 975 | */ 976 | MP_INLINE mp_float32 mp_distance_point_to_plane(mp_float32x2 p, mp_float32 planeA, mp_float32 planeB, mp_float32 planeC) 977 | { 978 | return mp_fabsf(planeA*p.x + planeB*p.y + planeC) / mp_sqrtf32(planeA*planeA + planeB*planeB); 979 | } 980 | 981 | 982 | 983 | #if defined(MP_FIXED64) 984 | #define MP_USE_FIXED64 985 | #elif defined(MP_FIXED32) 986 | #define MP_USE_FIXED32 987 | #elif defined(MP_FLOAT64) 988 | #define MP_USE_FLOAT64 989 | #else 990 | #define MP_USE_FLOAT32 991 | #endif 992 | 993 | #if defined(MP_USE_FLOAT32) 994 | typedef mp_float32 mp_real; 995 | typedef mp_float32x2 mp_vec2; 996 | typedef mp_float32x3 mp_vec3; 997 | typedef mp_float32x4 mp_vec4; 998 | typedef mp_float32x3x3 mp_mat3; 999 | typedef mp_float32x4x4 mp_mat4; 1000 | #define mp_one 1.0f 1001 | #define mp_real_from_int32 mp_float32_from_int32 1002 | #define mp_vec2f mp_float32x2f 1003 | #define mp_vec3f mp_float32x3f 1004 | #define mp_vec4f mp_float32x4f 1005 | #define mp_vec2fv mp_float32x2fv 1006 | #define mp_vec3fv mp_float32x3fv 1007 | #define mp_vec4fv mp_float32x4fv 1008 | #define mp_add mp_float32_add 1009 | #define mp_vec2_add mp_float32x2_add 1010 | #define mp_vec3_add mp_float32x3_add 1011 | #define mp_vec4_add mp_float32x4_add 1012 | #define mp_sub mp_float32_sub 1013 | #define mp_vec2_sub mp_float32x2_sub 1014 | #define mp_vec3_sub mp_float32x3_sub 1015 | #define mp_vec4_sub mp_float32x4_sub 1016 | #define mp_mul mp_float32_mul 1017 | #define mp_vec2_mul mp_float32x2_mul 1018 | #define mp_vec3_mul mp_float32x3_mul 1019 | #define mp_vec4_mul mp_float32x4_mul 1020 | #define mp_vec2_mul1 mp_float32x2_mul1 1021 | #define mp_vec3_mul1 mp_float32x3_mul1 1022 | #define mp_vec4_mul1 mp_float32x4_mul1 1023 | #define mp_div mp_float32_div 1024 | #define mp_vec2_div mp_float32x2_div 1025 | #define mp_vec3_div mp_float32x3_div 1026 | #define mp_vec4_div mp_float32x4_div 1027 | #endif 1028 | #if defined(MP_USE_FLOAT64) 1029 | typedef mp_float64 mp_real; 1030 | typedef mp_float64x2 mp_vec2; 1031 | typedef mp_float64x3 mp_vec3; 1032 | typedef mp_float64x4 mp_vec4; 1033 | typedef mp_float64x3x3 mp_mat3; 1034 | typedef mp_float64x4x4 mp_mat4; 1035 | #define mp_one 1.0 1036 | #endif 1037 | #if defined(MP_USE_FIXED32) 1038 | typedef mp_fixed32 mp_real; 1039 | typedef mp_fixed32x2 mp_vec2; 1040 | typedef mp_fixed32x3 mp_vec3; 1041 | typedef mp_fixed32x4 mp_vec4; 1042 | typedef mp_fixed32x3x3 mp_mat3; 1043 | typedef mp_fixed32x4x4 mp_mat4; 1044 | #define mp_one MP_FIXED32_ONE 1045 | #endif 1046 | #if defined(MP_USE_FIXED64) 1047 | typedef mp_fixed64 mp_real; 1048 | typedef mp_fixed64x2 mp_vec2; 1049 | typedef mp_fixed64x3 mp_vec3; 1050 | typedef mp_fixed64x4 mp_vec4; 1051 | typedef mp_fixed64x3x3 mp_mat3; 1052 | typedef mp_fixed64x4x4 mp_mat4; 1053 | #define mp_one MP_FIXED64_ONE 1054 | #endif 1055 | 1056 | 1057 | /********************************************************************************************************************** 1058 | 1059 | Collision Detection 1060 | =================== 1061 | 1062 | **********************************************************************************************************************/ 1063 | #ifndef MP_NO_COLLISION 1064 | 1065 | typedef enum 1066 | { 1067 | ma_shape_type_sphere, 1068 | ma_shape_type_ellipsoid, 1069 | ma_shape_type_box 1070 | } ma_shape_type; 1071 | 1072 | typedef struct 1073 | { 1074 | ma_shape_type type; 1075 | union 1076 | { 1077 | struct 1078 | { 1079 | mp_real radius; 1080 | } sphere; 1081 | struct 1082 | { 1083 | mp_vec3 radius; 1084 | } ellipsoid; 1085 | struct 1086 | { 1087 | mp_vec3 dimensions; 1088 | } box; 1089 | } data; 1090 | } mp_shape; 1091 | 1092 | mp_result mp_sphere_init(mp_real radius, mp_shape* pShape); 1093 | mp_result mp_box_init(mp_vec3 dimensions, mp_shape* pShape); 1094 | 1095 | 1096 | typedef struct 1097 | { 1098 | mp_shape shape; 1099 | mp_vec3 position; 1100 | mp_mat3 rotation; 1101 | } mp_collision_object; 1102 | 1103 | mp_result mp_collision_object_init(mp_shape shape, mp_collision_object* pCollisionObject); 1104 | 1105 | 1106 | typedef struct 1107 | { 1108 | mp_real _unused; 1109 | } mp_collision_world_config; 1110 | 1111 | mp_collision_world_config mp_collision_world_config_init(); 1112 | 1113 | 1114 | typedef struct 1115 | { 1116 | mp_real _unusedX; 1117 | mp_vec3 _unusedV; 1118 | } mp_collision_world; 1119 | 1120 | mp_result mp_collision_world_init(const mp_collision_world_config* pConfig, mp_collision_world* pCollisionWorld); 1121 | void mp_collision_world_uninit(mp_collision_world* pCollisionWorld); 1122 | mp_result mp_collision_world_add_object(mp_collision_world* pCollisionWorld, mp_collision_object* pCollisionObject); 1123 | mp_result mp_collision_world_remove_object(mp_collision_world* pCollisionWorld, mp_collision_object* pCollisionObject); 1124 | 1125 | #endif /* MP_NO_COLLISION_DETECTION */ 1126 | 1127 | 1128 | /********************************************************************************************************************** 1129 | 1130 | Dynamics 1131 | ======== 1132 | 1133 | **********************************************************************************************************************/ 1134 | #ifndef MP_NO_DYNAMICS 1135 | 1136 | typedef struct 1137 | { 1138 | #ifndef MP_NO_COLLISION 1139 | mp_collision_world_config collision; 1140 | #endif 1141 | mp_real timestep; 1142 | mp_vec3 gravity; 1143 | } mp_dynamics_world_config; 1144 | 1145 | mp_dynamics_world_config mp_dynamics_world_config_init(); 1146 | 1147 | 1148 | 1149 | typedef struct 1150 | { 1151 | mp_vec3 position; /* World position. */ 1152 | mp_mat3 rotation; 1153 | mp_vec3 linVelocity; /* Linear velocity. */ 1154 | mp_vec3 angVelocity; /* Angular velocity. */ 1155 | mp_real mass; /* Static if mass = 0. */ 1156 | mp_bool32 isKinematic; 1157 | } mp_dynamics_body; 1158 | 1159 | typedef struct 1160 | { 1161 | #ifndef MP_NO_COLLISION 1162 | mp_collision_world collision; 1163 | #endif 1164 | mp_real dt; /* Used in ma_dynamics_world_step() to keep track of the delta time. */ 1165 | mp_real timestep; /* Our fixed step time. */ 1166 | mp_vec3 gravity; 1167 | mp_dynamics_body bodies[128]; /* Temp. Will be replaced with a dynamic array later. */ 1168 | mp_uint32 bodyCount; 1169 | } mp_dynamics_world; 1170 | 1171 | mp_result mp_dynamics_world_init(const mp_dynamics_world_config* pConfig, mp_dynamics_world* pDynamicsWorld); 1172 | void mp_dynamics_world_uninit(mp_dynamics_world* pDynamicsWorld); 1173 | void mp_dynamics_world_set_fixed_timestep(mp_dynamics_world* pDynamicsWorld, mp_real timestep); 1174 | mp_real mp_dynamics_world_get_fixed_timestep(mp_dynamics_world* pDynamicsWorld); 1175 | void mp_dynamics_world_step(mp_dynamics_world* pDynamicsWorld, mp_real dt); 1176 | void mp_dynamics_world_set_gravity(mp_dynamics_world* pDynamicsWorld, mp_vec3 gravity); 1177 | mp_vec3 mp_dynamics_world_get_gravity(mp_dynamics_world* pDynamicsWorld); 1178 | 1179 | void mp_dynamics_world_create_body(mp_dynamics_world* pDynamicsWorld, mp_dynamics_body** ppDynamicsBody); 1180 | void mp_dynamics_world_delete_body(mp_dynamics_world* pDynamicsWorld, mp_dynamics_body** ppDynamicsBody); 1181 | void mp_dynamics_world_insert_body(mp_dynamics_world* pDynamicsWorld, mp_dynamics_body** ppDynamicsBody); 1182 | void mp_dynamics_world_remove_body(mp_dynamics_world* pDynamicsWorld, mp_dynamics_body** ppDynamicsBody); 1183 | 1184 | #endif /* MP_NO_DYNAMICS */ 1185 | 1186 | 1187 | #ifdef __cplusplus 1188 | } 1189 | #endif 1190 | #endif /* miniphysics_h */ 1191 | 1192 | 1193 | 1194 | /********************************************************************************************************************** 1195 | *********************************************************************************************************************** 1196 | 1197 | Implementation 1198 | 1199 | *********************************************************************************************************************** 1200 | **********************************************************************************************************************/ 1201 | #if defined(MINIPHYSICS_IMPLEMENTATION) 1202 | 1203 | #include /* For malloc(), free() */ 1204 | #include /* For memset() */ 1205 | #include 1206 | 1207 | #ifndef MP_MALLOC 1208 | #define MP_MALLOC(sz) malloc((sz)) 1209 | #endif 1210 | #ifndef MP_REALLOC 1211 | #define MP_REALLOC(p, sz) realloc((p), (sz)) 1212 | #endif 1213 | #ifndef MP_FREE 1214 | #define MP_FREE(p) free((p)) 1215 | #endif 1216 | #ifndef MP_ZERO_MEMORY 1217 | #define MP_ZERO_MEMORY(p, sz) memset((p), 0, (sz)) 1218 | #endif 1219 | #ifndef MP_COPY_MEMORY 1220 | #define MP_COPY_MEMORY(dst, src, sz) memcpy((dst), (src), (sz)) 1221 | #endif 1222 | #ifndef MP_ASSERT 1223 | #define MP_ASSERT(condition) assert(condition) 1224 | #endif 1225 | 1226 | #define MP_ZERO_OBJECT(p) MP_ZERO_MEMORY((p), sizeof(*(p))) 1227 | 1228 | #define MP_COUNTOF(p) (sizeof(p) / sizeof((p)[0])) 1229 | #define MP_MAX(x, y) (((x) > (y)) ? (x) : (y)) 1230 | #define MP_MIN(x, y) (((x) < (y)) ? (x) : (y)) 1231 | #define MP_CLAMP(x, lo, hi) (MP_MAX(lo, MP_MIN(x, hi))) 1232 | #define MP_ABS(x) (((x) > 0) ? (x) : -(x)) 1233 | #define MP_OFFSET_PTR(p, offset) (((mp_uint8*)(p)) + (offset)) 1234 | 1235 | 1236 | 1237 | /********************************************************************************************************************** 1238 | 1239 | Collision Detection 1240 | =================== 1241 | 1242 | **********************************************************************************************************************/ 1243 | #ifndef MP_NO_COLLISION 1244 | 1245 | mp_result mp_sphere_init(mp_real radius, mp_shape* pShape) 1246 | { 1247 | if (pShape == NULL) { 1248 | return MP_INVALID_ARGS; 1249 | } 1250 | 1251 | pShape->type = ma_shape_type_sphere; 1252 | pShape->data.sphere.radius = radius; 1253 | 1254 | return MP_SUCCESS; 1255 | } 1256 | 1257 | mp_result mp_box_init(mp_vec3 dimensions, mp_shape* pShape) 1258 | { 1259 | if (pShape == NULL) { 1260 | return MP_INVALID_ARGS; 1261 | } 1262 | 1263 | pShape->type = ma_shape_type_box; 1264 | pShape->data.box.dimensions = dimensions; 1265 | 1266 | return MP_SUCCESS; 1267 | } 1268 | 1269 | 1270 | mp_collision_world_config mp_collision_world_config_init() 1271 | { 1272 | mp_collision_world_config config; 1273 | 1274 | MP_ZERO_OBJECT(&config); 1275 | config._unused = 0; 1276 | 1277 | return config; 1278 | } 1279 | 1280 | mp_result mp_collision_world_init(const mp_collision_world_config* pConfig, mp_collision_world* pCollisionWorld) 1281 | { 1282 | if (pCollisionWorld == NULL) { 1283 | return MP_INVALID_ARGS; 1284 | } 1285 | 1286 | MP_ZERO_OBJECT(pCollisionWorld); 1287 | 1288 | return MP_SUCCESS; 1289 | } 1290 | 1291 | void mp_collision_world_uninit(mp_collision_world* pCollisionWorld) 1292 | { 1293 | if (pCollisionWorld == NULL) { 1294 | return; 1295 | } 1296 | 1297 | /* TODO: Implement me. */ 1298 | } 1299 | 1300 | #endif 1301 | 1302 | 1303 | /********************************************************************************************************************** 1304 | 1305 | Dynamics 1306 | ======== 1307 | 1308 | **********************************************************************************************************************/ 1309 | #ifndef MP_NO_DYNAMICS 1310 | 1311 | mp_dynamics_world_config mp_dynamics_world_config_init() 1312 | { 1313 | mp_dynamics_world_config config; 1314 | 1315 | MP_ZERO_OBJECT(&config); 1316 | config.collision = mp_collision_world_config_init(); 1317 | config.timestep = mp_div(mp_one, 144); /* 144 Hz */ 1318 | config.gravity = mp_vec3f(0, -10 * mp_one, 0); 1319 | 1320 | return config; 1321 | } 1322 | 1323 | mp_result mp_dynamics_world_init(const mp_dynamics_world_config* pConfig, mp_dynamics_world* pDynamicsWorld) 1324 | { 1325 | mp_result result; 1326 | 1327 | if (pDynamicsWorld == NULL) { 1328 | return MP_INVALID_ARGS; 1329 | } 1330 | 1331 | MP_ZERO_OBJECT(pDynamicsWorld); 1332 | pDynamicsWorld->dt = 0; 1333 | pDynamicsWorld->timestep = pConfig->timestep; 1334 | pDynamicsWorld->gravity = pConfig->gravity; 1335 | 1336 | #ifndef MP_NO_COLLISION 1337 | result = mp_collision_world_init(&pConfig->collision, &pDynamicsWorld->collision); 1338 | if (result != MP_SUCCESS) { 1339 | return result; 1340 | } 1341 | #endif 1342 | 1343 | return MP_SUCCESS; 1344 | } 1345 | 1346 | void mp_dynamics_world_uninit(mp_dynamics_world* pDynamicsWorld) 1347 | { 1348 | if (pDynamicsWorld == NULL) { 1349 | return; 1350 | } 1351 | 1352 | #ifndef MP_NO_COLLISION 1353 | mp_collision_world_uninit(&pDynamicsWorld->collision); 1354 | #endif 1355 | } 1356 | 1357 | void mp_dynamics_world_set_fixed_timestep(mp_dynamics_world* pDynamicsWorld, mp_real timestep) 1358 | { 1359 | if (pDynamicsWorld == NULL) { 1360 | return; 1361 | } 1362 | 1363 | pDynamicsWorld->timestep = timestep; 1364 | } 1365 | 1366 | mp_real mp_dynamics_world_get_fixed_timestep(mp_dynamics_world* pDynamicsWorld) 1367 | { 1368 | if (pDynamicsWorld == NULL) { 1369 | return 0; 1370 | } 1371 | 1372 | return pDynamicsWorld->timestep; 1373 | } 1374 | 1375 | static void mp_dynamics_world_step_fixed(mp_dynamics_world* pDynamicsWorld) 1376 | { 1377 | mp_uint32 iBody; 1378 | mp_vec3 gravityStep; 1379 | 1380 | MP_ASSERT(pDynamicsWorld != NULL); 1381 | 1382 | gravityStep = mp_vec3_mul1(pDynamicsWorld->gravity, pDynamicsWorld->timestep); 1383 | 1384 | for (iBody = 0; iBody < pDynamicsWorld->bodyCount; iBody += 1) { 1385 | mp_dynamics_body* pBody = &pDynamicsWorld->bodies[iBody]; 1386 | if (pBody->isKinematic) { 1387 | /* Kinematic body. */ 1388 | } else { 1389 | if (pBody->mass > 0) { 1390 | /* Apply gravity. */ 1391 | pBody->linVelocity = mp_vec3_add(pBody->linVelocity, gravityStep); 1392 | 1393 | /* Apply linear velocity. */ 1394 | pDynamicsWorld->bodies[iBody].position = mp_vec3_add(pDynamicsWorld->bodies[iBody].position, pDynamicsWorld->bodies[iBody].linVelocity); 1395 | 1396 | /* TODO: Apply angular velocity. */ 1397 | } else { 1398 | /* It's a static body. */ 1399 | } 1400 | } 1401 | } 1402 | } 1403 | 1404 | void mp_dynamics_world_step(mp_dynamics_world* pDynamicsWorld, mp_real dt) 1405 | { 1406 | if (pDynamicsWorld == NULL) { 1407 | return; 1408 | } 1409 | 1410 | /* We need to do multiple steps, depending on `dt` and our fixed timestep. For stability, we can only update the physics simulation based on the fixed timestep. */ 1411 | pDynamicsWorld->dt = mp_add(pDynamicsWorld->dt, dt); 1412 | 1413 | while (pDynamicsWorld->dt >= pDynamicsWorld->timestep) { 1414 | mp_dynamics_world_step_fixed(pDynamicsWorld); 1415 | pDynamicsWorld->dt = mp_sub(pDynamicsWorld->dt, pDynamicsWorld->timestep); 1416 | } 1417 | } 1418 | 1419 | void mp_dynamics_world_set_gravity(mp_dynamics_world* pDynamicsWorld, mp_vec3 gravity) 1420 | { 1421 | if (pDynamicsWorld == NULL) { 1422 | return; 1423 | } 1424 | 1425 | pDynamicsWorld->gravity = gravity; 1426 | } 1427 | 1428 | mp_vec3 mp_dynamics_world_get_gravity(mp_dynamics_world* pDynamicsWorld) 1429 | { 1430 | if (pDynamicsWorld == NULL) { 1431 | return mp_vec3f(0, 0, 0); 1432 | } 1433 | 1434 | return pDynamicsWorld->gravity; 1435 | } 1436 | 1437 | #endif 1438 | 1439 | #endif /* MINIPHYSICS_IMPLEMENTATION */ -------------------------------------------------------------------------------- /tests/bin/DO_NOT_DELETE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mackron/miniphysics/99ed8807bc1da443a67963ad9800cf5e9f9f32f3/tests/bin/DO_NOT_DELETE --------------------------------------------------------------------------------