42 const XYZPoint & to0,
const XYZPoint & to1,
const XYZPoint & to2 )
46 XYZVector
x1,y1,z1,
x2,y2,z2;
47 x1 = (fr1 - fr0).
Unit();
48 y1 = (fr2 - fr0).
Unit();
49 x2 = (to1 - to0).
Unit();
50 y2 = (to2 - to0).
Unit();
59 std::cerr <<
"Transform3D: Error : zero angle between axes" << std::endl;
63 std::cerr <<
"Transform3D: Warning: angles between axes are not equal" 75 double x1x = x1.
x();
double x1y = x1.
y();
double x1z = x1.
z();
76 double y1x = y1.
x();
double y1y = y1.
y();
double y1z = y1.
z();
77 double z1x = z1.
x();
double z1y = z1.
y();
double z1z = z1.
z();
79 double x2x = x2.
x();
double x2y = x2.
y();
double x2z = x2.
z();
80 double y2x = y2.
x();
double y2y = y2.
y();
double y2z = y2.
z();
81 double z2x = z2.
x();
double z2y = z2.
y();
double z2z = z2.
z();
83 double detxx = (y1y *z1z - z1y *y1z );
84 double detxy = -(y1x *z1z - z1x *y1z );
85 double detxz = (y1x *z1y - z1x *y1y );
86 double detyx = -(x1y *z1z - z1y *x1z );
87 double detyy = (x1x *z1z - z1x *x1z );
88 double detyz = -(x1x *z1y - z1x *x1y );
89 double detzx = (x1y *y1z - y1y *x1z );
90 double detzy = -(x1x *y1z - y1x *x1z );
91 double detzz = (x1x *y1y - y1x *x1y );
93 double txx = x2x *detxx + y2x *detyx + z2x *detzx;
94 double txy = x2x *detxy + y2x *detyy + z2x *detzy;
95 double txz = x2x *detxz + y2x *detyz + z2x *detzz;
96 double tyx = x2y *detxx + y2y *detyx + z2y *detzx;
97 double tyy = x2y *detxy + y2y *detyy + z2y *detzy;
98 double tyz = x2y *detxz + y2y *detyz + z2y *detzz;
99 double tzx = x2z *detxx + y2z *detyx + z2z *detzx;
100 double tzy = x2z *detxy + y2z *detyy + z2z *detzy;
101 double tzz = x2z *detxz + y2z *detyz + z2z *detzz;
105 double dx1 = fr0.
x(), dy1 = fr0.
y(), dz1 = fr0.
z();
106 double dx2 = to0.
x(), dy2 = to0.
y(), dz2 = to0.
z();
109 tyx, tyy, tyz, dy2-tyx*dx1-tyy*dy1-tyz*dz1,
110 tzx, tzy, tzz, dz2-tzx*dx1-tzy*dy1-tzz*dz1);
127 double det = fM[
kXX]*detxx - fM[
kXY]*detxy + fM[
kXZ]*detxz;
129 std::cerr <<
"Transform3D::inverse error: zero determinant" << std::endl;
132 det = 1./det; detxx *= det; detxy *= det; detxz *= det;
133 double detyx = (fM[
kXY]*fM[
kZZ] - fM[
kXZ]*fM[
kZY] )*det;
134 double detyy = (fM[
kXX]*fM[
kZZ] - fM[
kXZ]*fM[
kZX] )*det;
135 double detyz = (fM[
kXX]*fM[
kZY] - fM[
kXY]*fM[
kZX] )*det;
136 double detzx = (fM[
kXY]*fM[
kYZ] - fM[
kXZ]*fM[
kYY] )*det;
137 double detzy = (fM[
kXX]*fM[
kYZ] - fM[
kXZ]*fM[
kYX] )*det;
138 double detzz = (fM[
kXX]*fM[
kYY] - fM[
kXY]*fM[
kYX] )*det;
140 (detxx, -detyx, detzx, -detxx*fM[
kDX]+detyx*fM[
kDY]-detzx*fM[
kDZ],
141 -detxy, detyy, -detzy, detxy*fM[
kDX]-detyy*fM[
kDY]+detzy*fM[kDZ],
142 detxz, -detyz, detzz, -detxz*fM[
kDX]+detyz*fM[
kDY]-detzz*fM[kDZ]);
164 for (
int i = 0; i < 3; ++i)
167 for (
int i = 0; i < 3; ++i)
168 fM[
kYX+i] = rotData[3+i];
170 for (
int i = 0; i < 3; ++i)
171 fM[
kZX+i] = rotData[6+i];
176 fM[
kDX] = vecData[0];
177 fM[
kDY] = vecData[1];
178 fM[
kDZ] = vecData[2];
187 for (
int i = 0; i < 3; ++i) {
188 for (
int j = 0; j < 3; ++j)
189 fM[4*i + j] = rotData[3*i+j];
210 XYZPoint p( - d * n.
X() , - d *n.
Y(), -d *n.
Z() );
211 return Plane3D (
operator() (n),
operator() (p) );
221 os <<
"\n" << m[0] <<
" " << m[1] <<
" " << m[2] <<
" " << m[3] ;
222 os <<
"\n" << m[4] <<
" " << m[5] <<
" " << m[6] <<
" " << m[7] ;
223 os <<
"\n" << m[8] <<
" " << m[9] <<
" " << m[10]<<
" " << m[11] <<
"\n";
Class describing a geometrical plane in 3 dimensions.
This namespace contains pre-defined functions to be used in conjuction with TExecutor::Map and TExecu...
Scalar Y() const
Cartesian Y, converting if necessary from internal coordinate system.
PositionVector3D< Cartesian3D< double >, DefaultCoordinateSystemTag > XYZPoint
3D Point based on the cartesian coordinates x,y,z in double precision
void GetCoordinates(Scalar &a, Scalar &b, Scalar &c) const
get internal data into 3 Scalar numbers
Scalar Dot(const DisplacementVector3D< OtherCoords, Tag > &v) const
Return the scalar (dot) product of two displacement vectors.
std::ostream & operator<<(std::ostream &os, const AxisAngle &a)
Stream Output and Input.
static const double x2[5]
Vector Normal() const
Return normal vector to the plane as Cartesian DisplacementVector.
Scalar X() const
Cartesian X, converting if necessary from internal coordinate system.
Scalar HesseDistance() const
Return the Hesse Distance (distance from the origin) of the plane or the d coefficient expressed in n...
VecExpr< UnaryOp< Fabs< T >, VecExpr< A, T, D >, T >, T, D > fabs(const VecExpr< A, T, D > &rhs)
Rotation class with the (3D) rotation represented by a 3x3 orthogonal matrix.
void GetComponents(ForeignVector &v1, ForeignVector &v2, ForeignVector &v3) const
Get components into three vectors which will be the (orthonormal) columns of the rotation matrix...
static const double x1[5]
Scalar Z() const
Cartesian Z, converting if necessary from internal coordinate system.
DisplacementVector3D< Cartesian3D< double >, DefaultCoordinateSystemTag > XYZVector
3D Vector based on the cartesian coordinates x,y,z in double precision
Namespace for new Math classes and functions.
DisplacementVector3D Cross(const DisplacementVector3D< OtherCoords, Tag > &v) const
Return vector (cross) product of two displacement vectors, as a vector in the coordinate system of th...
SVector< T, D > Unit(const SVector< T, D > &rhs)
Unit.