1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm): SimTKcommon *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2005-12 Stanford University and the Authors. *
10 * Authors: Michael Sherman *
11 * Contributors: *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 #include "SimTKcommon.h"
25 #include "SimTKcommon/Testing.h"
26
27 using namespace SimTK;
28
29 #include <iostream>
30 #include <iomanip>
31 using std::cout;
32 using std::endl;
33 using std::setw;
34
testPhiMatrix()35 void testPhiMatrix() {
36 const Vec3 p = Test::randVec3();
37 PhiMatrix phi(p);
38 SpatialVec v1(Test::randSpatialVec());
39 SpatialMat m1(Test::randSpatialMat());
40
41 SimTK_TEST_EQ( phi*v1, SpatialVec(v1[0] + p%v1[1], v1[1]));
42 SimTK_TEST_EQ(~phi*v1, SpatialVec(v1[0], v1[1] - p%v1[0]));
43
44 SimTK_TEST_EQ(phi*v1, phi.toSpatialMat()*v1);
45 SimTK_TEST_EQ(phi*m1, phi.toSpatialMat()*m1);
46
47 SimTK_TEST_EQ(~phi*v1, (~phi).toSpatialMat()*v1);
48 SimTK_TEST_EQ(m1*~phi, m1*(~phi).toSpatialMat());
49 }
50
51 // TODO: this isn't a real regression test but it does catch
52 // compilation problems.
testMiscSpatialAlgebra()53 void testMiscSpatialAlgebra() {
54 SpatialVec v; SpatialRow r; SpatialMat m;
55 Mat33 m33(1,2,3,
56 4,5,6,
57 7,8,9);
58 Mat22 m22(10,20,
59 30,40);
60 cout << endl << "TEST: uninitialized should be NaN in Debug, random crap in Release" << endl;
61 cout << "rawVec=" << v << " rawRow=" << r << " rawMat=" << m;
62
63 cout << "SpatialMat::NRows=" << SpatialMat::NRows
64 << " SpatialMat::ArgDepth=" << CNT<SpatialMat>::ArgDepth
65 << " CNT<SpatialMat>::ArgDepth=" << SpatialMat::ArgDepth
66 << " SpatialRow::ArgDepth=" << SpatialRow::ArgDepth
67 << " SpatialVec::ArgDepth=" << SpatialVec::ArgDepth
68 << " Mat33::ArgDepth=" << Mat33::ArgDepth
69 << " CNT<Real>::ArgDepth=" << CNT<Real>::ArgDepth
70 << endl;
71
72 cout << endl << "TEST: set to 1; element wise for col/row; diagonal for mat" << endl;
73 v=1;r=1;m=1;
74 cout << "v=1:" << v << " r=1:" << r << " m=1:" << m;
75
76 SpatialVec sv( Vec3(1,2,-3), Vec3(.1,-.2,.3) );
77 Vec6& sv6 = Vec6::updAs(&sv[0][0]);
78 cout << "sv (" << &sv << ")=" << sv << endl;
79 cout << "sv6(" << &sv6 << ")=" << sv6 << endl;
80 cout << "sv.normalize() =" << sv.normalize() << endl;
81 cout << "sv6.normalize()=" << sv6.normalize() << endl;
82
83 SpatialVec::TNeg& nsv = sv.updNegate();
84 Vec6::TNeg& nsv6 = Vec6::TNeg::updAs(&nsv[0][0]);
85 cout << "nsv (" << &nsv << ")=" << nsv << endl;
86 cout << "nsv6(" << &nsv6 << ")=" << nsv6 << endl;
87 cout << "nsv.normalize() =" << nsv.normalize() << endl;
88 cout << "nsv6.normalize()=" << nsv6.normalize() << endl;
89
90 sv = sv.normalize();
91 cout << "after sv=sv.normalize(), sv=" << sv << endl;
92 cout << "now nsv+1=" << nsv+1 << ", (nsv+1).normalize()=" << (nsv+1).normalize() << endl;
93 nsv = (nsv+1).normalize();
94 cout << "after nsv=(nsv+1).normalize(), nsv=" << nsv << endl;
95
96 CNT<Vec3>::Result<double>::Mul xxx;
97
98 cout << endl << "TEST: v+v=" << v+v;
99 cout << endl << "TEST: v*3.=" << v*3.;
100 //cout << endl << "TEST: 3./v=" << 3./v;
101
102
103 cout << endl << "TEST: 3.f*v=" << 3.f*v;
104 cout << endl << "TEST: 3*v*2=" << 3*v*2;
105 cout << endl << "TEST: v/negator<Real>(3.)=" << v/negator<Real>(3.);
106 cout << endl << "TEST: v/complex<Real>(2.,3.)=" << v/complex<Real>(2.,3.);
107 cout << endl << "TEST: v/conjugate<Real>(2.,3.)=" << v/conjugate<Real>(2.,3.);
108 const conjugate<Real> cr23(2.,3.);
109 cout << endl << "TEST: v*negator<conjugate<Real>(2.,3.)>="
110 << v*negator<conjugate<Real> >::recast(cr23);
111 cout << endl << "TEST: v/negator<conjugate<Real>(2.,3.)>="
112 << v/negator<conjugate<Real> >::recast(cr23);
113 //cout << endl << "TEST: v-v=" << v-v;
114 //cout << endl << "TEST: r+r=" << r+r;
115 //cout << endl << "TEST: r-r=" << r-r;
116 //cout << endl << "TEST: m+m=" << m+m;
117 //cout << endl << "TEST: m-m=" << m-m;
118
119 // CONFORMING MULTIPLY
120
121 cout << endl << "TEST: multiply by identity matrix" << endl;
122 cout << "m*v:" << m*v << endl;
123 cout << " r*m:" << r*m << endl;
124 cout << " m*m:" << m*m;
125 cout << " m*m22:" << m*m22;
126
127 cout << endl << "TEST: scalar multiply, left and right" << endl;
128 cout << "v*9=" << v*9 << " 2*v=" << 2*v << endl;
129 cout << "r*9=" << r*9 << " 2*r=" << 2*r << endl;
130 cout << "m*9=" << m*9 << " 2*m=" << 2*m << endl;
131
132 cout << endl << "TEST: dot product" << endl;
133 cout << "r*v=" << r*v << " ~v*~r=" << ~v*~r << endl;
134
135 cout << endl << "TEST: outer product" << endl;
136 cout << "v*r=" << v*r << endl;
137 cout << " ~r*~v=" << ~r*~v << endl;
138
139 // NONCONFORMING MULTIPLY
140
141 cout << endl << "TEST: m*m33=" << m * m33 << " ~m*m33=" << ~m*m33;
142 cout << endl << "TEST: m33*m=" << m33 * m;
143 cout << endl << "TEST: r*m33=" << r * m33 << " ~v*m33=" << ~v*m33;
144 cout << endl << "TEST: m33*v=" << m33 * v << endl << endl;
145
146 Row<2, Mat33> m1233(Mat33(2), Mat33(3));
147 Vec3 v310(10);
148 SpatialRow sr = ~v310 * m1233;
149 cout << "v310=" << v310 << " m1233=" << m1233;
150 cout << "~v310*m1233=" << sr << endl;
151
152 cout << endl << "TEST: element multiply by non-scalar" << endl;
153 Row<2> rnew = r * Vec3(1,2,3);
154 cout << "r*vec(1,2,3)=" << rnew << endl;
155
156 // SYMMAT
157 SymMat<3> sy3 = SymMat<3>().setFromLower(Mat<3,3>(2, 99, 99,
158 3, 4, 99,
159 5, 6, 7));
160 SymMat<3> sy3d(-5);
161 cout << "sy3=" << sy3 << " sy3d=" << sy3d;
162 cout << "sy3+sy3d=" << sy3+sy3d;
163
164 // TRICKY CASE: 1x1 matrix, 1-row, 1-vec
165 Mat<1, 2, Row3> m11(Row3(1,2,3), Row3(4,5,6));
166 Row<1> r1(-.1);
167 Vec<2, Real> v1(10,20);
168 cout << "r1=" << r1 << " v1=" << v1 << " m11=" << m11;
169 cout << "m11*v1=" << m11*v1 << endl;
170 cout << "r1*m11=" << r1*m11 << endl;;
171 //cout << "r2*v1=" << r1*v1 << endl;
172 //cout << "v1*r1=" << v1*r1 << endl;;
173
174
175 MassProperties mprops(23, Vec3(1,2,3), UnitInertia::brick(.1,.2,.3));
176 cout << "MassProperties: " << mprops;
177 cout << "MassProperties.toSpatialMat: " << mprops.toSpatialMat();
178 cout << "MassProperties.toMat66: " << mprops.toMat66();
179 }
180
181
main()182 int main()
183 {
184 SimTK_START_TEST("SpatialAlgebraTest");
185 SimTK_SUBTEST(testPhiMatrix);
186 SimTK_SUBTEST(testMiscSpatialAlgebra);
187 SimTK_END_TEST();
188 }
189