void read_exif_header(FILE *fp, unsigned char *buffer, bool *is_valid, bool *is_little_endian)
{
// SOI
vread(buffer, 2, fp, is_valid);
vcmp(SOI, buffer, 2, is_valid);
// APP1 Marker
vread(buffer, 2, fp, is_valid);
vcmp(APP1_MARKER, buffer, 2, is_valid);
// EXIF Identifier Code
vseek(fp, 2, SEEK_CUR, is_valid); // Skip (APP1 Length)
vread(buffer, 6, fp, is_valid);
vcmp(EXIF_IDENT_CODE, buffer, 6, is_valid);
// Byte Order
vread(buffer, 2, fp, is_valid);
if(*is_valid && memcmp(TIFF_LITTLE_ENDIAN, buffer, 2) == 0)
*is_little_endian = true;
else if(*is_valid && memcmp(TIFF_BIG_ENDIAN, buffer, 2) == 0)
*is_little_endian = false;
else
*is_valid = false;
// EXIF Fixed Code
vread(buffer, 2, fp, is_valid);
vrev(buffer, 2, is_valid, is_little_endian);
vcmp(EXIF_FIXED_CODE, buffer, 2, is_valid);
// 0th IFD Offset
vread(buffer, 4, fp, is_valid);
vrev(buffer, 4, is_valid, is_little_endian);
// 0th IFD
vseek(fp, todecimal(buffer, 4) - 8, SEEK_CUR, is_valid);
// 0th IFD Entry Count
vread(buffer, 2, fp, is_valid);
vrev(buffer, 2, is_valid, is_little_endian);
}
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);
}
}
