ASM算法实现对齐功能代码

using UnityEngine;
using System.Collections;

public class myscript_jiaozheng : MonoBehaviour {


private Matrix4x4 OutMat = Matrix4x4.identity;


//get the weights
float [] GetWeightofPerPoint(Vector2 [] _glassPoints, Vector2 [] _facePoints)
{
int PointSize = _glassPoints.Length;
if (PointSize != _facePoints.Length)
return null;
double Epsilon = 1.0E-6;
float [] weightsPoint =new float[PointSize];//out

for (int i = 0; i < PointSize; i++) 
{
float totalvariance = 0.0f;

for (int j = 0; j < PointSize; j++) 
{
if (j == i)
continue;

float dis_glass = Vector2.Distance (_glassPoints[i], _glassPoints[j]);
float dis_face = Vector2.Distance (_facePoints[i], _facePoints[j]);

float mean_dis = (dis_glass + dis_face) / 2.0f;

float variance = ((mean_dis - dis_glass) * (mean_dis - dis_glass) + (mean_dis - dis_face) * (mean_dis - dis_face)) / 2.0f;

totalvariance += variance;
}
totalvariance += (float)Epsilon;
weightsPoint [i] = 1.0f / totalvariance;
}



//normalize the weights.
float totalweightsPoint = 0.0f;
totalweightsPoint += (float)Epsilon;

for (int i = 0; i < PointSize; i++) {
totalweightsPoint += weightsPoint [i];
}

for (int i = 0; i < PointSize; i++) {
weightsPoint [i] = weightsPoint [i] / totalweightsPoint;
}



return weightsPoint;
}

Matrix4x4 Get_axaytxty(Vector2 [] _glassPoints, Vector2 [] _facePoints)