glg.c
来自「游戏编程精粹６第中关于粒子的实时流体仿真系统,对入门的游戏开发者很有帮助．」· C语言 代码 · 共 938 行 · 第 1/2 页
C
938 行
/* rotate only around y-axis */
MAT4(m, 0, 0) = c;
MAT4(m, 2, 2) = c;
if (y < 0.0F)
{
MAT4(m, 0, 2) = -s;
MAT4(m, 2, 0) = s;
}
else
{
MAT4(m, 0, 2) = s;
MAT4(m, 2, 0) = -s;
}
}
}
else if (y == 0.0F)
{
if (z == 0.0F)
{
optimized = GLG_TRUE;
/* rotate only around x-axis */
MAT4(m, 1, 1) = c;
MAT4(m, 2, 2) = c;
if (x < 0.0F)
{
MAT4(m, 1, 2) = s;
MAT4(m, 2, 1) = -s;
}
else
{
MAT4(m, 1, 2) = -s;
MAT4(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 */
MAT4(m, 0,0) = (one_c * xx) + c;
MAT4(m, 0,1) = (one_c * xy) - zs;
MAT4(m, 0,2) = (one_c * zx) + ys;
/* M(0,3) = 0.0F;
*/
MAT4(m, 1, 0) = (one_c * xy) + zs;
MAT4(m, 1,1) = (one_c * yy) + c;
MAT4(m, 1,2) = (one_c * yz) - xs;
/* M(1,3) = 0.0F;
*/
MAT4(m, 2,0) = (one_c * zx) - ys;
MAT4(m, 2,1) = (one_c * yz) + xs;
MAT4(m, 2,2) = (one_c * zz) + c;
/* M(2,3) = 0.0F;
*/
/*
MAT4(m, 3,0) = 0.0F;
MAT4(m, 3,1) = 0.0F;
MAT4(m, 3,2) = 0.0F;
MAT4(m, 3,3) = 1.0F;
*/
}
}
void mat4_mul(matrix4* dst, const matrix4* m0, const matrix4* m1)
{
int row;
int col;
int i;
matrix4 result;
mat4_set_zero(&result);
for (row = 0; row < 4; row++)
for (col = 0; col < 4; col++)
for (i = 0; i < 4; i++)
MAT4(&result, row, col) += MAT4(m0, row, i)*MAT4(m1, i, col);
mat4_copy(dst, &result);
}
void mat4_set_rotate_around(matrix4* dst, Real angle, const vector3* v){ Real s; Real c; Real _c; Real x; Real y; Real z; s = (Real)sin(angle*DEG2RAD); c = (Real)cos(angle*DEG2RAD); _c = 1 - c; x = v->x; y = v->y; z = v->z; mat4_set(dst, c + x*x*_c, x*y*_c - z*s, x*z*_c + y*s, 0, x*y*_c + z*s, c + y*y*_c, y*z*_c - x*s, 0, x*z*_c - y*s, y*z*_c + x*s, c + z*z*_c, 0, 0, 0, 0, 1);}
void mat4_mulr_rotate(matrix4* dst, Real angle, Real x, Real y, Real z)
{
matrix4 tmp;
matrix4 result;
mat4_set_rotate(&tmp, angle, x, y, z);
mat4_mul(&result, dst, &tmp);
mat4_copy(dst, &result);
}
void mat4_mull_rotate(matrix4* dst, Real angle, Real x, Real y, Real z)
{
matrix4 tmp;
matrix4 result;
mat4_set_rotate(&tmp, angle, x, y, z);
mat4_mul(&result, &tmp, dst);
mat4_copy(dst, &result);
}
void mat4_translate(matrix4* dst, Real x, Real y, Real z){ matrix4 tmp;
matrix4 result;
mat4_set_translate(&tmp, x, y, z);
mat4_mul(&result, dst, &tmp);
mat4_copy(dst, &result);}
void mat4_invert(matrix4* dst, const matrix4* m)
{
Real d = mat4_determinant(m);
Real r;
matrix4 ret;
if (d == 0)
return;
r = 1/d;
MAT4(&ret, 0, 0) = r*(MAT4(m, 1, 1)*(MAT4(m, 2, 2)*MAT4(m, 3, 3) - MAT4(m, 2, 3)*MAT4(m, 3, 2)) + MAT4(m, 1, 2)*(MAT4(m, 2, 3)*MAT4(m, 3, 1) - MAT4(m, 2, 1)*MAT4(m, 3, 3)) + MAT4(m, 1, 3)*(MAT4(m, 2, 1)*MAT4(m, 3, 2) - MAT4(m, 2, 2)*MAT4(m, 3, 1)));
MAT4(&ret, 0, 1) = r*(MAT4(m, 2, 1)*(MAT4(m, 0, 2)*MAT4(m, 3, 3) - MAT4(m, 0, 3)*MAT4(m, 3, 2)) + MAT4(m, 2, 2)*(MAT4(m, 0, 3)*MAT4(m, 3, 1) - MAT4(m, 0, 1)*MAT4(m, 3, 3)) + MAT4(m, 2, 3)*(MAT4(m, 0, 1)*MAT4(m, 3, 2) - MAT4(m, 0, 2)*MAT4(m, 3, 1)));
MAT4(&ret, 0, 2) = r*(MAT4(m, 3, 1)*(MAT4(m, 0, 2)*MAT4(m, 1, 3) - MAT4(m, 0, 3)*MAT4(m, 1, 2)) + MAT4(m, 3, 2)*(MAT4(m, 0, 3)*MAT4(m, 1, 1) - MAT4(m, 0, 1)*MAT4(m, 1, 3)) + MAT4(m, 3, 3)*(MAT4(m, 0, 1)*MAT4(m, 1, 2) - MAT4(m, 0, 2)*MAT4(m, 1, 1)));
MAT4(&ret, 0, 3) = r*(MAT4(m, 0, 1)*(MAT4(m, 1, 3)*MAT4(m, 2, 2) - MAT4(m, 1, 2)*MAT4(m, 2, 3)) + MAT4(m, 0, 2)*(MAT4(m, 1, 1)*MAT4(m, 2, 3) - MAT4(m, 1, 3)*MAT4(m, 2, 1)) + MAT4(m, 0, 3)*(MAT4(m, 1, 2)*MAT4(m, 2, 1) - MAT4(m, 1, 1)*MAT4(m, 2, 2)));
MAT4(&ret, 1, 0) = r*(MAT4(m, 1, 2)*(MAT4(m, 2, 0)*MAT4(m, 3, 3) - MAT4(m, 2, 3)*MAT4(m, 3, 0)) + MAT4(m, 1, 3)*(MAT4(m, 2, 2)*MAT4(m, 3, 0) - MAT4(m, 2, 0)*MAT4(m, 3, 2)) + MAT4(m, 1, 0)*(MAT4(m, 2, 3)*MAT4(m, 3, 2) - MAT4(m, 2, 2)*MAT4(m, 3, 3)));
MAT4(&ret, 1, 1) = r*(MAT4(m, 2, 2)*(MAT4(m, 0, 0)*MAT4(m, 3, 3) - MAT4(m, 0, 3)*MAT4(m, 3, 0)) + MAT4(m, 2, 3)*(MAT4(m, 0, 2)*MAT4(m, 3, 0) - MAT4(m, 0, 0)*MAT4(m, 3, 2)) + MAT4(m, 2, 0)*(MAT4(m, 0, 3)*MAT4(m, 3, 2) - MAT4(m, 0, 2)*MAT4(m, 3, 3)));
MAT4(&ret, 1, 2) = r*(MAT4(m, 3, 2)*(MAT4(m, 0, 0)*MAT4(m, 1, 3) - MAT4(m, 0, 3)*MAT4(m, 1, 0)) + MAT4(m, 3, 3)*(MAT4(m, 0, 2)*MAT4(m, 1, 0) - MAT4(m, 0, 0)*MAT4(m, 1, 2)) + MAT4(m, 3, 0)*(MAT4(m, 0, 3)*MAT4(m, 1, 2) - MAT4(m, 0, 2)*MAT4(m, 1, 3)));
MAT4(&ret, 1, 3) = r*(MAT4(m, 0, 2)*(MAT4(m, 1, 3)*MAT4(m, 2, 0) - MAT4(m, 1, 0)*MAT4(m, 2, 3)) + MAT4(m, 0, 3)*(MAT4(m, 1, 0)*MAT4(m, 2, 2) - MAT4(m, 1, 2)*MAT4(m, 2, 0)) + MAT4(m, 0, 0)*(MAT4(m, 1, 2)*MAT4(m, 2, 3) - MAT4(m, 1, 3)*MAT4(m, 2, 2)));
MAT4(&ret, 2, 0) = r*(MAT4(m, 1, 3)*(MAT4(m, 2, 0)*MAT4(m, 3, 1) - MAT4(m, 2, 1)*MAT4(m, 3, 0)) + MAT4(m, 1, 0)*(MAT4(m, 2, 1)*MAT4(m, 3, 3) - MAT4(m, 2, 3)*MAT4(m, 3, 1)) + MAT4(m, 1, 1)*(MAT4(m, 2, 3)*MAT4(m, 3, 0) - MAT4(m, 2, 0)*MAT4(m, 3, 3)));
MAT4(&ret, 2, 1) = r*(MAT4(m, 2, 3)*(MAT4(m, 0, 0)*MAT4(m, 3, 1) - MAT4(m, 0, 1)*MAT4(m, 3, 0)) + MAT4(m, 2, 0)*(MAT4(m, 0, 1)*MAT4(m, 3, 3) - MAT4(m, 0, 3)*MAT4(m, 3, 1)) + MAT4(m, 2, 1)*(MAT4(m, 0, 3)*MAT4(m, 3, 0) - MAT4(m, 0, 0)*MAT4(m, 3, 3)));
MAT4(&ret, 2, 2) = r*(MAT4(m, 3, 3)*(MAT4(m, 0, 0)*MAT4(m, 1, 1) - MAT4(m, 0, 1)*MAT4(m, 1, 0)) + MAT4(m, 3, 0)*(MAT4(m, 0, 1)*MAT4(m, 1, 3) - MAT4(m, 0, 3)*MAT4(m, 1, 1)) + MAT4(m, 3, 1)*(MAT4(m, 0, 3)*MAT4(m, 1, 0) - MAT4(m, 0, 0)*MAT4(m, 1, 3)));
MAT4(&ret, 2, 3) = r*(MAT4(m, 0, 3)*(MAT4(m, 1, 1)*MAT4(m, 2, 0) - MAT4(m, 1, 0)*MAT4(m, 2, 1)) + MAT4(m, 0, 0)*(MAT4(m, 1, 3)*MAT4(m, 2, 1) - MAT4(m, 1, 1)*MAT4(m, 2, 3)) + MAT4(m, 0, 1)*(MAT4(m, 1, 0)*MAT4(m, 2, 3) - MAT4(m, 1, 3)*MAT4(m, 2, 0)));
MAT4(&ret, 3, 0) = r*(MAT4(m, 1, 0)*(MAT4(m, 2, 2)*MAT4(m, 3, 1) - MAT4(m, 2, 1)*MAT4(m, 3, 2)) + MAT4(m, 1, 1)*(MAT4(m, 2, 0)*MAT4(m, 3, 2) - MAT4(m, 2, 2)*MAT4(m, 3, 0)) + MAT4(m, 1, 2)*(MAT4(m, 2, 1)*MAT4(m, 3, 0) - MAT4(m, 2, 0)*MAT4(m, 3, 1)));
MAT4(&ret, 3, 1) = r*(MAT4(m, 2, 0)*(MAT4(m, 0, 2)*MAT4(m, 3, 1) - MAT4(m, 0, 1)*MAT4(m, 3, 2)) + MAT4(m, 2, 1)*(MAT4(m, 0, 0)*MAT4(m, 3, 2) - MAT4(m, 0, 2)*MAT4(m, 3, 0)) + MAT4(m, 2, 2)*(MAT4(m, 0, 1)*MAT4(m, 3, 0) - MAT4(m, 0, 0)*MAT4(m, 3, 1)));
MAT4(&ret, 3, 2) = r*(MAT4(m, 3, 0)*(MAT4(m, 0, 2)*MAT4(m, 1, 1) - MAT4(m, 0, 1)*MAT4(m, 1, 2)) + MAT4(m, 3, 1)*(MAT4(m, 0, 0)*MAT4(m, 1, 2) - MAT4(m, 0, 2)*MAT4(m, 1, 0)) + MAT4(m, 3, 2)*(MAT4(m, 0, 1)*MAT4(m, 1, 0) - MAT4(m, 0, 0)*MAT4(m, 1, 1)));
MAT4(&ret, 3, 3) = r*(MAT4(m, 0, 0)*(MAT4(m, 1, 1)*MAT4(m, 2, 2) - MAT4(m, 1, 2)*MAT4(m, 2, 1)) + MAT4(m, 0, 1)*(MAT4(m, 1, 2)*MAT4(m, 2, 0) - MAT4(m, 1, 0)*MAT4(m, 2, 2)) + MAT4(m, 0, 2)*(MAT4(m, 1, 0)*MAT4(m, 2, 1) - MAT4(m, 1, 1)*MAT4(m, 2, 0)));
mat4_copy(dst, &ret);
}
void mat4_transpose(matrix4* dst, const matrix4* m)
{
matrix4 result;
int row;
int col;
for (row = 0; row < 4; row++)
for (col = 0; col < 4; col++)
MAT4(&result, row, col) = MAT4(m, col, row);
mat4_copy(dst, &result);
}
Real mat4_determinant(const matrix4* m)
{
return (MAT4(m, 0, 0)*MAT4(m, 1, 1) - MAT4(m, 0, 1)*MAT4(m, 1, 0))
*(MAT4(m, 2, 2)*MAT4(m, 3, 3) - MAT4(m, 2, 3)*MAT4(m, 3, 2))
-(MAT4(m, 0, 0)*MAT4(m, 1, 2) - MAT4(m, 0, 2)*MAT4(m, 1, 0))
*(MAT4(m, 2, 1)*MAT4(m, 3, 3) - MAT4(m, 2, 3)*MAT4(m, 3, 1))
+(MAT4(m, 0, 0)*MAT4(m, 1, 3) - MAT4(m, 0, 3)*MAT4(m, 1, 0))
*(MAT4(m, 2, 1)*MAT4(m, 3, 2) - MAT4(m, 2, 2)*MAT4(m, 3, 1))
+(MAT4(m, 0, 1)*MAT4(m, 1, 2) - MAT4(m, 0, 2)*MAT4(m, 1, 1))
*(MAT4(m, 2, 0)*MAT4(m, 3, 3) - MAT4(m, 2, 3)*MAT4(m, 3, 0))
-(MAT4(m, 0, 1)*MAT4(m, 1, 3) - MAT4(m, 0, 3)*MAT4(m, 1, 1))
*(MAT4(m, 2, 0)*MAT4(m, 3, 2) - MAT4(m, 2, 2)*MAT4(m, 3, 0))
+(MAT4(m, 0, 2)*MAT4(m, 1, 3) - MAT4(m, 0, 3)*MAT4(m, 1, 2))
*(MAT4(m, 2, 0)*MAT4(m, 3, 1) - MAT4(m, 2, 1)*MAT4(m, 3, 0));
}
void mat4_dump(const matrix4* m)
{
printf(" [ %f %f %f %f ]\n", m->m[0], m->m[4], m->m[8], m->m[12]);
printf(" [ %f %f %f %f ]\n", m->m[1], m->m[5], m->m[9], m->m[13]);
printf(" [ %f %f %f %f ]\n", m->m[2], m->m[6], m->m[10], m->m[14]);
printf(" [ %f %f %f %f ]\n", m->m[3], m->m[7], m->m[11], m->m[15]);
}
Real distsq_tri_point(const vector3* v0,
const vector3* v1,
const vector3* v2,
const vector3* p)
{
/* refer to http://www.magic-software.com/Distance.html */
vector3 e0;
vector3 e1;
vector3 temp;
Real a;
Real b;
Real c;
Real d;
Real e;
Real f;
Real det;
Real inv_det;
Real s;
Real t;
Real numer;
Real denom;
Real tmp0;
Real tmp1;
vec3_sub(&e0, v1, v0);
vec3_sub(&e1, v2, v0);
a = vec3_dot(&e0, &e0);
b = vec3_dot(&e0, &e1);
c = vec3_dot(&e1, &e1);
vec3_sub(&temp, v0, p);
d = vec3_dot(&e0, &temp);
e = vec3_dot(&e1, &temp);
f = vec3_dot(&temp, &temp);
det = a*c - b*b;
s = b*e - c*d;
t = b*d - a*e;
if ((s + t) <= det)
{
if (s < 0)
{
if (t < 0)
{
/* region 4 */
if (d < 0.0f)
{
t = 0;
s = ((-d >= a) ? 1 : -d/a);
}
else
{
s = 0;
t = ((e >= 0) ? 0 : ((-e >= c) ? 1 : -e/c));
}
}
else
{
/* region 3 */
s = 0;
t = (e >= 0 ? 0 : (-e >= c ? 1 : -e/c));
}
}
else if (t < 0)
{
/* region 5 */
s = (d >= 0 ? 0 : (-d >= a ? 1 : -d/a));
t = 0;
}
else
{
/* region 0 */
inv_det = 1/det;
s *= inv_det;
t *= inv_det;
}
}
else
{
if (s < 0)
{
/* region 2 */
tmp0 = b + d;
tmp1 = c + e;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a - 2*b + c;
s = (numer >= denom ? 1 : numer/denom);
t = 1 - s;
}
else
{
s = 0;
t = (tmp1 <= 0 ? 1 : (e >= 0 ? 0 : -e/c));
}
}
else if (t < 0)
{
/* region 6 */
tmp0 = b + e;
tmp1 = a + d;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a - 2*b + c;
t = (numer >= denom ? 1 : numer/denom);
s = 1 - t;
}
else
{
t = 0;
s = (tmp1 <= 0 ? 1 : (d >= 0 ? 0 : -d/a));
}
}
else
{
/* region 1 */
numer = (c + e - b - d);
if (numer <= 0)
{
s = 0;
}
else
{
denom = a - 2*b + c;
s = (numer >= denom ? 1 : numer/denom);
}
t = 1 - s;
}
}
vec3_copy(&temp, v0);
vec3_scaleadd(&temp, &temp, s, &e0);
vec3_scaleadd(&temp, &temp, t, &e1);
return vec3_distsq(&temp, p);
}
Real dist_tri_point(const vector3* v0,
const vector3* v1,
const vector3* v2,
const vector3* p)
{
return glg_sqrt(distsq_tri_point(v0, v1, v2, p));
}
Real dist_tri_point2(const vector3* v0,
const vector3* v1,
const vector3* v2,
const vector3* p)
{
/*
vector3 e0;
vector3 e1;
vector3 tri_n;
vector3 p_tri;
vector3 org_p;
vec3_sub(&e0, v1, v0);
vec3_sub(&e1, v2, v0);
vec3_cross(&tri_n, &e0, &e1);
vec3_normalize(&tri_n);
vec3_sub(&org_p, p, v0);
vec3_set(&p_tri, vec3_dot(&e0, &org_p), vec3_dot(&e1, &org_p), vec3_dot(&tri_n, &org_p));
if ((p_tri.x > 0.0) && (p_tri.y > 0.0) && ((p_tri.x + p_tri.y) < (vec3_lensq(&e0) + vec3_lensq(&e1))))
// if ((p_tri.x > 0.0) && (p_tri.y > 0.0) && ((p_tri.x + p_tri.y) < 1.0))
{
return p_tri.z;
}
else return p_tri.z + p_tri.x*p_tri.x + p_tri.y*p_tri.y;//length(p_tri);
*/
/**** original ****/
vector3 e0;
vector3 e1;
vector3 tri_n;
vector3 p_tri;
matrix4 m;
matrix4 m_tri;
vec3_sub(&e0, v1, v0);
vec3_sub(&e1, v2, v0);
tri_normal(&tri_n, v0, v1, v2);
mat4_set_identity(&m);
MAT4(&m, 0, 0) = e0.x;
MAT4(&m, 1, 0) = e0.y;
MAT4(&m, 2, 0) = e0.z;
MAT4(&m, 0, 1) = e1.x;
MAT4(&m, 1, 1) = e1.y;
MAT4(&m, 2, 1) = e1.z;
MAT4(&m, 0, 2) = tri_n.x;
MAT4(&m, 1, 2) = tri_n.y;
MAT4(&m, 2, 2) = tri_n.z;
MAT4(&m, 0, 3) = v0->x;
MAT4(&m, 1, 3) = v0->y;
MAT4(&m, 2, 3) = v0->z;
mat4_invert(&m_tri, &m);
mat4_mulvec3(&p_tri, &m_tri, p);
//return p_tri.z;
// if ((0.0f < p_tri.x) && (p_tri.x < 1.0f) && (0.0f < p_tri.y) && (p_tri.y < 1.0f))
if ((0.0f <= p_tri.x) && (0.0f <= p_tri.y) && ((p_tri.x + p_tri.y) <= 1.0f))
return p_tri.z;
return p_tri.z + p_tri.x*p_tri.x + p_tri.y*p_tri.y;
}
void tri_normal(vector3* n,
const vector3* v0,
const vector3* v1,
const vector3* v2)
{
vector3 e0;
vector3 e1;
vec3_sub(&e0, v2, v0);
vec3_sub(&e1, v1, v0);
vec3_cross(n, &e0, &e1);
vec3_normalize(n, n);
}⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?