static void state_matrix(double *M, const state *s)
{
double T[16], R[16], p[3];
vneg(p, s->p);
mtranslate(T, p);
meuler (R, s->e);
mmultiply (M, R, T);
}
/*
* Function: ri_bem_set
*
* Setups a beam structure.
*
*
* Parameters:
*
* beam - Pointer to the beam to be set up.
* org - Origin of beam.
* dir[4] - Corner rays of beam.
*
*
* Returns:
*
* 0 if OK, otherwise if err
*/
int
ri_beam_set(
ri_beam_t *beam, /* [inout] */
ri_vector_t org,
ri_vector_t dir[4])
{
int i, j;
int mask;
int dominant_axis;
int zeros;
ri_float_t maxval;
beam->d = 1024.0;
beam->t_max = RI_INFINITY;
/*
* Check if beam's directions lie in same quadrant.
*/
for (i = 0; i < 3; i++) {
zeros = 0;
mask = 0;
for (j = 0; j < 4; j++) {
if (fabs(dir[j][i]) < RI_EPS) {
zeros++;
} else {
mask += (dir[j][i] < 0.0) ? 1 : -1;
}
}
if ( (mask != -(4 - zeros)) && (mask != (4 - zeros)) ) {
/* FIXME:
* split beam so that subdivided beam has same sign.
*/
fprintf(stderr, "TODO: Beam's dir does not have same sign.\n");
for (j = 0; j < 4; j++) {
fprintf(stderr, " dir[%d] = %f, %f, %f\n", j,
dir[j][0], dir[j][1], dir[j][2]);
}
return -1;
}
}
vcpy( beam->org, org );
/*
* Find dominant plane. Use dir[0]
*/
maxval = fabs(dir[0][0]);
dominant_axis = 0;
if ( (maxval < fabs(dir[0][1])) ) {
maxval = fabs(dir[0][0]);
dominant_axis = 1;
}
if ( (maxval < fabs(dir[0][2])) ) {
maxval = fabs(dir[0][2]);
dominant_axis = 2;
}
beam->dominant_axis = dominant_axis;
/*
* Precompute sign of direction.
* We know all 4 directions has same sign, thus dir[0] is used to
* get sign of direction for each axis.
*/
beam->dirsign[0] = (dir[0][0] < 0.0) ? 1 : 0;
beam->dirsign[1] = (dir[0][1] < 0.0) ? 1 : 0;
beam->dirsign[2] = (dir[0][2] < 0.0) ? 1 : 0;
/*
* Project beam dir onto axis-alied plane.
*/
{
ri_vector_t normals[3] = {
{ 1.0, 0.0, 0.0 }, { 0.0, 1.0, 0.0 }, { 0.0, 0.0, 1.0 } };
ri_vector_t normal;
ri_float_t t;
ri_float_t k;
vcpy( normal, normals[beam->dominant_axis] );
if (beam->dirsign[beam->dominant_axis]) {
vneg( normal );
}
for (i = 0; i < 4; i++) {
t = vdot( dir[i], normal );
if (fabs(t) > RI_EPS) {
k = beam->d / t;
} else {
k = 1.0;
}
beam->dir[i][0] = k * dir[i][0];
beam->dir[i][1] = k * dir[i][1];
beam->dir[i][2] = k * dir[i][2];
}
}
/*
* Precompute inverse of direction
*/
for (i = 0; i < 4; i++) {
beam->invdir[i][0] = safeinv( beam->dir[i][0], RI_EPS, RI_FLT_MAX );
beam->invdir[i][1] = safeinv( beam->dir[i][1], RI_EPS, RI_FLT_MAX );
beam->invdir[i][2] = safeinv( beam->dir[i][2], RI_EPS, RI_FLT_MAX );
}
/*
* Precompute normal plane of beam frustum
*/
vcross( beam->normal[0], beam->dir[1], beam->dir[0] );
vcross( beam->normal[1], beam->dir[2], beam->dir[1] );
vcross( beam->normal[2], beam->dir[3], beam->dir[2] );
vcross( beam->normal[3], beam->dir[0], beam->dir[3] );
beam->is_tetrahedron = 0;
return 0; /* OK */
}
static void
cpArbiterApplyImpulse_NEON(cpArbiter *arb)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
cpFloatx2_t surface_vr = vld((cpFloat_t *)&arb->surface_vr);
cpFloatx2_t n = vld((cpFloat_t *)&arb->n);
cpFloat_t friction = arb->u;
int numContacts = arb->count;
struct cpContact *contacts = arb->contacts;
for(int i=0; i<numContacts; i++) {
struct cpContact *con = contacts + i;
cpFloatx2_t r1 = vld((cpFloat_t *)&con->r1);
cpFloatx2_t r2 = vld((cpFloat_t *)&con->r2);
cpFloatx2_t perp = vmake(-1.0, 1.0);
cpFloatx2_t r1p = vmul(vrev(r1), perp);
cpFloatx2_t r2p = vmul(vrev(r2), perp);
cpFloatx2_t vBias_a = vld((cpFloat_t *)&a->v_bias);
cpFloatx2_t vBias_b = vld((cpFloat_t *)&b->v_bias);
cpFloatx2_t wBias = vmake(a->w_bias, b->w_bias);
cpFloatx2_t vb1 = vadd(vBias_a, vmul_n(r1p, vget_lane(wBias, 0)));
cpFloatx2_t vb2 = vadd(vBias_b, vmul_n(r2p, vget_lane(wBias, 1)));
cpFloatx2_t vbr = vsub(vb2, vb1);
cpFloatx2_t v_a = vld((cpFloat_t *)&a->v);
cpFloatx2_t v_b = vld((cpFloat_t *)&b->v);
cpFloatx2_t w = vmake(a->w, b->w);
cpFloatx2_t v1 = vadd(v_a, vmul_n(r1p, vget_lane(w, 0)));
cpFloatx2_t v2 = vadd(v_b, vmul_n(r2p, vget_lane(w, 1)));
cpFloatx2_t vr = vsub(v2, v1);
cpFloatx2_t vbn_vrn = vpadd(vmul(vbr, n), vmul(vr, n));
cpFloatx2_t v_offset = vmake(con->bias, -con->bounce);
cpFloatx2_t jOld = vmake(con->jBias, con->jnAcc);
cpFloatx2_t jbn_jn = vmul_n(vsub(v_offset, vbn_vrn), con->nMass);
jbn_jn = vmax(vadd(jOld, jbn_jn), vdup_n(0.0));
cpFloatx2_t jApply = vsub(jbn_jn, jOld);
cpFloatx2_t t = vmul(vrev(n), perp);
cpFloatx2_t vrt_tmp = vmul(vadd(vr, surface_vr), t);
cpFloatx2_t vrt = vpadd(vrt_tmp, vrt_tmp);
cpFloatx2_t jtOld = {};
jtOld = vset_lane(con->jtAcc, jtOld, 0);
cpFloatx2_t jtMax = vrev(vmul_n(jbn_jn, friction));
cpFloatx2_t jt = vmul_n(vrt, -con->tMass);
jt = vmax(vneg(jtMax), vmin(vadd(jtOld, jt), jtMax));
cpFloatx2_t jtApply = vsub(jt, jtOld);
cpFloatx2_t i_inv = vmake(-a->i_inv, b->i_inv);
cpFloatx2_t nperp = vmake(1.0, -1.0);
cpFloatx2_t jBias = vmul_n(n, vget_lane(jApply, 0));
cpFloatx2_t jBiasCross = vmul(vrev(jBias), nperp);
cpFloatx2_t biasCrosses = vpadd(vmul(r1, jBiasCross), vmul(r2, jBiasCross));
wBias = vadd(wBias, vmul(i_inv, biasCrosses));
vBias_a = vsub(vBias_a, vmul_n(jBias, a->m_inv));
vBias_b = vadd(vBias_b, vmul_n(jBias, b->m_inv));
cpFloatx2_t j = vadd(vmul_n(n, vget_lane(jApply, 1)), vmul_n(t, vget_lane(jtApply, 0)));
cpFloatx2_t jCross = vmul(vrev(j), nperp);
cpFloatx2_t crosses = vpadd(vmul(r1, jCross), vmul(r2, jCross));
w = vadd(w, vmul(i_inv, crosses));
v_a = vsub(v_a, vmul_n(j, a->m_inv));
v_b = vadd(v_b, vmul_n(j, b->m_inv));
// TODO would moving these earlier help pipeline them better?
vst((cpFloat_t *)&a->v_bias, vBias_a);
vst((cpFloat_t *)&b->v_bias, vBias_b);
vst_lane((cpFloat_t *)&a->w_bias, wBias, 0);
vst_lane((cpFloat_t *)&b->w_bias, wBias, 1);
vst((cpFloat_t *)&a->v, v_a);
vst((cpFloat_t *)&b->v, v_b);
vst_lane((cpFloat_t *)&a->w, w, 0);
vst_lane((cpFloat_t *)&b->w, w, 1);
vst_lane((cpFloat_t *)&con->jBias, jbn_jn, 0);
vst_lane((cpFloat_t *)&con->jnAcc, jbn_jn, 1);
vst_lane((cpFloat_t *)&con->jtAcc, jt, 0);
}
}
