void HapticsPSM::compute_average_normal(std::vector<tf::Vector3> &v_arr, tf::Vector3 &v){
tf::Vector3 new_n;
new_n.setValue(0,0,0);
if(v_arr.size() == 1){
new_n = v_arr.at(0);
}
else{
for(size_t i=0 ; i < v_arr.size() ; i++){
new_n = new_n + v_arr.at(i);
}
}
new_n.normalize();
if(v.length() == 0){
v = new_n; // This is for the first contact, since the current normal is zero, so assign the new value of the normal computed form collision checking
}
else{
//Check if new_n = 0 or in the negative direction as curr n, if it is, leave n unchanged
if(v.dot(new_n) == 0){ // If the dot product of the last and the current normal in 0, this could be due to the bug of getting negative normals
//v remains unchanged
ROS_INFO("New Normal Cancels out the previous one");
}
else if(v.dot(new_n) < 0 ){ //If the dot product of the last and the current normal in a negative number, this could be due to the bug of getting negative normals
v = -new_n;
ROS_INFO("New normal lies on the opposite plane");
}
else{
v = new_n;
}
}
}
/**
* Return vector of two Vector3s that form a basis for the space orthogonal to the ray
*/
std::vector<tf::Vector3> RayTracePluginUtils::getOrthogonalBasis(tf::Vector3 ray)
{
// ROS_INFO("Orthogonal Parts of %f, %f, %f", ray.getX(), ray.getY(), ray.getZ());
ray.normalize();
std::vector<tf::Vector3> v;
//Initialize vector on the most orthogonal axis
switch(ray.closestAxis()){
case 0:
v.push_back(tf::Vector3(0,0,1));
v.push_back(tf::Vector3(0,1,0));
break;
case 1:
v.push_back(tf::Vector3(0,0,1));
v.push_back(tf::Vector3(1,0,0));
break;
case 2:
default:
v.push_back(tf::Vector3(0,1,0));
v.push_back(tf::Vector3(1,0,0));
break;
}
//Recover the pure orthogonal parts
for(int i = 0; i < 2; i++){
v[i] = (v[i] - ray * ray.dot(v[i])).normalize();
// ROS_INFO("%f, %f, %f", v[i].getX(), v[i].getY(), v[i].getZ());
}
return v;
}
int extractFrame (const pcl::ModelCoefficients& coeffs,
const ARPoint& p1, const ARPoint& p2,
const ARPoint& p3, const ARPoint& p4,
tf::Matrix3x3 &retmat)
{
// Get plane coeffs and project points onto the plane
double a=0, b=0, c=0, d=0;
if(getCoeffs(coeffs, &a, &b, &c, &d) < 0)
return -1;
const tf::Vector3 q1 = project(p1, a, b, c, d);
const tf::Vector3 q2 = project(p2, a, b, c, d);
const tf::Vector3 q3 = project(p3, a, b, c, d);
const tf::Vector3 q4 = project(p4, a, b, c, d);
// Make sure points aren't the same so things are well-defined
if((q2-q1).length() < 1e-3)
return -1;
// (inverse) matrix with the given properties
const tf::Vector3 v = (q2-q1).normalized();
const tf::Vector3 n(a, b, c);
const tf::Vector3 w = -v.cross(n);
tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
// Possibly flip things based on third point
const tf::Vector3 diff = (q4-q3).normalized();
//ROS_INFO_STREAM("w = " << w << " and d = " << diff);
if (w.dot(diff)<0)
{
//ROS_INFO_STREAM("Flipping normal based on p3. Current value: " << m);
m[1] = -m[1];
m[2] = -m[2];
//ROS_INFO_STREAM("New value: " << m);
}
// Invert and return
retmat = m.inverse();
//cerr << "Frame is " << retmat << endl;
return 0;
}
void HapticsPSM::compute_deflection_along_normal(tf::Vector3 &n, tf::Vector3 &d, tf::Vector3 &d_along_n){
d_along_n = (d.dot(n)/n.length()) * n; //This gives us the dot product of current deflection in the direction of current normal
}