98051ba0c9
- Move existing OndselSolver, GNN ML layer, and tooling into GNN/ directory for integration in later phases - Add Create addon scaffold: package.xml, Init.py - Add expression DAG with eval, symbolic diff, simplification - Add parameter table with fixed/free variable tracking - Add quaternion rotation as polynomial Expr trees - Add RigidBody entity (7 DOF: position + unit quaternion) - Add constraint classes: Coincident, DistancePointPoint, Fixed - Add Newton-Raphson solver with symbolic Jacobian + numpy lstsq - Add pre-solve passes: substitution + single-equation - Add DOF counting via Jacobian SVD rank - Add KindredSolver IKCSolver bridge for kcsolve integration - Add 82 unit tests covering all modules Registers as 'kindred' solver via kcsolve.register_solver() when loaded by Create's addon_loader.
72 lines
1.9 KiB
Python
72 lines
1.9 KiB
Python
"""Quaternion math expressed as Expr trees.
|
|
|
|
All functions take Expr nodes (typically Var nodes from a RigidBody's
|
|
parameter set) and return Expr trees. Quaternion rotation is polynomial
|
|
in the quaternion components — no transcendentals needed.
|
|
|
|
Convention: quaternion (qw, qx, qy, qz) matches KCSolve's (w, x, y, z).
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from .expr import Const, Expr
|
|
|
|
|
|
def quat_rotate(
|
|
qw: Expr,
|
|
qx: Expr,
|
|
qy: Expr,
|
|
qz: Expr,
|
|
px: Expr,
|
|
py: Expr,
|
|
pz: Expr,
|
|
) -> tuple[Expr, Expr, Expr]:
|
|
"""Rotate point (px, py, pz) by unit quaternion (qw, qx, qy, qz).
|
|
|
|
Uses the standard formula: p' = q * p * q^{-1}
|
|
expanded into component form (polynomial in q):
|
|
|
|
rx = (1 - 2(qy^2 + qz^2)) * px + 2(qx*qy - qw*qz) * py + 2(qx*qz + qw*qy) * pz
|
|
ry = 2(qx*qy + qw*qz) * px + (1 - 2(qx^2 + qz^2)) * py + 2(qy*qz - qw*qx) * pz
|
|
rz = 2(qx*qz - qw*qy) * px + 2(qy*qz + qw*qx) * py + (1 - 2(qx^2 + qy^2)) * pz
|
|
"""
|
|
two = Const(2.0)
|
|
|
|
rx = (
|
|
(Const(1.0) - two * (qy * qy + qz * qz)) * px
|
|
+ two * (qx * qy - qw * qz) * py
|
|
+ two * (qx * qz + qw * qy) * pz
|
|
)
|
|
ry = (
|
|
two * (qx * qy + qw * qz) * px
|
|
+ (Const(1.0) - two * (qx * qx + qz * qz)) * py
|
|
+ two * (qy * qz - qw * qx) * pz
|
|
)
|
|
rz = (
|
|
two * (qx * qz - qw * qy) * px
|
|
+ two * (qy * qz + qw * qx) * py
|
|
+ (Const(1.0) - two * (qx * qx + qy * qy)) * pz
|
|
)
|
|
return rx, ry, rz
|
|
|
|
|
|
def world_point(
|
|
tx: Expr,
|
|
ty: Expr,
|
|
tz: Expr,
|
|
qw: Expr,
|
|
qx: Expr,
|
|
qy: Expr,
|
|
qz: Expr,
|
|
lx: float,
|
|
ly: float,
|
|
lz: float,
|
|
) -> tuple[Expr, Expr, Expr]:
|
|
"""Transform local point (lx, ly, lz) to world coordinates.
|
|
|
|
world = rotation(q, local) + translation
|
|
"""
|
|
clx, cly, clz = Const(lx), Const(ly), Const(lz)
|
|
rx, ry, rz = quat_rotate(qw, qx, qy, qz, clx, cly, clz)
|
|
return tx + rx, ty + ry, tz + rz
|