VectorInt16 getRotated(Quaternion q)
{
VectorInt16 r = new VectorInt16(x, y, z);
r.rotate(q);
return(r);
}
public VectorInt16 getNormalized()
{
VectorInt16 r = new VectorInt16(x, y, z);
r.normalize();
return(r);
}
// uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
// uint8_t dmpGetLinearAccel(long *data, uint8_t* packet);
VectorInt16 DmpGetLinearAccel(VectorInt16 vRaw, VectorFloat gravity)
{
// get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet)
VectorInt16 v = new VectorInt16();
v.x = (short)(vRaw.x - gravity.x * 4096);
v.y = (short)(vRaw.y - gravity.y * 4096);
v.z = (short)(vRaw.z - gravity.z * 4096);
return(v);
}
// uint8_t dmpGetLinearAccelInWorld(long *data, uint8_t* packet);
VectorInt16 DmpGetLinearAccelInWorld(VectorInt16 vReal, Quaternion q)
{
// rotate measured 3D acceleration vector into original state
// frame of reference based on orientation quaternion
VectorInt16 v = new VectorInt16();
//memcpy(v, vReal, sizeof(VectorInt16));
v = vReal;
v.rotate(q);
return(v);
}
VectorInt16 DmpGetAccel(uint8_t[] packet)
{
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
//if (packet == 0) packet = dmpPacketBuffer;
VectorInt16 v = new VectorInt16();
v.x = (int16_t)((packet[28] << 8) + packet[29]);
v.y = (int16_t)((packet[32] << 8) + packet[33]);
v.z = (int16_t)((packet[36] << 8) + packet[37]);
return(v);
}