public static double Jasnosc(Vector3D zrodlo, Vector3D wierzcholek, Vector3D srodek)
{
zrodlo -= srodek;
wierzcholek -= srodek;
return(Math.Max(0, Math.Cos(zrodlo.AngleTo(wierzcholek).Radians)));
}
/// <summary>
/// Generate a catenoid, then move it to a point on the hemisphere in S^3.
/// </summary>
private static Mesh Catenoid(double scale, Vector3D loc, double rld_height, double waist)
{
double h = waist * 3.5;
//Mesh mesh = StandardCatenoid( waist, h );
Mesh mesh = CatenoidSquared(waist, h);
mesh.Scale(scale * rld_height * 2 / h); // To make the catenoid meet up with the RLD solution.
// First move to north pole, then rotate down to the location.
Func <Vector3D, Vector3D> transform = v =>
{
v = H3Models.BallToUHS(v);
Vector3D northPole = new Vector3D(0, 0, 1);
Vector3D axis = loc.Cross(northPole);
if (!axis.Normalize()) // North or south pole?
{
return(v);
}
double anglXY = Euclidean2D.AngleToCounterClock(new Vector3D(1, 0), new Vector3D(loc.X, loc.Y));
v.RotateXY(anglXY);
double angleDown = loc.AngleTo(northPole);
v.RotateAboutAxis(axis, angleDown);
if (v.DNE || Infinity.IsInfinite(v))
{
throw new System.Exception();
}
return(v);
};
mesh.Transform(transform);
return(mesh);
}
private void OneChain()
{
// Here are all unattached edges.
var edgeKvps = m_edgeMap.Where(kvp => kvp.Value.Count == 1).ToArray();
Edge[] edges = edgeKvps.Select(kvp => kvp.Key).ToArray();
Trace.WriteLine(string.Format("Chain length {0}", edges.Length));
// Find all the vertices connected to two polygons.
// This will necessarily lie on the edges above.
// These are vertices that are candidates for filling in with a polygon.
Vector3D[] vertices = m_vertexMap.Where(kvp => kvp.Value.Count > 1).Select(kvp => kvp.Key).ToArray();
HashSet <Vector3D> edgeVerts = new HashSet <Vector3D>();
foreach (Edge e in edges)
{
edgeVerts.Add(e.Start);
edgeVerts.Add(e.End);
}
//Vector3D[] vertices = edgeVerts.ToArray();
// XXX - Things that are hard.
// - How can we tell when a vertex is done?
// - How to avoid duplicating edges and vertices?
// Attempt to fill those verts.
foreach (Vector3D v in vertices)
{
Edge[] connectedEdges = m_vertexToEdgeMap[v].Intersect(edges, new H3.Cell.EdgeEqualityComparer()).ToArray();
// Is it on the chain?
if (connectedEdges.Length != 2)
{
continue;
}
Edge e1 = connectedEdges[0];
Edge e2 = connectedEdges[1];
Vector3D v1 = e1.Opp(v) - v;
Vector3D v2 = e2.Opp(v) - v;
double angle = v1.AngleTo(v2);
int nSides = PolyWithAngle(angle);
if (nSides != 0)
{
Vector3D normal = v1.Cross(v2);
Polygon poly = Polygon.CreateEuclidean(nSides, v, e1.Opp(v), normal);
// Things may have changed, so we must double check.
if (m_edgeMap[e1].Count == 1 &&
m_edgeMap[e2].Count == 1)
{
AddPoly(poly);
Trace.WriteLine(string.Format("Added {0}-gon", nSides));
}
}
}
}
/// <summary>
///
/// </summary>
/// <param name="source"></param>
/// <param name="target"></param>
/// <param name="positive">正方向</param>
/// <returns></returns>
private double GetRotationAngel(Vector3D source, Vector3D target, Vector3D positive)
{
double angel = source.AngleTo(target);
Vector3D thirdAxis = source.Cross(target).Normalize();
if (thirdAxis.IsAlmostEqualTo(positive)) //如果是逆时针旋转得到则角度返号
{
angel = -angel;
}
return(angel);
}
/// <summary>
/// Assumes we are in the UHS model.
/// Normal to the input sphere means we are along an arc orthogonal to that sphere.
/// </summary>
private static System.Tuple <Vector3D, Vector3D> Thicken(Vector3D v, Sphere normal)
{
if (Tolerance.Zero(v.Z))
{
Sphere s = SphereFuncUHS(v);
Vector3D direction = v - normal.Center;
direction.Normalize();
direction *= s.Radius;
return(new System.Tuple <Vector3D, Vector3D>(v + direction, v - direction));
}
else
{
// We need to get the circle orthogonal to "normal" and z=0 and passing through v.
// v is on normal.
Vector3D z = new Vector3D(0, 0, -1);
Vector3D hypot = normal.Center - v;
double angle = hypot.AngleTo(z);
double d2 = v.Z / Math.Tan(angle);
Vector3D cen = v;
cen.Z = 0;
Vector3D direction = cen - normal.Center;
direction.Normalize();
direction *= d2;
cen += direction;
double rad = v.Dist(cen);
Vector3D n = direction;
n.Normalize();
n.RotateXY(Math.PI / 2);
// Sphere with non-euclidean center v;
Sphere s = SphereFuncUHS(v);
// Two points where this sphere intersects circle.
var result = s.Intersection(new Circle3D()
{
Center = cen, Radius = rad, Normal = n
});
// Our artificial thickening can cause this when s is too big relative to the circle.
// In this case, we are very close to the boundary, so we can do a different calc.
if (result == null)
{
direction = v - normal.Center;
direction.Normalize();
direction *= s.Radius;
result = new System.Tuple <Vector3D, Vector3D>(v + direction, v - direction);
}
return(result);
}
}
//The ToNative() method is in the new schema conversion folder hierarchy
public static SpeckleObject ToSpeckle(this GSA1DLoad dummyObject)
{
var newLines = ToSpeckleBase <GSA1DLoad>();
var typeName = dummyObject.GetType().Name;
var loads = new List <GSA1DLoad>();
var elements = Initialiser.GsaKit.GSASenderObjects.Get <GSA1DElement>();
var members = Initialiser.GsaKit.GSASenderObjects.Get <GSA1DMember>();
var keyword = dummyObject.GetGSAKeyword();
foreach (var k in newLines.Keys)
{
var p = newLines[k];
var loadSubList = new List <GSA1DLoad>();
// Placeholder load object to get list of elements and load values
// Need to transform to axis so one load definition may be transformed to many
var initLoad = new GSA1DLoad()
{
GWACommand = p, GSAId = k
};
try
{
initLoad.ParseGWACommand(elements, members);
}
catch (Exception ex)
{
Initialiser.AppResources.Messenger.Message(MessageIntent.TechnicalLog, MessageLevel.Error, ex,
"Keyword=" + keyword, "Index=" + k);
}
// Create load for each element applied
foreach (var nRef in initLoad.Value.ElementRefs)
{
var load = new GSA1DLoad
{
GWACommand = initLoad.GWACommand,
};
load.Value.Name = initLoad.Value.Name;
load.Value.ApplicationId = initLoad.Value.ApplicationId;
load.Value.LoadCaseRef = initLoad.Value.LoadCaseRef;
if (Initialiser.AppResources.Settings.TargetLayer == GSATargetLayer.Analysis)
{
// Transform load to defined axis
var gsaElem = elements.Where(e => e.Value.ApplicationId == nRef).First();
var elem = gsaElem.Value;
var loadAxis = load.Axis == 0 ? new StructuralAxis(
new StructuralVectorThree(new double[] { 1, 0, 0 }),
new StructuralVectorThree(new double[] { 0, 1, 0 }),
new StructuralVectorThree(new double[] { 0, 0, 1 })) :
Helper.LocalAxisEntity1D(elem.Value.ToArray(), elem.ZAxis); // Assumes if not global, local
load.Value.Loading = initLoad.Value.Loading;
load.Value.Loading.TransformOntoAxis(loadAxis);
// Perform projection
if (load.Projected)
{
var loadDirection = new Vector3D(
load.Value.Loading.Value[0],
load.Value.Loading.Value[1],
load.Value.Loading.Value[2]);
if (loadDirection.Length > 0)
{
var axisX = new Vector3D(elem.Value[5] - elem.Value[0], elem.Value[4] - elem.Value[1], elem.Value[3] - elem.Value[2]);
var angle = loadDirection.AngleTo(axisX);
var factor = Math.Sin(angle.Radians);
load.Value.Loading.Value[0] *= factor;
load.Value.Loading.Value[1] *= factor;
load.Value.Loading.Value[2] *= factor;
}
}
// If the loading already exists, add element ref to list
var match = loadSubList.Count() > 0 ? loadSubList.Where(l => ((l.Value).Loading.Value as List <double>)
.SequenceEqual((load.Value).Loading.Value as List <double>)).First() : null;
if (match != null)
{
match.Value.ElementRefs.Add(nRef);
}
else
{
load.Value.ElementRefs = new List <string>()
{
nRef
};
loadSubList.Add(load);
}
}
else
{
// Transform load to defined axis
var gsaMemb = members.Where(e => (e.Value).ApplicationId == nRef).First();
var memb = gsaMemb.Value;
var loadAxis = load.Axis == 0 ? new StructuralAxis(
new StructuralVectorThree(new double[] { 1, 0, 0 }),
new StructuralVectorThree(new double[] { 0, 1, 0 }),
new StructuralVectorThree(new double[] { 0, 0, 1 })) :
Helper.LocalAxisEntity1D(memb.Value.ToArray(), memb.ZAxis); // Assumes if not global, local
load.Value.Loading = initLoad.Value.Loading;
load.Value.Loading.TransformOntoAxis(loadAxis);
// Perform projection
if (load.Projected)
{
var loadDirection = new Vector3D(
load.Value.Loading.Value[0],
load.Value.Loading.Value[1],
load.Value.Loading.Value[2]);
if (loadDirection.Length > 0)
{
var axisX = new Vector3D(memb.Value[5] - memb.Value[0], memb.Value[4] - memb.Value[1], memb.Value[3] - memb.Value[2]);
var angle = loadDirection.AngleTo(axisX);
var factor = Math.Sin(angle.Radians);
load.Value.Loading.Value[0] *= factor;
load.Value.Loading.Value[1] *= factor;
load.Value.Loading.Value[2] *= factor;
}
}
// If the loading already exists, add element ref to list
var match = loadSubList.Count() > 0 ? loadSubList.Where(l => (l.Value).Loading.Equals(load.Value.Loading)).First() : null;
if (match != null)
{
match.Value.ElementRefs.Add(nRef);
}
else
{
load.Value.ElementRefs = new List <string>()
{
nRef
};
loadSubList.Add(load);
}
}
}
loads.AddRange(loadSubList);
}
if (loads.Count() > 0)
{
Initialiser.GsaKit.GSASenderObjects.AddRange(loads);
}
return((loads.Count() > 0) ? new SpeckleObject() : new SpeckleNull());
}
/// <summary>
/// Aligns the solution using the specified planets
/// </summary>
/// <remarks>WARNING!!! 3D MATH!!!</remarks>
/// <param name="alignment">The planets to align using.</param>
/// <param name="planetLocations">The planet location data.</param>
private void FlattenAlignment(string[] alignment, Dictionary <string, PlanetInfo> planetLocations)
{
// ** STEP ONE **
// Translate everything such that the first alignment planet is at { 0, 0, 0 }
Vector3D offset = planetLocations[alignment[0]].Location;
if (offset.Length > 0)
{
foreach (PlanetInfo curInfo in planetLocations.Values)
{
curInfo.Location -= offset;
}
}
// ** STEP TWO **
// Rotate everything such that the second alignment planet is on the x-axis (without translating the previous alignment planets)
Vector3D xAxis = new Vector3D(1, 0, 0);
Angle angle = planetLocations[alignment[1]].Location.AngleTo(xAxis);
Vector3D rotationAxis = xAxis.CrossProduct(planetLocations[alignment[1]].Location);
if (rotationAxis.Length > 0)
{
rotationAxis = rotationAxis.Normalize().ToVector3D();
foreach (PlanetInfo curInfo in planetLocations.Values)
{
curInfo.Location = curInfo.Location.Rotate(rotationAxis, -angle);
}
}
// ** STEP THREE **
// Rotate everything such that the third alignment planet is on the x/y-plane (without translating the previous alignment planets)
Point3D planetOne = planetLocations[alignment[0]].Location.ToPoint3D();
Point3D planetTwo = planetLocations[alignment[1]].Location.ToPoint3D();
Point3D planetThree = planetLocations[alignment[2]].Location.ToPoint3D();
Line3D oneTwoLine = new Line3D(planetOne, planetTwo);
Line3D lineToThird = oneTwoLine.LineTo(planetThree, false);
Vector3D vectorToThird = (lineToThird.EndPoint - lineToThird.StartPoint);
Vector3D destLine = new Vector3D(vectorToThird.Y < 0 ? -vectorToThird.X : vectorToThird.X, vectorToThird.Y < 0 ? -vectorToThird.Y : vectorToThird.Y, 0);
angle = destLine.AngleTo(vectorToThird);
rotationAxis = destLine.CrossProduct(vectorToThird);
if (rotationAxis.Length > 0)
{
rotationAxis = rotationAxis.Normalize().ToVector3D();
foreach (PlanetInfo curInfo in planetLocations.Values)
{
curInfo.Location = curInfo.Location.Rotate(rotationAxis, -angle);
}
}
// ** STEP FOUR **
// Invert the Z coordinates of all planets to ensure the first non-aligned planet has a positive Z location
if (planetLocations.Values.First(cur => !alignment.Contains(cur.Name)).Location.Z < 0)
{
foreach (PlanetInfo curInfo in planetLocations.Values)
{
curInfo.Location = new Vector3D(curInfo.Location.X, curInfo.Location.Y, -curInfo.Location.Z);
}
}
// Just double check the alignment worked...
foreach (string curAlignment in alignment)
{
if (Math.Round(planetLocations[curAlignment].Location.Z, 4) != 0)
{
throw new Exception("Alignment Failed!!!");
}
}
}
public static void CatenoidBasedSurface()
{
RLD_outputs outputs;
SurfaceInternal(out outputs);
double scale = m_params.Scale;
// Map a point for a given k/m from the hemihypersphere to the complex plane.
// You can also pass in -1 for k to get a point on the equator of the hemihypersphere.
double mInc = Math.PI * 2 / m_params.M;
Func <RLD_outputs, int, int, Vector3D> onPlane = (o, k, m) =>
{
double theta = k == -1 ? 0 : outputs.x_i[k];
theta += Math.PI / 2;
return
(Sterographic.SphereToPlane(
SphericalCoords.SphericalToCartesian(
new Vector3D(1, theta, m * mInc)
)
));
};
// Setup texture coords on fundamental triangle.
// We'll use a fundamental triangle in the southern hemisphere,
// with stereographically projected coords at (0,0), (1,0), and CCW on the unit circle depending on M.
Polygon p = new Polygon();
p.Segments.Add(Segment.Line(new Vector3D(), new Vector3D(1, 0)));
p.Segments.Add(Segment.Arc(new Vector3D(1, 0), onPlane(outputs, 1, 1), onPlane(outputs, -1, 1)));
p.Segments.Add(Segment.Line(onPlane(outputs, -1, 1), new Vector3D()));
int levels = 9;
TextureHelper.SetLevels(levels);
Vector3D[] coords = TextureHelper.TextureCoords(p, Geometry.Spherical, doGeodesicDome: true);
int[] elementIndices = TextureHelper.TextureElements(1, levels);
// Setup a nearTree for the catenoid locations (on the plane).
NearTree nearTree = new NearTree(Metric.Spherical);
for (int k = 1; k < outputs.x_i.Length; k++)
{
for (int m = 0; m <= 1; m++)
{
Vector3D loc = onPlane(outputs, k, m);
nearTree.InsertObject(new NearTreeObject()
{
ID = k, Location = loc
});
}
}
// Given a point on the plane, find the nearest catenoid center and calculate the height of the surface based on that.
// This also calculates the locking of the point.
Func <Vector3D, Tuple <double, Vector3D, Vector3D> > heightAndLocking = coord =>
{
NearTreeObject closest;
if (!nearTree.FindNearestNeighbor(out closest, coord, double.MaxValue))
{
throw new System.Exception();
}
Vector3D locked = new Vector3D();
if (p.Segments[0].IsPointOn(coord) ||
p.Segments[2].IsPointOn(coord))
{
locked = new Vector3D(1, 1, 0, 0);
}
//if( p.Segments[1].IsPointOn( v ) ) // Not working right for some reason, but line below will work.
if (Tolerance.Equal(coord.Abs(), 1))
{
locked = new Vector3D(1, 1, 1, 0);
}
Vector3D vSphere = Sterographic.PlaneToSphere(coord);
Vector3D cSphere = Sterographic.PlaneToSphere(closest.Location);
double dist = vSphere.AngleTo(cSphere);
int k = (int)closest.ID;
double waist = outputs.t_i[k];
double rld_height = outputs.phi_i[k];
double h = waist * 3.5 * 2; // height where catenoid will meet rld_height.
double factor = scale * rld_height * 2 / h; // Artifical scaling so we can see things.
dist /= factor;
double z = double.NaN;
if (dist >= waist)
{
z = waist * DonHatch.acosh(dist / waist);
}
else if (dist >= 0.7 * waist)
{
z = 0;
// Move the coord to the thinnest waist circle.
Mobius m = new Mobius();
m.Hyperbolic(Geometry.Spherical, coord.ToComplex(), waist / dist);
coord = m.Apply(coord);
}
if (dist < waist * 20)
{
locked = new Vector3D(1, 1, 1, 1);
}
return(new Tuple <double, Vector3D, Vector3D>(z * factor, locked, coord));
};
// Calculate all the coordinates.
Vector3D[] locks = new Vector3D[coords.Length];
for (int i = 0; i < coords.Length; i++)
{
Vector3D coord = coords[i];
var hl = heightAndLocking(coord);
locks[i] = hl.Item2;
coord = hl.Item3;
coords[i] = Normal(Sterographic.PlaneToSphere(coord), (double)hl.Item1);
}
// Relax it.
Relax(coords, elementIndices, locks);
Mesh mesh = new Mesh();
Sphere s = new Sphere();
for (int i = 0; i < elementIndices.Length; i += 3)
{
Vector3D a = coords[elementIndices[i]];
Vector3D b = coords[elementIndices[i + 1]];
Vector3D c = coords[elementIndices[i + 2]];
if (a.DNE || b.DNE || c.DNE)
{
continue;
}
for (int m = 0; m <= 0; m++)
{
mesh.Triangles.Add(new Mesh.Triangle(a, b, c));
mesh.Triangles.Add(new Mesh.Triangle(
s.ReflectPoint(a),
s.ReflectPoint(b),
s.ReflectPoint(c)));
a.RotateXY(mInc);
b.RotateXY(mInc);
c.RotateXY(mInc);
}
}
PovRay.WriteMesh(mesh, "RLD.pov");
}
public Angle AngleTo()
{
return(V1.AngleTo(V2));
}
private static void CompoundOfFive24Cells(ref H3.Cell.Edge[] edges)
{
List <H3.Cell.Edge> allEdges = new List <H3.Cell.Edge>();
Vector3D v600 = Sterographic.R3toS3(SimplexCalcs.VertexSpherical(3, 3, 5));
Vector3D v24 = Sterographic.R3toS3(SimplexCalcs.VertexSpherical(3, 4, 3));
Sphere[] mirrors600 = SimplexCalcs.MirrorsSpherical(3, 3, 5);
double a24 = v24.AngleTo(Sterographic.R3toS3(new Vector3D()));
double a600 = v600.AngleTo(Sterographic.R3toS3(new Vector3D()));
Matrix4D m600 = Matrix4D.MatrixToRotateinCoordinatePlane(a600, 2, 3);
Matrix4D m600_ = Matrix4D.MatrixToRotateinCoordinatePlane(-a600, 2, 3);
Matrix4D m24 = Matrix4D.MatrixToRotateinCoordinatePlane(a24, 2, 3);
Matrix4D m24_ = Matrix4D.MatrixToRotateinCoordinatePlane(-a24, 2, 3);
double eLength = 2 * Math.PI / 10; // 600-cell edge length
double a_id = Math.Asin(Math.Sin(eLength / 2) / Math.Sin(Math.PI / 3) * Math.Sin(Math.PI / 2));
eLength = 1.0 / Math.Sin(2 * Math.PI / 5); // icosahedron edge length
double a_i = Math.Asin(Math.Sin(eLength / 2) / Math.Sin(Math.PI / 3) * Math.Sin(Math.PI / 5));
Func <Vector3D, Vector3D> rot600 = v =>
{
v = Sterographic.R3toS3(v);
v = m600.RotateVector(v);
v = Sterographic.S3toR3(v);
return(v);
};
Func <Vector3D, int, Vector3D> rotOne = (v, idx) =>
{
v = Sterographic.R3toS3(v);
v = m24.RotateVector(v);
v = Sterographic.S3toR3(v);
v.RotateAboutAxis(new Vector3D(1, 0, 0), -a_id);
// Vertex to cell center.
v = Sterographic.R3toS3(v);
v = m600_.RotateVector(v);
v = Sterographic.S3toR3(v);
List <int> reflections = new List <int>();
if (idx == 0)
{
reflections.Add(2);
reflections.Add(1);
reflections.Add(2);
reflections.Add(0);
reflections.Add(1);
reflections.Add(2);
reflections.Add(1);
reflections.Add(2);
}
if (idx != 0)
{
reflections.Add(3);
}
if (idx == 2)
{
reflections.Add(1);
reflections.Add(2);
}
if (idx == 3)
{
reflections.Add(2);
reflections.Add(1);
}
if (idx == 4)
{
reflections.Add(1);
reflections.Add(0);
reflections.Add(1);
reflections.Add(2);
reflections.Add(0);
reflections.Add(1);
reflections.Add(0);
reflections.Add(1);
}
foreach (int reflection in reflections)
{
v = mirrors600[reflection].ReflectPoint(v);
}
v = Sterographic.R3toS3(v);
v = m600.RotateVector(v);
v = Sterographic.S3toR3(v);
//v.RotateAboutAxis( new Vector3D( 0, 0, 1 ), Math.PI/3 );
//v.RotateAboutAxis( new Vector3D( 1, 0, 0 ), -a_i*2 );
v = Sterographic.R3toS3(v);
//v = m24_.RotateVector( v );
v = Sterographic.S3toR3(v);
return(v);
};
for (int i = 0; i < 5; i++)
{
//if( i == 0 )
// continue;
allEdges.AddRange(edges.Select(e =>
{
H3.Cell.Edge newEdge = new H3.Cell.Edge(rotOne(e.Start, i), rotOne(e.End, i));
//H3.Cell.Edge newEdge = new H3.Cell.Edge( rot600( e.Start ), rot600( e.End ) );
switch (i)
{
case 0:
newEdge.Color = new Vector3D(1, 0, 0, 1);
//newEdge.Color = new Vector3D( 1, 1, 1, 0 );
break;
case 1:
newEdge.Color = new Vector3D(0, 1, 0, 2);
break;
case 2:
newEdge.Color = new Vector3D(0, 0, 1, 3);
break;
case 3:
newEdge.Color = new Vector3D(1, 0, 1, 4);
break;
case 4:
newEdge.Color = new Vector3D(0, 1, 1, 5);
break;
}
return(newEdge);
}));
}
edges = allEdges.ToArray();
//edges = edges.Where( e => Tolerance.Equal( 1, e.Start.Abs() ) || Tolerance.Equal( 1, e.End.Abs() ) ).ToArray();
HashSet <Vector3D> uniqueVerts = new HashSet <Vector3D>();
foreach (H3.Cell.Edge e in edges)
{
uniqueVerts.Add(e.Start);
uniqueVerts.Add(e.End);
}
System.Diagnostics.Trace.WriteLine("Number of verts = " + uniqueVerts.Count);
/*edges = edges.Where( e =>
* {
* Vector3D v = Tolerance.Equal( 1, e.Start.Abs() ) ? e.End : e.Start;
* if( v.Abs() >= 0.8 || v.Abs() <= 0.7 )
* return false;
*
* if( Tolerance.LessThanOrEqual( v.X, 0 ) || Tolerance.GreaterThanOrEqual( v.Y, 0 ) || Tolerance.GreaterThanOrEqual( v.Z, 0 ) )
* return false;
*
* return true;
* } ).ToArray();
*
* edges = edges.OrderBy( e => Tolerance.Equal( 1, e.Start.Abs() ) ? e.End.Abs() : e.Start.Abs() ).ToArray();*/
}
/// <summary>
/// Given 2 points on the surface of the ball, calculate the center and radius of the orthogonal circle.
/// </summary>
public static void OrthogonalCircle( Vector3D v1, Vector3D v2, out Vector3D center, out double radius )
{
// Picture at http://planetmath.org/OrthogonalCircles.html helpful for what I'm doing here.
double sectorAngle = v1.AngleTo( v2 );
if( Tolerance.Equal( sectorAngle, Math.PI ) )
{
center = Infinity.InfinityVector;
radius = double.PositiveInfinity;
return;
}
double distToCenter = m_pRadius / Math.Cos( sectorAngle / 2 );
center = v1 + v2;
center.Normalize();
center *= distToCenter;
radius = distToCenter * Math.Sin( sectorAngle / 2 );
}
/// <summary>
/// Calculate the hyperbolic midpoint of an edge.
/// Only works for non-ideal edges at the moment.
/// </summary>
public static Vector3D Midpoint( H3.Cell.Edge edge )
{
// Special case if edge has endpoint on origin.
// XXX - Really this should be special case anytime edge goes through origin.
Vector3D e1 = edge.Start;
Vector3D e2 = edge.End;
if( e1.IsOrigin || e2.IsOrigin )
{
if( e2.IsOrigin )
Utils.Swap<Vector3D>( ref e1, ref e2 );
return HalfTo( e2 );
}
// No doubt there is a much better way, but
// work in H2 slice transformed to xy plane, with e1 on x-axis.
double angle = e1.AngleTo( e2 ); // always <= 180
e1 = new Vector3D( e1.Abs(), 0 );
e2 = new Vector3D( e2.Abs(), 0 );
e2.RotateXY( angle );
// Mobius that will move e1 to origin.
Mobius m = new Mobius();
m.Isometry( Geometry.Hyperbolic, 0, -e1 );
e2 = m.Apply( e2 );
Vector3D midOnPlane = HalfTo( e2 );
midOnPlane= m.Inverse().Apply( midOnPlane );
double midAngle = e1.AngleTo( midOnPlane );
Vector3D mid = edge.Start;
mid.RotateAboutAxis( edge.Start.Cross( edge.End ), midAngle );
mid.Normalize( midOnPlane.Abs() );
return mid;
}
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();
}