public void SignedDistanceToPoint(string prps, string pns, string ps, double expected)
{
var plane = new Plane(UnitVector3D.Parse(pns), Point3D.Parse(prps));
var p = Point3D.Parse(ps);
Assert.AreEqual(expected, plane.SignedDistanceTo(p), 1E-6);
}
public void Parse(string rootPoint, string unitVector, string pds, string vds)
{
var plane = new Plane(Point3D.Parse(rootPoint), UnitVector3D.Parse(unitVector));
AssertGeometry.AreEqual(Point3D.Parse(pds), plane.RootPoint);
AssertGeometry.AreEqual(Vector3D.Parse(vds), plane.Normal);
}
public void SignedDistanceToOtherPlane(string prps, string pns, string otherPlaneRootPointString, string otherPlaneNormalString, double expectedValue)
{
var plane = new Plane(UnitVector3D.Parse(pns), Point3D.Parse(prps));
var otherPlane = new Plane(UnitVector3D.Parse(otherPlaneNormalString), Point3D.Parse(otherPlaneRootPointString));
Assert.AreEqual(expectedValue, plane.SignedDistanceTo(otherPlane), 1E-6);
}
public void SignedDistanceToRay(string prps, string pns, string rayThroughPointString, string rayDirectionString, double expectedValue)
{
var plane = new Plane(UnitVector3D.Parse(pns), Point3D.Parse(prps));
var otherPlane = new Ray3D(Point3D.Parse(rayThroughPointString), UnitVector3D.Parse(rayDirectionString));
Assert.AreEqual(expectedValue, plane.SignedDistanceTo(otherPlane), 1E-6);
}
public void PlaneXml(string rootPoint, string unitVector, string xml)
{
var plane = new Plane(Point3D.Parse(rootPoint), UnitVector3D.Parse(unitVector));
var result = AssertXml.XmlSerializerRoundTrip(plane, xml);
Assert.AreEqual(plane, result);
}
public void WGore(double ile)
{
UnitVector3D gora = new UnitVector3D(0, 1, 0);
pozycja -= new Vector3D(gora.X * ile, gora.Y * ile, gora.Z * -ile);
cel -= new Vector3D(gora.X * ile, gora.Y * ile, gora.Z * -ile);
}
public void WBok(double ile)
{
UnitVector3D prawo = new UnitVector3D(1, 0, 0);
pozycja -= new Vector3D(prawo.X * ile, prawo.Y * ile, prawo.Z * -ile);
cel -= new Vector3D(prawo.X * ile, prawo.Y * ile, prawo.Z * -ile);
}
public void DoPrzodu(double ile)
{
UnitVector3D przod = new UnitVector3D(0, 0, 1);
pozycja -= new Vector3D(przod.X * ile, przod.Y * ile, przod.Z * -ile);
cel -= new Vector3D(przod.X * ile, przod.Y * ile, przod.Z * -ile);
}
// window
public MainWindow() : base(WindowType.Toplevel)
{
Build();
isInitialized = false;
random = new Random();
// visual
surface = new ImageSurface(Format.RGB24, 2000, 2000);
root.ExposeEvent += (sender, e) => {
Context gc = Gdk.CairoHelper.Create(root.GdkWindow);
gc.SetSourceSurface(surface, 0, 0);
gc.Paint();
gc.Dispose();
};
root.AddEvents((int)Gdk.EventMask.ButtonPressMask);
// geometry
double dx = Math.Sin(INIT_ROT) * Math.Sin(Y_ANG);
double dy = Math.Cos(Y_ANG);
double dz = Math.Cos(INIT_ROT) * Math.Sin(Y_ANG);
UnitVector3D norm = new UnitVector3D(dx, dy, dz);
Point3D rootPoint = norm.ScaleBy(INIT_DIST).ToPoint3D();
proj = new Plane(rootPoint, norm);
yAxis = proj.Project(new UnitVector3D(0, 1, 0)).Direction;
xAxis = yAxis.CrossProduct(norm);
}
public void Orthogonal(string vs)
{
Vector3D v = Vector3D.Parse(vs);
UnitVector3D orthogonal = v.Orthogonal;
Assert.IsTrue(orthogonal.DotProduct(v) < 1e-6);
}
private void ProjectPoint(Point3D pointToProject, Point3D planeRootPoint, UnitVector3D planeNormal, Point3D projectedresult)
{
var plane = new Plane(planeNormal, planeRootPoint);
var projectOn = plane.Project(pointToProject);
AssertGeometry.AreEqual(projectedresult, projectOn, float.Epsilon);
}
public void SetToRotateToTest(string vs, string vts, string axisString)
{
var v = UnitVector3D.Parse(vs);
var vt = UnitVector3D.Parse(vts);
UnitVector3D?axis = null;
if (axisString != null)
{
axis = UnitVector3D.Parse(axisString);
}
CoordinateSystem cs = CoordinateSystem.RotateTo(v, vt, axis);
var rv = cs.Transform(v);
AssertGeometry.AreEqual(vt, rv);
CoordinateSystem invert = cs.Invert();
Vector3D rotateBack = invert.Transform(rv);
AssertGeometry.AreEqual(v, rotateBack);
cs = CoordinateSystem.RotateTo(vt, v, axis);
rotateBack = cs.Transform(rv);
AssertGeometry.AreEqual(v, rotateBack);
}
public void PlaneProtoBuf(string rootPoint, string unitVector)
{
var plane = new Plane(Point3D.Parse(rootPoint), UnitVector3D.Parse(unitVector));
var result = this.ProtobufRoundTrip(plane);
Assert.AreEqual(plane, result);
}
public void PlaneBinaryFormatter(string rootPoint, string unitVector)
{
var plane = new Plane(Point3D.Parse(rootPoint), UnitVector3D.Parse(unitVector));
var result = this.BinaryFormmaterRoundTrip(plane);
Assert.AreEqual(plane, result);
}
/// <summary>
/// Generates a rotation matrix about a given Z unit vector.
/// </summary>
/// <param name="zUnitVector">Z unit vector</param>
/// <param name="angle">Angle of rotation in radians</param>
/// <returns>Rotation matrix</returns>
public static Matrix <double> RotationMatrix(UnitVector3D zUnitVector, double angle)
{
return(Matrix3D.RotationAroundArbitraryVector(zUnitVector, Angle.FromRadians(angle)));
/*
* var cos = Math.Cos(angle);
* var sin = Math.Sin(angle);
*
* // TRANSPOSED MATRIX TO ACCOMODATE MULTIPLY FUNCTION
* return new Matrix3D(
* cos + Math.Pow(zUnitVector.X, 2) * (1 - cos),
* zUnitVector.Y * zUnitVector.X * (1 - cos) + zUnitVector.Z * sin,
* zUnitVector.Z * zUnitVector.X * (1 - cos) - zUnitVector.Y * sin,
* 0,
*
* zUnitVector.X * zUnitVector.Y * (1 - cos) - zUnitVector.Z * sin,
* cos + Math.Pow(zUnitVector.Y, 2) * (1 - cos),
* zUnitVector.Z * zUnitVector.Y * (1 - cos) + zUnitVector.X * sin,
* 0,
*
* zUnitVector.X * zUnitVector.Z * (1 - cos) + zUnitVector.Y * sin,
* zUnitVector.Y * zUnitVector.Z * (1 - cos) - zUnitVector.X * sin,
* cos + Math.Pow(zUnitVector.Z, 2) * (1 - cos),
* 0,
*
* 0, 0, 0, 1
* );
*/
}
public void XmlRoundtrip()
{
var uv = UnitVector3D.Create(0.2672612419124244, -0.53452248382484879, 0.80178372573727319);
var xml = "<UnitVector3D X=\"0.2672612419124244\" Y=\"-0.53452248382484879\" Z=\"0.80178372573727319\" />";
AssertXml.XmlRoundTrips(uv, xml, (expected, actual) => AssertGeometry.AreEqual(expected, actual));
}
public void Parse(string rootPoint, string unitVector, string eps, string evs)
{
var ray = new Ray3D(Point3D.Parse(rootPoint), UnitVector3D.Parse(unitVector));
AssertGeometry.AreEqual(Point3D.Parse(eps), ray.ThroughPoint);
AssertGeometry.AreEqual(Vector3D.Parse(evs), ray.Direction);
}
public void DirectionsTest(string p1s, string p2s, string evs)
{
var l = LineSegment3D.Parse(p1s, p2s);
var excpected = UnitVector3D.Parse(evs, tolerance: 1);
AssertGeometry.AreEqual(excpected, l.Direction);
}
public void Scale(string ivs, double s, string exs)
{
var uv = UnitVector3D.Parse(ivs);
var v = uv.ScaleBy(s);
AssertGeometry.AreEqual(Vector3D.Parse(exs), v, float.Epsilon);
}
public void PlaneDataContract(string rootPoint, string unitVector, string xml)
{
var plane = new Plane(Point3D.Parse(rootPoint), UnitVector3D.Parse(unitVector));
var result = this.DataContractRoundTrip(plane, xml);
Assert.AreEqual(plane, result);
}
public void DirectionsTest(string p1s, string p2s, string evs)
{
Line3D l = Line3D.Parse(p1s, p2s);
var excpected = UnitVector3D.Parse(evs);
AssertGeometry.AreEqual(excpected, l.Direction);
}
public double GetRoll() // which unit vector is used to calculate direction?
{
double actual_roll = 0;
try
{
UnitVector3D uv = new UnitVector3D(Z.X, Z.Y, Z.Z);
UnitVector3D u0 = new UnitVector3D(Z.X, 0, Z.Z);
MathNet.Spatial.Units.Angle angle = uv.AngleTo(u0);
if (Y.X < 0)
{
actual_roll = -angle.Degrees;
}
else
{
actual_roll = angle.Degrees;
}
}
catch (Exception)
{
}
return(Math.Floor(actual_roll));
}
public double GetPitch()
{
double actual_pitch = 0;
try
{
UnitVector3D uv = new UnitVector3D(X.X, X.Y, X.Z);
UnitVector3D u0 = new UnitVector3D(X.X, 0, X.Z);
MathNet.Spatial.Units.Angle angle = uv.AngleTo(u0);
if (X.Y < 0)
{
actual_pitch = -angle.Degrees;
}
else
{
actual_pitch = angle.Degrees;
}
}
catch (Exception)
{
}
return(Math.Floor(actual_pitch));
}
public void Ray3DProtoBuf(string ps, string vs, bool asElements)
{
var ray = new Ray3D(Point3D.Parse(ps), UnitVector3D.Parse(vs));
var result = this.ProtobufRoundTrip(ray);
Assert.AreEqual(ray, result);
AssertGeometry.AreEqual(ray, result, 1e-6);
}
public void Ray3DBinaryFormatter(string ps, string vs, bool asElements)
{
var ray = new Ray3D(Point3D.Parse(ps), UnitVector3D.Parse(vs));
var result = this.BinaryFormmaterRoundTrip(ray);
Assert.AreEqual(ray, result);
AssertGeometry.AreEqual(ray, result, 1e-6);
}
internal override void PreInit(HealpixManager man)
{
base.PreInit(man);
NormalCalm = new UnitVector3D(new UnitVector3D(-1, 0, 0) * Matrix.Transpose());
KQQaxisTanCotan_traverse = CalcKQQaxisTanCotan_traverse(GetKQQaxis_traverse());
}
public IfcDirection(DatabaseIfc db, Vector3D v) : base(db)
{
UnitVector3D unit = v.Normalize();
mDirectionRatioX = unit.X;
mDirectionRatioY = unit.Y;
mDirectionRatioZ = unit.Z;
}
public void Normalize(string vs, string evs)
{
var vector = Vector3D.Parse(vs);
var uv = vector.Normalize();
var expected = UnitVector3D.Parse(evs);
AssertGeometry.AreEqual(expected, uv, 1E-6);
}
public static void AreEqual(UnitVector3D expected, UnitVector3D actual, double tolerance = 1e-6, string message = "")
{
if (string.IsNullOrEmpty(message))
message = string.Format("Expected {0} but was {1}", expected, actual);
Assert.AreEqual(expected.X, actual.X, tolerance, message);
Assert.AreEqual(expected.Y, actual.Y, tolerance, message);
Assert.AreEqual(expected.Z, actual.Z, tolerance, message);
}
public void ToString(string vs, string format, string expected, double tolerance)
{
var v = UnitVector3D.Parse(vs);
string actual = v.ToString(format);
Assert.AreEqual(expected, actual);
AssertGeometry.AreEqual(v, UnitVector3D.Parse(actual), tolerance);
}
public void Ray3DDataContract(string ps, string vs, bool asElements, string xml)
{
var ray = new Ray3D(Point3D.Parse(ps), UnitVector3D.Parse(vs));
var result = this.DataContractRoundTrip(ray, xml);
Assert.AreEqual(ray, result);
AssertGeometry.AreEqual(ray, result, 1e-6);
}
public Circle3D(Point3D centerPoint, UnitVector3D axis, double radius)
{
this.CenterPoint = centerPoint;
this.Axis = axis;
this.Radius = radius;
}
public static void AreEqual(Vector3D expected, UnitVector3D actual, double tolerance = 1e-6, string message = "")
{
AreEqual(expected, actual.ToVector3D(), tolerance, message);
}
public static Vector<double> solve(
List<double[]> Vertices,
List<int[]> TrianglesVertices,
List<double[]> FacetNormals,
Vector<double> X, Point3D oRoot,
UnitVector3D vDir1,
UnitVector3D vDir2)
{
UnitVector3D dx, dy, dz;
dx = new UnitVector3D(new double[] { 1, 0, 0 });
dy = new UnitVector3D(new double[] { 0, 1, 0 });
dz = new UnitVector3D(new double[] { 0, 0, 1 });
UnitVector3D PlaneNormal = vDir1.CrossProduct(vDir2);
double E = 200 * 10 ^ 6;
double v = 0.3;
double h = 1;
double[,] MatrixK = new double[Vertices.Count * 3, Vertices.Count * 3];
double[] Vectorq = new double[Vertices.Count * 3];
double[,] MatrixD = new double[3, 3];
MatrixD[0, 0] = (E / ((1 + v) * (1 - 2 * v))) * (1 - v);
MatrixD[0, 1] = (E / ((1 + v) * (1 - 2 * v))) * (v);
MatrixD[1, 1] = (E / ((1 + v) * (1 - 2 * v))) * (1 - v);
MatrixD[1, 0] = (E / ((1 + v) * (1 - 2 * v))) * (v);
MatrixD[2, 2] = (E / ((1 + v) * (1 - 2 * v))) * (1 - v / 2);
Matrix<double> D = Matrix<double>.Build.DenseOfArray(MatrixD);
for (int j = 0; j < TrianglesVertices.Count; j++) {
int[] tri = TrianglesVertices[j];
UnitVector3D dx0, dy0, dz0;
UnitVector3D dxn, dyn, dzn;
Point3D p1, p2, p3;
Point3D p10, p20, p30;
Point3D p1n, p2n, p3n;
Point3D p11, p21, p31;
p1 = new Point3D(Vertices[tri[0]]);
p2 = new Point3D(Vertices[tri[1]]);
p3 = new Point3D(Vertices[tri[2]]);
p1n = new Point3D(new double[] { X[tri[0] * 2], X[tri[0] * 2 + 1], 0 });
p2n = new Point3D(new double[] { X[tri[1] * 2], X[tri[1] * 2 + 1], 0 });
p3n = new Point3D(new double[] { X[tri[2] * 2], X[tri[2] * 2 + 1], 0 });
dx0 = new UnitVector3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
dz0 = new UnitVector3D(FacetNormals[j]);
dy0 = dz0.CrossProduct(dx0);
dxn = new UnitVector3D(p2n.X - p1n.X, p2n.Y - p1n.Y, p2n.Z - p1n.Z);
dzn = PlaneNormal;
dyn = dxn.CrossProduct(dzn);
p10 = new Point3D(new double[] { 0, 0, 0 });
p20 = new Point3D(new double[] { p2.ToVector().DotProduct(dx0.ToVector()) - p1.ToVector().DotProduct(dx0.ToVector()), 0, 0 });
p30 = new Point3D(new double[] { p3.ToVector().DotProduct(dx0.ToVector()) - p1.ToVector().DotProduct(dx0.ToVector()), p3.ToVector().DotProduct(dy0.ToVector()) - p1.ToVector().DotProduct(dy0.ToVector()), 0 });
p11 = new Point3D(new double[] { 0, 0, 0 });
p21 = new Point3D(new double[] { p2n.ToVector().DotProduct(dxn.ToVector()) - p1n.ToVector().DotProduct(dxn.ToVector()), 0, 0 });
p31 = new Point3D(new double[] { p3n.ToVector().DotProduct(dxn.ToVector()) - p1n.ToVector().DotProduct(dxn.ToVector()), p3n.ToVector().DotProduct(dyn.ToVector()) - p1n.ToVector().DotProduct(dyn.ToVector()), 0 });
double[,] Transform = new double[3 * 3, 3 * 3];
for (int i = 0; i < 3; i++) {
Transform[i * 3 + 0, i * 3 + 0] = Math.Cos(dx0.AngleTo(dx).Radians);
Transform[i * 3 + 0, i * 3 + 1] = Math.Cos(dx0.AngleTo(dy).Radians);
Transform[i * 3 + 0, i * 3 + 2] = Math.Cos(dx0.AngleTo(dz).Radians);
Transform[i * 3 + 1, i * 3 + 0] = Math.Cos(dy0.AngleTo(dx).Radians);
Transform[i * 3 + 1, i * 3 + 1] = Math.Cos(dy0.AngleTo(dy).Radians);
Transform[i * 3 + 1, i * 3 + 2] = Math.Cos(dy0.AngleTo(dz).Radians);
Transform[i * 3 + 2, i * 3 + 0] = Math.Cos(dyn.AngleTo(dx).Radians);
Transform[i * 3 + 2, i * 3 + 1] = Math.Cos(dyn.AngleTo(dy).Radians);
Transform[i * 3 + 2, i * 3 + 2] = Math.Cos(dyn.AngleTo(dz).Radians);
}
double A = (p10.X * (p20.Y - p30.Y) + p20.X * (p30.Y - p10.Y) + p30.X * (p10.Y - p20.Y)) / 2;
double[,] MatrixB = new double[3, 3 * 3];
MatrixB[0, 0] = (1 / (2 * A)) * (p20.Y - p30.Y);
MatrixB[1, 1] = (1 / (2 * A)) * (p30.X - p20.X);
MatrixB[2, 0] = (1 / (2 * A)) * (p30.X - p20.X);
MatrixB[2, 1] = (1 / (2 * A)) * (p20.Y - p30.Y);
MatrixB[0, 3] = (1 / (2 * A)) * (p30.Y - p10.Y);
MatrixB[1, 4] = (1 / (2 * A)) * (p10.X - p30.X);
MatrixB[2, 3] = (1 / (2 * A)) * (p10.X - p30.X);
MatrixB[2, 4] = (1 / (2 * A)) * (p30.Y - p10.Y);
MatrixB[0, 6] = (1 / (2 * A)) * (p10.Y - p20.Y);
MatrixB[1, 7] = (1 / (2 * A)) * (p20.X - p10.X);
MatrixB[2, 6] = (1 / (2 * A)) * (p20.X - p10.X);
MatrixB[2, 7] = (1 / (2 * A)) * (p10.Y - p20.Y);
Matrix<double> T = Matrix<double>.Build.DenseOfArray(Transform);
Console.WriteLine(T);
Matrix<double> B = Matrix<double>.Build.DenseOfArray(MatrixB);
Console.WriteLine(B);
Matrix<double> Ke = B.Transpose() * D * B * h * A;
Console.WriteLine(Ke);
Ke = T.Transpose() * Ke * T;
Console.WriteLine(Ke);
for (int i1 = 0; i1 < 3; i1++) {
for (int i2 = 0; i2 < 3; i2++) {
for (int l = 0; l < 3; l++) {
for (int c = 0; c < 3; c++) {
MatrixK[tri[i1] * 3 + l, tri[i2] * 3 + c] += Ke[i1 * 3 + l, i2 * 3 + c];
}
}
}
}
Matrix<double> Temp = Matrix<double>.Build.DenseOfArray(MatrixK);
Console.WriteLine(Temp);
double[] vectorqt = new double[3 * 3];
vectorqt[0 * 3] += p11.X - p10.X;
vectorqt[0 * 3 + 1] += p11.Y - p10.Y;
vectorqt[1 * 3] += p21.X - p20.X;
vectorqt[1 * 3 + 1] += p21.Y - p20.Y;
vectorqt[2 * 3] += p31.X - p30.X;
vectorqt[2 * 3 + 1] += p31.Y - p30.Y;
Vector<double> qt = Vector<double>.Build.DenseOfArray(vectorqt);
Console.WriteLine(qt);
qt = T.Transpose() * qt * T;
Console.WriteLine(qt);
for (int i1 = 0; i1 < 3; i1++) {
for (int l = 0; l < 3; l++) {
Vectorq[tri[i1] * 3 + l] -= qt[i1 * 3 + l];
}
}
Vector<double> VTemp = Vector<double>.Build.DenseOfArray(Vectorq);
Console.WriteLine(VTemp);
}
Matrix<double> K = Matrix<double>.Build.DenseOfArray(MatrixK);
Console.WriteLine(K);
Vector<double> q = Vector<double>.Build.DenseOfArray(Vectorq);
Console.WriteLine(q);
Vector<double> Fi = K * q;
Console.WriteLine(Fi);
double w = 0.5;
Vector<double> qi = Vector<double>.Build.DenseOfArray(q.ToArray());
Vector<double> dqi = Vector<double>.Build.Dense(Vertices.Count);
for (int i = 0; i < 100; i++) {
// Console.WriteLine(dqi);
dqi = K.Solve(Fi);
Console.WriteLine(dqi);
// Console.WriteLine(qi);
qi = qi + w * dqi;
// Console.WriteLine(qi);
// Console.WriteLine(Fi);
Fi = K * qi;
// Console.WriteLine(Fi);
}
return qi;
}
public static Vector<double> Solve(List<double[]> Vertices, List<int[]> TrianglesEdges, List<int[]> Edges, List<int> IndiceOfFixedPoints, Point3D oRoot, UnitVector3D vDir1, UnitVector3D vDir2)
{
double[,] MatrixA = new double[Edges.Count*2, Vertices.Count * 2];
for (int i = 0; i < Edges.Count; i++) {
int[] CurTri = new int[] { 0, 0, 0 };
int indexDown0 = -1;
foreach (int[] t in TrianglesEdges) {
for (int j = 0; j < 3; j++) {
if (t[j] == i) { indexDown0 = j; break; }
}
if (indexDown0 != -1) { CurTri = t; break; }
}
int indexDown1 = indexDown0 - 1;
if (indexDown1 < 0) indexDown1 = 2;
int vi0 =0, vi1 = 0, vi2 = 0;
if (Edges[CurTri[indexDown0]][0]== Edges[CurTri[indexDown1]][0]) {
vi0 = Edges[CurTri[indexDown0]][1];
vi1= Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown1]][1];
} else if (Edges[CurTri[indexDown0]][0] == Edges[CurTri[indexDown1]][1]) {
vi0 = Edges[CurTri[indexDown0]][1];
vi1 = Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown1]][0];
} else if (Edges[CurTri[indexDown0]][1] == Edges[CurTri[indexDown1]][0]) {
vi0 = Edges[CurTri[indexDown0]][0];
vi2 = Edges[CurTri[indexDown0]][1];
vi1 = Edges[CurTri[indexDown1]][1];
} else if (Edges[CurTri[indexDown0]][1] == Edges[CurTri[indexDown1]][0]) {
vi0 = Edges[CurTri[indexDown0]][0];
vi1 = Edges[CurTri[indexDown0]][1];
vi2 = Edges[CurTri[indexDown1]][0];
}
Point3D vd0, vd1, vd2;
vd0 = new Point3D(Vertices[vi0]);
vd1 = new Point3D(Vertices[vi1]);
vd2 = new Point3D(Vertices[vi2]);
Line3D lU, lD;
lU = new Line3D(vd1, vd0);
lD = new Line3D(vd1, vd2);
double Ang = lU.Direction.AngleTo(lD.Direction).Radians;
double Len = lU.Length/ lD.Length ;
MatrixA[i * 2, vi0 * 2] = 1;
MatrixA[i * 2, vi0 * 2 + 1] = 0;
MatrixA[i * 2, vi2 * 2] = -Len * Math.Cos(Ang);
MatrixA[i * 2, vi2 * 2 + 1] = Len * Math.Sin(Ang);
MatrixA[i * 2, vi1 * 2] = Len * Math.Cos(Ang) - 1;
MatrixA[i * 2, vi1 * 2 + 1] = -Len * Math.Sin(Ang);
MatrixA[i * 2 + 1, vi0 * 2] = 0;
MatrixA[i * 2 + 1, vi0 * 2 + 1] = 1;
MatrixA[i * 2 + 1, vi2 * 2] = -Len * Math.Sin(Ang);
MatrixA[i * 2 + 1, vi2 * 2 + 1] = -Len * Math.Cos(Ang);
MatrixA[i * 2 + 1, vi1 * 2] = Len * Math.Sin(Ang);
MatrixA[i* 2 + 1, vi1 * 2 + 1] = Len * Math.Cos(Ang) - 1;
}
double[,] MatrixCa = new double[IndiceOfFixedPoints.Count * 2, Vertices.Count * 2];
double[] VectorR = new double[IndiceOfFixedPoints.Count * 2];
Plane oPlane = new Plane(vDir1.CrossProduct(vDir2), oRoot);
for (int i = 0; i < IndiceOfFixedPoints.Count; i++) {
MatrixCa[i * 2, IndiceOfFixedPoints[i] * 2] = 1;
VectorR[i * 2] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).X;
MatrixCa[i * 2 + 1, IndiceOfFixedPoints[i] * 2 + 1] = 1;
VectorR[i * 2 + 1] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).Y;
}
Matrix<double> Ca = Matrix<double>.Build.DenseOfArray(MatrixCa);
Console.WriteLine(Ca);
Vector<double> R = Vector<double>.Build.DenseOfArray(VectorR);
Console.WriteLine(R);
Matrix<double> A = Matrix<double>.Build.DenseOfArray(MatrixA);
Console.WriteLine(A);
double Penalty = 100;
Matrix<double> Ak = A.Transpose() * A + Penalty * Ca.Transpose() * Ca;
Console.WriteLine(Ak);
Vector<double> X = Ak.Solve(Penalty * Ca.Transpose() * R);
X = Ak.Solve(Penalty * Ca.Transpose() * R);
Console.WriteLine(X);
bool valid = true;
if (!valid) {
var solver = new MathNet.Numerics.LinearAlgebra.Double.Solvers.BiCgStab();
var preconditioner = new MathNet.Numerics.LinearAlgebra.Double.Solvers.MILU0Preconditioner();
var iterator = new MathNet.Numerics.LinearAlgebra.Solvers.Iterator<double>(
new MathNet.Numerics.LinearAlgebra.Solvers.ResidualStopCriterion<double>(1.0e-8),
new MathNet.Numerics.LinearAlgebra.Solvers.IterationCountStopCriterion<double>(1000));
var x = Ak.SolveIterative(Penalty * Ca.Transpose() * R, solver, iterator);
var status = iterator.Status;
Console.WriteLine(X);
}
return X;
}
public void ToDenseVector()
{
var l = Math.Sqrt(1 * 1 + 2 * 2 + 3 * 3);
var uv = new UnitVector3D(1 / l, 2 / l, 3 / l);
var denseVector = uv.ToVector();
Assert.AreEqual(3, denseVector.Count);
Assert.AreEqual(1 / l, denseVector[0], 1e-6);
Assert.AreEqual(2 / l, denseVector[1], 1e-6);
Assert.AreEqual(3 / l, denseVector[2], 1e-6);
}
public void XmlRoundTrips()
{
var uv = new UnitVector3D(0.2672612419124244, -0.53452248382484879, 0.80178372573727319);
var xml = @"<UnitVector3D X=""0.2672612419124244"" Y=""-0.53452248382484879"" Z=""0.80178372573727319"" />";
var elementXml = @"<UnitVector3D><X>0.2672612419124244</X><Y>-0.53452248382484879</Y><Z>0.80178372573727319</Z></UnitVector3D>";
AssertXml.XmlRoundTrips(uv, xml, (e, a) => AssertGeometry.AreEqual(e, a));
var serializer = new XmlSerializer(typeof(UnitVector3D));
var actuals = new[]
{
UnitVector3D.ReadFrom(XmlReader.Create(new StringReader(xml))),
(UnitVector3D)serializer.Deserialize(new StringReader(xml)),
(UnitVector3D)serializer.Deserialize(new StringReader(elementXml))
};
foreach (var actual in actuals)
{
AssertGeometry.AreEqual(uv, actual);
}
}
public Vector<double> Solve(List<double[]> Vertices, List<int[]> Triangles, List<int> IndiceOfFixedPoints, Point3D oRoot, UnitVector3D vDir1, UnitVector3D vDir2)
{
Vector<double> Solution = Vector<double>.Build.Dense(Triangles.Count * 3);
List<List<int[]>> Wheels = new List<List<int[]>>();
for (int i = 0; i < Vertices.Count; i++) {
List<int> TrianglesWithThisVertice = new List<int>();
for (int t = 0; t < Triangles.Count; t++) {
if (Triangles[t][0] == i || Triangles[t][1] == i || Triangles[t][2] == i) TrianglesWithThisVertice.Add(t);
}
int[] SheredEdge = new int[] { 0, 0 };
int Last = 0;
int Firt = 0;
if (Triangles[TrianglesWithThisVertice[0]][0] == i) { SheredEdge = new int[] {i, Triangles[TrianglesWithThisVertice[0]][1] }; }
else if( Triangles[TrianglesWithThisVertice[0]][1] == i) { SheredEdge = new int[] {i, Triangles[TrianglesWithThisVertice[0]][2] }; }
else if (Triangles[TrianglesWithThisVertice[0]][2] == i) { SheredEdge = new int[] { i, Triangles[TrianglesWithThisVertice[0]][0] }; }
List<int[]> WheelEdges = new List<int[]>();
WheelEdges.Add(SheredEdge);
do {
int found=-1 ;
for (int t = 0; t < TrianglesWithThisVertice.Count; t++) {
if (t != Last) {
if (SheredEdge.Contains(Triangles[TrianglesWithThisVertice[t]][0]) && SheredEdge.Contains(Triangles[TrianglesWithThisVertice[t]][1])) {
SheredEdge = new int[] { i, Triangles[TrianglesWithThisVertice[t]][2] }; found = t;
} else if (SheredEdge.Contains(Triangles[TrianglesWithThisVertice[t]][1]) && SheredEdge.Contains(Triangles[TrianglesWithThisVertice[t]][2])) {
SheredEdge = new int[] { i, Triangles[TrianglesWithThisVertice[t]][0] }; found = t;
} else if (SheredEdge.Contains(Triangles[TrianglesWithThisVertice[t]][0]) && SheredEdge.Contains(Triangles[TrianglesWithThisVertice[t]][2])) {
SheredEdge = new int[] { i, Triangles[TrianglesWithThisVertice[t]][1] }; found = t;
}
}
}
if (found != -1) {
Last = found;
if (Last == Firt) goto ClosedWeel;
WheelEdges.Add(SheredEdge);
} else goto Continue;
} while (true);
ClosedWeel:
Wheels.Add(WheelEdges);
Continue:
continue;
}
Matrix <double> StiffMatrix = Matrix<double>.Build.Dense(Vertices.Count+ Triangles.Count+ Wheels.Count, Triangles.Count * 3);
//Vertex consistency ∑ei = 2π −∑αi
Vector<double> VertexConsistency = Vector<double>.Build.Dense(Vertices.Count + Triangles.Count + Wheels.Count);
for (int i = 0; i < Vertices.Count; i++) {
Point3D CurrentVertex = new Point3D(Vertices[i]);
List<double> Angles = new List<double>();
foreach (int[] t in Triangles) {
int i1 = -1, i2 = 0, i0 = 0;
if (t[0] == i) { i1 = 0; } else if (t[1] == i) { i1 = 1; } else if (t[2] == i) { i1 = 2; }
if (i1 != -1) {
i2 = i1 + 1;
if (i2 == 3) i2 = 0;
i0 = i1 - 1;
if (i0 == -1) i0 = 2;
Point3D vd0, vd1, vd2;
vd0 = new Point3D(Vertices[t[i0]]);
vd1 = new Point3D(Vertices[t[i1]]);
vd2 = new Point3D(Vertices[t[i2]]);
Line3D lU, lD;
lU = new Line3D(vd1, vd0);
lD = new Line3D(vd1, vd2);
Angles.Add(lD.Direction.AngleTo(lU.Direction).Radians);
}
}
VertexConsistency[i] = 2 * Math.PI - Angles.Sum();
}
//Triangle Consistency eα +eβ +eγ = π − (α + β + γ)
Vector<double> TriangleConsistency = Vector<double>.Build.Dense(Vertices.Count + Triangles.Count + Wheels.Count);
for (int t = 0; t < Triangles.Count; t++) {
double[] Angles = new double[] { 0, 0, 0 };
for (int i = 0; i < 3; i++) {
int i1 = i, i2 = 0, i0 = 0;
i2 = i1 + 1;
if (i2 == 3) i2 = 0;
i0 = i1 - 1;
if (i0 == -1) i0 = 2;
Point3D vd0, vd1, vd2;
vd0 = new Point3D(Vertices[Triangles[t][i0]]);
vd1 = new Point3D(Vertices[Triangles[t][i1]]);
vd2 = new Point3D(Vertices[Triangles[t][i2]]);
Line3D lU, lD;
lU = new Line3D(vd1, vd0);
lD = new Line3D(vd1, vd2);
Angles[i] = lD.Direction.AngleTo(lU.Direction).Radians;
StiffMatrix[Vertices.Count + t, t * 3 + i] = 1;
}
TriangleConsistency[Vertices.Count + t] = Math.PI - Angles.Sum();
}
//Wheel Consistency ∑cot(βi) eβi − cot(γi) eγi =∑log(sinγi) − log(sinβi)
Vector<double> WheelConsistency = Vector<double>.Build.Dense(Vertices.Count + Triangles.Count + Wheels.Count);
for (int t = 0; t < Wheels.Count; t++) {
double cotβi = 0;
double cotγi = 0;
double logSinβi = 0;
double logSinγi = 0;
double[] Angles = new double[] { 0, 0, 0 };
for (int i = 0; i < Wheels[t].Count; i++) {
Point3D vd0, vd1, vd2;
vd0 = new Point3D(Vertices[Wheels[t][i][0]]);
vd1 = new Point3D(Vertices[Wheels[t][i][1]]);
int other = i + 1;
if (other == Wheels[t].Count) other = 0;
vd2 = new Point3D(Vertices[Wheels[t][other][1]]);
Line3D lV1, lV2, lOp;
lV1 = new Line3D(vd0, vd1);
lV2 = new Line3D(vd0, vd2);
lOp = new Line3D(vd1, vd2);
cotβi += Acot(Math.Abs(lV2.Direction.AngleTo(lOp.Direction).Radians));
cotγi += Acot(Math.Abs(lV1.Direction.AngleTo(lOp.Direction).Radians));
logSinβi += Math.Log(Math.Sin(Math.Abs(lV2.Direction.AngleTo(lOp.Direction).Radians)));
logSinγi += Math.Log(Math.Sin(Math.Abs(lV1.Direction.AngleTo(lOp.Direction).Radians)));
StiffMatrix[Vertices.Count + t, t * 3 + i] = 1;
}
TriangleConsistency[Vertices.Count + t] = Math.PI - Angles.Sum();
}
double[,] MatrixA = new double[Vertices.Count * 2, Vertices.Count * 2];
for (int i = 0; i < Vertices.Count; i++) {
int[] CurTri = new int[] { 0, 0, 0 };
int indexDown0 = -1;
foreach (int[] t in Triangles) {
for (int j = 0; j < 3; j++) {
if (t[j] == i) { indexDown0 = j; break; }
}
if (indexDown0 != -1) { CurTri = t; break; }
}
int indexDown1 = indexDown0 - 1;
if (indexDown1 < 0) indexDown1 = 2;
int indexDown2 = indexDown1 - 1;
if (indexDown2 < 0) indexDown2 = 2;
Point3D vd0, vd1, vd2;
vd0 = new Point3D(Vertices[CurTri[indexDown0]]);
vd1 = new Point3D(Vertices[CurTri[indexDown1]]);
vd2 = new Point3D(Vertices[CurTri[indexDown2]]);
Line3D lU, lD;
lU = new Line3D(vd1, vd0);
lD = new Line3D(vd1, vd2);
double Ang = -lU.Direction.AngleTo(lD.Direction).Radians;
double Len = lD.Length / lU.Length;
MatrixA[CurTri[indexDown0] * 2, CurTri[indexDown0] * 2] = 1;
MatrixA[CurTri[indexDown0] * 2, CurTri[indexDown0] * 2 + 1] = 0;
MatrixA[CurTri[indexDown0] * 2, CurTri[indexDown1] * 2] = -Len * Math.Cos(Ang);
MatrixA[CurTri[indexDown0] * 2, CurTri[indexDown1] * 2 + 1] = Len * Math.Sin(Ang);
MatrixA[CurTri[indexDown0] * 2, CurTri[indexDown2] * 2] = Len * Math.Cos(Ang) - 1;
MatrixA[CurTri[indexDown0] * 2, CurTri[indexDown2] * 2 + 1] = -Len * Math.Sin(Ang);
MatrixA[CurTri[indexDown0] * 2 + 1, CurTri[indexDown0] * 2] = 0;
MatrixA[CurTri[indexDown0] * 2 + 1, CurTri[indexDown0] * 2 + 1] = 1;
MatrixA[CurTri[indexDown0] * 2 + 1, CurTri[indexDown1] * 2] = -Len * Math.Sin(Ang);
MatrixA[CurTri[indexDown0] * 2 + 1, CurTri[indexDown1] * 2 + 1] = -Len * Math.Cos(Ang);
MatrixA[CurTri[indexDown0] * 2 + 1, CurTri[indexDown2] * 2] = Len * Math.Sin(Ang);
MatrixA[CurTri[indexDown0] * 2 + 1, CurTri[indexDown2] * 2 + 1] = Len * Math.Cos(Ang) - 1;
}
double[,] MatrixCa = new double[IndiceOfFixedPoints.Count * 2, Vertices.Count * 2];
double[] VectorR = new double[IndiceOfFixedPoints.Count * 2];
Plane oPlane = new Plane(vDir1.CrossProduct(vDir2), oRoot);
for (int i = 0; i < IndiceOfFixedPoints.Count; i++) {
MatrixCa[i * 2, IndiceOfFixedPoints[i] * 2] = 1;
VectorR[i * 2] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).X;
MatrixCa[i * 2 + 1, IndiceOfFixedPoints[i] * 2 + 1] = 1;
VectorR[i * 2 + 1] = new Point3D(Vertices[IndiceOfFixedPoints[i]]).ProjectOn(oPlane).Y;
}
Matrix<double> Ca = Matrix<double>.Build.DenseOfArray(MatrixCa);
Console.WriteLine(Ca);
Vector<double> R = Vector<double>.Build.DenseOfArray(VectorR);
Console.WriteLine(R);
Matrix<double> A = Matrix<double>.Build.DenseOfArray(MatrixA);
Console.WriteLine(A);
double Penalty = 1000;
Matrix<double> Ak = A.Transpose() * A + Penalty * Ca.Transpose() * Ca;
Console.WriteLine(Ak);
Vector<double> X = Ak.Solve(Penalty * Ca.Transpose() * R);
Console.WriteLine(X);
return X;
}
public static Vector<double> solve(
List<double[]> Vertices,
List<int[]> TrianglesVertices,
List<double[]> FacetNormals,
Vector<double> X, Point3D oRoot,
UnitVector3D vDir1,
UnitVector3D vDir2, Mesh M)
{
//Method from article -Wang,Smith. Surface flattening based on energy model. Computer-Aided Design (2002) 823-833
//PseudoCode
//Input: P - Set of nodes, in the initial position and N is the number of nodes
//Output: the final positions of P with E(o) minimized
// FOR i = 1 TO n
// mi = p / 3 Sum(Ak), where Ak is the area
// WHILE (Relative Area difference Es > Permissible accuracy or Relative edge length difference Ec > Permissible accuracy)
// AND Variation of E(o) > Permissible percentage €
// AND the number of iterations < Permissible number N
// FOR i = 1 TO n
// Compute Tensile force of Node Pi: Fi = Sum(C * (Dist(PiPj) - Dist(QiQj)))nij where(P - 2D - Q - 3D nij - Vector Pi to Pj)
// Compute new position of Pi qi = qi + dtqi. + dt ^ 2 / 2 qi..where qi.= qi.+ dtqi..and qi..= Fi / mi
// Compute Penalty force and aplly to Fpi
// Compute new position of Pi qi = qi + dtqi. + dt ^ 2 / 2 qi..where qi.= qi.+ dtqi..and qi..= Fpi / mi
// Compute new Es= Sum(TotalAreaNow - TotalAreaBefore) / TotalAreaNow
// Compute new Ec= Sum(TotalLenghtNow - TotalLenghtBefore) / TotalLenghtNow
// Compute new E(o)Sum(E(pi)) where E(pi) = 0.5 * Sum(C * (Dist(PiPj) - Dist(QiQj))) ^ 2
double[] Mass = new double [Vertices.Count];
int[] MassCounter = new int[Vertices.Count];
double Permissible = 0.00000001;
Vector<double> Fi = Vector<double>.Build.Dense(2);
List<Vector<double>> dqi = new List<Vector<double>>();
List<Vector<double>> qi = new List<Vector<double>>();
double LastEc = 0, Ec = 0;
double LastEs = 0, Es = 0;
double LastEo = 0, Eo = 0;
double C = 0.5;
double ro = 1;
double t = 0.01;
int N = 100;
int Iteration = 0;
double Total = 0;
double LastTotal = 0;
foreach (int[] tri in TrianglesVertices) {
double A = getArea(Vertices[tri[0]], Vertices[tri[1]], Vertices[tri[2]]);
double P = getPerimeter(Vertices[tri[0]], Vertices[tri[1]], Vertices[tri[2]]);
Mass[tri[0]] += ((double)1 / (double)3) * A * ro;
Mass[tri[1]] += ((double)1 / (double)3) * A * ro;
Mass[tri[2]] += ((double)1 / (double)3) * A * ro;
MassCounter[tri[0]] += 1;
MassCounter[tri[1]] += 1;
MassCounter[tri[2]] += 1;
LastEc += A;
LastEs += P;
}
for (int i = 0; i < Vertices.Count; i++) {
Mass[i] = Mass[i] / MassCounter[i];
qi.Add(Vector<double>.Build.DenseOfArray(new double[] { X[i * 2], X[i * 2 + 1], 0 }));
dqi.Add(Vector<double>.Build.DenseOfArray(new double[] { 0, 0, 0 }));
}
do {
Iteration += 1;
LastEo = Eo;
Eo = 0;
Total = 0;
for (int i = 0; i < Vertices.Count; i++) {
Point3D Pi = new Point3D(qi[i][0], qi[i][1], qi[i][2]);
Point3D Qi = new Point3D(Vertices[i][0], Vertices[i][1], Vertices[i][2]);
Fi = Vector<double>.Build.Dense(3);
for (int j = i+1; j < Vertices.Count; j++) {
if (i == j) continue;
Point3D Pj = new Point3D(qi[j][0], qi[j][1], qi[j][2]);
Point3D Qj = new Point3D(Vertices[j][0], Vertices[j][1], Vertices[j][2]);
UnitVector3D nij = new UnitVector3D((Pi.ToVector()- Pj.ToVector()).ToArray());
Fi += C * ((Pi.DistanceTo(Pj) - Qi.DistanceTo(Qj)) * nij).ToVector();
Eo += Math.Pow(C * ((Pi.DistanceTo(Pj) - Qi.DistanceTo(Qj))), 2);
}
Vector<double> ddqi = Fi / Mass[i];
Console.WriteLine(Fi);
dqi[i] += t * ddqi;
qi[i] += dqi[i] * t + 0.5 * Math.Pow(t, 2) * ddqi;
Total += Math.Abs(qi[i][0] - X[i * 2]);
Total += Math.Abs(qi[i][1] - X[i * 2] + 1);
}
Console.WriteLine(Total- LastTotal);
LastTotal = Total;
LastEc = Ec;
Ec = 0;
LastEs = Es;
Es = 0;
foreach (int[] tri in TrianglesVertices) {
double A = getArea(Vertices[tri[0]], Vertices[tri[1]], Vertices[tri[2]]);
double P = getPerimeter(Vertices[tri[0]], Vertices[tri[1]], Vertices[tri[2]]);
Ec += A;
Es += P;
}
if (((Ec - LastEc) / Ec < Permissible || (Es - LastEs) / Es < Permissible)
&& (Eo - LastEo) / Eo < Permissible
&& Iteration > N) break;
} while (true);
Vector<double> XResult = Vector<double>.Build.DenseOfArray(X.ToArray());
for (int i = 0; i < Vertices.Count; i++) {
//Console.WriteLine(qi[i][0] - X[i * 2]);
Total += Math.Abs(qi[i][0] - X[i * 2]);
XResult[i * 2] = qi[i][0];
//Console.WriteLine(qi[i][1] - X[i * 2+1]);
Total += Math.Abs(qi[i][1] - X[i * 2]+1);
XResult[i * 2+1] = qi[i][1];
}
Console.WriteLine(Total);
return XResult;
}
public Circle3D(Point3D p1, Point3D p2, UnitVector3D axis)
{
this.CenterPoint = Point3D.MidPoint(p1, p2);
this.Axis = axis;
this.Radius = p1.DistanceTo(CenterPoint);
}
internal void ComputePlaneofFlattening()
{
oRoot = new Point3D(new double[] { oVecPlane[0], oVecPlane[1], oVecPlane[2] });
vDir1 = new UnitVector3D(new double[] { oVecPlane[3], oVecPlane[4], oVecPlane[5] });
vDir2 = new UnitVector3D(new double[] { oVecPlane[6], oVecPlane[7], oVecPlane[8] });
}
private void ProjectPoint(Point3D pointToProject, Point3D planeRootPoint, UnitVector3D planeNormal, Point3D projectedresult)
{
var plane = new Plane(planeNormal, planeRootPoint);
var projectOn = plane.Project(pointToProject);
AssertGeometry.AreEqual(projectedresult, projectOn, float.Epsilon);
}