86 x1 = (fr1 - fr0).unit();
87 y1 = (fr2 - fr0).unit();
88 x2 = (to1 - to0).unit();
89 y2 = (to2 - to0).unit();
97 if (std::abs(1.0-cos1) <= 0.000001 || std::abs(1.0-cos2) <= 0.000001) {
98 std::cerr <<
"Transform3D: zero angle between axes" << std::endl;
101 if (std::abs(cos1-cos2) > 0.000001) {
102 std::cerr <<
"Transform3D: angles between axes are not equal"
114 double detxx = (y1.
y()*z1.
z() - z1.
y()*y1.
z());
115 double detxy = -(y1.
x()*z1.
z() - z1.
x()*y1.
z());
116 double detxz = (y1.
x()*z1.
y() - z1.
x()*y1.
y());
117 double detyx = -(x1.
y()*z1.
z() - z1.
y()*x1.
z());
118 double detyy = (x1.
x()*z1.
z() - z1.
x()*x1.
z());
119 double detyz = -(x1.
x()*z1.
y() - z1.
x()*x1.
y());
120 double detzx = (x1.
y()*y1.
z() - y1.
y()*x1.
z());
121 double detzy = -(x1.
x()*y1.
z() - y1.
x()*x1.
z());
122 double detzz = (x1.
x()*y1.
y() - y1.
x()*x1.
y());
124 double txx = x2.
x()*detxx + y2.
x()*detyx + z2.
x()*detzx;
125 double txy = x2.
x()*detxy + y2.
x()*detyy + z2.
x()*detzy;
126 double txz = x2.
x()*detxz + y2.
x()*detyz + z2.
x()*detzz;
127 double tyx = x2.
y()*detxx + y2.
y()*detyx + z2.
y()*detzx;
128 double tyy = x2.
y()*detxy + y2.
y()*detyy + z2.
y()*detzy;
129 double tyz = x2.
y()*detxz + y2.
y()*detyz + z2.
y()*detzz;
130 double tzx = x2.
z()*detxx + y2.
z()*detyx + z2.
z()*detzx;
131 double tzy = x2.
z()*detxy + y2.
z()*detyy + z2.
z()*detzy;
132 double tzz = x2.
z()*detxz + y2.
z()*detyz + z2.
z()*detzz;
136 double dx1 = fr0.
x(), dy1 = fr0.
y(), dz1 = fr0.
z();
137 double dx2 = to0.
x(), dy2 = to0.
y(), dz2 = to0.
z();
139 setTransform(txx, txy, txz, dx2-txx*dx1-txy*dy1-txz*dz1,
140 tyx, tyy, tyz, dy2-tyx*dx1-tyy*dy1-tyz*dz1,
141 tzx, tzy, tzz, dz2-tzx*dx1-tzy*dy1-tzz*dz1);
159 double det =
xx_*detxx -
xy_*detxy +
xz_*detxz;
161 std::cerr <<
"Transform3D::inverse error: zero determinant" << std::endl;
164 det = 1./det; detxx *= det; detxy *= det; detxz *= det;
172 (detxx, -detyx, detzx, -detxx*
dx_+detyx*
dy_-detzx*
dz_,
173 -detxy, detyy, -detzy, detxy*
dx_-detyy*
dy_+detzy*
dz_,
174 detxz, -detyz, detzz, -detxz*
dx_+detyz*
dy_-detzz*
dz_);
193 double sx = std::sqrt(xx_*xx_ + yx_*yx_ + zx_*zx_);
194 double sy = std::sqrt(xy_*xy_ + yy_*yy_ + zy_*zy_);
195 double sz = std::sqrt(xz_*xz_ + yz_*yz_ + zz_*zz_);
197 if (xx_*(yy_*zz_-yz_*zy_) -
198 xy_*(yx_*zz_-yz_*zx_) +
199 xz_*(yx_*zy_-yy_*zx_) < 0) sz = -sz;
200 scale.setTransform(sx,0,0,0, 0,sy,0,0, 0,0,sz,0);
201 rotation.setTransform(xx_/sx,xy_/sy,xz_/sz,0,
202 yx_/sx,yy_/sy,yz_/sz,0,
203 zx_/sx,zy_/sy,zz_/sz,0);
204 translation.setTransform(1,0,0,dx_, 0,1,0,dy_, 0,0,1,dz_);
250 double cx = p2.
x()-p1.
x(), cy = p2.
y()-p1.
y(), cz = p2.
z()-p1.
z();
251 double ll = std::sqrt(cx*cx + cy*cy + cz*cz);
253 std::cerr <<
"Rotate3D: zero axis" << std::endl;
255 double cosa = std::cos(
a), sina = std::sin(
a);
256 cx /= ll; cy /= ll; cz /= ll;
258 double txx = cosa + (1-cosa)*cx*cx;
259 double txy = (1-cosa)*cx*cy - sina*cz;
260 double txz = (1-cosa)*cx*cz + sina*cy;
262 double tyx = (1-cosa)*cy*cx + sina*cz;
263 double tyy = cosa + (1-cosa)*cy*cy;
264 double tyz = (1-cosa)*cy*cz - sina*cx;
266 double tzx = (1-cosa)*cz*cx - sina*cy;
267 double tzy = (1-cosa)*cz*cy + sina*cx;
268 double tzz = cosa + (1-cosa)*cz*cz;
270 double tdx = p1.
x(), tdy = p1.
y(), tdz = p1.
z();
272 setTransform(txx, txy, txz, tdx-txx*tdx-txy*tdy-txz*tdz,
273 tyx, tyy, tyz, tdy-tyx*tdx-tyy*tdy-tyz*tdz,
274 tzx, tzy, tzz, tdz-tzx*tdx-tzy*tdy-tzz*tdz);
290 double ll =
a*
a+
b*
b+c*c;
292 std::cerr <<
"Reflect3D: zero normal" << std::endl;
296 double aa =
a*
a*ll, ab =
a*
b*ll, ac =
a*c*ll, ad =
a*d*ll,
297 bb =
b*
b*ll, bc =
b*c*ll, bd =
b*d*ll,
298 cc = c*c*ll, cd = c*d*ll;
300 -ab-ab, aa-bb+cc, -bc-bc, -bd-bd,
301 -ac-ac, -bc-bc, aa+bb-cc, -cd-cd);