int main(){//int argc, char **argv) {
clock_t start;
double diff;
start = clock();
Eigen::VectorXd Torque(7);
Eigen::VectorXd x(14);
x << 1,1,1,1,1,1,1,1,1,1,1,1,1,1;
x = 0.1*x;
// std::cout << x << std::endl;
Torque = Mass_matrix(x)*x.block(0,0,7,1) + C_Vector(x);
diff = (double) (std::clock() - start) / (double) CLOCKS_PER_SEC * 1000;
cout << "printf: " << diff << '\n';
cout << "Torque: " << x << '\n';
return 0;
}
/**
* Gets the torque, product of the force applied in a particular point.
* @param mass the mass of the body on which the force is applied.
*/
Torque Force::getToque(const Vec& point) const {
return Torque(point ^ ((Vec&) *this));
}
