Robot pose (old)

package robot;

import java.io.*;

public class Pose {

    public float x, y, heading;

    public Pose () {
        reset();
    }

    public Pose (float x, float y, float heading) {
        this.x = x;
        this.y = y;
        this.heading = heading;
    }

    public Pose (Pose p) {
        copyFrom (p);
    }

    public void add (Pose delta) {

        float cos_phi = (float) Math.cos (heading);
        float sin_phi = (float) Math.sin (heading);

        x += cos_phi*delta.x - sin_phi*delta.y;
        y += sin_phi*delta.x + cos_phi*delta.y;
        heading += delta.heading;
    }

    public void subtract (Pose p) {

        float cos_phi = (float) Math.cos (p.heading);
        float sin_phi = (float) Math.sin (p.heading);

        float delta_x = x - p.x;
        float delta_y = y - p.y;

        x = cos_phi*delta_x + sin_phi*delta_y;
        y = -sin_phi*delta_x + cos_phi*delta_y;
        heading -= p.heading;
    }

    public void addWheelMotion (float leftDist, float rightDist) {

        float distance = (rightDist + leftDist) / 2;
        float delta_heading = (rightDist - leftDist) / (2 * Model.WHEEL_BASE);

        float cos_dphi = (float) Math.cos (delta_heading);
        float sin_dphi = (float) Math.sin (delta_heading);

        float delta_x = distance + Model.REF_POINT_POS*(cos_dphi - 1);
        float delta_y = Model.REF_POINT_POS * sin_dphi;

        float cos_phi = (float) Math.cos (heading);
        float sin_phi = (float) Math.sin (heading);

        x += cos_phi*delta_x - sin_phi*delta_y;
        y += sin_phi*delta_x + cos_phi*delta_y;
        heading += delta_heading;
    }

    public void reset () {
        x = 0;
        y = 0;
        heading = 0;
    }

    public void copyFrom (Pose p) {
        x = p.x;
        y = p.y;
        heading = p.heading;
    }

    public boolean equalTo (Pose p) {
        return x == p.x && y == p.y && heading == p.heading;
    }

    public void read (DataInputStream in) throws IOException {
        x = in.readFloat();
        y = in.readFloat();
        heading = in.readFloat();
    }

    public void write (DataOutputStream out) throws IOException {
        out.writeFloat (x);
        out.writeFloat (y);
        out.writeFloat (heading);
    }

}