public void GetFacesPolylines()
{
Polyline faceContour = new Polyline();
for (int i = 0; i < mesh.Faces.Count; i++)
{
faceContour = new Polyline();
Point3f[] pf;
if (mesh.Faces[i].IsQuad)
{
pf = new Point3f[4];
mesh.Faces.GetFaceVertices(i, out pf[0], out pf[1], out pf[2], out pf[3]);
}
else
{
pf = new Point3f[3];
mesh.Faces.GetFaceVertices(i, out pf[0], out pf[1], out pf[2], out pf[2]);
}
for (int j = 0; j < pf.Length; j++)
{
faceContour.Add(pf[j].X, pf[j].Y, pf[j].Z);
}
faceContour.Add(pf[0].X, pf[0].Y, pf[0].Z); // adds again first point to generate a closed polyline
fc.Add(faceContour);
}
}
public override Mesh getTrailEvaMesh(double zpos)
{
int z = getWIndex(zpos);
double trailmax = getMaxTrailValue();
Mesh evaMesh = new Mesh();
Plane worldXY = new Plane(new Point3d(0, 0, z), new Point3d(1, 0, z), new Point3d(0, 1, z));
Plane baseplane = new Plane(new Point3d(0, 0, getWMin()), new Vector3d(0, 0, 1));
evaMesh = Mesh.CreateFromPlane(baseplane, new Interval(getUMin(), getUMax()), new Interval(getVMin(), getVMax()), u, v);
for (int i = 0; i < evaMesh.Vertices.Count; i++)
{
Point3f vert = evaMesh.Vertices[i];
int x = getUIndex(vert.X);
int y = getVIndex(vert.Y);
float thisTrail = 0;
if (x < u - 1 && y < v - 1)
{
thisTrail = (float)(trail[x, y, z] * 255.0 / trailmax);
}
if (thisTrail > 255)
{
thisTrail = 255;
}
evaMesh.VertexColors.SetColor(i, (int)thisTrail, 0, 0);
}
return(evaMesh);
}
public virtual void calcProteinMainchainHydrogenBonds(BitArray bsA, BitArray bsB)
{
Point3f carbonPoint;
Point3f oxygenPoint;
for (int i = 0; i < monomerCount; ++i)
{
AminoMonomer residue = (AminoMonomer)monomers[i];
mainchainHbondOffsets[i] = 0;
/****************************************************************
* This does not acount for the first nitrogen in the chain
* is there some way to predict where it's hydrogen is?
* mth 20031219
****************************************************************/
if (i > 0 && residue.GroupID != JmolConstants.GROUPID_PROLINE)
{
Point3f nitrogenPoint = residue.NitrogenAtomPoint;
aminoHydrogenPoint.add(nitrogenPoint, vectorPreviousOC);
bondAminoHydrogen(i, aminoHydrogenPoint, bsA, bsB);
}
carbonPoint = residue.CarbonylCarbonAtomPoint;
oxygenPoint = residue.CarbonylOxygenAtomPoint;
vectorPreviousOC.sub(carbonPoint, oxygenPoint);
vectorPreviousOC.scale(1 / vectorPreviousOC.length());
}
}
protected void GeneratePointCloud(double[] averagedDepthFrameData)
{
double depthPoint;
// Setup variables for per-pixel loop
allPoints = new Point3f[trimmedWidth * trimmedHeight];
var tempPoint = new Point3f();
var arrayIndex = 0;
for (var rows = 0; rows < trimmedHeight; rows++)
{
for (var columns = 0; columns < trimmedWidth; columns++)
{
depthPoint = averagedDepthFrameData[arrayIndex];
tempPoint.X = (float)(columns * -unitsMultiplier * depthPixelSize.x);
tempPoint.Y = (float)(rows * -unitsMultiplier * depthPixelSize.y);
tempPoint.Z = (float)((depthPoint - sensorElevation) * -unitsMultiplier);
allPoints[arrayIndex] = tempPoint; // Add new point to point cloud itself
arrayIndex++;
}
}
// Keep only the desired amount of frames in the buffer
while (renderBuffer.Count >= averageFrames)
{
renderBuffer.RemoveFirst();
}
Core.LogTiming(ref output, timer, "Point cloud generation"); // Debug Info
}
public static Mesh Create(double r_ = 1)
{
Mesh mesh = new Mesh();
float length = (float)r_;
float width = (float)r_;
float height = (float)r_;
Point3f p0 = new Point3f(length * .5f, width * .5f, -height * .5f); //
Point3f p1 = new Point3f(-length * .5f, -width * .5f, -height * .5f); //
Point3f p2 = new Point3f(-length * .5f, width * .5f, height * .5f);
Point3f p3 = new Point3f(length * .5f, -width * .5f, height * .5f);
Point3f[] vertices = new Point3f[] { p0, p1, p2, p3 };
MeshFace[] mf = new MeshFace[]
{
new MeshFace(1, 0, 3),
new MeshFace(0, 1, 2),
new MeshFace(2, 3, 0),
new MeshFace(3, 2, 1),
};
mesh.Faces.AddFaces(mf);
mesh.Vertices.AddVertices(vertices);
mesh.RebuildNormals();
mesh.Clean();
return(mesh);
}
// return index of point in the middle of p1 and p2
private static int getMiddlePoint(int p1, int p2, ref List <Point3f> vertices, ref Dictionary <long, int> cache, float radius)
{
// first check if we have it already
bool firstIsSmaller = p1 < p2;
long smallerIndex = firstIsSmaller ? p1 : p2;
long greaterIndex = firstIsSmaller ? p2 : p1;
long key = (smallerIndex << 32) + greaterIndex;
int ret;
if (cache.TryGetValue(key, out ret))
{
return(ret);
}
// not in cache, calculate it
Point3f point1 = vertices[p1];
Point3f point2 = vertices[p2];
Point3f middle = new Point3f
(
(point1.X + point2.X) / 2f,
(point1.Y + point2.Y) / 2f,
(point1.Z + point2.Z) / 2f
);
// add vertex makes sure point is on unit sphere
int i = vertices.Count;
vertices.Add(middle.Unit() * radius);
// store it, return index
cache.Add(key, i);
return(i);
}
public void SolvePnPTestByArray()
{
var rvec = new double[] { 0, 0, 0 };
var tvec = new double[] { 0, 0, 0 };
var cameraMatrix = new double[3, 3]
{
{ 1, 0, 0 },
{ 0, 1, 0 },
{ 0, 0, 1 }
};
var dist = new double[] { 0, 0, 0, 0, 0 };
var objPts = new Point3f[]
{
new Point3f(0, 0, 1),
new Point3f(1, 0, 1),
new Point3f(0, 1, 1),
new Point3f(1, 1, 1),
new Point3f(1, 0, 2),
new Point3f(0, 1, 2)
};
double[,] jacobian;
Point2f[] imgPts;
Cv2.ProjectPoints(objPts, rvec, tvec, cameraMatrix, dist, out imgPts, out jacobian);
Cv2.SolvePnP(objPts, imgPts, cameraMatrix, dist, out rvec, out tvec);
}
/// <summary>
/// Calculates the 3-D point cloud using the native SDK functions.
/// </summary>
/// <returns>3-D point coordinates in meters.</returns>
/// <remarks>Format is [height, width, coordinate] with coordinate in {0,1,2} for x, y and z.</remarks>
private Point3fCameraImage Calc3DCoordinates()
{
CameraSpacePoint[] worldPoints = new CameraSpacePoint[depthHeight * depthWidth];
Point3fCameraImage img = new Point3fCameraImage(depthWidth, depthHeight);
lock (this.depthFrameData)
{
lock (cameraLock)
{
coordinateMapper.MapDepthFrameToCameraSpace(depthFrameData, worldPoints);
}
for (int y = 0; y < depthHeight; y++)
{
for (int x = 0; x < depthWidth; x++)
{
// mirror image
int idx = y * depthWidth + depthWidthMinusOne - x;
img[y, x] = new Point3f(worldPoints[idx].X * -1, worldPoints[idx].Y * -1, worldPoints[idx].Z);
}
}
img.TimeStamp = timestampDepth;
}
return(img);
}
/// <summary>
/// Find the square of the distance on the XY plane between two points
/// </summary>
/// <param name="ptA"></param>
/// <param name="ptB"></param>
/// <returns></returns>
public static double XYSquaredDistanceTo(this Point3d ptA, Point3f ptB)
{
double dX = ptA.X - ptB.X;
double dY = ptA.Y - ptB.Y;
return(dX * dX + dY * dY);
}
public static Point3f TransformPoint3f(this Mat pose, Point3f point)
{
if (pose.Cols != 4 || pose.Rows != 4)
{
throw new ArgumentException("Matrix must be a 4 x 4 matrix");
}
if (pose.Type() != MatType.CV_32FC1)
{
throw new ArgumentException("Matrix must be of float 32 bit type");
}
var m1 = new float[4, 4];
var data = new float[16];
pose.GetArray(out data);
Array.Copy(data, m1, data.Length);
var v1 = new float[4] {
point.X, point.Y, point.Z, 1
};
var homogenous = new Vec4f(
(m1[0, 0] * v1[0] + m1[0, 1] * v1[1] + m1[0, 2] * v1[2] + m1[0, 3] * v1[3]),
(m1[1, 0] * v1[0] + m1[1, 1] * v1[1] + m1[1, 2] * v1[2] + m1[1, 3] * v1[3]),
(m1[2, 0] * v1[0] + m1[2, 1] * v1[1] + m1[2, 2] * v1[2] + m1[2, 3] * v1[3]),
(m1[3, 0] * v1[0] + m1[3, 1] * v1[1] + m1[3, 2] * v1[2] + m1[3, 3] * v1[3])
);
return(new Point3f(homogenous.Item0, homogenous.Item1, homogenous.Item2));
}
public virtual void calcSegments()
{
if (segments != null)
{
return;
}
calcAxis();
/*
* System.out.println("axisA=" + axisA.x + "," + axisA.y + "," + axisA.z);
* System.out.println("axisB=" + axisB.x + "," + axisB.y + "," + axisB.z);
*/
segments = new Point3f[monomerCount + 1];
segments[monomerCount] = axisB;
segments[0] = axisA;
for (int i = monomerCount; --i > 0;)
{
Point3f point = segments[i] = new Point3f();
apolymer.getLeadMidPoint(monomerIndex + i, point);
projectOntoAxis(point);
}
/*
* for (int i = 0; i < segments.length; ++i) {
* Point3f point = segments[i];
* System.out.println("segment[" + i + "]=" +
* point.x + "," + point.y + "," + point.z);
* }
*/
}
static Tuple <Point3f[], float[]> ReadFile(string filename)
{
string line;
StreamReader file = new StreamReader(filename);
line = file.ReadLine();
int n = Convert.ToInt32(line);
Point3f[] points = new Point3f[n];
for (int k = 0; k != n; ++k)
{
line = file.ReadLine();
string[] lines = line.Split(new char[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
float x = Convert.ToSingle(lines[0]);
float y = Convert.ToSingle(lines[1]);
float z = Convert.ToSingle(lines[2]);
points[k] = new Point3f(x, y, z);
}
float[] weights = new float[n + 4];
for (int k = 0; k != n + 4; ++k)
{
line = file.ReadLine();
weights[k] = Convert.ToSingle(line);
}
file.Close();
return(new Tuple <Point3f[], float[]>(points, weights));
}
private Point3fImage CalcPoint3DImage()
{
// Recalculate Directions if necessary
if (null == _directions)
{
int stepCount = _radii.Length;
_directions = new float[stepCount, 2];
for (int j = 0; j < stepCount; ++j)
{
double theta = Math.PI * (double)(_startingAngle + ((stepCount - 1 - j) * _angularStepWidth)) / (180.0 * 10000.0);
_directions[j, 0] = (float)Math.Cos(theta);
_directions[j, 1] = (float)Math.Sin(theta);
}
}
// Generate 3D Data
Point3fImage image = new Point3fImage(_radii.Length, 1)
{
ChannelName = _channelName,
FrameNumber = _scanCounter,
};
for (int x = 0; x < _radii.Length; ++x)
{
float r = _scalingFactor * (float)_radii[x];
image[x] = new Point3f(x: (r * _directions[x, 0]),
y: 0.0f,
z: (r * _directions[x, 1]));
}
return(image);
}
public float scale; //入力座標と画像座標の長さの係数
/* コンストラクタ
* @param[in] width 表示領域の幅
* @param[in] height 表示領域の高さ
* @param[in] length スケール(表示領域全体の長さをいくつと対応付けるか
*/
public IsometricDraw(int width, int height, float length)
{
float imageLength;
if (width > height)
{
imageLength = height;
}
else
{
imageLength = width;
}
this.originAtImage.x = width / 2;
this.originAtImage.y = height / 2;
scale = (float)imageLength / length;
//scale = length/ (float)imageLength;
Point3f[] axisAt3D = new Point3f[3];
Point2f[] axisAt2D = new Point2f[3];
axisAtImage = new Point2f[3];
axisAt3D[0].x = 0.2f * length;
axisAt3D[1].y = 0.2f * length;
axisAt3D[2].z = 0.2f * length;
for (int i = 0; i < 3; i++)
{
axisAt2D[i] = Convert3DTo2D(axisAt3D[i]);
axisAtImage[i] = Convert2DToImage(axisAt2D[i]);
}
}
public static Mat <float> Iso2DICOM(Orientation orient, Point3f iso)
{
using (var tx = DICOM2Isocenter(orient, iso))
{
return(new Mat <float>(tx.Inv()));
}
}
private static Point3f MatrixToEulerAngles(Mat mHomography)
{
float num = (float)Math.Sqrt(mHomography.At <double>(0, 0) * mHomography.At <double>(0, 0) + mHomography.At <double>(1, 0) * mHomography.At <double>(1, 0));
float num2;
float num3;
float num4;
if (!((double)num < 1E-06))
{
num2 = (float)Math.Atan2(mHomography.At <double>(2, 1), mHomography.At <double>(2, 2));
num3 = (float)Math.Atan2(0.0 - mHomography.At <double>(2, 0), num);
num4 = (float)Math.Atan2(mHomography.At <double>(1, 0), mHomography.At <double>(0, 0));
}
else
{
num2 = (float)Math.Atan2(0.0 - mHomography.At <double>(1, 2), mHomography.At <double>(1, 1));
num3 = (float)Math.Atan2(0.0 - mHomography.At <double>(2, 0), num);
num4 = 0f;
}
Point3f result = default(Point3f);
result.X = (float)((double)num2 * (180.0 / Math.PI));
result.Y = (float)((double)num3 * (180.0 / Math.PI));
result.Z = (float)((double)num4 * (180.0 / Math.PI));
return(result);
}
public bool MeshLineIntersect(Rhino.Geometry.Mesh x, Rhino.Geometry.Line L) //this is now for reading triangles from Rhino Mesh objects; can be eventually replaced with something, which reads OBJ mesh objects
{
//List<Point3d[]> TVs = new List<Point3d[]>();
//List of Triangle Vertices
Point3d[] TV = new Point3d[3];
//Triangle Vertices
Point3f av = default(Point3f);
Point3f bv = default(Point3f);
Point3f cv = default(Point3f);
Point3f dv = default(Point3f);
//Each face As MeshFace In x.Faces
bool does = false;
for (int k = 0; k <= x.Faces.Count - 1; k++)
{
x.Faces.GetFaceVertices(k, out av, out bv, out cv, out dv);
TV = new Point3d[] { new Point3d(av), new Point3d(bv), new Point3d(cv) };
//TVs.Add(TV);
if (TriangleLineIntersect(TV, x.FaceNormals[k], L))
{
does = true;
}
}
return(does);//TVs.Exists(Lambda => TriangleLineIntersect(Lambda, L));
}
public static Point3f Unit(this Point3f p)
{
Vector3f v = new Vector3f(p.X, p.Y, p.Z);
v.Unitize();
return(new Point3f(v.X, v.Y, v.Z));
}
/// <summary>
/// 求一组点的平均点
/// </summary>
/// <param name="list"></param>
/// <returns></returns>
public static Point3f Average(List <Point3f> list)
{
Point3f[] _list = list.ToArray();
Point3f average = Average(_list);
return(average);
}
private static void TestPoint3fParam()
{
string paramName = "Point3fParam";
log.Info("Testing a ParamDesc<Point3f>");
Point3f min = new Point3f(float.MinValue, float.MinValue, float.MinValue);
Point3f max = new Point3f(float.MaxValue, float.MaxValue, float.MaxValue);
float[] somevalues = new float[] { 0f, 0.5f, 1f, 5f, 10.0503f, 100f, 255f, 999.99999f, 1000f };
List <Point3f> somepoints = new List <Point3f>();
foreach (var item in somevalues)
{
somepoints.Add(new Point3f(item, item, item));
somepoints.Add(new Point3f(-item, -item, -item));
}
// Test types
SetParameterToValid(paramName, min);
SetParameterToValid(paramName, max);
// Strings should be ok
foreach (var item in somepoints)
{
SetParameterToValid(paramName, item.ToString());
SetParameterToValid(paramName, item.ToString("R"));
SetParameterToValid(paramName, item.ToString("F"));
SetParameterToValid(paramName, item.ToString("F1"));
SetParameterToValid(paramName, item.ToString("F2"));
}
// invalid values
TestNonNumericValues(paramName);
}
private Point3fImage CalcDirections(InverseBrownConradyParams intrinsics, int width, int height)
{
Point3fImage directions = new Point3fImage(width, height);
for (int row = 0; row < height; row++)
{
float yp = (intrinsics.Cy - row) / intrinsics.Fy;
float yp2 = yp * yp;
for (int col = 0; col < width; col++)
{
float xp = (intrinsics.Cx - col) / intrinsics.Fx;
// correct the camera distortion
float r2 = xp * xp + yp2;
float r4 = r2 * r2;
float k = 1 + intrinsics.K1 * r2 + intrinsics.K2 * r4;
// Undistorted direction vector of the point
float x = xp * k;
float y = yp * k;
float s0Inv = (float)(1 / Math.Sqrt(x * x + y * y + 1)); //z is 1, since x and y are coordinates on normalized image plane.
directions[row, col] = new Point3f(-x * s0Inv, -y * s0Inv, s0Inv);
}
}
return(directions);
}
private void CheckCenterPoint(ICoordCalculator calculator)
{
Point3f pointZero;
Point3f pointX;
Point3f pointY;
Point3f pointZ;
if (IsLandscapePaperOrientation)
{
pointZero = new Point3f(148.5f, 105, 0);
}
else
{
pointZero = new Point3f(105, 148.5f, 0);
}
int axesLenght = 30;
pointX = new Point3f(pointZero.X + axesLenght, pointZero.Y, pointZero.Z);
pointY = new Point3f(pointZero.X, pointZero.Y + axesLenght, pointZero.Z);
pointZ = new Point3f(pointZero.X, pointZero.Y, pointZero.Z - axesLenght);
Point2f projectedZero = calculator.ProjectPoint(pointZero);
Point2f projectedX = calculator.ProjectPoint(pointX);
Point2f projectedY = calculator.ProjectPoint(pointY);
Point2f projectedZ = calculator.ProjectPoint(pointZ);
mFrame.Line(projectedZero, projectedX, Scalar.Red);
mFrame.Line(projectedZero, projectedY, Scalar.Green);
mFrame.Line(projectedZero, projectedZ, Scalar.Blue);
}
public void ComputeDesiredIsoValue()
{
float fieldVal, diff, minDiff; // prepare variables
currentFieldVal = Noise.Generate((float)(O.X * agentSim.simplexNoiseScale + agentSim.simplexNoiseTime),
(float)(O.Y * agentSim.simplexNoiseScale + agentSim.simplexNoiseTime),
(float)(O.Z * agentSim.simplexNoiseScale + agentSim.simplexNoiseTime)); // field value at current positions
minDiff = Math.Abs(currentFieldVal - agentSim.isoValue); // update current distance from iso value
Point3d target = O; // if no value is within the difference range, the target is the current position
if (minDiff > agentSim.isoValueTol)
{
for (int i = 0; i < SearchIcosahedron.Length; i++)
{
Point3f sample = (Point3f)(O + SearchIcosahedron[i] * agentSim.planeRadius);
fieldVal = Noise.Generate(sample.X * agentSim.simplexNoiseScale + agentSim.simplexNoiseTime,
sample.Y * agentSim.simplexNoiseScale + agentSim.simplexNoiseTime,
sample.Z * agentSim.simplexNoiseScale + agentSim.simplexNoiseTime);
diff = Math.Abs(fieldVal - agentSim.isoValue);
if (diff < minDiff)
{
minDiff = diff;
target = sample;
}
}
}
desPos += (target - O) * agentSim.isoValIntensity;
}
private void triangle(Point3d Orig, int P0, int P1, int P2, double R, int Ord, int max)
{
if (Ord < max)
{
Point3f Point3 = (new Point3f((M.Vertices[P0].X + M.Vertices[P1].X) / 2, (M.Vertices[P0].Y + M.Vertices[P1].Y) / 2, (M.Vertices[P0].Z + M.Vertices[P1].Z) / 2));
Vector3d PN = new Vector3d(Point3.X, Point3.Y, Point3.Z) - new Vector3d(Orig.X, Orig.Y, Orig.Z);
PN.Unitize();
Point3 = new Point3f((float)PN.X * (float)R, (float)PN.Y * (float)R, (float)PN.Z * (float)R);
Point3.X += (float)Orig.X;
Point3.Y += (float)Orig.Y;
Point3.Z += (float)Orig.Z;
Point3f Point4 = (new Point3f((M.Vertices[P2].X + M.Vertices[P1].X) / 2, (M.Vertices[P2].Y + M.Vertices[P1].Y) / 2, (M.Vertices[P2].Z + M.Vertices[P1].Z) / 2));
PN = new Vector3d(Point4.X, Point4.Y, Point4.Z) - new Vector3d(Orig.X, Orig.Y, Orig.Z);
PN.Unitize();
Point4 = new Point3f((float)PN.X * (float)R, (float)PN.Y * (float)R, (float)PN.Z * (float)R);
Point4.X += (float)Orig.X;
Point4.Y += (float)Orig.Y;
Point4.Z += (float)Orig.Z;
Point3f Point5 = (new Point3f((M.Vertices[P0].X + M.Vertices[P2].X) / 2, (M.Vertices[P0].Y + M.Vertices[P2].Y) / 2, (M.Vertices[P0].Z + M.Vertices[P2].Z) / 2));
PN = new Vector3d(Point5.X, Point5.Y, Point5.Z) - new Vector3d(Orig.X, Orig.Y, Orig.Z);
PN.Unitize();
Point5 = new Point3f((float)PN.X * (float)R, (float)PN.Y * (float)R, (float)PN.Z * (float)R);
Point5.X += (float)Orig.X;
Point5.Y += (float)Orig.Y;
Point5.Z += (float)Orig.Z;
//int P3 = M.Vertices.GetConnectedVertices();
//if (P3 < 0) P3 = M.VertexCount();
//M.SetVertex(P3, Point3);
//int P4 = M.Vertices.Search(Point4);
//if (P4 < 0) P4 = M.VertexCount();
//M.SetVertex(P4, Point4);
//int P5 = M.Vertices.Search(Point5);
//if (P5 < 0) P5 = M.VertexCount();
//M.SetVertex(P5, Point5);
int P3 = M.Vertices.Count;
M.Vertices.SetVertex(P3, Point3);
int P4 = M.Vertices.Count;
M.Vertices.SetVertex(P4, Point4);
int P5 = M.Vertices.Count;
M.Vertices.SetVertex(P5, Point5);
this.triangle(Orig, P0, P3, P5, R, Ord + 1, max);
this.triangle(Orig, P1, P4, P3, R, Ord + 1, max);
this.triangle(Orig, P2, P5, P4, R, Ord + 1, max);
this.triangle(Orig, P3, P4, P5, R, Ord + 1, max);
}
else
{
M.Faces.SetFace(M.Faces.Count, P0, P1, P2);
}
}
/* 3次元座標を画像座標に変換
* @param[in] coordinate3D 変換元の3次元座標
* @return 変換された座標
*/
public Point2f Convert3DToImage(Point3f coordinate3D)
{
Point2f coordinate2D, coordinateImage;
coordinate2D = Convert3DTo2D(coordinate3D);
coordinateImage = Convert2DToImage(coordinate2D);
return(coordinateImage);
}
public virtual void calc1Screen(Point3f center, Vector3f vector, short mad, float offsetFraction, Point3i screen)
{
pointT.set_Renamed(vector);
float scale = mad * offsetFraction;
pointT.scaleAdd(scale, center);
//viewer.transformPoint(pointT, screen);
}
public BVSector3D(Point3f pmPos, Vector2 pmDir, float radians, float pmHeight, bool pmClockwise)
{
pPos= pmPos;
dir= pmDir;
pTheta= radians;
pHeight= pmHeight;
bClockwise= pmClockwise;
}
/// <summary>
/// 计算两点之间距离
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static float Distance(Point3f a, Point3f b)
{
double x2 = Math.Pow(a.X - b.X, 2);
double y2 = Math.Pow(a.Y - b.Y, 2);
double z2 = Math.Pow(a.Z - b.Z, 2);
return((float)Math.Sqrt(x2 + y2 + z2));
}
/// <summary>
/// 返回两点（3f）之间向量（3f）
/// </summary>
/// <param name="origin"></param>
/// <param name="target"></param>
/// <returns></returns>
public static Vector3f P2P(Point3f origin, Point3f target)
{
float x = target.X - origin.X;
float y = target.Y - origin.Y;
float z = target.Z - origin.Z;
return(new Vector3f(x, y, z));
}
public List <Point3f> ConvertFloat3ToPoint3f(float3[] array)
{
Point3f[] pts = new Point3f[array.Length];
Parallel.For(0, array.Length, i =>
{
pts[i] = new Point3f(array[i].x, array[i].y, array[i].z);
});
return(pts.ToList());
}
private static Tuple <Ray, float> RayFromVertices(Point3f a, Point3f b)
{
EPoint pt = new EPoint(a.X, a.Y, a.Z);
EVector vec = new EVector(b.X - a.X, b.Y - a.Y, b.Z - a.Z);
Ray ray = new Ray(pt, vec);
float mag = EVector.Length(vec);
return(new Tuple <Ray, float>(ray, mag));
}
public Face(int tIndex, List<Vertex> tFaceVerts, Vector3f tNormal, Point3f tFaceCentre, WingedMesh tMesh)
//add something which stops you ever sending in only 1 or 2 verts?
{
index = tIndex;
faceVerts = tFaceVerts;
faceNormal = tNormal;
faceNormal.Unitize();
faceCentre = tFaceCentre;
faceEdges = new List<Edge>();
refMesh = tMesh;
initialiseEdges();
}
public MakeShadowTest()
{
this.sizeofMat = 6;
this.package = new ShadowPackage();
this.rnd = new Random();
this.center = new Point3f();
this.shadowCalor = new Scalar[sizeofMat];
this.players = new _playrer[sizeofMat];
this.pt = new Point[sizeofMat];
this.kinect = KinectSensor.GetDefault();
this.joints = new Joint[this.sizeofMat];
this.moveCenter = new Point3f();
this.vecofCenter = new Vec3f();
this.param = new Parameter();
}
/// <summary>
/// Convenient Point3d cast.
/// </summary>
/// <param name="PT"></param>
/// <returns></returns>
public static Point3d castPoint3d(Point3f PT)
{
return new Point3d(PT.X, PT.Y, PT.Z);
}
public static extern void imgproc_fitLine_Point3f(Point3f[] points, int pointsLength, [In, Out] float[] line,
int distType,double param, double reps, double aeps);
internal static extern bool ON_Mesh_GetFaceVertices(IntPtr pConstMesh, int face_index, ref Point3f p0, ref Point3f p1, ref Point3f p2, ref Point3f p3);
/// <summary>
/// For points in an image of a stereo pair, computes the corresponding epilines in the other image.
/// </summary>
/// <param name="points">Input points. N \times 1 or 1 x N matrix of type CV_32FC2 or CV_64FC2.</param>
/// <param name="whichImage">Index of the image (1 or 2) that contains the points .</param>
/// <param name="F">Fundamental matrix that can be estimated using findFundamentalMat() or stereoRectify() .</param>
/// <returns>Output vector of the epipolar lines corresponding to the points in the other image.
/// Each line ax + by + c=0 is encoded by 3 numbers (a, b, c) .</returns>
public static Point3f[] ComputeCorrespondEpilines(IEnumerable<Point3d> points,
int whichImage, double[,] F)
{
if (points == null)
throw new ArgumentNullException("points");
if (F == null)
throw new ArgumentNullException("F");
if (F.GetLength(0) != 3 && F.GetLength(1) != 3)
throw new ArgumentException("F != double[3,3]");
Point3d[] pointsArray = EnumerableEx.ToArray(points);
Point3f[] lines = new Point3f[pointsArray.Length];
NativeMethods.calib3d_computeCorrespondEpilines_array3d(
pointsArray, pointsArray.Length,
whichImage, F, lines);
return lines;
}
internal static extern void ON_Mesh_SetTopologyVertex(IntPtr pMesh, int index, Point3f point);
public static extern void calib3d_solvePnPRansac_vector(Point3f[] objectPoints, int objectPointsLength,
Point2f[] imagePoints, int imagePointsLength, double[,] cameraMatrix, double[] distCoeffs, int distCoeffsLength,
[Out] double[] rvec, [Out] double[] tvec, int useExtrinsicGuess, int iterationsCount, float reprojectionError,
double confidence, IntPtr inliers, int flags);
/// <summary>
/// Add two points together.
/// </summary>
/// <param name="PT"></param>
/// <param name="PT2"></param>
/// <returns></returns>
public static Point3d AddPTPT(Point3f PT, Vector3d PT2)
{
return new Point3d(PT.X + PT2.X, PT.Y + PT2.Y, PT.Z + PT2.Z);
}
public void Print(Point3f P)
{
Console.WriteLine(P.X.ToString() + "," + P.Y.ToString() + "," + P.Z.ToString());
}
public static List<tribox> FromMesh(Mesh mesh)
{
List<tribox> output = new List<tribox>();
if (mesh.Vertices.Count == 0) return output;
Point3d cen = new Point3d();
for (int i = 0; i < mesh.Vertices.Count; i++)
{
cen += mesh.Vertices[i];
}
cen /= mesh.Vertices.Count;
for (int i = 0; i < mesh.Faces.Count; i++)
{
Point3f[] pts = new Point3f[4];
pts[0] = new Point3f((float)cen.X, (float)cen.Y, (float)cen.Z);
pts[1] = mesh.Vertices[mesh.Faces[i].A];
pts[2] = mesh.Vertices[mesh.Faces[i].B];
pts[3] = mesh.Vertices[mesh.Faces[i].C];
output.Add(new tribox(pts));
}
return output;
}
public tribox(Point3f[] pts)
{
Cedges = new edge[6];
Points = new point[4];
Points[0] = new point(pts[0]); Points[1] = new point(pts[1]);
Points[2] = new point(pts[2]); Points[3] = new point(pts[3]);
Cedges[0] = new edge(Points[0], Points[1]);
Cedges[1] = new edge(Points[0], Points[2]);
Cedges[2] = new edge(Points[0], Points[3]);
Cedges[3] = new edge(Points[1], Points[2]);
Cedges[4] = new edge(Points[2], Points[3]);
Cedges[5] = new edge(Points[3], Points[1]);
}
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#endif
public Point3d PerpendicularFoot(Point3f point)
{
return PerpendicularFoot(point.X, point.Y, point.Z);
}
/// <summary>
/// 指定した点と直線の距離を返す
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
/// Returns the distance between this line and the specified point
/// </summary>
/// <param name="point"></param>
#endif
public double Distance(Point3f point)
{
return Distance(point.X, point.Y, point.Z);
}
/// <summary>
/// Adds a new vertex to the end of the Vertex list.
/// </summary>
/// <param name="vertex">Location of new vertex.</param>
/// <returns>The index of the newly added vertex.</returns>
public static int Add(this PlanktonVertexList vertexList, Point3f vertex)
{
return vertexList.Add(vertex.X, vertex.Y, vertex.Z);
}
public static extern void calib3d_solvePnP_vector(Point3f[] objectPoints, int objectPointsLength,
Point2f[] imagePoints, int imagePointsLength,
double[,] cameraMatrix, double[] distCoeffs, int distCoeffsLength,
[Out] double[] rvec, [Out] double[] tvec, int useExtrinsicGuess, int flags);
double Distance3D(Point3f point1, Point3f point2)
{
double squared1 = Math.Pow((point2.X - point1.X), 2);
double squared2 = Math.Pow((point2.Y - point1.Y), 2);
double squared3 = Math.Pow((point2.Z - point1.Z), 2);
double sum = squared1 + squared2 + squared3;
double result = Math.Sqrt(sum);
return result;
}
private void addFace(int[] faceIndices, Vector3f faceNormal, Point3f faceCentre)
{
List<Vertex> faceVerts = new List<Vertex>();
foreach (int i in faceIndices)
{
faceVerts.Add(vertices[i]);
}
faces.Add(new Face(faces.Count,faceVerts,faceNormal,faceCentre,this));
}
private void VertexInterp(double isolevel, Point3d p1, Point3d p2, double valp1, double valp2)
{
double mu;
if (Math.Abs(isolevel - valp1) < 0.00001)
Cpoint = new Point3f((float)p1.X, (float)p1.Y, (float)p1.Z);
if (Math.Abs(isolevel - valp2) < 0.00001)
Cpoint = new Point3f((float)p2.X, (float)p2.Y, (float)p2.Z);
if (Math.Abs(valp1 - valp2) < 0.00001)
Cpoint = new Point3f((float)p1.X, (float)p1.Y, (float)p1.Z);
Point3f p = new Point3f();
mu = (isolevel - valp1) / (valp2 - valp1);
p.X = (float)(p1.X + (p2.X - p1.X) * mu);
p.Y = (float)(p1.Y + (p2.Y - p1.Y) * mu);
p.Z = (float)(p1.Z + (p2.Z - p1.Z) * mu);
Cpoint = p;
}
private static Joint addJoint(Dictionary<Joint, Point3f> jointPositions, Joint joint)
{
//Add joint to JointPositions
Point3f jointPoint = new Point3f();
jointPoint.X = joint.Position.X;
jointPoint.Y = joint.Position.Y;
jointPoint.Z = joint.Position.Z;
jointPoint.tracked = (joint.TrackingState == JointTrackingState.Tracked);
jointPoint.time = DateTime.Now;
jointPositions.Add(joint, jointPoint);
return joint;
}
/// <summary>
/// <para>Sets or adds a vertex to the Vertex List.</para>
/// <para>If [index] is less than [Count], the existing vertex at [index] will be modified.</para>
/// <para>If [index] equals [Count], a new vertex is appended to the end of the vertex list.</para>
/// <para>If [index] is larger than [Count], the function will return false.</para>
/// </summary>
/// <param name="index">Index of vertex to set.</param>
/// <param name="vertex">Vertex location.</param>
/// <returns><c>true</c> on success, <c>false</c> on failure.</returns>
public static bool SetVertex(this PlanktonVertexList vertexList, int index, Point3f vertex)
{
return vertexList.SetVertex(index, vertex.X, vertex.Y, vertex.Z);
}
private void triangle(Point3d Orig, int P0, int P1, int P2, double R, int Ord, int max)
{
if (Ord < max)
{
Point3f Point3 = (new Point3f((M.Vertices[P0].X + M.Vertices[P1].X) / 2, (M.Vertices[P0].Y + M.Vertices[P1].Y) / 2, (M.Vertices[P0].Z + M.Vertices[P1].Z) / 2));
Vector3d PN = new Vector3d(Point3.X, Point3.Y, Point3.Z) - new Vector3d(Orig.X, Orig.Y, Orig.Z);
PN.Unitize();
Point3 = new Point3f((float)PN.X * (float)R, (float)PN.Y * (float)R, (float)PN.Z * (float)R);
Point3.X += (float)Orig.X;
Point3.Y += (float)Orig.Y;
Point3.Z += (float)Orig.Z;
Point3f Point4 = (new Point3f((M.Vertices[P2].X + M.Vertices[P1].X) / 2, (M.Vertices[P2].Y + M.Vertices[P1].Y) / 2, (M.Vertices[P2].Z + M.Vertices[P1].Z) / 2));
PN = new Vector3d(Point4.X, Point4.Y, Point4.Z) - new Vector3d(Orig.X, Orig.Y, Orig.Z);
PN.Unitize();
Point4 = new Point3f((float)PN.X * (float)R, (float)PN.Y * (float)R, (float)PN.Z * (float)R);
Point4.X += (float)Orig.X;
Point4.Y += (float)Orig.Y;
Point4.Z += (float)Orig.Z;
Point3f Point5 = (new Point3f((M.Vertices[P0].X + M.Vertices[P2].X) / 2, (M.Vertices[P0].Y + M.Vertices[P2].Y) / 2, (M.Vertices[P0].Z + M.Vertices[P2].Z) / 2));
PN = new Vector3d(Point5.X, Point5.Y, Point5.Z) - new Vector3d(Orig.X, Orig.Y, Orig.Z);
PN.Unitize();
Point5 = new Point3f((float)PN.X * (float)R, (float)PN.Y * (float)R, (float)PN.Z * (float)R);
Point5.X += (float)Orig.X;
Point5.Y += (float)Orig.Y;
Point5.Z += (float)Orig.Z;
//int P3 = M.Vertices.GetConnectedVertices();
//if (P3 < 0) P3 = M.VertexCount();
//M.SetVertex(P3, Point3);
//int P4 = M.Vertices.Search(Point4);
//if (P4 < 0) P4 = M.VertexCount();
//M.SetVertex(P4, Point4);
//int P5 = M.Vertices.Search(Point5);
//if (P5 < 0) P5 = M.VertexCount();
//M.SetVertex(P5, Point5);
int P3 = M.Vertices.Count;
M.Vertices.SetVertex(P3, Point3);
int P4 = M.Vertices.Count;
M.Vertices.SetVertex(P4, Point4);
int P5 = M.Vertices.Count;
M.Vertices.SetVertex(P5, Point5);
this.triangle(Orig, P0, P3, P5, R, Ord + 1, max);
this.triangle(Orig, P1, P4, P3, R, Ord + 1, max);
this.triangle(Orig, P2, P5, P4, R, Ord + 1, max);
this.triangle(Orig, P3, P4, P5, R, Ord + 1, max);
}
else
{
M.Faces.SetFace(M.Faces.Count, P0, P1, P2);
}
}
public static extern void core_Mat_push_back_Point3f(IntPtr self, Point3f v);
internal static extern bool ON_Mesh_Vertex(IntPtr ptr, int index, ref Point3f pt);
/// <summary>
/// Get array of vertex positions for a given mesh face
/// </summary>
/// <param name="faceIndex"></param>
/// <returns></returns>
private Point3f[] GetVerticesOfFace(int faceIndex)
{
Point3f[] VertexTemporary = new Point3f[4];
this.mesh.Faces.GetFaceVertices(faceIndex, out VertexTemporary[0], out VertexTemporary[1], out VertexTemporary[2], out VertexTemporary[3]);
return VertexTemporary;
}
internal static extern void ON_Mesh_TopologyVertex(IntPtr pConstMesh, int index, ref Point3f point);
public void Update_Position(Point3f Center)
{
for (int i = 0; i < m_DisplayMesh.Vertices.Count; i++)
{
Point3f Po = m_RhinoMesh.Vertices[i];
Point3f P = new Point3f(m_RhinoMesh.Vertices[i].X + Center.X - CurrentPos.X, m_RhinoMesh.Vertices[i].Y + Center.Y - CurrentPos.Y, m_RhinoMesh.Vertices[i].Z + Center.Z - CurrentPos.Z);
m_RhinoMesh.Vertices.SetVertex(i, P);
Point3f Pn = m_RhinoMesh.Vertices[i];
double x = Po.Y + Pn.Y;
}
CurrentPos = Center;
Update_Aim();
}
public point(Point3d pt)
{
this.loc = new Point3f((float)pt.X, (float)pt.Y, (float)pt.Z);
}
public point(Point3f pt)
{
this.loc = pt;
}