#include <iostream>
#include <cmath>
#include <vector>
#include <stdlib.h>
#include <chrono>
class Transform {
public:
Transform() :
transform ({{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}})
{}
void setTransform(double x, double y, double z, double roll, double pitch, double yaw) {
double r11, r12, r13;
double r21, r22, r23;
double r31, r32, r33;
r11 = cos(pitch)*cos(yaw)-cos(roll)*sin(yaw)*sin(pitch);
r12 = cos(pitch)*sin(yaw)+cos(roll)*cos(yaw)*sin(pitch);
r13 = sin(pitch)*sin(roll);
r21 = -sin(pitch)*cos(yaw)-cos(roll)*sin(yaw)*cos(pitch);
r22 = -sin(pitch)*sin(yaw)+cos(roll)*cos(yaw)*cos(pitch);
r23 = cos(pitch)*sin(roll);
r31 = sin(roll)*sin(yaw);
r32 = -sin(roll)*cos(yaw);
r33 = cos(roll);
transform[0][0] = r11;
transform[0][1] = r12;
transform[0][2] = r13;
transform[1][0] = r21;
transform[1][1] = r22;
transform[1][2] = r23;
transform[2][0] = r31;
transform[2][1] = r32;
transform[2][2] = r33;
transform[0][3] = x;
transform[1][3] = y;
transform[2][3] = z;
}
Transform& operator=(const Transform &rhs) {
for (int x = 0; x < 4; x++) { // row number of output
for (int y = 0; y < 4; y++) { // column number of output
this->transform[x][y] = rhs.transform[x][y];
}
}
return *this;
}
Transform operator*(const Transform &rhs) {
Transform out;
for (int x = 0; x < 4; x++) { // row number of output
for (int y = 0; y < 4; y++) { // column number of output
out.transform[x][y] = 0;
for (int z = 0; z < 4; z++) { // four elements are added for this output
out.transform[x][y] += this->transform[x][z] * rhs.transform[z][y];
}
}
}
return out;
}
private:
std::vector<std::vector<double>> transform;
};
inline double genRand(double low, double high) {
return rand()%(int)high+low+high-(int)high;
}
int main() {
// generate 4 random transforms
std::cout << "Creating the transforms" << std::endl;
std::vector<Transform> transforms;
transforms.resize(4);
unsigned int seed = std::chrono::system_clock::now().time_since_epoch() / std::chrono::seconds(1);
std::cout << "seeding rand: " << seed << std::endl;
srand(seed);
std::cout << "Setting Transform" << std::endl;
for (int i = 0; i < 4; ++i) {
transforms[i].setTransform(genRand(0, 10), genRand(0, 10), genRand(0, 10), genRand(0, 4.14), genRand(0, 3.14), genRand(0, 3.14));
}
std::cout << "Starting Clock" << std::endl;
const auto start = std::chrono::system_clock::now();
for (unsigned long i = 0; i < 1000000; ++i) {
//combine the transforms
Transform c;
c = transforms[0] * transforms[1] * transforms[2] * transforms[3];
}
auto end = std::chrono::system_clock::now();
auto elapsed = end - start;
std::cout << "elapsed Time: " << elapsed / std::chrono::milliseconds(1) << "ms\n";
}
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