// Converting Accord.Vector3 to Unity.Vector3
private UnityEngine.Vector3 AccordtoUnity(Accord.Math.Vector3 pos)
{
UnityEngine.Vector3 posTransformed = new UnityEngine.Vector3();
posTransformed.x = pos.X;
posTransformed.y = pos.Y;
posTransformed.z = pos.Z;
return(posTransformed);
}
private Vector4 CalculatePoint(Vector3 barycentricCoords, FilledTriangle triangle)
{
Vector4 point = new Vector4();
point.X = barycentricCoords.X * triangle.Mv1.Point.X + barycentricCoords.Y * triangle.Mv2.Point.X + barycentricCoords.Z * triangle.Mv3.Point.X;
point.Y = barycentricCoords.X * triangle.Mv1.Point.Y + barycentricCoords.Y * triangle.Mv2.Point.Y + barycentricCoords.Z * triangle.Mv3.Point.Y;
point.Z = barycentricCoords.X * triangle.Mv1.Point.Z + barycentricCoords.Y * triangle.Mv2.Point.Z + barycentricCoords.Z * triangle.Mv3.Point.Z;
return(point);
}
// Converting Unity.Vector3 to Accord.Vector3
private Accord.Math.Vector3 UnitytoAccord(UnityEngine.Vector3 pos)
{
Accord.Math.Vector3 posTransformed = new Accord.Math.Vector3();
posTransformed.X = pos.x;
posTransformed.Y = pos.y;
posTransformed.Z = pos.z;
return(posTransformed);
}
private Vector4 CalculateColor(Vector3 barycentricCoords, FilledTriangle triangle)
{
Vector4 point = new Vector4();
point.X = barycentricCoords.X * triangle.Vertices[0].Color.X + barycentricCoords.Y * triangle.Vertices[1].Color.X + barycentricCoords.Z * triangle.Vertices[2].Color.X;
point.Y = barycentricCoords.X * triangle.Vertices[0].Color.Y + barycentricCoords.Y * triangle.Vertices[1].Color.Y + barycentricCoords.Z * triangle.Vertices[2].Color.Y;
point.Z = barycentricCoords.X * triangle.Vertices[0].Color.Z + barycentricCoords.Y * triangle.Vertices[1].Color.Z + barycentricCoords.Z * triangle.Vertices[2].Color.Z;
return(point);
}
private Vector4 CalculateNormal(Vector3 barycentricCoords, FilledTriangle triangle)
{
Vector4 normal = new Vector4();
normal.X = barycentricCoords.X * triangle.Mv1.Normal.X + barycentricCoords.Y * triangle.Mv2.Normal.X + barycentricCoords.Z * triangle.Mv3.Normal.X;
normal.Y = barycentricCoords.X * triangle.Mv1.Normal.Y + barycentricCoords.Y * triangle.Mv2.Normal.Y + barycentricCoords.Z * triangle.Mv3.Normal.Y;
normal.Z = barycentricCoords.X * triangle.Mv1.Normal.Z + barycentricCoords.Y * triangle.Mv3.Normal.Z + barycentricCoords.Z * triangle.Mv3.Normal.Z;
return(Vector4.Normalize(normal));
}
private Vector3 CalculateBarycentric(int x, int y, FilledTriangle triangle)
{
var d = new Vector3(x, y, 1);
var w = triangle.A.Determinant;
var wx = Matrix3x3.CreateFromColumns(d, triangle.VB, triangle.VC).Determinant;
var wy = Matrix3x3.CreateFromColumns(triangle.VA, d, triangle.VC).Determinant;
var wz = Matrix3x3.CreateFromColumns(triangle.VA, triangle.VB, d).Determinant;
var xA = wx / w;
var xB = wy / w;
var xC = wz / w;
return(new Vector3(xA, xB, xC));
}
private void ApplyTransformation(out UnityEngine.Matrix4x4 Transformation)
{
//Calculating Centroids from both coordinate system
Accord.Math.Vector3 centroidA = CalculateCentroid(UnitytoAccord(ReadFrameLowerLeft), UnitytoAccord(ReadFrameLowerRight), UnitytoAccord(ReadFrameUpperLeft), UnitytoAccord(ReadFrameUpperRight));
Accord.Math.Vector3 centroidB = CalculateCentroid(UnitytoAccord(FrameLowerLeft), UnitytoAccord(FrameLowerRight), UnitytoAccord(FrameUpperLeft), UnitytoAccord(FrameUpperRight));
Matrix3x3 H = CovarianceMatrixStep(UnitytoAccord(ReadFrameLowerLeft) - centroidA, UnitytoAccord(FrameLowerLeft) - centroidB)
+ CovarianceMatrixStep(UnitytoAccord(ReadFrameLowerRight) - centroidA, UnitytoAccord(FrameLowerRight) - centroidB)
+ CovarianceMatrixStep(UnitytoAccord(ReadFrameUpperLeft) - centroidA, UnitytoAccord(FrameUpperLeft) - centroidB)
+ CovarianceMatrixStep(UnitytoAccord(ReadFrameUpperRight) - centroidA, UnitytoAccord(FrameUpperRight) - centroidB);
Matrix3x3 U;
Accord.Math.Vector3 E;
Matrix3x3 V;
Matrix3x3 R;
H.SVD(out U, out E, out V);
R = V * U.Transpose();
Debug.Log("Row " + R.GetRow(0));
Debug.Log("Row " + R.GetRow(1));
Debug.Log("Row " + R.GetRow(2));
if (R.Determinant < 0)
{
V.V02 = (-V.V02);
V.V12 = (-V.V12);
V.V22 = (-V.V22);
R = V * U.Transpose();
Debug.LogWarning("Reflection case");
}
Accord.Math.Vector3 Translation;
Translation = (NegativeMatrix(R) * centroidA + centroidB);
Debug.Log("Translation is" + Translation);
Transformation = UnityEngine.Matrix4x4.identity;
Transformation = AccordToUnityMatrix(Transformation, R, Translation);
Debug.Log("Trans" + Transformation.GetRow(0));
Debug.Log("Trans" + Transformation.GetRow(1));
Debug.Log("Trans" + Transformation.GetRow(2));
Debug.Log("Trans" + Transformation.GetRow(3));
Transformation.SetTRS(AccordtoUnity(Translation), Quaternion.LookRotation(Transformation.GetColumn(1),
Transformation.GetColumn(2)), UnityEngine.Vector3.one);
}
// Calculation of covariance matrix H
private Matrix3x3 CovarianceMatrixStep(Accord.Math.Vector3 difSetA, Accord.Math.Vector3 difSetB)
{
Matrix3x3 M;
M.V00 = difSetA.X * difSetB.X;
M.V01 = difSetA.X * difSetB.Y;
M.V02 = difSetA.X * difSetB.Z;
M.V10 = difSetA.Y * difSetB.X;
M.V11 = difSetA.Y * difSetB.Y;
M.V12 = difSetA.Y * difSetB.Z;
M.V20 = difSetA.Z * difSetB.X;
M.V21 = difSetA.Z * difSetB.Y;
M.V22 = difSetA.Z * difSetB.Z;
return(M);
}
// Function to apply transformation to 4th point/object
void ApplyTransformation()
{
//Calculating Centroids from both coordinate system
centroidA = (UnitytoAccord(CubeReg.transform.position) + UnitytoAccord(SphereReg.transform.position)
+ UnitytoAccord(CylinderReg.transform.position)) / 3;
centroidB = (UnitytoAccord(CubeTransformed.transform.position) + UnitytoAccord(SphereTransformed.transform.position)
+ UnitytoAccord(CylinderTransformed.transform.position)) / 3;
//Debug.Log("Centroid A is" + centroidA + " Centroid B is" + centroidB);
// Calculating Covariance Matrix
H = CovarianceMatrixStep(UnitytoAccord(CubeReg.transform.position) - centroidA, UnitytoAccord(CubeTransformed.transform.position) - centroidB)
+ CovarianceMatrixStep(UnitytoAccord(SphereReg.transform.position) - centroidA, UnitytoAccord(SphereTransformed.transform.position) - centroidB)
+ CovarianceMatrixStep(UnitytoAccord(CylinderReg.transform.position) - centroidA, UnitytoAccord(CylinderTransformed.transform.position) - centroidB);
H.SVD(out U, out E, out V);
R = V * U.Transpose();
//special reflection case
if (R.Determinant < 0)
{
V.V02 = (-V.V02);
V.V12 = (-V.V12);
V.V22 = (-V.V22);
R = V * U.Transpose();
Debug.LogWarning("Reflection case");
}
Translation = NegativeMatrix(R) * centroidA + centroidB;
//Debug.Log("Translation is" + Translation);
TransformationMatrix = AccordToUnityMatrix(TransformationMatrix, R, Translation);
//Debug.Log(TransformationMatrix);
// Transformaiton Matrix for Unity
TransformationMatrix.SetTRS(AccordtoUnity(Translation), Quaternion.LookRotation(TransformationMatrix.GetColumn(1),
TransformationMatrix.GetColumn(2)), UnityEngine.Vector3.one);
// Applying Translation and rotation to 4th point/object
TestObject.transform.position = TransformationMatrix.MultiplyPoint(InitPosition);
TestObject.transform.rotation = Quaternion.LookRotation(TransformationMatrix.GetColumn(1), TransformationMatrix.GetColumn(2)) * InitQT;
}
// Creating Unity Transformation matrix using 3x3 Rotation matrix and translation vector acquired from RigidTransform
UnityEngine.Matrix4x4 AccordToUnityMatrix(UnityEngine.Matrix4x4 UnityM, Accord.Math.Matrix3x3 RotationM, Accord.Math.Vector3 Trans)
{
UnityM.m00 = RotationM.V00;
UnityM.m10 = RotationM.V10;
UnityM.m20 = RotationM.V20;
UnityM.m01 = RotationM.V01;
UnityM.m11 = RotationM.V11;
UnityM.m21 = RotationM.V21;
UnityM.m02 = RotationM.V02;
UnityM.m12 = RotationM.V12;
UnityM.m22 = RotationM.V22;
UnityM.m03 = Trans.X;
UnityM.m13 = Trans.Y;
UnityM.m23 = Trans.Z;
UnityM.m30 = 0;
UnityM.m31 = 0;
UnityM.m32 = 0;
UnityM.m33 = 1;
return(UnityM);
}
private float CountZCoord(Vector3 barycentricCoords, FilledTriangle triangle)
{
float z = barycentricCoords.X * triangle.Vertices[0].Z + barycentricCoords.Y * triangle.Vertices[1].Z + barycentricCoords.Z * triangle.Vertices[2].Z;
return(z);
}
private FilledTriangle PrepareForFilling(FilledTriangle triangle, Camera camera, int j, int y, int y1, Vector3 barycentricCoords, float zp)
{
Vector4 normal = Vector4.Zero;
Vector4 point = Vector4.Zero;
;
Vector4 IO = Vector4.One;
if (Shader.Settings.IsPhong == true)
{
normal = CalculateNormal(barycentricCoords, triangle);
point = CalculatePoint(barycentricCoords, triangle);
IO = triangle.Vertices[0].Color;
}
else
{
IO = CalculateColor(barycentricCoords, triangle);
}
arguments[j, y - 1] = new ShadingArguments(camera, point, normal, IO);
_zBuffer[j, y1 - 1] = zp;
if (j <= minX)
{
minX = j;
}
if (j >= maxX)
{
maxX = j;
}
if (y - 1 <= minY)
{
minY = y - 1;
}
if (y - 1 >= maxY)
{
maxY = y - 1;
}
return(triangle);
}
public void FillPolygon(FilledTriangle triangle, Camera camera)
{
List <Vertex> vertices = triangle.Vertices;
int[] ind = Enumerable.Range(0, triangle.Vertices.Count).OrderBy(x => triangle.Vertices[x].Y).ToArray();
int ymin = vertices[ind[0]].Y;
int ymax = vertices[ind[vertices.Count - 1]].Y;
int k = 0;
List <Node> AET = new List <Node>();
for (int y = ymin; y <= ymax; y++)
{
while (vertices[ind[k]].Y == y - 1)
{
int i = ind[k];
int iPrev = (ind[k] - 1) % vertices.Count;
if (iPrev < 0)
{
iPrev += vertices.Count;
}
Vertex Pi = vertices[i];
Vertex PiPrev = vertices[iPrev];
CheckNeighbour(AET, iPrev, Pi, i, PiPrev);
int iNext = (ind[k] + 1) % vertices.Count;
if (iNext < 0)
{
iNext += vertices.Count;
}
Vertex PiNext = vertices[iNext];
CheckNeighbour(AET, i, Pi, iNext, PiNext);
k++;
}
// AET update
AET = AET.OrderBy(node => node.X).ToList();
var aet = AET;
Parallel.For(0, AET.Count - 1, i =>
{
i *= 2;
var y1 = y;
Parallel.For((int)Math.Round(aet[i].X), (int)Math.Round(aet[i + 1].X), j =>
{
Vector3 barycentricCoords = CalculateBarycentric(j, y1 - 1, triangle);
float zp = CountZCoord(barycentricCoords, triangle);
if (zp <= _zBuffer[j, y1 - 1])
{
triangle = PrepareForFilling(triangle, camera, j, y, y1, barycentricCoords, zp);
}
});
});
foreach (var t in AET)
{
t.X += t.iM;
}
}
}
private Accord.Math.Vector3 CalculateCentroid(Accord.Math.Vector3 first, Accord.Math.Vector3 second, Accord.Math.Vector3 third, Accord.Math.Vector3 fourth)
{
return((first + second + third + fourth) / 4);
}