float Plane3f::distance( const Vector3f& p ) const
{
// pick a point x on the plane
// get the vector x --> p
// distance is the projection on the unit normal
// xp dot unitNormal
auto x = pointOnPlane();
auto xp = p - x;
return Vector3f::dot( xp, unitNormal() );
}
bool Form::projectPoint(int x, int y, Vector3* point)
{
Scene* scene = _node->getScene();
Camera* camera;
if (scene && (camera = scene->getActiveCamera()))
{
// Get info about the form's position.
Matrix m = _node->getWorldMatrix();
Vector3 pointOnPlane(0, 0, 0);
m.transformPoint(&pointOnPlane);
// Unproject point into world space.
Ray ray;
camera->pickRay(Game::getInstance()->getViewport(), x, y, &ray);
// Find the quad's plane. We know its normal is the quad's forward vector.
Vector3 normal = _node->getForwardVectorWorld().normalize();
// To get the plane's distance from the origin, we project a point on the
// plane onto the plane's normal vector.
const float distance = fabs(Vector3::dot(pointOnPlane, normal));
Plane plane(normal, -distance);
// Check for collision with plane.
float collisionDistance = ray.intersects(plane);
if (collisionDistance != Ray::INTERSECTS_NONE)
{
// Multiply the ray's direction vector by collision distance and add that to the ray's origin.
point->set(ray.getOrigin() + collisionDistance*ray.getDirection());
// Project this point into the plane.
m.invert();
m.transformPoint(point);
return true;
}
}
return false;
}
vector<Point3d> PlaneDetector::getPlanes(Mat& rgbImage,
PointCloud<PointXYZ>& pcl, vector<Vec4i>& lines) {
vector<Point3d> foundPlanes;
for (int i = 0; i < lines.size(); i++) {
Vec4i line = lines[i];
Point2i a = Point2i(line[0], line[1]);
Point2i b = Point2i(line[2], line[3]);
if (norm(b - a) < 100)
continue;
for (int j = 0; j < 2; j++) {
Point2i diff = b - a;
float len = cv::norm(diff);
Point2i mid1(a.x + 4 * diff.x / 10, a.y + 3 * diff.y / 10);
Point2i mid2(a.x + 5 * diff.x / 10, a.y + 5 * diff.y / 10);
Point2i mid3(a.x + 6 * diff.x / 10, a.y + 7 * diff.y / 10);
Point2i rotated;
if (j == 0)
rotated = Point2i(-diff.y, diff.x);
else
rotated = Point2i(diff.y, -diff.x);
Point2i check1(mid1.x + rotated.x / 5, mid1.y + rotated.y / 5);
Point2i check2(mid2.x + rotated.x / 2, mid2.y + rotated.y / 2);
Point2i check3(mid3.x + rotated.x / 5, mid3.y + rotated.y / 5);
Point3d d1 = depthCloseToPoint(check1, pcl, 10);
Point3d d2 = depthCloseToPoint(check2, pcl, 10);
Point3d d3 = depthCloseToPoint(check3, pcl, 10);
bool planeAlreadyFound = false;
for (int j = 0; j < foundPlanes.size(); j++) {
if (fabs(pointOnPlane(d1, foundPlanes[j])) < DIST_EPSILON
|| fabs(pointOnPlane(d2, foundPlanes[j])) < DIST_EPSILON
|| fabs(pointOnPlane(d3, foundPlanes[j]))
< DIST_EPSILON) {
planeAlreadyFound = true;
break;
}
}
if (planeAlreadyFound) {
cv::line(rgbImage, check1, check2, Scalar(0, 0, 255), 3, 8);
cv::line(rgbImage, check2, check3, Scalar(0, 0, 255), 3, 8);
cv::line(rgbImage, check3, check1, Scalar(0, 0, 255), 3, 8);
continue;
}
cv::line(rgbImage, check1, check2, Scalar(255, 0, 0), 3, 8);
cv::line(rgbImage, check2, check3, Scalar(255, 0, 0), 3, 8);
cv::line(rgbImage, check3, check1, Scalar(255, 0, 0), 3, 8);
if (d1.z != -1 && d2.z != -1 && d3.z != -1) {
cv::line(rgbImage, check1, check2, Scalar(255, 255, 255), 3, 8);
cv::line(rgbImage, check2, check3, Scalar(255, 255, 255), 3, 8);
cv::line(rgbImage, check3, check1, Scalar(255, 255, 255), 3, 8);
vector<Point3d> v;
v.push_back(d1);
v.push_back(d2);
v.push_back(d3);
Point3d plane = getPlane(v);
foundPlanes.push_back(plane);
}
}
}
return foundPlanes;
}
vector<Point3d> PlaneDetector::getPlanes(Mat& rgbImage,
PointCloud<PointXYZ>& pcl, vector<Vec4i>& lines) {
vector<Point3d> foundPlanes;
for (int i = 0; i < 2; i++) {
Vec4i line = lines[i];
Point2i a = Point2i(line[0], line[1]);
Point2i b = Point2i(line[2], line[3]);
if (norm(b - a) < 100)
continue;
for (int j = 0; j < 2; j++) {
Point2i diff = b - a;
float len = cv::norm(diff);
Point2i mid1(a.x + 4 * diff.x / 10, a.y + 3 * diff.y / 10);
Point2i mid2(a.x + 5 * diff.x / 10, a.y + 5 * diff.y / 10);
Point2i mid3(a.x + 6 * diff.x / 10, a.y + 7 * diff.y / 10);
Point2i rotated;
if (j == 0)
rotated = Point2i(-diff.y, diff.x);
else
rotated = Point2i(diff.y, -diff.x);
Point2i check1;
Point2i check2;
Point2i check3;
Point3d d1;
Point3d d2;
Point3d d3;
if (i==0){
check1.x=300;
check1.y=260;
check2.x=320;
check2.y=220;
check3.x=340;
check3.y=240;
d1 = depthCloseToPoint(check1, pcl, 10);
d2 = depthCloseToPoint(check2, pcl, 10);
d3 = depthCloseToPoint(check3, pcl, 10);
// Point2i check1(mid1.x + rotated.x / 5, mid1.y + rotated.y / 5);
// Point2i check2(mid2.x + rotated.x / 2, mid2.y + rotated.y / 2);
// Point2i check3(mid3.x + rotated.x / 5, mid3.y + rotated.y / 5);
// int pt_1_x = 300;
// int pt_2_x = 320;
// int pt_3_x = 340;
// int pt_1_y = 260;
// int pt_2_y = 220;
// int pt_3_y = 240;
} else {
check1.x=330;
check1.y=400;
check2.x=450;
check2.y=420;
check3.x=390;
check3.y=380;
// check1(400, 260);
// check2(420, 220);
// check3(380, 240);
d1 = depthCloseToPoint(check1, pcl, 10);
d2 = depthCloseToPoint(check2, pcl, 10);
d3 = depthCloseToPoint(check3, pcl, 10);
}
// Point3d d1 = depthCloseToPoint(check1, pcl, 10);
// Point3d d2 = depthCloseToPoint(check2, pcl, 10);
// Point3d d3 = depthCloseToPoint(check3, pcl, 10);
bool planeAlreadyFound = false;
for (int j = 0; j < foundPlanes.size(); j++) {
if (fabs(pointOnPlane(d1, foundPlanes[j])) < DIST_EPSILON
|| fabs(pointOnPlane(d2, foundPlanes[j])) < DIST_EPSILON
|| fabs(pointOnPlane(d3, foundPlanes[j]))
< DIST_EPSILON) {
planeAlreadyFound = true;
break;
}
}
if (planeAlreadyFound) {
cv::line(rgbImage, check1, check2, Scalar(0, 0, 255), 3, 8);
cv::line(rgbImage, check2, check3, Scalar(0, 0, 255), 3, 8);
cv::line(rgbImage, check3, check1, Scalar(0, 0, 255), 3, 8);
continue;
}
cv::line(rgbImage, check1, check2, Scalar(255, 0, 0), 3, 8);
cv::line(rgbImage, check2, check3, Scalar(255, 0, 0), 3, 8);
cv::line(rgbImage, check3, check1, Scalar(255, 0, 0), 3, 8);
if (d1.z != -1 && d2.z != -1 && d3.z != -1) {
printf(
"3dots: %.2f %.2f %.2f, %.2f %.2f %.2f, %.2f %.2f %.2f\n",
d1.x, d1.y, d1.z, d2.x, d2.y, d2.z, d3.x, d3.y, d3.z);
cv::line(rgbImage, check1, check2, Scalar(255, 255, 255), 3, 8);
cv::line(rgbImage, check2, check3, Scalar(255, 255, 255), 3, 8);
cv::line(rgbImage, check3, check1, Scalar(255, 255, 255), 3, 8);
vector<Point3d> v;
v.push_back(d1);
v.push_back(d2);
v.push_back(d3);
Point3d plane = getPlane(v);
printf("Plane: %.2f %.2f %.2f\n", plane.x, plane.y, plane.z);
foundPlanes.push_back(plane);
}
}
}
return foundPlanes;
}
bool Scene::setupObject(Model** object, XMLElement* objectElement) {
if(!attributesExist(objectElement, {"type"})) {
errorDescription = errorDescription + "Object type not defined\n";
return true;
}
std::string type = objectElement->Attribute("type");
if(type == "sphere") {
const XMLElement* sphereOrigin = objectElement->FirstChildElement("origin");
const XMLElement* sphereRadius = objectElement->FirstChildElement("radius");
if (!sphereOrigin) {
errorDescription = errorDescription + "Sphere origin not defined.\n";
return true;
}
if(!sphereRadius) {
errorDescription = errorDescription + "Sphere radius not defined.\n";
return true;
}
if(!attributesExist(sphereOrigin, {"x", "y", "z"})) {
errorDescription = errorDescription + "Sphere origin coordinates not valid.\n";
return true;
}
if(!attributesExist(sphereRadius, {"r"})) {
errorDescription = errorDescription + "Sphere radius not defined.\n";
return true;
}
*object = new Sphere(Point3D(sphereOrigin->FindAttribute("x")->FloatValue(),
sphereOrigin->FindAttribute("y")->FloatValue(),
sphereOrigin->FindAttribute("z")->FloatValue()),
sphereRadius->FindAttribute("r")->FloatValue());
} else if (type == "square" || type == "polygonalshape") {
std::vector<Point3D> vertexList;
for(XMLElement* v = objectElement->FirstChildElement("vertex");
v != NULL;
v = v->NextSiblingElement("vertex")) {
const XMLElement* vertexElement = v;
if(!attributesExist(vertexElement, {"x", "y", "z"})) {
errorDescription = errorDescription + "Vertex coordinates not valid.\n";
return true;
}
Point3D vertex(vertexElement->FindAttribute("x")->FloatValue(),
vertexElement->FindAttribute("y")->FloatValue(),
vertexElement->FindAttribute("z")->FloatValue());
vertexList.push_back(vertex);
}
const XMLElement* pointOnPlaneElement = objectElement->FirstChildElement("pointOnPlane");
if(!pointOnPlaneElement) {
errorDescription = errorDescription + "Point on plane not defined.\n";
return true;
}
if(!attributesExist(pointOnPlaneElement, {"x", "y", "z"})) {
errorDescription = errorDescription + "Point on plane coordinates not valid.\n";
return true;
}
Point3D pointOnPlane(pointOnPlaneElement->FindAttribute("x")->FloatValue(),
pointOnPlaneElement->FindAttribute("y")->FloatValue(),
pointOnPlaneElement->FindAttribute("z")->FloatValue());
if(type == "square") {
std::string skyboxSideName = "";
if(attributesExist(objectElement, {"skyboxSide"})) {
skyboxSideName = objectElement->Attribute("skyboxSide");
}
*object = new Square(vertexList, pointOnPlane, skyboxSideName);
} else {
*object = new PolygonalShape(vertexList, pointOnPlane);
}
} else {
errorDescription = errorDescription + "Object type" + type + " doesn't exists.\n";
return true;
}
return false;
}
