bool PlaneGeometry::IntersectionLine( const PlaneGeometry* plane, Line3D& crossline ) const { Vector3D normal = this->GetNormal(); normal.Normalize(); Vector3D planeNormal = plane->GetNormal(); planeNormal.Normalize(); Vector3D direction = itk::CrossProduct( normal, planeNormal ); if ( direction.GetSquaredNorm() < eps ) return false; crossline.SetDirection( direction ); double N1dN2 = normal * planeNormal; double determinant = 1.0 - N1dN2 * N1dN2; Vector3D origin = this->GetOrigin().GetVectorFromOrigin(); Vector3D planeOrigin = plane->GetOrigin().GetVectorFromOrigin(); double d1 = normal * origin; double d2 = planeNormal * planeOrigin; double c1 = ( d1 - d2 * N1dN2 ) / determinant; double c2 = ( d2 - d1 * N1dN2 ) / determinant; Vector3D p = normal * c1 + planeNormal * c2; crossline.GetPoint().GetVnlVector() = p.GetVnlVector(); return true; }
void Line3D::jacobian(Matrix7x6d& Jp, Matrix7x6d& Jl, const Eigen::Isometry3d& x, const Line3D& l){ Jp.setZero(); Jl.setZero(); Matrix6d A=Matrix6d::Zero(); A.block<3,3>(0,0)=x.linear(); A.block<3,3>(0,3)= _skew(x.translation())*x.linear(); A.block<3,3>(3,3)=x.linear(); Vector6d v=(Vector6d)l; Line3D lRemapped(v); Matrix6d D = Matrix6d::Zero(); D.block<3,3>(0,0) = -_skew(l.d()); D.block<3,3>(0,3) = -2*_skew(l.w()); D.block<3,3>(3,3) = -2*_skew(l.d()); Jp.block<6,6>(0,0) = A*D; Vector3d d = l.d(); Vector3d w = l.w(); double ln = d.norm(); double iln = 1./ln; double iln3 = std::pow(iln,3); Matrix6d Jll; Jll << iln,0,0,-(w.x()*d.x())*iln3,-(w.x()*d.y())*iln3,-(w.x()*d.z())*iln3, 0,iln,0,-(w.y()*d.x())*iln3,-(w.y()*d.y())*iln3,-(w.y()*d.z())*iln3, 0,0,iln,-(w.z()*d.x())*iln3,-(w.z()*d.y())*iln3,-(w.z()*d.z())*iln3, 0,0,0,-(d.x()*d.x()-ln*ln)*iln3,-(d.x()*d.y())*iln3,-(d.x()*d.z())*iln3, 0,0,0,-(d.x()*d.y())*iln3,-(d.y()*d.y()-ln*ln)*iln3,-(d.y()*d.z())*iln3, 0,0,0,-(d.x()*d.z())*iln3,-(d.y()*d.z())*iln3,-(d.z()*d.z()-ln*ln)*iln3; Jl.block<6,6>(0,0) = A*Jll; Jl.block<1,6>(6,0) << 2*d.x(),2*d.y(),2*d.z(); }
Point3D Line3D::intersect_coplanar(Line3D another) { Point3D line_a_origpt = orig_pt(); Vector3D line_a_versor = versor(); Point3D line_b_origpt = another.orig_pt(); Vector3D line_b_versor = another.versor(); // creates a triplet of coplanar, non-coincident points double delta_distance = 100.0; Vector3D displ_vector_a = line_a_versor.scale(delta_distance); Vector3D displ_vector_b = line_b_versor.scale(delta_distance); Point3D point_a = Point3D(line_a_origpt.x(), line_a_origpt.y(), line_a_origpt.z()); Point3D point_b = displ_vector_b.move_pt(point_a); Point3D point_c = displ_vector_a.move_pt(point_a); CartesianPlane colinear_plane = CartesianPlane(point_a, point_b, point_c); //code inspired to: http://geomalgorithms.com/a05-_intersect-1.html#intersect2D_2Segments() Vector3D w_vect = Vector3D( line_a_origpt, line_b_origpt ); Vector3D vers_a_perp = colinear_plane.perp_versor_in_plane(line_a_versor); double factor_numerator = - vers_a_perp.scalar_prod(w_vect); double factor_denominator = vers_a_perp.scalar_prod(line_b_versor); double factor_scaling = factor_numerator / factor_denominator; Point3D intersection_pt3d = line_b_versor.scale(factor_scaling).move_pt(line_b_origpt); return intersection_pt3d; };
void PairRegressionAnalyzerGraphic::drawLine() { Line3D line; line.SetCurrentColor(Line3D::black); for (int i=1;i<linePoints.size();i++) { line.setPoints(linePoints[i-1], linePoints[i]); line.draw(); } }
bool Box3D::intersects(const Line3D& l) const { Line3D lloc; toLocal(l,lloc); AABB3D bbloc; bbloc.bmin.setZero(); bbloc.bmax=dims; return lloc.lineIntersects(bbloc); }
template<class T> Point3<T> Plane<T>::intersectPoint(const Line3D<T> &line) const { Point3<T> planePoint = Point3<T>(normal * -d); Vector3<T> lineVector = line.getA().vector(line.getB()); T numerator = normal.dotProduct(lineVector); Vector3<T> lineAToPlanePoint = line.getA().vector(planePoint); T t = normal.dotProduct(lineAToPlanePoint) / numerator; return (line.getA() + Point3<T>(t * lineVector)); }
void TransformWidget::Drag(int dx,int dy,Camera::Viewport& viewport) { dragX += dx; dragY += dy; Ray3D r; viewport.getClickSource(dragX,dragY,r.source); viewport.getClickVector(dragX,dragY,r.direction); if(hoverItem < 0) return; else if(hoverItem == 0) { Vector3 v; viewport.getMovementVectorAtDistance(dx,dy,clickDistance,v); T.t += v; } else if(hoverItem <= 3) { //translation Line3D axisLine; axisLine.source = clickPos; axisLine.direction = Vector3(T.R.col(hoverItem-1)); Real t,u; axisLine.closestPoint(r,t,u); T.t = clickTransform.t + axisLine.direction*t; } else { Plane3D ringPlane; Vector3 axis; if(hoverItem <= 6) axis = Vector3(clickTransform.R.col(hoverItem-4)); else axis = clickAxis; Vector3 x,y; GetCanonicalBasis(axis,x,y); //find rotation to minimize distance from clicked pos to drag ray Real cx = x.dot(clickPos - T.t); Real cy = y.dot(clickPos - T.t); ringPlane.setPointNormal(T.t,axis); Real t; bool res=ringPlane.intersectsRay(r,&t); //odd... no intersection if(res==false) return; Vector3 raypos = r.source + t*r.direction - T.t; Real rx = x.dot(raypos); Real ry = y.dot(raypos); if(Sqr(rx) + Sqr(ry) < 1e-5) return; Real theta = AngleDiff(Atan2(ry,rx),Atan2(cy,cx)); AngleAxisRotation aa; aa.axis = axis; aa.angle = theta; QuaternionRotation qR,qT,qRes; qR.setAngleAxis(aa); qT.setMatrix(clickTransform.R); qRes.mul(qR,qT); qRes.getMatrix(T.R); } Refresh(); }
bool Circle3D::intersects(const Line3D& l,Real* _t) const { Plane3D p; getPlane(p); Real t; if(p.intersectsLine(l,&t)) { if(t == Inf) { //line lies in plane t = l.closestPointParameter(center); } if(_t) (*_t)=t; Point3D pt; l.eval(t,pt); return DistanceLEQ(pt,center,radius); } return false; }
//------------------------------------------------------------------------------------------------------ //function that checks if sphere collides with a line segment //------------------------------------------------------------------------------------------------------ bool Sphere3D::IsColliding(const Line3D& secondLine) const { //make use of Line3D's line-sphere collision function return (secondLine.IsColliding(*this)); }
void IKGoal::GetClosestGoalTransform(const RigidTransform& T0,RigidTransform& T) const { //fill out rotation first if(rotConstraint == RotFixed) { GetFixedGoalRotation(T.R); } else if(rotConstraint == RotAxis) { //T.R*localAxis = endRotation GetMinimalRotation(localAxis,T0.R*endRotation,T.R); //make it so orthogonal directions perform a rotation similar to T0.R Vector3 lx,ly,rx,ry,refx; GetCanonicalBasis(localAxis,lx,ly); rx = T.R*rx; ry = T.R*ry; refx = T0.R*lx; Real x = dot(refx,rx); Real y = dot(refx,ry); //find the rotation about endRotation that gets closer to this Real theta = Atan2(y,x); AngleAxisRotation aa; aa.angle = theta; aa.axis = T0.R*endRotation; Matrix3 Rrot; aa.getMatrix(Rrot); T.R = Rrot*T.R; } else T.R = T0.R; T.t = endPosition - T.R*localPosition; if(posConstraint == PosPlanar) { //find closest transform on plane to T0.t Plane3D p; p.setPointNormal(T.t,direction); p.project(T0.t,T.t); } else if(posConstraint == PosLinear) { //find closest transform on line to T0.t Line3D line; line.source = T.t; line.direction = direction; line.closestPoint(T0.t,T.t); } else if(posConstraint == PosNone) T.t = T0.t; }
void Ray3D::closestPoint(const Line3D& l,Real& t,Real& u) const { Line3D::closestPoint(l,t,u); if(t < 0) { t=0; u=l.closestPointParameter(source); } }
bool Line3D::isparallel(Line3D another) { Vector3D vers_a = versor(); Vector3D vers_b = another.versor(); if (vers_a.isodirection(vers_b)) { return true; } else { return false;}; };
bool Cylinder3D::intersects(const Line3D& line,Real* tmin,Real* tmax) const { Real axistmin,axistmax; //quick reject - infinite cylinder Vector3 src=line.source-center; const Vector3& dir=line.direction; assert(FuzzyEquals(axis.normSquared(),One)); //quadratic equation Real a,b,c; Real dv,sv; dv = dir.dot(axis); sv = src.dot(axis); a=dir.normSquared()-Sqr(dv); b=Two*(dir.dot(src)-sv*dv); c=src.normSquared()-Sqr(sv)-Sqr(radius); int nroots=quadratic(a,b,c,axistmin,axistmax); //TODO: if the line is contained within the cylinder, ignore this if(nroots == 0) return false; else if(nroots == 1) axistmax=axistmin; else if(nroots == 2) { if(axistmin > axistmax) std::swap(axistmin,axistmax); } else if(nroots == -1) return false; else return false; //what case is this? //projection of intersection pts on the cyl axis Real axisumin,axisumax; Vector3 temp; line.eval(axistmin,temp); axisumin = axis.dot(temp-center); line.eval(axistmax,temp); axisumax = axis.dot(temp-center); //now check the caps Real tc; Circle3D cir; if(axisumin < 0) { //hits a cap first if(dv > 0) //line points along axis getBase(cir); else //line points against axis getCap(cir); if(!cir.intersects(line,&tc)) return false; axistmin = tc; } if(axisumin > height) { //hits a cap last if(dv > 0) //line points along axis getCap(cir); else //line points against axis getBase(cir); if(!cir.intersects(line,&tc)) return false; axistmax = tc; } //if(axistmin > axistmax) return false; if(tmin) *tmin=axistmin; if(tmax) *tmax=axistmax; return true; }
bool PlaneGeometry::IntersectionPointParam( const Line3D &line, double &t ) const { Vector3D planeNormal = this->GetNormal(); Vector3D lineDirection = line.GetDirection(); t = planeNormal * lineDirection; if ( fabs( t ) < eps ) { return false; } Vector3D diff; diff = this->GetOrigin() - line.GetPoint(); t = ( planeNormal * diff ) / t; return true; }
bool Circle3D::intersects(const Plane3D& p) const { Plane3D cp; Line3D l; Point3D lp; int res=p.allIntersections(cp,l); switch(res) { case 0: return false; case 1: //they intersect in line l l.closestPoint(center,lp); return DistanceLEQ(center,lp,radius); break; case 2: return true; default: fprintf(stderr,"Circle3D::intersects: Shouldn't get here\n"); abort(); } return false; }
unsigned int PlaneGeometry::IntersectWithPlane2D( const PlaneGeometry* plane, Point2D& lineFrom, Point2D &lineTo ) const { Line3D crossline; if ( this->IntersectionLine( plane, crossline ) == false ) return 0; Point2D point2; Vector2D direction2; this->Map( crossline.GetPoint(), point2 ); this->Map( crossline.GetPoint(), crossline.GetDirection(), direction2 ); return Line3D::RectangleLineIntersection( 0, 0, GetExtentInMM(0), GetExtentInMM(1), point2, direction2, lineFrom, lineTo ); }
bool mitk::PlaneGeometryDataMapper2D::CutCrossLineWithReferenceGeometry(const BaseGeometry* referenceGeometry, Line3D& crossLine) { Point3D boundingBoxMin, boundingBoxMax; boundingBoxMin = referenceGeometry->GetCornerPoint(0); boundingBoxMax = referenceGeometry->GetCornerPoint(7); Point3D indexLinePoint; Vector3D indexLineDirection; referenceGeometry->WorldToIndex(crossLine.GetPoint(), indexLinePoint); referenceGeometry->WorldToIndex(crossLine.GetDirection(), indexLineDirection); referenceGeometry->WorldToIndex(boundingBoxMin, boundingBoxMin); referenceGeometry->WorldToIndex(boundingBoxMax, boundingBoxMax); Point3D point1, point2; // Then, clip this line with the (transformed) bounding box of the // reference geometry. int nIntersections = Line3D::BoxLineIntersection(boundingBoxMin[0], boundingBoxMin[1], boundingBoxMin[2], boundingBoxMax[0], boundingBoxMax[1], boundingBoxMax[2], indexLinePoint, indexLineDirection, point1, point2); if (nIntersections < 2) { return false; } referenceGeometry->IndexToWorld(point1, point1); referenceGeometry->IndexToWorld(point2, point2); crossLine.SetPoints(point1, point2); return true; }
bool mitk::PlaneGeometryDataMapper2D::CutCrossLineWithPlaneGeometry(const PlaneGeometry* planeGeometry, Line3D& crossLine) { Point2D indexLinePoint; Vector2D indexLineDirection; planeGeometry->Map(crossLine.GetPoint(), indexLinePoint); planeGeometry->Map(crossLine.GetPoint(), crossLine.GetDirection(), indexLineDirection); planeGeometry->WorldToIndex(indexLinePoint, indexLinePoint); planeGeometry->WorldToIndex(indexLineDirection, indexLineDirection); mitk::Point2D intersectionPoints[2]; // Then, clip this line with the (transformed) bounding box of the // reference geometry. int nIntersections = Line3D::RectangleLineIntersection(planeGeometry->GetBounds()[0], planeGeometry->GetBounds()[2], planeGeometry->GetBounds()[1], planeGeometry->GetBounds()[3], indexLinePoint, indexLineDirection, intersectionPoints[0], intersectionPoints[1]); if (nIntersections < 2) { return false; } planeGeometry->IndexToWorld(intersectionPoints[0], intersectionPoints[0]); planeGeometry->IndexToWorld(intersectionPoints[1], intersectionPoints[1]); Point3D point1, point2; planeGeometry->Map(intersectionPoints[0], point1); planeGeometry->Map(intersectionPoints[1], point2); crossLine.SetPoints(point1, point2); return true; }
bool PlaneGeometry::IntersectionPoint( const Line3D &line, Point3D &intersectionPoint ) const { Vector3D planeNormal = this->GetNormal(); planeNormal.Normalize(); Vector3D lineDirection = line.GetDirection(); lineDirection.Normalize(); double t = planeNormal * lineDirection; if ( fabs( t ) < eps ) { return false; } Vector3D diff; diff = this->GetOrigin() - line.GetPoint(); t = ( planeNormal * diff ) / t; intersectionPoint = line.GetPoint() + lineDirection * t; return true; }
void PFO_CubeColorBlockGL::GetCoords(int indx, SC_CoordArray& topCoords, SC_CoordArray& bottomCoords, const double& reduceFactor) { cubeData->GetBlockCoords(indx, topCoords, bottomCoords); if (reduceFactor < 0.999) { Line3D pointLine; pointLine.stPt = cubeData->GetCoord(indx); for (int k = 0; k < topCoords.Size(); k++) { pointLine.endPt = topCoords[k]; topCoords[k] = pointLine.PointOnLine(reduceFactor); pointLine.endPt = bottomCoords[k]; bottomCoords[k] = pointLine.PointOnLine(reduceFactor); } } MapCoords(topCoords); MapCoords(bottomCoords); }
toxi::geom::Vec3D toxi::geom::Triangle3D::getClosestPointTo( Vec3D & v ) { Line3D edge = Line3D( a, b ); Vec3D Rab = edge.closestPointTo( v ); Vec3D Rbc = edge.set( b, c ).closestPointTo( v ); Vec3D Rca = edge.set( c, a ).closestPointTo( v ); float dAB = v.sub( Rab ).magSquared(); float dBC = v.sub( Rbc ).magSquared(); float dCA = v.sub( Rca ).magSquared(); float min = dAB; Vec3D result = Rab; if (dBC < min) { min = dBC; result = Rbc; } if (dCA < min) { result = Rca; } return result; }
bool Line3D::iscoincident(Line3D another) { if (not isparallel(another)) { return false; }; Point3D orig_pt_a = orig_pt(); Point3D orig_pt_b = another.orig_pt(); if (orig_pt_a.is_coincident(orig_pt_b)) { return true; } else { Vector3D test_line = Vector3D(orig_pt_a, orig_pt_b); if (test_line.isodirection(versor())) { return true;} else { return false;} } };
void Manipulator::followLine(Line3D line) { //go to the right orientation posAngles angles = calculateAngles(line.getStartPoint(), A1, A2); updateOrientation(M_PI - angles.theta2 - angles.theta3); //go smoothly to startpoint; goToPositionSmoothQuintic(line.getStartPoint()); float steps = line.getLength()/STEP_SIZE_CURVE; for (int i = 0; i < steps; i++) { Point3D p( (line.getStartPoint()).x + i*(line.getDirection()).getX(), (line.getStartPoint()).y + i*(line.getDirection()).getY(), (line.getStartPoint()).z + i*(line.getDirection()).getZ() ); cout<<"Going to position"<< p << endl; goToPositionPencil(p); usleep(20000); } }
QList<double*> ROITool::getIntersectionPoints(const QList<Line3D> &polygonSegments, const Line3D &sweepLine, const OrthogonalPlane &view) { QList<double*> intersectionPoints; int sortIndex = view.getXIndex(); int heightIndex = view.getYIndex(); QList<int> indexListOfSegmentsToIntersect = getIndexOfSegmentsCrossingAtHeight(polygonSegments, sweepLine.getFirstPoint().at(heightIndex), heightIndex); foreach (int segmentIndex, indexListOfSegmentsToIntersect) { int intersectionState; double *foundPoint = MathTools::infiniteLinesIntersection(polygonSegments.at(segmentIndex).getFirstPoint().getAsDoubleArray(), polygonSegments.at(segmentIndex).getSecondPoint().getAsDoubleArray(), sweepLine.getFirstPoint().getAsDoubleArray(), sweepLine.getSecondPoint().getAsDoubleArray(), intersectionState); if (intersectionState == MathTools::LinesIntersect) { // Must sort intersections horizontally in order to be able to get voxels inside polygon correctly bool found = false; int i = 0; while (!found && i < intersectionPoints.count()) { if (foundPoint[sortIndex] > intersectionPoints.at(i)[sortIndex]) { intersectionPoints.insert(i, foundPoint); found = true; } else { ++i; } } // If not found, it means it is the greatest point and must be placed at the end if (!found) { intersectionPoints << foundPoint; } } }
LineIntersect Plane::intersect(const Line3D &line, float *t, Vec3 &pt) { float f = line.direction * normal; if (fabs(f) <= EPSLION_E5)//表示平行平面 { if (fabs(computePointInPalne(line.begin)) <= EPSLION_E5)//点在平面上 { return LINE_IN_INTERSECT_EVERYWHERE; }else { return LINE_NO_INTERSECT; } } *t = -(normal.x * line.begin.x + normal.y * line.begin.y + normal.z * line.begin.z - normal.x * p.x - normal.y * p.y - normal.z * p.z) / f; pt = line.compute(*t); if(*t <= 1 && *t >= 0) return LINE_IN_INTERSECT_SEGMENT; return LINT_OUT_INTERSECT_SEGMENT; }
bool PlaneGeometry::IsOnPlane( const Line3D &line ) const { return ( (Distance( line.GetPoint() ) < eps) && (Distance( line.GetPoint2() ) < eps) ); }
int QgsTINInterpolator::insertData( QgsFeature* f, bool zCoord, int attr, InputType type ) { if ( !f ) { return 1; } QgsGeometry* g = f->geometry(); { if ( !g ) { return 2; } } //check attribute value double attributeValue = 0; bool attributeConversionOk = false; if ( !zCoord ) { QVariant attributeVariant = f->attribute( attr ); if ( !attributeVariant.isValid() ) //attribute not found, something must be wrong (e.g. NULL value) { return 3; } attributeValue = attributeVariant.toDouble( &attributeConversionOk ); if ( !attributeConversionOk || qIsNaN( attributeValue ) ) //don't consider vertices with attributes like 'nan' for the interpolation { return 4; } } //parse WKB. It is ugly, but we cannot use the methods with QgsPoint because they don't contain z-values for 25D types bool hasZValue = false; double x, y, z; unsigned char* currentWkbPtr = g->asWkb(); //maybe a structure or break line Line3D* line = 0; QGis::WkbType wkbType = g->wkbType(); switch ( wkbType ) { case QGis::WKBPoint25D: hasZValue = true; case QGis::WKBPoint: { currentWkbPtr += ( 1 + sizeof( int ) ); x = *(( double * )( currentWkbPtr ) ); currentWkbPtr += sizeof( double ); y = *(( double * )( currentWkbPtr ) ); if ( zCoord && hasZValue ) { currentWkbPtr += sizeof( double ); z = *(( double * )( currentWkbPtr ) ); } else { z = attributeValue; } Point3D* thePoint = new Point3D( x, y, z ); if ( mTriangulation->addPoint( thePoint ) == -100 ) { return -1; } break; } case QGis::WKBMultiPoint25D: hasZValue = true; case QGis::WKBMultiPoint: { currentWkbPtr += ( 1 + sizeof( int ) ); int* npoints = ( int* )currentWkbPtr; currentWkbPtr += sizeof( int ); for ( int index = 0; index < *npoints; ++index ) { currentWkbPtr += ( 1 + sizeof( int ) ); //skip endian and point type x = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); y = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); if ( hasZValue ) //skip z-coordinate for 25D geometries { z = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); } else { z = attributeValue; } } break; } case QGis::WKBLineString25D: hasZValue = true; case QGis::WKBLineString: { if ( type != POINTS ) { line = new Line3D(); } currentWkbPtr += ( 1 + sizeof( int ) ); int* npoints = ( int* )currentWkbPtr; currentWkbPtr += sizeof( int ); for ( int index = 0; index < *npoints; ++index ) { x = *(( double * )( currentWkbPtr ) ); currentWkbPtr += sizeof( double ); y = *(( double * )( currentWkbPtr ) ); currentWkbPtr += sizeof( double ); if ( zCoord && hasZValue ) //skip z-coordinate for 25D geometries { z = *(( double * )( currentWkbPtr ) ); } else { z = attributeValue; } if ( hasZValue ) { currentWkbPtr += sizeof( double ); } if ( type == POINTS ) { //todo: handle error code -100 mTriangulation->addPoint( new Point3D( x, y, z ) ); } else { line->insertPoint( new Point3D( x, y, z ) ); } } if ( type != POINTS ) { mTriangulation->addLine( line, type == BREAK_LINES ); } break; } case QGis::WKBMultiLineString25D: hasZValue = true; case QGis::WKBMultiLineString: { currentWkbPtr += ( 1 + sizeof( int ) ); int* nlines = ( int* )currentWkbPtr; int* npoints = 0; currentWkbPtr += sizeof( int ); for ( int index = 0; index < *nlines; ++index ) { if ( type != POINTS ) { line = new Line3D(); } currentWkbPtr += ( sizeof( int ) + 1 ); npoints = ( int* )currentWkbPtr; currentWkbPtr += sizeof( int ); for ( int index2 = 0; index2 < *npoints; ++index2 ) { x = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); y = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); if ( hasZValue ) //skip z-coordinate for 25D geometries { z = *(( double* ) currentWkbPtr ); currentWkbPtr += sizeof( double ); } else { z = attributeValue; } if ( type == POINTS ) { //todo: handle error code -100 mTriangulation->addPoint( new Point3D( x, y, z ) ); } else { line->insertPoint( new Point3D( x, y, z ) ); } } if ( type != POINTS ) { mTriangulation->addLine( line, type == BREAK_LINES ); } } break; } case QGis::WKBPolygon25D: hasZValue = true; case QGis::WKBPolygon: { currentWkbPtr += ( 1 + sizeof( int ) ); int* nrings = ( int* )currentWkbPtr; currentWkbPtr += sizeof( int ); int* npoints; for ( int index = 0; index < *nrings; ++index ) { if ( type != POINTS ) { line = new Line3D(); } npoints = ( int* )currentWkbPtr; currentWkbPtr += sizeof( int ); for ( int index2 = 0; index2 < *npoints; ++index2 ) { x = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); y = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); if ( hasZValue ) //skip z-coordinate for 25D geometries { z = *(( double* )currentWkbPtr );; currentWkbPtr += sizeof( double ); } else { z = attributeValue; } if ( type == POINTS ) { //todo: handle error code -100 mTriangulation->addPoint( new Point3D( x, y, z ) ); } else { line->insertPoint( new Point3D( x, y, z ) ); } } if ( type != POINTS ) { mTriangulation->addLine( line, type == BREAK_LINES ); } } break; } case QGis::WKBMultiPolygon25D: hasZValue = true; case QGis::WKBMultiPolygon: { currentWkbPtr += ( 1 + sizeof( int ) ); int* npolys = ( int* )currentWkbPtr; int* nrings; int* npoints; currentWkbPtr += sizeof( int ); for ( int index = 0; index < *npolys; ++index ) { currentWkbPtr += ( 1 + sizeof( int ) ); //skip endian and polygon type nrings = ( int* )currentWkbPtr; currentWkbPtr += sizeof( int ); for ( int index2 = 0; index2 < *nrings; ++index2 ) { if ( type != POINTS ) { line = new Line3D(); } npoints = ( int* )currentWkbPtr; currentWkbPtr += sizeof( int ); for ( int index3 = 0; index3 < *npoints; ++index3 ) { x = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); y = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); if ( hasZValue ) //skip z-coordinate for 25D geometries { z = *(( double* )currentWkbPtr ); currentWkbPtr += sizeof( double ); } else { z = attributeValue; } if ( type == POINTS ) { //todo: handle error code -100 mTriangulation->addPoint( new Point3D( x, y, z ) ); } else { line->insertPoint( new Point3D( x, y, z ) ); } } if ( type != POINTS ) { mTriangulation->addLine( line, type == BREAK_LINES ); } } } break; } default: //should not happen... break; } return 0; }
double PlaneGeometry::Angle( const Line3D &line ) const { return vnl_math::pi_over_2 - angle( line.GetDirection().GetVnlVector(), GetMatrixColumn(2) ); }
void mitk::PlaneCutFilter::_computeIntersection(itk::Image<TPixel, VImageDimension> *image, const PlaneGeometry *plane, const Geometry3D *geometry) { typedef itk::Image<TPixel, VImageDimension> ImageType; const typename ImageType::RegionType &image_region = image->GetLargestPossibleRegion(); const typename ImageType::SizeValueType max_x = image_region.GetSize(0ul), max_y = image_region.GetSize(1ul), max_z = image_region.GetSize(2ul), img_size = max_x * max_y; TPixel *data = image->GetBufferPointer(); Point3D p1, p2; //TODO: Better solution required! TPixel casted_background_level = static_cast<TPixel>(this->m_BackgroundLevel); p1[0] = 0; p2[0] = max_x - 1ul; if (FILL == this->m_FillMode) { for (unsigned long z = 0ul; z < max_z; ++z) { p1[2] = z; p2[2] = z; for (unsigned long y = 0ul; y < max_y; ++y) { p1[1] = y; p2[1] = y; Point3D p1_t, p2_t; geometry->IndexToWorld(p1, p1_t); geometry->IndexToWorld(p2, p2_t); if (plane->IsAbove(p1_t)) { if (plane->IsAbove(p2_t)) { if (0.0f == this->m_BackgroundLevel) { memset(&data[(y * max_x) + (z * img_size)], 0, max_x * sizeof(TPixel)); } else { TPixel *subdata = &data[(y * max_x) + (z * img_size)]; for (unsigned long x = 0; x < max_x; ++x) { subdata[x] = casted_background_level; } } } else { Point3D intersection; Line3D line; line.SetPoints(p1_t, p2_t); plane->IntersectionPoint(line, intersection); geometry->WorldToIndex(intersection, intersection); if (0.0f == this->m_BackgroundLevel) { memset(&data[(y * max_x) + (z * img_size)], 0, (static_cast<unsigned long>(roundf(intersection[0])) + 1u) * sizeof(TPixel)); } else { TPixel *subdata = &data[(y * max_x) + (z * img_size)]; const unsigned long x_size = static_cast<unsigned long>(roundf(intersection[0])) + 1u; for (unsigned long x = 0; x < x_size; ++x) { subdata[x] = casted_background_level; } } } } else if (plane->IsAbove(p2_t)) { Point3D intersection; Line3D line; line.SetPoints(p1_t, p2_t); plane->IntersectionPoint(line, intersection); geometry->WorldToIndex(intersection, intersection); if (0.0f == this->m_BackgroundLevel) { unsigned long x = static_cast<unsigned long>(roundf(intersection[0])); memset(&data[x + (y * max_x) + (z * img_size)], 0, (max_x - x) * sizeof(TPixel)); } else { unsigned long x = static_cast<unsigned long>(roundf(intersection[0])); TPixel *subdata = &data[x + (y * max_x) + (z * img_size)]; const unsigned long x_size = max_x - x; for (x = 0; x < x_size; ++x) { subdata[x] = casted_background_level; } } } } } } else { for (unsigned long z = 0ul; z < max_z; ++z) { p1[2] = z; p2[2] = z; for (unsigned long y = 0ul; y < max_y; ++y) { p1[1] = y; p2[1] = y; Point3D p1_t, p2_t; geometry->IndexToWorld(p1, p1_t); geometry->IndexToWorld(p2, p2_t); if (!plane->IsAbove(p1_t)) { if (!plane->IsAbove(p2_t)) { if (0.0f == this->m_BackgroundLevel) { memset(&data[(y * max_x) + (z * img_size)], 0, max_x * sizeof(TPixel)); } else { TPixel *subdata = &data[(y * max_x) + (z * img_size)]; for (unsigned long x = 0; x < max_x; ++x) { subdata[x] = casted_background_level; } } } else { Point3D intersection; Line3D line; line.SetPoints(p1_t, p2_t); plane->IntersectionPoint(line, intersection); geometry->WorldToIndex(intersection, intersection); if (0.0f == this->m_BackgroundLevel) { memset(&data[(y * max_x) + (z * img_size)], 0, (static_cast<unsigned long>(roundf(intersection[0])) + 1u) * sizeof(TPixel)); } else { TPixel *subdata = &data[(y * max_x) + (z * img_size)]; const unsigned long x_size = static_cast<unsigned long>(roundf(intersection[0])) + 1u; for (unsigned long x = 0; x < x_size; ++x) { subdata[x] = casted_background_level; } } } } else if (!plane->IsAbove(p2_t)) { Point3D intersection; Line3D line; line.SetPoints(p1_t, p2_t); plane->IntersectionPoint(line, intersection); geometry->WorldToIndex(intersection, intersection); if (0.0f == this->m_BackgroundLevel) { unsigned long x = static_cast<unsigned long>(roundf(intersection[0])); memset(&data[x + (y * max_x) + (z * img_size)], 0, (max_x - x) * sizeof(TPixel)); } else { unsigned long x = static_cast<unsigned long>(roundf(intersection[0])); TPixel *subdata = &data[x + (y * max_x) + (z * img_size)]; const unsigned long x_size = max_x - x; for (x = 0; x < x_size; ++x) { subdata[x] = casted_background_level; } } } } } } }
int main(int /*argc*/, char** /*argv*/) { Line3D l0; std::cout << "l0: " << std::endl; printPlueckerLine(std::cout, l0); std::cout << std::endl; Vector6d v; v << 1.0, 1.0, -0.3, 0.5, 0.2, 0.3; Line3D l1(v); l1.normalize(); std::cout << "v: "; printVector(std::cout, v); std::cout << std::endl; std::cout << "l1: " << std::endl; printPlueckerLine(std::cout, l1); std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl; std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl; std::cout << std::endl; Line3D l2(l1); std::cout << "l2: " << std::endl; printPlueckerLine(std::cout, l2); std::cout << std::endl; Eigen::Vector4d mv = l2.ominus(l1); Eigen::Quaterniond q(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z()); Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0); double yaw = euler_angles[0]; double pitch = euler_angles[1]; double roll = euler_angles[2]; std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl; std::cout << std::endl; v << 0.0, 0.0, 1.0, 0.5, 0.5, 0.0; l1 = Line3D(v); l1.normalize(); std::cout << "l1: " << std::endl; printPlueckerLine(std::cout, l1); std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl; std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl; std::cout << std::endl; v << 0.0, 0.0, 1.0, 0.5, -0.5, 0.0; l2 = Line3D(v); l2.normalize(); std::cout << "l2: " << std::endl; printPlueckerLine(std::cout, l2); std::cout << "azimuth: " << g2o::internal::getAzimuth(l2.d()) << std::endl; std::cout << "elevation: " << g2o::internal::getElevation(l2.d()) << std::endl; std::cout << std::endl; mv = l2.ominus(l1); q = Eigen::Quaterniond(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z()); euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0); yaw = euler_angles[0]; pitch = euler_angles[1]; roll = euler_angles[2]; std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl; std::cout << std::endl; Line3D l3 = Line3D(l1); std::cout << "l3: " << std::endl; printPlueckerLine(std::cout, l3); l3.oplus(mv); std::cout << "l3 oplus mv: " << std::endl; printPlueckerLine(std::cout, l3); std::cout << std::endl; std::vector<Line3D> l; v << 0.0, 0.0, 1.0, 1.0, 0.0, 0.0; Line3D ll = Line3D(v); ll.normalize(); l.push_back(ll); v << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0; ll = Line3D(v); ll.normalize(); l.push_back(ll); v << 0.0, 0.0, 1.0, 0.0, 0.0, 1.0; ll = Line3D(v); ll.normalize(); l.push_back(ll); for(size_t i = 0; i < l.size(); ++i) { Line3D& line = l[i]; std::cout << "line: " << line.d()[0] << " " << line.d()[1] << " " << line.d()[2] << " " << line.w()[0] << " " << line.w()[1] << " " << line.w()[2] << std::endl; } std::cout << std::endl; Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); T.translation().x() = 0.9; std::cout << "R: " << std::endl << T.linear() << std::endl; std::cout << "t: " << std::endl << T.translation() << std::endl; std::cout << std::endl; for(size_t i = 0; i < l.size(); ++i) { Line3D& line = l[i]; line = T * line; std::cout << "line: " << line.d()[0] << " " << line.d()[1] << " " << line.d()[2] << " " << line.w()[0] << " " << line.w()[1] << " " << line.w()[2] << std::endl; } std::cout << std::endl; return 0; }