kdtree_node() {
				index_l = -1;
				index_r = -2;
				bb_p1 = point3D(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
				bb_p2 = point3D(std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest());
				lchild = rchild = -1;
			}
	polyvertex_tetrahedron::polyvertex_tetrahedron() {
		const double s2 = sqrt(2.0), s3 = sqrt(3.0);
		vertices.push_back(point3D(0.0, 0.0, 1.0));
		vertices.push_back(point3D(-2.0 * s2 / 3.0, 0.0, -1.0 / 3.0));
		vertices.push_back(point3D(s2 / 3.0, s2 / s3, -1.0 / 3.0));
		vertices.push_back(point3D(s2 / 3.0, -s2 / s3, -1.0 / 3.0));
	}
	polyvertex_cube::polyvertex_cube() {
		const double inv_s3 = 1.0 / sqrt(3.0);
		vertices.push_back(point3D(inv_s3, inv_s3, inv_s3));
		vertices.push_back(point3D(inv_s3, inv_s3, -inv_s3));
		vertices.push_back(point3D(inv_s3, -inv_s3, inv_s3));
		vertices.push_back(point3D(inv_s3, -inv_s3, -inv_s3));
		reflect();
	}
	surface_mobius::surface_mobius(double radius_, double half_width_) {
		radius = radius_;
		half_width = half_width_;

		double xy = radius + half_width;
		double z = half_width;

		set_bbox(point3D(-xy, -xy, -z), point3D(xy, xy, z));
	}
	polyvertex_dodecahedron::polyvertex_dodecahedron() {
		const double s3 = sqrt(3.0), s5 = sqrt(5.0), t1 = (s5 + 1.0) / 2.0, t2 = (s5 - 1.0) / 2.0;
		vertices.push_back(point3D(0.0, 1.0, 0.0));
		vertices.push_back(point3D(0.0, s5 / 3.0, 2.0 / 3.0));
		vertices.push_back(point3D(s3 / 3.0, s5 / 3.0, -1.0 / 3.0));
		vertices.push_back(point3D(-s3 / 3.0, s5 / 3.0, -1.0 / 3.0));
		vertices.push_back(point3D(s3 / 3.0, 1.0 / 3.0, s5 / 3.0));
		vertices.push_back(point3D(t1 * s3 / 3.0, 1.0 / 3.0, t2 * t2 / 3.0));
		vertices.push_back(point3D(t2 * s3 / 3.0, 1.0 / 3.0, -t1 * t1 / 3.0));
		vertices.push_back(point3D(-t2 * s3 / 3.0, 1.0 / 3.0, -t1 * t1 / 3.0));
		vertices.push_back(point3D(-t1 * s3 / 3.0, 1.0 / 3.0, t2 * t2 / 3.0));
		vertices.push_back(point3D(-s3 / 3.0, 1.0 / 3.0, s5 / 3.0));
		reflect();
	}
			inline void build_box(const surface_mesh *stc_ptr) {
				if (index_l <= index_r) {
					std::pair<point3D, point3D> box = stc_ptr->build_box(index_l, index_r);
					bb_p1 = box.first;
					bb_p2 = box.second;
				}
				for (int child : { lchild, rchild } ) {
					if (child != -1) {
						const kdtree_node &node = stc_ptr->nodes[child];
						bb_p1 = point3D(std::min(bb_p1.x, node.bb_p1.x), std::min(bb_p1.y, node.bb_p1.y), std::min(bb_p1.z, node.bb_p1.z));
						bb_p2 = point3D(std::max(bb_p2.x, node.bb_p2.x), std::max(bb_p2.y, node.bb_p2.y), std::max(bb_p2.z, node.bb_p2.z));
					}
				}
			}
Exemple #7
0
//drawPlane. Draws the plane that particles will bounce on.
void drawPlane(){
	point3D a = point3D(100, 0, 100);	//3dpoints
	point3D b = point3D(100, 0, -100);
	point3D c = point3D(-100, 0, -100);
	point3D d = point3D(-100, 0, 100);
	
	plane ground = plane(a,b,c,d);		
	glColor3f(0.25, 0.25, 0.25);
	glBegin(GL_POLYGON);
		glVertex3f(ground.a.x, ground.a.y, ground.a.z);
		glVertex3f(ground.b.x, ground.b.y, ground.b.z);
		glVertex3f(ground.c.x, ground.c.y, ground.c.z);
		glVertex3f(ground.d.x, ground.d.y, ground.d.z);
	glEnd();
}
point3D MathLib3D::movePoint(point3D point, vec3D vector){
	float x = point.x + vector.x;
	float y = point.y + vector.y;
	float z = point.z + vector.z;
	point3D returnPoint = point3D(x,y,z);
	return(returnPoint);
};
Exemple #9
0
MSPoint3D MapUtils::convertIso(CCPoint& point)
{
	float x = point.y + point.x * .5;
	float z = point.y - point.x * .5;
	MSPoint3D point3D(x, 0, z);
	return point3D;
}
string point3D2String(Point3f point){

    stringstream point3D(stringstream::in | stringstream::out);
    point3D << point.x << " "
            << point.y << " "
            << point.z ;

    return point3D.str();
}
double Point3D_Recons::runLM(double* point3d, const double* poses, int pose_block_size, const vector<Feature>& features, ofstream& out) {  
  ML3 point3D(point3d[0], point3d[1], point3d[2]);
  
  out << "(" << point3d[0] << " " << point3d[1] << " " << point3d[2] << ")" << endl;
  vector<Ray> rays;
  projectRays_(features, rays);
  
  MLRayListForIntersection rayList(rays.size());
  for (unsigned int i = 0; i < rays.size(); ++i) {
    unsigned int camera = rays[i].cameraId;
    Matrix<float> C = (cali_.getCamera(camera)).C();
    
    unsigned int poseId = rays[i].poseId;
    const double* pose = poses + pose_block_size * poseId;
    double Rm[9];
    EulerAngleToRotaMat(pose, Rm);
    
    out << " " << i << " " << camera << " " << poseId << " " << rays[i].obs << endl;
//     cout << "  [";
//     for (int j = 0; j < 9; ++j) 
//       cout << Rm[j] << " ";
//     cout << "]" << endl;
    
    // tm + Rm*tc
    ML3 originW;
    originW.x = *(pose + 3) + Rm[0]*C[3][0] + Rm[1]*C[3][1] + Rm[2]*C[3][2];
    originW.y = *(pose + 4) + Rm[3]*C[3][0] + Rm[4]*C[3][1] + Rm[5]*C[3][2];
    originW.z = *(pose + 5) + Rm[6]*C[3][0] + Rm[7]*C[3][1] + Rm[8]*C[3][2];
    out << "  " << originW;
    
    RealPoint3D<float> ray = rays[i].dir;
//     cout << "   " << ray << endl;

    // ray * Rm
    ML3 directionW;
    directionW.x = Rm[0]*ray.x() + Rm[1]*ray.y() + Rm[2]*ray.z();
    directionW.y = Rm[3]*ray.x() + Rm[4]*ray.y() + Rm[5]*ray.z();
    directionW.z = Rm[6]*ray.x() + Rm[7]*ray.y() + Rm[8]*ray.z();
    out << "  " << directionW;
    
    ML3 test = point3D - originW;
//     cout << "   " << test;
    test.Normalize();
    out << "    " << test;
    // add rayList
    rayList.SetRay(i, originW, directionW, rays[i].inlier);
  }

//   cout << "(" << point3d[0] << " " << point3d[1] << " " << point3d[2] << ")" << endl;
  double RMS = rayList.ImproveWithLevenbergMarquardt(point3D, 10, 0);
  point3d[0] = point3D.x, point3d[1] = point3D.y, point3d[2] = point3D.z;
  out << RMS << " (" << point3d[0] << " " << point3d[1] << " " << point3d[2] << ")"  << endl << endl;
  return RMS;
}
	polyvertex_icosahedron::polyvertex_icosahedron() {
		const double s5 = sqrt(5.0), t1 = (s5 + 1.0) / 2.0, t2 = (s5 - 1.0) / 2.0, k1 = sqrt(t1 / s5), k2 = sqrt(t2 / s5);
		vertices.push_back(point3D(0.0, 1.0, 0.0));
		vertices.push_back(point3D(0.0, 1.0 / s5, 2.0 / s5));
		vertices.push_back(point3D(k1, 1.0 / s5, t2 / s5));
		vertices.push_back(point3D(k2, 1.0 / s5, -t1 / s5));
		vertices.push_back(point3D(-k2, 1.0 / s5, -t1 / s5));
		vertices.push_back(point3D(-k1, 1.0 / s5, t2 / s5));
		reflect();
	}
	void surface_fractal_terrain::generate_mesh(const std::vector<std::vector<double> > &height, int size, double sidelen) {
		double scale = sidelen / size, delta = 1.0 / size;
		std::vector<point3D> vertices;

#define __id(i, j) ((i) * (size + 1) + (j))
		for (int i = 0; i <= size; ++i) {
			for (int j = 0; j <= size; ++j) {
				vertices.push_back(point3D(i * scale, j * scale, height[i][j]));
			}
		}
		setup_vertex(vertices);
		for (int i = 0; i < size; ++i) {
			for (int j = 0; j < size; ++j) {
				double u = i * delta, v = j * delta;

				add_surface(__id(i, j), __id(i + 1, j), __id(i, j + 1)).set_UV(point2D(u, v), point2D(u + delta, v), point2D(u, v + delta));
				add_surface(__id(i + 1, j), __id(i + 1, j + 1), __id(i, j + 1)).set_UV(point2D(u + delta, v), point2D(u + delta, v + delta), point2D(u, v + delta));
			}
		}
#undef __id
		setup_tree();
		interpolate_normal();
	}
Exemple #14
0
ostream &operator<<(ostream &ps, TSolid &s)
{
    TSolid src = s.GetDimensional();
    int i, j, k;
    point3D pt = point3D(s.GetiMax(), s.GetjMax(), s.GetkMax());

    ps << endl;
    ps << "volume grid:" << endl;
    ps << "array size:\t" << pt << endl;
    ps << "x[m]\ty[m]\tz[m]\tT [K]\tQ [W/m3]" << endl;
    for (i = 0; i < pt.x; i++)
        for (j = 0; j < pt.y; j++)
            for (k = 0; k < pt.z; k++)
                ps << src(i, j, k) << endl;
    ps << "begin heat affected zone" << endl;
    ps << src.jBegin;
    ps << "end heat affected zone" << endl;
    ps << src.jEnd;
    ps << "max displacement [m]:\t" << src.ndMaxDSai << endl;
    ps << "max temperature [K]:\t" << src.ndMaxTemp << endl;
    ps << "max velocity [m/s]:\t" << src.ndMaxVelo << endl;
    ps << "vaporization rate [m3/s]:\t" << src.ndVapRate << endl;
    return ps;
}
Exemple #15
0
	ray::ray() {
		origin = point3D(0, 0, 0);
		dir = vector3D(0, 0, 1);
	}
Point3D LabelMapper::GetMappedPoint(int labelId, int pixelId)
{
	 Point3D pixel = _mappingCubicData->GetMappedPoint(pixelId);
	 Point3D shift = _mappingCubicShift->GetMappedPoint(labelId);
	 return point3D(pixel.x + shift.x, pixel.y + shift.y, pixel.z + shift.z);
}
PxVec3 CameraComponent::screenToWorldPoint(const PxVec2& position) const
{
	PxVec2 screenDim((PxReal)getCoreEngine()->getViewport()->getScreenWidth(), (PxReal)getCoreEngine()->getViewport()->getScreenHeight());
	PxVec3 point3D(2.0f * position.x / screenDim.x - 1.0f, -2.0f * position.y / screenDim.y + 1.0f, getTransform().getPosition().z);
	return m_camera.getViewProjection().inverseRT().rotate(point3D);
}
Exemple #18
0
point3D operator-(point3D p) {
  return point3D(-p.x, -p.y, -p.z);
}
	polyvertex_octahedron::polyvertex_octahedron() {
		vertices.push_back(point3D(0.0, 1.0, 0.0));
		vertices.push_back(point3D(0.0, 0.0, 1.0));
		vertices.push_back(point3D(1.0, 0.0, 0.0));
		reflect();
	}