void subdivision(Vector3D centre, float rayon, float rayonMin) { std::vector<Vector3D> lesCentres; if (rayon > rayonMin) { lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() - rayon / 2, centre.z() - rayon / 2)); lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() + rayon / 2, centre.z() - rayon / 2)); lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() - rayon / 2, centre.z() - rayon / 2)); lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() + rayon / 2, centre.z() - rayon / 2)); lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() - rayon / 2, centre.z() + rayon / 2)); lesCentres.push_back(Vector3D(centre.x() - rayon / 2, centre.y() + rayon / 2, centre.z() + rayon / 2)); lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() - rayon / 2, centre.z() + rayon / 2)); lesCentres.push_back(Vector3D(centre.x() + rayon / 2, centre.y() + rayon / 2, centre.z() + rayon / 2)); for (int i = 0; i < lesCentres.size(); i++) { subdivision(lesCentres.at(i), rayon / 2, rayonMin); } } else { finalSize = rayon; centreCube.push_back(centre); //cube(centre, rayon); } }
bool Triangulation::isCounterClockwise( const Vector3D & inPoint1, const Vector3D & inPoint2, const Vector3D & inPoint3) { return (inPoint3.y() - inPoint1.y())*(inPoint2.x() - inPoint1.x()) > (inPoint2.y() - inPoint1.y())*(inPoint3.x() - inPoint1.x()); }
void Road::getLaneRoadPoints(double s, int i, RoadPoint &pointIn, RoadPoint &pointOut, double &disIn, double &disOut) { LaneSection *section = (--laneSectionMap.upper_bound(s))->second; disIn = section->getDistanceToLane(s, i); disOut = disIn + section->getLaneWidth(s, i); //std::cerr << "s: " << s << ", i: " << i << ", distance: " << disIn << ", width: " << disOut-disIn << std::endl; Vector3D xyPoint = ((--xyMap.upper_bound(s))->second)->getPoint(s); Vector2D zPoint = ((--zMap.upper_bound(s))->second)->getPoint(s); double alpha = ((--lateralProfileMap.upper_bound(s))->second)->getAngle(s, (disOut + disIn) * 0.5); double beta = zPoint[1]; double gamma = xyPoint[2]; double Tx = (sin(alpha) * sin(beta) * cos(gamma) - cos(alpha) * sin(gamma)); double Ty = (sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma)); double Tz = (sin(alpha) * cos(beta)); //std::cout << "s: " << s << ", i: " << i << ", alpha: " << alpha << ", beta: " << beta << ", gamma: " << gamma << std::endl; double nx = sin(alpha) * sin(gamma) + cos(alpha) * sin(beta) * cos(gamma); double ny = cos(alpha) * sin(beta) * sin(gamma) - sin(alpha) * cos(gamma); double nz = cos(alpha) * cos(beta); pointIn = RoadPoint(xyPoint.x() + Tx * disIn, xyPoint.y() + Ty * disIn, zPoint[0] + Tz * disIn, nx, ny, nz); pointOut = RoadPoint(xyPoint.x() + Tx * disOut, xyPoint.y() + Ty * disOut, zPoint[0] + Tz * disOut, nx, ny, nz); //std::cout << "s: " << s << ", i: " << i << ", inner: x: " << pointIn.x() << ", y: " << pointIn.y() << ", z: " << pointIn.z() << std::endl; //std::cout << "s: " << s << ", i: " << i << ", outer: x: " << pointOut.x() << ", y: " << pointOut.y() << ", z: " << pointOut.z() << std::endl << std::endl; }
double CAHitsGenerator::DeltaEta(Vector3D p1, Vector3D p2){ //maybe it has to be redefined... Vector3D gV(p2.x()-p1.x(),p2.y()-p1.y(),p2.z()-p1.z()); double eta = gV.eta(); return eta; }
/** * \brief Gets the min x, min y, and min z of either points */ Vector3D getMinEnds(Vector3D& a, Vector3D& b) { Vector3D c = a; if (b.x() < c.x()) { c.x() = b.x(); } if (b.y() < c.y()) { c.y() = b.y(); } if (b.z() < c.z()) { c.z() = b.z(); } return c; }
Vector3D Vector3D::vector_prod(Vector3D another) { double x = _y * another.z() - _z * another.y(); double y = _z * another.x() - _x * another.z(); double z = _x * another.y() - _y * another.x(); return Vector3D(x, y, z); };
SphereVBO::SphereVBO(const Vector3D ¢er, float radius, int slices, bool textured) : VBO(textured), slices(slices) { int size = (slices + 1) * slices; int posSize = size * 3; int texSize = size * 2; int normSize = size * 3; if (radius < 0) radius = -radius; if (slices < 0) slices = -slices; if (slices < 4 || radius <= 0) return; SmartPointer<float>::Array posData = new float[posSize]; SmartPointer<float>::Array texData = new float[texSize]; SmartPointer<float>::Array normData = new float[normSize]; for (int j = 0; j < slices / 2; j++) { float theta1 = j * M_PI * 2.0 / slices - M_PI / 2.0; float theta2 = (j + 1) * M_PI * 2.0 / slices - M_PI / 2.0; for (int i = 0; i <= slices; i++) { float theta3 = i * M_PI * 2 / slices; int index = j * slices + i; int posI = index * 6; int texI = index * 4; int normI = index * 6; float x = cos(theta2) * cos(theta3); float y = sin(theta2); float z = cos(theta2) * sin(theta3); posData[posI++] = center.x() + radius * x; posData[posI++] = center.y() + radius * y; posData[posI++] = center.z() + radius * z; texData[texI++] = i / (float)slices; texData[texI++] = 2 * (j + 1) / (float)slices; normData[normI++] = x; normData[normI++] = y; normData[normI++] = z; x = cos(theta1) * cos(theta3); y = sin(theta1); z = cos(theta1) * sin(theta3); posData[posI++] = center.x() + radius * x; posData[posI++] = center.y() + radius * y; posData[posI++] = center.z() + radius * z; texData[texI++] = i / (float)slices; texData[texI++] = 2 * j / (float)slices; normData[normI++] = x; normData[normI++] = y; normData[normI++] = z; } } loadArrays(size, posData, texData, normData); }
float AABB::getShortestDistance(const hydra::math::Vector3D& inPoint) const { //if inside return 0.0f if(isInside(inPoint)) return 0.0f; //relative point's position Vector3D relativePos = inPoint - mCorner; bool x_positive = (relativePos.x() > (mVector.x() / 2.0f)), y_positive = (relativePos.y() > (mVector.y() / 2.0f)), z_positive = (relativePos.z() > (mVector.z() / 2.0f)); float closestCornerX = 0.0f, closestCornerY = 0.0f, closestCornerZ = 0.0f; if(x_positive) closestCornerX = mVector.x(); if(y_positive) closestCornerY = mVector.y(); if(z_positive) closestCornerZ = mVector.z(); Vector3D closestCorner(closestCornerX, closestCornerY, closestCornerZ); float distanceToClosestCorner = (relativePos - closestCorner).getMagnitude(); //if in the interval bool xIsInInterval = relativePos.x() > 0.0f && relativePos.x() < mVector.x(), yIsInInterval = relativePos.y() > 0.0f && relativePos.y() < mVector.y(), zIsInInterval = relativePos.z() > 0.0f && relativePos.z() < mVector.z(); assert(!(xIsInInterval && yIsInInterval && zIsInInterval)); float distanceToClosestPlane = -1.0f; if(xIsInInterval && yIsInInterval) { if(z_positive) { distanceToClosestPlane = fabsf(relativePos.z() - mVector.z()); } else { distanceToClosestPlane = fabsf(relativePos.z()); } } if(xIsInInterval && zIsInInterval) { if(y_positive) { distanceToClosestPlane = fabsf(relativePos.y() - mVector.y()); } else { distanceToClosestPlane = fabsf(relativePos.y()); } } if(zIsInInterval && yIsInInterval) { if(x_positive) { distanceToClosestPlane = fabsf(relativePos.x() - mVector.x()); } else { distanceToClosestPlane = fabsf(relativePos.x()); } } if(distanceToClosestPlane < 0.0f) return distanceToClosestCorner; else return std::min(distanceToClosestCorner, distanceToClosestPlane); }
Vector3D catmullRomInterpolate( const Vector3D& p0, const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, double t) { return Vector3D( catmullRomInterpolate(p0.x(), p1.x(), p2.x(), p3.x(), t), catmullRomInterpolate(p0.y(), p1.y(), p2.y(), p3.y(), t), catmullRomInterpolate(p0.z(), p1.z(), p2.z(), p3.z(), t)); }
bool inDaBox(Vector3D centre, float rayonf, Vector3D vertex) { //std::cout << vertex.x() << " " << vertex.y() << " " << vertex.z() << std::endl; if (vertex.x() >= centre.x() - rayonf && vertex.x() < centre.x() + rayonf && vertex.y() >= centre.y() - rayonf && vertex.y() < centre.y() + rayonf && vertex.z() >= centre.z() - rayonf && vertex.z() < centre.z() + rayonf) return true; else return false; }
BoundingBox3D::BoundingBox3D ( const Vector3D& inPoint1, const Vector3D& inPoint2) :mEmpty(false) { mCorner[0] = Vector3D( _MIN_(inPoint1.x(),inPoint2.x()), _MIN_(inPoint1.y(),inPoint2.y()), _MIN_(inPoint1.z(),inPoint2.z())); mCorner[1] = Vector3D( _MAX_(inPoint1.x(),inPoint2.x()), _MAX_(inPoint1.y(),inPoint2.y()), _MAX_(inPoint1.z(),inPoint2.z())); }
bool BoundingBox3D::contains ( const Vector3D& inPoint, const Tolerance& inTol )const { return inTol.isGreater(mCorner[0].x(),inPoint.x()) ||inTol.isGreater(mCorner[0].y(),inPoint.y()) ||inTol.isGreater(mCorner[0].z(),inPoint.z()) ||inTol.isLess(mCorner[1].x(),inPoint.x()) ||inTol.isLess(mCorner[1].y(),inPoint.y()) ||inTol.isLess(mCorner[1].z(),inPoint.z())?false:true; }
bool BoundingBox3D::isInClash ( const Vector3D& inPoint1, const Vector3D& inPoint2, const Tolerance& inTol)const { return inTol.isGreater(mCorner[0].x(),inPoint2.x()) ||inTol.isGreater(mCorner[0].y(),inPoint2.y()) ||inTol.isGreater(mCorner[0].z(),inPoint2.z()) ||inTol.isLess(mCorner[1].x(),inPoint1.x()) ||inTol.isLess(mCorner[1].y(),inPoint1.y()) ||inTol.isLess(mCorner[1].z(),inPoint1.z())?false:true; }
void MachineLinearizer::arc(const Vector3D &offset, double angle, plane_t plane) { const char *axesNames; switch (plane) { case XY: axesNames = "XYZ"; break; case XZ: axesNames = "XZY"; break; case YZ: axesNames = "YZX"; break; default: THROWS("Invalid plane: " << plane); } unsigned char axes[3]; for (unsigned i = 0; i < 3; i++) axes[i] = Axes::toIndex(axesNames[i]); Axes current = getPosition(); // Initial point double x = current[axes[0]]; double y = current[axes[1]]; double z = current[axes[2]]; // Center Vector2D center(x + offset.x(), y + offset.y()); // Start angle double startAngle = Vector2D(-offset.x(), -offset.y()).angleBetween(Vector2D(1, 0)); // Radius double radius = Vector2D(offset.x(), offset.y()).length(); // Segments unsigned segments = (unsigned)(fabs(angle) / (2 * M_PI) * arcPrecision); double zDelta = offset.z() / segments; double deltaAngle = -angle / segments; // Create segments for (unsigned i = 0; i < segments; i++) { double currentAngle = startAngle + deltaAngle * (i + 1); x = center.x() + radius * cos(currentAngle); y = center.y() + radius * sin(currentAngle); z += zDelta; // Move current[axes[0]] = x; current[axes[1]] = y; current[axes[2]] = z; move(current, false); } }
/** * Method is used to create node children: one of 8, configure it and set entities collection. * @param parent is pointer to new node parent. * @param position is children node position in 3D space. * @pram childID is children node ID(0-7). */ void OctTree::createNodeChildren(OctNode* parent, Vector3D position, const int childID) { OctNode* node = new OctNode(); node->parent = parent; for(int i = 0; i < 8; ++i) node->children[i] = nullptr; node->volume.min = Vector3D(parent->volume.min.x()*0.5f,parent->volume.min.y()*0.5f,parent->volume.min.z()*0.5f); node->volume.max = Vector3D(parent->volume.max.x()*0.5f,parent->volume.max.y()*0.5f,parent->volume.max.z()*0.5f); node->position = position; node->size = Vector3D(parent->size.x()*0.5f,parent->size.y()*0.5f,parent->size.z()*0.5f); for(unsigned i = 0; i < sceneEntities->size(); i++) { Vector3D entityMin = sceneEntities->at(i)->entityGeometry.geometryBox->min + sceneEntities->at(i)->entityState.position; Vector3D entityMax = sceneEntities->at(i)->entityGeometry.geometryBox->max + sceneEntities->at(i)->entityState.position; Vector3D nodeMin = node->volume.min + node->position; Vector3D nodeMax = node->volume.max + node->position; if( entityMin.x() <= nodeMax.x() && entityMin.x() >= nodeMin.x() && entityMin.y() <= nodeMax.y() && entityMin.y() >= nodeMin.y() && entityMin.z() <= nodeMax.z() && entityMin.z() >= nodeMin.z() && entityMax.x() <= nodeMax.x() && entityMax.x() >= nodeMin.x() && entityMax.y() <= nodeMax.y() && entityMax.y() >= nodeMin.y() && entityMax.z() <= nodeMax.z() && entityMax.z() >= nodeMin.z()) node->entities.push_back(sceneEntities->at(i)); } parent->children[childID] = node; }
void BoundingBox3D::add ( const Vector3D& inPoint) { if(mEmpty) { mCorner[0] = mCorner[1] = inPoint; mEmpty = false; return; } mCorner[0] = Vector3D( _MIN_(mCorner[0].x(),inPoint.x()), _MIN_(mCorner[0].y(),inPoint.y()), _MIN_(mCorner[0].z(),inPoint.z())); mCorner[1] = Vector3D( _MAX_(mCorner[1].x(),inPoint.x()), _MAX_(mCorner[1].y(),inPoint.y()), _MAX_(mCorner[1].z(),inPoint.z())); }
void Visualizer::AddVolume( TGeoShape *shape, Vector3D<Precision> p) { fVolumes.push_back(std::make_tuple( new TGeoVolume("SHAPE", shape, Vacuum), new TGeoTranslation(p.x(), p.y(), p.z())) ) ; }
void Visualizer::AddVolume( TGeoVolume *rootVolume, Vector3D<Precision> p) { fVolumes.push_back(std::make_tuple( rootVolume, new TGeoTranslation(p.x(), p.y(), p.z())) ) ; }
Transform Road::getRoadTransform(const double &s, const double &t) { Vector3D xyPoint = ((--xyMap.upper_bound(s))->second)->getPoint(s); Vector2D zPoint = ((--zMap.upper_bound(s))->second)->getPoint(s); //std::cerr << "Center: x: " << center.x() << ", y: " << center.y() << ", z: " << center.z() << std::endl; double alpha = ((--lateralProfileMap.upper_bound(s))->second)->getAngle(s, t); //std::cerr << "s: " << s << "Alpha: " << alpha << ", Beta: " << beta << ", Gamma: " << gamma << std::endl; //double sinalpha = sin(alpha); double cosalpha=cos(alpha); //double sinbeta = sin(beta); double cosbeta=cos(beta); //double singamma = sin(gamma); double cosgamma=cos(gamma); Quaternion qz(xyPoint[2], Vector3D(0, 0, 1)); Quaternion qya(zPoint[1], Vector3D(0, 1, 0)); Quaternion qxaa(alpha, Vector3D(1, 0, 0)); Quaternion q = qz * qya * qxaa; /* return RoadTransform( xyPoint.x() + (sinalpha*sinbeta*cosgamma-cosalpha*singamma)*t, xyPoint.y() + (sinalpha*sinbeta*singamma+cosalpha*cosgamma)*t, zPoint.z() + (sinalpha*cosbeta)*t, alpha, beta, gamma); */ return Transform(Vector3D(xyPoint.x(), xyPoint.y(), zPoint[0]) + (q * Vector3D(0, t, 0) * q.T()).getVector(), q); }
Vector4D::Vector4D(const Vector3D& v, float w) { vec[0] = v.x(); vec[1] = v.y(); vec[2] = v.z(); vec[3] = w; }
RoadPoint Road::getRoadPoint(double s, double t) { //RoadPoint center = getChordLinePoint(s); Vector3D xyPoint = ((--xyMap.upper_bound(s))->second)->getPoint(s); Vector2D zPoint = ((--zMap.upper_bound(s))->second)->getPoint(s); //std::cerr << "Center: x: " << center.x() << ", y: " << center.y() << ", z: " << center.z() << std::endl; double alpha = ((--lateralProfileMap.upper_bound(s))->second)->getAngle(s, t); double beta = zPoint[1]; double gamma = xyPoint[2]; //std::cerr << "s: " << s << "Alpha: " << alpha << ", Beta: " << beta << ", Gamma: " << gamma << std::endl; double sinalpha = sin(alpha); double cosalpha = cos(alpha); double sinbeta = sin(beta); double cosbeta = cos(beta); double singamma = sin(gamma); double cosgamma = cos(gamma); return RoadPoint(xyPoint.x() + (sinalpha * sinbeta * cosgamma - cosalpha * singamma) * t, xyPoint.y() + (sinalpha * sinbeta * singamma + cosalpha * cosgamma) * t, zPoint[0] + (sinalpha * cosbeta) * t, sinalpha * singamma + cosalpha * sinbeta * cosgamma, cosalpha * sinbeta * singamma - sinalpha * cosgamma, cosalpha * cosbeta); }
void OglWidget::drawVertex(const Vector3D &p) const { glPushMatrix(); glTranslatef(p.x(), p.y(), p.z()); gluSphere(quadratic, 0.1f , 8, 8); glPopMatrix(); }
void SliceContourGenerator::run(FieldFunction &func, GridTreeRef &grid) { // Progress unsigned completedCells = 0; unsigned totalCells = grid.getTotalCells(); // Compute slices const Vector3U &steps = grid.getSteps(); CubeSlice slice(grid); double resolution = grid.getResolution(); Vector3D p; Task::begin("Contouring cut surface"); for (unsigned z = 0; z < steps.z(); z++) { p.z() = grid.getOffset().z() + resolution * z; if (z) slice.shift(); slice.compute(*this, func); doSlice(func, slice, z); for (unsigned y = 0; y < steps.y(); y++) { p.y() = grid.getOffset().y() + resolution * y; for (unsigned x = 0; x < steps.x(); x++) { p.x() = grid.getOffset().x() + resolution * x; if (!func.cull(p, resolution * 1.1)) doCell(grid, slice, x, y); // Progress if ((++completedCells & 7) == 0 && !Task::update((double)completedCells / totalCells)) return; } } } }
void Workpiece::update(GCode::ToolPath &path) { if (!isAutomatic()) return; Rectangle3D bounds; // Guess workpiece bounds from cutting moves vector<SmartPointer<Sweep> > sweeps; vector<Rectangle3D> bboxes; for (unsigned i = 0; i < path.size(); i++) { const GCode::Move &move = path.at(i); if (move.getType() == GCode::MoveType::MOVE_RAPID) continue; int tool = move.getTool(); if (tool < 0) continue; if (sweeps.size() <= (unsigned)tool) sweeps.resize(tool + 1); if (sweeps[tool].isNull()) sweeps[tool] = ToolSweep::getSweep(path.getTools().get(tool)); sweeps[tool]->getBBoxes(move.getStartPt(), move.getEndPt(), bboxes, 0); } for (unsigned i = 0; i < bboxes.size(); i++) bounds.add(bboxes[i]); if (bounds == Rectangle3D()) return; // Start from z = 0 Vector3D bMin = bounds.getMin(); Vector3D bMax = bounds.getMax(); bounds = Rectangle3D(bMin, Vector3D(bMax.x(), bMax.y(), 0)); // At least 2mm thick if (bounds.getHeight() < 2) bounds.add(Vector3D(bMin.x(), bMin.y(), bMin.z() - 2)); if (bounds.isReal()) { // Add margin Vector3D margin = bounds.getDimensions() * getMargin() / 100.0; bounds.add(bounds.getMin() - margin); bounds.add(bounds.getMax() + Vector3D(margin.x(), margin.y(), 0)); setBounds(bounds); } }
void Camera::move( const Vector3D& deltaPos ) { Vector3D v = _target - _position; v.z() = 0.0f; v.normalize(); _target += v * deltaPos.x() + _right * deltaPos.y(); calculatePosition(); }
bool EdgePointXYZ::write(std::ostream& os) const { Vector3D p = measurement(); os << p.x() << " " << p.y() << " " << p.z(); for (int i = 0; i < 3; ++i) for (int j = i; j < 3; ++j) os << " " << information()(i, j); return os.good(); }
js::Value MatrixModule::getXYZ(const js::Arguments &args) { Vector3D v = ctx.machine.getPosition(XYZ); js::Value array = js::Value::createArray(); array.set(0, v.x()); array.set(1, v.y()); array.set(2, v.z()); return array; }
//----------------------------------------------------------------------------- void PrUnorganized_OP::print(std::ostream& os) //----------------------------------------------------------------------------- { os << getNumNodes() << ' ' << nInt_ << '\n'; int i; for(i=0; i<getNumNodes(); i++) { Vector3D p = cellstruct_.get3dNode(i); os << p.x() << " " << p.y() << " " << p.z(); os << " " << uv_[i].x() << " " << uv_[i].y() << "\n"; } }
* @param point Zu testender Punkt * @return true wenn Punkt im Wuerfel liegt */ bool Cube::enclosesPoint(const Vector3D& point) const { VTYPE x = point.x(), y = point.y(), z = point.z(); return !IS_SMALLER(x, m_vertices[0].x()) && !IS_GREATER(x, m_vertices[1].x()) && !IS_SMALLER(y, m_vertices[3].y()) && !IS_GREATER(y, m_vertices[0].y()) &&
///// paintGL ///////////////////////////////////////////////////////////////// void GLView::paintGL() /// Overridden from QGlWidget::paintGL(). Does the drawing of the OpenGL scene. { if(staticUpdateIndex != updateIndex) updateGLSettings(); ///// build a quaternion from the user's changes Quaternion<float> changeQuaternion(xRot, yRot, zRot); ///// multiply it with the current quaternion Quaternion<float> tempQuaternion = *orientationQuaternion; *orientationQuaternion = tempQuaternion*changeQuaternion; // no *= operator implemented yet => calling default constructor? ///// generate an axis-angle representation for OpenGL Vector3D<float> axis; float angle; orientationQuaternion->getAxisAngle(axis, angle); if(!animation) { ///// reset the changes xRot = 0.0f; yRot = 0.0f; zRot = 0.0f; } //if(baseParameters.antialias) //{ // glClear(GL_COLOR_BUFFER_BIT); //} //else //{ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //} glLoadIdentity(); //qDebug("GlView::xPos = %f, yPos = %f, zPos = %f", xPos, yPos, zPos); //qDebug("GlView::orientationQuaternion = (%f, %f, %f, %f)",orientationQuaternion->x(),orientationQuaternion->y(),orientationQuaternion->z(),orientationQuaternion->w()); ///// camera setup if(baseParameters.perspectiveProjection) gluLookAt(0.0f, 0.0f, zPos, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f); else resizeGL(width(), height()); glPushMatrix(); glTranslatef(xPos, yPos, 0.0f); glRotatef(angle, axis.x(), axis.y(), axis.z()); drawScene(); // pure virtual glPopMatrix(); glFlush(); // drawing is complete => send for execution if(animation) timer->start(redrawWait); }