--- a
+++ b/tool_funcs/getJointCentresForBone.m
@@ -0,0 +1,97 @@
+%-------------------------------------------------------------------------%
+%    Copyright (c) 2021 Modenese L.                                       %
+%    Author:   Luca Modenese,  2021                                       %
+%    email:    l.modenese@imperial.ac.uk                                  %
+% ----------------------------------------------------------------------- %
+function [prox_P, dist_P, bone_length, V] = getJointCentresForBone(osimModel, bone_to_deform)
+
+import org.opensim.modeling.*
+
+% body of interest
+cur_body = osimModel.getBodySet().get(bone_to_deform);
+
+disp('---------------------');
+disp(' COMPUTE BONE LENGTH ');
+disp('---------------------');
+disp(['Processing body: ', bone_to_deform])
+
+% initialise model
+si = osimModel.initSystem();
+
+%% get proximal joint centre 
+% body joint extraction
+if getOpenSimVersion()<4.0
+    % OpenSim 3.3
+    proxJoint = cur_body.getJoint();
+    % location in child
+    prox_loc = cur_body.getJoint().get_location;
+else
+    % OpenSim 4.x
+    proxJoint = getBodyJoint(osimModel, bone_to_deform, 1);
+    prox_loc = proxJoint.get_frames(1).get_translation();
+end
+
+% transform in MATLAB vectors
+prox_P = [prox_loc.get(0), prox_loc.get(1), prox_loc.get(2)];
+% NOTE: no need to check for CustomJoint here, as the child is not affected
+% by the SpatialTransform, which moves the child wrt the parent.
+
+%% get distal joint(s) centre(s)
+% here the body of interest is parent
+jointNameSet = getDistalJointNames(osimModel, bone_to_deform);
+
+for nj = 1:length(jointNameSet)
+    
+    % get current joint
+    cur_joint_name = jointNameSet{nj};
+    distJoint = osimModel.getJointSet().get(cur_joint_name);
+    
+    % location in parent
+    % OpenSim 3.3
+    if getOpenSimVersion()<4.0 
+        dist_loc = distJoint.get_location_in_parent;
+    else
+        % OpenSim 4.x
+        dist_loc = distJoint.get_frames(0).get_translation();
+    end 
+    
+    % offset from the spatial transform (in local body)
+    % take into account the spatialTransform
+    jointOffset = [0, 0, 0];
+    if strcmp(char(distJoint.getConcreteClassName()), 'CustomJoint')
+        localJointOffset = computeSpatialTransformTranslations(osimModel, distJoint);
+        jointOffsetV3 = Vec3(localJointOffset(1), localJointOffset(2), localJointOffset(3));
+        jointOffset = [jointOffsetV3.get(0), jointOffsetV3.get(1), jointOffsetV3.get(2)];
+    end
+    
+    % move to body of interest
+%     osimModel.getSimbodyEngine().transformPosition(si, distJoint.getBody(), jointOffsetV3, cur_body, jointOffsetV3);
+    
+    % sum the contributions
+    dist_P = [dist_loc.get(0), dist_loc.get(1), dist_loc.get(2)];
+    dist_P(nj,1:3) = dist_P + jointOffset;
+    % lengths
+    L(nj) = norm(prox_P-dist_P(nj,1:3));
+end
+
+%% compute length
+% in case of multiple joint centre take the further, so all joints are
+% transformed, if needed.
+% Example: tibiofemoral and patellofemoral joints.
+
+[bone_length, max_ind] = max(L);
+
+% distal point
+dist_P = dist_P(max_ind, :);
+
+% compute axis versor
+V = (dist_P-prox_P)/bone_length;
+
+% display output
+disp(['Proximal joint name  : ', char(proxJoint.getName())]);
+disp(['Proximal joint centre: ', sprintf('%.2f %.2f %.2f', prox_P)]);
+disp(['Distal joint name    : ', jointNameSet{max_ind}]);
+disp(['Distal joint centre  : ', sprintf('%.2f %.2f %.2f', dist_P)]);
+disp(['Total length of bone : ', sprintf('%.2f %.2f %.2f', bone_length), ' m']);
+
+end
\ No newline at end of file