bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
Eigen::Affine3f& transf)
{
// uz: versor pointing toward the destination
Eigen::Vector3f uz = target - rayo;
if (std::abs(uz.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
return false;
}
uz.normalize();
//std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
// ux: versor pointing toward the ground
Eigen::Vector3f ux = down - down.dot(uz) * uz;
if (std::abs(ux.norm()) < 1e-3) {
std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
return false;
}
ux.normalize();
//std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
Eigen::Vector3f uy = uz.cross(ux);
//std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
Eigen::Matrix3f rot;
rot << ux.x(), uy.x(), uz.x(),
ux.y(), uy.y(), uz.y(),
ux.z(), uy.z(), uz.z();
transf.setIdentity();
transf.translate(rayo);
transf.rotate(rot);
//std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
return true;
}
void fuzzyAffines()
{
std::vector<Eigen::Matrix4f> trans;
trans.reserve(count/10);
for( size_t i=0; i<count/10; i++ )
{
Eigen::Vector3f x = Eigen::Vector3f::Random();
Eigen::Vector3f y = Eigen::Vector3f::Random();
x.normalize();
y.normalize();
Eigen::Vector3f z = x.cross(y);
z.normalize();
y = z.cross(x);
y.normalize();
Eigen::Affine3f t = Eigen::Affine3f::Identity();
Eigen::Matrix3f r = Eigen::Matrix3f::Identity();
r.col(0) = x;
r.col(1) = y;
r.col(2) = z;
t.rotate(r);
t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );
trans.push_back( t.matrix() );
}
s_plot.setColor( Eigen::Vector4f(1,0,0,1) );
s_plot.setLineWidth( 3.0 );
s_plot( trans, nox::plot<float>::Pos | nox::plot<float>::CS );
}
/**
* @brief Computes the trackball's rotation, using stored initial and final position vectors.
*/
void computeRotationAngle (void)
{
//Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.
if(initialPosition.norm() > 0) {
initialPosition.normalize();
}
if(finalPosition.norm() > 0) {
finalPosition.normalize();
}
//cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;
Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);
if(rotationAxis.norm() != 0) {
rotationAxis.normalize();
}
float dot = initialPosition.dot(finalPosition);
float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.
Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));
quaternion = q * quaternion;
quaternion.normalize();
}
/* http://geomalgorithms.com/a07-_distance.html */
void intersectLines(const Eigen::Vector3f &p0, const Eigen::Vector3f &p1,
const Eigen::Vector3f &q0, const Eigen::Vector3f &q1,
Eigen::Vector3f &pointOnP, Eigen::Vector3f &pointOnQ)
{
Eigen::Vector3f w0 = p0 - q0;
/* the two vectors on the lines */
Eigen::Vector3f u = p1 - p0;
u.normalize();
Eigen::Vector3f v = q0 - q1;
v.normalize();
float a = u.dot(u);
float b = u.dot(v);
float c = v.dot(v);
float d = u.dot(w0);
float e = v.dot(w0);
float normFactor = a*c - b*b;
float sc = (b*e - c*d) / normFactor;
float tc = (a*e - b*d) / normFactor;
/* the two nearest points on the lines */
Eigen::ParametrizedLine<float, 3> lineP(p0, u);
pointOnP = lineP.pointAt(sc);
Eigen::ParametrizedLine<float, 3> lineQ(q0, v);
pointOnQ = lineQ.pointAt(tc);
}
/**
* Verifica si un plano corta a una nube en dos
* @param pc nube de puntos a evaluar
* @param coefs Coeficientes del plano
* @return True si lo corta, false en caso contrario.
*/
bool isPointCloudCutByPlane(PointCloud<PointXYZ>::Ptr pc, ModelCoefficients::Ptr coefs, PointXYZ p_plane){
/*
Algoritmo:
- Obtener vector normal a partir de coeficientes
- Iterar sobre puntos de la nube
- Calcular vector delta entre punto entregado (dentro del plano) y punto iterado
- Calcular ángulo entre vector delta y normal
- Angulos menores a PI/2 son de un lado, mayores son de otro
- Si aparecen puntos de ambos lados, retornar True, else, false.
*/
const double PI = 3.1416;
Eigen::Vector3f normal = Eigen::Vector3f(coefs->values[0], coefs->values[1], coefs->values[2]);
normal.normalize();
// cout << "Normal: " << normal << endl;
bool side;
// Iterar sobre los puntos
for (int i=0; i<pc->points.size(); i++){
// Calcular ángulo entre punto y normal
Eigen::Vector3f delta = Eigen::Vector3f (p_plane.x, p_plane.y, p_plane.z) - Eigen::Vector3f(pc->points[i].x, pc->points[i].y, pc->points[i].z);
delta.normalize();
double alpha = acos(normal.dot(delta));
// printf ("Alpha: %f°\n", (alpha*180/PI));
if (i==0){
side = (alpha < PI/2.0);
// printf("Lado escogido: %s", side ? "true": "false");
continue;
}
if (side != (alpha < PI/2.0)){
// printf("Nube es cortada por plano\n");
return true;
}
}
// printf("Nube NO es cortada por plano\n");
return false;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
eigenVectors_->clear();
centroids_->clear();
::pcl::ExtractIndices<Point> filter;
filter.setInputCloud(input);
rectangles_->resize(static_cast<std::size_t>(clusters_->size()));
for(std::size_t i = 0; i < clusters_->size(); ++i)
{
rectangles_->at(i).resize(4);
}
for(std::size_t i = 0; i < clusters_->size(); ++i)
{
if(clusters_->at(i).indices.size() < 3)
continue;
boost::shared_ptr< ::pcl::PointCloud<Point> > cloud;
cloud = boost::make_shared< ::pcl::PointCloud<Point> > ();
// extract indices into a cloud
filter.setIndices( ::pcl::PointIndicesPtr(
new ::pcl::PointIndices ((*clusters_)[i])) );
filter.filter(*cloud);
// extract the eigen vectors
::pcl::PointCloud< ::pcl::PointXYZ> proj;
pcl::PCA <Point > pca;
pca.setInputCloud(cloud);
eigenVectors_->push_back(pca.getEigenVectors());
centroids_->push_back(pca.getMean());
//generate the rectangles
Eigen::Vector3f center = Eigen::Vector3f(pca.getMean().x(),
pca.getMean().y(),
pca.getMean().z());
Eigen::Vector3f longAxis = Eigen::Vector3f(pca.getEigenVectors()(0,0),
pca.getEigenVectors()(1,0),
pca.getEigenVectors()(2,0));
Eigen::Vector3f shortAxis = Eigen::Vector3f(pca.getEigenVectors()(0,1),
pca.getEigenVectors()(1,1),
pca.getEigenVectors()(2,1));
longAxis.normalize();
longAxis = (*length_rectangles_)*longAxis;
shortAxis.normalize();
shortAxis = (*width_rectangles_)*shortAxis;
rectangles_->at(i)[0] = center - longAxis/2 - shortAxis/2;
rectangles_->at(i)[1] = center + longAxis/2 - shortAxis/2;
rectangles_->at(i)[2] = center + longAxis/2 + shortAxis/2;
rectangles_->at(i)[3] = center - longAxis/2 + shortAxis/2;
}
return ecto::OK;
}
/**
* @brief Compose rotation and translation
*/
void updateViewMatrix() override
{
Eigen::Vector3f xAxis = rotation_matrix.row(0);
Eigen::Vector3f yAxis = rotation_matrix.row(1);
Eigen::Vector3f zAxis = rotation_matrix.row(2);
if( rotation_Z_axis )
{
Eigen::AngleAxisf transfRotZ = Eigen::AngleAxisf(rotation_Z_axis, zAxis);
// compute X axis restricted to a rotation around Z axis
xAxis = transfRotZ * xAxis;
xAxis.normalize();
// compute Y axis restricted to a rotation around Z axis
yAxis = transfRotZ * yAxis;
yAxis.normalize();
rotation_Z_axis = 0.0;
}
Eigen::AngleAxisf transfRotY = Eigen::AngleAxisf(rotation_Y_axis, yAxis);
// compute X axis restricted to a rotation around Y axis
Eigen::Vector3f rotX = transfRotY * xAxis;
rotX.normalize();
Eigen::AngleAxisf transfRotX = Eigen::AngleAxisf(rotation_X_axis, rotX);
// rotate Z axis around Y axis, then rotate new Z axis around X new axis
Eigen::Vector3f rotZ = transfRotY * zAxis;
rotZ = transfRotX * rotZ;
rotZ.normalize();
// rotate Y axis around X new axis
Eigen::Vector3f rotY = transfRotX * yAxis;
rotY.normalize();
rotation_matrix.row(0) = rotX;
rotation_matrix.row(1) = rotY;
rotation_matrix.row(2) = rotZ;
resetViewMatrix();
view_matrix.rotate (rotation_matrix);
view_matrix.translate (translation_vector);
rotation_X_axis = 0.0;
rotation_Y_axis = 0.0;
}
png::image<png::rgb_pixel_16> bumpier::model::calculate_normals(const png::image<png::gray_pixel_16>& input, double phi, space_type space) const
{
int width = input.get_width(), height = input.get_height();
png::image<png::rgb_pixel_16> output(width, height);
for (int y = 0; y < height; y++)
for (int x = 0; x < width; x++)
{
int xmin = (x + width - 1) % width,
ymin = (y + height - 1) % height,
xmax = (x + 1) % width,
ymax = (y + 1) % height;
Eigen::Vector3f normal = (position(input, x, ymax) - position(input, x, ymin)).cross(
position(input, xmax, y) - position(input, xmin, y));
if(space == tangent_space)
normal = tangentspace(Eigen::Vector2f((float)x / width, (float)y / height)) * normal;
normal.normalize();
normal = (normal.array() * 0.5 + 0.5) * 0xFFFF;
output.set_pixel(x, y, png::rgb_pixel_16(normal[0], normal[1], normal[2]));
}
return output;
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::randomOrthogonalAxis (
Eigen::Vector3f const &axis,
Eigen::Vector3f &rand_ortho_axis)
{
if (!areEquals (axis.z (), 0.0f))
{
rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
}
else if (!areEquals (axis.y (), 0.0f))
{
rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
}
else if (!areEquals (axis.x (), 0.0f))
{
rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
}
rand_ortho_axis.normalize ();
// check if the computed x axis is orthogonal to the normal
//assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f));
}
pcl::PointCloud<pcl::PointNormal>::Ptr
meshToFaceCloud (const pcl::PolygonMesh &mesh)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZ> vertices;
pcl::fromPCLPointCloud2 (mesh.cloud, vertices);
for (size_t i = 0; i < mesh.polygons.size (); ++i)
{
if (mesh.polygons[i].vertices.size () != 3)
{
PCL_ERROR ("Found a polygon of size %d\n", mesh.polygons[i].vertices.size ());
continue;
}
Eigen::Vector3f v0 = vertices.at (mesh.polygons[i].vertices[0]).getVector3fMap ();
Eigen::Vector3f v1 = vertices.at (mesh.polygons[i].vertices[1]).getVector3fMap ();
Eigen::Vector3f v2 = vertices.at (mesh.polygons[i].vertices[2]).getVector3fMap ();
float area = ((v1 - v0).cross (v2 - v0)).norm () / 2. * 1E4;
Eigen::Vector3f normal = ((v1 - v0).cross (v2 - v0));
normal.normalize ();
pcl::PointNormal p_new;
p_new.getVector3fMap () = (v0 + v1 + v2)/3.;
p_new.normal_x = normal (0);
p_new.normal_y = normal (1);
p_new.normal_z = normal (2);
cloud->points.push_back (p_new);
}
cloud->height = 1;
cloud->width = cloud->size ();
return (cloud);
}
void Kamikaze::updateEnemy(Level * currLev) {
//todo move at ship;
if (!dead) {
Eigen::Vector3f shipLoc = currLev->ship->position;
Eigen::Vector3f direction = shipLoc - position;
direction.normalize();
direction /= 1000;
direction.z() -= currLev->ship->velocity.z();
Eigen::Vector3f mousePos = Eigen::Vector3f(shipLoc.x(), shipLoc.y(), shipLoc.z());
float rotateY = RADIANS_TO_DEGREES(atan((shipLoc.x() - position.x()) / (shipLoc.z() - position.z()))) - 90.0;
float rotateX = -RADIANS_TO_DEGREES(atan((shipLoc.y() - position.y()) / (shipLoc.z() - position.z())));
rotate.y() = rotateY;
rotate.x() = rotateX;
// if we're behind ship, keep going in that direction
if (direction.z() <= 0) {
direction[2] *= -1;
direction[2] += 0.2;
}
velocity = direction;
}
}
void StarCamera::calculateSpotVectors()
{
if(mSpots.empty())
throw std::runtime_error("No extracted spots in List");
bool zeroNorm = !(mDistortionCoeffi.norm() != 0.0f);
// Clear SpotVectors
mSpotVectors.clear();
std::vector<Spot>::iterator pSpot;
for(pSpot=mSpots.begin(); pSpot != mSpots.end(); ++pSpot)
{
// Substract principal point and divide by the focal length
Eigen::Vector2f Xd(pSpot->center.x, pSpot->center.y);
Xd = (Xd - mPrincipalPoint).array() / mFocalLength.array();
// Undo skew
Xd(0) = Xd(0) - mPixelSkew * Xd(1);
Eigen::Vector3f spotVec;
if(!zeroNorm) // Use epsilon environment?
{
Xd = undistortRadialTangential(Xd);
}
spotVec << Xd(0), Xd(1), 1.0f;
spotVec.normalize();
mSpotVectors.push_back(spotVec);
}
}
template <typename PointNT> bool
pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
std::vector <int> &pt_union_indices)
{
assert (end_pts.size () == 2);
assert (vect_at_end_pts.size () == 2);
double length[2];
for (size_t i = 0; i < 2; ++i)
{
length[i] = vect_at_end_pts[i].norm ();
vect_at_end_pts[i].normalize ();
}
double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
if (dot_prod < 0)
{
double ratio = length[0] / (length[0] + length[1]);
Eigen::Vector4f start_pt =
end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
Eigen::Vector3f vec;
getVectorAtPoint (intersection_pt, pt_union_indices, vec);
vec.normalize ();
double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
if (d2 < 0)
return (true);
}
return (false);
}
template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
const Eigen::Vector3f& origin,
const Eigen::Vector3f& end,
AlignedPointTVector &voxel_center_list,
float precision)
{
Eigen::Vector3f direction = end - origin;
float norm = direction.norm ();
direction.normalize ();
const float step_size = static_cast<const float> (resolution_) * precision;
// Ensure we get at least one step for the first voxel.
const int nsteps = std::max (1, static_cast<int> (norm / step_size));
OctreeKey prev_key;
bool bkeyDefined = false;
// Walk along the line segment with small steps.
for (int i = 0; i < nsteps; ++i)
{
Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
PointT octree_p;
octree_p.x = p.x ();
octree_p.y = p.y ();
octree_p.z = p.z ();
OctreeKey key;
this->genOctreeKeyforPoint (octree_p, key);
// Not a new key, still the same voxel.
if ((key == prev_key) && (bkeyDefined) )
continue;
prev_key = key;
bkeyDefined = true;
PointT center;
genLeafNodeCenterFromOctreeKey (key, center);
voxel_center_list.push_back (center);
}
OctreeKey end_key;
PointT end_p;
end_p.x = end.x ();
end_p.y = end.y ();
end_p.z = end.z ();
this->genOctreeKeyforPoint (end_p, end_key);
if (!(end_key == prev_key))
{
PointT center;
genLeafNodeCenterFromOctreeKey (end_key, center);
voxel_center_list.push_back (center);
}
return (static_cast<int> (voxel_center_list.size ()));
}
Eigen::Vector3f RayTracer::findReflect(Eigen::Vector3f ray, Eigen::Vector3f normal, SceneObject* obj) {
Eigen::Vector3f reflectRay;
reflectRay = ray + (2.0*normal*(normal.dot(-ray)));
reflectRay.normalize();
return reflectRay;
}
bool intersectWithCylinder(float radius, float m_x, float m_z, cv::Point2f pixel,const cv::Mat &P, const cv::Mat projector_position, cv::Point3f& S, float& out_y, float& out_phi, float &hit_angle){
// get one point that projects on the given pixel:
cv::Point3f point_on_ray;
project3D(pixel, P, 1, point_on_ray);
cv::Point3f center(projector_position.at<double>(0),projector_position.at<double>(1),projector_position.at<double>(2));
bool intersects_with_cylinder = intersect(center, point_on_ray, m_x,m_z, radius, S);
if (!intersects_with_cylinder) return false;
// HACK!
if (S.z > 0) return false;
out_y = S.y;
out_phi = atan2(S.x-m_x,-(S.z-m_z))/M_PI*180;
// compute angle between (projector, S) and (Center,S)
Eigen::Vector3f PS;
PS.x() = projector_position.at<double>(0)-S.x;
PS.y() = projector_position.at<double>(1)-S.y;
PS.z() = projector_position.at<double>(2)-S.z;
PS.normalize();
Eigen::Vector3f MS;
MS.x() = S.x-m_x;
MS.y() = 0;
MS.z() = S.z-m_z;
MS.normalize();
hit_angle = acos(PS.dot(MS))/M_PI*180;
return true;
}
void compute_plane(Eigen::Vector4f& plane, const pcl::PointCloud<pcl::PointXYZ>& points, int* inds)
{
Eigen::Vector3f first = points[inds[1]].getVector3fMap() - points[inds[0]].getVector3fMap();
Eigen::Vector3f second = points[inds[2]].getVector3fMap() - points[inds[0]].getVector3fMap();
Eigen::Vector3f normal = first.cross(second);
normal.normalize();
plane.segment<3>(0) = normal;
plane(3) = -normal.dot(points[inds[0]].getVector3fMap());
}
// Rotate the Vector 'normal_to_rotate' into 'base_normal'
// Returns a rotation matrix which can be used for the transformation
// The matrix also includes an empty translation vector
Eigen::Matrix< float, 4, 4 > rotateAroundCrossProductOfNormals(
Eigen::Vector3f base_normal,
Eigen::Vector3f normal_to_rotate)
{
// M
// Eigen::Vector3f plane_normal(dest.x, dest.y, dest.z);
// Compute the necessary rotation to align a face of the object with the camera's
// imaginary image plane
// N
// Eigen::Vector3f camera_normal;
// camera_normal(0)=0;
// camera_normal(1)=0;
// camera_normal(2)=1;
// Eigen::Vector3f camera_normal_normalized = camera_normal.normalized();
float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );
Eigen::Vector3f axis;
Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
// axis = plane_normal.cross(camera_normal) / (plane_normal.cross(camera_normal)).normalize();
firstAxis.normalize();
axis=firstAxis;
float c = costheta;
std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
float s = sqrt(1-c*c);
float CO = 1-c;
float x = axis(0);
float y = axis(1);
float z = axis(2);
Eigen::Matrix< float, 4, 4 > rotationBox;
rotationBox(0,0) = x*x*CO+c;
rotationBox(1,0) = y*x*CO+z*s;
rotationBox(2,0) = z*x*CO-y*s;
rotationBox(0,1) = x*y*CO-z*s;
rotationBox(1,1) = y*y*CO+c;
rotationBox(2,1) = z*y*CO+x*s;
rotationBox(0,2) = x*z*CO+y*s;
rotationBox(1,2) = y*z*CO-x*s;
rotationBox(2,2) = z*z*CO+c;
// Translation vector
rotationBox(0,3) = 0;
rotationBox(1,3) = 0;
rotationBox(2,3) = 0;
// The rest of the 4x4 matrix
rotationBox(3,0) = 0;
rotationBox(3,1) = 0;
rotationBox(3,2) = 0;
rotationBox(3,3) = 1;
return rotationBox;
}
void motion3(Eigen::Matrix3f &rot, Eigen::Vector3f &tr)
{
Eigen::Vector3f n;
n(0) = gen_random_float(-1,1);
n(1) = gen_random_float(-1,1);
n(2) = gen_random_float(-1,1);
n.normalize();
Eigen::AngleAxisf aa(gen_random_float(-0.02f,0.05f),n);
rot = aa.toRotationMatrix();
tr.fill(0);
}
void GenericDistanceConstraint::gradientFct(
const unsigned int i,
const unsigned int numberOfParticles,
const float mass[],
const Eigen::Vector3f x[],
void *userData,
Eigen::Matrix<float, 1, 3> &jacobian)
{
Eigen::Vector3f n = x[i] - x[1 - i];
n.normalize();
jacobian = n.transpose();
}
inline void
randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, double totalArea, Eigen::Vector3f& p, bool calcNormal, Eigen::Vector3f& n, bool calcColor, Eigen::Vector3f& c)
{
float r = static_cast<float> (uniform_deviate (rand ()) * totalArea);
std::vector<double>::iterator low = std::lower_bound (cumulativeAreas->begin (), cumulativeAreas->end (), r);
vtkIdType el = vtkIdType (low - cumulativeAreas->begin ());
double A[3], B[3], C[3];
vtkIdType npts = 0;
vtkIdType *ptIds = NULL;
polydata->GetCellPoints (el, npts, ptIds);
polydata->GetPoint (ptIds[0], A);
polydata->GetPoint (ptIds[1], B);
polydata->GetPoint (ptIds[2], C);
if (calcNormal)
{
// OBJ: Vertices are stored in a counter-clockwise order by default
Eigen::Vector3f v1 = Eigen::Vector3f (A[0], A[1], A[2]) - Eigen::Vector3f (C[0], C[1], C[2]);
Eigen::Vector3f v2 = Eigen::Vector3f (B[0], B[1], B[2]) - Eigen::Vector3f (C[0], C[1], C[2]);
n = v1.cross (v2);
n.normalize ();
}
float r1 = static_cast<float> (uniform_deviate (rand ()));
float r2 = static_cast<float> (uniform_deviate (rand ()));
randomPointTriangle (float (A[0]), float (A[1]), float (A[2]),
float (B[0]), float (B[1]), float (B[2]),
float (C[0]), float (C[1]), float (C[2]), r1, r2, p);
if (calcColor)
{
vtkUnsignedCharArray *const colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
if (colors && colors->GetNumberOfComponents () == 3)
{
double cA[3], cB[3], cC[3];
colors->GetTuple (ptIds[0], cA);
colors->GetTuple (ptIds[1], cB);
colors->GetTuple (ptIds[2], cC);
randomPointTriangle (float (cA[0]), float (cA[1]), float (cA[2]),
float (cB[0]), float (cB[1]), float (cB[2]),
float (cC[0]), float (cC[1]), float (cC[2]), r1, r2, c);
}
else
{
static bool printed_once = false;
if (!printed_once)
PCL_WARN ("Mesh has no vertex colors, or vertex colors are not RGB!");
printed_once = true;
}
}
}
Eigen::Matrix< float, 4, 4 > IAMethod::rotateAroundCrossProductOfNormals(
Eigen::Vector3f base_normal,
Eigen::Vector3f normal_to_rotate,
bool store_transformation)
{
normal_to_rotate *= -1; // The model is standing upside down, rotate the normal by 180 DEG
float costheta = normal_to_rotate.dot(base_normal) / (normal_to_rotate.norm() * base_normal.norm() );
Eigen::Vector3f axis;
Eigen::Vector3f firstAxis = normal_to_rotate.cross(base_normal);
firstAxis.normalize();
axis=firstAxis;
float c = costheta;
std::cout << "rotate COSTHETA: " << acos(c) << " RAD, " << ((acos(c) * 180) / M_PI) << " DEG" << std::endl;
float s = sqrt(1-c*c);
float CO = 1-c;
float x = axis(0);
float y = axis(1);
float z = axis(2);
Eigen::Matrix< float, 4, 4 > rotationBox;
rotationBox(0,0) = x*x*CO+c;
rotationBox(1,0) = y*x*CO+z*s;
rotationBox(2,0) = z*x*CO-y*s;
rotationBox(0,1) = x*y*CO-z*s;
rotationBox(1,1) = y*y*CO+c;
rotationBox(2,1) = z*y*CO+x*s;
rotationBox(0,2) = x*z*CO+y*s;
rotationBox(1,2) = y*z*CO-x*s;
rotationBox(2,2) = z*z*CO+c;
// Translation vector
rotationBox(0,3) = 0;
rotationBox(1,3) = 0;
rotationBox(2,3) = 0;
// The rest of the 4x4 matrix
rotationBox(3,0) = 0;
rotationBox(3,1) = 0;
rotationBox(3,2) = 0;
rotationBox(3,3) = 1;
if(store_transformation)
rotations_.push_back(rotationBox);
return rotationBox;
}
Eigen::AngleAxisf createRandomAA()
{
Eigen::Vector3f nn;
float a = M_PI*(rand()%1000)/2000.f; //angle
nn(0) = (rand()%1000)/1000.f;
nn(1) = (rand()%1000)/1000.f;
nn(2) = (rand()%1000)/1000.f;
nn.normalize();
//std::cout<<"rot angle\n"<<a<<"\n";
//std::cout<<"rot axis\n"<<nn<<"\n";
return Eigen::AngleAxisf(a,nn);
}
Eigen::Vector3f
VirtualTrackball::project_on_sphere ( const int x, const int y ) const
{
const int cx = this->_width / 2;
const int cy = this->_height / 2;
const int base = std::min ( cx,cy );
const float fx = ( x - cx ) * 1.0 / base / _radius;
const float fy = ( y - cy ) * 1.0 / base / _radius;
Eigen::Vector3f result ( fx, fy, 0 );
const float d = fx * fx + fy * fy;
if ( d < 1.0f ) result.z() = std::sqrt ( 1.0f - d );
result.normalize();
return result;
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::findIntersection (int level,
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
const Eigen::Vector4f &start_pt,
std::vector <int> &pt_union_indices,
Eigen::Vector4f &intersection)
{
assert (end_pts.size () == 2);
assert (vect_at_end_pts.size () == 2);
Eigen::Vector3f vec;
getVectorAtPoint (start_pt, pt_union_indices, vec);
double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
if ((fabs (d1) < 10e-3) || (level == max_binary_search_level_))
{
intersection = start_pt;
return;
}
else
{
vec.normalize ();
if (vec.dot (vect_at_end_pts[0]) < 0)
{
Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
new_end_pts[0] = end_pts[0];
new_end_pts[1] = start_pt;
new_vect_at_end_pts[0] = vect_at_end_pts[0];
new_vect_at_end_pts[1] = vec;
findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
return;
}
if (vec.dot (vect_at_end_pts[1]) < 0)
{
Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
new_end_pts[0] = start_pt;
new_end_pts[1] = end_pts[1];
new_vect_at_end_pts[0] = vec;
new_vect_at_end_pts[1] = vect_at_end_pts[1];
findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
return;
}
intersection = start_pt;
return;
}
}
void ProceduralQuad::GetNormals(void * data_start, unsigned int byte_stride)
{
Eigen::Vector3f normal = (corners[1]-corners[0]).cross(corners[2]-corners[0]);
normal.normalize();
float * dest = (float *) data_start;
unsigned int vertex_count = GetVertexCount();
for (unsigned int i = 0; i < vertex_count; ++i)
{
dest[0] = normal[0];
dest[1] = normal[1];
dest[2] = normal[2];
dest = (float *) ((char *) dest + byte_stride);
}
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::directedOrthogonalAxis (
Eigen::Vector3f const &axis,
Eigen::Vector3f const &axis_origin,
Eigen::Vector3f const &point,
Eigen::Vector3f &directed_ortho_axis)
{
Eigen::Vector3f projection;
projectPointOnPlane (point, axis_origin, axis, projection);
directed_ortho_axis = projection - axis_origin;
directed_ortho_axis.normalize ();
// check if the computed x axis is orthogonal to the normal
//assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f));
}
void NICPQGLViewer::updateCameraPosition(Eigen::Isometry3f pose) {
qglviewer::Camera *oldcam = camera();
qglviewer::Camera *cam = new StandardCamera();
setCamera(cam);
Eigen::Vector3f position = pose*Vector3f(-2.0f, 0.0f, 1.0f);
cam->setPosition(qglviewer::Vec(position[0], position[1], position[2]));
Eigen::Vector3f upVector = pose.linear()*Vector3f(0.0f, 0.0f, 0.5f);
upVector.normalize();
cam->setUpVector(qglviewer::Vec(upVector[0], upVector[1], upVector[2]), true);
Eigen::Vector3f lookAt = pose*Vector3f(4.0f, 0.0f, 0.0f);
cam->lookAt(qglviewer::Vec(lookAt[0], lookAt[1], lookAt[2]));
delete oldcam;
}
Eigen::Matrix4f rotate(float angle,Eigen::Vector3f v)
{
float c = cosf(angle);
float s = sinf(angle);
v.normalize();
float x = v.x();
float y = v.y();
float z = v.z();
const float vals[] =
{
x*x*(1-c)+c ,x*y*(1-c)-z*s,x*z*(1-c)+y*s,0,
y*x*(1-c)+z*s,y*y*(1-c)+c ,y*z*(1-c)-x*s,0,
x*z*(1-c)-y*s,y*z*(1-c)+x*s,z*z*(1-c)+c ,0,
0 ,0 ,0 ,1
};
return Eigen::Matrix4f(vals).transpose();
}
void HierarchicalWalkingIK::computeWalkingTrajectory(const Eigen::Matrix3Xf& comTrajectory,
const Eigen::Matrix6Xf& rightFootTrajectory,
const Eigen::Matrix6Xf& leftFootTrajectory,
std::vector<Eigen::Matrix3f>& rootOrientation,
Eigen::MatrixXf& trajectory)
{
int rows = outputNodeSet->getSize();
trajectory.resize(rows, rightFootTrajectory.cols());
rootOrientation.resize(rightFootTrajectory.cols());
Eigen::Vector3f com = colModelNodeSet->getCoM();
Eigen::VectorXf configuration;
int N = trajectory.cols();
Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f rightFootPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f chestPose = chest->getGlobalPose();
Eigen::Matrix4f pelvisPose = pelvis->getGlobalPose();
for (int i = 0; i < N; i++)
{
VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1), leftFootTrajectory.block(3, i, 3, 1), leftFootPose);
VirtualRobot::MathTools::posrpy2eigen4f(1000 * rightFootTrajectory.block(0, i, 3, 1), rightFootTrajectory.block(3, i, 3, 1), rightFootPose);
// FIXME the orientation of the chest and chest is specific to armar 4
// since the x-Axsis points in walking direction
Eigen::Vector3f xAxisChest = (leftFootPose.block(0, 1, 3, 1) + rightFootPose.block(0, 1, 3, 1))/2;
xAxisChest.normalize();
chestPose.block(0, 0, 3, 3) = Bipedal::poseFromXAxis(xAxisChest);
pelvisPose.block(0, 0, 3, 3) = Bipedal::poseFromYAxis(-xAxisChest);
std::cout << "Frame #" << i << ", ";
robot->setGlobalPose(leftFootPose);
computeStepConfiguration(1000 * comTrajectory.col(i),
rightFootPose,
chestPose,
pelvisPose,
configuration);
trajectory.col(i) = configuration;
rootOrientation[i] = leftFootPose.block(0, 0, 3, 3);
}
}