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));
}
}
}
//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);
};
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();
}
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;
}
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);
}
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();
}