SPH方法实现粒子与玻璃杯的碰撞

#include <STDLIB.H> #include <STRING.H> #include <MATH.H> #include <ASSERT.H> #include <CTYPE.H> #include "glg.h" #define EPSILON (Real)0.0000001 #define DEG2RAD (Real)3.14159265358979323846/180.0 const Real IDENTITY[16] = { GLG_ONE, GLG_ZERO, GLG_ZERO, GLG_ZERO, GLG_ZERO, GLG_ONE, GLG_ZERO, GLG_ZERO, GLG_ZERO, GLG_ZERO, GLG_ONE, GLG_ZERO, GLG_ZERO, GLG_ZERO, GLG_ZERO, GLG_ONE }; void vec3i_from_string(vector3i* v, const char* str) { int i; int n; int buf_i; int elem_index; char buf[200]; n = (int)strlen(str); assert(n < 200); buf_i = 0; elem_index = 0; for (i = 0; i < n; i++) { char ch = str[i]; if (isdigit(ch) || (ch == '-')) { buf[buf_i] = ch; buf_i++; } else if (ch == ',') { buf[buf_i] = 0; switch (elem_index) { case 0: v->x = atoi(buf); break; case 1: v->y = atoi(buf); break; //case 2: v->z = atof(buf); break; default: assert(GLG_FALSE); } buf_i = 0; elem_index++; } else if (ch == ';') { break; } } buf[buf_i] = 0; v->z = atoi(buf); } void vec3_from_string(vector3* v, const char* str) { int i; int n; int buf_i; int elem_index; char buf[200]; n = (int)strlen(str); assert(n < 200); buf_i = 0; elem_index = 0; for (i = 0; i < n; i++) { char ch = str[i]; if (isdigit(ch) || (ch == '.') || (ch == '-')) { buf[buf_i] = ch; buf_i++; } else if (ch == ',') { buf[buf_i] = 0; switch (elem_index) { case 0: v->x = (Real)atof(buf); break; case 1: v->y = (Real)atof(buf); break; //case 2: v->z = atof(buf); break; default: assert(GLG_FALSE); } buf_i = 0; elem_index++; } else if (ch == ';') { break; } } buf[buf_i] = 0; v->z = (Real)atof(buf); } void mat3_set(matrix3* m, Real e00, Real e01, Real e02, Real e10, Real e11, Real e12, Real e20, Real e21, Real e22) { MAT3(m, 0, 0) = e00; MAT3(m, 0, 1) = e01; MAT3(m, 0, 2) = e02; MAT3(m, 1, 0) = e10; MAT3(m, 1, 1) = e11; MAT3(m, 1, 2) = e12; MAT3(m, 2, 0) = e20; MAT3(m, 2, 1) = e21; MAT3(m, 2, 2) = e22; } void mat3_copy(matrix3* dst, const matrix3* src) { memcpy(dst, src, 9*sizeof(Real)); } void mat3_set_zero(matrix3* m) { memset(m, 0, 9*sizeof(Real)); } void mat3_set_identity(matrix3* m) { MAT3(m, 0, 0) = GLG_ONE; MAT3(m, 0, 1) = GLG_ZERO; MAT3(m, 0, 2) = GLG_ZERO; MAT3(m, 1, 0) = GLG_ZERO; MAT3(m, 1, 1) = GLG_ONE; MAT3(m, 1, 2) = GLG_ZERO; MAT3(m, 2, 0) = GLG_ZERO; MAT3(m, 2, 1) = GLG_ZERO; MAT3(m, 2, 2) = GLG_ONE; } void mat3_scale(matrix3* dst, Real s, matrix3* src) { int row; int col; for (row = 0; row < 3; row++) for (col = 0; col < 3; col++) MAT3(dst, row, col) = s*MAT3(src, row, col); } void mat3_set_rows(matrix3* m, const vector3* r0, const vector3* r1, const vector3* r2) { mat3_set(m, r0->x, r0->y, r0->z, r1->x, r1->y, r1->z, r2->x, r2->y, r2->z); } void mat3_add(matrix3* dst, const matrix3* m0, const matrix3* m1) { int row; int col; for (row = 0; row < 3; row++) for (col = 0; col < 3; col++) MAT3(dst, row, col) = MAT3(m0, row, col) + MAT3(m1, row, col); } void mat3_mul(matrix3* dst, const matrix3* m0, const matrix3* m1) { int row; int col; int i; matrix3 result; mat3_set_zero(&result); for (row = 0; row < 3; row++) for (col = 0; col < 3; col++) for (i = 0; i < 3; i++) MAT3(&result, row, col) += MAT3(m0, row, i)*MAT3(m1, i, col); mat3_copy(dst, &result); } void mat3_set_cols(matrix3* m, const vector3* c0, const vector3* c1, const vector3* c2) { mat3_set(m, c0->x, c1->x, c2->x, c0->y, c1->y, c2->y, c0->z, c1->z, c2->z); } void mat3_set_rotate(matrix3* m, Real angle, Real x, Real y, Real z) { Real xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c; // Real m[16]; GLG_BOOL optimized; s = (Real) sin( angle * DEG2RAD ); c = (Real) cos( angle * DEG2RAD ); mat3_set_identity(m); optimized = GLG_FALSE; if (x == GLG_ZERO) { if (y == GLG_ZERO) { if (z != GLG_ZERO) { optimized = GLG_TRUE; /* rotate only around z-axis */ MAT3(m, 0, 0) = c; MAT3(m, 1, 1) = c; if (z < 0.0F) { MAT3(m, 0, 1) = s; MAT3(m, 1, 0) = -s; } else { MAT3(m, 0, 1) = -s; MAT3(m, 1, 0) = s; } } } else if (z == 0.0F) { optimized = GLG_TRUE; /* rotate only around y-axis */ MAT3(m, 0, 0) = c; MAT3(m, 2, 2) = c; if (y < 0.0F) { MAT3(m, 0, 2) = -s; MAT3(m, 2, 0) = s; } else { MAT3(m, 0, 2) = s; MAT3(m, 2, 0) = -s; } } } else if (y == 0.0F) { if (z == 0.0F) { optimized = GLG_TRUE; /* rotate only around x-axis */ MAT3(m, 1, 1) = c; MAT3(m, 2, 2) = c; if (x < 0.0F) { MAT3(m, 1, 2) = s; MAT3(m, 2, 1) = -s; } else { MAT3(m, 1, 2) = -s; MAT3(m, 2, 1) = s; } } } if (!optimized) { const Real mag = glg_sqrt(x * x + y * y + z * z); if (mag <= 1.0e-4) { /* no rotation, leave mat as-is */ return; } x /= mag; y /= mag; z /= mag; xx = x * x; yy = y * y; zz = z * z; xy = x * y; yz = y * z; zx = z * x; xs = x * s; ys = y * s; zs = z * s; one_c = 1.0F - c; /* We already hold the identity-matrix so we can skip some statements */ MAT3(m, 0,0) = (one_c * xx) + c;