void World::build()
{
//a sample scene with 5 spheres
Sphere *sph1 = new Sphere();
sph1->set_color(Pixel(123, 255, 45));
sph1->set_center(Point3d(-400, 200, -300));
sph1->set_radius(250);
add_object(sph1);
Sphere *sph2 = new Sphere();
sph2->set_color(Pixel(255, 123, 45));
sph2->set_center(Point3d(400, -200,-100));
sph2->set_radius(200);
add_object(sph2);
Sphere *sph3 = new Sphere();
sph3->set_color(Pixel(45, 123, 255));
sph3->set_center(Point3d(400, 200,-100));
sph3->set_radius(150);
add_object(sph3);
Sphere *sph4 = new Sphere();
sph4->set_color(Pixel(123, 123, 45));
sph4->set_center(Point3d(-600, -200,-100));
sph4->set_radius(150);
add_object(sph4);
Sphere *sph5 = new Sphere();
sph5->set_color(Pixel(235, 34, 45));
sph5->set_center(Point3d(0, 0,-100));
sph5->set_radius(50);
add_object(sph5);
}
void Particle::initializeDefaultPhysics() {
position = Point3d();
velocity = Point3d(Utilities::randomNegOneToOne(),
Utilities::randomNegOneToOne(),
Utilities::randomNegOneToOne());
force = Point3d(-velocity.x, -velocity.y, -velocity.z);
mass = DEFAULT_MASS;
}
/**
* This method returns the target that the camera should be looking at
*/
Point3d ControllerEditor::getCameraTarget(){
if (conF == NULL)
return Point3d(0,1,0);
Character* ch = conF->getCharacter();
if (ch == NULL)
return Point3d(0,1,0);
//we need to get the character's position. We'll hack that a little...
return Point3d(ch->getRoot()->getCMPosition().x, 1, ch->getRoot()->getCMPosition().z);
}
std::vector<Point3d> IlluminanceMap_Impl::corners() const
{
std::vector<Point3d> result;
result.push_back(Point3d(0,this->yLength(),0));
result.push_back(Point3d(0,0,0));
result.push_back(Point3d(this->xLength(),0,0));
result.push_back(Point3d(this->xLength(),this->yLength(),0));
return result;
}
void Various_Processing_Component::NoiseAdditionUniform (PolyhedronPtr pMesh, double noiseIntensity, bool preserveBoundaries)
{
// mesh centre calculation based on discrete "moment".
//TODO: maybe in the future it will be based on volume moment.
int numVertex = pMesh->size_of_vertices();;
Vector centroid = Point3d(0,0,0) - CGAL::ORIGIN;
double distancetoCentroid = 0.0;
Vertex_iterator pVertex;
for (pVertex = pMesh->vertices_begin(); pVertex != pMesh->vertices_end(); pVertex++)
{
Vector vectemp = pVertex->point() - CGAL::ORIGIN;
centroid = centroid + vectemp;
}
centroid = centroid/numVertex;
// calculate the average distance from vertices to mesh centre
for (pVertex = pMesh->vertices_begin(); pVertex!= pMesh->vertices_end(); pVertex++)
{
Vector vectemp = pVertex->point() - CGAL::ORIGIN;
distancetoCentroid = distancetoCentroid + (double)std::sqrt((vectemp - centroid) * (vectemp - centroid));
}
distancetoCentroid = distancetoCentroid/numVertex;
// add random uniform-distributed (between [-noiseLevel, +noiseLevel])
srand((unsigned)time(NULL));
double noisex, noisey, noisez;
double noiseLevel = distancetoCentroid * noiseIntensity;
for (pVertex = pMesh->vertices_begin(); pVertex!= pMesh->vertices_end(); pVertex++)
{
// keep boundaries untouched if demanded by user
bool is_border_vertex = false;
bool stopFlag = false;
Halfedge_around_vertex_circulator hav = (*pVertex).vertex_begin();
do
{
if (hav->is_border()==true)
{
is_border_vertex = true;
stopFlag = true;
}
hav++;
} while ((hav!=(*pVertex).vertex_begin())&&(stopFlag==false));
if ((preserveBoundaries==true)&&(is_border_vertex==true))
continue;
noisex = noiseLevel * (1.0*rand()/RAND_MAX-0.5)*2;
noisey = noiseLevel * (1.0*rand()/RAND_MAX-0.5)*2;
noisez = noiseLevel * (1.0*rand()/RAND_MAX-0.5)*2;
Vector temp = Point3d(noisex, noisey, noisez) - CGAL::ORIGIN;
pVertex->point() = pVertex->point() + temp;
}
// for correct rendering, we need to update the mesh normals
pMesh->compute_normals();
}
Line::Line(const Span& sp){
// contructor from linear span
p0 = sp.p0;
v = sp.vs * sp.length;
length = sp.length;
// box = sp.box;
box.min = Point3d(sp.box.min);
box.max = Point3d(sp.box.max);
ok = !sp.NullSpan;
}
Camera::Camera(){
at = Point3d(0.0,5.0,0.0);
up = Point3d(0.0,1.0,0.0);
theta = M_PI/3;
phi = M_PI/3;
R = 43.0;
eye = transf_coord();
//glMatrixMode(GL_MODELVIEW);
//glLoadIdentity();
//gluLookAt(eye.x,eye.y,eye.z,at.x,at.y,at.z,up.x,up.y,up.z);
};
TEST_F(spelHelperTest, distSquared3D)
{
double b = sqrt(1.0 / 3.0);
Point3d A = Point3d(0.0, 0.0, 0.0);
Point3d B = Point3d(b, b, b);
Point3f C = Point3f(-b, -b, -b);
EXPECT_DOUBLE_EQ(0.0, spelHelper::distSquared3d(A, A));
EXPECT_DOUBLE_EQ(1.0, spelHelper::distSquared3d(A, B));
EXPECT_FLOAT_EQ(1.0, (float)spelHelper::distSquared3d(Point3f(0.0, 0.0, 0.0), C));
}
Camera::~Camera(void)
{
cameraCenter = Point3d();
topLeft = Point3d();
topRight = Point3d();
bottomLeft = Point3d();
fovX = 0;
fovY = 0;
rotation = 0;
xResolution = 0;
yResolution = 0;
}
void drawBoundingBox(const Point3d &pMin, const Point3d &pMax)
{
Point3d pts[8];
pts[0] = Point3d(pMin.x, pMin.y, pMin.z);
pts[1] = Point3d(pMax.x, pMin.y, pMin.z);
pts[2] = Point3d(pMin.x, pMax.y, pMin.z);
pts[3] = Point3d(pMin.x, pMin.y, pMax.z);
pts[4] = Point3d(pMax.x, pMax.y, pMin.z);
pts[5] = Point3d(pMax.x, pMin.y, pMax.z);
pts[6] = Point3d(pMax.x, pMax.y, pMax.z);
pts[7] = Point3d(pMin.x, pMax.y, pMax.z);
drawLine(pts[0], pts[1]);
drawLine(pts[0], pts[2]);
drawLine(pts[0], pts[3]);
drawLine(pts[6], pts[5]);
drawLine(pts[6], pts[4]);
drawLine(pts[6], pts[7]);
drawLine(pts[2], pts[4]);
drawLine(pts[2], pts[7]);
drawLine(pts[3], pts[5]);
drawLine(pts[3], pts[7]);
drawLine(pts[4], pts[1]);
drawLine(pts[5], pts[1]);
}
void VisualSceneSpecPoints :: BuildScene (int zoomall)
{
if (!mesh)
{
VisualScene::BuildScene(zoomall);
return;
}
Box3d box;
if (mesh->GetNSeg())
{
box.SetPoint (mesh->Point (mesh->LineSegment(1)[0]));
for (int i = 1; i <= mesh->GetNSeg(); i++)
{
box.AddPoint (mesh->Point (mesh->LineSegment(i)[0]));
box.AddPoint (mesh->Point (mesh->LineSegment(i)[1]));
}
}
else if (specpoints.Size() >= 2)
{
box.SetPoint (specpoints.Get(1).p);
for (int i = 2; i <= specpoints.Size(); i++)
box.AddPoint (specpoints.Get(i).p);
}
else
{
box = Box3d (Point3d (0,0,0), Point3d (1,1,1));
}
if (zoomall == 2 && ((vispar.centerpoint >= 1 && vispar.centerpoint <= mesh->GetNP()) ||
vispar.use_center_coords))
{
if (vispar.use_center_coords)
{
center.X() = vispar.centerx; center.Y() = vispar.centery; center.Z() = vispar.centerz;
}
else
center = mesh->Point (vispar.centerpoint);
}
else
center = Center (box.PMin(), box.PMax());
rad = 0.5 * Dist (box.PMin(), box.PMax());
CalcTransformationMatrices();
}
void Plane::Mirrored(Matrix* tmMirrored) {
// calculates a mirror transformation that mirrors 2d about plane
Point3d p1 = this->Near(Point3d(0.,0.,0.));
if(tmMirrored->m_unit == false) tmMirrored->Unit();
double nx = this->normal.getx();
double ny = this->normal.gety();
double nz = this->normal.getz();
// the translation
tmMirrored->e[ 3] = -2. * nx * this->d;
tmMirrored->e[ 7] = -2. * ny * this->d;
tmMirrored->e[11] = -2. * nz * this->d;
// the rest
tmMirrored->e[ 0] = 1. - 2. * nx * nx;
tmMirrored->e[ 5] = 1. - 2. * ny * ny;
tmMirrored->e[10] = 1. - 2. * nz * nz;
tmMirrored->e[ 1] = tmMirrored->e[ 4] = -2. * nx * ny;
tmMirrored->e[ 2] = tmMirrored->e[ 8] = -2. * nz * nx;
tmMirrored->e[ 6] = tmMirrored->e[ 9] = -2. * ny * nz;
tmMirrored->m_unit = false;
tmMirrored->m_mirrored = true;
}
void rgb_pcl::getCloudFeatures(cv::Point3d &position, double &hue, PointCloudPtr cloudCluster){
double R, G, B;
double size = cloudCluster->points.size();
R = 0; G = 0; B = 0;
double maxZ = 0;
for(auto cloud_point: cloudCluster->points){
position += Point3d(cloud_point.x, cloud_point.y, cloud_point.z);
R += cloud_point.r; G += cloud_point.g; B += cloud_point.b;
if(cloud_point.z > maxZ){
maxZ = cloud_point.z; //We want to grab the object at the highest z
}
}
position.x /= size; position.y /= size; position.z = maxZ;
R /= size; G /= size; B /= size;
double Cmax = maximum(R, G, B);
double Cmin = minimum(R, G, B);
double delta = Cmax - Cmin;
if(R > G && R > B){
hue = 60*((double)((G-B)/delta));
}else if(G > R && G > B){
hue = 60*((double)((B-R)/delta)+2);
}else if(B > R && B > G){
hue = 60*((double)((R-G)/delta)+4);
}
if(hue < 0)
hue += 360;
}
float CameraProjections::ComputeError(vector<OptimizorParam> &d)
{
double totalDis = 0;
double maxDis = -9999999;
int worseCoef = 3;
int totalCount = 0;
for (size_t i = 0; i < params.camCalibrator.opticalAngle.size(); i++)
{
cameraLocation = params.camCalibrator.cameraLocation[i];
cameraOrintation = params.camCalibrator.opticalAngle[i]
+ Point3d(d[0].data, d[1].data, d[2].data);
diagnalAngleView = d[3].data;
CalculateProjection();
Point2f res;
if (GetOnRealCordinate(params.camCalibrator.clicked[i], res))
{
double t = Distance2point(res, params.camCalibrator.rvizClicked[i]);
totalDis += t;
maxDis = std::max(maxDis, t);
totalCount++;
}
else
{
return 10000000000;
}
}
return (totalDis + (maxDis * worseCoef)) / (totalCount + worseCoef);
}
Point3d Point3d::centerOf(Point3d l, Point3d r)
{
return Point3d(
l.x / 2.0 + r.x / 2.0,
l.y / 2.0 + r.y / 2.0,
l.z / 2.0 + r.z / 2.0);
}
Point3d Point3d::minPoint()
{
return Point3d(
std::numeric_limits<double>::min(),
std::numeric_limits<double>::min(),
std::numeric_limits<double>::min());
}
Point3d crossProduct(Point3d const& l, Point3d const& r)
{
return Point3d (
l.y * r.z - l.z * r.y,
l.z * r.x - l.x * r.z,
l.x * r.y - l.y * r.x);
}
/// point plus a vector is a new point
Point3d Point3d::operator+(const Vector3d& vec) const
{
double newX = x()+vec.x();
double newY = y()+vec.y();
double newZ = z()+vec.z();
return Point3d(newX, newY, newZ);
}
/**
Default constructor - populate the data members using safe values..
*/
RBState::RBState(void){
//guess some default values if this constructor is used...
this->position = Point3d(0,0,0);
this->orientation = Quaternion(1,Vector3d(0,0,0));
this->velocity = Vector3d(0,0,0);
this->angularVelocity = Vector3d(0,0,0);
}
bool Line::atZ(double z, Point3d& p)const {
// returns p at z on line
if(FEQZ(this->v.getz())) return false;
double t = (z - this->p0.z) / this->v.getz();
p = Point3d(this->p0.x + t * this->v.getx(), this->p0.y + t * this->v.gety(), z);
return true;
}
GLCamera::GLCamera(void){
//initialize to the identity quaternion
orientation = Quaternion(1, 0, 0, 0);
camDistance = -4;
target = Point3d(0,0,0);
rotations = Vector3d(0,0,0);
}
Point3d Map::calculIntersection(
Point3d planeVector, Point3d planePoint,
Point3d lineVector, Point3d linePoint){
Point3d returnResult;
double vp1, vp2, vp3, n1, n2, n3, v1, v2, v3, m1, m2, m3, t,vpt;
vp1 = planeVector.x;
vp2 = planeVector.y;
vp3 = planeVector.z;
n1 = planePoint.x;
n2 = planePoint.y;
n3 = planePoint.z;
v1 = lineVector.x;
v2 = lineVector.y;
v3 = lineVector.z;
m1 = linePoint.x;
m2 = linePoint.y;
m3 = linePoint.z;
vpt = v1 * vp1 + v2 * vp2 + v3 * vp3;
if (vpt == 0){
returnResult=Point3d(); //si perpendiculaire, on retourne un verteur null
}
else{
t = ((n1 - m1) * vp1 + (n2 - m2) * vp2 + (n3 - m3) * vp3) / vpt;
returnResult.x = m1 + v1 * t;
returnResult.y = m2 + v2 * t;
returnResult.z = m3 + v3 * t;
}
return returnResult;
}
inline Point3d Center (const Point3d & p1, const Point3d & p2,
const Point3d & p3, const Point3d & p4)
{
return Point3d (0.25 * (p1.x[0] + p2.x[0] + p3.x[0] + p4.x[0]),
0.25 * (p1.x[1] + p2.x[1] + p3.x[1] + p4.x[1]),
0.25 * (p1.x[2] + p2.x[2] + p3.x[2] + p4.x[2]));
}
inline Point3d Center (const Point3d & p1, const Point3d & p2,
const Point3d & p3)
{
return Point3d (1.0/3.0 * (p1.x[0] + p2.x[0] + p3.x[0]),
1.0/3.0 * (p1.x[1] + p2.x[1] + p3.x[1]),
1.0/3.0 * (p1.x[2] + p2.x[2] + p3.x[2]));
}
// Set a local limit on the maximum mesh size allowed within a given box region
DLL_HEADER void Ng_RestrictMeshSizeBox (Ng_Mesh * mesh, double * pmin, double * pmax, double h)
{
for (double x = pmin[0]; x < pmax[0]; x += h)
for (double y = pmin[1]; y < pmax[1]; y += h)
for (double z = pmin[2]; z < pmax[2]; z += h)
((Mesh*)mesh) -> RestrictLocalH (Point3d (x, y, z), h);
}
Point3d CameraOpenCV::UnProject(const Point2d& p2d)
{
#ifdef HAS_OPENCV
cv::Point2d uv(p2d.x,p2d.y),px;
const cv::Mat src_pt(1, 1, CV_64FC2, &uv);
cv::Mat dst_pt(1, 1, CV_64FC2, &px);
cv::undistortPoints(src_pt, dst_pt, cam_k, cam_d);
return Point3d(px.x,px.y,1.);
#else
Point3d result((p2d.x-cx)*fx_inv,(p2d.y-cy)*fy_inv,1.);
double r2,r4,r6;
r2=result.x*result.x+result.y*result.y;
r4=r2*r2;
r6=r4*r2;
double& X=result.x;
double& Y=result.y;
double a,b0,b,c;
b0=1+k1*r2+k2*r4+k3*r6;
a=2.0*p2;
b=b0+2.0*p1*Y;
c=p2*r2-X;
if(a*a>0.0000001) result.x= (sqrt(b*b-4.0*a*c)-b)/(a*2.0);
a=2.0*p1;
b=b0+2.0*p2*X;
c=p1*r2-Y;
if(a*a>0.0000001) result.y= (sqrt(b*b-4.0*a*c)-b)/(a*2.0);
return result;
#endif
}
void MainApplication::rangeQuery()
{
labelTime->setText(QString("---"));
Point3d v1 = Point3d(minXRange->text().toDouble(), minYRange->text().toDouble(), minZRange->text().toDouble());
Point3d v2 = Point3d(maxXRange->text().toDouble(), maxYRange->text().toDouble(), maxZRange->text().toDouble());
std::vector<int> quvec;
std::chrono::time_point<std::chrono::system_clock, std::chrono::system_clock::duration> t1 = std::chrono::high_resolution_clock::now(); //start timer
quvec = _Tree3d.rangeQuery(v1, v2);
stopTimer(t1);
setColor(0.3);
setColor(quvec.begin(), quvec.end(), 0.9, 0, 0.2);
glWidget->update();
}
Ray PerspectiveCamera::generateRay(const Point2d &sample) const
{
Point3d ori(0);
Ray ray(ori, normalize(Vector3d(rasterToCamera_(Point3d(sample.x_, sample.y_, 0)))));
if (focal_distance_ > 0) {
static RandomNumberGenerator rng;
Point2d tmp = Point2d(rng.Uniform2(), rng.Uniform2());
ori = Point3d(tmp.x_, tmp.y_, 0) * radius_;
double z = -(focal_distance_ / ray.direction().z_);
Point3d hit(ray.direction() * z);
ray.setDirection(normalize(hit - ori));
}
ray.setOrigin(cameraToWorld_(ori));
return std::move(ray);
}
Point3d toUnitVector(Point3d const& orig)
{
double mag = magnitude(orig);
return Point3d(
orig.x / mag,
orig.y / mag,
orig.z / mag);
}
/* metoda care initialzeaza punctele cubului rubik */
void Rubik::set_points() {
points.push_back(Point3d(2,2,2)); //0
points.push_back(Point3d(2,2,-2)); //1
points.push_back(Point3d(-2,2,-2)); //2
points.push_back(Point3d(-2,2,2)); //3
points.push_back(Point3d(2,-2,2)); //4
points.push_back(Point3d(2,-2,-2)); //5
points.push_back(Point3d(-2,-2,-2)); //6
points.push_back(Point3d(-2,-2,2)); //7
}
