public static float3 mul(matrix3x3 left, float3 right)
{
return(new float3(
float3.dot(left.row1(), right),
float3.dot(left.row2(), right),
float3.dot(left.row3(), right)));
}
public static matrix3x3 mul(matrix3x3 left, matrix3x3 right)
{
matrix3x3 ret = new matrix3x3();
ret.m11 = float3.dot(left.row1(), right.col1()); ret.m12 = float3.dot(left.row1(), right.col2()); ret.m13 = float3.dot(left.row1(), right.col3());
ret.m21 = float3.dot(left.row2(), right.col1()); ret.m22 = float3.dot(left.row2(), right.col2()); ret.m23 = float3.dot(left.row2(), right.col3());
ret.m31 = float3.dot(left.row3(), right.col1()); ret.m32 = float3.dot(left.row3(), right.col2()); ret.m33 = float3.dot(left.row3(), right.col3());
return(ret);
}
public static matrix3x3 transpose(matrix3x3 m)
{
matrix3x3 ret = new matrix3x3();
ret.m11 = m.m11; ret.m12 = m.m21; ret.m13 = m.m31;
ret.m21 = m.m12; ret.m22 = m.m22; ret.m23 = m.m32;
ret.m31 = m.m13; ret.m32 = m.m23; ret.m33 = m.m33;
return(ret);
}
void CalibrateNormals(
int face1Index, float3 face1Normal,
int face2Index, float3 face2Normal,
float3[] inOutNormals, int count)
{
var canonNormals = faceNormals;
// int closestCanonNormal1 = findClosestNormal(canonNormals, count, face1Normal);
// int closestCanonNormal2 = findClosestNormal(canonNormals, count, face2Normal);
// We need to build a rotation matrix that turns canonical face normals into the reference frame
// of the accelerator, as defined by the measured coordinates of the 2 passed in face normals.
float3 canonFace1Normal = canonNormals[face1Index];
float3 canonFace2Normal = canonNormals[face2Index];
// Create our intermediate reference frame in both spaces
// Canonical space
float3 intX_Canon = canonFace1Normal; intX_Canon.normalize();
float3 intZ_Canon = float3.cross(intX_Canon, canonFace2Normal); intZ_Canon.normalize();
float3 intY_Canon = float3.cross(intZ_Canon, intX_Canon);
matrix3x3 int_Canon = new matrix3x3(intX_Canon, intY_Canon, intZ_Canon);
//BLE_LOG_INFO("intX_Canon: %d, %d, %d", (int)(intX_Canon.x* 100), (int) (intX_Canon.y* 100), (int) (intX_Canon.z* 100));
//BLE_LOG_INFO("intY_Canon: %d, %d, %d", (int)(intY_Canon.x* 100), (int) (intY_Canon.y* 100), (int) (intY_Canon.z* 100));
//BLE_LOG_INFO("intZ_Canon: %d, %d, %d", (int)(intY_Canon.x* 100), (int) (intY_Canon.y* 100), (int) (intY_Canon.z* 100));
// Accelerometer space
float3 intX_Acc = face1Normal; intX_Acc.normalize();
float3 intZ_Acc = float3.cross(intX_Acc, face2Normal); intZ_Acc.normalize();
float3 intY_Acc = float3.cross(intZ_Acc, intX_Acc);
matrix3x3 int_Acc = new matrix3x3(intX_Acc, intY_Acc, intZ_Acc);
// This is the matrix that rotates canonical normals into accelerometer reference frame
matrix3x3 rot = matrix3x3.mul(int_Acc, matrix3x3.transpose(int_Canon));
// NRF_LOG_INFO("row 1: %d, %d, %d", (int)(rot.row1().x * 100.0f), (int)(rot.row1().y * 100.0f), (int)(rot.row1().z * 100.0f));
// NRF_LOG_INFO("row 2: %d, %d, %d", (int)(rot.row2().x * 100.0f), (int)(rot.row2().y * 100.0f), (int)(rot.row2().z * 100.0f));
// NRF_LOG_INFO("row 3: %d, %d, %d", (int)(rot.row3().x * 100.0f), (int)(rot.row3().y * 100.0f), (int)(rot.row3().z * 100.0f));
// Now transform all the normals
for (int i = 0; i < count; ++i)
{
float3 canonNormal = inOutNormals[i];
//NRF_LOG_INFO("canon: %d, %d, %d", (int)(canonNormal.x * 100.0f), (int)(canonNormal.y * 100.0f), (int)(canonNormal.z * 100.0f));
float3 newNormal = matrix3x3.mul(rot, canonNormal);
//NRF_LOG_INFO("new: %d, %d, %d", (int)(newNormal.x * 100.0f), (int)(newNormal.y * 100.0f), (int)(newNormal.z * 100.0f));
inOutNormals[i] = newNormal;
}
}
