TYPED_TEST(Intesection_TEST, intersection_line_plane)
{
using Scalar = typename cgogn::geometry::vector_traits<TypeParam>::Scalar;
TypeParam a(Scalar(-1), Scalar(-1), Scalar(-3));
TypeParam v(Scalar(3), Scalar(0), Scalar(0));
TypeParam p_p(Scalar(2), Scalar(2), Scalar(1));
TypeParam p_d(Scalar(5), Scalar(1), Scalar(3));
EXPECT_TRUE(cgogn::geometry::intersection_line_plane(a,v,p_p,p_d));
p_d = TypeParam(Scalar(0), Scalar(0), Scalar(2));
EXPECT_FALSE(cgogn::geometry::intersection_line_plane(a,v,p_p,p_d));
}
double LipmConstHeightPlanner::forwardPass(const double *x0, Traj<1,1> &com) const
{
Eigen::Matrix<double,2,1> z;
Eigen::Matrix<double,1,1> u;
com = Traj<1,1>();
for (size_t i=0; i<_zmp_d.size(); i++) {
com.append(_zmp_d[i].time, _zmp_d[i].type, NULL, NULL, NULL, NULL);
}
com[0].x[0] = x0[0];
com[0].x[1] = x0[1];
#if 0
std::cout << _com.size() << " " << _zmp_d.size() << std::endl;
#endif
for (size_t i=0; i<_zmp_d.size()-1; i++) {
z(0) = com[i].x[0] - _zmp_d[i].x[0];
z(1) = com[i].x[1];
u = _K*z + _du[i];
com[i].u[0] = u(0);
z = _A*z + _B*u;
com[i+1].x[0] = z(0) + _zmp_d[i].x[0];
com[i+1].x[1] = z(1);
}
#if 0
std::fstream p_d("tmp/pd", std::fstream::out);
std::fstream p_a("tmp/pa", std::fstream::out);
std::fstream com_out("tmp/com", std::fstream::out);
for (size_t i=0; i<_zmp_d.size(); i++) {
p_d << _zmp_d[i].x[0] << std::endl;
p_a << com[i].u[0] + _zmp_d[i].x[0] << std::endl;
com_out << com[i].x[0] << std::endl;
}
p_d.close();
p_a.close();
com_out.close();
#endif
return 0;
}
void CameraParameters::image2field(
GVector::vector3d<double> &p_f, const GVector::vector2d<double> &p_i,
double z) const {
// Undo scaling and offset
GVector::vector2d<double> p_d(
(p_i.x - principal_point_x->getDouble()) / focal_length->getDouble(),
(p_i.y - principal_point_y->getDouble()) / focal_length->getDouble());
// Compensate for distortion (undistort)
GVector::vector2d<double> p_un;
radialDistortionInv(p_un,p_d);
// Now we got a ray on the z axis
GVector::vector3d<double> v(p_un.x, p_un.y, 1);
// Transform this ray into world coordinates
Quaternion<double> q_field2cam = Quaternion<double>(
q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
q_field2cam.norm();
GVector::vector3d<double> translation =
GVector::vector3d<double>(tx->getDouble(),ty->getDouble(),tz->getDouble());
Quaternion<double> q_field2cam_inv = q_field2cam;
q_field2cam_inv.invert();
GVector::vector3d<double> v_in_w =
q_field2cam_inv.rotateVectorByQuaternion(v);
GVector::vector3d<double> zero_in_w =
q_field2cam_inv.rotateVectorByQuaternion(
GVector::vector3d<double>(0,0,0) - translation);
// Compute the the point where the rays intersects the field
double t = GVector::ray_plane_intersect(
GVector::vector3d<double>(0,0,z), GVector::vector3d<double>(0,0,1).norm(),
zero_in_w, v_in_w.norm());
// Set p_f
p_f = zero_in_w + v_in_w.norm() * t;
}
