void ConvexPolygon::projectOnPlane(
const Eigen::Affine3f& pose, Eigen::Affine3f& output)
{
Eigen::Vector3f p(pose.translation());
Eigen::Vector3f output_p;
projectOnPlane(p, output_p);
// ROS_INFO("[ConvexPolygon::projectOnPlane] p: [%f, %f, %f]",
// p[0], p[1], p[2]);
// ROS_INFO("[ConvexPolygon::projectOnPlane] output_p: [%f, %f, %f]",
// output_p[0], output_p[1], output_p[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
coordinates().rotation() * Eigen::Vector3f::UnitZ());
Eigen::Quaternionf coords_rot(coordinates().rotation());
Eigen::Quaternionf pose_rot(pose.rotation());
// ROS_INFO("[ConvexPolygon::projectOnPlane] rot: [%f, %f, %f, %f]",
// rot.x(), rot.y(), rot.z(), rot.w());
// ROS_INFO("[ConvexPolygon::projectOnPlane] coords_rot: [%f, %f, %f, %f]",
// coords_rot.x(), coords_rot.y(), coords_rot.z(), coords_rot.w());
// ROS_INFO("[ConvexPolygon::projectOnPlane] pose_rot: [%f, %f, %f, %f]",
// pose_rot.x(), pose_rot.y(), pose_rot.z(), pose_rot.w());
// ROS_INFO("[ConvexPolygon::projectOnPlane] normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]);
// Eigen::Affine3f::Identity() *
// output.translation() = Eigen::Translation3f(output_p);
// output.rotation() = rot * pose.rotation();
//output = Eigen::Translation3f(output_p) * rot * pose.rotation();
output = Eigen::Affine3f(rot * pose.rotation());
output.pretranslate(output_p);
// Eigen::Vector3f projected_point = output * Eigen::Vector3f(0, 0, 0);
// ROS_INFO("[ConvexPolygon::projectOnPlane] output: [%f, %f, %f]",
// projected_point[0], projected_point[1], projected_point[2]);
}
void Kollision(CannonBall* cBall, RopeBall* rBall)
{
float Distance = length(cBall->pos - rBall->pos);
if (Distance <= (cBall->radius + rBall->radius))
{
printf("\n*** COLLISION WITH PENDULUM ***\n\n");
printf("Before collision:\n");
printf("Velocity: %.2f %.2f %.2f\n", cBall->linVel.x, cBall->linVel.y, cBall->linVel.z);
printf("Angular Velocity: %.2f %.2f %.2f\n", cBall->angVel.x, cBall->angVel.y, cBall->angVel.z);
const float e = 0.60f; //Coefficient of restitution
const float f = 0.16f; //Coefficient of friction
vec3f ep = normalize(cBall->pos - rBall->pos);
vec3f angularComponent = cross(ep,cBall->angVel) * cBall->radius;
vec3f cVel = cBall->linVel + angularComponent;
vec3f en = cross(normalize(cross((cVel - rBall->linVel), ep)), ep);
vec3f er1 = ep * -1.0f;
//velocity before kollision against ep
float v1p = dot(cBall->linVel, ep);
float v2p = dot(rBall->linVel, ep);
float sumMass = cBall->mass + rBall->mass;
//Velocity after kollision
float u1p = ((cBall->mass - rBall->mass*e) / sumMass)*v1p + (((1 - e)*rBall->mass) / sumMass)*v2p;
float u2p = (((1 - e)*cBall->mass) / sumMass) *v1p + ((rBall->mass - cBall->mass*e) / sumMass)*v2p;
vec3f cBallFinalU = cBall->linVel + (ep + en*f)*(u1p - v1p);
vec3f rBallFinalU = rBall->linVel + (ep + en*f)*(u2p - v2p);
//We assume the roll condition was not fullfilled
vec3f cAngVel = cross(er1, en)*(5.0f*f*(u1p - v1p) / (2.0f * cBall->radius));
cBall->linVel = cBallFinalU;
cBall->angVel = cBall->angVel + cAngVel;
//Make sure we don't get stuck in a collision loop
cBall->pos = rBall->pos + ep*(cBall->radius + rBall->radius + 0.1f);
vec3f tangentPlane = normalize(rBall->anchorpoint - rBall->pos);
vec3f rLinVel = projectOnPlane(rBallFinalU,tangentPlane);
vec3f rAngV = cross((rLinVel / rBall->ropelen), tangentPlane);
rBall->angVel = rAngV;
printf("After collision:\n");
printf("Velocity: %.2f %.2f %.2f\n",cBall->linVel.x, cBall->linVel.y, cBall->linVel.z);
printf("Angular Velocity: %.2f %.2f %.2f\n", cBall->angVel.x, cBall->angVel.y, cBall->angVel.z);
}
}
