static void CalcBSpline(
IntCoord cminus1x, IntCoord cminus1y, IntCoord cx, IntCoord cy,
IntCoord cplus1x, IntCoord cplus1y, IntCoord cplus2x, IntCoord cplus2y
) {
double p0x, p1x, p2x, p3x, tempx,
p0y, p1y, p2y, p3y, tempy;
ThirdPoint(
double(cx), double(cy), double(cplus1x), double(cplus1y), p1x, p1y
);
ThirdPoint(
double(cplus1x), double(cplus1y), double(cx), double(cy), p2x, p2y
);
ThirdPoint(
double(cx), double(cy), double(cminus1x), double(cminus1y),
tempx, tempy
);
Midpoint(tempx, tempy, p1x, p1y, p0x, p0y);
ThirdPoint(
double(cplus1x), double(cplus1y), double(cplus2x), double(cplus2y),
tempx, tempy
);
Midpoint(tempx, tempy, p2x, p2y, p3x, p3y);
AddBezierCurve(p0x, p0y, p1x, p1y, p2x, p2y, p3x, p3y);
}
void MultiLineObj::CalcSection (
Coord cminus1x, Coord cminus1y, Coord cx, Coord cy,
Coord cplus1x, Coord cplus1y, Coord cplus2x, Coord cplus2y
) {
double p0x, p1x, p2x, p3x, tempx,
p0y, p1y, p2y, p3y, tempy;
ThirdPoint(
double(cx), double(cy), double(cplus1x), double(cplus1y), p1x, p1y
);
ThirdPoint(
double(cplus1x), double(cplus1y), double(cx), double(cy), p2x, p2y
);
ThirdPoint(
double(cx), double(cy), double(cminus1x), double(cminus1y),
tempx, tempy
);
Midpoint(tempx, tempy, p1x, p1y, p0x, p0y);
ThirdPoint(
double(cplus1x), double(cplus1y), double(cplus2x), double(cplus2y),
tempx, tempy
);
Midpoint(tempx, tempy, p2x, p2y, p3x, p3y);
AddBezierArc(p0x, p0y, p1x, p1y, p2x, p2y, p3x, p3y);
}
SEGMENTH CombinePolygonPieces(SEGMENTH *A1, SEGMENTH *B1,
Boolean keepAinB, Boolean keepAnotinB,
Boolean keepBinA, Boolean keepBnotinA)
{
long i, j, err, numSegsA, numSegsB, numSegsC = 0;
WorldPoint m;
SEGMENTH C = 0, A2 = 0, B2 = 0, *A = 0, *B = 0;
err = 0;
numSegsA = _GetHandleSize((Handle)*A1) / sizeof(Segment);
numSegsB = _GetHandleSize((Handle)*B1) / sizeof(Segment);
A2 = (SEGMENTH)_NewHandle(numSegsA * sizeof(Segment));
if (_MemError()) { TechError("CombinePolygonPieces()", "_NewHandle()", 0); goto done; }
for (i = 0 ; i < numSegsA ; i++)
INDEXH(A2, i) = INDEXH(*A1, i);
A = &A2;
B2 = (SEGMENTH)_NewHandle(numSegsB * sizeof(Segment));
if (_MemError()) { TechError("CombinePolygonPieces()", "_NewHandle()", 0); goto done; }
for (i = 0 ; i < numSegsB ; i++)
INDEXH(B2, i) = INDEXH(*B1, i);
B = &B2;
for (i = 0 ; i < numSegsA ; i++)
for (j = 0 ; j < numSegsB ; j++)
if (SegmentTouchesSegment(INDEXH(*A, i), INDEXH(*B, j)) &&
!SameSegmentEndPoints(INDEXH(*A, i), INDEXH(*B, j))) {
m = PointOfIntersection(INDEXH(*A, i), INDEXH(*B, j));
if (err = InsertSegment(A, &numSegsA, i, m)) goto done;
if (err = InsertSegment(B, &numSegsB, j, m)) goto done;
}
C = (SEGMENTH)_NewHandle(0);
if (_MemError()) { TechError("CombinePolygonPieces()", "_NewHandle()", 0); goto done; }
for (i = 0 ; i < numSegsA ; i++) {
m = Midpoint(INDEXH(*A, i));
if ((keepAinB && PointInPolygon(m, *B, numSegsB, TRUE)) ||
(keepAnotinB && !PointInPolygon(m, *B, numSegsB, TRUE)))
if (err = AddSegment(&C, &numSegsC, INDEXH(*A, i))) goto done;
}
for (j = 0 ; j < numSegsB ; j++) {
m = Midpoint(INDEXH(*B, j));
if ((keepBinA && PointInPolygon(m, *A, numSegsA, TRUE)) ||
(keepBnotinA && !PointInPolygon(m, *A, numSegsA, TRUE)))
if (err = AddSegment(&C, &numSegsC, INDEXH(*B, j))) goto done;
}
SortSegments(C, numSegsC);
done:
if (A2) DisposeHandle((Handle)A2);
if (B2) DisposeHandle((Handle)B2);
if (err && C) DisposeHandle((Handle)C);
return err ? 0 : C;
}
bool Sphere::Init(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 4)
return false;
// get center
size_t c = samples.size() / 2;
m_center = Vec3f(0, 0, 0);
size_t midCount = 0;
for(size_t i = 0; i < c - 1; ++i)
for(size_t j = i + 1; j < c; ++j)
{
Vec3f mid;
if(!Midpoint(samples[i], samples[i + c], samples[j], samples[j + c], &mid))
continue;
m_center += mid;
++midCount;
}
if(!midCount)
return false;
m_center /= midCount;
m_radius = 0;
for(size_t i = 0; i < c; ++i)
{
float d = (samples[i] - m_center).length();
m_radius += d;
}
m_radius /= c;
return true;
}
void MultiLineObj::AddBezierArc (
double x0, double y0, double x1, double y1,
double x2, double y2, double x3, double y3
) {
double midx01, midx12, midx23, midlsegx, midrsegx, cx,
midy01, midy12, midy23, midlsegy, midrsegy, cy;
Midpoint(x0, y0, x1, y1, midx01, midy01);
Midpoint(x1, y1, x2, y2, midx12, midy12);
Midpoint(x2, y2, x3, y3, midx23, midy23);
Midpoint(midx01, midy01, midx12, midy12, midlsegx, midlsegy);
Midpoint(midx12, midy12, midx23, midy23, midrsegx, midrsegy);
Midpoint(midlsegx, midlsegy, midrsegx, midrsegy, cx, cy);
if (CanApproxWithLine(x0, y0, midlsegx, midlsegy, cx, cy)) {
AddLine(x0, y0, cx, cy);
} else if (
(midx01 != x1) || (midy01 != y1) || (midlsegx != x2) ||
(midlsegy != y2) || (cx != x3) || (cy != y3)
) {
AddBezierArc(x0, y0, midx01, midy01, midlsegx, midlsegy, cx, cy);
}
if (CanApproxWithLine(cx, cy, midx23, midy23, x3, y3)) {
AddLine(cx, cy, x3, y3);
} else if (
(cx != x0) || (cy != y0) || (midrsegx != x1) || (midrsegy != y1) ||
(midx23 != x2) || (midy23 != y2)
) {
AddBezierArc(cx, cy, midrsegx, midrsegy, midx23, midy23, x3, y3);
}
}
void SteerLib::GJK_EPA::UpdateEars(std::queue<Util::Vector>& ears, std::vector<struct edges> edges, std::vector<Util::Vector> shape)
{
for (int i = 0; i < shape.size(); i++) {
Util::Vector curr = shape[i];
Util::Vector n1, n2, testpoint;
for (int j = 0; j < edges.size(); j++) {
if (edges[j].point == curr) {
n1 = edges[j].neighbor1;
n2 = edges[j].neighbor2;
break;
}
}
// Get midpoint of the two neighbors
testpoint = Midpoint(n1, n2);
// Test the midpoint
// If it lies within the polygon, it's good
bool result = false;
int k, j;
for (k = 0, j = shape.size() - 1; k < shape.size(); j = k++) {
if ((shape[k].z > testpoint.z) != (shape[j].z > testpoint.z) && (testpoint.x < (shape[j].x - shape[k].x) * (testpoint.z - shape[k].z) / (shape[j].z - shape[k].z) + shape[k].x)) {
result = !result;
}
}
// TODO ONLY IF THEY'RE GONNA HAVE OTHER TEST CASES
// Need to do check if point is on an edge
// If midpoint is a point on shape, it's not an ear
if (std::find(shape.begin(), shape.end(), testpoint) != shape.end())
result = false;
if (result) {
if(!EarCheck(ears, shape[i]))
ears.push(shape[i]);
}
}
}
const R3Sphere R3Span::
BSphere(void) const
{
// Return bounding sphere
return R3Sphere(Midpoint(), 0.5 * Length());
}
