From 1312baa41d26bc7795b863dc49a1e0596b220d51 Mon Sep 17 00:00:00 2001
From: Joppe Blondel <joppe@joppeb.nl>
Date: Mon, 9 Mar 2026 18:27:26 +0100
Subject: [PATCH] pre-big change

---
 .gitignore                           |   1 +
 BondGraph/_1D/package.mo             | 234 -----------------------
 BondGraph/package.mo                 | 220 ++++++++++++++++++++-
 BondGraph/package.order              |   1 -
 {BondGraph/_2D => _2D}/package.mo    |   0
 {BondGraph/_2D => _2D}/package.order |   0
 _3D/package.mo                       | 275 +++++++++++++++++++++++++++
 {BondGraph/_1D => _3D}/package.order |   4 +-
 formatter.py                         |   9 +-
 test1d.mo                            |  17 ++
 test2d.mo                            | 103 +++++-----
 test3d.mo                            | 158 +++++++++++++++
 vis2d.mo                             |  84 ++++++--
 vis3d.mo                             | 103 ++++++++++
 14 files changed, 905 insertions(+), 304 deletions(-)
 delete mode 100644 BondGraph/_1D/package.mo
 rename {BondGraph/_2D => _2D}/package.mo (100%)
 rename {BondGraph/_2D => _2D}/package.order (100%)
 create mode 100644 _3D/package.mo
 rename {BondGraph/_1D => _3D}/package.order (77%)
 create mode 100644 test1d.mo
 create mode 100644 test3d.mo
 create mode 100644 vis3d.mo

diff --git a/.gitignore b/.gitignore
index 567609b..ad776c5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,2 @@
 build/
+__pycache__*
\ No newline at end of file
diff --git a/BondGraph/_1D/package.mo b/BondGraph/_1D/package.mo
deleted file mode 100644
index 85ae4e3..0000000
--- a/BondGraph/_1D/package.mo
+++ /dev/null
@@ -1,234 +0,0 @@
-within BondGraph;
-package _1D
-
-    connector BondPort "Bond graph power port"
-        Real e "Effort variable";
-        flow Real f "Flow variable";annotation(
-        Icon(graphics = {Rectangle(lineColor = {0, 0, 127}, fillColor = {0, 0, 127},fillPattern = FillPattern.Solid, extent = {{-60, 60}, {60, -60}})}));
-    end BondPort;
-
-    model J1 "Bond graph 1-junction (common flow, efforts sum to zero)"
-        parameter Integer N(min=1) = 2 "# of power ports";
-        parameter Real s[N] = fill(1.0, N)
-        "Bond orientation signs used in the effort balance";
-        BondPort P[N] "Power ports" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-        Real f;
-    equation
-        // Efforts sum to zero, with signs from bond directions
-        sum(s[i] * P[i].e for i in 1:N) = 0;
-
-        // Flows are all equal
-        for i in 2:N loop
-            P[i].f = P[i-1].f;
-        end for;
-        f = P[1].f;
-        annotation(
-        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end J1;
-
-    model J0 "Bond graph 0-junction (common effort, flows sum to zero)"
-        parameter Integer N(min=1) = 2 "# of power ports";
-        parameter Real s[N] = fill(1.0, N)
-        "Bond orientation signs used in the effort balance";
-        BondPort P[N] "Power ports" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-        Real e;
-    equation
-        // Flows sum to zero, with signs from bond directions
-        sum(s[i] * P[i].f for i in 1:N) = 0;
-
-        // Efforts are all equal
-        for i in 2:N loop
-            P[i].e = P[1].e;
-        end for;
-        e = P[1].e;
-        annotation(
-        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "0"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end J0;
-
-    partial model OnePortPassive "One-port passive bond graph element"
-        BondPort p "Generic power port" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-    end OnePortPassive;
-
-    partial model OnePortEnergetic "One-port storage element"
-        extends OnePortPassive annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {0, 80}, extent = {{-10, -10}, {10, 10}})));
-        Real state "Conserved quantity";
-    end OnePortEnergetic;
-
-    model C "Bond graph C element"
-        extends OnePortEnergetic(state(start=q0, fixed=true));
-        parameter Real c(min=0) = 1 "Capacitance";
-        parameter Real q0 = 0 "Initial stored quantity (charge)";
-    equation
-        der(state) = p.f;
-        p.e = state / c;
-        annotation(
-        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "C"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end C;
-
-    model I "Bond graph I element"
-        extends OnePortEnergetic(state(start=p0, fixed=true));
-        parameter Real I(min=0) = 1 "Inertance / inductance / mass";
-        parameter Real p0 = 0 "Initial stored quantity (momentum / flux)";
-    equation
-        der(state) = p.e;
-        p.f = state / I;
-        annotation(
-        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "I"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end I;
-
-    model R "Bond graph resistor"
-        extends OnePortPassive;
-        parameter Real R(min=0) = 1 "Resistance";
-    equation
-        p.e = R * p.f;
-        annotation(
-        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "R"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end R;
-
-    model Se "Effort source"
-        BondPort p annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-        parameter Real e0 "Imposed effort";
-    equation
-        p.e = e0;
-        annotation(
-        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Se"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end Se;
-
-    model Sf "Flow source"
-        BondPort p annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-        parameter Real f0 "Imposed flow";
-    equation
-        p.f = f0;
-        annotation(
-        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Sf"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end Sf;
-
-    model TF "Bond graph transformer"
-        BondPort p1 "Port 1" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-        BondPort p2 "Port 2" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-
-        parameter Real n = 1 "Transformer ratio";
-    equation
-        p1.e = n * p2.e;
-        p2.f = n * p1.f;
-        annotation(
-        Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "TF"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end TF;
-
-    model GY "Bond graph gyrator"
-        BondPort p1 "Port 1" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-        BondPort p2 "Port 2" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-        parameter Real r = 1 "Gyrator modulus";
-    equation
-        p1.e = r * p2.f;
-        p2.e = r * p1.f;
-        annotation(
-        Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "GY"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end GY;
-
-    model mSe "Bond graph modulated effort source"
-        BondPort p annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-        Modelica.Blocks.Interfaces.RealInput e0 "Imposed effort" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-
-    equation
-        p.e = e0;
-        annotation(
-        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSe"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end mSe;
-
-    model mSf "Bond graph modulated flow source"
-        BondPort p annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-        Modelica.Blocks.Interfaces.RealInput f0 "Imposed flow" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-
-    equation
-        p.f = f0;
-        annotation(
-        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSf"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-    Diagram(graphics));
-    end mSf;
-
-    model mTF "Bond graph modulated transformer"
-        BondPort p1 "Port 1" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-        BondPort p2 "Port 2" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-        Modelica.Blocks.Interfaces.RealInput m "Modulation" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-    equation
-        p1.e = m * p2.e;
-        p2.f = m * p1.f;
-        annotation(
-        Diagram(graphics),
-        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mTF"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}));
-    end mTF;
-
-    model mGY "Bond graph modulated gyrator"
-        BondPort p1 "Port 1" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-        BondPort p2 "Port 2" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-        Modelica.Blocks.Interfaces.RealInput m "Modulation" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-    equation
-        p1.e = m * p2.f;
-        p2.e = m * p1.f;
-        annotation(
-        Diagram(graphics),
-        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mGY"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}));
-    end mGY;
-
-    model fsensor
-        BondPort p annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-52, 0}, extent = {{-10, -10}, {10, 10}})));
-        Modelica.Blocks.Interfaces.RealOutput f "Flow output" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {58, 0}, extent = {{-8, -8}, {8, 8}})));
-    equation
-        // Ideal flow sensor in bond-graph form: zero effort loading.
-        p.e = 0;
-        f = p.f;
-        annotation(
-        Icon(graphics = {Text(origin = {-10, 0}, extent = {{-50, 60}, {50, -60}}, textString = "f", textStyle = {TextStyle.Italic}), Ellipse(origin = {-2, 0}, lineThickness = 5, extent = {{-50, 50}, {50, -50}})}));
-    end fsensor;
-
-    model esensor
-        BondPort p annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-52, 0}, extent = {{-10, -10}, {10, 10}})));
-        Modelica.Blocks.Interfaces.RealOutput e "Effort output" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {58, 0}, extent = {{-8, -8}, {8, 8}})));
-    equation
-        // Ideal effort sensor in bond-graph form: zero flow loading.
-        p.f = 0;
-        e = p.e;
-        annotation(
-        Icon(graphics = {Text(origin = {-10, 0}, extent = {{-50, 60}, {50, -60}}, textString = "e", textStyle = {TextStyle.Italic}), Ellipse(origin = {-2, 0}, lineThickness = 5, extent = {{-50, 50}, {50, -50}})}),
-    Diagram(graphics));
-    end esensor;
-
-    annotation(
-    Icon(graphics = {Text(origin = {50, 0}, extent = {{-50, 100}, {50, -100}}, textString = "R"), Line(origin = {-45.22, 20.19}, points = {{-58.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5)}),
-    uses(Modelica(version = "4.1.0")));
-end _1D;
\ No newline at end of file
diff --git a/BondGraph/package.mo b/BondGraph/package.mo
index c3f3235..58d62f2 100644
--- a/BondGraph/package.mo
+++ b/BondGraph/package.mo
@@ -1,10 +1,224 @@
 within ;
 package BondGraph
     extends Modelica.Icons.Package;
-    
+
+    connector BondPort "Bond graph power port"
+        Real e "Effort variable";
+        flow Real f "Flow variable";
+        annotation(
+        Icon(graphics = {Rectangle(lineColor = {0, 0, 127}, fillColor = {0, 0, 127},fillPattern = FillPattern.Solid, extent = {{-60, 60}, {60, -60}})}));
+    end BondPort;
+
+    model J1 "Bond graph 1-junction (common flow, efforts sum to zero)"
+        parameter Integer            N(min=1) = 2         "# of power ports";
+        parameter Real                s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
+
+        BondPort                    P[N]                    "Power ports"
+        annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+
+        Modelica.Blocks.Interfaces.RealOutput f            "Flow in junction"
+        annotation(
+        Placement(transformation(origin = {-100, -6}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {60, -60}, extent = {{-10, -10}, {10, 10}})));
+    equation
+        // Efforts sum to zero, with signs from bond directions
+        sum(s[i] * P[i].e for i in 1:N) = 0;
+
+        // Flows are all equal
+        for i in 2:N loop
+            P[i].f = P[i-1].f;
+        end for;
+
+        f = P[1].f * s[1];
+
+        annotation(
+        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end J1;
+
+    model J0 "Bond graph 1-junction (common effort, flows sum to zero)"
+        parameter Integer            N(min=1) = 2            "# of power ports";
+        parameter Real                s[N] = fill(1.0, N)    "Bond orientation signs used in the effort balance";
+
+        BondPort                    P[N]                    "Power ports"
+        annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+
+        Modelica.Blocks.Interfaces.RealOutput e            "Flow in junction"
+        annotation(
+        Placement(transformation(origin = {-100, -6}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {60, -60}, extent = {{-10, -10}, {10, 10}})));
+    equation
+        // Flows sum to zero, with signs from bond directions
+        sum(s[i] * P[i].f for i in 1:N) = 0;
+
+        // Efforts are all equal
+        for i in 2:N loop
+            P[i].f = P[i-1].f;
+        end for;
+
+        e = P[1].e * s[1];
+
+        annotation(
+        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "0"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end J0;
+
+    partial model OnePortPassive "One-port passive bond graph element"
+        BondPort p "Generic power port" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+    end OnePortPassive;
+
+    partial model OnePortEnergetic "One-port storage element"
+        extends OnePortPassive annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {0, 80}, extent = {{-10, -10}, {10, 10}})));
+        Real state "Conserved quantity";
+    end OnePortEnergetic;
+
+    model C "Bond graph C element"
+        extends OnePortEnergetic(state(start=q0, fixed=true));
+        parameter Real c(min=0) = 1 "Capacitance";
+        parameter Real q0 = 0 "Initial stored quantity (charge)";
+    equation
+        der(state) = p.f;
+        p.e = state / c;
+        annotation(
+        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "C"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end C;
+
+    model I "Bond graph I element"
+        extends OnePortEnergetic(state(start=p0, fixed=true));
+        parameter Real I(min=0) = 1 "Inertance / inductance / mass";
+        parameter Real p0 = 0 "Initial stored quantity (momentum / flux)";
+    equation
+        der(state) = p.e;
+        p.f = state / I;
+        annotation(
+        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "I"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end I;
+
+    model R "Bond graph resistor"
+        extends OnePortPassive;
+        parameter Real R(min=0) = 1 "Resistance";
+    equation
+        p.e = R * p.f;
+        annotation(
+        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "R"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end R;
+
+    model Se "Effort source"
+        BondPort p annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        parameter Real e0 "Imposed effort";
+    equation
+        p.e = e0;
+        annotation(
+        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Se"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end Se;
+
+    model Sf "Flow source"
+        BondPort p annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        parameter Real f0 "Imposed flow";
+    equation
+        p.f = f0;
+        annotation(
+        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Sf"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end Sf;
+
+    model TF "Bond graph transformer"
+        BondPort p1 "Port 1" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+        BondPort p2 "Port 2" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+
+        parameter Real n = 1 "Transformer ratio";
+    equation
+        p1.e = n * p2.e;
+        p2.f = n * p1.f;
+        annotation(
+        Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "TF"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end TF;
+
+    model GY "Bond graph gyrator"
+        BondPort p1 "Port 1" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+        BondPort p2 "Port 2" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        parameter Real r = 1 "Gyrator modulus";
+    equation
+        p1.e = r * p2.f;
+        p2.e = r * p1.f;
+        annotation(
+        Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "GY"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end GY;
+
+    model mSe "Bond graph modulated effort source"
+        BondPort p annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Interfaces.RealInput e0 "Imposed effort" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+
+    equation
+        p.e = e0;
+        annotation(
+        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSe"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end mSe;
+
+    model mSf "Bond graph modulated flow source"
+        BondPort p annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Interfaces.RealInput f0 "Imposed flow" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+
+    equation
+        p.f = f0;
+        annotation(
+        Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSf"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end mSf;
+
+    model mTF "Bond graph modulated transformer"
+        BondPort p1 "Port 1" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+        BondPort p2 "Port 2" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Interfaces.RealInput m "Modulation" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    equation
+        p1.e = m * p2.e;
+        p2.f = m * p1.f;
+        annotation(
+        Diagram(graphics),
+        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mTF"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}));
+    end mTF;
+
+    model mGY "Bond graph modulated gyrator"
+        BondPort p1 "Port 1" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+        BondPort p2 "Port 2" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Interfaces.RealInput m "Modulation" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    equation
+        p1.e = m * p2.f;
+        p2.e = m * p1.f;
+        annotation(
+        Diagram(graphics),
+        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mGY"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}));
+    end mGY;
+
 
 
     annotation(
     Diagram(graphics),
-    Icon(graphics = {Line(origin = {-45.22, 20.19}, points = {{-50.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {156.78, 20.19}, points = {{-60.7774, -20.1934}, {-134.777, -20.1934}, {-76.7774, 19.8066}}, thickness = 5), Line(origin = {28.19, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {66, 12}, points = {{-26, 0}, {30, 0}}, thickness = 5)}));
-end BondGraph;
\ No newline at end of file
+    Icon(graphics = {Line(origin = {-45.22, 20.19}, points = {{-50.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {156.78, 20.19}, points = {{-60.7774, -20.1934}, {-134.777, -20.1934}, {-76.7774, 19.8066}}, thickness = 5), Line(origin = {28.19, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {66, 12}, points = {{-26, 0}, {30, 0}}, thickness = 5)}),
+    uses(Modelica(version = "4.1.0")));
+end BondGraph;
diff --git a/BondGraph/package.order b/BondGraph/package.order
index 2ad5d50..abc4b4a 100644
--- a/BondGraph/package.order
+++ b/BondGraph/package.order
@@ -1,2 +1 @@
 _1D
-_2D
diff --git a/BondGraph/_2D/package.mo b/_2D/package.mo
similarity index 100%
rename from BondGraph/_2D/package.mo
rename to _2D/package.mo
diff --git a/BondGraph/_2D/package.order b/_2D/package.order
similarity index 100%
rename from BondGraph/_2D/package.order
rename to _2D/package.order
diff --git a/_3D/package.mo b/_3D/package.mo
new file mode 100644
index 0000000..6b24c73
--- /dev/null
+++ b/_3D/package.mo
@@ -0,0 +1,275 @@
+within BondGraph;
+
+package _3D
+  connector BondPort "Bond graph 3D multibond power port"
+    Real e[3] "Effort vector";
+    flow Real f[3] "Flow vector";
+    annotation(
+      Icon(graphics = {Rectangle(lineColor = {170, 0, 0}, fillColor = {170, 0, 0}, fillPattern = FillPattern.Solid, extent = {{-60, 60}, {60, -60}})}));
+  end BondPort;
+
+  model J1 "Bond graph 3D 1-junction (common flow, efforts sum to zero)"
+    parameter Integer N(min = 1) = 2 "# of power ports";
+    parameter Real s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
+    BondPort P[N] "Power ports" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+    Real f[3];
+  equation
+// Efforts sum to zero, with signs from bond directions
+    for j in 1:3 loop
+      sum(s[i]*P[i].e[j] for i in 1:N) = 0;
+    end for;
+// Flows are all equal
+    for i in 2:N loop
+      P[i].f = P[i - 1].f;
+    end for;
+    f = P[1].f;
+    annotation(
+      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
+  end J1;
+
+  model J0 "Bond graph 3D 0-junction (common effort, flows sum to zero)"
+    parameter Integer N(min = 1) = 2 "# of power ports";
+    parameter Real s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
+    BondPort P[N] "Power ports" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+    Real e[3];
+  equation
+// Flows sum to zero, with signs from bond directions
+    for j in 1:3 loop
+      sum(s[i]*P[i].f[j] for i in 1:N) = 0;
+    end for;
+// Efforts are all equal
+    for i in 2:N loop
+      P[i].e = P[1].e;
+    end for;
+    e = P[1].e;
+    annotation(
+      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "0", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end J0;
+
+  partial model OnePortPassive "One-port passive 3D multibond element"
+    BondPort p "Generic power port" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+  end OnePortPassive;
+
+  partial model OnePortEnergetic "One-port 3D multibond storage element"
+    extends OnePortPassive annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {0, 80}, extent = {{-10, -10}, {10, 10}})));
+    Real state[3] "Conserved quantity";
+  end OnePortEnergetic;
+
+  model C "Bond graph 3D C element"
+    extends OnePortEnergetic(state(start = q0, each fixed = true));
+    parameter Real c[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Capacitance matrix inverse denominator form";
+    parameter Real q0[3] = {0, 0, 0} "Initial stored quantity (charge)";
+  equation
+    der(state) = p.f;
+    c*p.e = state;
+    annotation(
+      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "C", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end C;
+
+  model I "Bond graph 3D I element"
+    extends OnePortEnergetic(state(start = p0, each fixed = true));
+    parameter Real I[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Inertance / inductance / mass matrix";
+    parameter Real p0[3] = {0, 0, 0} "Initial stored quantity (momentum / flux)";
+  equation
+    der(state) = p.e;
+    I*p.f = state;
+    annotation(
+      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "I", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end I;
+
+  model R "Bond graph 3D resistor"
+    extends OnePortPassive;
+    parameter Real R[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Resistance matrix";
+  equation
+    p.e = R*p.f;
+    annotation(
+      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "R", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end R;
+
+  model Se "Effort source"
+    BondPort p annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    parameter Real e0[3] "Imposed effort";
+  equation
+    p.e = e0;
+    annotation(
+      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Se", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end Se;
+
+  model Sf "Flow source"
+    BondPort p annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    parameter Real f0[3] "Imposed flow";
+  equation
+    p.f = f0;
+    annotation(
+      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Sf", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end Sf;
+
+  model TF "Bond graph 3D transformer"
+    BondPort p1 "Port 1" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+    BondPort p2 "Port 2" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    parameter Real n[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Transformer ratio matrix";
+  equation
+    p1.e = n*p2.e;
+    transpose(n)*p1.f = p2.f;
+    annotation(
+      Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "TF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end TF;
+
+  model GY "Bond graph 3D gyrator"
+    BondPort p1 "Port 1" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+    BondPort p2 "Port 2" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    parameter Real r[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Gyrator modulus matrix";
+  equation
+    p1.e = r*p2.f;
+    p2.e = transpose(r)*p1.f;
+    annotation(
+      Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "GY", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end GY;
+
+  model mSe "Bond graph modulated effort source"
+    BondPort p annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    Modelica.Blocks.Interfaces.RealInput e0 "Imposed effort" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    Modelica.Blocks.Interfaces.RealInput e1 "Imposed effort" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    Modelica.Blocks.Interfaces.RealInput e2 "Imposed effort" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+  equation
+    p.e = {e0, e1, e2};
+    annotation(
+      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSe", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end mSe;
+
+  model mSf "Bond graph modulated flow source"
+    BondPort p annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    Modelica.Blocks.Interfaces.RealInput f0 "Imposed flow" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    Modelica.Blocks.Interfaces.RealInput f1 "Imposed flow" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    Modelica.Blocks.Interfaces.RealInput f2 "Imposed flow" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+  equation
+    p.f = {f0, f1, f2};
+    annotation(
+      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSf", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
+      Diagram(graphics));
+  end mSf;
+
+  model mTF "Bond graph modulated transformer"
+    BondPort p1 "Port 1" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+    BondPort p2 "Port 2" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    Modelica.Blocks.Interfaces.RealInput m[3, 3] "Modulation" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+  equation
+    p1.e = m*p2.e;
+    transpose(m)*p1.f = p2.f;
+    annotation(
+      Diagram(graphics),
+      Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
+  end mTF;
+
+  model mGY "Bond graph modulated gyrator"
+    BondPort p1 "Port 1" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+    BondPort p2 "Port 2" annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+    Modelica.Blocks.Interfaces.RealInput m[3, 3] "Modulation" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+  equation
+    p1.e = m*p2.f;
+    p2.e = transpose(m)*p1.f;
+    annotation(
+      Diagram(graphics),
+      Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mGY", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
+  end mGY;
+
+  package TransRotUtils
+    model mTFrot3lin
+      BondPort pR annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+      BondPort pT annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+      parameter Real r_body[3] = {1, 0, 0} "Vector from rotational reference to translational point (body frame)";
+    protected
+      Real S[3, 3];
+    equation
+// Skew matrix such that S*x = r_body x x
+      S = [0, -r_body[3], r_body[2]; r_body[3], 0, -r_body[1]; -r_body[2], r_body[1], 0];
+// v = w x r_body = -S*w
+      pT.f = transpose(S)*pR.f;
+// tau = r_body x F = S*F
+      pR.e = S*pT.e;
+      annotation(
+        Icon(graphics = {Text(extent = {{-70, 100}, {70, -100}}, textString = "rlTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic})}));
+    end mTFrot3lin;
+
+    model rTF3D
+      BondPort p1 "Port 1" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+      BondPort p2 "Port 2" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+      Modelica.Blocks.Interfaces.RealInput phi "roll angle" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+      Modelica.Blocks.Interfaces.RealInput theta "pitch angle" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+      Modelica.Blocks.Interfaces.RealInput psi "yaw angle" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    protected
+      Real R[3, 3];
+    equation
+// ZYX rotation matrix from body to inertial frame.
+      R = [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi) - sin(psi)*cos(phi), cos(psi)*sin(theta)*cos(phi) + sin(psi)*sin(phi); sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi) + cos(psi)*cos(phi), sin(psi)*sin(theta)*cos(phi) - cos(psi)*sin(phi); -sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi)];
+      p1.e = R*p2.e;
+      transpose(R)*p1.f = p2.f;
+      annotation(
+        Diagram(graphics),
+        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "rTF", textStyle = {TextStyle.Bold, TextStyle.Italic, TextStyle.UnderLine})}));
+    end rTF3D;
+  end TransRotUtils;
+
+  model fsensor3d
+    BondPort p annotation(
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-52, 0}, extent = {{-10, -10}, {10, 10}})));
+    Modelica.Blocks.Interfaces.RealOutput f0 "Flow output" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, 26}, extent = {{-8, -8}, {8, 8}})));
+    Modelica.Blocks.Interfaces.RealOutput f1 "Flow output" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, 0}, extent = {{-8, -8}, {8, 8}})));
+    Modelica.Blocks.Interfaces.RealOutput f2 "Flow output" annotation(
+      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, -26}, extent = {{-8, -8}, {8, 8}})));
+  equation
+// Ideal flow sensor in bond-graph form: zero effort loading.
+    p.e = {0, 0, 0};
+    f0 = p.f[1];
+    f1 = p.f[2];
+    f2 = p.f[3];
+    annotation(
+      Icon(graphics = {Text(origin = {-10, 0}, extent = {{-50, 60}, {50, -60}}, textString = "f", textStyle = {TextStyle.Italic}), Ellipse(origin = {-2, 0}, lineThickness = 5, extent = {{-50, 50}, {50, -50}})}));
+  end fsensor3d;
+  annotation(
+    Icon(graphics = {Text(origin = {50, 0}, extent = {{-50, 100}, {50, -100}}, textString = "R", textStyle = {TextStyle.Bold, TextStyle.Italic}), Line(origin = {-45.22, 20.19}, points = {{-58.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {-78, 16}, points = {{-26, 0}, {30, 0}}, thickness = 5), Line(origin = {-78, -16}, points = {{-26, 0}, {30, 0}}, thickness = 5)}),
+    uses(Modelica(version = "4.1.0")),
+    Diagram(graphics));
+end _3D;
\ No newline at end of file
diff --git a/BondGraph/_1D/package.order b/_3D/package.order
similarity index 77%
rename from BondGraph/_1D/package.order
rename to _3D/package.order
index d6374e3..4e0fc4f 100644
--- a/BondGraph/_1D/package.order
+++ b/_3D/package.order
@@ -14,5 +14,5 @@ mSe
 mSf
 mTF
 mGY
-fsensor
-esensor
+TransRotUtils
+fsensor3d
diff --git a/formatter.py b/formatter.py
index 4bcdf23..14d3244 100755
--- a/formatter.py
+++ b/formatter.py
@@ -24,7 +24,9 @@ END_RE = re.compile(r"^\s*end\b")
 
 
 def format_modelica(text: str) -> str:
-    lines = text.replace("\t", "    ").splitlines()
+    normalized = text.replace("\r\n", "\n").replace("\r", "\n").replace("\t", "    ")
+    had_trailing_newline = normalized.endswith("\n")
+    lines = normalized.split("\n")
     out = []
     indent = 0
 
@@ -63,7 +65,10 @@ def format_modelica(text: str) -> str:
         elif SECTION_RE.match(stripped):
             indent += 1
 
-    return "\n".join(out) + "\n"
+    formatted = "\n".join(out)
+    if had_trailing_newline:
+        return formatted
+    return formatted + "\n"
 
 
 def main():
diff --git a/test1d.mo b/test1d.mo
new file mode 100644
index 0000000..83a342c
--- /dev/null
+++ b/test1d.mo
@@ -0,0 +1,17 @@
+model test1d
+  BondGraph.J1 j1(N = 3, s = {-1, -1, 1})  annotation(
+    Placement(transformation(extent = {{-10, -10}, {10, 10}})));
+  BondGraph.R r1(R = 1)  annotation(
+    Placement(transformation(origin = {0, 40}, extent = {{-10, -10}, {10, 10}})));
+  BondGraph.R r2(R = 1)  annotation(
+    Placement(transformation(origin = {0, -40}, extent = {{-10, -10}, {10, 10}})));
+  BondGraph.Se se(e0 = 1)  annotation(
+    Placement(transformation(origin = {-40, 0}, extent = {{-10, -10}, {10, 10}})));
+equation
+  connect(j1.P[1], r1.p) annotation(
+    Line(points = {{0, 0}, {0, 40}}, color = {0, 0, 127}));
+  connect(j1.P[2], r2.p) annotation(
+    Line(points = {{0, 0}, {0, -40}}, color = {0, 0, 127}));
+  connect(se.p, j1.P[3]) annotation(
+    Line(points = {{-32, 0}, {0, 0}}, color = {0, 0, 127}));
+end test1d;
\ No newline at end of file
diff --git a/test2d.mo b/test2d.mo
index a98b66e..195e88c 100644
--- a/test2d.mo
+++ b/test2d.mo
@@ -1,94 +1,99 @@
 model test2d
-  import _1D = BondGraph._1D;
-  import _2D = BondGraph._2D;
-  _1D.J1 w_com(N = 4, s = {1, -1, -1, -1}) annotation(
+    import _1D = BondGraph._1D;
+    import _2D = BondGraph._2D;
+    _1D.J1 w_com(N = 4, s = {1, -1, -1, -1}) annotation(
     Placement(transformation(origin = {0, 120}, extent = {{-10, -10}, {10, 10}})));
-  _2D.TransRotUtils.mTFrot2lin mTFrot2lin(r_body = {0.5, 0}) annotation(
+    _2D.TransRotUtils.mTFrot2lin mTFrot2lin(r_body = {0.5, 0}) annotation(
     Placement(transformation(origin = {0, 80}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
-  _2D.J0 j0(N = 3, s = {1, 1, -1}) annotation(
+    _2D.J0 j0(N = 3, s = {1, 1, -1}) annotation(
     Placement(transformation(origin = {0, 40}, extent = {{-10, -10}, {10, 10}})));
-  _2D.J1 v_p_bff(N = 2, s = {1, 1}) annotation(
+    _2D.J1 v_p_bff(N = 2, s = {1, 1}) annotation(
     Placement(transformation(origin = {-40, 40}, extent = {{-10, -10}, {10, 10}})));
-  _2D.J1 v_com_bff(N = 2, s = {-1, -1}) annotation(
+    _2D.J1 v_com_bff(N = 2, s = {-1, -1}) annotation(
     Placement(transformation(origin = {40, 40}, extent = {{-10, -10}, {10, 10}})));
-  _2D.TransRotUtils.rTF rTF annotation(
+    _2D.TransRotUtils.rTF rTF annotation(
     Placement(transformation(origin = {40, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 90)));
-  _1D.fsensor wsensor annotation(
+    _1D.fsensor wsensor annotation(
     Placement(transformation(origin = {50, 90}, extent = {{-10, -10}, {10, 10}})));
-  Modelica.Blocks.Continuous.Integrator w2phi(k = 1) annotation(
+    Modelica.Blocks.Continuous.Integrator w2phi(k = 1) annotation(
     Placement(transformation(origin = {90, 90}, extent = {{-10, -10}, {10, 10}})));
-  _2D.J1 v_com_in(N = 4, s = {1, -1, -1, 1}) annotation(
+    _2D.J1 v_com_in(N = 4, s = {1, -1, 1, 1}) annotation(
     Placement(transformation(origin = {40, -40}, extent = {{-10, -10}, {10, 10}})));
-  _2D.I mass(I = [1, 0; 0, 1]) annotation(
+    _2D.I mass(I = [1, 0; 0, 1]) annotation(
     Placement(transformation(origin = {80, -40}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._2D.Sf ground(f0 = {0, 0}) annotation(
+    BondGraph._2D.Sf ground(f0 = {0, 0}) annotation(
     Placement(transformation(origin = {-80, 40}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._1D.I m_inertial(I = 0.001) annotation(
+    BondGraph._1D.I m_inertial(I = 0.001) annotation(
     Placement(transformation(origin = {40, 120}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._1D.Sf ground2(f0 = 0) annotation(
+    BondGraph._1D.Sf ground2(f0 = 0) annotation(
     Placement(transformation(origin = {-80, 120}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._1D.J0 j1(N = 3, s = {1, -1, -1}) annotation(
+    BondGraph._1D.J0 j1(N = 3, s = {1, -1, -1}) annotation(
     Placement(transformation(origin = {-40, 120}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._1D.J1 w_hinge(N = 2, s = {1, -11}) annotation(
+    BondGraph._1D.J1 w_hinge(N = 2, s = {1, -1}) annotation(
     Placement(transformation(origin = {-40, 90}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._1D.R hinge_r(R = 0.01) annotation(
+    BondGraph._1D.R hinge_r(R = 0.08) annotation(
     Placement(transformation(origin = {-80, 90}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._2D.Se g(e0 = {0, 9.81}) annotation(
+    BondGraph._2D.Se g(e0 = {0, -9.81}) annotation(
     Placement(transformation(origin = {2, -40}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph._2D.fsensor2d v_inertial annotation(
+    BondGraph._2D.fsensor2d v_inertial annotation(
     Placement(transformation(origin = {40, -60}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
-  Modelica.Blocks.Continuous.Integrator vx_to_x(k = -1, y_start = -0.5)  annotation(
+    Modelica.Blocks.Continuous.Integrator vx_to_x(k = 1, y_start = -0.5)  annotation(
     Placement(transformation(origin = {70, -70}, extent = {{-10, -10}, {10, 10}})));
-  Modelica.Blocks.Continuous.Integrator vy_to_y annotation(
+    Modelica.Blocks.Continuous.Integrator vy_to_y(k = 1)  annotation(
     Placement(transformation(origin = {70, -100}, extent = {{-10, -10}, {10, 10}})));
-  vis2d link;
+    vis2d vis2d1 annotation(
+    Placement(transformation(origin = {150, -6}, extent = {{-10, -10}, {10, 10}})));
 equation
-  connect(w_com.P[2], mTFrot2lin.pR) annotation(
+    connect(w_com.P[2], mTFrot2lin.pR) annotation(
     Line(points = {{0, 120}, {0, 88}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(mTFrot2lin.pT, j0.P[1]) annotation(
+    connect(mTFrot2lin.pT, j0.P[1]) annotation(
     Line(points = {{0, 72}, {0, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(v_com_bff.P[1], j0.P[2]) annotation(
+    connect(v_com_bff.P[1], j0.P[2]) annotation(
     Line(points = {{40, 40}, {0, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(j0.P[3], v_p_bff.P[1]) annotation(
+    connect(j0.P[3], v_p_bff.P[1]) annotation(
     Line(origin = {-1, 0}, points = {{0, 40}, {-40, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(wsensor.f, w2phi.u) annotation(
+    connect(wsensor.f, w2phi.u) annotation(
     Line(points = {{58, 90}, {78, 90}}, color = {0, 0, 127}));
-  connect(w2phi.y, rTF.phi) annotation(
+    connect(w2phi.y, rTF.phi) annotation(
     Line(points = {{102, 90}, {110, 90}, {110, 0}, {48, 0}}, color = {0, 0, 127}, arrow = {Arrow.None, Arrow.Filled}));
-  connect(v_com_in.P[2], mass.p) annotation(
+    connect(v_com_in.P[2], mass.p) annotation(
     Line(points = {{40, -40}, {80, -40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(w_com.P[3], wsensor.p) annotation(
-    Line(points = {{0, 120}, {44, 90}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(w_com.P[1], m_inertial.p) annotation(
+    connect(w_com.P[3], wsensor.p) annotation(
+    Line(points = {{0, 120}, {44, 90}}, color = {0, 0, 127}, pattern = LinePattern.Dash, thickness = 0.75, arrow = {Arrow.None, Arrow.Open}, arrowSize = 6));
+    connect(w_com.P[1], m_inertial.p) annotation(
     Line(points = {{0, 120}, {40, 120}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(ground2.p, j1.P[1]) annotation(
+    connect(ground2.p, j1.P[1]) annotation(
     Line(points = {{-72, 120}, {-40, 120}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(j1.P[2], w_com.P[4]) annotation(
+    connect(j1.P[2], w_com.P[4]) annotation(
     Line(points = {{-40, 120}, {0, 120}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.Half, Arrow.None}, arrowSize = 6));
-  connect(ground.p, v_p_bff.P[2]) annotation(
+    connect(ground.p, v_p_bff.P[2]) annotation(
     Line(points = {{-72, 40}, {-40, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(j1.P[3], w_hinge.P[1]) annotation(
+    connect(j1.P[3], w_hinge.P[1]) annotation(
     Line(points = {{-40, 120}, {-40, 90}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(w_hinge.P[2], hinge_r.p) annotation(
+    connect(w_hinge.P[2], hinge_r.p) annotation(
     Line(points = {{-40, 90}, {-80, 90}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(g.p, v_com_in.P[3]) annotation(
+    connect(g.p, v_com_in.P[3]) annotation(
     Line(points = {{10, -40}, {40, -40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(v_com_bff.P[2], rTF.p2) annotation(
+    connect(v_com_bff.P[2], rTF.p2) annotation(
     Line(points = {{40, 40}, {40, 8}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(rTF.p1, v_com_in.P[1]) annotation(
+    connect(rTF.p1, v_com_in.P[1]) annotation(
     Line(points = {{40, -8}, {40, -40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(v_com_in.P[4], v_inertial.p) annotation(
-    Line(points = {{40, -40}, {40, -54}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
-  connect(v_inertial.f0, vx_to_x.u) annotation(
+    connect(v_com_in.P[4], v_inertial.p) annotation(
+    Line(points = {{40, -40}, {40, -54}}, color = {0, 85, 0}, pattern = LinePattern.Dash, thickness = 0.75, arrow = {Arrow.None, Arrow.Open}, arrowSize = 6));
+    connect(v_inertial.f0, vx_to_x.u) annotation(
     Line(points = {{42, -66}, {42, -70}, {58, -70}}, color = {0, 0, 127}));
-  connect(v_inertial.f1, vy_to_y.u) annotation(
+    connect(v_inertial.f1, vy_to_y.u) annotation(
     Line(points = {{38, -66}, {38, -100}, {58, -100}}, color = {0, 0, 127}));
-  link.r = {vx_to_x.y, vy_to_y.y, 0};
-  link.phi = w2phi.y;
-  annotation(
+    connect(w2phi.y, vis2d1.phi) annotation(
+    Line(points = {{102, 90}, {110, 90}, {110, 0}, {141, 0}}, color = {0, 0, 127}));
+    connect(vx_to_x.y, vis2d1.x) annotation(
+    Line(points = {{82, -70}, {110, -70}, {110, -6}, {141, -6}}, color = {0, 0, 127}));
+    connect(vy_to_y.y, vis2d1.y) annotation(
+    Line(points = {{82, -100}, {118, -100}, {118, -12}, {141, -12}}, color = {0, 0, 127}));
+    annotation(
     uses(Modelica(version = "4.1.0")),
     experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02),
-    Diagram(coordinateSystem(extent = {{-100, 140}, {120, -120}})),
+    Diagram(coordinateSystem(extent = {{-100, 140}, {160, -120}})),
     Icon(coordinateSystem(extent = {{-200, -200}, {200, 200}})),
     version = "");
 end test2d;
\ No newline at end of file
diff --git a/test3d.mo b/test3d.mo
new file mode 100644
index 0000000..e6cf210
--- /dev/null
+++ b/test3d.mo
@@ -0,0 +1,158 @@
+model test3d
+  import _3D = BondGraph._3D;
+
+  // ---------------------------------------------------------------------------
+  // Parameters
+  // ---------------------------------------------------------------------------
+  parameter Real J[3,3] = [0.05, 0, 0;
+                           0, 0.06, 0;
+                           0, 0, 0.02] "Body inertia tensor in body frame";
+  parameter Real m[3,3] = [1, 0, 0;
+                           0, 1, 0;
+                           0, 0, 1] "Translational inertia (mass matrix)";
+  parameter Real r_body[3] = {0.5, 0, 0} "Vector from hinge point to COM in body frame";
+  parameter Real R_rot[3,3] = [0.02, 0, 0;
+                               0, 0.02, 0;
+                               0, 0, 0.02] "Rotational damping matrix";
+  parameter Real g = 9.81 "Gravity magnitude";
+
+  // ---------------------------------------------------------------------------
+  // Rotational dynamics (body frame)
+  // ---------------------------------------------------------------------------
+  _3D.J1 w_com(N = 6, s = {1, -1, -1, -1, -1, -1}) "Common angular velocity junction";
+  _3D.I J_inertial(I = J) "Angular momentum storage";
+  _3D.R hinge_r(R = R_rot) "Rotational damping";
+  _3D.fsensor3d wsensor "Measures body angular velocity omega";
+
+  // Gyroscopic term implementation (power-conserving structure):
+  // - TF maps omega -> h = J*omega (angular momentum)
+  // - mGY with modulation S(omega) generates tau_g = omega x h
+  _3D.TF Hmap(n = J) "Maps between omega and angular-momentum port";
+  _3D.mGY gyro "Modulated gyrator for omega x h coupling";
+
+  // ---------------------------------------------------------------------------
+  // Kinematics and frame transforms
+  // ---------------------------------------------------------------------------
+  _3D.TransRotUtils.mTFrot3lin mTFrot3lin(r_body = r_body) "v = omega x r_body coupling";
+  _3D.J0 j0(N = 3, s = {1, 1, -1}) "Velocity composition at COM in body frame";
+  _3D.J1 v_p_bff(N = 2, s = {1, 1}) "Hinge-point velocity (body frame)";
+  _3D.J1 v_com_bff(N = 2, s = {-1, -1}) "COM velocity (body frame)";
+  _3D.TransRotUtils.rTF3D rTF3D "Body <-> inertial frame transform";
+
+  // ---------------------------------------------------------------------------
+  // Translational dynamics (inertial frame)
+  // ---------------------------------------------------------------------------
+  _3D.J1 v_com_in(N = 4, s = {1, -1, -1, 1}) "COM velocity in inertial frame";
+  _3D.I mass(I = m) "Translational momentum storage";
+  _3D.Sf ground(f0 = {0, 0, 0}) "Ground translational velocity";
+  _3D.mSe gravity "Gravity effort source";
+  _3D.fsensor3d v_inertial "Measures inertial COM velocity";
+
+  // ---------------------------------------------------------------------------
+  // States and signals
+  // ---------------------------------------------------------------------------
+  Real omega[3] "Body angular velocity";
+  Real h[3] "Body angular momentum J*omega";
+  Real S_omega[3,3] "Skew matrix of omega";
+
+  Real phi(start = 0) "Roll";
+  Real theta(start = 0.1) "Pitch";
+  Real psi(start = 0) "Yaw";
+  Real euler_dot[3] "{phi_dot, theta_dot, psi_dot}";
+
+  Real x(start = -0.5) "COM x (inertial)";
+  Real y(start = 0) "COM y (inertial)";
+  Real z(start = -0.2) "COM z (inertial)";
+
+  Modelica.Blocks.Sources.RealExpression xSig(y = x);
+  Modelica.Blocks.Sources.RealExpression ySig(y = y);
+  Modelica.Blocks.Sources.RealExpression zSig(y = z);
+  Modelica.Blocks.Sources.RealExpression phiSig(y = phi);
+  Modelica.Blocks.Sources.RealExpression thetaSig(y = theta);
+  Modelica.Blocks.Sources.RealExpression psiSig(y = psi);
+  vis3d vis3d1;
+
+equation
+  // ---------------------------------------------------------------------------
+  // Rotational network connections
+  // ---------------------------------------------------------------------------
+  connect(w_com.P[1], J_inertial.p);
+  connect(w_com.P[2], mTFrot3lin.pR);
+  connect(w_com.P[3], wsensor.p);
+  connect(w_com.P[4], hinge_r.p);
+  connect(w_com.P[5], gyro.p1);
+  connect(w_com.P[6], Hmap.p1);
+  connect(Hmap.p2, gyro.p2);
+
+  // ---------------------------------------------------------------------------
+  // Body-frame COM kinematics and frame transform
+  // ---------------------------------------------------------------------------
+  connect(mTFrot3lin.pT, j0.P[1]);
+  connect(v_com_bff.P[1], j0.P[2]);
+  connect(j0.P[3], v_p_bff.P[1]);
+  connect(ground.p, v_p_bff.P[2]);
+
+  connect(v_com_bff.P[2], rTF3D.p2);
+  connect(rTF3D.p1, v_com_in.P[1]);
+
+  // Feed frame-transform angles from state variables.
+  connect(phiSig.y, rTF3D.phi);
+  connect(thetaSig.y, rTF3D.theta);
+  connect(psiSig.y, rTF3D.psi);
+  connect(phiSig.y, vis3d1.phi);
+  connect(thetaSig.y, vis3d1.theta);
+  connect(psiSig.y, vis3d1.psi);
+
+  // ---------------------------------------------------------------------------
+  // Translational dynamics connections
+  // ---------------------------------------------------------------------------
+  connect(v_com_in.P[2], mass.p);
+  connect(gravity.p, v_com_in.P[3]);
+  connect(v_com_in.P[4], v_inertial.p);
+
+  // Gravity as effort in inertial frame.
+  gravity.e0 = 0;
+  gravity.e1 = 0;
+  gravity.e2 = g;
+
+  // ---------------------------------------------------------------------------
+  // Gyroscopic modulation and H-map related equations
+  // ---------------------------------------------------------------------------
+  omega = {wsensor.f0, wsensor.f1, wsensor.f2};
+  h = J * omega;
+
+  // Skew matrix S(omega) so that S(omega)*x = omega x x.
+  S_omega = [0, -omega[3], omega[2];
+             omega[3], 0, -omega[1];
+             -omega[2], omega[1], 0];
+  gyro.m = S_omega;
+
+  // ---------------------------------------------------------------------------
+  // 3D attitude kinematics (explicit H(q) mapping form)
+  // Using ZYX convention with body rates omega -> Euler rates.
+  // Note: singular at cos(theta)=0.
+  // ---------------------------------------------------------------------------
+  euler_dot[1] = omega[1] + sin(phi) * tan(theta) * omega[2] + cos(phi) * tan(theta) * omega[3];
+  euler_dot[2] = cos(phi) * omega[2] - sin(phi) * omega[3];
+  euler_dot[3] = sin(phi) / cos(theta) * omega[2] + cos(phi) / cos(theta) * omega[3];
+
+  der(phi) = euler_dot[1];
+  der(theta) = euler_dot[2];
+  der(psi) = euler_dot[3];
+
+  // ---------------------------------------------------------------------------
+  // Position integration
+  // ---------------------------------------------------------------------------
+  der(x) = v_inertial.f0;
+  der(y) = v_inertial.f1;
+  der(z) = v_inertial.f2;
+
+  connect(xSig.y, vis3d1.x);
+  connect(ySig.y, vis3d1.y);
+  connect(zSig.y, vis3d1.z);
+
+  annotation(
+    uses(Modelica(version = "4.1.0")),
+    experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02),
+    Diagram(coordinateSystem(extent = {{-140, 160}, {200, -160}})));
+end test3d;
diff --git a/vis2d.mo b/vis2d.mo
index f01c5ef..0e7d067 100644
--- a/vis2d.mo
+++ b/vis2d.mo
@@ -5,9 +5,21 @@ model vis2d
   parameter Real L = 1;
   parameter Real w = 0.02;
   parameter Real orbRadius = 0.04;
+  parameter Real axisLength = 0.25;
+  parameter Real axisWidth = 0.01;
+  parameter Real planeSize = 2.0;
+  parameter Real planeThickness = 0.002;
+  parameter Real planeOffset = -0.001;
 
-  input Real r[3];
-  input Real phi;
+  Modelica.Blocks.Interfaces.RealInput phi annotation(
+    Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 60}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Interfaces.RealInput y annotation(
+    Placement(transformation(origin = {-98, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -60}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Interfaces.RealInput x annotation(
+    Placement(transformation(origin = {-98, 40}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 0}, extent = {{-10, -10}, {10, 10}})));
+protected
+  Real Rz[3,3];
+  Real r[3];
 
   Visualizers.Advanced.Shape link(
     shapeType = "cylinder",
@@ -18,11 +30,7 @@ model vis2d
     lengthDirection = {1, 0, 0},
     widthDirection = {0, 1, 0},
     r = r,
-    R = Frames.from_T(
-      [cos(phi), -sin(phi), 0;
-       sin(phi),  cos(phi), 0;
-       0,         0,        1],
-      zeros(3)));
+    R = Frames.from_T(Rz, zeros(3)));
 
   Visualizers.Advanced.Shape orb(
     shapeType = "sphere",
@@ -33,9 +41,59 @@ model vis2d
     lengthDirection = {1, 0, 0},
     widthDirection = {0, 1, 0},
     r = r,
-    R = Frames.from_T(
-      [cos(phi), -sin(phi), 0;
-       sin(phi),  cos(phi), 0;
-       0,         0,        1],
-      zeros(3)));
-end vis2d;
+    R = Frames.from_T(Rz, zeros(3)));
+
+  Visualizers.Advanced.Shape xAxis(
+    shapeType = "cylinder",
+    length = axisLength,
+    width = axisWidth,
+    height = axisWidth,
+    color = {255, 0, 0},
+    specularCoefficient = 0.1,
+    r_shape = {axisLength / 2, 0, 0},
+    lengthDirection = {1, 0, 0},
+    widthDirection = {0, 1, 0},
+    r = zeros(3),
+    R = Frames.from_T([1, 0, 0;
+                       0, 1, 0;
+                       0, 0, 1], zeros(3)));
+
+  Visualizers.Advanced.Shape yAxis(
+    shapeType = "cylinder",
+    length = axisLength,
+    width = axisWidth,
+    height = axisWidth,
+    color = {0, 200, 0},
+    specularCoefficient = 0.1,
+    r_shape = {0, axisLength / 2, 0},
+    lengthDirection = {0, 1, 0},
+    widthDirection = {0, 0, 1},
+    r = zeros(3),
+    R = Frames.from_T([1, 0, 0;
+                       0, 1, 0;
+                       0, 0, 1], zeros(3)));
+
+  Visualizers.Advanced.Shape zAxis(
+    shapeType = "cylinder",
+    length = axisLength,
+    width = axisWidth,
+    height = axisWidth,
+    color = {0, 90, 255},
+    specularCoefficient = 0.1,
+    r_shape = {0, 0, axisLength / 2},
+    lengthDirection = {0, 0, 1},
+    widthDirection = {1, 0, 0},
+    r = zeros(3),
+    R = Frames.from_T([1, 0, 0;
+                       0, 1, 0;
+                       0, 0, 1], zeros(3)));
+equation
+  r = {x, y, 0};
+  Rz = [cos(phi), -sin(phi), 0;
+        sin(phi),  cos(phi), 0;
+        0,         0,        1];
+      
+  annotation(
+    uses(Modelica(version = "4.1.0")),
+  Icon(graphics = {Rectangle(lineColor = {204, 204, 204}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, extent = {{-100, 100}, {100, -100}}, radius = 10), Text(extent = {{-100, 100}, {100, -100}}, textString = "VIS")}));
+end vis2d;
\ No newline at end of file
diff --git a/vis3d.mo b/vis3d.mo
new file mode 100644
index 0000000..4594599
--- /dev/null
+++ b/vis3d.mo
@@ -0,0 +1,103 @@
+model vis3d
+  import Modelica.Mechanics.MultiBody.Visualizers;
+  import Modelica.Mechanics.MultiBody.Frames;
+
+  parameter Real L = 1;
+  parameter Real w = 0.02;
+  parameter Real orbRadius = 0.04;
+  parameter Real axisLength = 0.25;
+  parameter Real axisWidth = 0.01;
+
+  Modelica.Blocks.Interfaces.RealInput psi annotation(
+    Placement(transformation(origin = {-98, -70}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 20}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Interfaces.RealInput theta annotation(
+    Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 50}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Interfaces.RealInput phi annotation(
+    Placement(transformation(origin = {-98, -14}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 80}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Interfaces.RealInput z annotation(
+    Placement(transformation(origin = {-98, 14}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -80}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Interfaces.RealInput y annotation(
+    Placement(transformation(origin = {-98, 42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -50}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Interfaces.RealInput x annotation(
+    Placement(transformation(origin = {-98, 70}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -20}, extent = {{-10, -10}, {10, 10}})));
+protected
+  Real R[3,3];
+  Real r[3];
+
+  Visualizers.Advanced.Shape link(
+    shapeType = "cylinder",
+    length = L,
+    width = w,
+    height = w,
+    r_shape = {-L / 2, 0, 0},
+    lengthDirection = {1, 0, 0},
+    widthDirection = {0, 1, 0},
+    r = r,
+    R = Frames.from_T(R, zeros(3)));
+
+  Visualizers.Advanced.Shape orb(
+    shapeType = "sphere",
+    length = 2 * orbRadius,
+    width = 2 * orbRadius,
+    height = 2 * orbRadius,
+    r_shape = {L / 2 - orbRadius, 0, 0},
+    lengthDirection = {1, 0, 0},
+    widthDirection = {0, 1, 0},
+    r = r,
+    R = Frames.from_T(R, zeros(3)));
+
+  Visualizers.Advanced.Shape xAxis(
+    shapeType = "cylinder",
+    length = axisLength,
+    width = axisWidth,
+    height = axisWidth,
+    color = {255, 0, 0},
+    specularCoefficient = 0.1,
+    r_shape = {axisLength / 2, 0, 0},
+    lengthDirection = {1, 0, 0},
+    widthDirection = {0, 1, 0},
+    r = zeros(3),
+    R = Frames.from_T([1, 0, 0;
+                       0, 1, 0;
+                       0, 0, 1], zeros(3)));
+
+  Visualizers.Advanced.Shape yAxis(
+    shapeType = "cylinder",
+    length = axisLength,
+    width = axisWidth,
+    height = axisWidth,
+    color = {0, 200, 0},
+    specularCoefficient = 0.1,
+    r_shape = {0, axisLength / 2, 0},
+    lengthDirection = {0, 1, 0},
+    widthDirection = {0, 0, 1},
+    r = zeros(3),
+    R = Frames.from_T([1, 0, 0;
+                       0, 1, 0;
+                       0, 0, 1], zeros(3)));
+
+  Visualizers.Advanced.Shape zAxis(
+    shapeType = "cylinder",
+    length = axisLength,
+    width = axisWidth,
+    height = axisWidth,
+    color = {0, 90, 255},
+    specularCoefficient = 0.1,
+    r_shape = {0, 0, axisLength / 2},
+    lengthDirection = {0, 0, 1},
+    widthDirection = {1, 0, 0},
+    r = zeros(3),
+    R = Frames.from_T([1, 0, 0;
+                       0, 1, 0;
+                       0, 0, 1], zeros(3)));
+
+equation
+  r = {x, y, z};
+  R = [cos(psi) * cos(theta), cos(psi) * sin(theta) * sin(phi) - sin(psi) * cos(phi), cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi);
+       sin(psi) * cos(theta), sin(psi) * sin(theta) * sin(phi) + cos(psi) * cos(phi), sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi);
+       -sin(theta), cos(theta) * sin(phi), cos(theta) * cos(phi)];
+
+  annotation(
+    uses(Modelica(version = "4.1.0")),
+    Icon(graphics = {Rectangle(lineColor = {204, 204, 204}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, extent = {{-100, 100}, {100, -100}}, radius = 10), Text(extent = {{-100, 100}, {100, -100}}, textString = "VIS")}));
+end vis3d;
\ No newline at end of file