TransformStorage.java
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2011, Sjoerd van den Dries
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Technische Universiteit Eindhoven nor the
00014  *       names of its contributors may be used to endorse or promote products
00015  *       derived from this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 package tfjava;
00032 
00033 import javax.vecmath.Quat4d;
00034 import javax.vecmath.Vector3d;
00035 import javax.vecmath.Matrix4d;
00036 
00048 public class TransformStorage {
00049        
00051     protected Frame parentFrame;
00053     protected Frame childFrame;
00055     protected long timeStamp;
00057     protected Vector3d translation;
00059     protected Quat4d rotation; 
00060 
00064     public TransformStorage(Vector3d translation, Quat4d rotation, long timeStamp, Frame parentFrame, Frame childFrame) {
00065         this.childFrame = childFrame; 
00066         this.parentFrame = parentFrame;
00067         this.timeStamp = timeStamp;
00068         this.translation = translation;
00069         this.rotation = rotation;
00070     }   
00071    
00076     public static TransformStorage interpolate(TransformStorage t1, TransformStorage t2, long time) {
00077         long time1 = t1.getTimeStamp();
00078         long time2 = t2.getTimeStamp();
00079           
00080         // Check for zero distance case 
00081         if (time1 == time2) {
00082           return t1;
00083         }
00084                 
00085         //Calculate the ratio
00086         double ratio = (double)(time - time1) / (time2 - time1);        
00087                 
00088         // interpolate translation
00089         Vector3d transRet = new Vector3d();
00090         transRet.interpolate(t1.getTranslation(), t2.getTranslation(), ratio); 
00091         
00092         // interpolate rotation
00093         Quat4d rotRet = new Quat4d();
00094         rotRet.interpolate(t1.getRotation(), t2.getRotation(), ratio);
00095         
00096         // original tf implementation (see cache.cpp) does not 'interpolate' timestamp. I do.
00097                 
00098         return new TransformStorage(transRet, rotRet, time, t1.parentFrame, t1.childFrame);
00099     }       
00100     
00104     public Frame getChildFrame() {
00105         return childFrame;
00106     }
00107     
00111     public Frame getParentFrame() {
00112         return parentFrame;
00113     }
00114     
00118     public long getTimeStamp() {
00119         return timeStamp;
00120     }
00121     
00125     public Vector3d getTranslation() {
00126         return translation;
00127     }
00128     
00132     public Quat4d getRotation() {        
00133         return rotation;
00134     }   
00135     
00139     public String toString() {
00140         return "[" + parentFrame.getFrameID() + " -> " + childFrame.getFrameID() + ", "
00141                    + ((double)timeStamp / 1E9) + ", " + getTranslation() + ", " + getRotation() + "]";
00142     } 
00143     
00144 }


tfjava
Author(s): Sjoerd van den Dries
autogenerated on Thu Jan 2 2014 11:07:04