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;