function [K, M] = triangle_fem(nodes, family, E, rho, nu, d)
% TRIANGLE_FEM Free-free frame system for one mode family of a bent rod.
%
% The rod centerline is the open polyline `nodes` (2 x nn, metres, in the
% x-y plane), discretized into nn - 1 two-node elements. Because the geometry
% is planar, the six degrees of freedom per node decouple exactly into two
% independent families of three, and each family is assembled separately:
%   family 0 (in-plane):     ux, uy, theta_z   (in-plane bending + stretching)
%   family 1 (out-of-plane): uz, theta_x, theta_y (out-of-plane bending + torsion)
% The bends are where the families earn their keep: within one straight leg
% out-of-plane bending and torsion are independent, but a change of direction
% rotates the (torsion, bending-rotation) pair, so the corner converts one
% into the other. That conversion is what the per-element rotation T applies,
% and it is what makes a triangle sound different from a straight rod.
%
% Elements are Euler-Bernoulli (cubic Hermite bending, linear axial and
% torsion interpolation) with consistent mass including rotary inertia.
% Shear deformation is neglected, which leaves the highest retained modes a
% few percent sharp for a rod this thick; see the README.
%
% Sign conventions: in-plane bending uses theta_z = +dv/dx, giving the
% textbook stiffness block; out-of-plane bending uses theta_y = -dw/dx, so
% the odd coupling terms flip sign (the similarity transform Sf below).
%
% Returns K and M, each 3 nn x 3 nn, symmetric; M is positive definite, so
% chol(M) reduces the generalized eigenproblem to a symmetric standard one.

nn = size(nodes, 2);
ndof = 3 * nn;
K = zeros(ndof);
M = zeros(ndof);

Aa = pi * d^2 / 4;                 % cross-section area
Iz = pi * d^4 / 64;                % bending second moment (circular)
Jp = pi * d^4 / 32;                % polar moment = torsion constant (circular)
Gs = E / (2 * (1 + nu));

for e = 1:nn - 1
  dx = nodes(1, e + 1) - nodes(1, e);
  dy = nodes(2, e + 1) - nodes(2, e);
  l = sqrt(dx^2 + dy^2);
  c = dx / l;
  s = dy / l;

  % bending block on the (transverse, rotation) pairs of both nodes
  Kb = (E * Iz / l^3) * ...
    [12, 6 * l, -12, 6 * l; ...
     6 * l, 4 * l^2, -6 * l, 2 * l^2; ...
     -12, -6 * l, 12, -6 * l; ...
     6 * l, 2 * l^2, -6 * l, 4 * l^2];
  Mb = (rho * Aa * l / 420) * ...
    [156, 22 * l, 54, -13 * l; ...
     22 * l, 4 * l^2, 13 * l, -3 * l^2; ...
     54, 13 * l, 156, -22 * l; ...
     -13 * l, -3 * l^2, -22 * l, 4 * l^2] ...
    + (rho * Iz / (30 * l)) * ...
    [36, 3 * l, -36, 3 * l; ...
     3 * l, 4 * l^2, -3 * l, -l^2; ...
     -36, -3 * l, 36, -3 * l; ...
     3 * l, -l^2, -3 * l, 4 * l^2];

  Ke = zeros(6, 6);
  Me = zeros(6, 6);
  if family == 0
    % local order (u1, v1, t1, u2, v2, t2): axial on u, bending on (v, t)
    Ka2 = (E * Aa / l) * [1, -1; -1, 1];
    Ma2 = (rho * Aa * l / 6) * [2, 1; 1, 2];
    ia = [1 4];
    ib = [2 3 5 6];
    Ke(ia, ia) = Ka2;
    Me(ia, ia) = Ma2;
    Ke(ib, ib) = Kb;
    Me(ib, ib) = Mb;
    T = [c, s, 0; -s, c, 0; 0, 0, 1];
  else
    % local order (w1, tx1, ty1, w2, tx2, ty2): torsion on tx (about the leg
    % axis), bending on (w, ty) with theta_y = -dw/dx
    Kt2 = (Gs * Jp / l) * [1, -1; -1, 1];
    Mt2 = (rho * Jp * l / 6) * [2, 1; 1, 2];
    Sf = diag([1, -1, 1, -1]);
    ia = [2 5];
    ib = [1 3 4 6];
    Ke(ia, ia) = Kt2;
    Me(ia, ia) = Mt2;
    Ke(ib, ib) = Sf * Kb * Sf;
    Me(ib, ib) = Sf * Mb * Sf;
    T = [1, 0, 0; 0, c, s; 0, -s, c];
  end

  TT = blkdiag(T, T);
  Kg = TT' * Ke * TT;
  Mg = TT' * Me * TT;
  dofs = [3 * e - 2, 3 * e - 1, 3 * e, 3 * e + 1, 3 * e + 2, 3 * e + 3];
  K(dofs, dofs) = K(dofs, dofs) + Kg;
  M(dofs, dofs) = M(dofs, dofs) + Mg;
end
end