Point CollisionDetection::projectLineToEdge(Point _c, Point _p1, Point _p2)
{
float boundaryMinX = _c.x - screenWidth / 2;
float boundaryMaxX = _c.x + screenWidth / 2;
float boundaryMinY = _c.y - screenHeight / 2;
float boundaryMaxY = _c.y + screenHeight / 2;
Point temp1;
Point temp2;
temp1 = { boundaryMinX, boundaryMinY };
temp2 = { boundaryMaxX, boundaryMinY };
Point p1 = getLineIntersect(_p1, _p2, temp1, temp2);
temp1 = { boundaryMaxX, boundaryMinY };
temp2 = { boundaryMaxX, boundaryMaxY };
Point p2 = getLineIntersect(_p1, _p2, temp1, temp2);
temp1 = { boundaryMaxX, boundaryMaxY };
temp2 = { boundaryMinX, boundaryMaxY };
Point p3 = getLineIntersect(_p1, _p2, temp1, temp2);
temp1 = { boundaryMinX, boundaryMinY };
temp2 = { boundaryMinX, boundaryMaxY };
Point p4 = getLineIntersect(_p1, _p2, temp1, temp2);
vector<Point> temppoints;
if (_p2.x >= _p1.x)
{
if (_p2.y >= _p1.y)
{
temppoints.push_back(p2);
temppoints.push_back(p3);
return getClosestPoint(_p1, temppoints);
}
if (_p2.y < _p1.y)
{
temppoints.push_back(p1);
temppoints.push_back(p2);
return getClosestPoint(_p1, temppoints);
}
}
if (_p2.x < _p1.x)
{
if (_p2.y >= _p1.y)
{
temppoints.push_back(p3);
temppoints.push_back(p4);
return getClosestPoint(_p1, temppoints);
}
if (_p2.y < _p1.y)
{
temppoints.push_back(p4);
temppoints.push_back(p1);
return getClosestPoint(_p1, temppoints);
}
}
}
void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint, const ESM::Pathgrid::Point &endPoint,
const MWWorld::CellStore* cell, bool allowShortcuts)
{
mPath.clear();
if(mCell != cell) mIsGraphConstructed = false;
mCell = cell;
if(allowShortcuts)
{
if(MWBase::Environment::get().getWorld()->castRay(startPoint.mX, startPoint.mY, startPoint.mZ,
endPoint.mX, endPoint.mY, endPoint.mZ))
allowShortcuts = false;
}
if(!allowShortcuts)
{
const ESM::Pathgrid *pathGrid =
MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->mCell);
float xCell = 0;
float yCell = 0;
if (mCell->isExterior())
{
xCell = mCell->mCell->mData.mX * ESM::Land::REAL_SIZE;
yCell = mCell->mCell->mData.mY * ESM::Land::REAL_SIZE;
}
int startNode = getClosestPoint(pathGrid, startPoint.mX - xCell, startPoint.mY - yCell,startPoint.mZ);
int endNode = getClosestPoint(pathGrid, endPoint.mX - xCell, endPoint.mY - yCell, endPoint.mZ);
if(startNode != -1 && endNode != -1)
{
if(!mIsGraphConstructed) buildPathgridGraph(pathGrid);
mPath = aStarSearch(pathGrid,startNode,endNode,xCell,yCell);//findPath(startNode, endNode, mGraph);
if(!mPath.empty())
{
mPath.push_back(endPoint);
mIsPathConstructed = true;
}
}
}
else
{
mPath.push_back(endPoint);
mIsPathConstructed = true;
}
if(mPath.empty())
mIsPathConstructed = false;
}
void PathFinder::buildPath(ESM::Pathgrid::Point startPoint,ESM::Pathgrid::Point endPoint,
const ESM::Pathgrid* pathGrid,float xCell,float yCell)
{
int start = getClosestPoint(pathGrid,startPoint.mX - xCell,startPoint.mY - yCell,startPoint.mZ);
int end = getClosestPoint(pathGrid,endPoint.mX - xCell,endPoint.mY - yCell,endPoint.mZ);
if(start != -1 && end != -1)
{
PathGridGraph graph = buildGraph(pathGrid,xCell,yCell);
mPath = findPath(start,end,graph);
}
mPath.push_back(endPoint);
mIsPathConstructed = true;
}
bool insideSurface(const Point3D &p, const Surface &s, const SpacialHash &faceHash, const SpacialHash &faceHashHiRes, ClosestPointInfo *inf, float stopBelow, bool allowQuickTest){
if (inf){
inf->type = DIST_TYPE_INVALID;
inf->pClose.x = inf->pClose.y = inf->pClose.z = acos(10);
}
// using crossings test
bool in = insideSurfaceCrossingIter(p, s, faceHashHiRes);
/*
bool inGem = insideSurfaceGem(p, s);
if (inGem != in){
OUTPUTINFO("Point classified differently %d %d\n", in, inGem);
ClosestPointInfo cp;
getClosestPoint(&cp, p, s, faceHash);
float d = p.distance(cp.pClose);
OUTPUTINFO("\tD = %f\n", d);
}*/
// using closest point test
//bool in1 = insideSurfaceClosest(p, s, faceHash, inf, stopBelow, allowQuickTest);
// may need to get the closest point
if (!allowQuickTest && inf && inf->type == DIST_TYPE_INVALID)
getClosestPoint(inf, p, s, faceHash);
return in;
}
MetalineEnemy::Vertex MetalineEnemy::getClosestPoint(VertexWithFieldStrength v, LineBlob b)
{
Vertex temp;
temp.x = v.x;
temp.y = v.y;
temp.z = v.z;
return getClosestPoint(temp, b);
}
void CollisionFace::SphereCollision(const VC3 &position, float radius, Storm3D_CollisionInfo &colinfo, bool accurate)
{
VC3 closest;
getClosestPoint(position, vertex0, vertex1, vertex2, closest);
float sqRange = closest.GetSquareRangeTo(position);
if(sqRange < radius * radius && sqRange < colinfo.range * colinfo.range)
{
colinfo.hit = true;
colinfo.range = sqrtf(sqRange);
colinfo.position = closest;
colinfo.plane_normal = plane.planenormal;
colinfo.inside_amount = radius - colinfo.range;
float planeRange = plane.GetPointRange(position);
if(planeRange < 0)
colinfo.plane_normal = -colinfo.plane_normal;
}
}
float MetalineEnemy::calculateFieldStrength(VertexWithFieldStrength v)
{
float fieldStrength = 0;
for (int i = 0; i < numBlobs; i++)
{
Vertex closest = getClosestPoint(v, blobs[i]);
float distanceSquared = (closest.x - v.x) * (closest.x - v.x) +
(closest.y - v.y) * (closest.y - v.y) +
(closest.z - v.z) * (closest.z - v.z);
if (distanceSquared < fieldStrengthDistanceSquaredThreshold)
{
fieldStrength += (fieldStrengthConstantSquaredSquared * distanceSquared * distanceSquared) -
(fieldStrengthConstantSquared * distanceSquared) + 0.25f;
}
}
return fieldStrength;
}
void main()
{
vec3 pos = texture2D( tPositions, vUv ).xyz;
vec3 vel = texture2D( tVelocities, vUv).xyz;
vec3 closestPoint = getClosestPoint(pos).xyz;
vec3 palantirForce = getPullToBottom(pos, closestPoint).xyz;
if (onLogo(pos, closestPoint)) {
vel.y -= multiplier * 0.05 + rand(pos.xz) * 0.05;
vel.z += closestPoint.z * multiplier * 0.3;
vel.y += randomNum * 2.0 ;
} else {
vel.y += palantirForce.y * 1.0 ;
vel.z += palantirForce.z * 1.0 ;
}
vel.x *= 0.93;
vel.y *= 0.93;
vel.z *= 0.93;
// Write new position out
gl_FragColor = vec4(vel, 1.0);
}
MetalineEnemy::Vertex MetalineEnemy::getNormal(Vertex v)
{
Vertex normal;
normal.x = 0.f;
normal.y = 0.f;
normal.z = 0.f;
for (int i = 0; i < numBlobs; i++)
{
/*float xDif = blobs[i].center.x - v.x;
float yDif = blobs[i].center.y - v.y;
float zDif = blobs[i].center.z - v.z;*/
LineBlob curLineBlob = blobs[i];
Vertex closest = getClosestPoint(v, blobs[i]);
float xDif = closest.x - v.x;
float yDif = closest.y - v.y;
float zDif = closest.z - v.z;
float distanceSquared = (xDif * xDif) + (yDif * yDif) + (zDif * zDif);
if ((fieldStrengthConstantSquared * distanceSquared) < 0.5f)
{
float result = 4 * fieldStrengthConstantSquared *
(fieldStrengthConstantSquared * distanceSquared - 0.5f);
normal.x += xDif * result;
normal.y += yDif * result;
normal.z += zDif * result;
}
}
return normalize(normal);
}
Point CollisionDetection::getClosestTarget(Point _p1, Point _p2)
{
_p2.x = floor(_p2.x);
_p2.y = floor(_p2.y);
_p1.x = floor(_p1.x);
_p1.y = floor(_p1.y);
vector<Point> temppoints;
for (int i = 0; i < Rects.size(); i++)
{
if (Rects[i]->getType() != player)
{
if (checkRectLineIntersect(Rects[i]->getPosition(), Rects[i]->getWidth(), Rects[i]->getHeight(), _p2, _p1))
{
Point test = getClosestRectLineIntersect(Rects[i]->getPosition(), Rects[i]->getWidth(), Rects[i]->getHeight(), _p2, _p1);
temppoints.push_back(test);
}
}
}
for (int i = 0; i < Rects.size(); i++)
{
if (Rects[i]->getType() != player)
{
if (checkRectLineIntersect(_p1, Rects[i]->getWidth(), Rects[i]->getHeight(), _p2, _p1))
{
if (temppoints.size() > 0 && getClosestPoint(_p1, temppoints).x != 0 && getClosestPoint(_p1, temppoints).y != 0)
{
if (getClosestPoint(_p1, temppoints).x < 30 && getClosestPoint(_p1, temppoints).y < 30)
{
return getClosestPoint(_p1, temppoints);
}
return getClosestPoint(_p1, temppoints);
}
else
{
return projectLineToEdge(Point{ cameraPos.x - 10, cameraPos.y - 10 }, _p1, _p2);
}
}
}
}
return Point{ 1000, 1000 };
}
/*
* NOTE: This method may fail to find a path. The caller must check the
* result before using it. If there is no path the AI routies need to
* implement some other heuristics to reach the target.
*
* NOTE: It may be desirable to simply go directly to the endPoint if for
* example there are no pathgrids in this cell.
*
* NOTE: startPoint & endPoint are in world co-ordinates
*
* Updates mPath using aStarSearch() or ray test (if shortcut allowed).
* mPath consists of pathgrid points, except the last element which is
* endPoint. This may be useful where the endPoint is not on a pathgrid
* point (e.g. combat). However, if the caller has already chosen a
* pathgrid point (e.g. wander) then it may be worth while to call
* pop_back() to remove the redundant entry.
*
* NOTE: co-ordinates must be converted prior to calling getClosestPoint()
*
* |
* | cell
* | +-----------+
* | | |
* | | |
* | | @ |
* | i | j |
* |<--->|<---->| |
* | +-----------+
* | k
* |<---------->| world
* +-----------------------------
*
* i = x value of cell itself (multiply by ESM::Land::REAL_SIZE to convert)
* j = @.x in local co-ordinates (i.e. within the cell)
* k = @.x in world co-ordinates
*/
void PathFinder::buildPath(const ESM::Pathgrid::Point &startPoint,
const ESM::Pathgrid::Point &endPoint,
const MWWorld::CellStore* cell,
bool allowShortcuts)
{
mPath.clear();
if(allowShortcuts)
{
// if there's a ray cast hit, can't take a direct path
if (!MWBase::Environment::get().getWorld()->castRay(
static_cast<float>(startPoint.mX), static_cast<float>(startPoint.mY), static_cast<float>(startPoint.mZ),
static_cast<float>(endPoint.mX), static_cast<float>(endPoint.mY), static_cast<float>(endPoint.mZ)))
{
mPath.push_back(endPoint);
return;
}
}
if(mCell != cell || !mPathgrid)
{
mCell = cell;
mPathgrid = MWBase::Environment::get().getWorld()->getStore().get<ESM::Pathgrid>().search(*mCell->getCell());
}
// Refer to AiWander reseach topic on openmw forums for some background.
// Maybe there is no pathgrid for this cell. Just go to destination and let
// physics take care of any blockages.
if(!mPathgrid || mPathgrid->mPoints.empty())
{
mPath.push_back(endPoint);
return;
}
// NOTE: getClosestPoint expects local co-ordinates
CoordinateConverter converter(mCell->getCell());
// NOTE: It is possible that getClosestPoint returns a pathgrind point index
// that is unreachable in some situations. e.g. actor is standing
// outside an area enclosed by walls, but there is a pathgrid
// point right behind the wall that is closer than any pathgrid
// point outside the wall
osg::Vec3f startPointInLocalCoords(converter.ToLocalVec3(startPoint));
int startNode = getClosestPoint(mPathgrid, startPointInLocalCoords);
osg::Vec3f endPointInLocalCoords(converter.ToLocalVec3(endPoint));
std::pair<int, bool> endNode = getClosestReachablePoint(mPathgrid, cell,
endPointInLocalCoords,
startNode);
// AiWander has logic that depends on whether a path was created,
// deleting allowed nodes if not. Hence a path needs to be created
// even if the start and the end points are the same.
// NOTE: aStarSearch will return an empty path if the start and end
// nodes are the same
if(startNode == endNode.first)
{
ESM::Pathgrid::Point temp(mPathgrid->mPoints[startNode]);
converter.ToWorld(temp);
mPath.push_back(temp);
mPath.push_back(endPoint);
return;
}
mPath = mCell->aStarSearch(startNode, endNode.first);
assert(!mPath.empty());
// convert supplied path to world co-ordinates
for (std::list<ESM::Pathgrid::Point>::iterator iter(mPath.begin()); iter != mPath.end(); ++iter)
{
converter.ToWorld(*iter);
}
// If endNode found is NOT the closest PathGrid point to the endPoint,
// assume endPoint is not reachable from endNode. In which case,
// path ends at endNode.
//
// So only add the destination (which may be different to the closest
// pathgrid point) when endNode was the closest point to endPoint.
//
// This logic can fail in the opposite situate, e.g. endPoint may
// have been reachable but happened to be very close to an
// unreachable pathgrid point.
//
// The AI routines will have to deal with such situations.
if(endNode.second)
mPath.push_back(endPoint);
return;
}
void ofxMapaMok::draw(ofTexture *_texture){
if (_texture != NULL)
if ((_texture->getWidth() != textWidth ) ||
(_texture->getHeight() != textHeight) )
ofLog(OF_LOG_WARNING, "The applied texture have a diferent size of the one spected");
if( setupMode == SETUP_SELECT ) {
// Init easyCam
//
cam.begin( (ofRectangle)*this );
// Reference
//
if(refMode == REFERENCE_AXIS)
ofDrawAxis(100);
else if (refMode == REFERENCE_GRID)
ofDrawGrid(100);
// Scale
//
ofScale(scale, scale, scale);
// Render
//
render(_texture);
// Update dots positions
//
if( setupMode ) {
imageMesh = getProjectedMesh(objectMesh);
}
cam.end();
// On any tipe of setup mode
//
if( setupMode ) {
ofPushStyle();
// Draw all points cyan small
//
ofSetColor( ofxCv::cyanPrint );
for(int i=0; i< imageMesh.getVertices().size(); i++){
if( inside(ofVec2f(imageMesh.getVertex(i).x,imageMesh.getVertex(i).y))){
ofCircle(imageMesh.getVertex(i).x, imageMesh.getVertex(i).y, 2);
}
}
// Draw all reference points cyan
//
int n = referencePoints.size();
for(int i = 0; i < n; i++) {
if(referencePoints[i]) {
drawLabeledPoint(i, imageMesh.getVertex(i), ofxCv::cyanPrint );
}
}
// Check to see if anything is selected
// Draw hover point magenta
//
int choice;
float distance;
if (imageMesh.getNumVertices())
{
ofVec3f selected = getClosestPointOnMesh(imageMesh, ofGetAppPtr()->mouseX, ofGetAppPtr()->mouseY, &choice, &distance);
if(!ofGetMousePressed() && distance < selectionRadius) {
hoverChoice = choice;
hoverSelected = true;
drawLabeledPoint(choice, selected, ofxCv::magentaPrint);
} else {
hoverSelected = false;
}
}
else
{
hoverSelected = false;
ofLogError() << "Mesh is empty";
}
// Draw selected point yellow
//
if( selectedVert ) {
int choice = selectionChoice;
ofVec2f selected = imageMesh.getVertex(choice);
drawLabeledPoint(choice, selected, ofxCv::yellowPrint, ofColor::white, ofColor::black);
}
ofPopStyle();
}
} else {
if ( calibrationReady ) {
begin(near, far);
render(_texture);
if(setupMode) {
imageMesh = getProjectedMesh(objectMesh);
}
end();
} else {
ofDrawBitmapString("CALIBRATION NOT READY", x+width*0.5-80, y+height*0.5 );
ofDrawBitmapString("(you need to set more dots)", x+width*0.5-100, y+height*0.5 + 15);
}
if( setupMode ) {
ofPushStyle();
// draw all reference points cyan
//
int n = referencePoints.size();
for(int i = 0; i < n; i++) {
if(referencePoints[i]) {
if(i == selectionChoice){
drawLabeledPoint(i, ofxCv::toOf(imagePoints[i]), ofxCv::yellowPrint, ofColor::white, ofColor::black);
}else{
drawLabeledPoint(i, ofxCv::toOf(imagePoints[i]), ofxCv::cyanPrint);
}
}
}
// move points that need to be dragged
// draw selected yellow
//
int choice = selectionChoice;
if(selectedVert) {
referencePoints[choice] = true;
cv::Point2f& cur = imagePoints[choice];
if(cur == cv::Point2f()) {
if(calibrationReady) {
cur = ofxCv::toCv(ofVec2f(imageMesh.getVertex(choice)));
} else {
cur = cv::Point2f(ofGetAppPtr()->mouseX, ofGetAppPtr()->mouseY);
}
}
}
if(dragging) {
cv::Point2f& cur = imagePoints[choice];
float rate = ofGetMousePressed(0) ? slowLerpRate : fastLerpRate;
cur = cv::Point2f(ofLerp(cur.x, ofGetAppPtr()->mouseX, rate), ofLerp(cur.y, ofGetAppPtr()->mouseY, rate));
drawLabeledPoint(choice, ofxCv::toOf(cur), ofxCv::yellowPrint, ofColor::white, ofColor::black);
ofSetColor(ofColor::black);
ofRect( ofxCv::toOf(cur), 1, 1);
} else if(arrowing) {
cv::Point2f& cur = imagePoints[choice];
drawLabeledPoint(choice, ofxCv::toOf(cur), ofxCv::yellowPrint, ofColor::white, ofColor::black);
ofSetColor(ofColor::black);
ofRect( ofxCv::toOf(cur), 1, 1);
} else {
// check to see if anything is selected
// draw hover magenta
float distance;
ofVec2f selected = ofxCv::toOf(getClosestPoint(imagePoints, ofGetAppPtr()->mouseX, ofGetAppPtr()->mouseY, &choice, &distance));
if(!ofGetMousePressed() && referencePoints[choice] && distance < selectionRadius) {
hoverChoice = choice;
hoverSelected = true;
drawLabeledPoint(choice, selected, ofxCv::magentaPrint);
} else {
hoverSelected = false;
}
}
ofPopStyle();
}
}
if( setupMode != SETUP_NONE ) {
if (inside(ofGetMouseX(), ofGetMouseY())){
ofPushStyle();
ofSetColor(255,100);
ofNoFill();
ofRect( (ofRectangle)*this );
if ( setupMode == SETUP_CALIBRATE)
ofDrawBitmapString("CALIBRATE the DOT", x+width*0.5-80,y+15);
else if ( setupMode == SETUP_SELECT ){
if (selectedVert)
ofDrawBitmapString("PRESS SPACE to CALIBRATE", x+width*0.5-100,y+15);
else
ofDrawBitmapString("CLICK ONE DOT", x+width*0.5-60,y+15);
}
ofPopStyle();
}
}
}
void Points::moveClosestPoint(float x, float y, float tox, float toy)
{
int pntnum = getClosestPoint(x,y);
pointlist[pntnum].setX(tox);
pointlist[pntnum].setY(toy);
}
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;
}
Real Line::distance(const Pnt3r &point) const
{
return (point - getClosestPoint(point)).length();
}
bool insideSurfaceClosest(const Point3D &pTest, const Surface &s, const SpacialHash &faceHash, ClosestPointInfo *inf, float stopBelow, bool allowQuickTest){
if (inf)
inf->type = DIST_TYPE_INVALID;
// quick bounding box test
if (allowQuickTest){
if (pTest.x < s.pMin.x || pTest.x > s.pMax.x ||
pTest.y < s.pMin.y || pTest.y > s.pMax.y ||
pTest.z < s.pMin.z || pTest.z > s.pMax.z){
return false;
}
}
ClosestPointInfo localClosestInf;
if (!inf)
inf = &localClosestInf;
float dist = getClosestPoint(inf, pTest, s, faceHash, stopBelow);
if (dist < stopBelow){
// optimise for dodec
return true;
}
// vector to point on surface
Vector3D v;
v.difference(pTest, inf->pClose);
v.norm();
if (inf->type == FACE){
// face test
Vector3D n;
s.getTriangleNormal(&n, inf->triangle);
double dt = n.dot(v);
return dt <= 0;
}
else if (inf->type == EDGE){
// edge test
const Surface::Triangle *tri = &s.triangles.index(inf->triangle);
// edge will be between vertices v[num] and v[(num+1)%3]
int e[2];
e[0] = tri->v[inf->num];
e[1] = tri->v[(inf->num+1)%3];
int neigh = findNeighbour(s, *tri, e);
if (neigh >= 0){
// make a plane for one of the triangles
Vector3D n1;
s.getTriangleNormal(&n1, inf->triangle);
Point3D p1 = s.vertices.index(e[0]).p;
Plane pl1;
pl1.assign(n1, p1);
// get the point from the other triangle which is not part of edge
const Surface::Triangle *tri2 = &s.triangles.index(neigh);
for (int i = 0; i < 3; i++){
if (tri2->v[i] != e[0] && tri2->v[i] != e[1])
break;
}
CHECK_DEBUG0(i != 3);
Point3D p2 = s.vertices.index(e[1]).p;
// get signed distance to plane
float dist = pl1.dist(p2);
// need normal for second triangle
Vector3D n2;
s.getTriangleNormal(&n2, neigh);
if (dist <= 0.0f){
// faces form convex spike, back facing to both
return v.dot(n1) <= 0 && v.dot(n2) <= 0;
}
else{
// faces form concavity, back facing to either
return v.dot(n1) <= 0 || v.dot(n2) <= 0;
}
}
else{
OUTPUTINFO("HHHHHMMMMMMM loose edge\n");
return false; // only one triangle on edge - use face ??
}
}
else{// if (minType == VERTEX)
// chosen triangle
const Surface::Triangle *tri = &s.triangles.index(inf->triangle);
// chosen vertex
int vI = tri->v[inf->num];
Vector3D n;
s.getVertexNormal(&n, vI);
return n.dot(v) <= 0;
/*
// get all faces
Array<int> tris;
s.findNeighbours(&tris, vI, inf->triangle);
// behind test for all faces
int numTri = tris.getSize();
for (int i = 0; i < numTri; i++){
Vector3D n;
s.getTriangleNormal(&n, tris.index(i));
double dt = n.dot(v);
if (dt > 0)
return false;
}
// must be behind all
return true;*/
}
}
