git.gag.com Git - debian/openrocket/blob - src/net/sf/openrocket/util/Transformation.java

   1 package net.sf.openrocket.util;
   2
   3 import java.util.*;
   4
   5 /**
   6  * Defines an affine transformation of the form  A*x+c,  where x and c are Coordinates and
   7  * A is a 3x3 matrix.
   8  *
   9  * The Transformations are immutable.  All modification methods return a new transformation.
  10  *
  11  * @author Sampo Niskanen <sampo.niskanen@iki.fi>
  12  */
  13
  14 public class Transformation implements java.io.Serializable {
  15
  16
  17         public static final Transformation IDENTITY =
  18                 new Transformation();
  19
  20         public static final Transformation PROJECT_XY =
  21                 new Transformation(new double[][]{{1,0,0},{0,1,0},{0,0,0}});
  22         public static final Transformation PROJECT_YZ =
  23                 new Transformation(new double[][]{{0,0,0},{0,1,0},{0,0,1}});
  24         public static final Transformation PROJECT_XZ =
  25                 new Transformation(new double[][]{{1,0,0},{0,0,0},{0,0,1}});
  26
  27         private static final int X = 0;
  28         private static final int Y = 1;
  29         private static final int Z = 2;
  30
  31         private final Coordinate translate;
  32         private final double[][] rotation = new double[3][3];
  33
  34         /**
  35          * Create identity transformation.
  36          */
  37         public Transformation() {
  38                 translate = new Coordinate(0,0,0);
  39                 rotation[X][X]=1;
  40                 rotation[Y][Y]=1;
  41                 rotation[Z][Z]=1;
  42         }
  43
  44         /**
  45          * Create transformation with only translation.
  46          * @param x Translation in x-axis.
  47          * @param y Translation in y-axis.
  48          * @param z Translation in z-axis.
  49          */
  50         public Transformation(double x,double y,double z) {
  51                 translate = new Coordinate(x,y,z);
  52                 rotation[X][X]=1;
  53                 rotation[Y][Y]=1;
  54                 rotation[Z][Z]=1;
  55         }
  56
  57         /**
  58          * Create transformation with only translation.
  59          * @param translation  The translation term.
  60          */
  61         public Transformation(Coordinate translation) {
  62                 this.translate = translation;
  63                 rotation[X][X]=1;
  64                 rotation[Y][Y]=1;
  65                 rotation[Z][Z]=1;
  66         }
  67
  68         /**
  69          * Create transformation with given rotation matrix and translation.
  70          * @param rotation
  71          * @param translation
  72          */
  73         public Transformation(double[][] rotation, Coordinate translation) {
  74                 for (int i=0; i<3; i++)
  75                         for (int j=0; j<3; j++)
  76                                 this.rotation[i][j] = rotation[i][j];
  77                 this.translate = translation;
  78         }
  79
  80
  81         /**
  82          * Create transformation with given rotation matrix and translation.
  83          * @param rotation
  84          * @param translation
  85          */
  86         public Transformation(double[][] rotation) {
  87                 for (int i=0; i<3; i++)
  88                         for (int j=0; j<3; j++)
  89                                 this.rotation[i][j] = rotation[i][j];
  90                 this.translate = Coordinate.NUL;
  91         }
  92
  93
  94
  95
  96
  97         /**
  98          * Transform a coordinate according to this transformation.
  99          *
 100          * @param orig  the coordinate to transform.
 101          * @return              the result.
 102          */
 103         public Coordinate transform(Coordinate orig) {
 104                 final double x,y,z;
 105
 106                 x = rotation[X][X]*orig.x + rotation[X][Y]*orig.y + rotation[X][Z]*orig.z + translate.x;
 107                 y = rotation[Y][X]*orig.x + rotation[Y][Y]*orig.y + rotation[Y][Z]*orig.z + translate.y;
 108                 z = rotation[Z][X]*orig.x + rotation[Z][Y]*orig.y + rotation[Z][Z]*orig.z + translate.z;
 109
 110                 return new Coordinate(x,y,z,orig.weight);
 111         }
 112
 113
 114         /**
 115          * Transform an array of coordinates.  The transformed coordinates are stored
 116          * in the same array, and the array is returned.
 117          *
 118          * @param orig  the coordinates to transform.
 119          * @return              <code>orig</code>, with the coordinates transformed.
 120          */
 121         public Coordinate[] transform(Coordinate[] orig) {
 122                 for (int i=0; i < orig.length; i++) {
 123                         orig[i] = transform(orig[i]);
 124                 }
 125                 return orig;
 126         }
 127
 128         /**
 129          * Transforms all coordinates in a Collection.  The original coordinate elements are
 130          * removed from the set and replaced with the transformed ones.  The Collection given
 131          * must implement the .clear() and .addAll() methods.
 132          *
 133          * @param set  Collection of coordinates to transform.
 134          */
 135         public void transform(Collection<Coordinate> set) {
 136                 ArrayList<Coordinate> temp = new ArrayList<Coordinate>(set.size());
 137                 Iterator<Coordinate> iter = set.iterator();
 138                 while (iter.hasNext())
 139                         temp.add(this.transform(iter.next()));
 140                 set.clear();
 141                 set.addAll(temp);
 142         }
 143
 144         /**
 145          * Applies only the linear transformation  A*x
 146          * @param orig  Coordinate to transform.
 147          */
 148         public Coordinate linearTransform(Coordinate orig) {
 149                 final double x,y,z;
 150
 151                 x = rotation[X][X]*orig.x + rotation[X][Y]*orig.y + rotation[X][Z]*orig.z;
 152                 y = rotation[Y][X]*orig.x + rotation[Y][Y]*orig.y + rotation[Y][Z]*orig.z;
 153                 z = rotation[Z][X]*orig.x + rotation[Z][Y]*orig.y + rotation[Z][Z]*orig.z;
 154
 155                 return new Coordinate(x,y,z,orig.weight);
 156         }
 157
 158         /**
 159          * Applies the given transformation before this tranformation.  The resulting
 160          * transformation result.transform(c) will equal this.transform(other.transform(c)).
 161          *
 162          * @param other  Transformation to apply
 163          * @return   The new transformation
 164          */
 165         public Transformation applyTransformation(Transformation other) {
 166                 // other = Ax+b
 167                 // this = Cx+d
 168                 // C(Ax+b)+d = CAx + Cb+d
 169
 170                 // Translational portion
 171                 Transformation combined = new Transformation(
 172                                 this.linearTransform(other.translate).add(this.translate)
 173                 );
 174
 175                 // Linear portion
 176                 for (int i=0; i<3; i++) {
 177                         final double x,y,z;
 178                         x = rotation[i][X];
 179                         y = rotation[i][Y];
 180                         z = rotation[i][Z];
 181                         combined.rotation[i][X] =
 182                                 x*other.rotation[X][X] + y*other.rotation[Y][X] + z*other.rotation[Z][X];
 183                         combined.rotation[i][Y] =
 184                                 x*other.rotation[X][Y] + y*other.rotation[Y][Y] + z*other.rotation[Z][Y];
 185                         combined.rotation[i][Z] =
 186                                 x*other.rotation[X][Z] + y*other.rotation[Y][Z] + z*other.rotation[Z][Z];
 187                 }
 188                 return combined;
 189         }
 190
 191
 192         /**
 193          * Rotate around x-axis a given angle.
 194          * @param theta  The angle to rotate in radians.
 195          * @return  The transformation.
 196          */
 197         public static Transformation rotate_x(double theta) {
 198                 return new Transformation(new double[][]{
 199                                 {1,0,0},
 200                                 {0,Math.cos(theta),-Math.sin(theta)},
 201                                 {0,Math.sin(theta),Math.cos(theta)}});
 202         }
 203
 204         /**
 205          * Rotate around y-axis a given angle.
 206          * @param theta  The angle to rotate in radians.
 207          * @return  The transformation.
 208          */
 209         public static Transformation rotate_y(double theta) {
 210                 return new Transformation(new double[][]{
 211                                 {Math.cos(theta),0,Math.sin(theta)},
 212                                 {0,1,0},
 213                                 {-Math.sin(theta),0,Math.cos(theta)}});
 214         }
 215
 216         /**
 217          * Rotate around z-axis a given angle.
 218          * @param theta  The angle to rotate in radians.
 219          * @return  The transformation.
 220          */
 221         public static Transformation rotate_z(double theta) {
 222                 return new Transformation(new double[][]{
 223                                 {Math.cos(theta),-Math.sin(theta),0},
 224                                 {Math.sin(theta),Math.cos(theta),0},
 225                                 {0,0,1}});
 226         }
 227
 228
 229
 230         public void print(String... str) {
 231                 for (String s: str) {
 232                         System.out.println(s);
 233                 }
 234                 System.out.printf("[%3.2f %3.2f %3.2f]   [%3.2f]\n",
 235                                 rotation[X][X],rotation[X][Y],rotation[X][Z],translate.x);
 236                 System.out.printf("[%3.2f %3.2f %3.2f] + [%3.2f]\n",
 237                                 rotation[Y][X],rotation[Y][Y],rotation[Y][Z],translate.y);
 238                 System.out.printf("[%3.2f %3.2f %3.2f]   [%3.2f]\n",
 239                                 rotation[Z][X],rotation[Z][Y],rotation[Z][Z],translate.z);
 240                 System.out.println();
 241         }
 242
 243
 244         public static void main(String[] arg) {
 245                 Transformation t;
 246
 247                 t = new Transformation();
 248                 t.print("Empty");
 249                 t = new Transformation(1,2,3);
 250                 t.print("1,2,3");
 251                 t = new Transformation(new Coordinate(2,3,4));
 252                 t.print("coord 2,3 4");
 253
 254                 t = Transformation.rotate_y(0.01);
 255                 t.print("rotate_y 0.01");
 256
 257                 t = new Transformation(-1,0,0);
 258                 t = t.applyTransformation(Transformation.rotate_y(0.01));
 259                 t = t.applyTransformation(new Transformation(1,0,0));
 260                 t.print("shift-rotate-shift");
 261         }
 262
 263 }