StampedTransform.java

Go to the documentation of this file.

/* 
 * Copyright (c) 2011, Sjoerd van den Dries
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Technische Universiteit Eindhoven nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */


package tfjava;

import ros.communication.Time;

import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
import javax.vecmath.Point3d;
import javax.vecmath.Matrix4d;

public class StampedTransform {       
        
    public String frameID;
    public String childFrameID;
    public Time timeStamp;
    protected Matrix4d transform;
        
    public StampedTransform(Vector3d translation, Quat4d rotation, Time timeStamp, String frameID, String childFrameID) {
        this.childFrameID = childFrameID; 
        this.frameID = frameID;
        this.timeStamp = timeStamp;        
        this.transform = new Matrix4d(rotation, translation, 1);
    }
    
    public StampedTransform(Matrix4d transform, Time timeStamp, String frameID, String childFrameID) {
        this.childFrameID = childFrameID; 
        this.frameID = frameID;
        this.timeStamp = timeStamp;
        this.transform = transform;
    }
    
    public static StampedTransform getIdentity() {
        Matrix4d identity = new Matrix4d();
        identity.setIdentity();        
        return new StampedTransform(identity, null, null, null);
    }
    
    public StampedTransform invert() {
        transform.invert();
        String mem = this.childFrameID;
        this.childFrameID = frameID;
        this.frameID = mem;
        return this;
    }
    
    public void mul(StampedTransform t1) {        
        this.transform.mul(t1.transform);
        this.childFrameID = t1.childFrameID;
    }

    public void mul(StampedTransform t1, StampedTransform t2) {
        this.transform.mul(t1.transform, t2.transform);
        this.frameID = t1.frameID;
        this.childFrameID = t2.childFrameID;
    }
    
    public void transformPose(Matrix4d pose, Matrix4d poseOut) {
        poseOut.mul(getMatrix4(), pose);
    }
    
    public void transformPoint(Point3d point, Point3d pointOut) {
        getMatrix4().transform(point, pointOut);
    }
    
    public void transformVector(Vector3d vector, Vector3d vectorOut) {
        getMatrix4().transform(vector, vectorOut);
    }
    
    public void transformPose(Stamped<Matrix4d> stampedIn, Stamped<Matrix4d> stampedOut) {
        if (stampedIn.frameID != frameID) {
            // TODO: throw error
            return;
        }
        transformPose(stampedIn.getData(), stampedOut.getData());
        stampedOut.frameID = childFrameID;
        stampedOut.timeStamp = new Time(timeStamp);
    }
    
    public void transformPoint(Stamped<Point3d> stampedIn, Stamped<Point3d> stampedOut) {
        if (stampedIn.frameID != frameID) {
            // TODO: throw error
            return;
        }
        transformPoint(stampedIn.getData(), stampedOut.getData());
        stampedOut.frameID = childFrameID;
        stampedOut.timeStamp = new Time(timeStamp);
    }  
    
    public void transformVector(Stamped<Vector3d> stampedIn, Stamped<Vector3d> stampedOut) {
        if (stampedIn.frameID != frameID) {
            // TODO: throw error
            return;
        }
        transformVector(stampedIn.getData(), stampedOut.getData());
        stampedOut.frameID = childFrameID;
        stampedOut.timeStamp = new Time(timeStamp);
    }          
 
    public Vector3d getTranslation() {
        Vector3d out = new Vector3d();
        transform.get(out);
        return out;
    }
    
    public void getTranslation(Vector3d out) {
        transform.get(out);
    }    
    public Quat4d getRotation() {
        Quat4d out = new Quat4d();
        transform.get(out);
        return out;
    }
    
    public void getRotation(Quat4d out) {
        transform.get(out);
    }
    
    public Matrix4d getMatrix4() {
        return transform;
    }      
    
    public String toString() {
        return "[" + frameID + " -> " + childFrameID + ", " + timeStamp + ", " + getMatrix4() + "]";
    } 
    
}