public double[] TransformPoint(double[] pt, IMathTransform transform)
{
IMathPoint point = m_MathUtils.CreatePoint(pt) as IMathPoint;
point = point.MultiplyTransform(transform) as IMathPoint;
return(point.ArrayData as double[]);
}
/// <summary>
/// 此点在 XY Plane 中的角度
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public static double Angle2D(this IMathPoint p)
{
var pData = p.ArrayData as double[];
var angle = System.Math.Atan2(pData[1], pData[0]);
return(angle < 0.0 ? angle + 2 * System.Math.PI : angle);
}
public static MathVector Project(this ITriadManipulator m, swTriadManipulatorControlPoints_e h, IMathPoint p, IMathVector cameraVector, IMathUtility mathP)
{
IMathUtility math = mathP;
var zero = (IMathPoint) math.CreatePoint(new[] {0, 0, 0});
double pT, qT;
switch (h)
{
case swTriadManipulatorControlPoints_e.swTriadManipulatorOrigin:
return (MathVector) p.Subtract(m.Origin);
case swTriadManipulatorControlPoints_e.swTriadManipulatorXAxis:
return ClosestPointOnAxis(m, p, cameraVector, m.XAxis);
case swTriadManipulatorControlPoints_e.swTriadManipulatorYAxis:
return ClosestPointOnAxis(m, p, cameraVector, m.YAxis);
case swTriadManipulatorControlPoints_e.swTriadManipulatorZAxis:
return ClosestPointOnAxis(m, p, cameraVector, m.ZAxis);
case swTriadManipulatorControlPoints_e.swTriadManipulatorXYPlane:
return (MathVector) p.Subtract(m.Origin);
case swTriadManipulatorControlPoints_e.swTriadManipulatorYZPlane:
return (MathVector) p.Subtract(m.Origin);
case swTriadManipulatorControlPoints_e.swTriadManipulatorZXPlane:
return (MathVector) p.Subtract(m.Origin);
default:
throw new ArgumentOutOfRangeException(nameof(h), h, null);
}
}
public static MathPoint Project(this IMathPoint point, IMathPoint origin, IMathVector axis)
{
var a = (IMathVector) point.Subtract(origin);
var t = a.Project(axis);
var v = (MathVector) axis.Scale(t);
return (MathPoint) origin.AddVector(v);
}
/// <summary>
/// Sets the direction of the macro feature dimension.
/// </summary>
/// <param name="dim">Pointer to dimension. Usually retrieved from <see cref="DimensionData.Dimension"/></param>
/// <param name="originPt">Dimension starting attach point</param>
/// <param name="dir">Direction of the dimension</param>
/// <param name="length">Length of the dimension (usually equal to its value)</param>
/// <param name="extDir">Optional direction of extension line</param>
/// <remarks>Call this method within the <see cref="CodeStack.SwEx.MacroFeature.MacroFeatureEx{TParams}.OnSetDimensions(ISldWorks, IModelDoc2, IFeature, DimensionDataCollection, TParams)"/></remarks>
public static void SetDirection(this IDimension dim,
Point originPt, Vector dir, double length, Vector extDir = null)
{
var dimDirVec = m_MathUtils.CreateVector(dir.ToArray()) as MathVector;
var startPt = m_MathUtils.CreatePoint(originPt.ToArray()) as IMathPoint;
var endPt = m_MathUtils.CreatePoint(originPt.Move(dir, length).ToArray()) as IMathPoint;
var refPts = new IMathPoint[]
{
startPt,
endPt,
m_MathUtils.CreatePoint(new double[3]) as IMathPoint
};
if (extDir == null)
{
var yVec = new Vector(0, 1, 0);
if (dir.IsSame(yVec))
{
extDir = new Vector(1, 0, 0);
}
else
{
extDir = yVec.Cross(dir);
}
}
var extDirVec = m_MathUtils.CreateVector(extDir.ToArray()) as MathVector;
dim.DimensionLineDirection = dimDirVec;
dim.ExtensionLineDirection = extDirVec;
dim.ReferencePoints = refPts;
}
public static Camera createCamera(Component2 fieldOfView, SldWorks swApp, ModelDoc2 swDoc, AssemblyDoc swAssembly, SelectionMgr swSelectionMgr, MathUtility mathUtils)
{
IMathPoint centreOfVision = getCentreOfVision(fieldOfView, swDoc, swSelectionMgr, mathUtils);
if (centreOfVision == null)
{
swApp.SendMsgToUser2("you need to insert the camera first!", 1, 1);
return(null);
}
RefPlane cameraNormalPlane = getCameraNormalPlane(fieldOfView, swDoc, swSelectionMgr);
// note that we reverse this transform because our plane normal is pointing the wrong direction :(
MathTransform cameraTransform = Camera.reverseTransform(cameraNormalPlane.Transform, mathUtils);
MathVector cameraDirection = getCameraDirection(cameraTransform, mathUtils);
drawRayForCamera(cameraDirection, centreOfVision, swDoc, swSelectionMgr);
Camera camera = new Camera(fieldOfView, mathUtils.CreatePoint((double[])centreOfVision.ArrayData), cameraDirection);
camera.swApp = swApp;
camera.swDoc = swDoc;
camera.swAssembly = swAssembly;
camera.swSelectionMgr = swSelectionMgr;
camera.mathUtils = mathUtils;
return(camera);
}
/// <summary>
/// Returns the angle in the xy plane between 0 and 2 Pi
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public static double Angle2D(this IMathPoint p)
{
var pData = p.ArrayData.CastArray <double>();
var angle = Math.Atan2(pData[1], pData[0]);
return(angle < 0.0 ? angle + 2 * Math.PI : angle);
}
/// <summary>
/// 计算在某轴上的投影
/// </summary>
/// <param name="point"></param>
/// <param name="origin"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static MathPoint Project(this IMathPoint point, IMathPoint origin, IMathVector axis)
{
var a = (IMathVector)point.Subtract(origin);
var t = a.Project(axis);
var v = (MathVector)axis.Scale(t);
return((MathPoint)origin.AddVector(v));
}
private Point GetCanvasPoint(IMathPoint p)
{
return(new Point()
{
X = this.m_canvas.Width * p[0],
Y = this.m_canvas.Height * (1.0 - p[1])
});
}
public static IMathPoint MoveAlongVector(this IMathPoint pt, IMathVector dir, double dist)
{
dir = dir.Normalise().Scale(dist) as IMathVector;
var centerPt = pt.AddVector(dir) as IMathPoint;
return(centerPt);
}
internal Point GetBitmapPoint(IMathPoint mathPoint)
{
return(new Point()
{
X = (int)(mathPoint[0] * this.m_box.Width),
Y = this.m_box.Height - (int)(mathPoint[1] * this.m_box.Height)
});
}
public static void visualizeRay(MathVector ray, IMathPoint origin)
{
double[] rayData = (double[])ray.ArrayData;
double[] originData = (double[])origin.ArrayData;
swDoc.CreateLine2(originData[0], originData[1], originData[2],
originData[0] + rayData[0], originData[1] + rayData[1], originData[2] + rayData[2]);
}
public Point TransformPoint(Point point, TransformationMaxtrix transform)
{
IMathPoint mathPt = m_MathUtils.CreatePoint(point.ToArray()) as IMathPoint;
var swTransform = m_MathUtils.CreateTransFormTransformationMaxtrix(transform);
mathPt = mathPt.MultiplyTransform(swTransform) as IMathPoint;
return(new Point(mathPt.ArrayData as double[]));
}
private void ExportToStl(string filePath, float[] tessTriangs, float[] tessNorms, double[] transformMatrix)
{
IMathUtility mathUtils = swApp.IGetMathUtility();
IMathTransform transform = (mathUtils.CreateTransform(transformMatrix) as IMathTransform).IInverse();
using (FileStream fileStream = File.Create(filePath))
{
using (BinaryWriter writer = new BinaryWriter(fileStream))
{
byte[] header = new byte[80];
writer.Write(header);
uint triangsCount = (uint)tessTriangs.Length / 9;
writer.Write(triangsCount);
for (uint i = 0; i < triangsCount; i++)
{
float normalX = tessNorms[i * 9];
float normalY = tessNorms[i * 9 + 1];
float normalZ = tessNorms[i * 9 + 2];
IMathVector mathVec = mathUtils.CreateVector(
new double[] { normalX, normalY, normalZ }) as IMathVector;
mathVec = mathVec.MultiplyTransform(transform) as IMathVector;
double[] vec = mathVec.ArrayData as double[];
writer.Write((float)vec[0]);
writer.Write((float)vec[1]);
writer.Write((float)vec[2]);
for (uint j = 0; j < 3; j++)
{
float vertX = tessTriangs[i * 9 + j * 3];
float vertY = tessTriangs[i * 9 + j * 3 + 1];
float vertZ = tessTriangs[i * 9 + j * 3 + 2];
IMathPoint mathPt = mathUtils.CreatePoint(
new double[] { vertX, vertY, vertZ }) as IMathPoint;
mathPt = mathPt.MultiplyTransform(transform) as IMathPoint;
double[] pt = mathPt.ArrayData as double[];
writer.Write((float)pt[0]);
writer.Write((float)pt[1]);
writer.Write((float)pt[2]);
}
ushort atts = 0;
writer.Write(atts);
}
}
}
}
public MathVector getRayFromNamedPoint(string name)
{
IMathPoint namedPoint = Camera.getNamedPoint(name, fieldOfView, swDoc, swSelectionMgr, mathUtils);
double[] otherData = (double[])namedPoint.ArrayData;
double[] centreData = (double[])centreOfVision.ArrayData;
double[] cameraDir = { otherData[0] - centreData[0], otherData[1] - centreData[1], otherData[2] - centreData[2] };
return(mathUtils.CreateVector(cameraDir));
}
/// <summary>
/// Find the vector in model space from the point to the viewers eye.
/// </summary>
/// <param name="modelView"></param>
/// <param name="mathUtility"></param>
/// <param name="p"></param>
/// <returns></returns>
private static MathVector ViewVector(IModelView modelView, IMathUtility mathUtility, IMathPoint p)
{
var world2screen = modelView.Transform;
var pScreen = (MathPoint) p.MultiplyTransform(world2screen);
var vv = (IMathVector) mathUtility.CreateVector(new[] {0.0, 0, 1});
var pScreenUp = (MathPoint) pScreen.AddVector(vv);
var pWorldDelta = (MathPoint) pScreenUp.MultiplyTransform((MathTransform) world2screen.Inverse());
var viewVector = (MathVector) p.Subtract(pWorldDelta);
return viewVector;
}
internal static void MetricEquals(IMathPoint p, IMathPoint c)
{
if (p != null && c != null)
{
if (p.CoordinatesCount.Equals(c.CoordinatesCount))
{
return;
}
throw new ArgumentException("несоответствие размерностей обрабатываемых точек");
}
throw new ArgumentNullException("входной параметр IMathPoint равен null");
}
private static Double Euclidean(IMathPoint p, IMathPoint c)
{
DataMiningMath.MetricEquals(p, c);
Double result = 0;
for (Int32 i = 0; i < c.CoordinatesCount; i++)
{
result += (p[i] - c[i]) * (p[i] - c[i]);
}
return(result);
}
private static Double Cityblock(IMathPoint p, IMathPoint c)
{
DataMiningMath.MetricEquals(p, c);
Double result = 0;
for (Int32 i = 0; i < c.CoordinatesCount; i++)
{
result += p[i] < c[i] ? c[i] - p[i] : p[i] - c[i];
}
return(result);
}
internal static IMathPoint MeanPoint(IMathPoint p, IMathPoint c)
{
DataMiningMath.MetricEquals(p, c);
IMathPoint meanPoint = c.Clone() as IMathPoint;
for (Int32 i = 0; i < c.CoordinatesCount; i++)
{
meanPoint[i] = 0.5 * (p[i] + c[i]);
}
return(meanPoint);
}
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);
}
}
private static Double Cosine(IMathPoint p, IMathPoint c)
{
DataMiningMath.MetricEquals(p, c);
Double pc = 0, pp = 0, cc = 0;
for (Int32 i = 0; i < c.CoordinatesCount; i++)
{
pc += p[i] * c[i];
pp += p[i] * p[i];
cc += c[i] * c[i];
}
return(1 - (pc / System.Math.Sqrt(pp * cc)));
}
internal static List <IMathPoint> InitCenters(IMathSet mathSet, ClusteringOptions options)
{
IMathPoint minPoint = DataMiningMath.GetLimitPoint(mathSet, (Double c, Double p) => c > p);
IMathPoint maxPoint = DataMiningMath.GetLimitPoint(mathSet, (Double c, Double p) => c < p);
if (options.Initialization == InitializationType.RANDOM)
{
return(GetRandomClusterCenters(minPoint, maxPoint, options));
}
else
{
throw new ArgumentException("поддерживаемый метод инициализации центров кластеризации: random");
}
}
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;
}
}
private static void drawRayForCamera(MathVector cameraDirection, IMathPoint centreOfVision, ModelDoc2 swDoc, SelectionMgr swSelectionMgr)
{
swDoc.ClearSelection2(true);
Camera.removeRayIfPresent(swDoc);
double[] cameraDirData = (double[])cameraDirection.ArrayData;
double[] cameraOriginData = (double[])centreOfVision.ArrayData;
swDoc.Insert3DSketch2(true);
swDoc.SketchManager.AddToDB = true;
swDoc.CreateLine2(cameraOriginData[0], cameraOriginData[1], cameraOriginData[2],
cameraOriginData[0] + cameraDirData[0], cameraOriginData[1] + cameraDirData[1], cameraOriginData[2] + cameraDirData[2]);
swDoc.Insert3DSketch2(true);
swDoc.SketchManager.AddToDB = false;
Feature sketch = swSelectionMgr.GetSelectedObject6(1, 0);
sketch.Name = CAMERA_RAY_NAME;
swDoc.ClearSelection2(true);
}
public bool isRayWithinFOV(IMathPoint rayTo)
{
IMathPoint source = getCentreOfVision();
MathVector direction = ((MathVector)mathUtils.CreateVector(new double[] {
rayTo.ArrayData[0] - source.ArrayData[0],
rayTo.ArrayData[1] - source.ArrayData[1],
rayTo.ArrayData[2] - source.ArrayData[2],
})).Normalise();
// now we need to see if our direction is less extreme than all of the corner directions.
rayVectors();
double theta = Math.Acos(direction.Dot(cameraDirection.Normalise()));
// TODO : this could be better
return(theta < VIEWINGANGLEUPDOWN && theta < VIEWINGANGLERIGHTLEFT);
}
public static IMathPoint getNamedPoint(string name, Component2 parent)
{
swDoc.ClearSelection2(true);
swDoc.Extension.SelectByID2(name + "@" + parent.GetSelectByIDString(),
"DATUMPOINT", 0, 0, 0, false, 0, null, 0);
IFeature namedPointFeature = swSelectionMgr.GetSelectedObject6(1, -1) as IFeature;
if (namedPointFeature == null)
{
return(null);
}
measure.Calculate(null);
IMathPoint namedPoint = mathUtils.CreatePoint(new double[] { measure.X, measure.Y, measure.Z });
return(namedPoint);
}
public IMathTransform GetTransformBetweenVectorsAroundPoint(
double[] vec1, double[] vec2, double[] pt)
{
IMathVector mathVec1 = m_MathUtils.CreateVector(vec1) as IMathVector;
IMathVector mathVec2 = m_MathUtils.CreateVector(vec2) as IMathVector;
IMathVector crossVec = mathVec1.Cross(mathVec2) as IMathVector;
double dot = mathVec1.Dot(mathVec2);
double vec1Len = mathVec1.GetLength();
double vec2Len = mathVec2.GetLength();
double angle = Math.Acos(dot / vec1Len * vec2Len);
IMathPoint mathPt = m_MathUtils.CreatePoint(pt) as IMathPoint;
return(m_MathUtils.CreateTransformRotateAxis(mathPt, crossVec, angle) as IMathTransform);
}
private static IMathPoint getNamedPoint(string name, Component2 fieldOfView, ModelDoc2 swDoc, SelectionMgr swSelectionMgr, MathUtility mathUtils)
{
swDoc.ClearSelection2(true);
swDoc.Extension.SelectByID2(name + "@" + fieldOfView.GetSelectByIDString(),
"DATUMPOINT", 0, 0, 0, false, 0, null, 0);
IFeature namedPointFeature = swSelectionMgr.GetSelectedObject6(1, -1) as IFeature;
if (namedPointFeature == null)
{
return(null);
}
Measure measure = swDoc.Extension.CreateMeasure();
measure.Calculate(null);
IMathPoint namedPoint = mathUtils.CreatePoint(new double[] { measure.X, measure.Y, measure.Z });
return(namedPoint);
}
public TransformationMaxtrix GetTransformBetweenVectorsAroundPoint(
Vector firstVector, Vector secondVector, Point point)
{
IMathVector mathVec1 = (m_MathUtils.CreateVector(firstVector.ToArray()) as IMathVector).Normalise();
IMathVector mathVec2 = (m_MathUtils.CreateVector(secondVector.ToArray()) as IMathVector).Normalise();
IMathVector crossVec = (mathVec1.Cross(mathVec2) as IMathVector).Normalise();
double dot = mathVec1.Dot(mathVec2);
double vec1Len = mathVec1.GetLength();
double vec2Len = mathVec2.GetLength();
double angle = System.Math.Acos(dot / vec1Len * vec2Len);
IMathPoint mathPt = m_MathUtils.CreatePoint(point.ToArray()) as IMathPoint;
var mathTransform = m_MathUtils.CreateTransformRotateAxis(mathPt, crossVec, angle) as IMathTransform;
return(mathTransform.ToTransformationMaxtrix());
}
internal static IMathPoint GetLimitPoint(IMathSet mathSet, Func <Double, Double, Boolean> comparisonFunc)
{
if (1 < mathSet.RowsCount)
{
IMathPoint limitPoint = mathSet[0].Clone() as IMathPoint;
for (Int32 r = 0; r < mathSet.RowsCount; r++)
{
for (Int32 c = 0; c < mathSet[r].CoordinatesCount; c++)
{
if (comparisonFunc(limitPoint[c], mathSet[r][c]))
{
limitPoint[c] = mathSet[r][c];
}
}
}
return(limitPoint);
}
throw new ArgumentException("входной набор IMathSet должен содержать минимум 2 точки с данными");
}
internal static List <IMathPoint> GetRandomClusterCenters(IMathPoint minPoint, IMathPoint maxPoint, ClusteringOptions options)
{
Random rnd = new Random();
List <IMathPoint> startClusterPoints = new List <IMathPoint>(options.ClusterCount);
// обход центров кластеризации:
for (Int32 i = 0; i < options.ClusterCount; i++)
{
startClusterPoints.Add((IMathPoint)minPoint.Clone());
// обход координат центра кластеризации:
for (Int32 c = 0; c < startClusterPoints[i].CoordinatesCount; c++)
{
startClusterPoints[i][c] = rnd.NextDouble() * (maxPoint[c] - minPoint[c]) + minPoint[c];
}
startClusterPoints[i].Class = string.Format("class {0}", i + 1);
startClusterPoints[i].Cluster = string.Format("cluster {0}", i + 1);
}
return(startClusterPoints);
}
public void drawInitalRay()
{
centreOfVision = (MathPoint)Camera.getCentreOfVision(fieldOfView, swDoc, swSelectionMgr, mathUtils);
if (centreOfVision == null)
{
Utilities.alert("you need to insert the camera first!");
return;
}
IMathPoint otherReference = Camera.getDirectionReference(fieldOfView, swDoc, swSelectionMgr, mathUtils);
double[] centreData = (double[])centreOfVision.ArrayData;
double[] otherData = (double[])otherReference.ArrayData;
double[] cameraDir = { otherData[0] - centreData[0], otherData[1] - centreData[1], otherData[2] - centreData[2] };
cameraDirection = mathUtils.CreateVector(cameraDir);
Camera.drawRayForCamera(cameraDirection, centreOfVision, swDoc, swSelectionMgr);
castRayCentres = null;
castRayVectors = null;
}
public static bool quickCheck(Component2 component)
{
IMathPoint source = Utilities.getNamedPoint("centre of flag top", component);
if (source == null)
{
// then we need to pick some other kind of point... we'll try....
Feature origin = component.FirstFeature();
while (origin != null)
{
if (origin.GetTypeName().Equals("OriginProfileFeature"))
{
origin.Select(false);
Measure measure = swDoc.Extension.CreateMeasure();
measure.Calculate(null);
source = mathUtils.CreatePoint(new double[] { measure.X, measure.Y, measure.Z });
}
origin = origin.GetNextFeature();
}
}
return(camera.isRayWithinFOV(source));
}
private static MathVector ProjectRelative(IMathPoint origin, IMathVector axis, IMathPoint point)
{
return (MathVector) point.Project(origin, axis).Subtract(origin);
}
public static bool Equals(this IMathPoint a, IMathPoint b, double tol)
{
var v = a.SubtractTs(b);
return v.GetLength() < tol;
}
public static MathVector SubtractTs(this IMathPoint a, IMathPoint b)
{
return (MathVector)a.Subtract(b);
}
public static IMathPoint RotateByAngle(this IMathUtility m, IMathPoint p, IMathVector axis, double angle)
{
var transformation = GetRotationFromAxisAndAngle(m, axis, angle);
return p.MultiplyTransformTs(transformation);
}
public static IMathPoint TranslateByVector(this IMathUtility m, IMathPoint p, IMathVector v)
{
var transformation = GetTranslationFromVector(m, v);
return p.MultiplyTransformTs(transformation);
}
public static IBody2 CreateSheetFromSurface(this IModeler modeler, ISurface surf, IMathPoint p0, IMathPoint p1)
{
var uvLow = surf.GetClosestPointOnTs(p0.ArrayData.CastArray<double>().ToVector3());
var uvHigh = surf.GetClosestPointOnTs(p1.ArrayData.CastArray<double>().ToVector3());
return modeler.CreateSheetFromSurface(surf, uvLow, uvHigh);
}
/// <summary>
/// http://geomalgorithms.com/a07-_distance.html
/// </summary>
/// <param name="pa"></param>
/// <param name="va"></param>
/// <param name="pb"></param>
/// <param name="vb"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
private static bool ClosestPointBetweenLines(IMathPoint pOrigin, IMathVector pVector, IMathPoint qOrigin, IMathVector qVector, out double pT, out double qT)
{
var w0 = (IMathVector) (pOrigin.Subtract(qOrigin));
var u = pVector.Normalise();
var v = qVector.Normalise();
var a = u.Dot(u);
var b = u.Dot(v);
var c = v.Dot(v);
var d = u.Dot(w0);
var e = v.Dot(w0);
var den = (a*c - b*b);
if (Math.Abs(den) < 1e-12)
{
pT = 0;
qT = 0;
return false;
}
pT = (b*e - c*d)/den;
qT = (a*a - b*d)/den;
return true;
}
