int main(int argc, char **argv) {
vec3 vector, vectorMultiplied;
vec2 add;
mat3x3 transform;
vector.x = 3;
vector.y = 0;
vector.z = 1;
add.x = 1000;
add.y = 1000;
printVec3(vector);
vectorMultiplied = vector;
mat3x3Identity(transform);
mat3x3Rotate2D(transform, PI / 4);
mat3x3Scale2D(transform, 2);
mat3x3Translate(transform, add);
printMat3x3(transform);
printVec3(mat3x3GetCol(transform, 1));
printf("Applying transformation matrix...\n\n");
vectorMultiplied = mat3x3MultiplyVector(transform, vector);
printVec3(vectorMultiplied);
printf("Length: %.2f\n", vec2Length(vector.vec2));
return 0;
}
void test_updateWheelContactGeom() {
const Real rad = .325;
WheelContactGeom contact;
//terrain
SurfaceVector surfs;
//flat(surfs);
//ramp(surfs);
grid(surfs, ResourceDir() + std::string("gridsurfdata.txt") );
//wheel pose
HomogeneousTransform HT_wheel_to_world;
VecOrient orient = {0,0,0}; //assume WMRSIM_USE_QUATERNION 0
Vec3 pos = {2.5, -1.0, rad};
//adjust height of wheel
Real zsurf;
surfacesHeight(surfs,pos,zsurf);
pos[2] += zsurf;
poseToHT(orient,pos,HT_wheel_to_world); //assumes WMRSIM_USE_QUATERNION 0
updateWheelContactGeom(surfs, HT_wheel_to_world, rad, contact );
//print
std::cout << "wheel pos = \n";
printVec3(pos,-1,-1);
std::cout << "dz = " << contact.dz << std::endl;
std::cout << "contact angle (deg)= " << RADTODEG(contact.angle) << std::endl;
std::cout << "\nHT_contact_to_wheel=\n";
printHT(contact.HT_wheel,-1,-1);
std::cout << "\nHT_contact_to_world=\n";
printHT(contact.HT_world,-1,-1);
if (1) {
//time it
int nw = 4;
timeval t0, t1;
int n= (int) 1e5;
gettimeofday(&t0, NULL);
for (int i=0; i<n*nw; i++) {
updateWheelContactGeom(surfs, HT_wheel_to_world, rad, contact );
}
gettimeofday(&t1, NULL);
std::cout << "num wheels: " << nw << std::endl;
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
}
}
void test_updateTrackContactGeom() {
const Real rad = .2;
const Real rad2 = .2;
const Real L = .5;
TrackContactGeom contact;
initTrackContactGeom(rad,rad2,L,contact);
const int np = contact.get_np();
//terrain
SurfaceVector surfs;
//flat(surfs);
//ramp(surfs);
grid(surfs, ResourceDir() + std::string("gridsurfdata.txt") );
//track pose
HomogeneousTransform HT_track_to_world;
VecOrient orient = {0,0,0}; //assume WMRSIM_USE_QUATERNION 0
Vec3 pos = {3.0, 0, rad-.01};
//adjust height of track
Real zsurf;
surfacesHeight(surfs,pos,zsurf);
pos[2] += zsurf;
poseToHT(orient,pos,HT_track_to_world);
updateTrackContactGeom(surfs, HT_track_to_world, contact );
//print
std::cout << "track pos = \n";
printVec3(pos,-1,-1);
std::cout << "dz=\n"; printMatReal(1,np,contact.dz,-1,-1); std::cout << std::endl;
}
void test_transform() {
VecEuler euler;
Vec3 translation;
HomogeneousTransform HT,HT2;
setEuler(DEGTORAD(10),DEGTORAD(20),DEGTORAD(30),euler);
setVec3(1,2,3,translation);
poseToHT(euler,translation,HT);
std::cout << "HT=\n"; printHT(HT,-1,-1);
//test rotation matrix
Mat3 Rx,Ry,Rz,Rot,Tmp;
Rotx(euler[0],Rx);
Roty(euler[1],Ry);
Rotz(euler[2],Rz);
multMatMat3(Ry,Rx,Tmp);
multMatMat3(Rz,Tmp,Rot);
std::cout << "Rotz(yaw)*Roty(pit)*Rotx(rol)=\n"; printMat3(Rot,-1,-1);
//test invert transform
invertHT(HT,HT2);
std::cout << "inv(HT)=\n"; printHT(HT2,-1,-1);
//test compose HT
if (1) {
//time it
int n = (int) 1e8;
timeval t0, t1;
//compose Homogeneous Transforms
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
composeHT(HT,HT,HT2);
}
gettimeofday(&t1, NULL);
std::cout << "HT*HT=\n"; printHT(HT2,-1,-1);
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
} else {
composeHT(HT,HT,HT2);
std::cout << "HT*HT=\n"; printHT(HT2,-1,-1);
}
composeInvHT(HT,HT,HT2);
std::cout << "inv(HT)*HT=\n"; printHT(HT2,-1,-1);
//test transform point
Vec3 p,q;
setVec3(2,3,4,p);
std::cout << "p=\n"; printVec3(p,-1,-1);
applyHT(HT,p,q);
std::cout << "q=HT*p=\n"; printVec3(q,-1,-1);
applyInvHT(HT,p,q);
std::cout << "q=inv(HT)*p=\n"; printVec3(q,-1,-1);
//test converting between angular velocity and Euler rates
Vec3 vel={1,2,3};
VecEuler eulerrate;
MatEuler T;
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
velToEulerrate(euler,vel,eulerrate,T);
std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1);
std::cout << "T_vel_to_eulerrate=\n"; printMatReal(3,3,T,-1,-1);
std::cout << "convert back:\n";
eulerrateToVel(euler,eulerrate,vel,0);
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
std::cout << std::endl;
//test at singularity
setEuler(0,M_PI/2,0,euler);
setVec3(0,0,1,vel);
velToEulerrate(euler,vel,eulerrate,T);
std::cout << "test at singularity:\n";
std::cout << "euler=\n"; printEuler(euler,-1,-1);
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1);
}
