public void Obroc(Vector3D kat)
{
przod = Math3D.ObrocWokolOsi(przod, gora, -kat.Y);
prawo = gora.CrossProduct(przod);
przod = Math3D.ObrocWokolOsi(przod, prawo, -kat.X);
gora = przod.CrossProduct(prawo);
prawo = Math3D.ObrocWokolOsi(prawo, przod, -kat.Z);
gora = przod.CrossProduct(prawo);
cel = pozycja - przod;
}
private Point2D RelativeToDisplay(Point3D centroid, UnitVector3D norm, UnitVector3D xPositive, Vector2D size, Point3D point)
{
var zPositive = xPositive.CrossProduct(norm);
var vec = point - centroid;
var x = vec.DotProduct(xPositive) / size.X + 0.5;
var y = 1 - (vec.DotProduct(zPositive) / size.Y + 0.5);
return(new Point2D(x, y));
}
/// <summary>
/// Gets a rotation matrix corresponding to a forward vector.
/// </summary>
/// <param name="forward">The specified forward vector.</param>
/// <returns>The corresponding rotation matrix.</returns>
/// <remarks>The X axis of the matrix will correspond to the specified forward vector.</remarks>
public static Matrix <double> ToRotationMatrix(this UnitVector3D forward)
{
// Compute left and up directions from the given forward direction.
var left = UnitVector3D.ZAxis.CrossProduct(forward);
var up = forward.CrossProduct(left);
// Create a corresponding 3x3 matrix from the 3 directions.
var rotationMatrix = Matrix <double> .Build.Dense(3, 3);
rotationMatrix.SetColumn(0, forward.ToVector());
rotationMatrix.SetColumn(1, left.ToVector());
rotationMatrix.SetColumn(2, up.ToVector());
return(rotationMatrix);
}
/// <summary>
/// Calculates position and angle of a new tile connecting to this connection and connects it.
/// The connector given as parameter determines also the connecting object.
/// </summary>
/// <param name="connector">Connector on the new tile by which it connects</param>
/// <returns>New tile in space connected to this connection.</returns>
public TileInSpace ConnectObject(ConnectorOnTile connector)
{
// Place the old and the new object into the same plane or line
TileInSpace newTile = new TileInSpace(connector.OnTile, Point3D.Origin, OnTile.Quaternion);
int index = connector.OnTile.Connectors.IndexOf(connector);
ConnectorOnTileInSpace newConnector = newTile.Connectors[index];
UnitVector3D axis = OnTile.Vertices.Normal; // rotation axis - TRY REVERSE IN CASE OF MALFUNCTION
// Edge-to-edge
if (newConnector.Positions.Count == 2 && Positions.Count == 2)
{
// Rotate the new object so that connector directions are opposite
Vector3D vector1 = Positions[1] - Positions[0];
Vector3D vector2 = newConnector.Positions[0] - newConnector.Positions[1];
Angle angle = vector2.SignedAngleTo(vector1, axis);
newTile.Rotate(axis, angle);
// Rotate the new tile around the connector edge so that
// the angle between both tiles is the angle associated with the connector
newTile.Rotate(vector1.Normalize(), Angle.FromDegrees(180) - Angle); // TRY REVERSE IN CASE OF MALFUNCTION
// Move the new object so that the connectors match
newTile.Move(Positions[0] - newConnector.Positions[1]);
}
// Point-to-point
else if (newConnector.Positions.Count == 1 && Positions.Count == 1)
{
// Rotate the new object around an axis perpendicular to it by the angle associated with the connector
var newSegment = newTile.Vertices as Segment3D;
var thisSegment = OnTile.Vertices as Segment3D;
var thisAxis = thisSegment?.Vector.Normalize() ?? OnTile.Vertices.Normal;
if (newSegment == null)
{
// Tile connecting to a segment
if (thisSegment != null)
{
axis = newTile.Vertices.Normal.CrossProduct(-Math.Sign(connector.Angle.Radians) * thisSegment.Vector).Normalize();
}
// ELSE 2D tile connecting to a 2D tile by a single point - should never happen
}
else
{
if (OnTile.Vertices is Polygon3D) // Segment connecting to a tile
{
axis = axis.CrossProduct(-newSegment.Vector).Normalize();
}
// ELSE segment connecting to a segment - axis set above as a normal to this segment
}
newTile.Rotate(axis, Angle); // TRY REVERSE AXIS OR ROTATION IN CASE OF MALFUNCTION
// the newly connected object has one degree of freedom - rotate randomly
newTile.Rotate(thisAxis, Angle.FromRadians(Randomizer.NextDoubleBetween(0, 2 * Math.PI)));
// Move the new object so that the connectors match
newTile.Move(Positions[0] - newConnector.Positions[0]);
}
else
{
throw new ArgumentException("Connecting incompatible connectors:" + this + "\n and" + newConnector);
}
ConnectTo(newConnector);
return(newTile);
}
public Vector <double> Solve(List <double[]> Vertices, List <int[]> Triangles, List <int> IndiceOfFixedPoints, Point3D oRoot, UnitVector3D vDir1, UnitVector3D vDir2)
{
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)
{
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 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;
}
private Point3D CalibPointOnDisplay(Point3D centroid, UnitVector3D norm, UnitVector3D xPositive, Vector2D size, Point2D relativeCoordinate)
{
var zPositive = xPositive.CrossProduct(norm);
return(centroid + (relativeCoordinate.X - 0.5) * size.X * xPositive + ((1 - relativeCoordinate.Y) - 0.5) * size.Y * zPositive);
}
public UnitVector3D CrossProduct()
{
return(UnitVector3D1.CrossProduct(UnitVector3D2));
}
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 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);
}
void Rekalkulacja()
{
przod = (pozycja - cel).Normalize();
prawo = gora.CrossProduct(przod);
gora = przod.CrossProduct(prawo);
}
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 void DoCommand(int cmd_id)
{
IRobotStructure structure = robot_app.Project.Structure;
// Get bars and nodes
IRobotCollection bars = structure.Bars.GetAll();
IRobotCollection nodes = structure.Nodes.GetAll();
// Create 3D points at nodes
var points = new Dictionary <int, Point3D>();
for (int i = 1; i <= nodes.Count; i++)
{
var node = (IRobotNode)nodes.Get(i);
points[i] = new Point3D(node.X, node.Y, node.Z);
}
// Create 3D vectors for each bar and index of bars connected to a node
var vectors = new Dictionary <int, Vector3D>();
var vect_by_pt = new DefaultDict <int, List <Vector3D> >();
for (int i = 1; i <= bars.Count; i++)
{
var bar = (IRobotBar)bars.Get(i);
var start_pt = points[bar.StartNode];
var end_pt = points[bar.EndNode];
vectors[i] = end_pt - start_pt;
vect_by_pt[bar.StartNode].Add(vectors[i]);
vect_by_pt[bar.EndNode].Add(vectors[i]);
}
;
foreach (KeyValuePair <int, Vector3D> vector in vectors)
{
// `u` is the vector corresponding to the bar
Vector3D u = vector.Value;
UnitVector3D u_norm = u.Normalize();
int start = bars.Get(vector.Key).StartNode;
// TODO: How about the other end?
// Find the most orthogonal vector `v`
Vector3D most_orth_v = u;
double cur_min = 1;
foreach (Vector3D x in vect_by_pt[start])
{
UnitVector3D x_norm = x.Normalize();
double dot_prod = Math.Abs(u_norm.DotProduct(x_norm));
if (dot_prod < cur_min)
{
most_orth_v = x;
cur_min = dot_prod;
}
}
if (cur_min > 0.95)
{
continue;
}
var v = most_orth_v;
var v_norm = v.Normalize();
// Vector `a` is vector a orthogonal to `u` in (u,v) plane
Vector3D a = v - u_norm.DotProduct(v) * u;
UnitVector3D a_norm = a.Normalize();
// Vector `c` is orthogonal to `u` in the global (X,Y) plane
UnitVector3D c = u_norm.CrossProduct(UnitVector3D.ZAxis);
// Vector `d` is orthogonal to `c` and `u`
UnitVector3D d = c.CrossProduct(u_norm);
// Calculate the angles of `a` with `d` and `c`
Angle theta1 = a.AngleTo(d);
Angle theta2 = a.AngleTo(c);
// Calculate gamma from `theta1` and `theta2`
Angle gamma = (theta2.Degrees < 90) ? theta1 : -theta1;
double gamma_up = (gamma.Degrees < 0) ? gamma.Degrees + 90 : gamma.Degrees - 90;
// Set `Gamma` attribute of bar
IRobotBar bar = bars.Get(vector.Key);
bar.Gamma = gamma_up;
}
// Redraw all views
robot_app.Project.ViewMngr.Refresh();
}