private void MatrixToEuler(CMatrix3x3 rm, out float rx, out float ry, out float rz)
{
float r11 = rm.ax.val;
float r12 = rm.ay.val;
float r13 = rm.az.val;
float r21 = rm.bx.val;
float r22 = rm.by.val;
float r23 = rm.bz.val;
float r31 = rm.cx.val;
float r32 = rm.cy.val;
float r33 = rm.cz.val;
double Y = -Math.Asin(Clamp(r13, -1.0f, 1.0f));
double C = Math.Cos(Y);
ry = (float)Y;
double rotx, roty, X, Z;
if (Math.Abs(C) > 0.0005)
{
double invC = 1.0 / C;
rotx = r33 * invC;
roty = r23 * invC;
X = Math.Atan2(roty, rotx);
rotx = r11 * invC;
roty = r12 * invC;
Z = Math.Atan2(roty, rotx);
}
else
{
X = 0.0;
Z = Math.Atan2(-r21, r22);
}
rx = (float)X;
rz = (float)Z;
if (rx < 0)
{
rx += (float)(Math.PI * 2.0);
}
if (ry < 0)
{
ry += (float)(Math.PI * 2.0);
}
if (rz < 0)
{
rz += (float)(Math.PI * 2.0);
}
}
private void EulerToMatrix(float rx, float ry, float rz, CMatrix3x3 rm)
{
double sr = Math.Sin(rx);
double cr = Math.Cos(rx);
double sp = Math.Sin(ry);
double cp = Math.Cos(ry);
double sy = Math.Sin(rz);
double cy = Math.Cos(rz);
rm.ax.val = (float)(cp * cy);
rm.ay.val = (float)(cp * sy);
rm.az.val = (float)(-sp);
double srsp = sr * sp;
double crsp = cr * sp;
rm.bx.val = (float)(srsp * cy - cr * sy);
rm.by.val = (float)(srsp * sy + cr * cy);
rm.bz.val = (float)(sr * cp);
rm.cx.val = (float)(crsp * cy + sr * sy);
rm.cy.val = (float)(crsp * sy - sr * cy);
rm.cz.val = (float)(cr * cp);
}
private void AddLayer(string layerFileName, string layerName, ref int meshId)
{
float RADIANS_TO_DEGREES = (float)(180 / Math.PI);
CR2WFile layer;
using (var fs = new FileStream(layerFileName, FileMode.Open, FileAccess.Read))
using (var reader = new BinaryReader(fs))
{
layer = new CR2WFile();
layer.Read(reader);
fs.Close();
}
TreeNode layerNode = new TreeNode(layerName);
foreach (var chunk in layer.chunks)
{
if (chunk.REDType == "CSectorData")
{
CSectorData sd = (CSectorData)chunk.data;
progressBar.Invoke((MethodInvoker) delegate
{
progressBar.Maximum = sd.BlockData.Count;
});
// only add sector node if there are meshes
foreach (var block in sd.BlockData)
{
if (block.packedObjectType == Enums.BlockDataObjectType.Mesh)
{
SVector3D position = block.position;
CMatrix3x3 rot = block.rotationMatrix;
float rx, ry, rz;
MatrixToEuler(rot, out rx, out ry, out rz); // radians
Vector3Df rotation = new Vector3Df(rx * RADIANS_TO_DEGREES, ry * RADIANS_TO_DEGREES, rz * RADIANS_TO_DEGREES);
Vector3Df translation = new Vector3Df(position.X.val, position.Y.val, position.Z.val);
SBlockDataMeshObject mo = (SBlockDataMeshObject)block.packedObject;
ushort meshIndex = mo.meshIndex.val;
if (meshIndex > sd.Resources.Count)
{
continue;
}
string meshName = sd.Resources[meshIndex].pathHash.val;
if (string.IsNullOrEmpty(meshName))
{
continue;
}
RenderMessage message = new RenderMessage(MessageType.ADD_MESH_NODE, meshName, translation, rotation, layerNode);
commandQueue.Enqueue(message);
progressBar.Invoke((MethodInvoker) delegate
{
progressBar.PerformStep();
});
}
}
}
}
}
public bool doStep()
{
if (_stepCounter >= maxStep)
{
return(false);
}
_stepCounter++;
if (isConverge())
{
return(false);
}
int i, j;
// Calculate Movement
int detectedEdgeCnt = calculateMovement(_XdX);
// Compute Change
CXYVector currentShape = new CXYVector(_PDMData.mean);
CMatrix3x3 pose = CXYVector.realign(currentShape, _XdX, _weight);
// Get Residual Adjustment
CXYVector XdX = new CXYVector(_XdX);
XdX.transform(pose.inverse());
double [] dx = XdX.substractDouble(_PDMData.mean);
// Translate Into Model Parameter
double [] db = new double[_PDMData.freedom];
double [,] vectorsPtr = _PDMData.eigen.vectors;
double dimension = _size * 2;
for (i = 0; i < _PDMData.freedom; i++)
{
db[i] = 0;
for (j = 0; j < dimension; j++)
{
db[i] += vectorsPtr[j, i] * dx[j];
}
}
// Find Out If Parameter Is Out Of Limit
double dm = 0.0;
for (i = 0; i < _PDMData.freedom; i++)
{
dm += (db[i] * db[i]) / _PDMData.eigen.values[i];
}
// If It Does, Apply Limit
dm = Math.Sqrt(dm);
if (dm > 3.0)
{
for (i = 0; i < _PDMData.freedom; i++)
{
db[i] *= 3.0 / dm;
}
}
// Update Parameter
currentShape = PDMData.generateNewVariation(-1, db);
CXYVector.realign(currentShape, _XdX, _weight);
//currentShape.transform(pose);
_energy = _ASMResult.distance(currentShape);
_ASMResult = currentShape;
return(detectedEdgeCnt > (_ASMResult.size / 10));
}
