void quadstretch(float x,float y,float l,float t,
float w,float h,float *qx,float *qy,float *ret){
/*
--x & y are the original point
-- l & t are left and top of original rect
-- w & h are width and height of original rect
--qx and qy are arrays of 4 points describing the new quad.
--1. strip it down to floats between 0 and 1
-- so we know where it is, relative to the boundaries.
*/
float xF = (x-l)/float(w);
float yF = (y-t)/float(h);
// 2. generate "vertical"
float interpTop[2];
interpolatePoints(qx[0],qy[0],qx[1],qy[1],xF,interpTop);
float interpBottom[2];
interpolatePoints(qx[3],qy[3],qx[2],qy[2],xF,interpBottom);
//3. generate "horizontal"
float interpLeft[2];
interpolatePoints(qx[0],qy[0],qx[3],qy[3],yF,interpLeft);
float interpRight[2];
interpolatePoints(qx[1],qy[1],qx[2],qy[2],yF,interpRight);
intersect_lines(interpTop [0], interpTop[1],
interpBottom[0],interpBottom[1],
interpLeft [0], interpLeft[1],
interpRight [0], interpRight[1],ret);
}
Vector2d leastSquaresEstimate(vector<Vector2f> points,
Vector2f p0, Vector2f p3,
Vector2d d1, Vector2d d2)
{
// hack up points
if(points.size() <= 10)
points = interpolatePoints(points);
if(points.size() <= 20)
points = interpolatePoints(points);
qDebug() << "num points in arc" << points.size();
int m = points.size()-2; // number of points to fit
int n = 2; // number of control points to find
Eigen::MatrixXd Z(m, n);
Eigen::MatrixXd Y(m, n);
Eigen::VectorXd pu(m);
Eigen::VectorXd pv(m);
for(int i = 0; i < m; ++i)
{
int idx = i+1;
Vector2f pt = points[idx];
double t = double(idx)/(m-1);
auto b0 = Bernstein3(0, t);
auto b1 = Bernstein3(1, t);
auto b2 = Bernstein3(2, t);
auto b3 = Bernstein3(3, t);
// Filling in the matrices
Z(i, 0) = b1*d1[0];
Z(i, 1) = b2*d2[0];
Y(i, 0) = b1*d1[1];
Y(i, 1) = b2*d2[1];
auto pp = pt - p0*(b0 + b1) - p3*(b2 + b3);
pu(i) = pp[0];
pv(i) = pp[1];
}
auto Zt = Z.transpose();
auto Yt = Y.transpose();
auto A = Zt*Z+Yt*Y;
auto rhs = Zt*pu+Yt*pv;
Eigen::VectorXd ans = A.inverse()*rhs;
auto s = makeVector2d(ans(0), ans(1));
qDebug() << "least squares" << s[0] << s[1];
s[0]=fabs(s[0])/**10*/;
s[1]=fabs(s[1]);
return s;
}
void makeLines()
{
glBegin(GL_LINE_STRIP);
glColor3f(1.0f, 1.0f, 1.0f);
for (int i = 0; i < g_iNumOfSplines; i++) {
for (int j = 1; j < g_Splines[i].numControlPoints - 2; j++) {
std::vector<Point> pList;
int index = 1;
float u = 0.5f;
float uVector[4] = { 0, 0, 0, 0 };
float uBasis[4] = { 0, 0, 0, 0 };
pList.push_back(g_Splines[i].points[j]);
createControlMatrix(j, g_Splines[i], g_mControlMatrix);
// Subdivide segment recursively
interpolatePoints(g_Splines[i].points[j], g_Splines[i].points[j + 1],
pList, g_mBasisMatrix, g_mControlMatrix, g_fMaxDistance, index, u, 0.0f, 1.0f, uVector, uBasis);
for (int k = 0; k < index; k++) {
glVertex3d(pList[k].x - 75, pList[k].y - 30, pList[k].z - 6);
}
for (auto iter = pList.begin(); iter != pList.end(); iter++) {
pointsList.push_back(*iter);
totalPoints++;
}
}
// Add last vertex
pointsList.push_back(g_Splines[i].points[g_Splines[i].numControlPoints - 2]);
glVertex3d(g_Splines[i].points[g_Splines[i].numControlPoints - 2].x - 75, g_Splines[i].points[g_Splines[i].numControlPoints - 2].y - 30, g_Splines[i].points[g_Splines[i].numControlPoints - 2].z - 6);
}
glEnd();
}
//------------------------------------------------------------------------
FloatPointVector* interpolatePolygonPoints(PointVector* points) {
size_t pointCount = points->size();
FloatPointVector* res = new FloatPointVector;
for(size_t i = 0; i < pointCount; i++) {
interpolatePoints(res, (*points)[(i-1 + pointCount) % pointCount],
(*points)[i]);
}
return res;
}
void AngleList::makeAngles(int sides, float startAngle, float stopAngle)
{
double twoPi = 3.14159 * 2.0;
float twoPiInv = 1.0f / (float)twoPi;
if (sides < 1)
throw "number of sides not greater than zero";
if (stopAngle <= startAngle)
throw "stopAngle not greater than startAngle";
if ((sides == 3 || sides == 4 || sides == 24))
{
startAngle *= twoPiInv;
stopAngle *= twoPiInv;
const Angle *sourceAngles;
int sourceAnglesLen;
if (sides == 3) {
sourceAngles = (Angle *)&angles3;
sourceAnglesLen = 4;
} else if (sides == 4) {
sourceAngles = (Angle *)&angles4;
sourceAnglesLen = 5;
} else {
sourceAngles = (Angle *)&angles24;
sourceAnglesLen = 25;
}
int startAngleIndex = (int)(startAngle * sides);
int endAngleIndex = sourceAnglesLen - 1;
if (stopAngle < 1.0f)
endAngleIndex = (int)(stopAngle * sides) + 1;
if (endAngleIndex == startAngleIndex)
endAngleIndex++;
for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++)
{
angles.push_back(sourceAngles[angleIndex]);
if (sides == 3)
normals.push_back(normals3[angleIndex]);
else if (sides == 4)
normals.push_back(normals4[angleIndex]);
}
if (startAngle > 0.0f)
angles[0] = interpolatePoints(startAngle, angles[0], angles[1]);
if (stopAngle < 1.0f)
{
int lastAngleIndex = angles.size() - 1;
angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]);
}
}
else
{
double stepSize = twoPi / sides;
int startStep = (int)(startAngle / stepSize);
double angle = stepSize * startStep;
int step = startStep;
double stopAngleTest = stopAngle;
if (stopAngle < twoPi)
{
stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1);
if (stopAngleTest < stopAngle)
stopAngleTest += stepSize;
if (stopAngleTest > twoPi)
stopAngleTest = twoPi;
}
while (angle <= stopAngleTest)
{
Angle newAngle;
newAngle.angle = (float)angle;
newAngle.X = (float)cos(angle);
newAngle.Y = (float)sin(angle);
angles.push_back(newAngle);
step += 1;
angle = stepSize * step;
}
if (startAngle > angles[0].angle)
{
Angle newAngle;
intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)cos(startAngle), (float)sin(startAngle));
newAngle.angle = startAngle;
newAngle.X = iX;
newAngle.Y = iY;
angles[0] = newAngle;
}
int index = angles.size() - 1;
if (stopAngle < angles[index].angle)
{
Angle newAngle;
intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)cos(stopAngle), (float)sin(stopAngle));
newAngle.angle = stopAngle;
newAngle.X = iX;
newAngle.Y = iY;
angles[index] = newAngle;
}
}
}