void
plMassTranslate(PLmass *m, float dx, float dy, float dz)
{
float3x3 re;
mf3_ident(re);
float3 r = vf3_set(dx, dy, dz);
float rdot = vf3_dot(r, r);
re[0][0] = rdot;
re[1][1] = rdot;
re[2][2] = rdot;
float3x3 rout;
vf3_outprod(rout, r, r);
float3x3 madj;
mf3_sub(madj, re, rout);
float3x3 mtmp;
mf3_s_mul(mtmp, madj, m->m);
mf3_add2(m->I, mtmp);
m->cog = vf3_add(m->cog, r);
mf3_inv2(m->I_inv, m->I);
}
float3
ooGeoEllipseSegPoint(OOellipse *e, double t)
{
double pos = fmod(t, (double)(e->vec.length));
size_t i = (size_t) pos;
float3 a = (float3) e->vec.elems[(i + 1) % e->vec.length];
float3 b = (float3) e->vec.elems[i % e->vec.length];
double frac = pos - floor(pos);
float3 av, bv;
av = a;
bv = b;
// Lineraly interpolate between point b and a
float3 abdiff = vf3_sub(av, bv);
float3 tmp = vf3_set(frac, frac, frac);
float3 fv = vf3_mul(abdiff, tmp);//v_s_mul(abdiff, (float)frac); BUG IN v_s_mul???
float3 res = vf3_add(bv, fv);
return res;
}
{
float3x3 mat;
q_mf3_convert(mat, q);
plMassRotateM(m, mat);
}
void
plMassAdd(PLmass * restrict md, const PLmass * restrict ms)
{
float recip = 1.0f / (md->m + ms->m);
float3 a = vf3_s_mul(md->cog, md->m);
float3 b = vf3_s_mul(ms->cog, ms->m);
float3 c = vf3_add(a, b);
md->cog = vf3_s_mul(c, recip);
md->m += ms->m;
for (int i = 0 ; i < 3 ; ++ i) {
md->I[i] = vf3_add(md->I[i], ms->I[i]);
}
mf3_inv2(md->I_inv, md->I);
}
void
plMassMod(PLmass *m, float newMass)
{
float s = newMass / m->m;
m->m = newMass;
