Merge pull request #104 from Geodan/fix_roll_pitch_yaw

fix roll pitch yaw

Author: bertt

README.md

@@ -58,7 +58,10 @@ Input database table contains following columns:
 
 . scale - double with instance scale (all directions);
 
-. rotation - double with horizontal rotation angle (0 - 360 degrees);
+. rotation - double with horizontal rotation angle (0 - 360 degrees) (legacy, used in i3dm / non-GPU mode);
+. yaw - double with yaw/heading in degrees (clockwise-positive);
+. pitch - double with pitch in degrees;
+. roll - double with roll in degrees;
 
 . tags - json with instance attribute information;
 
@@ -150,6 +153,12 @@ $ i3dm.export -c "Host=localhost;Username=postgres;Password=postgres;Database=t
 
 For getting started with i3dm.export tool see [getting started](docs/getting_started.md).
 
+## Technical documentation (transforms)
+
+For a detailed explanation of how instance transforms are computed (ECEF/ENU to glTF Y-up, handedness, matrix conventions, and GPU instancing TRS composition), see:
+
+- [Technical notes: coordinate systems & transforms](docs/technical-transforms.md)
+
 ## Benchmarking
 
 1] Benchmark on Daily Digital Obstacles files

docs/technical-transforms.md

@@ -0,0 +1,209 @@
+# Technical notes: coordinate systems & transforms
+
+This document explains how `i3dm.export` computes **translation**, **rotation**, and **scale** for instances in both output modes:
+
+- `--use_gpu_instancing=true`: outputs a `.glb` that uses `EXT_mesh_gpu_instancing`.
+- `--use_gpu_instancing=false`: outputs `i3dm` (or `cmpt` containing `i3dm`) using `NORMAL_UP` / `NORMAL_RIGHT`.
+
+It also documents the coordinate-system conversions between **ECEF (EPSG:4978)** and **glTF Y-up**, and clarifies the **matrix conventions** used by glTF vs `System.Numerics`.
+
+## 1) Coordinate systems
+
+### 1.1 ECEF (EPSG:4978)
+Internally, instance positions are converted to **ECEF** (Earth-Centered, Earth-Fixed) coordinates.
+
+- Right-handed coordinate system.
+- Units: meters.
+- Axes (typical convention):
+  - +X: intersection of equator and prime meridian.
+  - +Y: 90° east on equator.
+  - +Z: north pole.
+
+### 1.2 Local tangent frame (ENU)
+For each instance position we derive a local tangent basis:
+
+- **E**: East (tangent)
+- **N**: North (tangent)
+- **U**: Up (surface normal / ellipsoid normal)
+
+This ENU basis is right-handed:
+
+```
+E × N = U
+```
+
+Implementation note:
+- `SpatialConverter.EcefToEnu(position)` computes an orthonormal E/N/U basis at that ECEF position.
+- `EnuCalculator.GetLocalEnuCesium(position, heading, pitch, roll)` starts from that base frame and applies yaw/pitch/roll.
+
+### 1.3 glTF space (Y-up)
+glTF uses a **right-handed** coordinate system with:
+
+- +X right
+- +Y up
+- +Z forward
+
+The exporter outputs **Y-up** glTF.
+
+### 1.4 ECEF → glTF Y-up swizzle
+The exporter maps vectors/points from ECEF to glTF Y-up using the same swizzle in both position and orientation code:
+
+```
+ToYUp(x, y, z) = ( x,  z, -y )
+```
+
+So:
+- ECEF +Z (up-ish) becomes glTF +Y.
+- ECEF +Y becomes glTF -Z.
+
+This keeps the resulting glTF basis right-handed.
+
+## 2) Angle conventions (Yaw / Pitch / Roll)
+
+Angles are in **degrees**.
+
+For GPU instancing, the instance record provides:
+
+- **Yaw**: rotation around local **Up** axis (heading)
+- **Pitch**: rotation around local **East** axis
+- **Roll**: rotation around local **North/Forward** axis
+
+Important: the code uses the same convention as the legacy non-GPU rotation: **clockwise-positive** (as seen from the positive axis direction).
+
+Implementation note:
+- `Rotator.RotateVector(...)` converts clockwise-positive degrees into the standard right-hand-rule rotation by using `360 - angle` internally.
+
+## 3) Matrix conventions: glTF vs System.Numerics
+
+### 3.1 glTF convention
+- Matrices are stored **column-major** in the file.
+- Transforms conceptually use **column vectors**:
+
+```
+world = M * local
+```
+
+### 3.2 System.Numerics convention
+`System.Numerics.Matrix4x4` + `Vector3.Transform(v, M)` uses **row-vector semantics**:
+
+```
+world = local * M
+```
+
+This is the single biggest source of confusion when converting between math written for glTF/Cesium (column vectors) and code using `System.Numerics`.
+
+### 3.3 How this repo constructs rotation matrices
+When we build a rotation matrix from a basis, we intentionally store the **world basis vectors in the matrix rows** so that:
+
+- local X maps to East
+- local Y maps to Up
+- local Z maps to Forward
+
+With row-vector semantics that means:
+
+```
+(1,0,0) * M = East
+(0,1,0) * M = Up
+(0,0,1) * M = Forward
+```
+
+This is why `GPUTileHandler.GetTransformationMatrix(...)` writes basis vectors into the **rows**.
+
+## 4) Export mode: `--use_gpu_instancing=true` (EXT_mesh_gpu_instancing)
+
+### 4.1 What glTF applies at runtime
+In glTF, each mesh node has a node transform (TRS or matrix). With `EXT_mesh_gpu_instancing`, each instance adds its own TRS.
+
+Conceptually (glTF / column vector notation):
+
+```
+worldVertex = NodeWorld * InstanceTRS * vertex
+```
+
+(Where `NodeWorld` includes the full scene graph above the mesh node.)
+
+### 4.2 Preserving node transforms from the input model
+Many models (including Blender exports) include **axis-correction** or other transforms in the scene graph.
+If we drop them, some nodes will be misplaced or rotated.
+
+This exporter preserves per-node transforms by collecting all nodes with meshes and using their `node.WorldMatrix`.
+
+Code path:
+- `GPUTileHandler.CollectNodesWithMeshes(...)`
+- Each mesh node contributes:
+  - the mesh geometry
+  - the node’s `WorldMatrix`
+
+### 4.3 Instance TRS calculation
+For each instance we compute:
+
+1) **Position** (ECEF) → **glTF Y-up** using `ToYUp(Point)`.
+
+2) **Orientation**:
+   - Compute ENU basis at the ECEF position.
+   - Apply yaw/pitch/roll in ENU.
+   - Swizzle each basis vector to glTF Y-up: `ToYUp(Vector3)`.
+   - Re-orthonormalize to reduce numerical drift.
+   - Build a rotation matrix whose rows are `{East, Up, Forward}`.
+   - Convert to quaternion with `Quaternion.CreateFromRotationMatrix`.
+
+3) **Scale**:
+   - Uniform: `Scale`
+   - Non-uniform: `ScaleNonUniform[3]` when `--use_scale_non_uniform=true`.
+
+### 4.4 Combining node and instance transforms
+Each output mesh node uses:
+
+- `nodeTransform = node.WorldMatrix` (from the input model)
+- `instanceTransform = TRS` computed above
+
+And we create a combined transform:
+
+- `combined = nodeTransform * instanceTransform`
+
+(Exact multiplication order is handled by `AffineTransform.Multiply(...)` in SharpGLTF; the intended effect is “apply node’s authored transform, then apply instance TRS”.)
+
+### 4.5 RTC (relative-to-center) translation
+To keep numbers small, we use the first instance position as a per-tile translation anchor.
+
+- We subtract this anchor from each instance translation.
+- At the end, the anchor is applied back to nodes.
+
+This improves numerical precision in clients.
+
+## 5) Export mode: `--use_gpu_instancing=false` (i3dm)
+
+In i3dm, per-instance orientation is encoded via two vectors:
+
+- `NORMAL_RIGHT` (local +X)
+- `NORMAL_UP` (local +Y)
+
+The client derives the final orientation from these vectors.
+
+Current behavior:
+- The non-GPU path currently uses the legacy single **horizontal rotation** value (`rotation`) as a heading angle.
+- It derives `NORMAL_RIGHT` and `NORMAL_UP` from the ENU basis.
+
+(If/when full yaw/pitch/roll is enabled for i3dm, the same ENU + swizzle principles apply; only the storage format differs.)
+
+## 6) Common pitfalls / why models can look “tilted”
+
+1) **Mixing handedness or sign conventions**
+   - Clockwise-positive vs right-hand-rule is easy to flip.
+
+2) **Row-vectors vs column-vectors**
+   - A basis written into columns will be wrong when used with `Vector3.Transform(v, M)`.
+
+3) **Dropping input node transforms**
+   - Some models rely on an axis-correction node; if ignored, the model will be sideways or parts won’t move.
+
+4) **Non-orthonormal basis drift**
+   - Small floating point errors can accumulate; we re-orthonormalize the basis before creating quaternions.
+
+## 7) Code pointers
+
+- ENU basis: `src\Cesium\SpatialConverter.cs` (`EcefToEnu`)
+- Y-up swizzle: `src\GPUTileHandler.cs` (`ToYUp`)
+- Instance TRS (GPU): `src\GPUTileHandler.cs` (`GetInstanceTransform`)
+- Node transform preservation: `src\GPUTileHandler.cs` (`CollectNodesWithMeshes`)
+- Yaw/pitch/roll application: `src\EnuCalculator.cs`

i3dm.export.sln

@@ -1,14 +1,15 @@
 
 Microsoft Visual Studio Solution File, Format Version 12.00
-# Visual Studio Version 17
-VisualStudioVersion = 17.4.33403.182
+# Visual Studio Version 18
+VisualStudioVersion = 18.4.11506.43 insiders
 MinimumVisualStudioVersion = 10.0.40219.1
 Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "doc", "doc", "{68333F7F-D9B8-4215-AE46-6B3AFF14FFBB}"
 	ProjectSection(SolutionItems) = preProject
 		docs\Box.glb = docs\Box.glb
 		docs\getting_started.md = docs\getting_started.md
 		README.md = README.md
 		docs\screenshot.png = docs\screenshot.png
+		docs\technical-transforms.md = docs\technical-transforms.md
 		docs\trees.png = docs\trees.png
 	EndProjectSection
 EndProject

src/EnuCalculator.cs

@@ -7,12 +7,40 @@ public class EnuCalculator
 {
     public static (Vector3 East, Vector3 North, Vector3 Up) GetLocalEnu(double angle, Vector3 position)
     {
-        return GetLocalEnuCesium(position, (float)angle, 0, 0);
+        return GetLocalEnuCesium(position, angle, 0, 0);
     }
 
     public static (Vector3 East, Vector3 North, Vector3 Up) GetLocalEnuCesium(Vector3 position, double heading, double pitch = 0, double roll = 0)
     {
-        var eastUp = CesiumTransformer.GetEastUp(position, heading);
-        return (eastUp.East, new Vector3(0, 0, 0), eastUp.Up);
+        var transform = SpatialConverter.EcefToEnu(position);
+
+        var east = Vector3.Normalize(new Vector3(transform.M11, transform.M12, transform.M13));
+        var north = Vector3.Normalize(new Vector3(transform.M21, transform.M22, transform.M23));
+        var up = Vector3.Normalize(new Vector3(transform.M31, transform.M32, transform.M33));
+
+        if (heading != 0)
+        {
+            east = Vector3.Normalize(Rotator.RotateVector(east, up, heading));
+            north = Vector3.Normalize(Rotator.RotateVector(north, up, heading));
+        }
+
+        if (pitch != 0)
+        {
+            north = Vector3.Normalize(Rotator.RotateVector(north, east, pitch));
+            up = Vector3.Normalize(Rotator.RotateVector(up, east, pitch));
+        }
+
+        if (roll != 0)
+        {
+            east = Vector3.Normalize(Rotator.RotateVector(east, north, roll));
+            up = Vector3.Normalize(Rotator.RotateVector(up, north, roll));
+        }
+
+        // Re-orthonormalize to avoid drift.
+        east = Vector3.Normalize(east);
+        north = Vector3.Normalize(Vector3.Cross(up, east));
+        up = Vector3.Normalize(Vector3.Cross(east, north));
+
+        return (east, north, up);
     }
 }

src/GPUTileHandler.cs

@@ -256,26 +256,22 @@ private static AffineTransform GetInstanceTransform(Instance instance, bool UseS
         // roll  : rotation around local Forward axis
         var positionVector3 = new Vector3((float)point.X, (float)point.Y, (float)point.Z);
 
-        var enu = EnuCalculator.GetLocalEnu(instance.Yaw, positionVector3);
+        var enu = EnuCalculator.GetLocalEnuCesium(positionVector3, instance.Yaw, instance.Pitch, instance.Roll);
+
         var east = Vector3.Normalize(enu.East);
         var up = Vector3.Normalize(enu.Up);
-        var forward = Vector3.Normalize(Vector3.Cross(east, up));
+        var forward = Vector3.Normalize(enu.North);
 
-        if (instance.Pitch != 0)
-        {
-            forward = Vector3.Normalize(Cesium.Rotator.RotateVector(forward, east, instance.Pitch));
-            up = Vector3.Normalize(Cesium.Rotator.RotateVector(up, east, instance.Pitch));
-        }
+        // Convert basis from ECEF to glTF Y-up and build the quaternion in that coordinate system.
+        var eastYUp = Vector3.Normalize(ToYUp(east));
+        var upYUp = Vector3.Normalize(ToYUp(up));
+        var forwardYUp = Vector3.Normalize(ToYUp(forward));
 
-        if (instance.Roll != 0)
-        {
-            east = Vector3.Normalize(Cesium.Rotator.RotateVector(east, forward, instance.Roll));
-            up = Vector3.Normalize(Cesium.Rotator.RotateVector(up, forward, instance.Roll));
-        }
-
-        forward = Vector3.Normalize(Vector3.Cross(east, up));
+        // Orthonormalize (numerical stability).
+        forwardYUp = Vector3.Normalize(Vector3.Cross(eastYUp, upYUp));
+        upYUp = Vector3.Normalize(Vector3.Cross(forwardYUp, eastYUp));
 
-        var m4 = GetTransformationMatrix((east, new Vector3(0, 0, 0), up), forward);
+        var m4 = GetTransformationMatrix((eastYUp, new Vector3(0, 0, 0), upYUp), forwardYUp);
         var res = Quaternion.CreateFromRotationMatrix(m4);
 
         var position2 = new Vector3((float)(position.X - translation.X), (float)(position.Y - translation.Y), (float)(position.Z - translation.Z));
@@ -286,7 +282,7 @@ private static AffineTransform GetInstanceTransform(Instance instance, bool UseS
 
         var transformation = new AffineTransform(
             scale,
-            new Quaternion(-res.X, -res.Z, res.Y, res.W),
+            res,
             position2);
         return transformation;
     }
@@ -337,22 +333,34 @@ private static PropertyTable GetPropertyTable(StructuralMetadataClass schemaClas
 
     private static Matrix4x4 GetTransformationMatrix((Vector3 East, Vector3 North, Vector3 Up) enu, Vector3 forward)
     {
+        // System.Numerics uses row-vector semantics (v * M). To map local axes (X,Y,Z) -> world axes,
+        // the world basis vectors must be stored in the matrix rows.
         var m4 = new Matrix4x4();
+
         m4.M11 = enu.East.X;
-        m4.M21 = enu.East.Y;
-        m4.M31 = enu.East.Z;
+        m4.M12 = enu.East.Y;
+        m4.M13 = enu.East.Z;
 
-        m4.M12 = enu.Up.X;
+        m4.M21 = enu.Up.X;
         m4.M22 = enu.Up.Y;
-        m4.M32 = enu.Up.Z;
+        m4.M23 = enu.Up.Z;
 
-        m4.M13 = forward.X;
-        m4.M23 = forward.Y;
+        m4.M31 = forward.X;
+        m4.M32 = forward.Y;
         m4.M33 = forward.Z;
+
+        m4.M44 = 1;
         return m4;
     }
+
     private static Point ToYUp(Point position)
     {
         return new Point((double)position.X, (double)position.Z, (double)position.Y * -1);
     }
+
+    private static Vector3 ToYUp(Vector3 v)
+    {
+        return new Vector3(v.X, v.Z, -v.Y);
+    }
+
 }