void
mat_lookatv(Mat *m, const Vec *eye, const Vec *center, const Vec *up)
{
Vec z;
vec_subv(center, eye, &z);
vec_norm(&z);
Vec up_norm;
memcpy(&up_norm, up, sizeof(Vec));
vec_norm(&up_norm);
Vec x;
vec_cross(&z, &up_norm, &x);
vec_norm(&x);
Vec y;
vec_cross(&x, &z, &y);
vec_norm(&y);
Mat lookat = {{
x.data[0], x.data[1], x.data[2], 0.0,
y.data[0], y.data[1], y.data[2], 0.0,
-z.data[0], -z.data[1], -z.data[2], 0.0,
0, 0, 0, 1
}};
mat_translate(&lookat, -eye->data[0], -eye->data[1], -eye->data[2]);
memcpy(m, &lookat, sizeof(Mat));
}
void PHYS_con_stick_apply(PHYS_ConStick * con)
{
float delta[3];
vec_sub(delta, con->node[1], con->node[0]);
float dsqr = vec_sqr(delta);
float lsqr = con->len * con->len;
float diff = lsqr / (dsqr+lsqr) - 0.5;
vec_mulv(delta, diff);
vec_subv(con->node[0], delta);
vec_addv(con->node[1], delta);
}
void PHYS_verlet_apply(float pos[3], float oldpos[3], float force[3])
{
float t[3];
float fs[3];
float fr[3];
vec_cpy(t, pos);
vec_mul(fs, force, KE_spf*KE_spf);
vec_sub(fr, pos, oldpos);
vec_mulv(fr, -0.1);
vec_addv(fs, fr);
vec_addv(pos, pos);
vec_subv(pos, oldpos);
vec_addv(pos, fs);
vec_cpy(oldpos, t);
}
