001/* 002* AxisAngle -- An rotation axis and angle combination that represents the orientation between 003* two reference frames. 004* 005* Copyright (C) 2009-2014 by Joseph A. Huwaldt. 006* All rights reserved. 007* 008* This library is free software; you can redistribute it and/or 009* modify it under the terms of the GNU Lesser General Public 010* License as published by the Free Software Foundation; either 011* version 2 of the License, or (at your option) any later version. 012* 013* This library is distributed in the hope that it will be useful, 014* but WITHOUT ANY WARRANTY; without even the implied warranty of 015* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 016* Lesser General Public License for more details. 017* 018* You should have received a copy of the GNU Lesser General Public License 019* along with this program; if not, write to the Free Software 020* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 021* Or visit: http://www.gnu.org/licenses/lgpl.html 022**/ 023package jahuwaldt.js.param; 024 025import javax.measure.quantity.*; 026import javax.measure.unit.SI; 027 028import javolution.context.ObjectFactory; 029import javolution.context.StackContext; 030import static javolution.lang.MathLib.*; 031import javolution.xml.XMLFormat; 032import javolution.xml.stream.XMLStreamException; 033import javolution.xml.XMLSerializable; 034import javolution.text.Text; 035import javolution.text.TextBuilder; 036 037 038/** 039* <p> This class represents a rotation axis and rotation angle made up of Float64 040* elements. AxisAngles may be used to represents a relative orientation 041* (attitude or rotation transformation) between two different reference 042* frames; B wrt A or BA. It can be used to transform coordinates in reference frame A 043* to reference frame B (A2B).</p> 044* 045* <p> Reference: <a href="http://en.wikipedia.org/wiki/Axis_angle"> 046* Wikipedia: Axis angle</a></p> 047* 048* <p> Modified by: Joseph A. Huwaldt </p> 049* 050* @author Joseph A. Huwaldt Date: October 22, 2009 051* @version February 27, 2014 052**/ 053public final class AxisAngle extends AbstractRotation<AxisAngle> implements XMLSerializable { 054 055 private static final long serialVersionUID = -8836950942718466904L; 056 057 /** 058 * The index to the vector X component of this axis angle rotation representation's axis. 059 **/ 060 public static final int X = 0; 061 062 /** 063 * The index to the vector Y component of this axis angle rotation representation's axis. 064 **/ 065 public static final int Y = 1; 066 067 /** 068 * The index to the vector Z component of this axis angle rotation representation's axis. 069 **/ 070 public static final int Z = 2; 071 072 /** 073 * The index to the angular component of this axis angle rotation representation. 074 **/ 075 public static final int THETA = 3; 076 077 078 /** 079 * The elements of the rotation axis. 080 **/ 081 private Vector3D<Dimensionless> _axis; 082 083 /** 084 * The rotation angle about the rotation axis. 085 **/ 086 private Parameter<Angle> _theta; 087 088 089 /** 090 * Returns a {@link AxisAngle} instance containing the specified rotation axis and 091 * rotation angle. 092 * 093 * @param axis The vector representing the rotation axis direction. 094 * @param angle The rotation angle about the rotation axis. 095 * @return The axis/angle rotation transformation having the specified values. 096 */ 097 public static AxisAngle valueOf(Vector3D<?> axis, Parameter<Angle> angle) { 098 099 AxisAngle aa = AxisAngle.newInstance(); 100 aa._axis = axis.toUnitVector(); 101 aa._theta = angle; 102 103 return aa; 104 } 105 106 /** 107 * Returns a new {@link AxisAngle} instance constructed from the specified rotation transform. 108 * 109 * @param transform The Rotation transform to convert to a new quaternion. 110 * @return the quaternion representing the specified attitude transform. 111 */ 112 public static AxisAngle valueOf(Rotation<?> transform) { 113 AxisAngle aa; 114 if (transform instanceof AxisAngle) 115 aa = copyOf((AxisAngle)transform); 116 else { 117 Quaternion q = transform.toQuaternion(); 118 Vector3D<Dimensionless> axis = q.getVector().toUnitVector(); 119 double thetaV = 2*acos(q.getScalarValue()); 120 Parameter<Angle> theta = Parameter.valueOf(thetaV, SI.RADIAN); 121 aa = AxisAngle.valueOf(axis, theta); 122 } 123 return aa; 124 } 125 126 127 /** 128 * Returns the rotation axis part of this axis/angle rotation transformation 129 * as a unit vector. 130 **/ 131 public Vector3D<Dimensionless> getAxis() { 132 return _axis; 133 } 134 135 /** 136 * Return the rotation angle. 137 **/ 138 public Parameter<Angle> getAngle() { 139 return _theta; 140 } 141 142 /** 143 * Returns the spatial inverse of this transformation: AB rather than BA. 144 * 145 * @return <code>this' = this^*</code> 146 **/ 147 @Override 148 public AxisAngle transpose() { 149 StackContext.enter(); 150 try { 151 Quaternion q = this.toQuaternion(); 152 q = q.transpose(); 153 AxisAngle AA = AxisAngle.valueOf(q); 154 AA._theta = AA._theta.to(_theta.getUnit()); 155 return StackContext.outerCopy(AA); 156 } finally { 157 StackContext.exit(); 158 } 159 } 160 161 /** 162 * Returns the product of this rotation transform with the specified rotation transform. 163 * If this axis/angle rotation is BA and that is AC then the returned 164 * value is: BC = BA times AC (or C2B = A2B times C2A). 165 * 166 * @param that the rotation transform multiplier. 167 * @return <code>this times that</code> 168 */ 169 @Override 170 public AxisAngle times(Rotation<?> that) { 171 StackContext.enter(); 172 try { 173 Quaternion q = this.toQuaternion(); 174 q = q.times(that); 175 AxisAngle AA = AxisAngle.valueOf(q); 176 AA._theta = AA._theta.to(_theta.getUnit()); 177 return StackContext.outerCopy(AA); 178 } finally { 179 StackContext.exit(); 180 } 181 } 182 183 /** 184 * Returns the division of this rotation transform with the specified rotation transform. 185 * 186 * @param that the rotation transform divisor. 187 * @return <code>this / that</code> 188 */ 189 public AxisAngle divide(Rotation<?> that) { 190 StackContext.enter(); 191 try { 192 Quaternion q = this.toQuaternion(); 193 q = q.divide(that); 194 AxisAngle AA = AxisAngle.valueOf(q); 195 AA._theta = AA._theta.to(_theta.getUnit()); 196 return StackContext.outerCopy(AA); 197 } finally { 198 StackContext.exit(); 199 } 200 } 201 202 /** 203 * Transforms a 3D vector from frame A to B using this axis/angle rotation. 204 * 205 * @param v the vector expressed in frame A. 206 * @return the vector expressed in frame B. 207 **/ 208 @Override 209 public <Q extends Quantity> Vector3D<Q> transform(Coordinate3D<Q> v) { 210 Quaternion q = this.toQuaternion(); 211 return q.transform(v); 212 } 213 214 /** 215 * Returns a direction cosine transformation matrix from this axis/angle rotation. 216 * 217 * @return a direction cosine matrix that converts from frame A to B. 218 * @see <a href="http://en.wikipedia.org/wiki/Rotation_matrix#AxisAngle"> 219 * Wikipedia: Rotation Matrix-AxisAngle</a> 220 **/ 221 @Override 222 public DCMatrix toDCM() { 223 Quaternion q = toQuaternion(); 224 return q.toDCM(); 225 } 226 227 /** 228 * Returns a quaternion representing this rotation transformation. 229 * 230 * @return a quaternion that converts from frame A to B. 231 * @see <a href="http://en.wikipedia.org/wiki/AxisAngle">> 232 * Wikipedia: AxisAngle</a> 233 **/ 234 @Override 235 public Quaternion toQuaternion() { 236 return Quaternion.valueOf(_axis, _theta); 237 } 238 239 /** 240 * Returns a copy of this rotation transform 241 * {@link javolution.context.AllocatorContext allocated} 242 * by the calling thread (possibly on the stack). 243 * 244 * @return an identical and independent copy of this rotation transform. 245 */ 246 @Override 247 public AxisAngle copy() { 248 return copyOf(this); 249 } 250 251 /** 252 * Returns the text representation of this rotation transform. 253 * 254 * @return the text representation of this rotation transform. 255 */ 256 @Override 257 public Text toText() { 258 final int dimension = 3; 259 TextBuilder tmp = TextBuilder.newInstance(); 260 if (this.isApproxEqual(IDENTITY)) 261 tmp.append("{IDENTITY}"); 262 else { 263 tmp.append("{ axis = {"); 264 for (int i = 0; i < dimension; i++) { 265 tmp.append(_axis.get(i)); 266 if (i != dimension - 1) 267 tmp.append(", "); 268 } 269 tmp.append("}, theta = "); 270 tmp.append(_theta); 271 tmp.append('}'); 272 } 273 Text txt = tmp.toText(); 274 TextBuilder.recycle(tmp); 275 return txt; 276 } 277 278 /** 279 * Compares this AxisAngle against the specified object for strict 280 * equality (same rotation type and same values). 281 * 282 * @param obj the object to compare with. 283 * @return <code>true</code> if this rotation is identical to that 284 * rotation; <code>false</code> otherwise. 285 **/ 286 @Override 287 public boolean equals(Object obj) { 288 if (this == obj) 289 return true; 290 if ((obj == null) || (obj.getClass() != this.getClass())) 291 return false; 292 293 AxisAngle that = (AxisAngle)obj; 294 if (!this._theta.equals(that._theta)) 295 return false; 296 297 return this._axis.equals(that._axis); 298 } 299 300 /** 301 * Returns the hash code for this rotation. 302 * 303 * @return the hash code value. 304 */ 305 @Override 306 public int hashCode() { 307 int hash = 7; 308 309 int var_code = _theta.hashCode(); 310 hash = hash*31 + var_code; 311 312 var_code = _axis.hashCode(); 313 hash = hash*31 + var_code; 314 315 return hash; 316 } 317 318 319 /** 320 * Holds the default XML representation. For example: 321 * <pre> 322 * <AxisAngle> 323 * <Axis unit="Dimensionless"> 324 * <X value="1.0" /> 325 * <Y value="0.0" /> 326 * <Z value="2.0" /> 327 * <W value="1.0" /> 328 * </Axis> 329 * <Angle value="10" unit="rad"/> 330 * </AxisAngle> 331 * </pre> 332 */ 333 protected static final XMLFormat<AxisAngle> XML = new XMLFormat<AxisAngle>(AxisAngle.class) { 334 335 @Override 336 public AxisAngle newInstance(Class<AxisAngle> cls, InputElement xml) throws XMLStreamException { 337 return FACTORY.object(); 338 } 339 340 @Override 341 public void read(InputElement xml, AxisAngle aa) throws XMLStreamException { 342 343 Vector3D<?> axis = xml.get("Axis", Vector3D.class); 344 Parameter<Angle> angle = xml.get("Angle", Parameter.class); 345 if (!angle.getUnit().isCompatible(SI.RADIAN)) 346 throw new XMLStreamException( RESOURCES.getString("aaBadUnits") ); 347 348 aa._axis = axis.toUnitVector(); 349 aa._theta = angle; 350 351 if (xml.hasNext()) 352 throw new XMLStreamException( RESOURCES.getString("toManyXMLElementsErr") ); 353 } 354 355 @Override 356 public void write(AxisAngle aa, OutputElement xml) throws XMLStreamException { 357 358 xml.add(aa._axis, "Axis", Vector3D.class); 359 xml.add(aa._theta, "Angle", Parameter.class); 360 361 } 362 }; 363 364 365 /////////////////////// 366 // Factory creation. // 367 /////////////////////// 368 369 private static final ObjectFactory<AxisAngle> FACTORY = new ObjectFactory<AxisAngle>() { 370 @Override 371 protected AxisAngle create() { 372 return new AxisAngle(); 373 } 374 }; 375 376 private static AxisAngle newInstance() { 377 AxisAngle o = FACTORY.object(); 378 return o; 379 } 380 381 private static AxisAngle copyOf(AxisAngle original) { 382 AxisAngle o = AxisAngle.newInstance(); 383 o._axis = original._axis.copy(); 384 o._theta = original._theta.copy(); 385 return o; 386 } 387 388 private AxisAngle() {} 389 390 391}