HUMIM Tracker: /home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/dynamics/mbd/mbd_compute_collision

Go to the documentation of this file.
00001 #ifndef OPENTISSUE_DYNAMICS_MBD_UTIL_COMPUTE_COLLISION_MATRIX_H
00002 #define OPENTISSUE_DYNAMICS_MBD_UTIL_COMPUTE_COLLISION_MATRIX_H
00003 //
00004 // OpenTissue, A toolbox for physical based simulation and animation.
00005 // Copyright (C) 2007 Department of Computer Science, University of Copenhagen
00006 //
00007 #include <OpenTissue/configuration.h>
00008 
00009 
00010 namespace OpenTissue
00011 {
00012   namespace mbd
00013   {
00014 
00039     template<typename real_type,typename vector3_type,typename matrix3x3_type>
00040     matrix3x3_type compute_collision_matrix(
00041       real_type const & m_a
00042       , matrix3x3_type const & I_a
00043       , vector3_type const & r_a
00044       , real_type const & m_b
00045       , matrix3x3_type const & I_b
00046       , vector3_type const & r_b
00047       )
00048     {
00049       matrix3x3_type K;
00050       real_type K00 =  r_a(2)*I_a(1,1)*r_a(2) - 2*r_a(2)*I_a(1,2)*r_a(1) + r_a(1)*I_a(2,2)*r_a(1);
00051       real_type K01 =  - r_a(2)*I_a(0,1)*r_a(2) + r_a(1)*I_a(0,2)*r_a(2) + r_a(2)*I_a(1,2)*r_a(0) - r_a(1)*I_a(2,2)*r_a(0);
00052       real_type K02 =  r_a(2)*I_a(0,1)*r_a(1) - r_a(1)*I_a(0,2)*r_a(1) - r_a(2)*I_a(1,1)*r_a(0) + r_a(1)*I_a(1,2)*r_a(0);
00053       real_type K11 =  r_a(2)*I_a(0,0)*r_a(2) - 2*r_a(0)*I_a(0,2)*r_a(2) + r_a(0)*I_a(2,2)*r_a(0);
00054       real_type K12 =  - r_a(2)*I_a(0,0)*r_a(1) + r_a(2)*I_a(0,1)*r_a(0) + r_a(0)*I_a(0,2)*r_a(1) - r_a(0)*I_a(1,2)*r_a(0);
00055       real_type K22 =  r_a(1)*I_a(0,0)*r_a(1) - 2*r_a(0)*I_a(0,1)*r_a(1) + r_a(0)*I_a(1,1)*r_a(0);
00056       K(0,0) = m_a + K00;
00057       K(0,1) = K01;
00058       K(0,2) = K02;
00059       K(1,0) = K01;
00060       K(1,1) = m_a + K11;
00061       K(1,2) = K12;
00062       K(2,0) = K02;
00063       K(2,1) = K12;
00064       K(2,2) = m_a + K22;
00065       K00 =  r_b(2)*I_b(1,1)*r_b(2) - 2*r_b(2)*I_b(1,2)*r_b(1) + r_b(1)*I_b(2,2)*r_b(1);
00066       K01 =  - r_b(2)*I_b(0,1)*r_b(2) + r_b(1)*I_b(0,2)*r_b(2) + r_b(2)*I_b(1,2)*r_b(0) - r_b(1)*I_b(2,2)*r_b(0);
00067       K02 =  r_b(2)*I_b(0,1)*r_b(1) - r_b(1)*I_b(0,2)*r_b(1) - r_b(2)*I_b(1,1)*r_b(0) + r_b(1)*I_b(1,2)*r_b(0);
00068       K11 =  r_b(2)*I_b(0,0)*r_b(2) - 2*r_b(0)*I_b(0,2)*r_b(2) + r_b(0)*I_b(2,2)*r_b(0);
00069       K12 =  - r_b(2)*I_b(0,0)*r_b(1) + r_b(2)*I_b(0,1)*r_b(0) + r_b(0)*I_b(0,2)*r_b(1) - r_b(0)*I_b(1,2)*r_b(0);
00070       K22 =  r_b(1)*I_b(0,0)*r_b(1) - 2*r_b(0)*I_b(0,1)*r_b(1) + r_b(0)*I_b(1,1)*r_b(0);
00071       K(0,0) += m_b + K00;
00072       K(0,1) += K01;
00073       K(0,2) += K02;
00074       K(1,0) += K01;
00075       K(1,1) += m_b + K11;
00076       K(1,2) += K12;
00077       K(2,0) += K02;
00078       K(2,1) += K12;
00079       K(2,2) += m_b + K22;
00080       return K;
00081     }
00082 
00083   } //End of namespace mbd
00084 
00085 } //End of namespace OpenTissue
00086 
00087 #endif // OPENTISSUE_DYNAMICS_MBD_UTIL_COMPUTE_COLLISION_MATRIX_H
/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/dynamics/mbd/mbd_compute_collision_matrix.h
