Skip to content

Commit 9d9e5f7

Browse files
rondlhrondlh
committed
Formatting update
Formatting update
1 parent cf921fd commit 9d9e5f7

1 file changed

Lines changed: 55 additions & 52 deletions

File tree

‎src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java‎

Lines changed: 55 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -10,11 +10,11 @@
1010
import com.google.gson.annotations.Expose;
1111
import com.neuronrobotics.sdk.common.Log;
1212

13-
// Auto-generated Javadoc
13+
// Auto-generated Javadoc
1414
/**
1515
* This class is to represent a 3x3 rotation sub-matrix This class also contains
1616
* static methods for dealing with 3x3 rotations.
17-
*
17+
*
1818
* @author Kevin Harrington
1919
*
2020
*/
@@ -37,7 +37,6 @@ public class RotationNR {
3737
private static RotationOrder order = RotationOrder.ZYX;
3838
@Expose (serialize = false, deserialize = false)
3939
private static RotationConvention convention = RotationConvention.VECTOR_OPERATOR;
40-
4140

4241
/**
4342
* Null constructor forms a.
@@ -48,10 +47,10 @@ public RotationNR() {
4847
/**
4948
* Instatiate using the
5049
* org.apache.commons.math3.geometry.euclidean.threed.Rotation .
51-
*
50+
*
5251
* @param store
53-
* A org.apache.commons.math3.geometry.euclidean.threed.Rotation
54-
* instance
52+
* A org.apache.commons.math3.geometry.euclidean.threed.Rotation
53+
* instance
5554
*/
5655
public RotationNR(Rotation store) {
5756
setStorage(store);
@@ -61,20 +60,24 @@ public RotationNR(Rotation store) {
6160
* Instantiates a new rotation nr.
6261
*
6362
** @param tilt the tilt in Degrees
64-
* @param azimuth the azimuth in Degrees
63+
* @param azimuth the azimuth in Degrees
6564
* @param elevation the elevation in Degrees
6665
*/
6766
// create a new object with the given simplified rotations
6867
public RotationNR(double tilt, double azimuth, double elevation) {
68+
6969
if (!Double.isFinite(tilt))
70-
throw new RuntimeException("Value can not be "+tilt);
70+
throw new RuntimeException("Value can not be " + tilt);
71+
7172
if (!Double.isFinite(azimuth))
72-
throw new RuntimeException("Value can not be "+azimuth);
73+
throw new RuntimeException("Value can not be " + azimuth);
74+
7375
if (!Double.isFinite(elevation))
74-
throw new RuntimeException("Value can not be "+elevation);
75-
if (elevation > 90 || elevation < -90) {
76+
throw new RuntimeException("Value can not be " + elevation);
77+
78+
if ((elevation > 90) || (elevation < -90))
7679
throw new RuntimeException("Elevation can not be greater than 90 nor less than -90");
77-
}
80+
7881
loadFromAngles(tilt, azimuth, elevation);
7982
if (Double.isNaN(getRotationMatrix2QuaturnionW()) || Double.isNaN(getRotationMatrix2QuaturnionX())
8083
|| Double.isNaN(getRotationMatrix2QuaturnionY()) || Double.isNaN(getRotationMatrix2QuaturnionZ())) {
@@ -86,7 +89,7 @@ public RotationNR(double tilt, double azimuth, double elevation) {
8689
}
8790

8891
public RotationNR(EulerAxis axis, double rot) {
89-
this(axis==EulerAxis.tilt?rot:0,axis==EulerAxis.azimuth?rot:0,axis==EulerAxis.elevation?rot:0);
92+
this((axis == EulerAxis.tilt) ? rot : 0, (axis == EulerAxis.azimuth) ? rot : 0, (axis == EulerAxis.elevation) ? rot : 0);
9093
}
9194

9295
/**
@@ -115,7 +118,7 @@ public RotationNR(double[] values) {
115118
*/
116119
public static RotationNR getRotationX(double rotationAngleDegrees) {
117120
double[][] rotation = new double[3][3];
118-
double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees;
121+
double rotationAngleRadians = rotationAngleDegrees * Math.PI / 180;
119122

120123
// Rotation matrix, 1st column
121124
rotation[0][0] = 1;
@@ -141,7 +144,7 @@ public static RotationNR getRotationX(double rotationAngleDegrees) {
141144
*/
142145
public static RotationNR getRotationY(double rotationAngleDegrees) {
143146
double[][] rotation = new double[3][3];
144-
double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees;
147+
double rotationAngleRadians = rotationAngleDegrees * Math.PI / 180;
145148

146149
// Rotation matrix, 1st column
147150
rotation[0][0] = Math.cos(rotationAngleRadians);
@@ -167,7 +170,7 @@ public static RotationNR getRotationY(double rotationAngleDegrees) {
167170
*/
168171
public static RotationNR getRotationZ(double rotationAngleDegrees) {
169172
double[][] rotation = new double[3][3];
170-
double rotationAngleRadians = Math.PI / 180 * rotationAngleDegrees;
173+
double rotationAngleRadians = rotationAngleDegrees * Math.PI / 180;
171174

172175
// Rotation matrix, 1st column
173176
rotation[0][0] = Math.cos(rotationAngleRadians);
@@ -205,11 +208,10 @@ public RotationNR(double w, double x, double y, double z) {
205208
*/
206209
public RotationNR(Matrix m) {
207210
double[][] rotation = new double[3][3];
208-
for (int i = 0; i < 3; i++) {
209-
for (int j = 0; j < 3; j++) {
211+
for (int i = 0; i < 3; i++)
212+
for (int j = 0; j < 3; j++)
210213
rotation[i][j] = m.get(i, j);
211-
}
212-
}
214+
213215
loadRotations(rotation);
214216
}
215217

@@ -221,11 +223,11 @@ public RotationNR(Matrix m) {
221223
private void loadRotations(double[][] rotM) {
222224
if (rotM.length != 3)
223225
throw new RuntimeException("Must be 3x3 rotation matrix");
224-
for (int i = 0; i < 3; i++) {
225-
if (rotM[i].length != 3) {
226+
227+
for (int i = 0; i < 3; i++)
228+
if (rotM[i].length != 3)
226229
throw new RuntimeException("Must be 3x3 rotation matrix");
227-
}
228-
}
230+
229231
setStorage(new Rotation(rotM, 0.00001));
230232
}
231233

@@ -241,7 +243,7 @@ public double[][] getRotationMatrix() {
241243

242244
/*
243245
* (non-Javadoc)
244-
*
246+
*
245247
* @see java.lang.Object#toString()
246248
*/
247249
// return a string representation of the invoking object
@@ -251,10 +253,9 @@ public String toString() {
251253
+ ", " + "y=" + getRotationMatrix2QuaturnionY() + ", " + "z=" + getRotationMatrix2QuaturnionZ() + "\n"
252254
+ "Rotation angle (degrees): " + "az= " + Math.toDegrees(getRotationAzimuthRadians()) + ", elev= "
253255
+ Math.toDegrees(getRotationElevation()) + ", tilt=" + Math.toDegrees(getRotationTilt());
254-
}catch(Exception ex){
256+
} catch(Exception ex){
255257
return "Rotation error" + ex.getLocalizedMessage();
256258
}
257-
258259
}
259260

260261
/**
@@ -286,18 +287,23 @@ public String toString(double[][] array) {
286287
* @param z the z
287288
*/
288289
protected void quaternion2RotationMatrix(double w, double x, double y, double z) {
290+
289291
if (Double.isNaN(w))
290292
throw new RuntimeException("Value can not be NaN");
293+
291294
if (Double.isNaN(x))
292295
throw new RuntimeException("Value can not be NaN");
296+
293297
if (Double.isNaN(y))
294298
throw new RuntimeException("Value can not be NaN");
299+
295300
if (Double.isNaN(z))
296301
throw new RuntimeException("Value can not be NaN");
297-
this.w=w;
298-
this.x= -x;
299-
this.y= -y;
300-
this.z= -z;
302+
303+
this.w = w;
304+
this.x = -x;
305+
this.y = -y;
306+
this.z = -z;
301307
}
302308

303309
/**
@@ -309,7 +315,7 @@ protected void quaternion2RotationMatrix(double w, double x, double y, double z)
309315
* @return true, if successful
310316
*/
311317
public static boolean bound(double low, double high, double n) {
312-
return n >= low && n <= high;
318+
return ((n >= low) && (n <= high));
313319
}
314320

315321
private void loadFromAngles(double tilt, double azimuth, double elevation) {
@@ -340,7 +346,6 @@ public double getRotationElevationRadians() {
340346
*
341347
* @return the rotation azimuth in radians
342348
*/
343-
344349
public double getRotationAzimuthRadians() {
345350
return getAngle(0);
346351
}
@@ -361,15 +366,14 @@ public double getRotationTiltDegrees() {
361366
*/
362367
public double getRotationElevationDegrees() {
363368
return Math.toDegrees(getRotationElevationRadians());
364-
365369
}
366370

367371
/**
368372
* Gets the rotation azimuth.
369373
*
370374
* @return the rotation azimuth in degrees
371375
*/
372-
376+
373377
public double getRotationAzimuthDegrees() {
374378
return Math.toDegrees( getRotationAzimuthRadians());
375379
}
@@ -402,40 +406,39 @@ public double getRotationElevation() {
402406
*/
403407
@Deprecated
404408
public double getRotationAzimuth() {
405-
return getRotationAzimuthRadians();
409+
return getRotationAzimuthRadians();
406410
}
407411

408412
private void simpilfyAngles(double [] angles){
409-
double epsilon=1.0E-7;
410-
if(Math.abs(angles[0] - Math.toRadians(180)) < epsilon&&
411-
Math.abs(angles[2] - Math.toRadians(180)) < epsilon ){
412-
if(!(Math.abs(getRotationMatrix2QuaturnionZ())>epsilon))
413-
angles[0]=0;
414-
if(!(Math.abs(getRotationMatrix2QuaturnionX())>epsilon))
415-
angles[2]=0;
413+
double epsilon = 1.0E-7;
414+
if ((Math.abs(angles[0] - Math.toRadians(180)) < epsilon) &&
415+
Math.abs(angles[2] - Math.toRadians(180)) < epsilon)) {
416+
if (!(Math.abs(getRotationMatrix2QuaturnionZ()) > epsilon))
417+
angles[0] = 0;
418+
419+
if (!(Math.abs(getRotationMatrix2QuaturnionX()) > epsilon))
420+
angles[2] = 0;
416421
}
417422
}
418423

419424
private double eulerFix(double offsetSize, int index){
420-
double offset = (index==1?offsetSize:0);
425+
double offset = ((index == 1) ? offsetSize : 0);
421426
TransformNR current = new TransformNR(0, 0, 0, this);
422-
TransformNR newTf = current.times(new TransformNR(0, 0, 0, new RotationNR(0,0,Math.toDegrees(offsetSize))));
427+
TransformNR newTf = current.times(new TransformNR(0, 0, 0, new RotationNR(0, 0, Math.toDegrees(offsetSize))));
423428
double[] angles = newTf.getRotation().getStorage().getAngles(getOrder(), getConvention());
424429
simpilfyAngles(angles);
425-
double finalResult= angles[index];
426-
return finalResult+offset;
430+
double finalResult = angles[index];
431+
return finalResult + offset;
427432
}
428433

429-
private double getAngle(int index){
430-
434+
private double getAngle(int index) {
431435
try {
432436
return getStorage().getAngles(getOrder(), getConvention())[index];
433437
} catch (CardanEulerSingularityException e) {
434438
try {
435-
return eulerFix( Math.toRadians(0.001), index);
439+
return eulerFix(Math.toRadians(0.001), index);
436440
} catch (CardanEulerSingularityException ex) {
437-
return eulerFix( Math.toRadians(-0.001), index);
438-
441+
return eulerFix(Math.toRadians(-0.001), index);
439442
}
440443
}
441444
}

0 commit comments

Comments
 (0)