# Section 04: World Transforms and Spatial Query Functions

## Overview

This section adds world-space transform calculation and spatial query functions to the scene resolver module at `/Users/salvatorefarruggio/Desktop/godotiq/src/godotiq/parsers/scene_resolver.py`. After section 03 establishes the `ResolvedScene` with recursively-expanded instance trees, this section computes where every node actually sits in 3D world space by composing parent-child transform chains using raw basis matrices.

The key functions added are:

- `calculate_world_transforms()` -- walks a `ResolvedScene` tree top-down, composing local transforms into world transforms, and returns a flat `list[WorldNode]`.
- `find_nodes_by_type()` -- filter `WorldNode` list by Godot node type.
- `find_nodes_by_group()` -- filter `WorldNode` list by group membership.
- `find_nodes_in_radius()` -- filter `WorldNode` list by Euclidean distance from a center point.

All transform composition uses raw column-major 3x3 basis matrices (not Euler angles) to avoid gimbal lock. Euler angles are extracted only at the final output step.

## Dependencies

- **Section 01 (value-parser-raw):** The `decompose_transform3d()` function must include a `"raw"` key in its return dict containing the original 12-float tuple. This is critical -- `calculate_world_transforms()` reads `node.properties["transform"]["raw"]` to extract basis columns without Euler round-tripping.
- **Section 03 (scene-resolver-core):** The `ResolvedScene` dataclass, `WorldNode` dataclass, and `resolve_scene()` function must exist. This section adds `calculate_world_transforms()` and the query functions to the same file.

## File to Modify

`/Users/salvatorefarruggio/Desktop/godotiq/src/godotiq/parsers/scene_resolver.py`

This file is created by section 03. This section adds new functions and any needed internal helpers to it.

## Background: Data Structures from Section 03

For reference, these are the dataclasses that section 03 creates in `scene_resolver.py`. Do not recreate them -- they already exist when this section is implemented.

```python
@dataclass
class ResolvedScene:
    source_path: str
    root: TscnNode
    total_nodes: int
    instance_sources: dict[str, str]  # node full_path -> source res:// path
    unresolved: list[str]

@dataclass
class WorldNode:
    node: TscnNode
    world_position: tuple[float, float, float]
    world_rotation: tuple[float, float, float]  # YXZ Euler radians
    world_scale: tuple[float, float, float]
    full_path: str
    depth: int
    is_instance_root: bool
    instance_source: str | None
```

The `TscnNode` dataclass (from `tscn_parser.py`) has these relevant fields:

```python
@dataclass
class TscnNode:
    name: str = ""
    type: str = ""
    children: list[TscnNode] = field(default_factory=list)
    groups: list[str] = field(default_factory=list)
    position: tuple[float, float, float] | None = None
    rotation: tuple[float, float, float] | None = None
    scale: tuple[float, float, float] | None = None
    properties: dict[str, object] = field(default_factory=dict)
```

The `transform` property on a node (stored in `node.properties["transform"]`) is a dict returned by `decompose_transform3d()`:

```python
{"position": (ox, oy, oz), "rotation": (rx, ry, rz), "scale": (sx, sy, sz), "raw": (b00, b10, b20, b01, b11, b21, b02, b12, b22, ox, oy, oz)}
```

## Tests

All tests below belong in `/Users/salvatorefarruggio/Desktop/godotiq/tests/test_parsers/test_scene_resolver.py`, which is created by section 06. However, they are specified here so that an implementer can write them first (TDD) to validate the transform math before wiring up the full integration.

### Unit Tests -- Transform Math

These tests exercise the internal transform composition and point transformation logic. They can test internal helper functions directly, or test through `calculate_world_transforms()` with hand-constructed `TscnNode` trees.

**`test_compose_identity`**

Composing the identity transform with itself yields identity. Construct a root node with no `transform` property and a child node also with no `transform` property. After `calculate_world_transforms()`, both nodes should have `world_position = (0, 0, 0)`, `world_rotation = (0, 0, 0)`, and `world_scale = (1, 1, 1)`.

**`test_compose_translation`**

Two pure translations compose by adding origins. Root has `Transform3D(1,0,0, 0,1,0, 0,0,1, 10,0,0)` (identity basis, origin at 10,0,0). Child has `Transform3D(1,0,0, 0,1,0, 0,0,1, 0,0,5)` (identity basis, origin at 0,0,5). Child's world position should be `(10, 0, 5)`.

**`test_compose_rotation_translation`**

Parent rotated 90 degrees around Y axis, child at local `(0,0,1)`. A 90-degree Y rotation maps the local Z axis onto the world X axis. So the child's world position should be approximately `(1, 0, 0)`.

The parent transform for 90 degrees Y rotation is:
```
Transform3D(0, 0, -1, 0, 1, 0, 1, 0, 0, 0, 0, 0)
```
(cos90=0, sin90=1: column 0 = (0,0,-1), column 1 = (0,1,0), column 2 = (1,0,0))

The child transform is:
```
Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1)
```

Expected child world position: approximately `(1, 0, 0)`. Use `pytest.approx` with a tolerance around 1e-6 for floating-point comparisons.

**`test_basis_multiply_identity`**

Identity basis multiplied by any basis equals that same basis. This can be tested by constructing a node tree where the parent has identity transform and the child has a non-trivial rotation. The child's world rotation should match its local rotation.

**`test_basis_xform_point`**

Transform a point by a rotated basis. A 90-degree Y rotation basis applied to point `(0, 0, 1)` should yield approximately `(1, 0, 0)`. This tests the `basis.xform(point)` operation: `col0*point.x + col1*point.y + col2*point.z`.

**`test_reconstruct_from_raw`**

Given raw 12 floats from a known transform, extract the column vectors and verify they match expected values. For example, from `(0, 0, -1, 0, 1, 0, 1, 0, 0, 5, 3, 2)`:
- Column 0 (X axis): `(0, 0, -1)`
- Column 1 (Y axis): `(0, 1, 0)`
- Column 2 (Z axis): `(1, 0, 0)`
- Origin: `(5, 3, 2)`

### Integration Tests -- World Transform Calculation

These tests use `ResolvedScene` objects (either hand-constructed or from actual fixture resolution) and pass them through `calculate_world_transforms()`.

**`test_world_position_root`**

Root node with transform having identity basis and origin `(5, 0, 0)`. After `calculate_world_transforms()`, root `WorldNode.world_position == (5, 0, 0)`.

**`test_world_position_child`**

Root at `(10, 0, 0)` (no rotation), child at local `(0, 0, 5)`. Child world position should be `(10, 0, 5)`.

**`test_world_position_with_rotation`**

Root at origin, rotated 90 degrees around Y. Child at local `(0, 0, 1)`. Child world position should be approximately `(1, 0, 0)` (Z maps to X under 90-degree Y rotation).

**`test_world_position_deep_nesting`**

4 levels of nodes, each with a pure translation (identity basis). World position of the deepest node is the sum of all ancestor translations plus its own.

**`test_world_position_with_scale`**

Parent scaled 2x uniformly. Child at local `(1, 0, 0)`. Child world position equals `parent_position + (2, 0, 0)` (the local offset is scaled by the parent's scale).

The parent transform for uniform 2x scale is:
```
Transform3D(2, 0, 0, 0, 2, 0, 0, 0, 2, 0, 0, 0)
```

Child at local (1,0,0):
```
Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0)
```

Expected child world position: `(2, 0, 0)`.

**`test_world_position_resolved_instance`**

Resolve `main.tscn` which instances `fdm_printer.tscn`. Then call `calculate_world_transforms()` on the resolved scene. Verify that a child node inside the instanced FDMPrinter has a world position equal to the FDMPrinter instance's world position plus the child's local offset (adjusted for any rotation on the instance).

This test requires the sample project fixtures from section 02.

**`test_is_instance_root_populated`**

After resolving and calculating transforms, the `WorldNode` corresponding to the instance root node has `is_instance_root=True` and `instance_source` set to the correct `res://` path.

### Integration Tests -- Query Functions

**`test_find_nodes_by_type`**

After resolving a sample scene and calculating world transforms, call `find_nodes_by_type(world_nodes, "Node3D")`. Verify the result count matches expectations and all returned nodes have `node.type == "Node3D"`.

**`test_find_nodes_by_group`**

After resolving, call `find_nodes_by_group(world_nodes, "some_group")`. Verify results contain only nodes whose `node.groups` list includes `"some_group"`.

**`test_find_nodes_in_radius_includes_nearby`**

Place a center point near a known node's world position. Call `find_nodes_in_radius(world_nodes, center, radius)` with a radius large enough to include it. Verify the node appears in results.

**`test_find_nodes_in_radius_excludes_far`**

Use a center point far from all nodes with a small radius. Verify the result is empty.

## Implementation Details

### Internal Transform Representation

During tree traversal, transforms are represented as a tuple of `(basis, origin)` where:

- `basis` is a tuple of 3 column vectors, each a 3-tuple of floats: `((col0x, col0y, col0z), (col1x, col1y, col1z), (col2x, col2y, col2z))`
- `origin` is a 3-tuple: `(x, y, z)`
- Identity transform: `basis=((1,0,0),(0,1,0),(0,0,1)), origin=(0,0,0)`

This representation is used only internally during the tree walk. The final `WorldNode` output uses decomposed `world_position`, `world_rotation`, and `world_scale` tuples.

### Extracting Local Transform from a TscnNode

```python
def _get_local_transform(node: TscnNode) -> tuple[tuple[tuple[float, ...], ...], tuple[float, float, float]]:
    """Extract the local transform from a node as (basis_columns, origin).

    Uses the 'raw' key from the transform property when available.
    Falls back to identity if no transform property exists.
    """
```

Logic:
1. Check if `"transform"` exists in `node.properties`.
2. If yes, check if it is a dict with a `"raw"` key (it should be, given the section 01 change).
3. Extract from the raw 12-float tuple:
   - Column 0 (X axis): `(raw[0], raw[1], raw[2])`
   - Column 1 (Y axis): `(raw[3], raw[4], raw[5])`
   - Column 2 (Z axis): `(raw[6], raw[7], raw[8])`
   - Origin: `(raw[9], raw[10], raw[11])`
4. If `"raw"` key is missing (defensive fallback), reconstruct from the node's `position`, `rotation`, `scale` fields. This is less accurate due to Euler round-tripping but prevents crashes.
5. If no `"transform"` property exists at all, return the identity transform.

### Basis Multiplication

Implement `_basis_multiply(a, b)` for 3x3 column-major matrix multiplication. Given two bases `a` and `b`, each represented as 3 column vectors:

```python
def _basis_multiply(
    a: tuple[tuple[float, ...], ...],
    b: tuple[tuple[float, ...], ...],
) -> tuple[tuple[float, ...], ...]:
    """Multiply two 3x3 basis matrices (column-major representation).

    Each basis is a tuple of 3 column vectors.
    Result[col] = a * b[col], i.e., each result column is a transformed by the corresponding b column.
    """
```

The formula for column `j` of the result: `result_col_j = a.xform(b_col_j)` where `xform` applies the basis to a point.

### Point Transformation (xform)

```python
def _basis_xform(basis: tuple[tuple[float, ...], ...], point: tuple[float, float, float]) -> tuple[float, float, float]:
    """Transform a point by a basis matrix: col0*x + col1*y + col2*z."""
```

This computes:
```
result_x = col0[0]*point[0] + col1[0]*point[1] + col2[0]*point[2]
result_y = col0[1]*point[0] + col1[1]*point[1] + col2[1]*point[2]
result_z = col0[2]*point[0] + col1[2]*point[1] + col2[2]*point[2]
```

### Transform Composition

```python
def _compose_transforms(parent, child):
    """Compose parent and child transforms (matching Godot's Transform3D operator*=).

    child_world.origin = parent.basis.xform(child_local.origin) + parent.origin
    child_world.basis  = parent.basis * child_local.basis
    """
```

This is the core operation. Given:
- `parent = (parent_basis, parent_origin)`
- `child = (child_basis, child_origin)`

The composed world transform is:
- `world_origin = _basis_xform(parent_basis, child_origin) + parent_origin` (component-wise addition)
- `world_basis = _basis_multiply(parent_basis, child_basis)`

### Euler Angle Extraction from World Basis

After composition, decompose the final world basis back to YXZ Euler angles and scale for the `WorldNode` output. The decomposition logic is the same algorithm used in `decompose_transform3d()` in `value_parser.py`:

1. Compute scale from column vector magnitudes: `sx = |col0|`, `sy = |col1|`, `sz = |col2|`.
2. Check determinant sign for negative scale correction.
3. Normalize each column by its scale factor.
4. Apply YXZ Euler extraction from the normalized basis rows.

Rather than duplicating this code, consider calling `decompose_transform3d()` by reconstructing the 12-float tuple from the composed basis and origin, then reading off `position`, `rotation`, and `scale` from the result. This reuses the existing tested decomposition logic. The 12-float tuple is assembled as:

```python
raw = (
    basis[0][0], basis[0][1], basis[0][2],  # column 0
    basis[1][0], basis[1][1], basis[1][2],  # column 1
    basis[2][0], basis[2][1], basis[2][2],  # column 2
    origin[0], origin[1], origin[2],         # origin
)
result = decompose_transform3d(raw)
```

### calculate_world_transforms()

```python
def calculate_world_transforms(resolved: ResolvedScene) -> list[WorldNode]:
    """Walk the resolved scene tree and compute world-space transforms for every node.

    Uses depth-first traversal, composing parent transforms with child local transforms
    using raw basis matrix math (no Euler angle composition, avoiding gimbal lock).

    Args:
        resolved: A ResolvedScene from resolve_scene().

    Returns:
        Flat list of WorldNode, one per node in the tree, in depth-first order.
    """
```

Algorithm:

1. Define the identity transform: `IDENTITY = (((1,0,0),(0,1,0),(0,0,1)), (0,0,0))`.
2. Initialize an empty result list.
3. Define a recursive inner function `_walk(node, parent_world_transform, parent_path, depth)`:
   a. Get the node's local transform using `_get_local_transform(node)`.
   b. Compose with parent's world transform: `world_transform = _compose_transforms(parent_world_transform, local_transform)`.
   c. Decompose the world transform to get `world_position`, `world_rotation`, `world_scale`.
   d. Compute `full_path`: empty string for root (depth 0), `node.name` for direct children of root, `parent_path + "/" + node.name` for deeper nodes.
   e. Check `full_path` against `resolved.instance_sources` to set `is_instance_root` and `instance_source`.
   f. Create `WorldNode` and append to result list.
   g. Recurse into `node.children`, passing the current world transform, the current full_path, and `depth + 1`.
4. Call `_walk(resolved.root, IDENTITY, "", 0)`.
5. Return the result list.

### Query Functions

```python
def find_nodes_by_type(world_nodes: list[WorldNode], node_type: str) -> list[WorldNode]:
    """Return all WorldNodes whose node.type matches the given type string."""

def find_nodes_by_group(world_nodes: list[WorldNode], group: str) -> list[WorldNode]:
    """Return all WorldNodes whose node.groups contains the given group name."""

def find_nodes_in_radius(
    world_nodes: list[WorldNode],
    center: tuple[float, float, float],
    radius: float,
) -> list[WorldNode]:
    """Return all WorldNodes within Euclidean distance `radius` from `center`.

    Distance formula: sqrt((wx-cx)^2 + (wy-cy)^2 + (wz-cz)^2)
    Comparison uses <= (inclusive).
    """
```

These are straightforward list comprehensions:

- `find_nodes_by_type`: filter where `wn.node.type == node_type`
- `find_nodes_by_group`: filter where `group in wn.node.groups`
- `find_nodes_in_radius`: compute Euclidean distance from `wn.world_position` to `center`, include if `<= radius`. Use `math.sqrt` or compare squared distances for efficiency (compare `dist_sq <= radius * radius` to avoid the square root).

## Path Construction Rules

The `full_path` in `WorldNode` follows these conventions (matching section 03):

- Root node: `full_path = ""` (empty string)
- Direct child of root: `full_path = child.name` (e.g., `"Equipment"`)
- Deeper nodes: `full_path = parent_path + "/" + child.name` (e.g., `"Equipment/FDMPrinter_1/WorkPoint"`)
- Instance-expanded children inherit the parent instance node's path as prefix. If `Equipment/FDMPrinter_1` was expanded from `fdm_printer.tscn`, its children become `Equipment/FDMPrinter_1/PrinterModel`, etc.

## Known Limitations

- **2D nodes:** Only 3D transforms (`Transform3D`) are handled. `Node2D` position/rotation/scale as separate properties are not processed.
- **Editable children:** The `[editable path="..."]` feature for deep child overrides in instanced scenes is not implemented.
- **Child ordering:** The `index` attribute on override nodes (controlling child order) is not processed.
- **Euler decomposition precision:** The final Euler angle extraction from the composed world basis can lose precision near gimbal lock configurations. The world position and scale are always accurate since they are derived directly from the composed matrix.

## Verification

After implementation, run the scene resolver tests:

```bash
cd /Users/salvatorefarruggio/Desktop/godotiq && python -m pytest tests/test_parsers/test_scene_resolver.py -v -k "transform or compose or basis or world or radius or type or group"
```

All transform math unit tests and world position integration tests should pass. The query function tests should also pass.