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