001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.utils;
019
020import edu.wpi.first.math.geometry.*;
021import org.photonvision.common.dataflow.structures.Packet;
022
023@SuppressWarnings("doclint")
024public class PacketUtils {
025    public static final int ROTATION2D_BYTE_SIZE = Double.BYTES;
026    public static final int QUATERNION_BYTE_SIZE = Double.BYTES * 4;
027    public static final int ROTATION3D_BYTE_SIZE = QUATERNION_BYTE_SIZE;
028    public static final int TRANSLATION2D_BYTE_SIZE = Double.BYTES * 2;
029    public static final int TRANSLATION3D_BYTE_SIZE = Double.BYTES * 3;
030    public static final int TRANSFORM2D_BYTE_SIZE = TRANSLATION2D_BYTE_SIZE + ROTATION2D_BYTE_SIZE;
031    public static final int TRANSFORM3D_BYTE_SIZE = TRANSLATION3D_BYTE_SIZE + ROTATION3D_BYTE_SIZE;
032    public static final int POSE2D_BYTE_SIZE = TRANSLATION2D_BYTE_SIZE + ROTATION2D_BYTE_SIZE;
033    public static final int POSE3D_BYTE_SIZE = TRANSLATION3D_BYTE_SIZE + ROTATION3D_BYTE_SIZE;
034
035    public static void packRotation2d(Packet packet, Rotation2d rotation) {
036        packet.encode(rotation.getRadians());
037    }
038
039    public static Rotation2d unpackRotation2d(Packet packet) {
040        return new Rotation2d(packet.decodeDouble());
041    }
042
043    public static void packQuaternion(Packet packet, Quaternion quaternion) {
044        packet.encode(quaternion.getW());
045        packet.encode(quaternion.getX());
046        packet.encode(quaternion.getY());
047        packet.encode(quaternion.getZ());
048    }
049
050    public static Quaternion unpackQuaternion(Packet packet) {
051        return new Quaternion(
052                packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble());
053    }
054
055    public static void packRotation3d(Packet packet, Rotation3d rotation) {
056        packQuaternion(packet, rotation.getQuaternion());
057    }
058
059    public static Rotation3d unpackRotation3d(Packet packet) {
060        return new Rotation3d(unpackQuaternion(packet));
061    }
062
063    public static void packTranslation2d(Packet packet, Translation2d translation) {
064        packet.encode(translation.getX());
065        packet.encode(translation.getY());
066    }
067
068    public static Translation2d unpackTranslation2d(Packet packet) {
069        return new Translation2d(packet.decodeDouble(), packet.decodeDouble());
070    }
071
072    public static void packTranslation3d(Packet packet, Translation3d translation) {
073        packet.encode(translation.getX());
074        packet.encode(translation.getY());
075        packet.encode(translation.getZ());
076    }
077
078    public static Translation3d unpackTranslation3d(Packet packet) {
079        return new Translation3d(packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble());
080    }
081
082    public static void packTransform2d(Packet packet, Transform2d transform) {
083        packTranslation2d(packet, transform.getTranslation());
084        packRotation2d(packet, transform.getRotation());
085    }
086
087    public static Transform2d unpackTransform2d(Packet packet) {
088        return new Transform2d(unpackTranslation2d(packet), unpackRotation2d(packet));
089    }
090
091    public static void packTransform3d(Packet packet, Transform3d transform) {
092        packTranslation3d(packet, transform.getTranslation());
093        packRotation3d(packet, transform.getRotation());
094    }
095
096    public static Transform3d unpackTransform3d(Packet packet) {
097        return new Transform3d(unpackTranslation3d(packet), unpackRotation3d(packet));
098    }
099
100    public static void packPose2d(Packet packet, Pose2d pose) {
101        packTranslation2d(packet, pose.getTranslation());
102        packRotation2d(packet, pose.getRotation());
103    }
104
105    public static Pose2d unpackPose2d(Packet packet) {
106        return new Pose2d(unpackTranslation2d(packet), unpackRotation2d(packet));
107    }
108
109    public static void packPose3d(Packet packet, Pose3d pose) {
110        packTranslation3d(packet, pose.getTranslation());
111        packRotation3d(packet, pose.getRotation());
112    }
113
114    public static Pose3d unpackPose3d(Packet packet) {
115        return new Pose3d(unpackTranslation3d(packet), unpackRotation3d(packet));
116    }
117}