aboutsummaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Eigen/src/Geometry/Quaternion.h31
-rw-r--r--demos/opengl/icosphere.cpp40
-rw-r--r--demos/opengl/quaternion_demo.cpp194
-rw-r--r--test/geometry.cpp10
4 files changed, 156 insertions, 119 deletions
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 97add91d9..b82f37e7d 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -163,6 +163,15 @@ public:
*/
inline Scalar norm() const { return m_coeffs.norm(); }
+ /** \returns the dot product of \c *this and \a other
+ * Geometrically speaking, the dot product of two unit quaternions
+ * corresponds to the cosine of half the angle between the two rotations.
+ * \sa angularDistance()
+ */
+ inline Scalar dot(const Quaternion& other) const { return m_coeffs.dot(other.m_coeffs); }
+
+ inline Scalar angularDistance(const Quaternion& other) const;
+
Matrix3 toRotationMatrix(void) const;
template<typename Derived1, typename Derived2>
@@ -357,22 +366,28 @@ inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
}
+/** \returns the angle (in radian) between two rotations
+ * \sa dot()
+ */
+template <typename Scalar>
+inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
+{
+ double d = ei_abs(this->dot(other));
+ if (d>=1.0)
+ return 0;
+ return 2.0 * std::acos(d);
+}
+
/** \returns the spherical linear interpolation between the two quaternions
* \c *this and \a other at the parameter \a t
*/
template <typename Scalar>
Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
{
- // FIXME options for this function would be:
- // 1 - Quaternion& fromSlerp(Scalar t, const Quaternion& q0, const Quaternion& q1);
- // which set *this from the s-lerp and returns *this
- // 2 - Quaternion slerp(Scalar t, const Quaternion& other) const
- // which returns the s-lerp between this and other
- // ??
static const Scalar one = Scalar(1) - precision<Scalar>();
- Scalar d = m_coeffs.dot(other.m_coeffs);
+ Scalar d = this->dot(other);
Scalar absD = ei_abs(d);
- if (d>=one)
+ if (absD>=one)
return *this;
// theta is the angle between the 2 quaternions
diff --git a/demos/opengl/icosphere.cpp b/demos/opengl/icosphere.cpp
index 5f7823124..042ad6076 100644
--- a/demos/opengl/icosphere.cpp
+++ b/demos/opengl/icosphere.cpp
@@ -25,6 +25,7 @@
#include "icosphere.h"
#include <GL/gl.h>
+#include <map>
using namespace Eigen;
@@ -74,24 +75,39 @@ const std::vector<int>& IcoSphere::indices(int level) const
void IcoSphere::_subdivide(void)
{
+ typedef unsigned long long Key;
+ std::map<Key,int> edgeMap;
const std::vector<int>& indices = *mIndices.back();
mIndices.push_back(new std::vector<int>);
std::vector<int>& refinedIndices = *mIndices.back();
int end = indices.size();
for (int i=0; i<end; i+=3)
{
- int i0, i1, i2;
- Vector3f v0 = mVertices[i0=indices[i+0]];
- Vector3f v1 = mVertices[i1=indices[i+1]];
- Vector3f v2 = mVertices[i2=indices[i+2]];
- int start = mVertices.size();
- mVertices.push_back( (v0+v1).normalized() );
- mVertices.push_back( (v1+v2).normalized() );
- mVertices.push_back( (v2+v0).normalized() );
- refinedIndices.push_back(i0); refinedIndices.push_back(start+0); refinedIndices.push_back(start+2);
- refinedIndices.push_back(i1); refinedIndices.push_back(start+1); refinedIndices.push_back(start+0);
- refinedIndices.push_back(i2); refinedIndices.push_back(start+2); refinedIndices.push_back(start+1);
- refinedIndices.push_back(start+0); refinedIndices.push_back(start+1); refinedIndices.push_back(start+2);
+ int ids0[3], // indices of outer vertices
+ ids1[3]; // indices of edge vertices
+ for (int k=0; k<3; ++k)
+ {
+ int k1 = (k+1)%3;
+ int e0 = indices[i+k];
+ int e1 = indices[i+k1];
+ ids0[k] = e0;
+ if (e1>e0)
+ std::swap(e0,e1);
+ Key edgeKey = Key(e0) | (Key(e1)<<32);
+ std::map<Key,int>::iterator it = edgeMap.find(edgeKey);
+ if (it==edgeMap.end())
+ {
+ ids1[k] = mVertices.size();
+ edgeMap[edgeKey] = ids1[k];
+ mVertices.push_back( (mVertices[e0]+mVertices[e1]).normalized() );
+ }
+ else
+ ids1[k] = it->second;
+ }
+ refinedIndices.push_back(ids0[0]); refinedIndices.push_back(ids1[0]); refinedIndices.push_back(ids1[2]);
+ refinedIndices.push_back(ids0[1]); refinedIndices.push_back(ids1[1]); refinedIndices.push_back(ids1[0]);
+ refinedIndices.push_back(ids0[2]); refinedIndices.push_back(ids1[2]); refinedIndices.push_back(ids1[1]);
+ refinedIndices.push_back(ids1[0]); refinedIndices.push_back(ids1[1]); refinedIndices.push_back(ids1[2]);
}
mListIds.push_back(0);
}
diff --git a/demos/opengl/quaternion_demo.cpp b/demos/opengl/quaternion_demo.cpp
index dbfc16c6b..31aeb4d73 100644
--- a/demos/opengl/quaternion_demo.cpp
+++ b/demos/opengl/quaternion_demo.cpp
@@ -41,6 +41,89 @@
using namespace Eigen;
+class FancySpheres
+{
+ public:
+ FancySpheres()
+ {
+ const int levels = 4;
+ const float scale = 0.33;
+ float radius = 100;
+ std::vector<int> parents;
+
+ // leval 0
+ mCenters.push_back(Vector3f::Zero());
+ parents.push_back(-1);
+ mRadii.push_back(radius);
+
+ // generate level 1 using icosphere vertices
+ radius *= 0.45;
+ {
+ float dist = mRadii[0]*0.9;
+ for (int i=0; i<12; ++i)
+ {
+ mCenters.push_back(mIcoSphere.vertices()[i] * dist);
+ mRadii.push_back(radius);
+ parents.push_back(0);
+ }
+ }
+
+ static const float angles [10] = {
+ 0, 0,
+ M_PI, 0.*M_PI,
+ M_PI, 0.5*M_PI,
+ M_PI, 1.*M_PI,
+ M_PI, 1.5*M_PI
+ };
+
+ // generate other levels
+ int start = 1;
+ for (int l=1; l<levels; l++)
+ {
+ radius *= scale;
+ int end = mCenters.size();
+ for (int i=start; i<end; ++i)
+ {
+ Vector3f c = mCenters[i];
+ Vector3f ax0 = (c - mCenters[parents[i]]).normalized();
+ Vector3f ax1 = ax0.unitOrthogonal();
+ Quaternionf q;
+ q.setFromTwoVectors(Vector3f::UnitZ(), ax0);
+ Transform3f t = Translation3f(c) * q * Scaling3f(mRadii[i]+radius);
+ for (int j=0; j<5; ++j)
+ {
+ Vector3f newC = c + ( (AngleAxisf(angles[j*2+1], ax0)
+ * AngleAxisf(angles[j*2+0] * (l==1 ? 0.35 : 0.5), ax1)) * ax0)
+ * (mRadii[i] + radius*0.8);
+ mCenters.push_back(newC);
+ mRadii.push_back(radius);
+ parents.push_back(i);
+ }
+ }
+ start = end;
+ }
+ }
+
+ void draw()
+ {
+ int end = mCenters.size();
+ glEnable(GL_NORMALIZE);
+ for (int i=0; i<end; ++i)
+ {
+ Transform3f t = Translation3f(mCenters[i]) * Scaling3f(mRadii[i]);
+ gpu.pushMatrix(GL_MODELVIEW);
+ gpu.multMatrix(t.matrix(),GL_MODELVIEW);
+ mIcoSphere.draw(2);
+ gpu.popMatrix(GL_MODELVIEW);
+ }
+ glDisable(GL_NORMALIZE);
+ }
+ protected:
+ std::vector<Vector3f> mCenters;
+ std::vector<float> mRadii;
+ IcoSphere mIcoSphere;
+};
+
// generic linear interpolation method
template<typename T> T lerp(float t, const T& a, const T& b)
@@ -156,7 +239,8 @@ void RenderingWidget::grabFrame(void)
void RenderingWidget::drawScene()
{
- float length = 50;
+ static FancySpheres sFancySpheres;
+ float length = 200;
gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitX(), Color(1,0,0,1));
gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitY(), Color(0,1,0,1));
gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitZ(), Color(0,0,1,1));
@@ -183,13 +267,14 @@ void RenderingWidget::drawScene()
glEnable(GL_LIGHT1);
glColor3f(0.4, 0.7, 0.4);
- glVertexPointer(3, GL_FLOAT, 0, mVertices[0].data());
- glNormalPointer(GL_FLOAT, 0, mNormals[0].data());
- glEnableClientState(GL_VERTEX_ARRAY);
- glEnableClientState(GL_NORMAL_ARRAY);
- glDrawArrays(GL_TRIANGLES, 0, mVertices.size());
- glDisableClientState(GL_VERTEX_ARRAY);
- glDisableClientState(GL_NORMAL_ARRAY);
+ sFancySpheres.draw();
+// glVertexPointer(3, GL_FLOAT, 0, mVertices[0].data());
+// glNormalPointer(GL_FLOAT, 0, mNormals[0].data());
+// glEnableClientState(GL_VERTEX_ARRAY);
+// glEnableClientState(GL_NORMAL_ARRAY);
+// glDrawArrays(GL_TRIANGLES, 0, mVertices.size());
+// glDisableClientState(GL_VERTEX_ARRAY);
+// glDisableClientState(GL_NORMAL_ARRAY);
glDisable(GL_LIGHTING);
}
@@ -413,93 +498,6 @@ void RenderingWidget::initializeGL()
mCamera.setTarget(Vector3f(0, 0, 0));
mInitFrame.orientation = mCamera.orientation().inverse();
mInitFrame.position = mCamera.viewMatrix().translation();
-
- // create a kind of fractal sphere
- {
- IcoSphere pattern;
-
- int levels = 3;
- float scale = 0.45;
- float radius = 100;
- std::vector<Vector3f> centers;
- std::vector<int> parents;
- std::vector<float> radii;
- centers.push_back(Vector3f::Zero());
- parents.push_back(-1);
- radii.push_back(radius);
- radius *= scale;
-
- // generate level 1 using icosphere vertices
- {
- float dist = radii[0]*0.9;
- for (int i=0; i<12; ++i)
- {
- centers.push_back(pattern.vertices()[i] * dist);
- radii.push_back(radius);
- parents.push_back(0);
- }
- }
-
- scale = 0.33;
- static const float angles [10] = {
- 0, 0,
- M_PI, 0.*M_PI,
- M_PI, 0.5*M_PI,
- M_PI, 1.*M_PI,
- M_PI, 1.5*M_PI};
-
- // generate other levels
- int start = 1;
- float maxAngle = M_PI/2;
- for (int l=1; l<levels; l++)
- {
- radius *= scale;
- int end = centers.size();
- for (int i=start; i<end; ++i)
- {
- Vector3f c = centers[i];
- Vector3f ax0, ax1;
- if (parents[i]==-1)
- ax0 = Vector3f::UnitZ();
- else
- ax0 = (c - centers[parents[i]]).normalized();
- ax1 = ax0.unitOrthogonal();
- Quaternionf q;
- q.setFromTwoVectors(Vector3f::UnitZ(), ax0);
- Transform3f t = Translation3f(c) * q * Scaling3f(radii[i]+radius);
- for (int j=0; j<5; ++j)
- {
- Vector3f newC = c + ( (AngleAxisf(angles[j*2+1], ax0)
- * AngleAxisf(angles[j*2+0] * (l==1 ? 0.35 : 0.5), ax1)) * ax0)*(radii[i] + radius*0.8);
- centers.push_back(newC);
- radii.push_back(radius);
- parents.push_back(i);
- }
- }
- start = end;
- maxAngle = M_PI/2;
- }
- parents.clear();
- // instanciate the geometry
- {
- const std::vector<int>& sphereIndices = pattern.indices(2);
- std::cout << "instanciate geometry... (" << sphereIndices.size() * centers.size() << " vertices)\n";
- mVertices.reserve(sphereIndices.size() * centers.size());
- mNormals.reserve(sphereIndices.size() * centers.size());
- int end = centers.size();
- for (int i=0; i<end; ++i)
- {
- Transform3f t = Translation3f(centers[i]) * Scaling3f(radii[i]);
- // copy vertices
- for (unsigned int j=0; j<sphereIndices.size(); ++j)
- {
- Vector3f v = pattern.vertices()[sphereIndices[j]];
- mVertices.push_back(t * v);
- mNormals.push_back(v);
- }
- }
- }
- }
}
void RenderingWidget::resizeGL(int width, int height)
@@ -539,10 +537,7 @@ void RenderingWidget::resetCamera()
Frame aux1 = mCamera.frame();
aux1.orientation = aux1.orientation.inverse();
aux1.position = mCamera.viewMatrix().translation();
- float rangle = AngleAxisf(aux0.orientation.inverse() * aux1.orientation).angle();
- if (rangle>M_PI)
- rangle = 2.*M_PI - rangle;
- float duration = rangle * 0.9;
+ float duration = aux0.orientation.angularDistance(aux1.orientation) * 0.9;
if (duration<0.1) duration = 0.1;
// put the camera at that time step:
@@ -660,3 +655,4 @@ int main(int argc, char *argv[])
}
#include "quaternion_demo.moc"
+
diff --git a/test/geometry.cpp b/test/geometry.cpp
index cb9726346..a87c8a2ad 100644
--- a/test/geometry.cpp
+++ b/test/geometry.cpp
@@ -48,6 +48,10 @@ template<typename Scalar> void geometry(void)
typedef Translation<Scalar,2> Translation2;
typedef Translation<Scalar,3> Translation3;
+ Scalar largeEps = test_precision<Scalar>();
+ if (ei_is_same_type<Scalar,float>::ret)
+ largeEps = 1e-3f;
+
Quaternion q1, q2;
Vector3 v0 = Vector3::Random(),
v1 = Vector3::Random(),
@@ -82,6 +86,12 @@ template<typename Scalar> void geometry(void)
q1 = AngleAxis(a, v0.normalized());
q2 = AngleAxis(a, v1.normalized());
+ // angular distance
+ Scalar refangle = ei_abs(AngleAxis(q1.inverse()*q2).angle());
+ if (refangle>M_PI)
+ refangle = 2.*M_PI - refangle;
+ VERIFY(ei_isApprox(q1.angularDistance(q2), refangle, largeEps));
+
// rotation matrix conversion
VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
VERIFY_IS_APPROX(q1 * q2 * v2,
generated by cgit v1.2.3 (git 2.39.1) at 2024-05-18 22:47:49 +0000