/// <summary>
/// Calcualtes if the two vectors intersect
/// </summary>
/// <param name="axis1Dir">The direction of the first axis</param>
/// <param name="axis1Point">a point on the first axis</param>
/// <param name="axis2Dir">The direction of the second axis</param>
/// <param name="axis2Point">a point on the second axis</param>
/// <returns></returns>
public static bool AxisIntersect(MathVector axis1Dir, MathPoint axis1Point, MathVector axis2Dir, MathPoint axis2Point)
{
MathVector pointVect = GetVectorBetweenPoints(axis1Point, axis2Point);
if (pointVect.GetLength() < VectorCalcs.errorVal || VectorCalcs.IsParallel(pointVect.Cross(axis1Dir), pointVect.Cross(axis2Dir)))
return true;
else
return false;
}
public static int oldSubindex (MathVector ic, int l)
{
int i = 0;
for (int k = 0; k < MathVector.NDIM; k++) {
if (((int)ic.value (k) & l) != 0)
i += Cell.NSUB >> (k + 1);
}
return i;
}
/**
* Create a HG object.
*
* @param b the body object
* @param p a vector that represents the body
*/
public static HG makeHG (Body b, MathVector p)
{
HG hg = new HG ();
hg.pskip = b;
hg.pos0 = p.cloneMathVector ();
hg.phi0 = 0.0;
hg.acc0 = MathVector.makeMathVector ();
return hg;
}
/**
* Construct an empty 3 dimensional vector for use in Barnes-Hut algorithm.
*/
public static MathVector makeMathVector()
{
MathVector v = new MathVector();
v.data = new double[NDIM];
for(int i = 0; i < NDIM; i++)
v.data[i] = 0.0;
return v;
}
/**
* Create a copy of the vector.
*
* @return a clone of the math vector
*/
public MathVector cloneMathVector()
{
MathVector v = new MathVector();
v.data = new double[NDIM];
for(int i = 0; i < NDIM; i++)
v.data[i] = data[i];
return v;
}
/**
* Descend Tree and insert particle. We're at a cell so
* we need to move down the tree.
*
* @param p the body to insert into the tree
* @param xpic
* @param l
* @param tree the root of the tree
* @return the subtree with the new body inserted
*/
public override Cell loadTree(Body p, MathVector xpic, int l, BTree tree)
{
// move down one level
int si = Node.oldSubindex(xpic, l);
Node rt = subp[si];
if(rt != null)
subp[si] = rt.loadTree(p, xpic, l >> 1, tree);
else
subp[si] = p;
return this;
}
private static MathVector ClosestPointOnAxis(ITriadManipulator m, IMathPoint p, IMathVector cameraVector, MathVector axis)
{
double pT;
double qT;
if (ClosestPointBetweenLines(m.Origin, axis, p, cameraVector, out pT, out qT))
{
return (MathVector) axis.Scale(pT);
}
else
{
return null;
}
}
/**
* Multiply the vector times a scalar and place the result in this vector.
*
* @param u the vector
* @param s the scalar value
*/
public void multScalar2(MathVector u, double s)
{
for(int i = 0; i < NDIM; i++)
data[i] = u.data[i] * s;
}
/**
* Subtract two vectors and the result is placed in this vector.
*
* @param u the first operand of the subtraction.
* @param v the second opernd of the subtraction
*/
public void subtraction2(MathVector u, MathVector v)
{
for(int i = 0; i < NDIM; i++)
data[i] = u.data[i] - v.data[i];
}
/**
* Subtract two vectors and the result is placed in this vector.
* This vector contain the first operand.
*
* @param u the other operand of the subtraction.
*/
public void subtraction1(MathVector u)
{
for(int i = 0; i < NDIM; i++)
data[i] = data[i] - u.data[i];
}
public void crossProduct(MathVector u, MathVector w)
{
data[0] = u.data[1] * w.data[2] - u.data[2] * w.data[1];
data[1] = u.data[2] * w.data[0] - u.data[0] * w.data[2];
data[2] = u.data[0] * w.data[1] - u.data[1] * w.data[0];
}
/**
* Evaluate gravitational field on the body.
* The original olden version calls a routine named "walkscan",
* but we use the same name that is in the Barnes code.
*/
public void hackGravity (double rsize, Node root)
{
MathVector pos0 = pos.cloneMathVector ();
HG hg = HG.makeHG (this, pos);
hg = root.walkSubTree (rsize * rsize, hg);
phi = hg.phi0;
newAcc = hg.acc0;
}
private void Start()
{
print("向量长度:" + t1.position.magnitude + "_" + MathVector.V3Magnitude(t1.position));
print("向量长度平方:" + t1.position.sqrMagnitude + "_" + MathVector.V3SqrMagnitude(t1.position));
print("向量点乘(内积):" + Vector3.Dot(t1.position, t2.position) + "_" + MathVector.V3Dot(t1.position, t2.position));
print("向量夹角(0到180):" + Vector3.Angle(t1.position, t2.position) + "_" + MathVector.V3Angle(t1.position, t2.position));
print("向量夹角(0到180 带正负):" + Vector3.SignedAngle(t2.position, t1.position, Vector3.up) + "_" + MathVector.V3SignedAngle(t2.position, t1.position, Vector3.up));
print("向量夹角(0到360):" + MathVector.V3SignedAngleBetween(t2.position, t1.position, Vector3.up));
print("向量投影(a在b上投影):" + Vector3.Project(t1.position, t2.position) + "_" + MathVector.V3Project(t1.position, t2.position));
print("向量投影(a在法线为n的平面上的投影向量):" + Vector3.ProjectOnPlane(t1.position, Vector3.forward) + "_" + MathVector.V3ProjectOnPlane(t1.position, Vector3.forward));
print("向量叉乘(外积):" + Vector3.Cross(t1.position, t2.position) + "_" + MathVector.V3Cross(t1.position, t2.position));
Debug.DrawLine(Vector3.zero, MathVector.V3Cross(t1.position, t2.position), Color.black, 1000f);
Vector3 v1 = new Vector3(1, 2, 3);
v1.Add(new Vector3(1, 2, 3));
Debug.Log(v1);
v1.Add(new Vector3(0, 0, 0), new Vector3(1, 1, 1));
Debug.Log(v1);
}
public void incrementalSub(MathVector u)
{
for(int i = 0; i < NDIM; i++)
data[i] = data[i] - u.data[i];
}
/// <summary>
/// Finds if the vectors are perpendicular
/// </summary>
/// <param name="vec1">Vector 1</param>
/// <param name="vec2">Vector 2</param>
/// <returns>True if the vectors are perpendicular</returns>
public static bool IsPerpendicular(MathVector vec1, MathVector vec2)
{
return Math.Abs(vec1.Dot(vec2)) < errorVal;
}
/// <summary>
/// Finds if the vectors are parallel
/// </summary>
/// <param name="vec1">Vector 1</param>
/// <param name="vec2">Vector 2</param>
/// <returns>True if the vectors are parallel</returns>
public static bool IsParallel(MathVector vec1, MathVector vec2)
{
return vec1.Cross(vec2).GetLength() < errorVal;
}
/// <summary>
/// Calculates the limit values of the axis
/// </summary>
/// <param name="limitObjs">Array of the 4 limit selections in order of lowerLimitStop,lowerLimitEdge,upperLimitStop,upperLimitEdge. If any spot is null the current stored value will be used. if limitObjs is null all current values will be used</param>
/// <returns>True if the limits where succesfully calculated</returns>
public override bool CalcLimits(int index, object obj)
{
if (Axis == null)
{
return false;
}
if (UseCustomMovementLimits)
{
return false;
}
bool successfulCalculation = true;
MathVector[] limits = new MathVector[4]; //contains the vectors for each limit axis. for revolute joints the angle is represented and all vectors are normalised, for prismatic all vectors are parallel and the length represents the limit
double[] Point;
if (owner.jointSpecifics.OriginPt != null)
Point = owner.jointSpecifics.OriginValues.Point;
else
Point = this.Point;
object[] limitObjs = { LowerLimitStop, LowerLimitEdge, UpperLimitStop, UpperLimitEdge };
if (index != -1)
{
limitObjs[index] = obj;
}
MathVector axisVector = mathUtil.CreateVector(new double[] { AxisX, AxisY, AxisZ }); //the vector of the rotation/translation axis
double errorVal = .000001;
for (int i = 0; i < limitObjs.Length; i++)
{
if (limitObjs[i] == null)//checks that a limit is stored
{
successfulCalculation = false;
continue;
}
MathTransform componentTransform = null;
Component2 limComp = (Component2)((IEntity)limitObjs[i]).GetComponent();
if ( limComp != null)
{
componentTransform = limComp.Transform2; //stores the component transform if there is one. this will be used to convert the edge vectors to the global coordinate system
}
if (limitObjs[i] is IEdge)//if the face is an edge
{
object[] faces = ((IEdge)limitObjs[i]).GetTwoAdjacentFaces2();
MathVector faceVector1 = mathUtil.CreateVector(((IFace2)faces[0]).Normal);//get normal vectors of each adjacent face, they will be 0 if no normal vector exists
MathVector faceVector2 = mathUtil.CreateVector(((IFace2)faces[1]).Normal);
if (faceVector1.GetLength() > 0 && faceVector2.GetLength() > 0)//if both exist, the edge is a line
{
double[] untransformedPoint = ((IEdge)limitObjs[i]).GetStartVertex().getPoint();
double[] startPoint = VectorCalcs.TransformPoint(componentTransform,untransformedPoint); //gets the start point of the edge and converts it to global space
limits[i] = VectorCalcs.GetScaledVector(axisVector,startPoint,Point);//projects the edge vector on to the axis vector
}
else
{
double[] closestPoint;
if (faceVector1.GetLength() > 0)//gets the closest point on the one face that exist
{
closestPoint = ((IFace2)faces[0]).GetClosestPointOn(Point[0], Point[1], Point[2]);
}
else
{
closestPoint = ((IFace2)faces[1]).GetClosestPointOn(Point[0], Point[1], Point[2]);
}
double[] transformedPoint = VectorCalcs.TransformPoint(componentTransform, closestPoint);//transforms the closest point to global space
limits[i] = VectorCalcs.GetScaledVector(axisVector, transformedPoint,Point);
}
}
else if (limitObjs[i] is IFace2)//if limit is a face
{
double[] closestPoint = ((IFace2)limitObjs[i]).GetClosestPointOn(Point[0], Point[1], Point[2]);//finds the closest point on the face
double[] transformedPoint = VectorCalcs.TransformPoint(componentTransform, closestPoint);//transforms the point to global space
limits[i] = VectorCalcs.GetScaledVector(axisVector, transformedPoint,Point);
}
else if (limitObjs[i] is IVertex)//if limit is a vertex
{
double[] transformedPoint = VectorCalcs.TransformPoint(componentTransform, ((IVertex)limitObjs[i]).GetPoint());//transforms the vertex's coordinates to global space
limits[i] = VectorCalcs.GetScaledVector(axisVector, transformedPoint,Point);
}
else
{
object tempObject = ((IFeature)limitObjs[i]).GetSpecificFeature2();
if (tempObject is IRefAxis)//if limit is a referance axis
{
double[] refAxisPoints = ((IRefAxis)tempObject).GetRefAxisParams();
double[] transformedPoint;
if (componentTransform != null)//if the axis is part of a subcomponent transform its point to global space
{
transformedPoint = VectorCalcs.TransformPoint(componentTransform, refAxisPoints);
}
else
{
transformedPoint = new double[] { refAxisPoints[0], refAxisPoints[1], refAxisPoints[2] };
}
limits[i] = VectorCalcs.GetScaledVector(axisVector, transformedPoint,Point);
}
else if (tempObject is IRefPlane)//if limit is a reference plane
{
MathTransform planeTransform = ((IRefPlane)tempObject).Transform;//gets the transform of the plane
if (componentTransform != null)//if the plane is part of a sub component, transform it again into global space
{
planeTransform = planeTransform.Multiply(componentTransform);
}
double[] tempArr = { planeTransform.ArrayData[9], planeTransform.ArrayData[10], planeTransform.ArrayData[11] };
limits[i] = VectorCalcs.GetScaledVector(axisVector, tempArr,Point);
}
else if (tempObject is IRefPoint)//if limit is a reference point
{
double[] transformedPoint;
if (componentTransform != null)//gets point location and transforms to global space if the point is part of a sub component
{
transformedPoint = ((IRefPoint)tempObject).GetRefPoint().MultiplyTransform(componentTransform).ArrayData;
}
else
{
transformedPoint = ((IRefPoint)tempObject).GetRefPoint().ArrayData;
}
limits[i] = VectorCalcs.GetScaledVector(axisVector, transformedPoint,Point);
}
}
}
MathVector movementVector;
movementVector = null;
if (limitObjs[0] != null && limitObjs[1] != null)
{
LowerLimit = -limits[0].Subtract(limits[1]).GetLength();//length between the lower limits
movementVector = limits[1].Subtract(limits[0]).Normalise();
}
else
{
LowerLimit = 0;
}
if (limitObjs[2] != null && limitObjs[3] != null)
{
UpperLimit = limits[2].Subtract(limits[3]).GetLength();//length between the upper limits
movementVector = limits[2].Subtract(limits[3]).Normalise();
}
else
{
UpperLimit = 0;
}
if (movementVector != null)
{
AxisIsFlipped = (movementVector.Add(axisVector).GetLength() < errorVal) ^ AxisIsFlipped;//if the movment is in the opposite direction as the axis, the axis must be flipped
}
limitVectors = limits;
return successfulCalculation;
}
/**
* Compute integerized coordinates.
*
* @return the coordinates or null if rp is out of bounds
*/
public MathVector intcoord (MathVector vp)
{
MathVector xp = MathVector.makeMathVector ();
double imxv = (double)Node.IMAX;
double xsc = (vp.value (0) - rmin.value (0)) / rsize;
if (0.0 <= xsc && xsc < 1.0)
xp.setValue (0, Math.Floor (imxv * xsc));
else
return null;
xsc = (vp.value (1) - rmin.value (1)) / rsize;
if (0.0 <= xsc && xsc < 1.0)
xp.setValue (1, Math.Floor (imxv * xsc));
else
return null;
xsc = (vp.value (2) - rmin.value (2)) / rsize;
if (0.0 <= xsc && xsc < 1.0)
xp.setValue (2, Math.Floor (imxv * xsc));
else
return null;
return xp;
}
/// <summary>
/// Sets an arrow to the give locations
/// </summary>
/// <param name="point">Origin point of the arrow</param>
/// <param name="vector">Direction Vector of the arrow</param>
/// <param name="length">length of the arrow</param>
/// <param name="arrow">The arrow to be edited</param>
protected void SetArrowAtLoc(MathPoint point, MathVector vector, double length, DragArrowManipulator arrow)
{
arrow.ShowRuler = false;
arrow.FixedLength = true;
arrow.ShowOppositeDirection = false;
arrow.AllowFlip = false;
arrow.Length = length;
arrow.Direction = vector;
arrow.Origin = point;
}
public static MathTransform ComposeTransform(this IMathUtility math, MathVector translate, double scale = 1.0)
{
return math.ComposeTransform(math.XAxis(), math.YAxis(), math.ZAxis(), translate, scale);
}
public static MathVector ZAxis(this IMathUtility m) => _ZAxis ?? (_ZAxis = m.Vector(_ZAxisArray));
public double distance(MathVector v)
{
double tmp = 0.0;
for(int i = 0; i < NDIM; i++)
tmp += ((data[i] - v.data[i]) * (data[i] - v.data[i]));
return Math.Sqrt(tmp);
}
public void crossProduct(MathVector u, MathVector w)
{
data[0] = u.data[1] * w.data[2] - u.data[2] * w.data[1];
data[1] = u.data[2] * w.data[0] - u.data[0] * w.data[2];
data[2] = u.data[0] * w.data[1] - u.data[1] * w.data[0];
}
/**
* Add two vectors and the result is placed in this vector.
*
* @param u the other operand of the addition
*/
public void addition(MathVector u)
{
for(int i = 0; i < NDIM; i++)
data[i] = data[i] + u.data[i];
}
/**
* Add a scalar to each element in the vector and put the
* result in this vector.
*
* @param u a vector
* @param s the scalar
*/
public void addScalar(MathVector u, double s)
{
for(int i = 0; i < NDIM; i++)
data[i] = u.data[i] + s;
}
public abstract Cell loadTree(Body p, MathVector xpic, int l, BTree root);
/// <summary>
/// Finds if the given point lies on the given line
/// </summary>
/// <param name="dir">The direction of the line</param>
/// <param name="pointOnLine">A point somewhere on the line</param>
/// <param name="otherPoint">The point that is being checked</param>
/// <returns>Returns true if the point is is on the line</returns>
public static bool IsPointOnLine(MathVector dir, MathPoint pointOnLine, MathPoint otherPoint)
{
MathVector PointVector = GetVectorBetweenPoints(pointOnLine, otherPoint).Normalise();
return (IsParallel(dir, PointVector));
}
/**
* Descend Tree and insert particle. We're at a body so we need to
* create a cell and attach this body to the cell.
*
* @param p the body to insert
* @param xpic
* @param l
* @param tree the root of the data structure
* @return the subtree with the new body inserted
*/
public override Cell loadTree (Body p, MathVector xpic, int l, BTree tree)
{
// create a Cell
Cell retval = Cell.makeCell ();
int si = subindex (tree, l);
// attach this Body node to the cell
retval.subp [si] = this;
// move down one level
si = Node.oldSubindex (xpic, l);
Node rt = retval.subp [si];
if (rt != null)
retval.subp [si] = rt.loadTree (p, xpic, l >> 1, tree);
else
retval.subp [si] = p;
return retval;
}
/**
* Construct an empty node
*/
public void initNode()
{
mass = 0.0;
pos = MathVector.makeMathVector();
}
/// <summary>
/// Finds if the 2 vectors are in the same direction
/// </summary>
/// <param name="v1">THe first vector</param>
/// <param name="v2">The second vector</param>
/// <returns>whether the 2 vectors have the same direction</returns>
public static bool IsSameDirection(MathVector v1, MathVector v2)
{
v1 = v1.Normalise();
v2 = v2.Normalise();
return v1.Subtract(v2).GetLength() < errorVal;
}
/// <summary>
/// Draws preview arrows for the coordinate system. Axes should be calculated before hand.
/// </summary>
public void DrawOriginPreview()
{
triadManip = null;
swManips = null;
GC.Collect();
if (robot == null || robot.OriginPt == null || robot.Direction == null || robot.BasePlane == null)
{
return;
}
triadManip = null;
swManips = null;
GC.Collect();
ModelViewManager swModViewMgr = modelDoc.ModelViewManager;
MathPoint robotOrigin = mathUtil.CreatePoint(new double[] { robot.OriginX, robot.OriginY, robot.OriginZ });
swManips = new Manipulator[1];
MathVector[] axisVectors = new MathVector[3];
axisVectors[0] = mathUtil.CreateVector(new double[] { robot.AxisX, robot.AxisY, robot.AxisZ });
axisVectors[1] = CalcNormalVector(robot.BasePlane); //calculate vertical axis as normal of plane
axisVectors[2] = axisVectors[1].Cross(axisVectors[0]); //calculate horizontal axis as cross product between plane normal and direction axis.
swManips[0] = swModViewMgr.CreateManipulator((int)swManipulatorType_e.swTriadManipulator, this);
triadManip = (TriadManipulator)swManips[0].GetSpecificManipulator();
triadManip.DoNotShow = (int)swTriadManipulatorDoNotShow_e.swTriadManipulatorDoNotShowXYRING | (int)swTriadManipulatorDoNotShow_e.swTriadManipulatorDoNotShowYZRING
| (int)swTriadManipulatorDoNotShow_e.swTriadManipulatorDoNotShowZXRING | (int)swTriadManipulatorDoNotShow_e.swTriadManipulatorDoNotShowXYPlane |
(int)swTriadManipulatorDoNotShow_e.swTriadManipulatorDoNotShowYZPlane | (int)swTriadManipulatorDoNotShow_e.swTriadManipulatorDoNotShowZXPlane;
triadManip.XAxis = axisVectors[0];
triadManip.YAxis = axisVectors[2];
triadManip.ZAxis = axisVectors[1];
triadManip.Origin = robotOrigin;
triadManip.UpdatePosition();
swManips[0].Show(modelDoc);
}
private static double LinePlaneIntersection(MathPoint origin, MathVector axis0, MathVector axis1, MathPoint lineOrigin, MathVector lineVector)
{
double x1, y1, z1;
double x2, y2, z2;
double x3, y3, z3;
double x4, y4, z4;
double x5, y5, z5;
double t;
var p0 = (MathPoint)origin.AddVector(axis0);
var p1 = (MathPoint)origin.AddVector(axis1);
Destructure(origin, out x1, out y1, out z1);
Destructure(p0, out x2, out y2, out z2);
Destructure(p1, out x3, out y3, out z3);
var p2 = (MathPoint)origin.AddVector(lineVector);
Destructure(lineOrigin, out x4, out y4, out z4);
Destructure(p2, out x5, out y5, out z5);
Matrix a = new DenseMatrix(4, 4, new double[]
{
1, x1, y1, z1, 1, x2, y2, z2, 1, x3, y3, z3, 1, x4, y4, z4
});
Matrix b = new DenseMatrix(4, 4, new double[]
{
1, x1, y1, z1, 1, x2, y2, z2, 1, x3, y3, z3, 0, x5 - x4, y5 - y4, z5 - z4
});
var da = a.Determinant();
var db = b.Determinant();
t = da / db;
return(t);
}
/// <summary>
/// Projects the vector between the inputed point and a point on the axis line and then scales
/// The axis to the projected length.
/// </summary>
/// <param name="axis">The axis vector that will be projected onto</param>
/// <param name="point">the point on the other axis</param>
/// <returns>A vector scaled to the projection</returns>
public static MathVector GetScaledVector(MathVector axis, double[] point1, double[] point2)
{
double[] tempArray = { point1[0] - point2[0], point1[1] - point2[1], point1[2] - point2[2] };
MathVector pointVector = RobotInfo.mathUtil.CreateVector(tempArray);//creates a vector between the point on the line and the origin of the joint
return axis.Scale(pointVector.Dot(axis));
}
private static MathVector ClosestPointOnAxis(ITriadManipulator m, IMathPoint p, IMathVector cameraVector, MathVector axis)
{
double pT;
double qT;
if (ClosestPointBetweenLines(m.Origin, axis, p, cameraVector, out pT, out qT))
{
return((MathVector)axis.Scale(pT));
}
else
{
return(null);
}
}
