From d7e02bc0b3d04c42f42670435d4c1e0c0aa3d69b Mon Sep 17 00:00:00 2001
From: Oliver Sander <sander@igpm.rwth-aachen.de>
Date: Fri, 10 Jun 2011 14:38:31 +0000
Subject: [PATCH] implement the various derivatives of the squared distance
[[Imported from SVN: r7415]]
---
dune/gfe/rigidbodymotion.hh | 80 +++++++++++++++++++++++++++++++++----
1 file changed, 72 insertions(+), 8 deletions(-)
diff --git a/dune/gfe/rigidbodymotion.hh b/dune/gfe/rigidbodymotion.hh
index af9f4ab8..388ed40d 100644
--- a/dune/gfe/rigidbodymotion.hh
+++ b/dune/gfe/rigidbodymotion.hh
@@ -100,32 +100,96 @@ struct RigidBodyMotion
}
/** \brief Compute the Hessian of the squared distance function keeping the first argument fixed */
- static Dune::FieldMatrix<double,7,7> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q) {
- DUNE_THROW(Dune::NotImplemented, "!");
+ static Dune::FieldMatrix<double,7,7> secondDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ {
+ Dune::FieldMatrix<double,7,7> result(0);
+
+ // The linear part
+ Dune::FieldMatrix<double,3,3> linearPart = RealTuple<3>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
+ for (int i=0; i<3; i++)
+ for (int j=0; j<3; j++)
+ result[i][j] = linearPart[i][j];
+
+ // The rotation part
+ Dune::FieldMatrix<double,4,4> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+ for (int i=0; i<4; i++)
+ for (int j=0; j<4; j++)
+ result[3+i][3+j] = rotationPart[i][j];
+
+ return result;
}
/** \brief Compute the mixed second derivate \partial d^2 / \partial da db
Unlike the distance itself the squared distance is differentiable at zero
*/
- static Dune::FieldMatrix<double,7,7> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q) {
- DUNE_THROW(Dune::NotImplemented, "!");
+ static Dune::FieldMatrix<double,7,7> secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ {
+ Dune::FieldMatrix<double,7,7> result(0);
+
+ // The linear part
+ Dune::FieldMatrix<double,3,3> linearPart = RealTuple<3>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.r,q.r);
+ for (int i=0; i<3; i++)
+ for (int j=0; j<3; j++)
+ result[i][j] = linearPart[i][j];
+
+ // The rotation part
+ Dune::FieldMatrix<double,4,4> rotationPart = Rotation<dim,ctype>::secondDerivativeOfDistanceSquaredWRTFirstAndSecondArgument(p.q,q.q);
+ for (int i=0; i<4; i++)
+ for (int j=0; j<4; j++)
+ result[3+i][3+j] = rotationPart[i][j];
+
+ return result;
}
/** \brief Compute the third derivative \partial d^3 / \partial dq^3
Unlike the distance itself the squared distance is differentiable at zero
*/
- static Tensor3<double,7,7,7> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q) {
- DUNE_THROW(Dune::NotImplemented, "!");
+ static Tensor3<double,7,7,7> thirdDerivativeOfDistanceSquaredWRTSecondArgument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ {
+ Tensor3<double,7,7,7> result(0);
+
+ // The linear part
+ Tensor3<double,3,3,3> linearPart = RealTuple<3>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.r,q.r);
+ for (int i=0; i<3; i++)
+ for (int j=0; j<3; j++)
+ for (int k=0; k<3; k++)
+ result[i][j][k] = linearPart[i][j][k];
+
+ // The rotation part
+ Tensor3<double,4,4,4> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTSecondArgument(p.q,q.q);
+ for (int i=0; i<4; i++)
+ for (int j=0; j<4; j++)
+ for (int k=0; k<4; k++)
+ result[3+i][3+j][3+j] = rotationPart[i][j][k];
+
+ return result;
}
/** \brief Compute the mixed third derivative \partial d^3 / \partial da db^2
Unlike the distance itself the squared distance is differentiable at zero
*/
- static Tensor3<double,7,7,7> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q) {
- DUNE_THROW(Dune::NotImplemented, "!");
+ static Tensor3<double,7,7,7> thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(const RigidBodyMotion<dim,ctype> & p, const RigidBodyMotion<dim,ctype> & q)
+ {
+ Tensor3<double,7,7,7> result(0);
+
+ // The linear part
+ Tensor3<double,3,3,3> linearPart = RealTuple<3>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.r,q.r);
+ for (int i=0; i<3; i++)
+ for (int j=0; j<3; j++)
+ for (int k=0; k<3; k++)
+ result[i][j][k] = linearPart[i][j][k];
+
+ // The rotation part
+ Tensor3<double,4,4,4> rotationPart = Rotation<dim,ctype>::thirdDerivativeOfDistanceSquaredWRTFirst1AndSecond2Argument(p.q,q.q);
+ for (int i=0; i<4; i++)
+ for (int j=0; j<4; j++)
+ for (int k=0; k<4; k++)
+ result[3+i][3+j][3+j] = rotationPart[i][j][k];
+
+ return result;
}
--
GitLab