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;
}
