1 /*
2 *_________________________________________________________________________*
3 * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
4 * DESCRIPTION: SEE READ-ME *
5 * FILE NAME: onfunction.cpp *
6 * AUTHORS: See Author List *
7 * GRANTS: See Grants List *
8 * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
9 * LICENSE: Please see License Agreement *
10 * DOWNLOAD: Free at www.rpi.edu/~anderk5 *
11 * ADMINISTRATOR: Prof. Kurt Anderson *
12 * Computational Dynamics Lab *
13 * Rensselaer Polytechnic Institute *
14 * 110 8th St. Troy NY 12180 *
15 * CONTACT: anderk5@rpi.edu *
16 *_________________________________________________________________________*/
17
18
19 #include "onfunctions.h"
20 #include "matrixfun.h"
21 #include <iostream>
22 using namespace std;
23
24 // friend of Vect3 & Vect6
OnPopulateSVect(Vect3 & angular,Vect3 & linear,Vect6 & sV)25 void OnPopulateSVect(Vect3& angular, Vect3& linear, Vect6& sV){
26 sV.elements[0] = angular.elements[0];
27 sV.elements[1] = angular.elements[1];
28 sV.elements[2] = angular.elements[2];
29 sV.elements[3] = linear.elements[0];
30 sV.elements[4] = linear.elements[1];
31 sV.elements[5] = linear.elements[2];
32 }
33
34 // friend of Vect3, Mat3x3, & Mat6x6
OnPopulateSC(Vect3 & gamma,Mat3x3 & C,Mat6x6 & SC)35 void OnPopulateSC(Vect3& gamma, Mat3x3& C, Mat6x6& SC){
36 // the block diagonals
37
38 // the gamma cross with transform
39 Mat3x3 temp; Mat3x3 temp2;
40 SC.Zeros();
41 temp.Zeros();
42 temp2.Zeros();
43 //FastTMult(C,gamma,temp);
44 temp(1,2)= -gamma(3); temp(1,3)= gamma(2); temp(2,1)= gamma(3);
45 temp(2,3)= -gamma(1); temp(3,1)= -gamma(2); temp(3,2)= gamma(1);
46 FastMult(temp,C,temp2);
47
48 SC(1,4)=temp2(1,1); SC(2,4)=temp2(2,1); SC(3,4)=temp2(3,1);
49 SC(1,5)=temp2(1,2); SC(2,5)=temp2(2,2); SC(3,5)=temp2(3,2);
50 SC(1,6)=temp2(1,3); SC(2,6)=temp2(2,3); SC(3,6)=temp2(3,3);
51
52 SC(1,1)=C(1,1); SC(2,1)=C(2,1); SC(3,1)=C(3,1);
53 SC(1,2)=C(1,2); SC(2,2)=C(2,2); SC(3,2)=C(3,2);
54 SC(1,3)=C(1,3); SC(2,3)=C(2,3); SC(3,3)=C(3,3);
55
56 SC(4,4)=C(1,1); SC(5,4)=C(2,1); SC(6,4)=C(3,1);
57 SC(4,5)=C(1,2); SC(5,5)=C(2,2); SC(6,5)=C(3,2);
58 SC(4,6)=C(1,3); SC(5,6)=C(2,3); SC(6,6)=C(3,3);
59
60 }
61
62 // friend of Mat3x3 & Mat6x6
OnPopulateSI(Mat3x3 & inertia,double mass,Mat6x6 & sI)63 void OnPopulateSI(Mat3x3& inertia, double mass, Mat6x6& sI){
64
65 sI(4,4)=mass; sI(5,5)=mass; sI(6,6)=mass;
66 sI(1,1)=inertia(1,1); sI(1,2)=inertia(1,2); sI(1,3)=inertia(1,3);
67 sI(2,1)=inertia(2,1); sI(2,2)=inertia(2,2); sI(2,3)=inertia(2,3);
68 sI(3,1)=inertia(3,1); sI(3,2)=inertia(3,2); sI(3,3)=inertia(3,3);
69 }
70