public bool EqualsPoint(MyVector2 v)
{
if ((this - v).Length() < 1e-6)
{
return(true);
}
else
{
return(false);
}
}
public void Project3dToBelongPlane()
{
// prepare 2d coordinate
MyVector3 circle_norm = this.belongPlane.Normal();
MyVector3 X = new MyVector3(1, 0, 0);
MyVector3 Y = new MyVector3(0, 1, 0);
MyVector3 Z = new MyVector3(0, 0, 1);
MyVector3 rotAxis = Z.Cross(circle_norm).Normalize();
double angle_cos = Z.Dot(circle_norm);
if (angle_cos > 1)
{
angle_cos = 1;
}
if (angle_cos < -1)
{
angle_cos = -1;
}
double rotAngle = Math.Acos(angle_cos);
MyMatrix4d Mat = MyMatrix4d.RotationMatrix(rotAxis, rotAngle);
MyVector3 X_new = (Mat * new MyVector4(X)).XYZ().Normalize();
MyVector3 Y_new = (Mat * new MyVector4(Y)).XYZ().Normalize();
CoordinateFrame frame = new CoordinateFrame(this.belongPlane.planeCenter, X_new, Y_new, circle_norm);
// projection and denoise(leave out far away points)
MyVector3 tmpc = frame.GetPointLocalCoord(this.center);
this.center2d = new MyVector2(tmpc.x, tmpc.y);
for (int i = 0; i < this.circleSlice3d.Count; i++)
{
MyVector3 vert = this.circleSlice3d[i];
MyVector3 tmp = frame.GetPointLocalCoord(vert);
MyVector2 tmp2 = new MyVector2(tmp.x, tmp.y);
double dist = Math.Abs((tmp2 - this.center2d).Length() - this.radius);
if (dist > 0.5 * this.radius && dist < 0.75 * this.radius)
{
this.circleSlice3d.RemoveAt(i);
i--;
}
else
{
this.circleSlice2d.Add(tmp2);
}
}
}
public bool PointInPlane2d(MyVector2 p)
{
this.ProjectVerticesTo2d();
bool c = false;
List <MyVector2> points = this.planeVertices2d;
int n = points.Count;
for (int i = 0, j = n - 1; i < n; j = i++)
{
if (((points[i].y > p.y) != (points[j].y > p.y)) &&
(p.x < (points[j].x - points[i].x) * (p.y - points[i].y) / (points[j].y - points[i].y) + points[i].x))
{
c = !c;
}
}
return(c);
}
private MyVector3 MapToSphere(MyVector2 pt)
{
#region old map
//MyVector2 v = new MyVector2();
////v.x = (w - pt.x) * adjustWidth;
//v.x = (pt.x - this.w) * adjustWidth;
//v.y = (this.h - pt.y) * adjustHeight;
//double lenSq = v.Dot(v);
//MyVector3 v3 = new MyVector3(v.x, 0, v.y);
//if (lenSq > 1.0)
//{
// double norm = 1.0 / Math.Sqrt(lenSq);
// return new MyVector3(v.x * norm, -v.y * norm, 0);
//}
//else
//{
// return new MyVector3(v.x, Math.Sqrt(1.0 - lenSq), v.y);
//}
#endregion
//just. 1116
MyVector2 v = new MyVector2();
v.x = (this.w - pt.x) * adjustWidth / radius;
v.y = (this.h - pt.y) * adjustHeight / radius;
double lenSq = v.Dot(v);
//MyVector3 v3 = new MyVector3(v.x, 0, v.y);
if (lenSq > 1.0)
{
double norm = 1.0 / Math.Sqrt(lenSq);
return(new MyVector3(v.x * norm, v.y * norm, 0));
}
else
{
return(new MyVector3(v.x, v.y, Math.Sqrt(1.0 - lenSq)));
}
}
public void Drag(MyVector2 pt)
{
edPt = pt;
edVec = MapToSphere(pt);
//angle = Math.Acos(stVec.Dot(edVec));
double epsilon = 1.0e-5;
MyVector3 prep = stVec.Cross(edVec);
if (prep.Length() > epsilon)
{
quat = new MyVector4();
quat.x = prep.x;
quat.y = prep.y;
quat.z = prep.z;
quat.w = stVec.x + edVec.x + stVec.y + edVec.y + stVec.z + edVec.z;
}
else
{
quat = new MyVector4();
}
//if (prep.Length() > epsilon)
//{
// quat = new MyVector3();
// quat.x = prep.x;
// quat.y = prep.y;
// quat.z = prep.z;
//}
//else
// quat = new MyVector3();
//angle = Math.PI / 2.0 * prep.Length();
}
public void ProjCenterTo2d()
{
MyVector3 tmp = frame.GetPointLocalCoord(center);
center2d = tmp.XY();
}
public double Dot(MyVector2 v)
{
return(x * v.x + y * v.y);
}
public MyVector3(MyVector2 v, double z)
{
this.x = v.x;
this.y = v.y;
this.z = z;
}
public MyVector3(MyVector2 v)
{
this.x = v.x;
this.y = v.y;
this.z = 0;
}
public MyVector2(MyVector2 v)
{
this.x = v.x;
this.y = v.y;
}
static public double Distance(MyVector2 a, MyVector2 b)
{
return((a - b).Length());
}
public double Cross(MyVector2 v)
{
return(x * v.y - y * v.x);
}
public bool Equals(MyVector2 v)
{
return((this.x == v.x) && (this.y == v.y) ? true : false);
}
public void Optimizing()
{
// do optimize in 2d space
// each gcuboid has a list of polygon(defined in MyPolygon.cs)
// input data : polygon's 2d information, including center2d, cornerPoints2d and slicePoints2d
// output data : new position of cornerPoints2d
#region a theoretically fast method, but there are mystery bugs in it for now =.=
K = this.polyList.Count;
N = this.polyList[0].CornerPoints2d.Count;
//// compute every triangle's initial height
//h = new double[K * N]; // using Heron's formula to compute height
//CLineSegment[] bottomEdges = new CLineSegment[K * N]; // bottom edges (used to calculate dis)
//int triangleIdx = 0;
int idx = 0;
center2d = new MyVector2[K];
foreach (MyPolygon mp in this.polyList)
{
center2d[idx] = new MyVector2();
foreach (MyVector2 vert in mp.CornerPoints2d)
{
center2d[idx] += vert;
}
center2d[idx] /= mp.CornerPoints2d.Count;
//double edgeLengthA = (mp.CornerPoints2d[0] - center2d[idx]).Length();
//for (int i = 0; i < mp.CornerPoints2d.Count; i++)
//{
// bottomEdges[triangleIdx] = new CLineSegment(mp.CornerPoints2d[i], mp.CornerPoints2d[(i + 1) % N]);
// double edgeLengthB = (mp.CornerPoints2d[(i + 1) % N] - center2d).Length();
// double edgeLengthBottom = bottomEdges[triangleIdx].GetLineSegmentLength();
// double p = (edgeLengthA + edgeLengthB + edgeLengthBottom) / 2;
// double S = Math.Sqrt(p * (p - edgeLengthA) * (p - edgeLengthB) * (p - edgeLengthBottom));
// h[triangleIdx] = 2 * S / edgeLengthBottom;
// triangleIdx++;
// edgeLengthA = edgeLengthB;
//}
idx++;
}
//// compute initial distance between slice points and its nearest bottom edge
//dis = new double[M_sum]; // initial distances
//segmentIdx = new int[M_sum]; // belong to which bottom edge
//int disIdx = 0, mpIdx = 0;
//foreach (MyPolygon mp in this.polyList)
//{
// foreach (MyVector2 mv2 in mp.SlicePoints2d)
// {
// double minDis = double.MaxValue;
// for (int i = mpIdx; i < mpIdx + N; i++)
// {
// // must be countercwise, and when point is on the left of the segment, dis > 0
// double tmpDis = bottomEdges[i].GetDistance(mv2) * -bottomEdges[i].GetPointLocation(mv2);
// if (Math.Abs(minDis) > Math.Abs(tmpDis))
// {
// minDis = tmpDis;
// segmentIdx[disIdx] = i;
// }
// }
// dis[disIdx++] = minDis;
// }
// mpIdx += N;
//}
//// saved rho (to compute deltaRho in each iteration)
//rho = new double[K];
//for (int i = 0; i < K; i++)
// rho[i] = 1.0;
#endregion
// begin to optimize
double[] x = new double[K];
for (int i = 0; i < K; i++)
{
x[i] = 1.0;
}
double epsg = 1e-4;
double epsf = 0;
double epsx = 0;
int maxits = 100; //300
alglib.minlmstate state;
alglib.minlmreport rep;
alglib.minlmcreatev(2, x, 0.01, out state);
alglib.minlmsetcond(state, epsg, epsf, epsx, maxits);
alglib.minlmoptimize(state, funcFitPolygon1, null, null);
alglib.minlmresults(state, out x, out rep);
idx = 0;
foreach (MyPolygon mp in this.polyList)
{
MyVector3 center = mp.Center;
for (int i = 0; i < mp.CornerPoints3d.Count; i++)
{
mp.CornerPoints3d[i] = x[idx] * (mp.CornerPoints3d[i] - center) + center;
}
idx++;
}
}
private void funcFitPolygon1(double[] x, double[] f, object obj)
{
f[0] = 0.0;
f[1] = 0.0;
int idx = 0;
foreach (MyPolygon mp in this.polyList)
{
MyVector2[] vertices = new MyVector2[N];
for (int i = 0; i < mp.CornerPoints2d.Count; i++)
{
vertices[i] = center2d[idx] + (mp.CornerPoints2d[i] - center2d[idx]) * x[idx];
}
CLine[] segments = new CLine[N];
for (int i = 0; i < mp.CornerPoints2d.Count; i++)
{
segments[i] = new CLine(vertices[i], vertices[(i + 1) % N]);
}
foreach (MyVector2 mv2 in mp.SlicePoints2d)
{
double minDis = double.MaxValue;
for (int i = 0; i < N; i++)
{
double disTmp = segments[i].GetDistance(mv2);
if (minDis > disTmp)
{
minDis = disTmp;
}
}
f[0] += minDis;
}
idx++;
}
// compute f[1]
for (int i = 0; i < K; i++)
{
if (i == 0)
{
f[1] += Math.Pow(x[i] - x[i + 1], 2);
}
else if (i == K - 1)
{
f[1] += Math.Pow(x[i] - x[i - 1], 2);
}
else
{
f[1] += Math.Pow(2 * x[i] - x[i - 1] - x[i + 1], 2);
}
}
f[1] *= 1e-1;
if (outputIdx == 0)
{
Console.WriteLine(f[0].ToString("0.000") + "\t" + f[1].ToString("0.000"));
}
outputIdx = (outputIdx + 1) % 1000;
}