private static Matrix3D AlingVector(Vector3D _fixed, Vector3D _to_rotate)
{
if (_fixed == null || _to_rotate == null)
{
return(Matrix3D.Identity);
}
// see file C:\_TU\Code-Test\_theory\RotationMatrix.docx
Vector3D v_fixed = new Vector3D(_fixed.X, _fixed.Y, _fixed.Z);
Vector3D v_to_rotate = new Vector3D(_to_rotate.X, _to_rotate.Y, _to_rotate.Z);
v_fixed.Normalize();
v_to_rotate.Normalize();
Vector3D v = Vector3D.CrossProduct(v_to_rotate, v_fixed);
double s = v.Length;
double c = Vector3D.DotProduct(v_to_rotate, v_fixed);
Matrix3D Mvx_ = new Matrix3D(0, -v.Z, v.Y, 0,
v.Z, 0, -v.X, 0,
-v.Y, v.X, 0, 0,
0, 0, 0, 0);
Matrix3D Mvx = Mvx_.Transpose();
Matrix3D R;
if (s != 0)
{
//R = Matrix3D.Identity + Mvx + ((1 - c) / (s * s)) * (Mvx * Mvx);
// Matrix3D R_1 = Mvx.Add(Matrix3D.Identity);
R = Mvx.Add(Matrix3D.Identity).Add((Mvx * Mvx).Mult((1 - c) / (s * s)));
}
else
{
R = Matrix3D.Identity;
if (c < 0)
{
R.Rotate(new Quaternion(new Vector3D(0, 0, 1), 180));
}
}
// debug
//Vector3D test = R.Transform(v_to_rotate);
// debug
return(R);
}
internal void UpdateWeights(Matrix3D diffs, double biasDiff)
{
Weights.Add(diffs);
_bias += biasDiff;
}