Ejemplo n.º 1
0
        private static Translation2d intersectionInternal(Pose2d a, Pose2d b)
        {
            Rotation2d    a_r = a.getRotation();
            Rotation2d    b_r = b.getRotation();
            Translation2d a_t = a.getTranslation();
            Translation2d b_t = b.getTranslation();

            double tan_b = b_r.tan();
            double t     = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y()) / (a_r.sin() - a_r.cos() * tan_b);

            if (Double.IsNaN(t))
            {
                return(new Translation2d(Double.PositiveInfinity, Double.PositiveInfinity));
            }
            return(a_t.translateBy(a_r.toTranslation().scale(t)));
        }
Ejemplo n.º 2
0
        /**
         * Logical inverse of the above.
         */
        public static Twist2d log(Pose2d transform)
        {
            double dtheta        = transform.getRotation().getRadians();
            double half_dtheta   = 0.5 * dtheta;
            double cos_minus_one = transform.getRotation().cos() - 1.0;
            double halftheta_by_tan_of_halfdtheta;

            if (Math.Abs(cos_minus_one) < kEps)
            {
                halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
            }
            else
            {
                halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
            }
            Translation2d translation_part = transform.getTranslation()
                                             .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));

            return(new Twist2d(translation_part.x(), translation_part.y(), dtheta));
        }
Ejemplo n.º 3
0
 public Rotation2d(Translation2d direction, bool normalize) : this(direction.x(), direction.y(), normalize)
 {
 }