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 }