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

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