diff --git a/test/fdcheck.cc b/test/fdcheck.cc
index 13e3be54d47325e5d715d20b05cd0a0e202fa375..805e5fdd4c302edb641d7c566a3dd0b087fb86a6 100644
--- a/test/fdcheck.cc
+++ b/test/fdcheck.cc
@@ -16,225 +16,10 @@ const int blocksize = 6;
using namespace Dune;
-void testDDExp()
-{
- array<FieldVector<double,3>, 125> v;
- int ct = 0;
- double eps = 1e-4;
-
- for (int i=-2; i<3; i++)
- for (int j=-2; j<3; j++)
- for (int k=-2; k<3; k++) {
- v[ct][0] = i;
- v[ct][1] = j;
- v[ct][2] = k;
- ct++;
- }
-
- for (size_t i=0; i<v.size(); i++) {
-
- // Compute FD approximation of second derivative of exp
- Dune::array<Dune::FieldMatrix<double,3,3>, 4> fdDDExp;
-
- for (int j=0; j<3; j++) {
-
- for (int k=0; k<3; k++) {
-
- if (j==k) {
-
- Quaternion<double> forwardQ = Quaternion<double>::exp(v[i][0] + (j==0)*eps,
- v[i][1] + (j==1)*eps,
- v[i][2] + (j==2)*eps);
- Quaternion<double> centerQ = Quaternion<double>::exp(v[i][0],v[i][1],v[i][2]);
- Quaternion<double> backwardQ = Quaternion<double>::exp(v[i][0] - (j==0)*eps,
- v[i][1] - (j==1)*eps,
- v[i][2] - (j==2)*eps);
-
- for (int l=0; l<4; l++)
- fdDDExp[l][j][j] = (forwardQ[l] - 2*centerQ[l] + backwardQ[l]) / (eps*eps);
-
-
- } else {
-
- SkewMatrix<double,3> ffV(v[i]); ffV.axial()[j] += eps; ffV.axial()[k] += eps;
- SkewMatrix<double,3> fbV(v[i]); fbV.axial()[j] += eps; fbV.axial()[k] -= eps;
- SkewMatrix<double,3> bfV(v[i]); bfV.axial()[j] -= eps; bfV.axial()[k] += eps;
- SkewMatrix<double,3> bbV(v[i]); bbV.axial()[j] -= eps; bbV.axial()[k] -= eps;
-
- Quaternion<double> forwardForwardQ = Quaternion<double>::exp(ffV);
- Quaternion<double> forwardBackwardQ = Quaternion<double>::exp(fbV);
- Quaternion<double> backwardForwardQ = Quaternion<double>::exp(bfV);
- Quaternion<double> backwardBackwardQ = Quaternion<double>::exp(bbV);
-
- for (int l=0; l<4; l++)
- fdDDExp[l][j][k] = (forwardForwardQ[l] + backwardBackwardQ[l]
- - forwardBackwardQ[l] - backwardForwardQ[l]) / (4*eps*eps);
-
- }
-
- }
-
- }
-
- // Compute analytical second derivative of exp
- Dune::array<Dune::FieldMatrix<double,3,3>, 4> ddExp;
- Rotation<double,3>::DDexp(v[i], ddExp);
-
- for (int m=0; m<4; m++)
- for (int j=0; j<3; j++)
- for (int k=0; k<3; k++)
- if ( std::abs(fdDDExp[m][j][k] - ddExp[m][j][k]) > eps) {
- std::cout << "Error at v = " << v[i]
- << "[" << m << ", " << j << ", " << k << "] "
- << " fd: " << fdDDExp[m][j][k]
- << " analytical: " << ddExp[m][j][k] << std::endl;
- }
- }
-}
-
-void testDerivativeOfInterpolatedPosition()
-{
- array<Quaternion<double>, 6> q;
-
- FieldVector<double,3> xAxis(0); xAxis[0] = 1;
- FieldVector<double,3> yAxis(0); yAxis[1] = 1;
- FieldVector<double,3> zAxis(0); zAxis[2] = 1;
-
- q[0] = Quaternion<double>(xAxis, 0);
- q[1] = Quaternion<double>(xAxis, M_PI/2);
- q[2] = Quaternion<double>(yAxis, 0);
- q[3] = Quaternion<double>(yAxis, M_PI/2);
- q[4] = Quaternion<double>(zAxis, 0);
- q[5] = Quaternion<double>(zAxis, M_PI/2);
-
- double eps = 1e-7;
-
- for (int i=0; i<6; i++) {
-
- for (int j=0; j<6; j++) {
-
- for (int k=0; k<7; k++) {
-
- double s = k/6.0;
-
- array<Quaternion<double>,6> fdGrad;
-
- // ///////////////////////////////////////////////////////////
- // First: test the interpolated position
- // ///////////////////////////////////////////////////////////
- fdGrad[0] = Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(eps,0,0)), q[j], s);
- fdGrad[0] -= Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(-eps,0,0)), q[j], s);
- fdGrad[0] /= 2*eps;
-
- fdGrad[1] = Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,eps,0)), q[j], s);
- fdGrad[1] -= Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,-eps,0)), q[j], s);
- fdGrad[1] /= 2*eps;
-
- fdGrad[2] = Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,0,eps)), q[j], s);
- fdGrad[2] -= Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,0,-eps)), q[j], s);
- fdGrad[2] /= 2*eps;
-
- fdGrad[3] = Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(eps,0,0)), s);
- fdGrad[3] -= Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(-eps,0,0)), s);
- fdGrad[3] /= 2*eps;
-
- fdGrad[4] = Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,eps,0)), s);
- fdGrad[4] -= Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,-eps,0)), s);
- fdGrad[4] /= 2*eps;
-
- fdGrad[5] = Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,0,eps)), s);
- fdGrad[5] -= Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,0,-eps)), s);
- fdGrad[5] /= 2*eps;
-
- // Compute analytical gradient
- array<Quaternion<double>,6> grad;
- RodLocalStiffness<OneDGrid,double>::interpolationDerivative(q[i], q[j], s, grad);
-
- for (int l=0; l<6; l++) {
- Quaternion<double> diff = fdGrad[l];
- diff -= grad[l];
- if (diff.two_norm() > 1e-6) {
- std::cout << "Error in position " << l << ": fd: " << fdGrad[l]
- << " analytical: " << grad[l] << std::endl;
- }
-
- }
-
- // ///////////////////////////////////////////////////////////
- // Second: test the interpolated velocity vector
- // ///////////////////////////////////////////////////////////
-
- for (int l=1; l<7; l++) {
-
- double intervalLength = l/(double(3));
-
- fdGrad[0] = Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(eps,0,0)),
- q[j], s, intervalLength);
- fdGrad[0] -= Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(-eps,0,0)),
- q[j], s, intervalLength);
- fdGrad[0] /= 2*eps;
-
- fdGrad[1] = Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,eps,0)),
- q[j], s, intervalLength);
- fdGrad[1] -= Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,-eps,0)),
- q[j], s, intervalLength);
- fdGrad[1] /= 2*eps;
-
- fdGrad[2] = Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,0,eps)),
- q[j], s, intervalLength);
- fdGrad[2] -= Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,0,-eps)),
- q[j], s, intervalLength);
- fdGrad[2] /= 2*eps;
-
- fdGrad[3] = Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(eps,0,0)), s, intervalLength);
- fdGrad[3] -= Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(-eps,0,0)), s, intervalLength);
- fdGrad[3] /= 2*eps;
-
- fdGrad[4] = Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,eps,0)), s, intervalLength);
- fdGrad[4] -= Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,-eps,0)), s, intervalLength);
- fdGrad[4] /= 2*eps;
-
- fdGrad[5] = Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,0,eps)), s, intervalLength);
- fdGrad[5] -= Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,0,-eps)), s, intervalLength);
- fdGrad[5] /= 2*eps;
-
- // Compute analytical velocity vector gradient
- RodLocalStiffness<OneDGrid,double>::interpolationVelocityDerivative(q[i], q[j], s, intervalLength, grad);
-
- for (int m=0; m<6; m++) {
- Quaternion<double> diff = fdGrad[m];
- diff -= grad[m];
- if (diff.two_norm() > 1e-6) {
- std::cout << "Error in velocity " << m
- << ": s = " << s << " of (" << intervalLength << ")"
- << " fd: " << fdGrad[m] << " analytical: " << grad[m] << std::endl;
- }
-
- }
-
- }
-
- }
-
- }
-
- }
-
-}
int main (int argc, char *argv[]) try
{
- // //////////////////////////////////////////////
- // Test second derivative of exp
- // //////////////////////////////////////////////
- testDDExp();
-
- // //////////////////////////////////////////////
- // Test derivative of interpolated position
- // //////////////////////////////////////////////
- testDerivativeOfInterpolatedPosition();
- exit(0);
typedef std::vector<RigidBodyMotion<double,3> > SolutionType;
// ///////////////////////////////////////
diff --git a/test/rotationtest.cc b/test/rotationtest.cc
index 8f29d67215ed50951cfe6a3af407b78567ce8afb..055be5e76fd373edcd4a12524641069dda3671e5 100644
--- a/test/rotationtest.cc
+++ b/test/rotationtest.cc
@@ -10,6 +10,214 @@
using namespace Dune;
+void testDDExp()
+{
+ array<FieldVector<double,3>, 125> v;
+ int ct = 0;
+ double eps = 1e-4;
+
+ for (int i=-2; i<3; i++)
+ for (int j=-2; j<3; j++)
+ for (int k=-2; k<3; k++) {
+ v[ct][0] = i;
+ v[ct][1] = j;
+ v[ct][2] = k;
+ ct++;
+ }
+
+ for (size_t i=0; i<v.size(); i++) {
+
+ // Compute FD approximation of second derivative of exp
+ Dune::array<Dune::FieldMatrix<double,3,3>, 4> fdDDExp;
+
+ for (int j=0; j<3; j++) {
+
+ for (int k=0; k<3; k++) {
+
+ if (j==k) {
+
+ Quaternion<double> forwardQ = Quaternion<double>::exp(v[i][0] + (j==0)*eps,
+ v[i][1] + (j==1)*eps,
+ v[i][2] + (j==2)*eps);
+ Quaternion<double> centerQ = Quaternion<double>::exp(v[i][0],v[i][1],v[i][2]);
+ Quaternion<double> backwardQ = Quaternion<double>::exp(v[i][0] - (j==0)*eps,
+ v[i][1] - (j==1)*eps,
+ v[i][2] - (j==2)*eps);
+
+ for (int l=0; l<4; l++)
+ fdDDExp[l][j][j] = (forwardQ[l] - 2*centerQ[l] + backwardQ[l]) / (eps*eps);
+
+
+ } else {
+
+ SkewMatrix<double,3> ffV(v[i]); ffV.axial()[j] += eps; ffV.axial()[k] += eps;
+ SkewMatrix<double,3> fbV(v[i]); fbV.axial()[j] += eps; fbV.axial()[k] -= eps;
+ SkewMatrix<double,3> bfV(v[i]); bfV.axial()[j] -= eps; bfV.axial()[k] += eps;
+ SkewMatrix<double,3> bbV(v[i]); bbV.axial()[j] -= eps; bbV.axial()[k] -= eps;
+
+ Quaternion<double> forwardForwardQ = Quaternion<double>::exp(ffV);
+ Quaternion<double> forwardBackwardQ = Quaternion<double>::exp(fbV);
+ Quaternion<double> backwardForwardQ = Quaternion<double>::exp(bfV);
+ Quaternion<double> backwardBackwardQ = Quaternion<double>::exp(bbV);
+
+ for (int l=0; l<4; l++)
+ fdDDExp[l][j][k] = (forwardForwardQ[l] + backwardBackwardQ[l]
+ - forwardBackwardQ[l] - backwardForwardQ[l]) / (4*eps*eps);
+
+ }
+
+ }
+
+ }
+
+ // Compute analytical second derivative of exp
+ Dune::array<Dune::FieldMatrix<double,3,3>, 4> ddExp;
+ Rotation<double,3>::DDexp(v[i], ddExp);
+
+ for (int m=0; m<4; m++)
+ for (int j=0; j<3; j++)
+ for (int k=0; k<3; k++)
+ if ( std::abs(fdDDExp[m][j][k] - ddExp[m][j][k]) > eps) {
+ std::cout << "Error at v = " << v[i]
+ << "[" << m << ", " << j << ", " << k << "] "
+ << " fd: " << fdDDExp[m][j][k]
+ << " analytical: " << ddExp[m][j][k] << std::endl;
+ }
+ }
+}
+
+void testDerivativeOfInterpolatedPosition()
+{
+ array<Quaternion<double>, 6> q;
+
+ FieldVector<double,3> xAxis(0); xAxis[0] = 1;
+ FieldVector<double,3> yAxis(0); yAxis[1] = 1;
+ FieldVector<double,3> zAxis(0); zAxis[2] = 1;
+
+ q[0] = Quaternion<double>(xAxis, 0);
+ q[1] = Quaternion<double>(xAxis, M_PI/2);
+ q[2] = Quaternion<double>(yAxis, 0);
+ q[3] = Quaternion<double>(yAxis, M_PI/2);
+ q[4] = Quaternion<double>(zAxis, 0);
+ q[5] = Quaternion<double>(zAxis, M_PI/2);
+
+ double eps = 1e-7;
+
+ for (int i=0; i<6; i++) {
+
+ for (int j=0; j<6; j++) {
+
+ for (int k=0; k<7; k++) {
+
+ double s = k/6.0;
+
+ array<Quaternion<double>,6> fdGrad;
+
+ // ///////////////////////////////////////////////////////////
+ // First: test the interpolated position
+ // ///////////////////////////////////////////////////////////
+ fdGrad[0] = Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(eps,0,0)), q[j], s);
+ fdGrad[0] -= Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(-eps,0,0)), q[j], s);
+ fdGrad[0] /= 2*eps;
+
+ fdGrad[1] = Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,eps,0)), q[j], s);
+ fdGrad[1] -= Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,-eps,0)), q[j], s);
+ fdGrad[1] /= 2*eps;
+
+ fdGrad[2] = Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,0,eps)), q[j], s);
+ fdGrad[2] -= Rotation<double,3>::interpolate(q[i].mult(Quaternion<double>::exp(0,0,-eps)), q[j], s);
+ fdGrad[2] /= 2*eps;
+
+ fdGrad[3] = Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(eps,0,0)), s);
+ fdGrad[3] -= Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(-eps,0,0)), s);
+ fdGrad[3] /= 2*eps;
+
+ fdGrad[4] = Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,eps,0)), s);
+ fdGrad[4] -= Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,-eps,0)), s);
+ fdGrad[4] /= 2*eps;
+
+ fdGrad[5] = Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,0,eps)), s);
+ fdGrad[5] -= Rotation<double,3>::interpolate(q[i], q[j].mult(Quaternion<double>::exp(0,0,-eps)), s);
+ fdGrad[5] /= 2*eps;
+
+ // Compute analytical gradient
+ array<Quaternion<double>,6> grad;
+ RodLocalStiffness<OneDGrid,double>::interpolationDerivative(q[i], q[j], s, grad);
+
+ for (int l=0; l<6; l++) {
+ Quaternion<double> diff = fdGrad[l];
+ diff -= grad[l];
+ if (diff.two_norm() > 1e-6) {
+ std::cout << "Error in position " << l << ": fd: " << fdGrad[l]
+ << " analytical: " << grad[l] << std::endl;
+ }
+
+ }
+
+ // ///////////////////////////////////////////////////////////
+ // Second: test the interpolated velocity vector
+ // ///////////////////////////////////////////////////////////
+
+ for (int l=1; l<7; l++) {
+
+ double intervalLength = l/(double(3));
+
+ fdGrad[0] = Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(eps,0,0)),
+ q[j], s, intervalLength);
+ fdGrad[0] -= Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(-eps,0,0)),
+ q[j], s, intervalLength);
+ fdGrad[0] /= 2*eps;
+
+ fdGrad[1] = Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,eps,0)),
+ q[j], s, intervalLength);
+ fdGrad[1] -= Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,-eps,0)),
+ q[j], s, intervalLength);
+ fdGrad[1] /= 2*eps;
+
+ fdGrad[2] = Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,0,eps)),
+ q[j], s, intervalLength);
+ fdGrad[2] -= Rotation<double,3>::interpolateDerivative(q[i].mult(Quaternion<double>::exp(0,0,-eps)),
+ q[j], s, intervalLength);
+ fdGrad[2] /= 2*eps;
+
+ fdGrad[3] = Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(eps,0,0)), s, intervalLength);
+ fdGrad[3] -= Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(-eps,0,0)), s, intervalLength);
+ fdGrad[3] /= 2*eps;
+
+ fdGrad[4] = Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,eps,0)), s, intervalLength);
+ fdGrad[4] -= Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,-eps,0)), s, intervalLength);
+ fdGrad[4] /= 2*eps;
+
+ fdGrad[5] = Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,0,eps)), s, intervalLength);
+ fdGrad[5] -= Rotation<double,3>::interpolateDerivative(q[i], q[j].mult(Quaternion<double>::exp(0,0,-eps)), s, intervalLength);
+ fdGrad[5] /= 2*eps;
+
+ // Compute analytical velocity vector gradient
+ RodLocalStiffness<OneDGrid,double>::interpolationVelocityDerivative(q[i], q[j], s, intervalLength, grad);
+
+ for (int m=0; m<6; m++) {
+ Quaternion<double> diff = fdGrad[m];
+ diff -= grad[m];
+ if (diff.two_norm() > 1e-6) {
+ std::cout << "Error in velocity " << m
+ << ": s = " << s << " of (" << intervalLength << ")"
+ << " fd: " << fdGrad[m] << " analytical: " << grad[m] << std::endl;
+ }
+
+ }
+
+ }
+
+ }
+
+ }
+
+ }
+
+}
+
+
+
void testRotation(Rotation<double,3> q)
{
// Make sure it really is a unit quaternion
@@ -118,6 +326,16 @@ int main (int argc, char *argv[]) try
for (int i=0; i<nTestPoints; i++)
testRotation(testPoints[i]);
+ // //////////////////////////////////////////////
+ // Test second derivative of exp
+ // //////////////////////////////////////////////
+ testDDExp();
+
+ // //////////////////////////////////////////////
+ // Test derivative of interpolated position
+ // //////////////////////////////////////////////
+ testDerivativeOfInterpolatedPosition();
+
} catch (Exception e) {
std::cout << e << std::endl;