Fix azimuth and elevation fields values for AWSIM (#278)

* Fix azimuth and elevation fields values for AWSIM * Review changes
RobotecAI · Jun 11, 2024 · 4b68cc7 · 4b68cc7
1 parent fd0abc9
commit 4b68cc7
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 10 deletions.
diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h
@@ -309,8 +309,18 @@ typedef enum : int32_t
 	RGL_FIELD_RAY_IDX_U32,
 	RGL_FIELD_ENTITY_ID_I32,
 	RGL_FIELD_DISTANCE_F32,
-	RGL_FIELD_AZIMUTH_F32,   // In radians. Assuming up vector is Y, forward vector is Z.
-	RGL_FIELD_ELEVATION_F32, // In radians. Assuming up vector is Y, forward vector is Z.
+	/**
+	 * Azimuth angle of the hit point in radians.
+	 * Currently only compatible with engines that generate rays as follows:
+	 * uses a left-handed coordinate system, rotation applies in ZXY order, up vector is Y, forward vector is Z
+	 */
+	RGL_FIELD_AZIMUTH_F32,
+	/**
+	 * Elevation angle of the hit point in radians.
+	 * Currently only compatible with engines that generate rays as follows:
+	 * uses a left-handed coordinate system, rotation applies in ZXY order, up vector is Y, forward vector is Z
+	 */
+	RGL_FIELD_ELEVATION_F32,
 	RGL_FIELD_RING_ID_U16,
 	RGL_FIELD_RETURN_TYPE_U8,
 	RGL_FIELD_TIME_STAMP_F64,

diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu
@@ -128,11 +128,13 @@ extern "C" __global__ void __raygen__()
 
 	// Assuming up vector is Y, forward vector is Z (true for Unity).
 	// TODO(msz-rai): allow to define up and forward vectors in RGL
+	// Assuming rays are generated in left-handed coordinate system with the rotation applied in ZXY order.
+	// TODO(msz-rai): move ray generation to RGL to unify rotations
 	if (ctx.azimuth != nullptr) {
-		ctx.azimuth[rayIdx] = rayLocal.toRotationYRad();
+		ctx.azimuth[rayIdx] = rayLocal.toRotationYOrderZXYLeftHandRad();
 	}
 	if (ctx.elevation != nullptr) {
-		ctx.elevation[rayIdx] = rayLocal.toRotationXRad();
+		ctx.elevation[rayIdx] = rayLocal.toRotationXOrderZXYLeftHandRad();
 	}
 
 	if (ctx.doApplyDistortion) {

diff --git a/src/math/Mat3x4f.hpp b/src/math/Mat3x4f.hpp
@@ -143,13 +143,36 @@ struct Mat3x4f
 		return {rc[0][0], rc[0][1], rc[0][2], 0.0f, rc[1][0], rc[1][1], rc[1][2], 0.0f, rc[2][0], rc[2][1], rc[2][2], 0.0f};
 	}
 
-	HostDevFn inline float toRotationXRad() const { return atan2f(rc[2][1], rc[2][2]); }
-
-	HostDevFn inline float toRotationYRad() const { return atan2f(-rc[2][0], sqrtf(powf(rc[2][1], 2) + powf(rc[2][2], 2))); }
-
-	HostDevFn inline float toRotationZRad() const { return atan2f(rc[1][0], rc[0][0]); }
+	HostDevFn inline float toRotationXOrderZXYLeftHandRad() const
+	{
+		// Assuming rotation has been applied in the order: z x y in left-handed coordinate system
+		// Based on: https://www.geometrictools.com/Documentation/EulerAngles.pdf
+		// Taking transpose of the rotation matrix, because X axis facing opposite direction in left-handed coordinate system
+		if (rc[1][2] < 1) {
+			if (rc[1][2] > -1) {
+				return asinf(rc[1][2]);
+			} else {
+				return -static_cast<float>(M_PI) / 2.0f;
+			}
+		} else {
+			return static_cast<float>(M_PI) / 2.0f;
+		}
+	}
 
-	HostDevFn inline Vec3f toRotationRad() const { return {toRotationXRad(), toRotationYRad(), toRotationZRad()}; }
+	HostDevFn inline float toRotationYOrderZXYLeftHandRad() const
+	{
+		// Assuming rotation has been applied in the order: z x y in left-handed coordinate system
+		// Based on: https://www.geometrictools.com/Documentation/EulerAngles.pdf
+		if (rc[2][1] < 1) {
+			if (rc[2][1] > -1) {
+				return atan2f(-rc[2][0], rc[2][2]);
+			} else {
+				return 0.0f;
+			}
+		} else {
+			return 0.0f;
+		}
+	}
 
 	HostDevFn inline Vec3f scaleVec() const
 	{