Go to the documentation of this file.00001 #ifndef OPENTISSUE_DYNAMICS_MBD_UTIL_MATH_COMPUTE_DIAGONAL_H
00002 #define OPENTISSUE_DYNAMICS_MBD_UTIL_MATH_COMPUTE_DIAGONAL_H
00003
00004
00005
00006
00007
00008
00009
00010 #include <OpenTissue/configuration.h>
00011
00012 #include <OpenTissue/core/math/math_is_number.h>
00013
00014 #include <cassert>
00015
00016 namespace OpenTissue
00017 {
00018 namespace mbd
00019 {
00020 namespace math
00021 {
00022
00038 template<typename matrix_type,typename vector_type>
00039 void compute_diagonal(matrix_type const & J, matrix_type const & WJT, vector_type & d)
00040 {
00041 typedef typename matrix_type::size_type size_type;
00042 typedef typename matrix_type::value_type real_type;
00043
00044 assert(J.size1()>0 || !"compute_diagonal(): J was empty");
00045 assert(J.size2()>0 || !"compute_diagonal(): J was empty");
00046 assert(J.size1()==WJT.size1() || !"compute_diagonal(): J and WJT did not match dimensions");
00047 assert(J.size2()==WJT.size2() || !"compute_diagonal(): J and WJT did not match dimensions");
00048
00049 d.resize(J.size1(), false);
00050
00051 for (size_type i = 0; i < J.size1 (); ++i)
00052 {
00053 size_type begin = J.index1_data()[i];
00054 size_type end = J.index1_data()[i+1];
00055
00056 assert( (end-begin)==12 || !"compute_diagonal(): incorrect format of J" );
00057 assert( J.index1_data()[i] == WJT.index1_data()[i] || !"compute_diagonal(): WJT and J did not have the same format");
00058 assert( J.index1_data()[i+1] == WJT.index1_data()[i+1] || !"compute_diagonal(): WJT and J did not have the same format");
00059
00060 size_type j = begin;
00061 real_type const & j1 = J.value_data()[j]; ++j;
00062 real_type const & j2 = J.value_data()[j]; ++j;
00063 real_type const & j3 = J.value_data()[j]; ++j;
00064 real_type const & j4 = J.value_data()[j]; ++j;
00065 real_type const & j5 = J.value_data()[j]; ++j;
00066 real_type const & j6 = J.value_data()[j];
00067
00068 j = end-6;
00069 real_type const & j7 = J.value_data()[j]; ++j;
00070 real_type const & j8 = J.value_data()[j]; ++j;
00071 real_type const & j9 = J.value_data()[j]; ++j;
00072 real_type const & j10 = J.value_data()[j]; ++j;
00073 real_type const & j11 = J.value_data()[j]; ++j;
00074 real_type const & j12 = J.value_data()[j];
00075
00076 j = begin;
00077 real_type const & wjt1 = WJT.value_data()[j]; ++j;
00078 real_type const & wjt2 = WJT.value_data()[j]; ++j;
00079 real_type const & wjt3 = WJT.value_data()[j]; ++j;
00080 real_type const & wjt4 = WJT.value_data()[j]; ++j;
00081 real_type const & wjt5 = WJT.value_data()[j]; ++j;
00082 real_type const & wjt6 = WJT.value_data()[j];
00083
00084 j = end-6;
00085 real_type const & wjt7 = WJT.value_data()[j]; ++j;
00086 real_type const & wjt8 = WJT.value_data()[j]; ++j;
00087 real_type const & wjt9 = WJT.value_data()[j]; ++j;
00088 real_type const & wjt10 = WJT.value_data()[j]; ++j;
00089 real_type const & wjt11 = WJT.value_data()[j]; ++j;
00090 real_type const & wjt12 = WJT.value_data()[j];
00091
00092 assert(is_number(j1) || !"compute_diagonal(): not a number encountered");
00093 assert(is_number(j2) || !"compute_diagonal(): not a number encountered");
00094 assert(is_number(j3) || !"compute_diagonal(): not a number encountered");
00095 assert(is_number(j4) || !"compute_diagonal(): not a number encountered");
00096 assert(is_number(j5) || !"compute_diagonal(): not a number encountered");
00097 assert(is_number(j6) || !"compute_diagonal(): not a number encountered");
00098 assert(is_number(j7) || !"compute_diagonal(): not a number encountered");
00099 assert(is_number(j8) || !"compute_diagonal(): not a number encountered");
00100 assert(is_number(j9) || !"compute_diagonal(): not a number encountered");
00101 assert(is_number(j10) || !"compute_diagonal(): not a number encountered");
00102 assert(is_number(j11) || !"compute_diagonal(): not a number encountered");
00103 assert(is_number(j12) || !"compute_diagonal(): not a number encountered");
00104
00105 assert(is_number(wjt1) || !"compute_diagonal(): not a number encountered");
00106 assert(is_number(wjt2) || !"compute_diagonal(): not a number encountered");
00107 assert(is_number(wjt3) || !"compute_diagonal(): not a number encountered");
00108 assert(is_number(wjt4) || !"compute_diagonal(): not a number encountered");
00109 assert(is_number(wjt5) || !"compute_diagonal(): not a number encountered");
00110 assert(is_number(wjt6) || !"compute_diagonal(): not a number encountered");
00111 assert(is_number(wjt7) || !"compute_diagonal(): not a number encountered");
00112 assert(is_number(wjt8) || !"compute_diagonal(): not a number encountered");
00113 assert(is_number(wjt9) || !"compute_diagonal(): not a number encountered");
00114 assert(is_number(wjt10) || !"compute_diagonal(): not a number encountered");
00115 assert(is_number(wjt11) || !"compute_diagonal(): not a number encountered");
00116 assert(is_number(wjt12) || !"compute_diagonal(): not a number encountered");
00117
00118 d(i) = j1*wjt1 + j2*wjt2 + j3*wjt3 + j4*wjt4 + j5*wjt5 + j6*wjt6 + j7*wjt7 + j8*wjt8 + j9*wjt9 + j10*wjt10 + j11*wjt11 + j12*wjt12;
00119
00120 assert(is_number(d(i)) || !"compute_diagonal(): not a number encountered");
00121 }
00122 }
00123
00124 }
00125 }
00126 }
00127
00128 #endif
