static int
polySnip(const Vert2_list *contour, int u, int v, int w, int n, int *V)
{
int p;
double Ax, Ay, Bx, By, Cx, Cy, Px, Py;
Ax = contour->v[V[u]].mX;
Ay = contour->v[V[u]].mY;
Bx = contour->v[V[v]].mX;
By = contour->v[V[v]].mY;
Cx = contour->v[V[w]].mX;
Cy = contour->v[V[w]].mY;
if ( EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false;
for (p=0;p<n;p++)
{
if( (p == u) | (p == v) | (p == w) ) continue;
Px = contour->v[V[p]].mX;
Py = contour->v[V[p]].mY;
if (insideTriangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py)) return false;
}
return true;
}
bool EyeCalibration::snip(CalibrationPointVector &contour,int u,int v,int w,int n,int *V) {
int p;
CalibrationPoint A, B, C, P;
A = contour[V[u]];
B = contour[V[v]];
C = contour[V[w]];
if ( EPSILON > (((B.x()-A.x())*(C.y()-A.y())) - ((B.y()-A.y())*(C.x()-A.x()))) ) return false;
for (p=0;p<n;p++)
{
if( (p == u) || (p == v) || (p == w) ) continue;
if (insideTriangle(A,B,C,contour[V[p]])) return false;
}
return true;
}
void triangle(Vec3i t0, Vec3i t1, Vec3i t2, Vec2i uv0, Vec2i uv1, Vec2i uv2,
float intensity, int* zbuf, TGAImage& image) {
if (isDegenerated(t0,t1,t2)) return;
if (t0.y > t1.y) {
std::swap(t0, t1);
std::swap(uv0, uv1);
}
if (t0.y > t2.y) {
std::swap(t0, t2);
std::swap(uv0, uv2);
}
if (t1.y > t2.y) {
std::swap(t1, t2);
std::swap(uv1, uv2);
}
int* boundbox = getBoundBox(t0, t1, t2);
Vec2i vertex(t1.x - t0.x, t1.y - t0.y);
Vec2i tmpUv(uv1.x - uv0.x, uv1.y - uv0.y);
for (int x = boundbox[0]; x <= boundbox[2]; x++) {
for (int y = boundbox[1]; y <= boundbox[3]; y++) {
if (insideTriangle(x, y, t0, t1, t2)) {
int idx = x + y * width;
int z = find_z(x, y, t0, t1, t2);
Vec2i P(x, y), v(t0.x, t0.y);
Vec2i s01(t1.x - t0.x, t1.y - t0.y), s02(t2.x - t0.x, t2.y - t0.y), sp0(t0.x - P.x, t0.y - P.y);
Vec3i tmp1(s01.x, s02.x, sp0.x), tmp2(s01.y, s02.y, sp0.y);
Vec3i tmp3 = tmp1 ^ tmp2;
Vec3f res(tmp3.x, tmp3.y, tmp3.z);
if (res.z != 0) {
res = res * (1 / res.z);
} else {
continue;
}
Vec2f uvP = uv0 * (1 - res.x - res.y) + uv1 * res.x + uv2 * res.y;
if (zbuf[idx] < z) {
zbuf[idx] = z;
TGAColor color = model->getDiffusive(uvP);
image.set(x, y, TGAColor(color.r * intensity , color.g * intensity , color.b * intensity , 255));
}
}
}
}
delete[] boundbox;
}
bool TGen::Engine::EarClippingTriangulator::snip(int u, int v, int w, int n, int * V) {
const TGen::Vector3 & A = points[(V[u])];
const TGen::Vector3 & B = points[(V[v])];
const TGen::Vector3 & C = points[(V[w])];
if (std::numeric_limits<float>::epsilon() > abs(((B.x - A.x) * (C.y - A.y)) - ((B.y - A.y) * (C.x - A.x))) )
return true;
for (int p = 0; p < n; p++) {
if ((p == u) || (p == v) || (p == w))
continue;
const TGen::Vector3 & P = points[(V[p])];
if (insideTriangle(A, B, C, P))
return false;
}
return true;
}
bool Intersect::checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
FCL_REAL t)
{
return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t);
}
bool EyeCalibration::calibrate() {
char buf[512];
// Print starting message
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Starting Calibration\n");
// Create buffer
if (this->eyeVectorArray == NULL)
this->eyeVectorArray = (float*) malloc(sizeof(float) * ClientHandler::getDikablisViewingSize());
sprintf_s(buf, "Creating buffer of size %d bytes\n", sizeof(float) * ClientHandler::getDikablisViewingSize());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
// Print the number of points used in the calibration
sprintf_s(buf, "A total of %d points was collected.\n", calibrationPoints.size());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
if (calibrationPoints.size() < 4) {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Not enough points to calculate calibration.\n");
return false;
}
// Min/Max coordinates of the polygon
CalibrationPoint
minX = calibrationPoints.at(0),
minY = calibrationPoints.at(0),
maxX = calibrationPoints.at(0),
maxY = calibrationPoints.at(0);
// Print each points information and get the min and max
for (unsigned int i = 0; i < calibrationPoints.size(); i++) {
CalibrationPoint point = calibrationPoints.at(i);
osg::Vec3 ray = point.ray();
// Print info
sprintf_s(buf, "Point %d (%d, %d) has a value of (%f, %f, %f)\n",
i + 1,
point.x(), point.y(),
ray.x(), ray.y(), ray.z());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
if (minX.x() > point.x())
minX = point;
if (minY.y() > point.y())
minY = point;
if (maxX.x() < point.x())
maxX = point;
if (maxY.y() < point.y())
maxY = point;
}
// Print Bounding Info
{
sprintf_s(buf, "Point with min X value (%d, %d)\n",
minX.x(), minX.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
sprintf_s(buf, "Point with min Y value (%d, %d)\n",
minY.x(), minY.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
sprintf_s(buf, "Point with max X value (%d, %d)\n",
maxX.x(), maxX.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
sprintf_s(buf, "Point with max Y value (%d, %d)\n",
maxY.x(), maxY.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
// Get center
this->center_x = (maxX.x() - minX.x())/2;
this->center_y = (maxY.y() - minY.y())/2;
sprintf_s(buf, "Center point is (%d, %d)\n",
center_x, center_y);
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
}
// Get Convex Hull
CalibrationPointVector boundingPoints;
SegmentVector convexHull = calculateConvexHull(calibrationPoints);
// Get the bounding points of the convex hull
boundingPointsOfHull(convexHull, boundingPoints);
// Print each segment information
for (unsigned int i = 0; i < convexHull.size(); i++) {
Segment segment = convexHull.at(i);
sprintf_s(buf, "Edge %d: From (%d, %d) to (%d, %d)\n",
i + 1,
segment.x1(), segment.y1(),
segment.x2(), segment.y2());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
}
// Resort the bounding points
sort(boundingPoints, center_x, center_y);
// Triangulate
CalibrationPointVector traingles;
triangulate(boundingPoints, traingles);
// Check if the triangulation is valid
if (traingles.size() % 3 != 0) {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Error: Triangulating polygon.\n");
return false;
}
// Get the remaining inner points of the convex hull
CalibrationPointVector innerPoints;
for (unsigned int i = 0; i < calibrationPoints.size(); i++) {
CalibrationPoint point = calibrationPoints[i];
bool found = false;
for (unsigned int k = 0; k < boundingPoints.size(); k++) {
if (point.equal(boundingPoints[k])) {
found = true;
break;
}
}
if (!found)
innerPoints.push_back(point);
}
unsigned int traingleCount = traingles.size() / 3;
for (unsigned int i = 0; i < innerPoints.size(); i++) {
CalibrationPoint P = innerPoints[i];
for (unsigned int k = 0; k < traingleCount; k++) {
CalibrationPoint A = traingles[3*k + 0];
CalibrationPoint B = traingles[3*k + 1];
CalibrationPoint C = traingles[3*k + 2];
if (insideTriangle(A,B,C,P)) {
traingles.erase(traingles.begin()+3*k, traingles.begin()+3*k+3);
CalibrationPoint center, P0;
// Triangle 1
CalibrationPointVector tri1;
tri1.push_back(A);
tri1.push_back(B);
tri1.push_back(P);
P0 = A + (B - A)/2.f;
center = P + (P0 - P)*(3.f/2.f);
sort(tri1, center.x(), center.y());
// Triangle 2
CalibrationPointVector tri2;
tri2.push_back(A);
tri2.push_back(C);
tri2.push_back(P);
P0 = A + (C - A)/2.f;
center = P + (P0 - P)*(3.f/2.f);
sort(tri2, center.x(), center.y());
// Triangle 3
CalibrationPointVector tri3;
tri3.push_back(B);
tri3.push_back(C);
tri3.push_back(P);
P0 = B + (C - B)/2.f;
center = P + (P0 - P)*(3.f/2.f);
sort(tri3, center.x(), center.y());
traingles.insert(traingles.end(), tri1.begin(), tri1.end());
traingles.insert(traingles.end(), tri2.begin(), tri2.end());
traingles.insert(traingles.end(), tri3.begin(), tri3.end());
break;
}
}
}
if (traingles.size() % 3 != 0) {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Error: Triangulating inner points in polygon.\n");
return false;
}
traingleCount = traingles.size() / 3;
// Flip edges of the triangles
for (int k = traingleCount - 1; k > 0 ; k--) {
CalibrationPointVector tri1;
tri1.push_back(traingles[3*k + 0]);
tri1.push_back(traingles[3*k + 1]);
tri1.push_back(traingles[3*k + 2]);
for (int i = k - 1; i >= 0; i--) {
CalibrationPointVector tri2;
tri2.push_back(traingles[3*i + 0]);
tri2.push_back(traingles[3*i + 1]);
tri2.push_back(traingles[3*i + 2]);
// Get shared edge if exists
int shared = 0;
CalibrationPointVector sharedPoints;
CalibrationPoint nonSharedPoint;
for (unsigned int idx1 = 0; idx1 < 3; idx1++) {
for (unsigned int idx2 = 0; idx2 < 3; idx2++) {
if (tri2[idx2].equal(tri1[idx1])) {
sharedPoints.push_back(tri1[idx1]);
shared++;
} else {
nonSharedPoint = tri2[idx2];
}
}
}
if (shared == 2) {
if (isInCircumCircle(nonSharedPoint, tri1)) {
sprintf_s(buf, "Point %d - (%d, %d): In circle of triangle (%d, %d), (%d, %d), (%d, %d)\n",
i + 1, nonSharedPoint.x(), nonSharedPoint.y(),
tri1[0].x(), tri1[0].y(),
tri1[1].x(), tri1[1].y(),
tri1[2].x(), tri1[2].y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
// Sort tri1
for (unsigned int idx = 0; idx < 3; idx++) {
if (tri1[idx].equal(sharedPoints[0]) || tri1[idx].equal(sharedPoints[1])) {
CalibrationPoint shared = tri1[idx];
tri1.erase(tri1.begin()+idx);
tri1.insert(tri1.begin(), shared);
}
}
// Sort tri2
for (unsigned int idx = 0; idx < 3; idx++) {
if (tri2[idx].equal(sharedPoints[0]) || tri2[idx].equal(sharedPoints[1])) {
CalibrationPoint shared = tri2[idx];
tri2.erase(tri2.begin()+idx);
tri2.insert(tri2.begin(), shared);
}
}
// Clean up old triangles
traingles.erase(traingles.begin()+3*k, traingles.begin()+3*k+3);
traingles.erase(traingles.begin()+3*i, traingles.begin()+3*i+3);
CalibrationPoint center, P0;
// New Triangle 1
CalibrationPointVector new_tri1;
new_tri1.push_back(tri1[0]);
new_tri1.push_back(tri1[2]);
new_tri1.push_back(tri2[2]);
P0 = tri1[0] + (tri2[2] - tri1[0])/2.f;
center = tri1[2] + (P0 - tri1[2])*(3.f/2.f);
sort(new_tri1, center.x(), center.y());
// New Triangle 2
CalibrationPointVector new_tri2;
new_tri2.push_back(tri1[1]);
new_tri2.push_back(tri1[2]);
new_tri2.push_back(tri2[2]);
P0 = tri1[1] + (tri2[2] - tri1[1])/2.f;
center = tri1[2] + (P0 - tri1[2])*(3.f/2.f);
sort(new_tri2, center.x(), center.y());
traingles.insert(traingles.end(), new_tri1.begin(), new_tri1.end());
traingles.insert(traingles.end(), new_tri2.begin(), new_tri2.end());
break;
}
}
}
}
// Draw triangles
for (unsigned int j = 0; j < (viewingHeight+2*viewingMargin); j++) {
for (unsigned int i = 0; i < (viewingWidth+2*viewingMargin); i++) {
CalibrationPoint P(i - viewingMargin, j - viewingMargin, osg::Vec3(0.f, 0.f, 0.f));
for (unsigned int k = 0; k < traingleCount; k++) {
CalibrationPoint A = traingles[3*k + 0];
CalibrationPoint B = traingles[3*k + 1];
CalibrationPoint C = traingles[3*k + 2];
if (insideTriangle(A,B,C,P)) {
osg::Vec3 ray;
float det = A.x()*B.y()-B.x()*A.y()+B.x()*C.y()-C.x()*B.y()+C.x()*A.y()-A.x()*C.y();
osg::Vec3 a = (A.ray()*(B.y()-C.y())+B.ray()*(C.y()-A.y())+C.ray()*(A.y()-B.y())) / det;
osg::Vec3 b = (A.ray()*(C.x()-B.x())+B.ray()*(A.x()-C.x())+C.ray()*(B.x()-A.x())) / det;
osg::Vec3 c = (A.ray()*(B.x()*C.y()-C.x()*B.y())+B.ray()*(C.x()*A.y()-A.x()*C.y())+C.ray()*(A.x()*B.y()-B.x()*A.y())) / det;
ray = a*P.x()+b*P.y()+c;
unsigned long location = 3*(j*(viewingWidth+2*viewingMargin) + i);
this->eyeVectorArray[location + 0] = ray.x();
this->eyeVectorArray[location + 1] = ray.y();
this->eyeVectorArray[location + 2] = ray.z();
break;
}
}
}
}
// Draw Outside
for (unsigned int j = 0; j < (viewingHeight+2*viewingMargin); j++) {
for (unsigned int i = 0; i < (viewingWidth+2*viewingMargin); i++) {
CalibrationPoint P(i - viewingMargin, j - viewingMargin, osg::Vec3(0.f, 0.f, 0.f));
bool inside = false;
for (unsigned int k = 0; k < traingleCount; k++) {
CalibrationPoint A = traingles[3*k + 0];
CalibrationPoint B = traingles[3*k + 1];
CalibrationPoint C = traingles[3*k + 2];
if (insideTriangle(A,B,C,P)) {
inside = true;
break;
}
}
if (!inside) {
CalibrationPoint closest;
float min_distance = -1;
for (unsigned int k = 0; k < convexHull.size(); k++) {
CalibrationPoint A = convexHull[k].p1();
CalibrationPoint B = convexHull[k].p2();
CalibrationPoint P0 = getClosestPoint(A, B, P, true);
float diff_x = P0.x() - P.x();
float diff_y = P0.y() - P.y();
float distance = diff_x*diff_x + diff_y*diff_y;
if (min_distance < 0 || min_distance > distance) {
min_distance = distance;
closest = P0;
}
}
osg::Vec3 ray = closest.ray();
unsigned long location = 3*(j*(viewingWidth+2*viewingMargin) + i);
this->eyeVectorArray[location + 0] = ray.x();
this->eyeVectorArray[location + 1] = ray.y();
this->eyeVectorArray[location + 2] = ray.z();
}
}
}
// Testing
#if 0
// Draw points
for (unsigned int c = 0; c < calibrationPoints.size(); c++) {
CalibrationPoint point = calibrationPoints[c];
for (int j = -10; j < 10; j++) {
for (int i = -10; i < 10; i++) {
int x = point.x() + i + viewingMargin;
int y = point.y() + j + viewingMargin;
if (x >= 0 && x < (int)(viewingWidth+2*viewingMargin) &&
y >= 0 && y < (int)(viewingHeight+2*viewingMargin)) {
unsigned long location = 3*(y*(viewingWidth+2*viewingMargin) + x);
this->eyeVectorArray[location + 0] = 255;
this->eyeVectorArray[location + 1] = 0;
this->eyeVectorArray[location + 2] = 0;
}
}
}
}
// Draw bounding points
for (unsigned int c = 0; c < boundingPoints.size(); c++) {
CalibrationPoint point = boundingPoints[c];
for (int j = -10; j < 10; j++) {
for (int i = -10; i < 10; i++) {
int x = point.x() + i + viewingMargin;
int y = point.y() + j + viewingMargin;
if (x >= 0 && x < (int)(viewingWidth+2*viewingMargin) &&
y >= 0 && y < (int)(viewingHeight+2*viewingMargin)) {
unsigned long location = 3*(y*(viewingWidth+2*viewingMargin) + x);
this->eyeVectorArray[location + 0] = 0;
this->eyeVectorArray[location + 1] = 255;
this->eyeVectorArray[location + 2] = 0;
}
}
}
}
#endif
saveRayMap();
ClientHandler* client = AppData::getInstance()->getClient();
if (client) {
client->setRayCalibration(this->eyeVectorArray);
client->setHeadId(this->rbHeadId);
} else {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Failed: Unable to find client.\n");
return false;
}
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Done\n");
return true;
}
