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; }