/// <summary>
/// Creates a Matrix3x3 that flattens geometry into a shadow.
/// </summary>
/// <param name="light">The light direction. If the W component is 0, the light is directional light; if the
/// W component is 1, the light is a point light.</param>
/// <param name="plane">The plane onto which to project the geometry as a shadow. This parameter is assumed to be normalized.</param>
/// <returns>The shadow Matrix3x3.</returns>
public static MyMatrix3x3 Shadow(MyVector4 light, MyPlane plane)
{
MyMatrix3x3 result;
Shadow(ref light, ref plane, out result);
return(result);
}
/// <summary>
/// Creates a translation Matrix5x4 using the specified offsets.
/// </summary>
/// <param name="value">The offset for all three coordinate planes.</param>
/// <returns>The created translation Matrix5x4.</returns>
public static MyMatrix5x4 Translation(MyVector4 value)
{
MyMatrix5x4 result;
Translation(ref value, out result);
return(result);
}
/// <summary>
/// Creates a Matrix5x4 that scales along the x-axis, y-axis, and y-axis.
/// </summary>
/// <param name="scale">Scaling factor for all three axes.</param>
/// <returns>The created scaling Matrix5x4.</returns>
public static MyMatrix5x4 Scaling(MyVector4 scale)
{
MyMatrix5x4 result;
Scaling(ref scale, out result);
return(result);
}
/// <summary>
/// Initializes a new instance of the <see cref="MyColor"/> struct.
/// </summary>
/// <param name="value">The red, green, blue, and alpha components of the color.</param>
public MyColor(MyVector4 value)
{
R = ToByte(value.X);
G = ToByte(value.Y);
B = ToByte(value.Z);
A = ToByte(value.W);
}
/// <summary>
/// Creates a matrix that flattens geometry into a shadow from this the plane onto which to project the geometry as a shadow.
/// This plane is assumed to be normalized
/// </summary>
/// <param name="light">The light direction. If the W component is 0, the light is directional light; if the
/// W component is 1, the light is a point light.</param>
/// <returns>The shadow matrix.</returns>
public MyMatrix Shadow(MyVector4 light)
{
MyMatrix result;
Shadow(ref light, out result);
return(result);
}
/// <summary>
/// Initializes a new instance of the <see cref="MyColor4"/> struct.
/// </summary>
/// <param name="value">The red, green, blue, and alpha components of the color.</param>
public MyColor4(MyVector4 value)
{
Red = value.X;
Green = value.Y;
Blue = value.Z;
Alpha = value.W;
}
/// <summary>
/// Creates a matrix that flattens geometry into a shadow from this the plane onto which to project the geometry as a shadow.
/// This plane is assumed to be normalized
/// </summary>
/// <param name="light">The light direction. If the W component is 0, the light is directional light; if the
/// W component is 1, the light is a point light.</param>
/// <param name="result">When the method completes, contains the shadow matrix.</param>
public void Shadow(ref MyVector4 light, out MyMatrix result)
{
float dot = (this.Normal.X * light.X) + (this.Normal.Y * light.Y) + (this.Normal.Z * light.Z) + (this.D * light.W);
float x = -this.Normal.X;
float y = -this.Normal.Y;
float z = -this.Normal.Z;
float d = -this.D;
result.M11 = (x * light.X) + dot;
result.M21 = y * light.X;
result.M31 = z * light.X;
result.M41 = d * light.X;
result.M12 = x * light.Y;
result.M22 = (y * light.Y) + dot;
result.M32 = z * light.Y;
result.M42 = d * light.Y;
result.M13 = x * light.Z;
result.M23 = y * light.Z;
result.M33 = (z * light.Z) + dot;
result.M43 = d * light.Z;
result.M14 = x * light.W;
result.M24 = y * light.W;
result.M34 = z * light.W;
result.M44 = (d * light.W) + dot;
}
/// <summary>
/// Creates a Matrix3x3 that flattens geometry into a shadow.
/// </summary>
/// <param name="light">The light direction. If the W component is 0, the light is directional light; if the
/// W component is 1, the light is a point light.</param>
/// <param name="plane">The plane onto which to project the geometry as a shadow. This parameter is assumed to be normalized.</param>
/// <param name="result">When the method completes, contains the shadow Matrix3x3.</param>
public static void Shadow(ref MyVector4 light, ref MyPlane plane, out MyMatrix3x3 result)
{
float dot = (plane.Normal.X * light.X) + (plane.Normal.Y * light.Y) + (plane.Normal.Z * light.Z) + (plane.D * light.W);
float x = -plane.Normal.X;
float y = -plane.Normal.Y;
float z = -plane.Normal.Z;
float d = -plane.D;
result.M11 = (x * light.X) + dot;
result.M21 = y * light.X;
result.M31 = z * light.X;
result.M12 = x * light.Y;
result.M22 = (y * light.Y) + dot;
result.M32 = z * light.Y;
result.M13 = x * light.Z;
result.M23 = y * light.Z;
result.M33 = (z * light.Z) + dot;
}
/// <summary>
/// Creates a translation Matrix5x4 using the specified offsets.
/// </summary>
/// <param name="value">The offset for all three coordinate planes.</param>
/// <param name="result">When the method completes, contains the created translation Matrix5x4.</param>
public static void Translation(ref MyVector4 value, out MyMatrix5x4 result)
{
Translation(value.X, value.Y, value.Z, value.W, out result);
}
/// <summary>
/// Creates a Matrix5x4 that scales along the x-axis, y-axis, y-axis and w-axis
/// </summary>
/// <param name="scale">Scaling factor for all three axes.</param>
/// <param name="result">When the method completes, contains the created scaling Matrix5x4.</param>
public static void Scaling(ref MyVector4 scale, out MyMatrix5x4 result)
{
Scaling(scale.X, scale.Y, scale.Z, scale.W, out result);
}
public static void computeVertexNormal(int areaWidth, ref List <MyVector3> points, ref double[][] inputDataMS, int w, int h, bool displayResult = false)
{
List <double> pointNormals = new List <double>();
int loop = 0;
for (int i = 0; i < h; i++)
{
for (int j = 0; j < w; j++)
{
List <MyVector3> candidatePoints = new List <MyVector3>();
for (int i1 = -areaWidth; i1 <= areaWidth; i1++)
{
for (int j1 = -areaWidth; j1 <= areaWidth; j1++)
{
int iT = i + i1, jT = j + j1;
if (iT >= 0 && iT < h && jT >= 0 && jT < w)
{
if (!(points[iT * w + jT].x == 0 && points[iT * w + jT].y == 0 && points[iT * w + jT].z == 0))
{
candidatePoints.Add(points[iT * w + jT]);
}
}
}
}
MyVector4 planeABCD;
if (candidatePoints.Count < areaWidth * areaWidth)
{
planeABCD = new MyVector4(0, 0, 0, 0);
}
else
{
planeABCD = pointToPlaneClustering.RANSACNormal(candidatePoints);
}
inputDataMS[loop][3] = planeABCD.x;
inputDataMS[loop][4] = planeABCD.y;
inputDataMS[loop][5] = planeABCD.z;
loop++;
pointNormals.AddRange(new double[3] {
planeABCD.x, planeABCD.y, planeABCD.z
});
}
}
if (displayResult)
{
loop = 0;
Image <Bgr, byte> image1 = new Image <Bgr, byte>(w, h);
image1.SetZero();
for (int i = 0; i < h; i++)
{
for (int j = 0; j < w; j++)
{
double rd = pointNormals[3 * loop] > 0 ? pointNormals[3 * loop] : -pointNormals[3 * loop] * 0;
double gd = pointNormals[3 * loop + 1] > 0 ? pointNormals[3 * loop + 1] : -pointNormals[3 * loop + 1] * 0;
double bd = pointNormals[3 * loop + 2] > 0 ? pointNormals[3 * loop + 2] : -pointNormals[3 * loop + 2] * 0;
byte r = (byte)(rd * 255);
byte g = (byte)(gd * 255);
byte b = (byte)(bd * 255);
image1[i, j] = new Bgr(b, g, r);
loop++;
}
}
new ImageViewer(image1, "1 - PointNormal").Show();
}
}
public void SceondPlaneExtraction()
{
if (clusteringPlaneRec == null || clusteringPlaneRec.Count == 0)
{
return;
}
bool[] labelsFlag = new bool[pointLabels.Length];
for (int i = 0; i < pointLabels.Length; i++)
{
labelsFlag[i] = false;
}
extractionPlaneRec = new List <pointPlaneClass>();
for (int i = 0; i < 20; i++)
{
if (clusteringPlaneRec[0].Value < w * h * 0.0005) // too few points
{
break;
}
List <int> validPointIdx = new List <int>();
MyVector4 returnedValue = RANSACNormal(clusteringPlaneRec[0].Points, validPointIdx, 0.008, 1.0, 200);
extractionPlaneRec.Add(new pointPlaneClass(i, validPointIdx.Count));
extractionPlaneRec[i].NormalABC = new MyVector3(returnedValue.x, returnedValue.y, returnedValue.z);
extractionPlaneRec[i].D = returnedValue.w;
for (int j = validPointIdx.Count - 1; j >= 0; j--)
{
MyVector3 mv3T = clusteringPlaneRec[0].Points[validPointIdx[j]];
extractionPlaneRec[i].Points.Add(mv3T);
clusteringPlaneRec[0].Points.RemoveAt(validPointIdx[j]);
extractionPlaneRec[i].PointsIdx.Add(clusteringPlaneRec[0].PointsIdx[validPointIdx[j]]);
labelsFlag[clusteringPlaneRec[0].PointsIdx[validPointIdx[j]]] = true;
pointLabels[clusteringPlaneRec[0].PointsIdx[validPointIdx[j]]] = i;
clusteringPlaneRec[0].PointsIdx.RemoveAt(validPointIdx[j]);
clusteringPlaneRec[0].Value--;
}
// re sort
if (clusteringPlaneRec.Count > 1)
{
for (int j = 1; j < clusteringPlaneRec.Count; j++)
{
if (clusteringPlaneRec[j - 1].Value >= clusteringPlaneRec[j].Value)
{
break;
}
pointPlaneClass lcT = clusteringPlaneRec[j - 1];
clusteringPlaneRec[j - 1] = clusteringPlaneRec[j];
clusteringPlaneRec[j] = lcT;
}
}
}
for (int i = 0; i < pointLabels.Length; i++)
{
if (labelsFlag[i] == false)
{
pointLabels[i] = -1;
}
}
}
/// <summary>
/// Gets random <see cref="MyVector4"/> within range.
/// </summary>
/// <param name="random">Current <see cref="System.Random"/>.</param>
/// <param name="min">Minimum.</param>
/// <param name="max">Maximum.</param>
/// <returns>Random <see cref="MyVector4"/>.</returns>
public static MyVector4 NextVector4(this Random random, MyVector4 min, MyVector4 max)
{
return(new MyVector4(random.NextFloat(min.X, max.X), random.NextFloat(min.Y, max.Y), random.NextFloat(min.Z, max.Z), random.NextFloat(min.W, max.W)));
}
/// <summary>
/// Calculates the dot product of the specified vector and plane.
/// </summary>
/// <param name="left">The source plane.</param>
/// <param name="right">The source vector.</param>
/// <returns>The dot product of the specified plane and vector.</returns>
public static float Dot(MyPlane left, MyVector4 right)
{
return((left.Normal.X * right.X) + (left.Normal.Y * right.Y) + (left.Normal.Z * right.Z) + (left.D * right.W));
}
/// <summary>
/// Calculates the dot product of the specified vector and plane.
/// </summary>
/// <param name="left">The source plane.</param>
/// <param name="right">The source vector.</param>
/// <param name="result">When the method completes, contains the dot product of the specified plane and vector.</param>
public static void Dot(ref MyPlane left, ref MyVector4 right, out float result)
{
result = (left.Normal.X * right.X) + (left.Normal.Y * right.Y) + (left.Normal.Z * right.Z) + (left.D * right.W);
}
private MyVector4 quarToVector(Quaternion q)
{
MyVector4 v = new MyVector4(q.x, q.y, q.z, q.w);
return(v);
}
public void MergePlanes()
{
GraphAdjList planeGraph = new GraphAdjList(extractionPlaneRec.Count);
for (int i = 0; i < extractionPlaneRec.Count; i++)
{
for (int j = i + 1; j < extractionPlaneRec.Count; j++)
{
double dis1 = 0;
for (int loop = 0; loop < extractionPlaneRec[i].Value; loop++)
{
dis1 += Math.Abs(extractionPlaneRec[i].Points[loop].x * extractionPlaneRec[j].NormalABC.x +
extractionPlaneRec[i].Points[loop].y * extractionPlaneRec[j].NormalABC.y +
extractionPlaneRec[i].Points[loop].z * extractionPlaneRec[j].NormalABC.z -
extractionPlaneRec[j].D) / extractionPlaneRec[j].D;
}
dis1 /= extractionPlaneRec[i].Value;
double dis2 = 0;
for (int loop = 0; loop < extractionPlaneRec[j].Value; loop++)
{
dis2 += Math.Abs(extractionPlaneRec[j].Points[loop].x * extractionPlaneRec[i].NormalABC.x +
extractionPlaneRec[j].Points[loop].y * extractionPlaneRec[i].NormalABC.y +
extractionPlaneRec[j].Points[loop].z * extractionPlaneRec[i].NormalABC.z -
extractionPlaneRec[i].D) / extractionPlaneRec[i].D;
}
dis2 /= extractionPlaneRec[j].Value;
double dis = (dis1 * extractionPlaneRec[j].Value + dis2 * extractionPlaneRec[i].Value) /
(extractionPlaneRec[i].Value + extractionPlaneRec[j].Value);
if (extractionPlaneRec[i].NormalABC.Dot(extractionPlaneRec[j].NormalABC) > Math.Cos(10 * Math.PI / 180) &&
dis < 0.04)
{
planeGraph.AddEdge(i, j);
}
}
}
int groupIdx = 0;
int[] clusterInd = new int[extractionPlaneRec.Count];
bool[] visited = new bool[extractionPlaneRec.Count];
DepthFirstSearch dfs = new DepthFirstSearch();
for (int i = 0; i < extractionPlaneRec.Count; i++)
{
if (!visited[i])
{
dfs.DFS(planeGraph, ref visited, i, ref clusterInd, groupIdx);
groupIdx++;
}
}
mergedPlaneRec = new List <pointPlaneClass>();
for (int i = 0; i < groupIdx; i++)
{
mergedPlaneRec.Add(new pointPlaneClass(i, 0));
}
for (int i = 0; i < extractionPlaneRec.Count; i++)
{
mergedPlaneRec[clusterInd[i]].Value += extractionPlaneRec[i].Value;
mergedPlaneRec[clusterInd[i]].Points.AddRange(extractionPlaneRec[i].Points);
mergedPlaneRec[clusterInd[i]].PointsIdx.AddRange(extractionPlaneRec[i].PointsIdx);
foreach (int pi in extractionPlaneRec[i].PointsIdx)
{
pointLabels[pi] = clusterInd[i];
}
}
for (int i = 0; i < mergedPlaneRec.Count; i++)
{
MyVector4 returnedvalue = pointToPlaneClustering.RANSACNormal(mergedPlaneRec[i].Points, null, 0.0002, 0.9, 100);
mergedPlaneRec[i].NormalABC = new MyVector3(returnedvalue.x, returnedvalue.y, returnedvalue.z);
mergedPlaneRec[i].D = returnedvalue.w;
}
}