public void Orthogonal(string vs)
{
Vector3D v = Vector3D.Parse(vs);
UnitVector3D orthogonal = v.Orthogonal;
Assert.IsTrue(orthogonal.DotProduct(v) < 1e-6);
}
public static double CentrifugalByDotProduct(BasinDotProduct b, double a, Point3D axisEnd, out double aTraverse)
{
var surfaceCalm = new Plane(b.NormalCalm, b.r);
var QonAxisPlane = new Plane(axisEnd, b.Q3, Basin3.O3);
// aSphere direction
var aSphereLine = surfaceCalm.IntersectionWith(QonAxisPlane);
var b3unit = b.RadiusLine.Direction;
// lays in surfaceCalm plane, directed to equator of AxisOfRotation if Math.Abs used
var aSphere = //Math.Abs
(a * AxisOfRotation.DotProduct(b3unit)); // axisOrtohonal.Direction.DotProduct(aSphereLine.Direction));
var aMeridianLine = surfaceCalm.IntersectionWith(b.MeridianCalm); //new Plane(OzEnd, Q3, O3);
var aTraverseLine = surfaceCalm.IntersectionWith(b.TraverseCalm);
Assert.AreEqual(0, aMeridianLine.Direction.DotProduct(aTraverseLine.Direction), .000000001);
aTraverse = Math.Abs(aSphere * aSphereLine.Direction.DotProduct(aTraverseLine.Direction));
var planeOZ = new Plane(Basin3.Oz);
var planeAxis = new Plane(AxisOfRotation);
//directed to equator of Oz if Math.Abs used
double aMeridian;
/*if (aSphereLine.IsCollinear(aMeridianLine, .1))
* {
* aMeridian = aSphere;
* }
* else*/
{
var dotProduct = aSphereLine.Direction.DotProduct(aMeridianLine.Direction);
aMeridian = Math.Abs(aSphere * dotProduct);
}
// if (AxisOfRotation != Basin.Oz)
var spin = new Plane(Basin3.OzEnd, b3unit.ToPoint3D(), axisEnd).Normal.DotProduct(b3unit);
// "north" hemisphere of AxisOfRotation
if (b3unit.DotProduct(AxisOfRotation) > 0)
{
if (spin > 0)
{
aTraverse = -aTraverse;
}
}
else
{
if (spin < 0)
{
aTraverse = -aTraverse;
}
}
//aMeridian<0 if Q3 between planes or inside of cones (bug when angle is near 90) of OZ and AxisOfRotation
if (planeOZ.SignedDistanceTo(b.Q3) * planeAxis.SignedDistanceTo(b.Q3) < 0)
{
aMeridian = -aMeridian;
}
else
{
var coneAxis = new UnitVector3D((Basin3.Oz + AxisOfRotation).ToVector());
if (new UnitVector3D(b.Q3.ToVector()).DotProduct(coneAxis) > coneAxis.DotProduct(Basin3.Oz))
{
//inside cone
aMeridian = -aMeridian;
}
}
b.Visual = aTraverse;
b.Visual = aMeridian;
return(aMeridian);
}
public double DotProductVector3D()
{
return(UnitVector3D1.DotProduct(Vector3D));
}
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();
}
