frame.cpp
1 /****************************************************************************
2 
3  Copyright (C) 2002-2008 Gilles Debunne. All rights reserved.
4 
5  This file is part of the QGLViewer library version 2.3.10.
6 
7  http://www.libqglviewer.com - contact@libqglviewer.com
8 
9  This file may be used under the terms of the GNU General Public License
10  versions 2.0 or 3.0 as published by the Free Software Foundation and
11  appearing in the LICENSE file included in the packaging of this file.
12  In addition, as a special exception, Gilles Debunne gives you certain
13  additional rights, described in the file GPL_EXCEPTION in this package.
14 
15  libQGLViewer uses dual licensing. Commercial/proprietary software must
16  purchase a libQGLViewer Commercial License.
17 
18  This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
19  WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
20 
21 *****************************************************************************/
22 
23 #include "domUtils.h"
24 #include "frame.h"
25 #include <math.h>
26 
27 using namespace qglviewer;
28 using namespace std;
29 
30 
36  : constraint_(NULL), referenceFrame_(NULL)
37 {}
38 
45 Frame::Frame(const Vec& position, const Quaternion& orientation)
46  : t_(position), q_(orientation), constraint_(NULL), referenceFrame_(NULL)
47 {}
48 
55 {
56  // Automatic compiler generated version would not emit the modified() signals as is done in
57  // setTranslationAndRotation.
59  setConstraint(frame.constraint());
61  return *this;
62 }
63 
68 Frame::Frame(const Frame& frame)
69  : QObject()
70 {
71  (*this) = frame;
72 }
73 
75 
123 const GLdouble* Frame::matrix() const
124 {
125  static GLdouble m[4][4];
126  getMatrix(m);
127  return (const GLdouble*)(m);
128 }
129 
131 void Frame::getMatrix(GLdouble m[4][4]) const
132 {
133  q_.getMatrix(m);
134 
135  m[3][0] = t_[0];
136  m[3][1] = t_[1];
137  m[3][2] = t_[2];
138 }
139 
141 void Frame::getMatrix(GLdouble m[16]) const
142 {
143  q_.getMatrix(m);
144 
145  m[12] = t_[0];
146  m[13] = t_[1];
147  m[14] = t_[2];
148 }
149 
165 {
166  Frame fr(-(q_.inverseRotate(t_)), q_.inverse());
168  return fr;
169 }
170 
196 const GLdouble* Frame::worldMatrix() const
197 {
198  // This test is done for efficiency reasons (creates lots of temp objects otherwise).
199  if (referenceFrame())
200  {
201  static Frame fr;
202  fr.setTranslation(position());
203  fr.setRotation(orientation());
204  return fr.matrix();
205  }
206  else
207  return matrix();
208 }
209 
211 void Frame::getWorldMatrix(GLdouble m[4][4]) const
212 {
213  const GLdouble* mat = worldMatrix();
214  for (int i=0; i<4; ++i)
215  for (int j=0; j<4; ++j)
216  m[i][j] = mat[i*4+j];
217 }
218 
220 void Frame::getWorldMatrix(GLdouble m[16]) const
221 {
222  const GLdouble* mat = worldMatrix();
223  for (int i=0; i<16; ++i)
224  m[i] = mat[i];
225 }
226 
228 void Frame::setFromMatrix(const GLdouble m[4][4])
229 {
230  if (fabs(m[3][3]) < 1E-8)
231  {
232  qWarning("Frame::setFromMatrix: Null homogeneous coefficient");
233  return;
234  }
235 
236  double rot[3][3];
237  for (int i=0; i<3; ++i)
238  {
239  t_[i] = m[3][i] / m[3][3];
240  for (int j=0; j<3; ++j)
241  // Beware of the transposition (OpenGL to European math)
242  rot[i][j] = m[j][i] / m[3][3];
243  }
244  q_.setFromRotationMatrix(rot);
245  Q_EMIT modified();
246 }
247 
272 void Frame::setFromMatrix(const GLdouble m[16])
273 {
274  GLdouble mat[4][4];
275  for (int i=0; i<4; ++i)
276  for (int j=0; j<4; ++j)
277  mat[i][j] = m[i*4+j];
278  setFromMatrix(mat);
279 }
280 
282 
283 
285 void Frame::setTranslation(float x, float y, float z)
286 {
287  setTranslation(Vec(x, y, z));
288 }
289 
291 void Frame::getTranslation(float& x, float& y, float& z) const
292 {
293  const Vec t = translation();
294  x = t[0];
295  y = t[1];
296  z = t[2];
297 }
298 
300 void Frame::setRotation(double q0, double q1, double q2, double q3)
301 {
302  setRotation(Quaternion(q0, q1, q2, q3));
303 }
304 
308 void Frame::getRotation(double& q0, double& q1, double& q2, double& q3) const
309 {
310  const Quaternion q = rotation();
311  q0 = q[0];
312  q1 = q[1];
313  q2 = q[2];
314  q3 = q[3];
315 }
316 
318 
327 void Frame::translate(const Vec& t)
328 {
329  Vec tbis = t;
330  translate(tbis);
331 }
332 
336 {
337  if (constraint())
338  constraint()->constrainTranslation(t, this);
339  t_ += t;
340  Q_EMIT modified();
341 }
342 
344 void Frame::translate(float x, float y, float z)
345 {
346  Vec t(x,y,z);
347  translate(t);
348 }
349 
351 void Frame::translate(float& x, float& y, float& z)
352 {
353  Vec t(x,y,z);
354  translate(t);
355  x = t[0];
356  y = t[1];
357  z = t[2];
358 }
359 
368 void Frame::rotate(const Quaternion& q)
369 {
370  Quaternion qbis = q;
371  rotate(qbis);
372 }
373 
377 {
378  if (constraint())
379  constraint()->constrainRotation(q, this);
380  q_ *= q;
381  q_.normalize(); // Prevents numerical drift
382  Q_EMIT modified();
383 }
384 
386 void Frame::rotate(double& q0, double& q1, double& q2, double& q3)
387 {
388  Quaternion q(q0,q1,q2,q3);
389  rotate(q);
390  q0 = q[0];
391  q1 = q[1];
392  q2 = q[2];
393  q3 = q[3];
394 }
395 
397 void Frame::rotate(double q0, double q1, double q2, double q3)
398 {
399  Quaternion q(q0,q1,q2,q3);
400  rotate(q);
401 }
402 
414 void Frame::rotateAroundPoint(Quaternion& rotation, const Vec& point)
415 {
416  if (constraint())
417  constraint()->constrainRotation(rotation, this);
418  q_ *= rotation;
419  q_.normalize(); // Prevents numerical drift
420  Vec trans = point + Quaternion(inverseTransformOf(rotation.axis()), rotation.angle()).rotate(position()-point) - t_;
421  if (constraint())
422  constraint()->constrainTranslation(trans, this);
423  t_ += trans;
424  Q_EMIT modified();
425 }
426 
429 void Frame::rotateAroundPoint(const Quaternion& rotation, const Vec& point)
430 {
431  Quaternion rot = rotation;
432  rotateAroundPoint(rot, point);
433 }
434 
436 
443 void Frame::setPosition(const Vec& position)
444 {
445  if (referenceFrame())
447  else
448  setTranslation(position);
449 }
450 
452 void Frame::setPosition(float x, float y, float z)
453 {
454  setPosition(Vec(x, y, z));
455 }
456 
462 void Frame::setPositionAndOrientation(const Vec& position, const Quaternion& orientation)
463 {
464  if (referenceFrame())
465  {
466  t_ = referenceFrame()->coordinatesOf(position);
468  }
469  else
470  {
471  t_ = position;
472  q_ = orientation;
473  }
474  Q_EMIT modified();
475 }
476 
477 
483 void Frame::setTranslationAndRotation(const Vec& translation, const Quaternion& rotation)
484 {
485  t_ = translation;
486  q_ = rotation;
487  Q_EMIT modified();
488 }
489 
490 
492 void Frame::getPosition(float& x, float& y, float& z) const
493 {
494  Vec p = position();
495  x = p.x;
496  y = p.y;
497  z = p.z;
498 }
499 
505 void Frame::setOrientation(const Quaternion& orientation)
506 {
507  if (referenceFrame())
508  setRotation(referenceFrame()->orientation().inverse() * orientation);
509  else
510  setRotation(orientation);
511 }
512 
514 void Frame::setOrientation(double q0, double q1, double q2, double q3)
515 {
516  setOrientation(Quaternion(q0, q1, q2, q3));
517 }
518 
526 void Frame::getOrientation(double& q0, double& q1, double& q2, double& q3) const
527 {
528  Quaternion o = orientation();
529  q0 = o[0];
530  q1 = o[1];
531  q2 = o[2];
532  q3 = o[3];
533 }
534 
538 {
539  Quaternion res = rotation();
540  const Frame* fr = referenceFrame();
541  while (fr != NULL)
542  {
543  res = fr->rotation() * res;
544  fr = fr->referenceFrame();
545  }
546  return res;
547 }
548 
549 
551 
557 {
558  Vec deltaT = translation - this->translation();
559  if (constraint())
560  constraint()->constrainTranslation(deltaT, this);
561 
562  setTranslation(this->translation() + deltaT);
563  translation = this->translation();
564 }
565 
571 {
572  Quaternion deltaQ = this->rotation().inverse() * rotation;
573  if (constraint())
574  constraint()->constrainRotation(deltaQ, this);
575 
576  // Prevent numerical drift
577  deltaQ.normalize();
578 
579  setRotation(this->rotation() * deltaQ);
580  q_.normalize();
581  rotation = this->rotation();
582 }
583 
587 {
588  Vec deltaT = translation - this->translation();
589  Quaternion deltaQ = this->rotation().inverse() * rotation;
590 
591  if (constraint())
592  {
593  constraint()->constrainTranslation(deltaT, this);
594  constraint()->constrainRotation(deltaQ, this);
595  }
596 
597  // Prevent numerical drift
598  deltaQ.normalize();
599 
600  t_ += deltaT;
601  q_ *= deltaQ;
602  q_.normalize();
603 
604  translation = this->translation();
605  rotation = this->rotation();
606 
607  Q_EMIT modified();
608 }
609 
613 {
614  if (referenceFrame())
615  position = referenceFrame()->coordinatesOf(position);
616 
618 }
619 
623 {
624  if (referenceFrame())
625  orientation = referenceFrame()->orientation().inverse() * orientation;
626 
627  setRotationWithConstraint(orientation);
628 }
629 
633 {
634  if (referenceFrame())
635  {
636  position = referenceFrame()->coordinatesOf(position);
637  orientation = referenceFrame()->orientation().inverse() * orientation;
638  }
639  setTranslationAndRotationWithConstraint(position, orientation);
640 }
641 
642 
644 
656 void Frame::setReferenceFrame(const Frame* const refFrame)
657 {
659  qWarning("Frame::setReferenceFrame would create a loop in Frame hierarchy");
660  else
661  {
662  bool identical = (referenceFrame_ == refFrame);
663  referenceFrame_ = refFrame;
664  if (!identical)
665  Q_EMIT modified();
666  }
667 }
668 
672 {
673  const Frame* f = frame;
674  while (f != NULL)
675  {
676  if (f == this)
677  return true;
678  f = f->referenceFrame();
679  }
680  return false;
681 }
682 
684 
693 Vec Frame::coordinatesOf(const Vec& src) const
694 {
695  if (referenceFrame())
697  else
698  return localCoordinatesOf(src);
699 }
700 
707 {
708  const Frame* fr = this;
709  Vec res = src;
710  while (fr != NULL)
711  {
712  res = fr->localInverseCoordinatesOf(res);
713  fr = fr->referenceFrame();
714  }
715  return res;
716 }
717 
723 {
724  return rotation().inverseRotate(src - translation());
725 }
726 
732 {
733  return rotation().rotate(src) + translation();
734 }
735 
740 Vec Frame::coordinatesOfFrom(const Vec& src, const Frame* const from) const
741 {
742  if (this == from)
743  return src;
744  else
745  if (referenceFrame())
747  else
748  return localCoordinatesOf(from->inverseCoordinatesOf(src));
749 }
750 
755 Vec Frame::coordinatesOfIn(const Vec& src, const Frame* const in) const
756 {
757  const Frame* fr = this;
758  Vec res = src;
759  while ((fr != NULL) && (fr != in))
760  {
761  res = fr->localInverseCoordinatesOf(res);
762  fr = fr->referenceFrame();
763  }
764 
765  if (fr != in)
766  // in was not found in the branch of this, res is now expressed in the world
767  // coordinate system. Simply convert to in coordinate system.
768  res = in->coordinatesOf(res);
769 
770  return res;
771 }
772 
774 
776 void Frame::getCoordinatesOf(const float src[3], float res[3]) const
777 {
778  const Vec r = coordinatesOf(Vec(src));
779  for (int i=0; i<3 ; ++i)
780  res[i] = r[i];
781 }
782 
784 void Frame::getInverseCoordinatesOf(const float src[3], float res[3]) const
785 {
786  const Vec r = inverseCoordinatesOf(Vec(src));
787  for (int i=0; i<3 ; ++i)
788  res[i] = r[i];
789 }
790 
792 void Frame::getLocalCoordinatesOf(const float src[3], float res[3]) const
793 {
794  const Vec r = localCoordinatesOf(Vec(src));
795  for (int i=0; i<3 ; ++i)
796  res[i] = r[i];
797 }
798 
800 void Frame::getLocalInverseCoordinatesOf(const float src[3], float res[3]) const
801 {
802  const Vec r = localInverseCoordinatesOf(Vec(src));
803  for (int i=0; i<3 ; ++i)
804  res[i] = r[i];
805 }
806 
808 void Frame::getCoordinatesOfIn(const float src[3], float res[3], const Frame* const in) const
809 {
810  const Vec r = coordinatesOfIn(Vec(src), in);
811  for (int i=0; i<3 ; ++i)
812  res[i] = r[i];
813 }
814 
816 void Frame::getCoordinatesOfFrom(const float src[3], float res[3], const Frame* const from) const
817 {
818  const Vec r = coordinatesOfFrom(Vec(src), from);
819  for (int i=0; i<3 ; ++i)
820  res[i] = r[i];
821 }
822 
823 
825 
834 Vec Frame::transformOf(const Vec& src) const
835 {
836  if (referenceFrame())
838  else
839  return localTransformOf(src);
840 }
841 
848 {
849  const Frame* fr = this;
850  Vec res = src;
851  while (fr != NULL)
852  {
853  res = fr->localInverseTransformOf(res);
854  fr = fr->referenceFrame();
855  }
856  return res;
857 }
858 
863 Vec Frame::localTransformOf(const Vec& src) const
864 {
865  return rotation().inverseRotate(src);
866 }
867 
873 {
874  return rotation().rotate(src);
875 }
876 
881 Vec Frame::transformOfFrom(const Vec& src, const Frame* const from) const
882 {
883  if (this == from)
884  return src;
885  else
886  if (referenceFrame())
887  return localTransformOf(referenceFrame()->transformOfFrom(src, from));
888  else
889  return localTransformOf(from->inverseTransformOf(src));
890 }
891 
896 Vec Frame::transformOfIn(const Vec& src, const Frame* const in) const
897 {
898  const Frame* fr = this;
899  Vec res = src;
900  while ((fr != NULL) && (fr != in))
901  {
902  res = fr->localInverseTransformOf(res);
903  fr = fr->referenceFrame();
904  }
905 
906  if (fr != in)
907  // in was not found in the branch of this, res is now expressed in the world
908  // coordinate system. Simply convert to in coordinate system.
909  res = in->transformOf(res);
910 
911  return res;
912 }
913 
915 
917 void Frame::getTransformOf(const float src[3], float res[3]) const
918 {
919  Vec r = transformOf(Vec(src));
920  for (int i=0; i<3 ; ++i)
921  res[i] = r[i];
922 }
923 
925 void Frame::getInverseTransformOf(const float src[3], float res[3]) const
926 {
927  Vec r = inverseTransformOf(Vec(src));
928  for (int i=0; i<3 ; ++i)
929  res[i] = r[i];
930 }
931 
933 void Frame::getLocalTransformOf(const float src[3], float res[3]) const
934 {
935  Vec r = localTransformOf(Vec(src));
936  for (int i=0; i<3 ; ++i)
937  res[i] = r[i];
938 }
939 
941 void Frame::getLocalInverseTransformOf(const float src[3], float res[3]) const
942 {
943  Vec r = localInverseTransformOf(Vec(src));
944  for (int i=0; i<3 ; ++i)
945  res[i] = r[i];
946 }
947 
949 void Frame::getTransformOfIn(const float src[3], float res[3], const Frame* const in) const
950 {
951  Vec r = transformOfIn(Vec(src), in);
952  for (int i=0; i<3 ; ++i)
953  res[i] = r[i];
954 }
955 
957 void Frame::getTransformOfFrom(const float src[3], float res[3], const Frame* const from) const
958 {
959  Vec r = transformOfFrom(Vec(src), from);
960  for (int i=0; i<3 ; ++i)
961  res[i] = r[i];
962 }
963 
965 
985 QDomElement Frame::domElement(const QString& name, QDomDocument& document) const
986 {
987  // TODO: use translation and rotation instead when referenceFrame is coded...
988  QDomElement e = document.createElement(name);
989  e.appendChild(position().domElement("position", document));
990  e.appendChild(orientation().domElement("orientation", document));
991  return e;
992 }
993 
1002 void Frame::initFromDOMElement(const QDomElement& element)
1003 {
1004  // TODO: use translation and rotation instead when referenceFrame is coded...
1005 
1006  // Reset default values. Attention: destroys constraint.
1007  // *this = Frame();
1008  // This instead ? Better : what is not set is not changed.
1009  // setPositionAndOrientation(Vec(), Quaternion());
1010 
1011  QDomElement child=element.firstChild().toElement();
1012  while (!child.isNull())
1013  {
1014  if (child.tagName() == "position")
1015  setPosition(Vec(child));
1016  if (child.tagName() == "orientation")
1018 
1019  child = child.nextSibling().toElement();
1020  }
1021 }
1022 
1024 
1045 void Frame::alignWithFrame(const Frame* const frame, bool move, float threshold)
1046 {
1047  Vec directions[2][3];
1048  for (int d=0; d<3; ++d)
1049  {
1050  Vec dir((d==0)? 1.0 : 0.0, (d==1)? 1.0 : 0.0, (d==2)? 1.0 : 0.0);
1051  if (frame)
1052  directions[0][d] = frame->inverseTransformOf(dir);
1053  else
1054  directions[0][d] = dir;
1055  directions[1][d] = inverseTransformOf(dir);
1056  }
1057 
1058  float maxProj = 0.0f;
1059  float proj;
1060  unsigned short index[2];
1061  index[0] = index[1] = 0;
1062  for (int i=0; i<3; ++i)
1063  for (int j=0; j<3; ++j)
1064  if ( (proj=fabs(directions[0][i]*directions[1][j])) >= maxProj )
1065  {
1066  index[0] = i;
1067  index[1] = j;
1068  maxProj = proj;
1069  }
1070 
1071  Frame old;
1072  old=*this;
1073 
1074  float coef = directions[0][index[0]] * directions[1][index[1]];
1075  if (fabs(coef) >= threshold)
1076  {
1077  const Vec axis = cross(directions[0][index[0]], directions[1][index[1]]);
1078  float angle = asin(axis.norm());
1079  if (coef >= 0.0)
1080  angle = -angle;
1081  // setOrientation(Quaternion(axis, angle) * orientation());
1082  rotate(rotation().inverse() * Quaternion(axis, angle) * orientation());
1083 
1084  // Try to align an other axis direction
1085  unsigned short d = (index[1]+1) % 3;
1086  Vec dir((d==0)? 1.0 : 0.0, (d==1)? 1.0 : 0.0, (d==2)? 1.0 : 0.0);
1087  dir = inverseTransformOf(dir);
1088 
1089  float max = 0.0f;
1090  for (int i=0; i<3; ++i)
1091  {
1092  float proj = fabs(directions[0][i]*dir);
1093  if (proj > max)
1094  {
1095  index[0] = i;
1096  max = proj;
1097  }
1098  }
1099 
1100  if (max >= threshold)
1101  {
1102  const Vec axis = cross(directions[0][index[0]], dir);
1103  float angle = asin(axis.norm());
1104  if (directions[0][index[0]] * dir >= 0.0)
1105  angle = -angle;
1106  // setOrientation(Quaternion(axis, angle) * orientation());
1107  rotate(rotation().inverse() * Quaternion(axis, angle) * orientation());
1108  }
1109  }
1110 
1111  if (move)
1112  {
1113  Vec center;
1114  if (frame)
1115  center = frame->position();
1116 
1117  // setPosition(center - orientation().rotate(old.coordinatesOf(center)));
1118  translate(center - orientation().rotate(old.coordinatesOf(center)) - translation());
1119  }
1120 }
1121 
1126 void Frame::projectOnLine(const Vec& origin, const Vec& direction)
1127 {
1128  // If you are trying to find a bug here, because of memory problems, you waste your time.
1129  // This is a bug in the gcc 3.3 compiler. Compile the library in debug mode and test.
1130  // Uncommenting this line also seems to solve the problem. Horrible.
1131  // cout << "position = " << position() << endl;
1132  // If you found a problem or are using a different compiler, please let me know.
1133  const Vec shift = origin - position();
1134  Vec proj = shift;
1135  proj.projectOnAxis(direction);
1136  translate(shift-proj);
1137 }