diff --git a/.gitignore b/.gitignore index df8f7399cc..5d979dd5ab 100644 --- a/.gitignore +++ b/.gitignore @@ -56,6 +56,7 @@ delete_this_to_stop_optimization*.txt .idea/codeStyles/codeStyleConfig.xml .idea/codeStyles/Project.xml .idea/editor.xml +.idea/copilot/** .cache/clangd/** **/.vscode/ @@ -71,6 +72,8 @@ delete_this_to_stop_optimization*.txt .idea/deployment.xml .idea/other.xml +.idea/encodings.xml +.idea/inspectionProfiles/Project_Default.xml # Clang-Tidy .clang-tidy @@ -80,4 +83,3 @@ cmake-build-* ## File-based project format: *.iws -.vscode/settings.json diff --git a/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m b/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m index f0565655a6..43f9078c40 100644 --- a/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m +++ b/Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m @@ -324,19 +324,15 @@ function muscleDrivenJointMomentTracking() % Ignore coordinates that are locked, prescribed, or coupled to other % coordinates via CoordinateCouplerConstraints (true by default). jointMomentTracking.setIgnoreConstrainedCoordinates(true); -coordinateSet = model.getCoordinateSet(); -for i = 0:coordinateSet.getSize()-1 - coordinate = coordinateSet.get(i); - coordName = coordinate.getName(); - % Don't track generalized forces associated with pelvis residuals. - if contains(string(coordName), "pelvis") - jointMomentTracking.setWeightForCoordinate(coordName, 0); - end - % Encourage better tracking of the ankle joint moments. - if contains(string(coordName), "ankle") - jointMomentTracking.setWeightForCoordinate(coordName, 100); - end -end + +% Do not track generalized forces associated with pelvis residuals. +jointMomentTracking.setWeightForGeneralizedForcePattern(".*pelvis.*", 0); + +% Encourage better tracking of the ankle joint moments. +jointMomentTracking.setWeightForGeneralizedForce("ankle_angle_r_moment", 100); +jointMomentTracking.setWeightForGeneralizedForce("ankle_angle_l_moment", 100); + +% Add the joint moment tracking goal to the problem. problem.addGoal(jointMomentTracking); % Update the solver problem and tolerances. diff --git a/Bindings/Java/tests/TestMocoSlidingMass.java b/Bindings/Java/tests/TestMocoSlidingMass.java index ffbf9c4466..8bd543d162 100644 --- a/Bindings/Java/tests/TestMocoSlidingMass.java +++ b/Bindings/Java/tests/TestMocoSlidingMass.java @@ -82,7 +82,7 @@ public static void testMocoSlidingMass() throws Exception { // ===================== if (MocoCasADiSolver.isAvailable()) { MocoCasADiSolver ms = study.initCasADiSolver(); - ms.set_num_mesh_intervals(100); + ms.set_num_mesh_intervals(50); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/Bindings/OpenSimHeaders_common.h b/Bindings/OpenSimHeaders_common.h index 44c5f343c5..333b4d7a05 100644 --- a/Bindings/OpenSimHeaders_common.h +++ b/Bindings/OpenSimHeaders_common.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include diff --git a/Bindings/OpenSimHeaders_moco.h b/Bindings/OpenSimHeaders_moco.h index a14e6c7d17..903e56d793 100644 --- a/Bindings/OpenSimHeaders_moco.h +++ b/Bindings/OpenSimHeaders_moco.h @@ -7,6 +7,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py b/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py index 61eb1eebde..97c26ecc57 100644 --- a/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py +++ b/Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py @@ -279,18 +279,15 @@ def muscleDrivenJointMomentTracking(): # Ignore coordinates that are locked, prescribed, or coupled to other # coordinates via CoordinateCouplerConstraints (true by default). jointMomentTracking.setIgnoreConstrainedCoordinates(True) - coordinateSet = model.getCoordinateSet() - for i in range(coordinateSet.getSize()): - coordinate = coordinateSet.get(i) - coordName = coordinate.getName() - # Don't track generalized forces associated with pelvis residuals. - if 'pelvis' in coordName: - jointMomentTracking.setWeightForCoordinate(coordName, 0) - - # Encourage better tracking of the ankle joint moments. - if 'ankle' in coordName: - jointMomentTracking.setWeightForCoordinate(coordName, 100) - + + # Do not track generalized forces associated with pelvis residuals. + jointMomentTracking.setWeightForGeneralizedForcePattern('.*pelvis.*', 0) + + # Encourage better tracking of the ankle joint moments. + jointMomentTracking.setWeightForGeneralizedForce('ankle_angle_r_moment', 100) + jointMomentTracking.setWeightForGeneralizedForce('ankle_angle_l_moment', 100) + + # Add the joint moment tracking goal to the problem. problem.addGoal(jointMomentTracking) # Update the solver tolerances. diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index eda1bf7f33..f2c65bf9dd 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -35,6 +35,27 @@ def createSlidingMassModel(): return model +def createDoubleSlidingMassModel(): + model = createSlidingMassModel() + body = osim.Body("body2", 10.0, osim.Vec3(0), osim.Inertia(0)) + model.addComponent(body) + + joint = osim.SliderJoint("slider2", model.getGround(), body) + coord = joint.updCoordinate(osim.SliderJoint.Coord_TranslationX) + coord.setName("position"); + model.addComponent(joint); + + actu = osim.CoordinateActuator() + actu.setCoordinate(coord) + actu.setName("actuator2") + actu.setOptimalForce(1) + model.addComponent(actu) + + model.finalizeConnections() + model.initSystem() + return model + + class TestSwigAddtlInterface(unittest.TestCase): def test_bounds(self): model = osim.Model() @@ -391,3 +412,33 @@ def test_changing_costs(self): # Change the weights of the costs. effort.setWeight(0.1) assert(study.solve().getFinalTime() < 0.8 * finalTime0) + + def test_expression_based_parameter_goal(self): + study = osim.MocoStudy() + mp = study.updProblem() + mp.setModel(createDoubleSlidingMassModel()) + mp.setTimeBounds(0, 1) + mp.setStateInfo("/slider/position/value", [-5, 5], 0, [0.2, 0.3]) + mp.setStateInfo("/slider/position/speed", [-20, 20]) + mp.setStateInfo("/slider2/position/value", [-5, 5], 1, [1.2, 1.3]) + mp.setStateInfo("/slider2/position/speed", [-20, 20]) + + parameter = osim.MocoParameter("sphere_mass", "body", "mass", + osim.MocoBounds(0, 10)) + mp.addParameter(parameter) + parameter2 = osim.MocoParameter("sphere2_mass", "body2", "mass", + osim.MocoBounds(0, 10)) + mp.addParameter(parameter2) + total_weight = 7 + mass_goal = osim.MocoExpressionBasedParameterGoal() + mp.addGoal(mass_goal) + mass_goal.setExpression(f"(p+q-{total_weight})^2") + mass_goal.addParameter(parameter, "p") + mass_goal.addParameter(parameter2, "q") + + ms = study.initTropterSolver() + ms.set_num_mesh_intervals(25) + sol = study.solve() + + self.assertAlmostEqual(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass"), + total_weight) diff --git a/Bindings/common.i b/Bindings/common.i index 0b84575d90..ec66faca3f 100644 --- a/Bindings/common.i +++ b/Bindings/common.i @@ -60,6 +60,7 @@ %include %include %include +%include %include %include diff --git a/Bindings/moco.i b/Bindings/moco.i index ce3fba4389..95af4e3216 100644 --- a/Bindings/moco.i +++ b/Bindings/moco.i @@ -24,6 +24,7 @@ namespace OpenSim { %include %include %include +%include %include %include %include @@ -48,6 +49,8 @@ namespace OpenSim { %include %include +%include +%include %include %include diff --git a/CHANGELOG.md b/CHANGELOG.md index 669aa8777e..a1a282a8d5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,20 @@ This is not a comprehensive list of changes but rather a hand-curated collection v4.6 ==== +- The performance of `getStateVariableValue`, `getStateVariableDerivativeValue`, and `getModelingOption` was improved in + the case where provided string is just the name of the value, rather than a path to it (#3782) +- Fixed bugs in `MocoStepTimeAsymmetryGoal::printDescriptionImpl()` where there were missing or incorrect values printed. (#3842) +- Added `ModOpPrescribeCoordinateValues` which can prescribe motion of joints in a model given a table of data. (#3862) +- Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to + allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from + occurring in the output. (#3867) +- Added `Output`s to `ExpressionBasedCoordinateForce`, `ExpressionBasedPointToPointForce`, and `ExpressionBasedBushingForce` for accessing force values. (#3872) +- `PointForceDirection` no longer has a virtual destructor, is `final`, and its `scale` functionality + has been marked as `[[deprecated]]` (#3890) +- Added `ExpressionBasedFunction` for creating `Function`s based on user-defined mathematical expressions. (#3892) + +v4.5.1 +====== - Added support for list `Socket`s via the macro `OpenSim_DECLARE_LIST_SOCKET`. The macro-generated method `appendSocketConnectee_*` can be used to connect `Object`s to a list `Socket`. In addition, `Component` and Socket have new `getConnectee` overloads that take an index to a desired object in the list `Socket` (#3652). @@ -68,10 +82,7 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Fixed bugs in `MocoCasOCProblem` and `CasOC::Problem` with incorrect string formatting. (#3828) - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830) - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834) -- The performance of `getStateVariableValue`, `getStateVariableDerivativeValue`, and `getModelingOption` was improved in - the case where provided string is just the name of the value, rather than a path to it (#3782) -- Fixed bugs in `MocoStepTimeAsymmetryGoal::printDescriptionImpl()` where there were missing or incorrect values printed. (#3842) - +- Upgraded the Ipopt dependency Metis to version 5.1.0 on Unix and macOS to enable building on `osx-arm64` (#3874). v4.5 ==== diff --git a/CHANGELOG_MOCO.md b/CHANGELOG_MOCO.md index 7d72241444..2f19bb1bec 100644 --- a/CHANGELOG_MOCO.md +++ b/CHANGELOG_MOCO.md @@ -1,17 +1,52 @@ Moco Change Log =============== -1.3.1 +1.4.0 ----- - 2024-08-30: Added `MocoInverse::initializeKinematics()` to allow users to retrieve kinematics after converting the `MocoInverse` to a `MocoStudy`. +- 2024-08-26: Changed all `printDescription()` and `printDescriptionImpl()` methods to log at level info + instead of cout. + +- 2024-08-21: Added support for the 'projection' method for enforcing kinematic + constraints from Bordalba et al. (2023) to `MocoCasADiSolver`. + This method allows enforcing kinematic constraints with any + transcription scheme and can be enabled using by setting the + property `kinematic_constraint_method` to `'Bordalba2023'`. The + existing default method from Posa et al. (2016) can also be + specified using `'Posa2016'`. + +- 2024-08-21: The `MocoSolver` properties `interpolate_control_midpoints` and + `enforce_path_constraint_midpoints` have been renamed to + `interpolate_control_mesh_interior_points` and + `enforce_path_constraint_mesh_interior_points`, respectively. This + change was made to clarify that these properties apply to the new + Legendre-Gauss and Legendre-Gauss-Radau transcription schemes which + have collocation points in the mesh interval interior that do not + necessarily coincide with the mesh interval midpoints (unlike + Hermite-Simpson transcription). + +- 2024-08-15: Added `MocoExpressionBasedParameterGoal` to enable minimizing any arithmetic expression + of parameter values. + +- 2024-07-26: Added `MocoStateBoundConstraint` and `MocoOutputBoundConstraint` to enable bounding + state variables or output values by one or two `Function`s, similar to + `MocoControlBoundConstraint`. + +- 2024-07-22: Added support for `MocoOutputGoal`s and `MocoOutputConstraint`s that are + composed of two `Output`s. This applies to all types of Output goals + (`MocoInitialOutputGoal`, `MocoFinalOutputGoal`, etc.). The two `Output`s + can be combined by addition, subtraction, multiplication, or division. + - 2024-07-08: Fixed a bug in `DeGrooteFregly2016Muscle` where updates to properties `pennation_angle_at_optimal`, `optimal_fiber_length`, `max_contraction_velocity`, and `tendon_strain_at_one_norm_force` during parameter optimization did not affect certain model calculations, and as a result were not changing during optimization. +1.3.1 +----- - 2024-07-08: Fixed a bug where deserialization of an OpenSim model with the `Bhargava2004SmoothedMuscleMetabolics` component would not properly set the muscle masses listed, resulting in incorrect metabolics values being computed. @@ -22,9 +57,9 @@ Moco Change Log (e.g., getInputControlsTrajectory(), generateControlsFromModelControllers()). `MocoGoal`s and `MocoPathConstraint`s have been updated to support Input controls. See the Moco User Guide for details. - + - 2024-04-01: Added `MocoGeneralizedForceTrackingGoal` to enable joint moment tracking - in `MocoProblem`s, and added the utility `calcGeneralizeForces()` to + in `MocoProblem`s, and added the utility `calcGeneralizedForces()` to `MocoStudy` for computing joint moments from a `MocoTrajectory`. Added a sub-example to exampleMocoTrack (C++, Python, and Matlab) to feature this new functionality. diff --git a/OpenSim/Actuators/ModelOperators.h b/OpenSim/Actuators/ModelOperators.h index ef78c85c8d..8b94cb4ec5 100644 --- a/OpenSim/Actuators/ModelOperators.h +++ b/OpenSim/Actuators/ModelOperators.h @@ -19,9 +19,10 @@ * -------------------------------------------------------------------------- */ #include "ModelProcessor.h" - #include +#include #include +#include "OpenSim/Simulation/TableProcessor.h" namespace OpenSim { @@ -208,6 +209,47 @@ class OSIMACTUATORS_API ModOpReplacePathsWithFunctionBasedPaths } }; +/// Prescribe motion to Coordinate%s in a model by providing a table containing +/// time series data of Coordinate values. Any columns in the provided table +/// (e.g., "/jointset/ankle_r/ankle_angle_r/value") that do not match a valid +/// path to a Joint Coordinate value in the model will be ignored. A GCVSpline +/// function is created for each column of Coordinate values and this function +/// is assigned to the `prescribed_function` property for the matching Coordinate. +/// In addition, the `prescribed` property for each matching Coordinate is set +/// to "true". +class OSIMACTUATORS_API ModOpPrescribeCoordinateValues : public ModelOperator { + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpPrescribeCoordinateValues, ModelOperator); + OpenSim_DECLARE_PROPERTY(coordinate_values, TableProcessor, + "The table of coordinate value data to prescribe to the model") + +public: + ModOpPrescribeCoordinateValues(TableProcessor table) { + constructProperty_coordinate_values(table); + } + void operate(Model& model, const std::string&) const override { + model.finalizeFromProperties(); + TimeSeriesTable table = get_coordinate_values().process(); + GCVSplineSet statesSpline(table); + + for (const std::string& pathString: table.getColumnLabels()) { + ComponentPath path = ComponentPath(pathString); + if (path.getNumPathLevels() < 3) { + continue; + } + std::string jointPath = path.getParentPath().getParentPath().toString(); + if (!model.hasComponent(jointPath)) { + log_warn("Found column label '{}', but it does not match a " + "joint coordinate value in the model.", pathString); + continue; + } + Coordinate& q = model.updComponent(jointPath).updCoordinate(); + q.setPrescribedFunction(statesSpline.get(pathString)); + q.setDefaultIsPrescribed(true); + } + } +}; + } // namespace OpenSim #endif // OPENSIM_MODELOPERATORS_H diff --git a/OpenSim/Actuators/Test/linear_move.sto b/OpenSim/Actuators/Test/linear_move.sto new file mode 100644 index 0000000000..6637a288f7 --- /dev/null +++ b/OpenSim/Actuators/Test/linear_move.sto @@ -0,0 +1,80 @@ +inDegrees=no +num_controls=1 +num_derivatives=0 +num_input_controls=0 +num_iterations=9 +num_multipliers=0 +num_parameters=0 +num_slacks=0 +num_states=2 +objective=17.777778 +objective_goal=17.777778 +solver_duration=0.189080 +status=Solve_Succeeded +success=true +DataType=double +version=3 +OpenSimVersion=4.5.1-2024-07-30-f086c9f48 +endheader +time /jointset/slider/position/value /jointset/slider/position/speed /actuator +0 -1 0 13.33333337479673 +0.05 -0.9983518518480435 0.0655555556803972 12.88888889736215 +0.1 -0.9934814814717465 0.1288888889736214 12.44444441992756 +0.15 -0.9854999999874255 0.1900000000621961 12.00000001550229 +0.2 -0.974518518499229 0.2488888891286444 11.55555561107703 +0.25 -0.9606481481111379 0.3055555560004879 11.11111113766036 +0.3 -0.9439999999386316 0.3600000005052481 10.66666666424369 +0.35 -0.9246851850966852 0.4122222228456786 10.22222227192854 +0.4 -0.9028148146867566 0.4622222232245334 9.777777879613383 +0.45 -0.8784999998182323 0.5100000009633601 9.333333215917294 +0.5 -0.8518518516457286 0.5555555553837064 8.888888552221205 +0.55 -0.8229814813125906 0.5988888878403209 8.444444430424593 +0.6000000000000001 -0.7919999998718462 0.6399999996879524 8.000000308627978 +0.65 -0.7590185183827263 0.6788888891997272 7.55555549608197 +0.7 -0.7241481480195857 0.715555554648772 7.111110683535962 +0.75 -0.6874999999516969 0.7499999980669467 6.666666683733872 +0.8 -0.6491851852128745 0.7822222214861108 6.22222268393178 +0.85 -0.6093148148360156 0.8122222229294871 5.777777893418768 +0.8999999999999999 -0.567999999985802 0.8400000004202984 5.333333102905756 +0.95 -0.5253518518386016 0.8655555552341611 4.888888822639325 +1 -0.4814814814857414 0.8888888886466917 4.444444542372896 +1.05 -0.4365000000064881 0.9100000001058856 4.000000041304648 +1.1 -0.3905185185169086 0.9288888890597381 3.5555555402364 +1.15 -0.3436481481395661 0.9455555556704761 3.111111104058768 +1.2 -0.2959999999862089 0.9600000001003259 2.666666667881135 +1.25 -0.2476851851661622 0.972222222332448 2.222222224967726 +1.3 -0.1988148147898736 0.9822222223500031 1.777777782054316 +1.35 -0.149499999968269 0.9900000001411365 1.333333334399007 +1.4 -0.09985185181306472 0.9955555556939932 0.888888886743697 +1.45 -0.04998148143600051 0.9988888890190036 0.4444444432604505 +1.5 5.187879260919816e-11 1.000000000126598 -2.227959634272017e-10 +1.55 0.04998148153970364 0.9988888890168498 -0.4444444436763597 +1.6 0.09985185191660939 0.9955555556898341 -0.8888888871299233 +1.65 0.1495000000715612 0.9900000001352692 -1.333333334696047 +1.7 0.198814814892839 0.9822222223428736 -1.777777782262169 +1.75 0.2476851852687225 0.9722222223229228 -2.222222225718217 +1.8 0.2960000000881765 0.9600000000856915 -2.666666669174265 +1.85 0.3436481482406782 0.9455555556516544 -3.111111104440555 +1.9 0.3905185186170698 0.928888889041286 -3.555555539706845 +1.95 0.4365000001056856 0.9100000000836407 -4.000000043351224 +2 0.4814814815834636 0.8888888886077736 -4.444444546995603 +2.05 0.5253518519337876 0.8655555551713926 -4.888888827556889 +2.1 0.5680000000772224 0.8400000003322049 -5.333333108118175 +2.149999999999999 0.6093148149225125 0.8122222228232857 -5.777777895449512 +2.2 0.6491851852939401 0.7822222213777098 -6.222222682780849 +2.25 0.6875000000275761 0.7499999979696728 -6.666666680433908 +2.3 0.7241481480911034 0.7155555545733703 -7.111110678086965 +2.350000000000001 0.759018518451018 0.6788888891433432 -7.555555493923847 +2.4 0.7919999999374514 0.6399999996341318 -8.000000309760729 +2.45 0.822981481375192 0.5988888877705855 -8.444444435657866 +2.5 0.8518518517040183 0.5555555552775534 -8.888888561555005 +2.55 0.8784999998704475 0.5100000008345333 -9.333333215653024 +2.6 0.9028148147329635 0.4622222231210231 -9.777777869751041 +2.65 0.9246851851385094 0.4122222227650854 -10.222222272624 +2.7 0.9439999999758993 0.360000000394783 -10.66666667549696 +2.75 0.9606481481417346 0.3055555558492993 -11.11111114269661 +2.8 0.9745185185218959 0.2488888889678173 -11.55555560989625 +2.85 0.9855000000022787 0.189999999912079 -12.00000001239905 +2.9 0.9934814814795618 0.1288888888438267 -12.44444441490185 +2.95 0.9983518518503287 0.06555555559561541 -12.88888888438266 +3 1 0 -13.33333335386348 diff --git a/OpenSim/Actuators/Test/testModelProcessor.cpp b/OpenSim/Actuators/Test/testModelProcessor.cpp index 6e73c35b6f..f64ceb7771 100644 --- a/OpenSim/Actuators/Test/testModelProcessor.cpp +++ b/OpenSim/Actuators/Test/testModelProcessor.cpp @@ -16,7 +16,8 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ - +#include "OpenSim/Common/Sine.h" +#include #include #include #include @@ -123,3 +124,70 @@ TEST_CASE("ModOpReplaceMusclesWithPathActuators") { 0); CHECK(processedModel.countNumComponents() == 1); } + +TEST_CASE("ModOpPrescribeMotion") { + // model has a slider as a Component + auto unprescribedModel = ModelFactory::createSlidingPointMass(); + + // add another slider as a Joint + auto* body = new Body("body2", 10.0, SimTK::Vec3(0), SimTK::Inertia(0)); + unprescribedModel.addComponent(body); + body->attachGeometry(new Sphere(0.05)); + auto* joint = new SliderJoint("slider2", unprescribedModel.getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + unprescribedModel.addJoint(joint); + + unprescribedModel.finalizeConnections(); + + // create sine data + std::string componentPath = "/slider/position/value"; + std::string jointPath = "/jointset/slider2/position/value"; + std::vector paths = {componentPath, jointPath}; + double interval = 0.05; + double duration = 3.0; + std::vector times; + int numEntries = duration / interval + 1; + for (int i = 0; i < numEntries; ++i) { + times.push_back(i * interval); + } + int numColumns = static_cast(paths.size()); + SimTK::Matrix positions(numEntries, numColumns); + for (int col = 0; col < numColumns; ++col) { + SimTK::Vector x(1); + for (int row = 0; row < numEntries; ++row) { + x[0] = row * interval + col; // +col makes each column different + positions.set(row, col, Sine().calcValue(x)); + } + } + TimeSeriesTable table(times, positions, paths); + + // prescribe motion data to model + TableProcessor table_processor = TableProcessor(table); + ModelProcessor modelProcessor(unprescribedModel); + modelProcessor.append(ModOpPrescribeCoordinateValues(table_processor)); + Model model = modelProcessor.process(); + + // check that positions match + auto* reporter = new StatesTrajectoryReporter(); + reporter->setName("reporter"); + reporter->set_report_time_interval(interval); + model.addComponent(reporter); + SimTK::State& state = model.initSystem(); + model.realizePosition(state); + Manager manager(model, state); + manager.integrate(duration); + StatesTrajectory statesTraj = reporter->getStates(); + + for (const std::string& path : paths) { + int jointColumn = static_cast(table.getColumnIndex(path)); + for (int itime = 0; itime < static_cast(statesTraj.getSize()); ++itime) { + state = statesTraj[itime]; + double time = state.getTime(); + double posActual = model.getStateVariableValue(state, path); + SimTK::RowVectorView row = table.getNearestRow(time); + double posExpected = row[jointColumn]; + REQUIRE(posActual == Catch::Approx(posExpected).margin(1e-3)); + } + } +} diff --git a/OpenSim/Auxiliary/auxiliaryTestFunctions.h b/OpenSim/Auxiliary/auxiliaryTestFunctions.h index df97fdda6c..1f244af7fd 100644 --- a/OpenSim/Auxiliary/auxiliaryTestFunctions.h +++ b/OpenSim/Auxiliary/auxiliaryTestFunctions.h @@ -268,14 +268,18 @@ OpenSim::Object* randomize(OpenSim::Object* obj) double Ixy = 0.01*Ixx; prop = SimTK::Vec6(Ixx, Ixx, Ixx, Ixy, Ixy, Ixy); } else if (ts == "string") { - string base("ABCXYZ"); - if (isList){ - stringstream val; - val << base << "_" << ap.size(); - ap.appendValue(val.str()); - } - else{ - ap.updValue() = base; + // We cannot use an arbitrary string for ExpressionBasedBushingForce + // properties since they must contain specific variable names (e.g., + // "theta_x", "delta_x", etc.). + if (obj->getConcreteClassName() != "ExpressionBasedBushingForce") { + string base("ABCXYZ"); + if (isList) { + stringstream val; + val << base << "_" << ap.size(); + ap.appendValue(val.str()); + } else { + ap.updValue() = base; + } } } else if (ts == "double" && isList && ap.getMaxListSize() < 20) { for (int i=0; i< ap.getMaxListSize(); ++i) diff --git a/OpenSim/CMakeLists.txt b/OpenSim/CMakeLists.txt index 8d53dedd8f..7660943102 100644 --- a/OpenSim/CMakeLists.txt +++ b/OpenSim/CMakeLists.txt @@ -3,7 +3,6 @@ add_subdirectory(Common) add_subdirectory(Simulation) add_subdirectory(Actuators) add_subdirectory(Analyses) -add_subdirectory(Utilities) add_subdirectory(ExampleComponents) add_subdirectory(Tools) add_subdirectory(Moco) diff --git a/OpenSim/Common/CMakeLists.txt b/OpenSim/Common/CMakeLists.txt index ef0508c4b6..afabfd39de 100644 --- a/OpenSim/Common/CMakeLists.txt +++ b/OpenSim/Common/CMakeLists.txt @@ -16,7 +16,7 @@ OpenSimAddLibrary( KIT Common AUTHORS "Clay_Anderson-Ayman_Habib-Peter_Loan" # Clients of osimCommon need not link to ezc3d. - LINKLIBS PUBLIC ${Simbody_LIBRARIES} spdlog::spdlog + LINKLIBS PUBLIC ${Simbody_LIBRARIES} spdlog::spdlog osimLepton PRIVATE ${ezc3d_LIBRARY} INCLUDES ${INCLUDES} SOURCES ${SOURCES} diff --git a/OpenSim/Common/CommonUtilities.cpp b/OpenSim/Common/CommonUtilities.cpp index 85f987e771..e827ca0569 100644 --- a/OpenSim/Common/CommonUtilities.cpp +++ b/OpenSim/Common/CommonUtilities.cpp @@ -85,7 +85,7 @@ SimTK::Vector OpenSim::createVector( SimTK::Vector OpenSim::interpolate(const SimTK::Vector& x, const SimTK::Vector& y, const SimTK::Vector& newX, - const bool ignoreNaNs) { + const bool ignoreNaNs, const bool extrapolate) { OPENSIM_THROW_IF(x.size() != y.size(), Exception, "Expected size of x to equal size of y, but size of x " @@ -116,8 +116,11 @@ SimTK::Vector OpenSim::interpolate(const SimTK::Vector& x, SimTK::Vector newY(newX.size(), SimTK::NaN); for (int i = 0; i < newX.size(); ++i) { const auto& newXi = newX[i]; - if (x_no_nans[0] <= newXi && newXi <= x_no_nans[x_no_nans.size() - 1]) + bool inBounds = (x_no_nans[0] <= newXi) && + (newXi <= x_no_nans[x_no_nans.size() - 1]); + if (extrapolate || inBounds) { newY[i] = function.calcValue(SimTK::Vector(1, newXi)); + } } return newY; } diff --git a/OpenSim/Common/CommonUtilities.h b/OpenSim/Common/CommonUtilities.h index 01cdd6f543..795e22cf1e 100644 --- a/OpenSim/Common/CommonUtilities.h +++ b/OpenSim/Common/CommonUtilities.h @@ -91,13 +91,18 @@ SimTK::Vector createVector(std::initializer_list elements); /// Linearly interpolate y(x) at new values of x. The optional 'ignoreNaNs' /// argument will ignore any NaN values contained in the input vectors and /// create the interpolant from the non-NaN values only. Note that this option -/// does not necessarily prevent NaN values from being returned in 'newX', which -/// will have NaN for any values of newX outside of the range of x. +/// does not necessarily prevent NaN values from being returned, which will +/// have NaN for any values of newX outside of the range of x. This is done with +/// the 'extrapolate' option. If the 'extrapolate' argument is true, then the +/// interpolant values will be extrapolated based on a piecewise linear function. +/// Setting both 'ignoreNaNs' and 'extrapolate' to true prevents NaN values from +/// occurring in the interpolant. /// @throws Exception if x and y are different sizes, or x or y is empty. /// @ingroup commonutil OSIMCOMMON_API SimTK::Vector interpolate(const SimTK::Vector& x, const SimTK::Vector& y, - const SimTK::Vector& newX, const bool ignoreNaNs = false); + const SimTK::Vector& newX, const bool ignoreNaNs = false, + const bool extrapolate = false); /// An OpenSim XML file may contain file paths that are relative to the /// directory containing the XML file; use this function to convert that diff --git a/OpenSim/Common/ExpressionBasedFunction.cpp b/OpenSim/Common/ExpressionBasedFunction.cpp new file mode 100644 index 0000000000..f92e4edefe --- /dev/null +++ b/OpenSim/Common/ExpressionBasedFunction.cpp @@ -0,0 +1,131 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: ExpressionBasedFunction.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "ExpressionBasedFunction.h" + +#include +#include +#include +#include + +using namespace OpenSim; + +class SimTKExpressionBasedFunction : public SimTK::Function { +public: + SimTKExpressionBasedFunction(const std::string& expression, + const std::vector& variables) : + m_expression(expression), m_variables(variables) { + + // Check that the variable names are unique. + std::set uniqueVariables; + for (const auto& variable : m_variables) { + if (!uniqueVariables.insert(variable).second) { + OPENSIM_THROW(Exception, + fmt::format("Variable '{}' is defined more than once.", + variable)); + } + } + + // Create the expression programs for the value and its derivatives. + Lepton::ParsedExpression parsedExpression = + Lepton::Parser::parse(m_expression).optimize(); + m_valueProgram = parsedExpression.createProgram(); + + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + Lepton::ParsedExpression diffExpression = + parsedExpression.differentiate(m_variables[i]).optimize(); + m_derivativePrograms.push_back(diffExpression.createProgram()); + } + + try { + std::map vars; + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + vars[m_variables[i]] = 0; + } + m_valueProgram.evaluate(vars); + + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + m_derivativePrograms[i].evaluate(vars); + } + } catch (Lepton::Exception& ex) { + std::string msg = ex.what(); + if (msg.compare(0, 30, "No value specified for variable")) { + std::string undefinedVar = msg.substr(32, msg.size() - 32); + OPENSIM_THROW(Exception, + fmt::format("Variable '{}' is not defined. Use " + "setVariables() to explicitly define this variable. " + "Or, remove it from the expression.", undefinedVar)); + } else { + OPENSIM_THROW(Exception, "Lepton parsing error: {}", msg); + } + } + } + + SimTK::Real calcValue(const SimTK::Vector& x) const override { + OPENSIM_ASSERT(x.size() == static_cast(m_variables.size())); + std::map vars; + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + vars[m_variables[i]] = x[i]; + } + return m_valueProgram.evaluate(vars); + } + + SimTK::Real calcDerivative(const SimTK::Array_& derivComponents, + const SimTK::Vector& x) const override { + OPENSIM_ASSERT(x.size() == static_cast(m_variables.size())); + OPENSIM_ASSERT(derivComponents.size() == 1); + if (derivComponents[0] < static_cast(m_variables.size())) { + std::map vars; + for (int i = 0; i < static_cast(m_variables.size()); ++i) { + vars[m_variables[i]] = x[i]; + } + return m_derivativePrograms[derivComponents[0]].evaluate(vars); + } + return 0.0; + } + + int getArgumentSize() const override { + return static_cast(m_variables.size()); + } + int getMaxDerivativeOrder() const override { return 1; } + SimTKExpressionBasedFunction* clone() const override { + return new SimTKExpressionBasedFunction(*this); + } + +private: + std::string m_expression; + std::vector m_variables; + Lepton::ExpressionProgram m_valueProgram; + std::vector m_derivativePrograms; +}; + +SimTK::Function* ExpressionBasedFunction::createSimTKFunction() const { + OPENSIM_THROW_IF_FRMOBJ(get_expression().empty(), Exception, + "The expression has not been set. Use setExpression().") + + std::vector variables; + for (int i = 0; i < getProperty_variables().size(); ++i) { + variables.push_back(get_variables(i)); + } + return new SimTKExpressionBasedFunction(get_expression(), variables); +} \ No newline at end of file diff --git a/OpenSim/Common/ExpressionBasedFunction.h b/OpenSim/Common/ExpressionBasedFunction.h new file mode 100644 index 0000000000..b05d2dc70d --- /dev/null +++ b/OpenSim/Common/ExpressionBasedFunction.h @@ -0,0 +1,134 @@ +#ifndef OPENSIM_EXPRESSION_BASED_FUNCTION_H_ +#define OPENSIM_EXPRESSION_BASED_FUNCTION_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: ExpressionBasedFunction.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "osimCommonDLL.h" +#include "Function.h" +namespace OpenSim { + +/** + * A function based on a user-defined mathematical expression. + * + * This class allows users to define a function based on a mathematical + * expression (e.g., "x*sqrt(y-8)"). The expression can be a function of any + * number of independent variables. The expression is parsed and evaluated using + * the Lepton library. + * + * Set the expression using setExpression(). Any variables used in the + * expression must be explicitly defined using setVariables(). This + * implementation allows computation of first-order derivatives only. + * + * # Creating Expressions + * + * Expressions can contain variables, constants, operations, parentheses, commas, + * spaces, and scientific "e" notation. The full list of supported operations is: + * sqrt, exp, log, sin, cos, sec, csc, tan, cot, asin, acos, atan, sinh, cosh, + * tanh, erf, erfc, step, delta, square, cube, recip, min, max, abs, +, -, *, /, + * and ^. + */ +class OSIMCOMMON_API ExpressionBasedFunction : public Function { + OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedFunction, Function); + +public: +//============================================================================== +// PROPERTIES +//============================================================================== + OpenSim_DECLARE_PROPERTY(expression, std::string, + "The mathematical expression defining this Function."); + OpenSim_DECLARE_LIST_PROPERTY(variables, std::string, + "The independent variables used by this Function's expression. " + "In XML, variable names should be space-separated."); + +//============================================================================== +// METHODS +//============================================================================== + + /** Default constructor. */ + ExpressionBasedFunction() { constructProperties(); } + + /** Convenience constructor. + * + * @param expression The expression that defines this Function. + * @param variables The independent variable names of this expression. + */ + ExpressionBasedFunction(std::string expression, + const std::vector& variables) { + constructProperties(); + set_expression(std::move(expression)); + setVariables(variables); + } + + /** + * The mathematical expression that defines this Function. The expression + * should be a function of the variables defined via setVariables(). + * + * @note The expression cannot contain any whitespace characters. + */ + void setExpression(std::string expression) { + set_expression(std::move(expression)); + } + /// @copydoc setExpression() + const std::string& getExpression() const { + return get_expression(); + } + + /** + * The independent variable names of this expression. The variables names + * should be unique and should be comprised of alphabetic characters or any + * characters not reserved by Lepton (i.e., +, -, *, /, and ^). Variable + * names can contain numbers as long they do not come first in the name + * (e.g., "var0"). The input vector passed to calcValue() and + * calcDerivative() should be in the same order as the variables defined + * here. + */ + void setVariables(const std::vector& variables) { + for (const auto& var : variables) { + append_variables(var); + } + } + /// @copydoc setVariables() + std::vector getVariables() const { + std::vector variables; + for (int i = 0; i < getProperty_variables().size(); ++i) { + variables.push_back(get_variables(i)); + } + return variables; + } + + /** + * Return a pointer to a SimTK::Function object that implements this + * function. + */ + SimTK::Function* createSimTKFunction() const override; + +private: + void constructProperties() { + constructProperty_expression(""); + constructProperty_variables(); + } +}; + +} // namespace OpenSim + +#endif // OPENSIM_EXPRESSION_BASED_FUNCTION_H_ \ No newline at end of file diff --git a/OpenSim/Common/RegisterTypes_osimCommon.cpp b/OpenSim/Common/RegisterTypes_osimCommon.cpp index 2be4dec912..114a4fde20 100644 --- a/OpenSim/Common/RegisterTypes_osimCommon.cpp +++ b/OpenSim/Common/RegisterTypes_osimCommon.cpp @@ -41,6 +41,7 @@ #include "MultiplierFunction.h" #include "PolynomialFunction.h" #include "MultivariatePolynomialFunction.h" +#include "ExpressionBasedFunction.h" #include "SignalGenerator.h" @@ -88,6 +89,7 @@ OSIMCOMMON_API void RegisterTypes_osimCommon() Object::registerType( MultiplierFunction() ); Object::registerType( PolynomialFunction() ); Object::registerType( MultivariatePolynomialFunction() ); + Object::registerType( ExpressionBasedFunction() ); Object::registerType( SignalGenerator() ); diff --git a/OpenSim/Common/Test/testFunctions.cpp b/OpenSim/Common/Test/testFunctions.cpp index c28c955f1c..d854c914c9 100644 --- a/OpenSim/Common/Test/testFunctions.cpp +++ b/OpenSim/Common/Test/testFunctions.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include @@ -83,15 +84,61 @@ TEST_CASE("SignalGenerator") { } TEST_CASE("Interpolate using PiecewiseLinearFunction") { - SimTK::Vector x = createVector({0, 1}); - SimTK::Vector y = createVector({1, 0}); - SimTK::Vector newX = createVector({-1, 0.25, 0.75, 1.5}); - SimTK::Vector newY = OpenSim::interpolate(x, y, newX); - - SimTK_TEST(SimTK::isNaN(newY[0])); - SimTK_TEST_EQ(newY[1], 0.75); - SimTK_TEST_EQ(newY[2], 0.25); - SimTK_TEST(SimTK::isNaN(newY[3])); + SECTION("New X out of original range") { + SimTK::Vector x = createVector({0, 1}); + SimTK::Vector y = createVector({1, 0}); + SimTK::Vector newX = createVector({-1, 0.25, 0.75, 1.5}); + SimTK::Vector newY = OpenSim::interpolate(x, y, newX); + + SimTK_TEST(SimTK::isNaN(newY[0])); + SimTK_TEST_EQ(newY[1], 0.75); + SimTK_TEST_EQ(newY[2], 0.25); + SimTK_TEST(SimTK::isNaN(newY[3])); + } + SECTION("New X out of original range, extrapolate") { + SimTK::Vector x = createVector({0, 1}); + SimTK::Vector y = createVector({1, 0}); + SimTK::Vector newX = createVector({-1, 0.25, 0.75, 1.5}); + SimTK::Vector newY = OpenSim::interpolate(x, y, newX, false, true); + + SimTK_TEST(!SimTK::isNaN(newY[0])); + SimTK_TEST_EQ(newY[1], 0.75); + SimTK_TEST_EQ(newY[2], 0.25); + SimTK_TEST(!SimTK::isNaN(newY[3])); + } + SECTION("First and last Y are NaN, extrapolate") { + SimTK::Vector x = createVector({0, 1, 2, 3, 4, 5}); + SimTK::Vector y = createVector({SimTK::NaN, 0, 3, 4, SimTK::NaN, SimTK::NaN}); + SimTK::Vector newY = OpenSim::interpolate(x, y, x, true, true); + + for (int i = 0; i < newY.size(); ++i) { + SimTK_TEST(!SimTK::isNaN(newY[i])); + } + } + + SimTK::Vector x = createVector({0, 1, 2, 3}); + SimTK::Vector y = createVector({SimTK::NaN, 1, 2, SimTK::NaN}); + SECTION("Ignore NaNs, extrapolate") { + SimTK::Vector newY = OpenSim::interpolate(x, y, x, true, true); + SimTK_TEST_EQ(newY[0], 0); + SimTK_TEST_EQ(newY[1], 1); + SimTK_TEST_EQ(newY[2], 2); + SimTK_TEST_EQ(newY[3], 3); + } + SECTION("Don't ignore NaNs, extrapolate") { + SimTK::Vector newY = OpenSim::interpolate(x, y, x, false, true); + SimTK_TEST(SimTK::isNaN(newY[0])); + SimTK_TEST_EQ(newY[1], 1); + SimTK_TEST(SimTK::isNaN(newY[2])); + SimTK_TEST(SimTK::isNaN(newY[3])); + } + SECTION("Ignore NaNs, don't extrapolate") { + SimTK::Vector newY = OpenSim::interpolate(x, y, x, true, false); + SimTK_TEST(SimTK::isNaN(newY[0])); + SimTK_TEST_EQ(newY[1], 1); + SimTK_TEST_EQ(newY[2], 2); + SimTK_TEST(SimTK::isNaN(newY[3])); + } } TEST_CASE("MultivariatePolynomialFunction") { @@ -214,3 +261,92 @@ TEST_CASE("solveBisection()") { REQUIRE_THROWS_AS(solveBisection(parabola, -5, 5), OpenSim::Exception); } } + +TEST_CASE("ExpressionBasedFunction") { + const SimTK::Real x = SimTK::Test::randReal(); + const SimTK::Real y = SimTK::Test::randReal(); + const SimTK::Real z = SimTK::Test::randReal(); + + SECTION("Square-root function") { + ExpressionBasedFunction f("sqrt(x)", {"x"}); + REQUIRE_THAT(f.calcValue(createVector({x})), + Catch::Matchers::WithinAbs(std::sqrt(x), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x})), + Catch::Matchers::WithinAbs(0.5 / std::sqrt(x), 1e-10)); + } + + SECTION("Exponential function") { + ExpressionBasedFunction f("exp(x)", {"x"}); + REQUIRE_THAT(f.calcValue(createVector({x})), + Catch::Matchers::WithinAbs(std::exp(x), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x})), + Catch::Matchers::WithinAbs(std::exp(x), 1e-10)); + } + + SECTION("Multivariate function") { + ExpressionBasedFunction f("2*x^3 + 3*y*z^2", {"x", "y", "z"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(2*x*x*x + 3*y*z*z, 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(6*x*x, 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(3*z*z, 1e-10)); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(6*y*z, 1e-10)); + } + + + SECTION("Sinusoidal function") { + ExpressionBasedFunction f("x*sin(y*z^2)", {"x", "y", "z"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(x * std::sin(y*z*z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(std::sin(y*z*z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(x*z*z*std::cos(y*z*z), 1e-10)); + } + + SECTION("Undefined variable in expression") { + ExpressionBasedFunction f("x*y", {"x"}); + REQUIRE_THROWS_WITH(f.calcValue(createVector({x, y})), + Catch::Matchers::ContainsSubstring( + "Variable 'y' is not defined.")); + } + + SECTION("Extra variables should have zero derivative") { + ExpressionBasedFunction f("x*y", {"x", "y", "z"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(x*y, 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(y, 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(x, 1e-10)); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(0.0, 1e-10)); + } + + SECTION("Derivative of nonexistent variable") { + ExpressionBasedFunction f("x*y", {"x", "y"}); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y})), + Catch::Matchers::WithinAbs(0.0, 1e-10)); + } + + SECTION("Variable defined multiple times") { + ExpressionBasedFunction f("x", {"x", "x"}); + REQUIRE_THROWS_WITH(f.calcValue(createVector({x})), + Catch::Matchers::ContainsSubstring( + "Variable 'x' is defined more than once.")); + } + + SECTION("Non-alphabetic variable names") { + ExpressionBasedFunction f("@^2 + %*cos(&)", {"@", "%", "&"}); + REQUIRE_THAT(f.calcValue(createVector({x, y, z})), + Catch::Matchers::WithinAbs(x*x + y*std::cos(z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({0}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(2*x, 1e-10)); + REQUIRE_THAT(f.calcDerivative({1}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(std::cos(z), 1e-10)); + REQUIRE_THAT(f.calcDerivative({2}, createVector({x, y, z})), + Catch::Matchers::WithinAbs(-y*std::sin(z), 1e-10)); + } +} \ No newline at end of file diff --git a/OpenSim/Common/osimCommon.h b/OpenSim/Common/osimCommon.h index 23cd153237..19adf1e7bb 100644 --- a/OpenSim/Common/osimCommon.h +++ b/OpenSim/Common/osimCommon.h @@ -29,6 +29,7 @@ #include "CommonUtilities.h" #include "Constant.h" #include "DataTable.h" +#include "ExpressionBasedFunction.h" #include "FunctionSet.h" #include "GCVSpline.h" #include "GCVSplineSet.h" diff --git a/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp b/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp index 9b91023ebb..d8e8713455 100644 --- a/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp +++ b/OpenSim/Examples/Moco/example3DWalking/exampleMocoTrack.cpp @@ -299,18 +299,15 @@ void muscleDrivenJointMomentTracking() { // Ignore coordinates that are locked, prescribed, or coupled to other // coordinates via CoordinateCouplerConstraints (true by default). jointMomentTracking->setIgnoreConstrainedCoordinates(true); - for (const auto& coordinate : model.getComponentList()) { - const auto& coordName = coordinate.getName(); - // Don't track generalized forces associated with pelvis residuals. - if (coordName.find("pelvis") != std::string::npos) { - jointMomentTracking->setWeightForGeneralizedForce(coordName, 0); - } - // Encourage better tracking of the ankle joint moments. - if (coordName.find("ankle") != std::string::npos) { - jointMomentTracking->setWeightForGeneralizedForce(coordName, 100); - } - } + // Do not track generalized forces associated with pelvis residuals. + jointMomentTracking->setWeightForGeneralizedForcePattern(".*pelvis.*", 0); + + // Encourage better tracking of the ankle joint moments. + jointMomentTracking->setWeightForGeneralizedForce( + "ankle_angle_r_moment", 100); + jointMomentTracking->setWeightForGeneralizedForce( + "ankle_angle_l_moment", 100); // Update the solver problem and tolerances. auto& solver = study.updSolver(); diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index bbf91454a4..f618f15df8 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -40,6 +40,8 @@ set(MOCO_SOURCES MocoGoal/MocoControlGoal.cpp MocoGoal/MocoControlTrackingGoal.h MocoGoal/MocoControlTrackingGoal.cpp + MocoGoal/MocoExpressionBasedParameterGoal.h + MocoGoal/MocoExpressionBasedParameterGoal.cpp MocoGoal/MocoJointReactionGoal.h MocoGoal/MocoJointReactionGoal.cpp MocoGoal/MocoOrientationTrackingGoal.h @@ -109,6 +111,10 @@ set(MOCO_SOURCES MocoStudyFactory.cpp MocoScaleFactor.h MocoScaleFactor.cpp + MocoOutputBoundConstraint.cpp + MocoOutputBoundConstraint.h + MocoStateBoundConstraint.cpp + MocoStateBoundConstraint.h ) if(OPENSIM_WITH_CASADI) list(APPEND MOCO_SOURCES @@ -171,7 +177,9 @@ target_compile_definitions(osimMoco PRIVATE OpenSimAddInstallRPATHSelf(TARGET osimMoco LOADER) OpenSimAddInstallRPATHSimbody(TARGET osimMoco LOADER FROM "${CMAKE_INSTALL_LIBDIR}") - +if (OPENSIM_WITH_TROPTER) + OpenSimAddInstallRPATHSelf(TARGET tropter LOADER) +endif() install(TARGETS osimMoco EXPORT OpenSimTargets ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index 53dec48350..d0e3a5bb45 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -73,7 +73,7 @@ casadi::Sparsity calcJacobianSparsityWithPerturbation(const VectorDM& x0s, return combinedSparsity; } -casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, +casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, bool symmetric) const { using casadi::DM; using casadi::Slice; @@ -193,7 +193,7 @@ casadi::Sparsity Endpoint::get_sparsity_in(casadi_int i) { return casadi::Sparsity(0, 0); } } -casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, +casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const { using casadi::Slice; if (i == 0) { @@ -243,8 +243,8 @@ VectorDM EndpointConstraint::eval(const VectorDM& args) const { return out; } -template -casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( +template +casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( casadi_int i) { if (i == 0) { return casadi::Sparsity::dense( @@ -256,9 +256,9 @@ casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( return casadi::Sparsity::dense( m_casProblem->getNumAuxiliaryResidualEquations(), 1); } else if (i == 3) { - if (CalcKCErrors) { - int numRows = m_casProblem->getNumKinematicConstraintEquations(); - return casadi::Sparsity::dense(numRows, 1); + if (calcKCErr) { + return casadi::Sparsity::dense( + m_casProblem->getNumKinematicConstraintEquations(), 1); } else { return casadi::Sparsity(0, 0); } @@ -267,9 +267,8 @@ casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( } } -template -VectorDM MultibodySystemExplicit::eval( - const VectorDM& args) const { +template +VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), args.at(3), args.at(4), args.at(5)}; VectorDM out((int)n_out()); @@ -278,7 +277,7 @@ VectorDM MultibodySystemExplicit::eval( } Problem::MultibodySystemExplicitOutput output{out[0], out[1], out[2], out[3]}; - m_casProblem->calcMultibodySystemExplicit(input, CalcKCErrors, output); + m_casProblem->calcMultibodySystemExplicit(input, calcKCErr, output); return out; } @@ -290,9 +289,7 @@ casadi::Sparsity VelocityCorrection::get_sparsity_in(casadi_int i) { return casadi::Sparsity::dense(1, 1); } else if (i == 1) { return casadi::Sparsity::dense( - m_casProblem->getNumStates() - - m_casProblem->getNumAuxiliaryStates(), - 1); + m_casProblem->getNumMultibodyStates(), 1); } else if (i == 2) { return casadi::Sparsity::dense(m_casProblem->getNumSlacks(), 1); } else if (i == 3) { @@ -314,8 +311,7 @@ casadi::DM VelocityCorrection::getSubsetPoint( const VariablesDM& fullPoint, casadi_int i) const { int itime = 0; using casadi::Slice; - const int NMBS = m_casProblem->getNumStates() - - m_casProblem->getNumAuxiliaryStates(); + const int NMBS = m_casProblem->getNumMultibodyStates(); if (i == 0) { return fullPoint.at(initial_time); } else if (i == 1) { @@ -336,8 +332,57 @@ VectorDM VelocityCorrection::eval(const VectorDM& args) const { return out; } -template -casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( +casadi::Sparsity StateProjection::get_sparsity_in(casadi_int i) { + if (i == 0) { + return casadi::Sparsity::dense(1, 1); + } else if (i == 1) { + return casadi::Sparsity::dense( + m_casProblem->getNumMultibodyStates(), 1); + } else if (i == 2) { + return casadi::Sparsity::dense(m_casProblem->getNumSlacks(), 1); + } else if (i == 3) { + return casadi::Sparsity::dense(m_casProblem->getNumParameters(), 1); + } else { + return casadi::Sparsity(0, 0); + } +} + +casadi::Sparsity StateProjection::get_sparsity_out(casadi_int i) { + if (i == 0) { + return casadi::Sparsity::dense( + m_casProblem->getNumMultibodyStates(), 1); + } else { + return casadi::Sparsity(0, 0); + } +} + +casadi::DM StateProjection::getSubsetPoint( + const VariablesDM& fullPoint, casadi_int i) const { + int itime = 0; + using casadi::Slice; + const int NMBS = m_casProblem->getNumMultibodyStates(); + if (i == 0) { + return fullPoint.at(initial_time); + } else if (i == 1) { + return fullPoint.at(states)(Slice(0, NMBS), itime); + } else if (i == 2) { + return fullPoint.at(slacks)(Slice(), itime); + } else if (i == 3) { + return fullPoint.at(parameters); + } else { + return casadi::DM(); + } +} + +VectorDM StateProjection::eval(const VectorDM& args) const { + VectorDM out{casadi::DM(sparsity_out(0))}; + m_casProblem->calcStateProjection( + args.at(0).scalar(), args.at(1), args.at(2), args.at(3), out[0]); + return out; +} + +template +casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( casadi_int i) { if (i == 0) { return casadi::Sparsity::dense( @@ -349,9 +394,9 @@ casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( return casadi::Sparsity::dense( m_casProblem->getNumAuxiliaryResidualEquations(), 1); } else if (i == 3) { - if (CalcKCErrors) { - int numRows = m_casProblem->getNumKinematicConstraintEquations(); - return casadi::Sparsity::dense(numRows, 1); + if (calcKCErr) { + return casadi::Sparsity::dense( + m_casProblem->getNumKinematicConstraintEquations(), 1); } else { return casadi::Sparsity(0, 0); } @@ -360,9 +405,8 @@ casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( } } -template -VectorDM MultibodySystemImplicit::eval( - const VectorDM& args) const { +template +VectorDM MultibodySystemImplicit::eval(const VectorDM& args) const { Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), args.at(3), args.at(4), args.at(5)}; VectorDM out((int)n_out()); @@ -370,9 +414,9 @@ VectorDM MultibodySystemImplicit::eval( out[i] = casadi::DM(sparsity_out(i)); } - Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], + Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], out[3]}; - m_casProblem->calcMultibodySystemImplicit(input, CalcKCErrors, output); + m_casProblem->calcMultibodySystemImplicit(input, calcKCErr, output); return out; } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index f816f34b46..8a1ccef08c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -61,7 +61,7 @@ class Function : public casadi::Callback { bool has_jac_sparsity(casadi_int oind, casadi_int iind) const override { return !m_fullPointsForSparsityDetection->empty(); } - casadi::Sparsity get_jac_sparsity(casadi_int oind, casadi_int iind, + casadi::Sparsity get_jac_sparsity(casadi_int oind, casadi_int iind, bool symmetric) const override; protected: @@ -69,7 +69,7 @@ class Function : public casadi::Callback { private: /// Here, "point" refers to a vector of all variables in the optimization - /// problem. This function returns a subset of the variables at a point for + /// problem. This function returns a subset of the variables at a point for /// a given input index. VectorDM getSubsetPointsForSparsityDetection(casadi_int iind) const { VectorDM out(m_fullPointsForSparsityDetection->size()); @@ -81,9 +81,9 @@ class Function : public casadi::Callback { } /// As of CasADi 3.6, callback functions need to return Jacobian sparsity - /// patterns for each pair of inputs and outputs. Therefore this function + /// patterns for each pair of inputs and outputs. Therefore this function /// returns a subset point for a given input index. - virtual casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + virtual casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const { int itime = 0; using casadi::Slice; @@ -233,7 +233,7 @@ class Endpoint : public Function { /// provided point, but applying the integrand function and quadrature /// scheme here is complicated. For simplicity, we provide the integral as /// 0. - casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const override; protected: @@ -256,7 +256,7 @@ class EndpointConstraint : public Endpoint { /// This function should compute forward dynamics (explicit multibody dynamics), /// auxiliary explicit dynamics, and the errors for the kinematic constraints. -template +template class MultibodySystemExplicit : public Function { public: casadi_int get_n_out() override final { return 4; } @@ -297,11 +297,39 @@ class VelocityCorrection : public Function { casadi::Sparsity get_sparsity_in(casadi_int i) override final; casadi::Sparsity get_sparsity_out(casadi_int i) override final; VectorDM eval(const VectorDM& args) const override; - casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const override; }; -template +/// This function should compute a state projection term to make feasible +/// problems that enforce kinematic constraints and their derivatives. +class StateProjection : public Function { +public: + casadi_int get_n_in() override final { return 4; } + casadi_int get_n_out() override final { return 1; } + std::string get_name_in(casadi_int i) override final { + switch (i) { + case 0: return "time"; + case 1: return "multibody_states"; + case 2: return "slacks"; + case 3: return "parameters"; + default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); + } + } + std::string get_name_out(casadi_int i) override final { + switch (i) { + case 0: return "state_projection"; + default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); + } + } + casadi::Sparsity get_sparsity_in(casadi_int i) override final; + casadi::Sparsity get_sparsity_out(casadi_int i) override final; + VectorDM eval(const VectorDM& args) const override; + casadi::DM getSubsetPoint( + const VariablesDM& fullPoint, casadi_int i) const override; +}; + +template class MultibodySystemImplicit : public Function { casadi_int get_n_out() override final { return 4; } std::string get_name_out(casadi_int i) override final { @@ -313,6 +341,7 @@ class MultibodySystemImplicit : public Function { default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); } } + casadi::Sparsity get_sparsity_out(casadi_int i) override final; VectorDM eval(const VectorDM& args) const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index b5dd2ff97a..f815638e3c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -50,30 +50,21 @@ DM HermiteSimpson::createMeshIndicesImpl() const { return indices; } -void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); - - int time_i; - int time_mid; - int time_ip1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - time_i = 2 * imesh; // Needed for defects and path constraints. - // We enforce defect constraints on a mesh interval basis, so add // constraints until the number of mesh intervals is reached. - time_mid = 2 * imesh + 1; - time_ip1 = 2 * imesh + 2; - - const auto h = m_times(time_ip1) - m_times(time_i); - const auto x_i = x(Slice(), time_i); - const auto x_mid = x(Slice(), time_mid); - const auto x_ip1 = x(Slice(), time_ip1); - const auto xdot_i = xdot(Slice(), time_i); - const auto xdot_mid = xdot(Slice(), time_mid); - const auto xdot_ip1 = xdot(Slice(), time_ip1); + const auto h = m_times(2 * imesh + 2) - m_times(2 * imesh); + const auto x_i = x[imesh](Slice(), 0); + const auto x_mid = x[imesh](Slice(), 1); + const auto x_ip1 = x[imesh](Slice(), 2); + const auto xdot_i = xdot[imesh](Slice(), 0); + const auto xdot_mid = xdot[imesh](Slice(), 1); + const auto xdot_ip1 = xdot[imesh](Slice(), 2); // Hermite interpolant defects. defects(Slice(0, NS), imesh) = @@ -88,7 +79,7 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, void HermiteSimpson::calcInterpolatingControlsImpl( const casadi::MX& controls, casadi::MX& interpControls) const { if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { + m_solver.getInterpolateControlMeshInteriorPoints()) { int time_i; int time_mid; int time_ip1; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 8c7f25f428..ff11784445 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -37,9 +37,12 @@ namespace CasOC { /// /// Kinematic constraints and path constraints. /// ------------------------------------------- -/// Kinematic constraint and path constraint errors are enforced only at the -/// mesh points. Errors at collocation points at the mesh interval midpoint -/// are ignored. +/// Position- and velocity-level kinematic constraint errors and path constraint +/// errors are enforced only at the mesh points. In the kinematic constraint +/// method by Bordalba et al. (2023), the acceleration-level constraints are +/// also enforced at the collocation points. In the kinematic constraint method +/// by Posa et al. (2016), the acceleration-level constraints are only enforced +/// at the mesh points. class HermiteSimpson : public Transcription { public: HermiteSimpson(const Solver& solver, const Problem& problem) @@ -47,7 +50,8 @@ class HermiteSimpson : public Transcription { casadi::DM grid = casadi::DM::zeros(1, (2 * m_solver.getMesh().size()) - 1); const auto& mesh = m_solver.getMesh(); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); + const bool interpControls = + m_solver.getInterpolateControlMeshInteriorPoints(); casadi::DM pointsForInterpControls; if (interpControls) { pointsForInterpControls = @@ -63,15 +67,16 @@ class HermiteSimpson : public Transcription { } } } - createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), + createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), 3, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, + casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 365c604a75..f65eff500b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -39,6 +39,9 @@ enum Var { derivatives, // TODO: Rename to accelerations? /// Constant in time. parameters, + /// A "mirror" of the multibody states used in the projection method + /// for solving kinematic constraints. + projection_states, /// For internal use (never actually a key for Variables). multibody_states = 100 }; @@ -61,10 +64,12 @@ struct Iterate { std::vector slack_names; std::vector derivative_names; std::vector parameter_names; + std::vector projection_state_names; int iteration = -1; /// Return a new iterate in which the data is resampled at the times in /// newTimes. - Iterate resample(const casadi::DM& newTimes) const; + Iterate resample(const casadi::DM& newTimes, + bool appendProjectionStates) const; }; /// This struct is used to return a solution to a problem. Use `stats` diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 33db331af3..50e8a81756 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -54,18 +54,17 @@ DM LegendreGauss::createMeshIndicesImpl() const { return indices; } -void LegendreGauss::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void LegendreGauss::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * (m_degree + 1); const auto h = m_times(igrid + m_degree + 1) - m_times(igrid); - const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); - const auto xdot_i = xdot(Slice(), - Slice(igrid + 1, igrid + m_degree + 1)); - const auto x_ip1 = x(Slice(), igrid + m_degree + 1); + const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1)); + const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1)); + const auto x_ip1 = x[imesh](Slice(), m_degree + 1); // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); @@ -82,7 +81,7 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, void LegendreGauss::calcInterpolatingControlsImpl( const casadi::MX& controls, casadi::MX& interpControls) const { if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { + m_solver.getInterpolateControlMeshInteriorPoints()) { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * (m_degree + 1); const auto c_i = controls(Slice(), igrid); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 493801633f..02ac20834c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -47,9 +47,10 @@ namespace CasOC { /// /// Kinematic constraints and path constraints. /// ------------------------------------------- -/// Kinematic constraint and path constraint errors are enforced only at the -/// mesh points. Errors at collocation points within the mesh interval midpoint -/// are ignored. +/// Position- and velocity-level kinematic constraint errors and path constraint +/// errors are enforced only at the mesh points. In the kinematic constraint +/// method by Bordalba et al. [3], the acceleration-level constraints are also +/// enforced at the collocation points. /// /// References /// ---------- @@ -71,7 +72,8 @@ class LegendreGauss : public Transcription { const int numMeshIntervals = (int)mesh.size() - 1; const int numGridPoints = (int)mesh.size() + numMeshIntervals * m_degree; casadi::DM grid = casadi::DM::zeros(1, numGridPoints); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); + const bool interpControls = + m_solver.getInterpolateControlMeshInteriorPoints(); casadi::DM pointsForInterpControls; if (interpControls) { pointsForInterpControls = casadi::DM::zeros(1, @@ -105,14 +107,15 @@ class LegendreGauss : public Transcription { grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, (m_degree + 1) * m_problem.getNumStates(), + m_degree + 2, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 281a753987..915ff8ba82 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -52,17 +52,16 @@ DM LegendreGaussRadau::createMeshIndicesImpl() const { return indices; } -void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void LegendreGaussRadau::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; const auto h = m_times(igrid + m_degree) - m_times(igrid); - const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); - const auto xdot_i = xdot(Slice(), - Slice(igrid + 1, igrid + m_degree + 1)); + const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1)); + const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1)); // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); @@ -75,7 +74,7 @@ void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, void LegendreGaussRadau::calcInterpolatingControlsImpl( const casadi::MX& controls, casadi::MX& interpControls) const { if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { + m_solver.getInterpolateControlMeshInteriorPoints()) { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; const auto c_i = controls(Slice(), igrid); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 191cb3990f..ebd4fef4f7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -46,9 +46,10 @@ namespace CasOC { /// /// Kinematic constraints and path constraints. /// ------------------------------------------- -/// Kinematic constraint and path constraint errors are enforced only at the -/// mesh points. Errors at collocation points within the mesh interval midpoint -/// are ignored. +/// Position- and velocity-level kinematic constraint errors and path constraint +/// errors are enforced only at the mesh points. In the kinematic constraint +/// method by Bordalba et al. [3], the acceleration-level constraints are also +/// enforced at the collocation points. /// /// References /// ---------- @@ -71,7 +72,8 @@ class LegendreGaussRadau : public Transcription { const int numGridPoints = (int)mesh.size() + numMeshIntervals * (m_degree - 1); casadi::DM grid = casadi::DM::zeros(1, numGridPoints); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); + const bool interpControls = + m_solver.getInterpolateControlMeshInteriorPoints(); casadi::DM pointsForInterpControls; if (interpControls) { pointsForInterpControls = casadi::DM::zeros(1, @@ -107,14 +109,15 @@ class LegendreGaussRadau : public Transcription { grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, m_degree * m_problem.getNumStates(), + m_degree + 1, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp index 740cf59782..172aa879eb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp @@ -31,14 +31,16 @@ using OpenSim::Exception; namespace CasOC { -Iterate Iterate::resample(const casadi::DM& newTimes) const { +Iterate Iterate::resample(const casadi::DM& newTimes, + bool appendProjectionStates = false) const { // Since we are converting to a MocoTrajectory and immediately converting // back to a CasOC::Iterate after resampling, we do not need to provide // Input control indexes, even if they are present in the MocoProblem. auto mocoTraj = OpenSim::convertToMocoTrajectory(*this); auto simtkNewTimes = OpenSim::convertToSimTKVector(newTimes); mocoTraj.resample(simtkNewTimes); - return OpenSim::convertToCasOCIterate(mocoTraj); + return OpenSim::convertToCasOCIterate(mocoTraj, mocoTraj.getSlackNames(), + appendProjectionStates); } std::vector diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h index 6d729123a6..94fc3049fd 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h @@ -22,7 +22,6 @@ #include "CasOCFunction.h" #include #include -#include namespace OpenSim { class MocoCasADiSolver; @@ -208,15 +207,17 @@ class Problem { std::move(multInitialBounds), std::move(multFinalBounds), kinLevel}); - if (kinLevel == KinematicLevel::Position) - ++m_numHolonomicConstraintEquations; - else if (kinLevel == KinematicLevel::Velocity) - ++m_numNonHolonomicConstraintEquations; - else if (kinLevel == KinematicLevel::Acceleration) - ++m_numAccelerationConstraintEquations; - } - /// Add a slack velocity correction variable to the problem associated with - /// a kinematic constraint in the model. + if (!isPrescribedKinematics()) { + if (kinLevel == KinematicLevel::Position) + ++m_numHolonomicConstraintEquations; + else if (kinLevel == KinematicLevel::Velocity) + ++m_numNonHolonomicConstraintEquations; + else if (kinLevel == KinematicLevel::Acceleration) + ++m_numAccelerationConstraintEquations; + } + } + /// Add a slack variable to the problem associated with a kinematic + /// constraint in the model. void addSlack(std::string name, Bounds bounds) { m_slackInfos.push_back({std::move(name), std::move(bounds)}); } @@ -240,6 +241,15 @@ class Problem { void setKinematicConstraintBounds(Bounds bounds) { m_kinematicConstraintBounds = std::move(bounds); } + /// Set the method used to enforce kinematic constraints. + void setKinematicConstraintMethod(std::string method) { + OPENSIM_THROW_IF(method != "Bordalba2023" && method != "Posa2016", + OpenSim::Exception, + "Invalid method kinematic constraint method.") + m_kinematicConstraintMethod = std::move(method); + m_isKinematicConstraintMethodBordalba2023 = + m_kinematicConstraintMethod == "Bordalba2023"; + } /// Add a constant (time-invariant) variable to the optimization problem. void addParameter(std::string name, Bounds bounds) { m_paramInfos.push_back({std::move(name), std::move(bounds)}); @@ -316,7 +326,10 @@ class Problem { const casadi::DM& multibody_states, const casadi::DM& slacks, const casadi::DM& parameters, casadi::DM& velocity_correction) const = 0; - + virtual void calcStateProjection(const double& time, + const casadi::DM& multibody_states, const casadi::DM& slacks, + const casadi::DM& parameters, + casadi::DM& projection) const = 0; virtual void calcCostIntegrand(int /*costIndex*/, const ContinuousInput& /*input*/, double& /*integrand*/) const {} virtual void calcCost(int /*costIndex*/, const CostInput& /*input*/, @@ -370,10 +383,31 @@ class Problem { } } } + if (getNumKinematicConstraintEquations() && + isKinematicConstraintMethodBordalba2023()) { + for (const auto& info : m_stateInfos) { + if (info.type == StateType::Coordinate) { + auto name = info.name; + auto leafpos = name.find("/value"); + OPENSIM_THROW_IF(leafpos == std::string::npos, + OpenSim::Exception, "Internal error."); + name.replace(leafpos, 6, "/value/projection"); + it.projection_state_names.push_back(name); + } + if (info.type == StateType::Speed) { + auto name = info.name; + auto leafpos = name.find("/speed"); + OPENSIM_THROW_IF(leafpos == std::string::npos, + OpenSim::Exception, "Internal error."); + name.replace(leafpos, 6, "/speed/projection"); + it.projection_state_names.push_back(name); + } + } + } for (const auto& auxDerivName : m_auxiliaryDerivativeNames) { it.derivative_names.push_back(auxDerivName); } - + for (const auto& info : m_paramInfos) it.parameter_names.push_back(info.name); return it; @@ -456,12 +490,21 @@ class Problem { pointsForSparsityDetection); } - if (m_enforceConstraintDerivatives) { - mutThis->m_velocityCorrectionFunc = + if (getEnforceConstraintDerivatives() && + getNumKinematicConstraintEquations()) { + if (isKinematicConstraintMethodBordalba2023()) { + mutThis->m_stateProjectionFunc = + OpenSim::make_unique(); + mutThis->m_stateProjectionFunc->constructFunction(this, + "state_projection", finiteDiffScheme, + pointsForSparsityDetection); + } else { + mutThis->m_velocityCorrectionFunc = OpenSim::make_unique(); - mutThis->m_velocityCorrectionFunc->constructFunction(this, + mutThis->m_velocityCorrectionFunc->constructFunction(this, "velocity_correction", finiteDiffScheme, pointsForSparsityDetection); + } } } @@ -476,6 +519,9 @@ class Problem { int getNumDerivatives() const { return getNumAccelerations() + getNumAuxiliaryResidualEquations(); } + int getNumProjectionStates() const { + return getNumProjectionConstraintEquations(); + } int getNumSlacks() const { return (int)m_slackInfos.size(); } /// This is the number of generalized coordinates, which may be greater /// than the number of generalized speeds. @@ -488,6 +534,7 @@ class Problem { return 0; } } + int getNumMultibodyStates() const { return m_numCoordinates + m_numSpeeds; } int getNumAuxiliaryStates() const { return m_numAuxiliaryStates; } int getNumCosts() const { return (int)m_costInfos.size(); } bool isPrescribedKinematics() const { return m_prescribedKinematics; } @@ -505,19 +552,32 @@ class Problem { int getNumAuxiliaryResidualEquations() const { return m_numAuxiliaryResiduals; } - int getNumKinematicConstraintEquations() const { + int getNumQErr() const { // If all kinematics are prescribed, we assume that the prescribed // kinematics obey any kinematic constraints. Therefore, the kinematic // constraints would be redundant, and we need not enforce them. + if (m_prescribedKinematics) return 0; + return m_numHolonomicConstraintEquations; + } + int getNumUErr() const { if (m_prescribedKinematics) return 0; if (m_enforceConstraintDerivatives) { - return 3 * m_numHolonomicConstraintEquations + - 2 * m_numNonHolonomicConstraintEquations + + return m_numHolonomicConstraintEquations + + m_numNonHolonomicConstraintEquations; + } + return m_numNonHolonomicConstraintEquations; + } + int getNumUDotErr() const { + if (m_prescribedKinematics) return 0; + if (m_enforceConstraintDerivatives) { + return m_numHolonomicConstraintEquations + + m_numNonHolonomicConstraintEquations + m_numAccelerationConstraintEquations; } - return m_numHolonomicConstraintEquations + - m_numNonHolonomicConstraintEquations + - m_numAccelerationConstraintEquations; + return m_numAccelerationConstraintEquations; + } + int getNumKinematicConstraintEquations() const { + return getNumQErr() + getNumUErr() + getNumUDotErr(); } /// Create a vector of names for scalar kinematic constraint equations. /// The length of the vector is getNumKinematicConstraintEquations(). @@ -546,6 +606,21 @@ class Problem { const Bounds& getKinematicConstraintBounds() const { return m_kinematicConstraintBounds; } + std::string getKinematicConstraintMethod() const { + return m_kinematicConstraintMethod; + } + bool isKinematicConstraintMethodBordalba2023() const { + return m_isKinematicConstraintMethodBordalba2023; + } + int getNumProjectionConstraintEquations() const { + if ((getNumQErr() + getNumUErr()) && + isKinematicConstraintMethodBordalba2023()) { + return getNumMultibodyStates(); + } else { + return 0; + } + + } const Bounds& getTimeInitialBounds() const { return m_timeInitialBounds; } const Bounds& getTimeFinalBounds() const { return m_timeFinalBounds; } const std::vector& getStateInfos() const { return m_stateInfos; } @@ -580,12 +655,18 @@ class Problem { return *m_multibodyFuncIgnoringConstraints; } /// Get a function to compute the velocity correction to qdot when enforcing - /// kinematic constraints and their derivatives. We require a separate - /// function for this since we don't actually compute qdot within the - /// multibody system. + /// kinematic constraints and their derivatives using the method by + /// Posa et al. (2016). We require a separate function for this since we don't + /// actually compute qdot within the multibody system. const casadi::Function& getVelocityCorrection() const { return *m_velocityCorrectionFunc; } + /// Get a function to compute the state projection term when enforcing + /// kinematic constraints and their derivatives using the method by + /// Bordalba et al. (2023). + const casadi::Function& getStateProjection() const { + return *m_stateProjectionFunc; + } const casadi::Function& getImplicitMultibodySystem() const { return *m_implicitMultibodyFunc; } @@ -614,8 +695,10 @@ class Problem { int m_numAccelerationConstraintEquations = 0; bool m_enforceConstraintDerivatives = false; std::string m_dynamicsMode = "explicit"; + std::string m_kinematicConstraintMethod = "Posa2016"; std::vector m_auxiliaryDerivativeNames; bool m_isDynamicsModeImplicit = false; + bool m_isKinematicConstraintMethodBordalba2023 = false; bool m_prescribedKinematics = false; int m_numMultibodyDynamicsEquationsIfPrescribedKinematics = 0; Bounds m_kinematicConstraintBounds; @@ -633,6 +716,7 @@ class Problem { std::unique_ptr> m_implicitMultibodyFuncIgnoringConstraints; std::unique_ptr m_velocityCorrectionFunc; + std::unique_ptr m_stateProjectionFunc; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp index 7bc3f4a456..4ed090fbff 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp @@ -125,7 +125,10 @@ Solution Solver::solve(const Iterate& guess) const { const auto guessTimes = transcription->createTimes(guessCopy.variables.at(initial_time), guessCopy.variables.at(final_time)); - guessCopy = guessCopy.resample(guessTimes); + bool appendProjectionStates = + m_problem.getNumKinematicConstraintEquations() && + m_problem.isKinematicConstraintMethodBordalba2023(); + guessCopy = guessCopy.resample(guessTimes, appendProjectionStates); pointsForSparsityDetection->push_back(guessCopy.variables); } else if (m_sparsity_detection == "random") { // Make sure the exact same sparsity pattern is used every time. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h index ec0951dc04..37464a0505 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h @@ -104,26 +104,48 @@ class Solver { void setImplicitAuxiliaryDerivativesWeight(double weight) { m_implicitAuxiliaryDerivativesWeight = weight; } - - /// Whether or not to constrain control values at mesh interval midpoints - /// by linearly interpolating control values from mesh interval endpoints. - /// @note Only applies to Hermite-Simpson collocation. - void setInterpolateControlMidpoints(bool tf) { - m_interpolateControlMidpoints = tf; + bool getMinimizeStateProjection() const { + return m_minimizeStateProjection; + } + void setMinimizeStateProjection(bool tf) { + m_minimizeStateProjection = tf; } - bool getInterpolateControlMidpoints() const { - return m_interpolateControlMidpoints; + double getStateProjectionWeight() const { + return m_stateProjectionWeight; + } + void setStateProjectionWeight(double weight) { + m_stateProjectionWeight = weight; } - /// Whether or not to enforce path constraints at mesh interval midpoints. - /// @note Only applies to Hermite-Simpson collocation. + /// Whether or not to constrain control values interior to the mesh interval + /// by linearly interpolating control values from mesh interval endpoints. + /// + /// @note For Hermite-Simpson collocation, this applies to the time point + /// at the midpoint of the mesh interval. For Legendre-Gauss and + /// Legendre-Gauss-Radau collocation, this applies to all time points + /// in the interior of the mesh interval. + void setInterpolateControlMeshInteriorPoints(bool tf) { + m_interpolateControlMeshInteriorPoints = tf; + } + /// @copydoc setInterpolateControlMidpoints() + bool getInterpolateControlMeshInteriorPoints() const { + return m_interpolateControlMeshInteriorPoints; + } + + /// Whether or not to enforce path constraints at points interior to the + /// mesh interval. + /// + /// @note For Hermite-Simpson collocation, this applies to the time point + /// at the midpoint of the mesh interval. For Legendre-Gauss and + /// Legendre-Gauss-Radau collocation, this applies to all time points + /// in the interior of the mesh interval. /// @note Does not apply to implicit dynamics residuals, as these are /// always enforced at mesh interval midpoints. - void setEnforcePathConstraintMidpoints(bool tf) { - m_enforcePathConstraintMidpoints = tf; + void setEnforcePathConstraintMeshInteriorPoints(bool tf) { + m_enforcePathConstraintMeshInteriorPoints = tf; } - bool getEnforcePathConstraintMidpoints() const { - return m_enforcePathConstraintMidpoints; + bool getEnforcePathConstraintMeshInteriorPoints() const { + return m_enforcePathConstraintMeshInteriorPoints; } void setOptimSolver(std::string optimSolver) { @@ -201,8 +223,10 @@ class Solver { double m_implicitMultibodyAccelerationsWeight = 1.0; bool m_minimizeImplicitAuxiliaryDerivatives = false; double m_implicitAuxiliaryDerivativesWeight = 1.0; - bool m_interpolateControlMidpoints = true; - bool m_enforcePathConstraintMidpoints = false; + bool m_minimizeStateProjection = true; + double m_stateProjectionWeight = 1e-6; + bool m_interpolateControlMeshInteriorPoints = true; + bool m_enforcePathConstraintMeshInteriorPoints = false; Bounds m_implicitMultibodyAccelerationBounds; Bounds m_implicitAuxiliaryDerivativeBounds; std::string m_finite_difference_scheme = "central"; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index a2738647a7..08adbe140b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -1,9 +1,9 @@ /* -------------------------------------------------------------------------- * * OpenSim Moco: CasOCTranscription.cpp * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -82,6 +82,7 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numPointsPerMeshInterval, const casadi::DM& pointsForInterpControls) { // Set the grid. // ------------- @@ -94,18 +95,26 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numMeshIntervals = m_numMeshPoints - 1; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; + m_numPointsPerMeshInterval = numPointsPerMeshInterval; m_pointsForInterpControls = pointsForInterpControls; m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() : 0; m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); + m_numProjectionStates = m_problem.getNumProjectionConstraintEquations(); + m_numUDotErrorPoints = m_problem.isKinematicConstraintMethodBordalba2023() + ? m_numGridPoints + : m_numMeshPoints; m_numConstraints = m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + - m_problem.getNumKinematicConstraintEquations() * m_numMeshPoints + - m_problem.getNumControls() * (int)pointsForInterpControls.numel(); + m_problem.getNumQErr() * m_numMeshPoints + + m_problem.getNumUErr() * m_numMeshPoints + + m_problem.getNumUDotErr() * m_numUDotErrorPoints + + m_problem.getNumControls() * (int)pointsForInterpControls.numel() + + m_problem.getNumProjectionConstraintEquations() * m_numMeshIntervals; m_constraints.endpoint.resize( m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { @@ -113,8 +122,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numConstraints += info.num_outputs; } m_constraints.path.resize(m_problem.getPathConstraintInfos().size()); - m_numPathConstraintPoints = m_solver.getEnforcePathConstraintMidpoints() - ? m_numGridPoints : m_numMeshPoints; + m_numPathConstraintPoints = + m_solver.getEnforcePathConstraintMeshInteriorPoints() + ? m_numGridPoints : m_numMeshPoints; for (int ipc = 0; ipc < (int)m_constraints.path.size(); ++ipc) { const auto& info = m_problem.getPathConstraintInfos()[ipc]; m_numConstraints += info.size() * m_numPathConstraintPoints; @@ -127,28 +137,60 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVars[final_time] = MX::sym("final_time"); m_scaledVars[states] = MX::sym("states", m_problem.getNumStates(), m_numGridPoints); + m_scaledVars[projection_states] = MX::sym( + "projection_states", m_numProjectionStates, m_numMeshIntervals); m_scaledVars[controls] = MX::sym("controls", m_problem.getNumControls(), m_numGridPoints); m_scaledVars[multipliers] = MX::sym( "multipliers", m_problem.getNumMultipliers(), m_numGridPoints); m_scaledVars[derivatives] = MX::sym( "derivatives", m_problem.getNumDerivatives(), m_numGridPoints); - - // TODO: This assumes that slack variables are applied at all - // collocation points on the mesh interval interior. - m_scaledVars[slacks] = MX::sym( - "slacks", m_problem.getNumSlacks(), m_numMeshInteriorPoints); m_scaledVars[parameters] = MX::sym("parameters", m_problem.getNumParameters(), 1); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + // In the projection method for enforcing kinematic constraints, the + // slack variables are applied at the mesh points at the end of each + // mesh interval. + m_scaledVars[slacks] = MX::sym( + "slacks", m_problem.getNumSlacks(), m_numMeshIntervals); + } else { + // In the Posa et al. 2016 method for enforcing kinematic constraints, + // the mesh interior points will be the mesh interval midpoints in the + // Hermite-Simpson collocation scheme. + m_scaledVars[slacks] = MX::sym( + "slacks", m_problem.getNumSlacks(), m_numMeshInteriorPoints); + } + + // Each vector contains MX matrix elements (states or state derivatives) + // needed to construct the defect constraints for an individual mesh + // interval. + m_statesByMeshInterval = MXVector(m_numMeshIntervals); + m_stateDerivativesByMeshInterval = MXVector(m_numMeshIntervals); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + m_statesByMeshInterval[imesh] = + MX(m_problem.getNumStates(), m_numPointsPerMeshInterval); + m_stateDerivativesByMeshInterval[imesh] = + MX(m_problem.getNumStates(), m_numPointsPerMeshInterval); + } + m_projectionStateDistances = MX(m_numProjectionStates, m_numMeshIntervals); + m_meshIndicesMap = createMeshIndices(); std::vector meshIndicesVector; std::vector meshInteriorIndicesVector; + std::vector projectionStateIndicesVector; + std::vector notProjectionStateIndicesVector; for (int i = 0; i < m_meshIndicesMap.size2(); ++i) { if (m_meshIndicesMap(i).scalar() == 1) { meshIndicesVector.push_back(i); + if (i == 0) { + notProjectionStateIndicesVector.push_back(i); + } else { + projectionStateIndicesVector.push_back(i); + } } else { meshInteriorIndicesVector.push_back(i); + notProjectionStateIndicesVector.push_back(i); } } @@ -165,9 +207,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_meshIndices = makeTimeIndices(meshIndicesVector); m_meshInteriorIndices = makeTimeIndices(meshInteriorIndicesVector); - m_pathConstraintIndices = m_solver.getEnforcePathConstraintMidpoints() - ? makeTimeIndices(gridIndicesVector) - : makeTimeIndices(meshIndicesVector); + m_pathConstraintIndices = + m_solver.getEnforcePathConstraintMeshInteriorPoints() + ? makeTimeIndices(gridIndicesVector) + : makeTimeIndices(meshIndicesVector); + m_projectionStateIndices = makeTimeIndices(projectionStateIndicesVector); + m_notProjectionStateIndices = + makeTimeIndices(notProjectionStateIndicesVector); // Set variable bounds. // -------------------- @@ -246,11 +292,11 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, if (m_problem.getNumAuxiliaryResidualEquations()) { setVariableBounds(derivatives, Slice(m_problem.getNumAccelerations(), - m_problem.getNumDerivatives()), + m_problem.getNumDerivatives()), Slice(), m_solver.getImplicitAuxiliaryDerivativeBounds()); setVariableScaling(derivatives, Slice(m_problem.getNumAccelerations(), - m_problem.getNumDerivatives()), + m_problem.getNumDerivatives()), Slice(), m_solver.getImplicitAuxiliaryDerivativeBounds()); } } @@ -263,6 +309,20 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, ++isl; } } + { + // Use the bounds from the "true" multibody states to set the bounds for + // the projection states. + const auto& stateInfos = m_problem.getStateInfos(); + for (int ips = 0; ips < m_numProjectionStates; ++ips) { + const auto& info = stateInfos[ips]; + setVariableBounds( + projection_states, ips, Slice(0, m_numMeshIntervals - 1), + info.bounds); + setVariableBounds(projection_states, ips, -1, info.finalBounds); + setVariableScaling(projection_states, Slice(), Slice(), + info.bounds); + } + } { const auto& paramInfos = m_problem.getParameterInfos(); int ip = 0; @@ -281,11 +341,49 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, MX::repmat(m_unscaledVars[parameters], 1, m_numGridPoints); m_paramsTrajMesh = MX::repmat(m_unscaledVars[parameters], 1, m_numMeshPoints); - m_paramsTrajMeshInterior = MX::repmat(m_unscaledVars[parameters], 1, - m_numMeshInteriorPoints); + m_paramsTrajMeshInterior = + MX::repmat(m_unscaledVars[parameters], 1, m_numMeshInteriorPoints); m_paramsTrajPathCon = - MX::repmat(m_unscaledVars[parameters], 1, - m_numPathConstraintPoints); + MX::repmat(m_unscaledVars[parameters], 1, m_numPathConstraintPoints); + m_paramsTrajProjState = + MX::repmat(m_unscaledVars[parameters], 1, m_numMeshIntervals); + + casadi_int istart = 0; + int numStates = m_problem.getNumStates(); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + casadi_int numPts = m_numPointsPerMeshInterval; + casadi_int iend = istart + numPts - 1; + if (m_numProjectionStates) { + // The states at all points in the mesh interval except the last + // point are the regular state variables. + m_statesByMeshInterval[imesh](Slice(), Slice(0, numPts-1)) = + m_unscaledVars[states](Slice(), Slice(istart, iend)); + + // The multibody states at the last point in the mesh interval are + // the projection states. + m_statesByMeshInterval[imesh] + (Slice(0, m_numProjectionStates), numPts-1) = + m_unscaledVars[projection_states](Slice(), imesh); + + // The non-multibody states at the last point (i.e., auxiliary state + // variables for muscles) are also the same as the regular state + // variables (there are no projection states for these variables). + m_statesByMeshInterval[imesh]( + Slice(m_numProjectionStates, numStates), numPts-1) = + m_unscaledVars[states]( + Slice(m_numProjectionStates, numStates), iend); + + // Calculate the distance between the regular multibody states and + // the projection multibody states. + m_projectionStateDistances(Slice(), imesh) = + m_unscaledVars[projection_states](Slice(), imesh) - + m_unscaledVars[states](Slice(0, m_numProjectionStates), iend); + } else { + m_statesByMeshInterval[imesh](Slice(), Slice()) = + m_unscaledVars[states](Slice(), Slice(istart, iend+1)); + } + istart = iend; + } } void Transcription::transcribe() { @@ -308,6 +406,7 @@ void Transcription::transcribe() { // Initialize memory for state derivatives and defects. // ---------------------------------------------------- m_xdot = MX(NS, m_numGridPoints); + m_xdot_projection = MX(m_numProjectionStates, m_numMeshIntervals); m_constraints.defects = MX(casadi::Sparsity::dense( m_numDefectsPerMeshInterval, m_numMeshIntervals)); m_constraintsLowerBounds.defects = @@ -335,48 +434,115 @@ void Transcription::transcribe() { // Initialize memory for kinematic constraints. // -------------------------------------------- - int numKinematicConstraints = - m_problem.getNumKinematicConstraintEquations(); - m_constraints.kinematic = MX( - casadi::Sparsity::dense(numKinematicConstraints, m_numMeshPoints)); + int nqerr = m_problem.getNumQErr(); + int nuerr = m_problem.getNumUErr(); + int nudoterr = m_problem.getNumUDotErr(); + int nkc = m_problem.getNumKinematicConstraintEquations(); const auto& kcBounds = m_problem.getKinematicConstraintBounds(); - m_constraintsLowerBounds.kinematic = casadi::DM::repmat( - kcBounds.lower, numKinematicConstraints, m_numMeshPoints); - m_constraintsUpperBounds.kinematic = casadi::DM::repmat( - kcBounds.upper, numKinematicConstraints, m_numMeshPoints); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic = MX(casadi::Sparsity::dense( + nqerr + nuerr, m_numMeshPoints)); + m_constraintsLowerBounds.kinematic = casadi::DM::repmat( + kcBounds.lower, nqerr + nuerr, m_numMeshPoints); + m_constraintsUpperBounds.kinematic = casadi::DM::repmat( + kcBounds.upper, nqerr + nuerr, m_numMeshPoints); + + m_constraints.kinematic_udoterr = MX(casadi::Sparsity::dense( + nudoterr, m_numUDotErrorPoints)); + m_constraintsLowerBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.lower, nudoterr, m_numUDotErrorPoints); + m_constraintsUpperBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.upper, nudoterr, m_numUDotErrorPoints); + } else { + m_constraints.kinematic = MX(casadi::Sparsity::dense( + nkc, m_numMeshPoints)); + m_constraintsLowerBounds.kinematic = casadi::DM::repmat( + kcBounds.lower, nkc, m_numMeshPoints); + m_constraintsUpperBounds.kinematic = casadi::DM::repmat( + kcBounds.upper, nkc, m_numMeshPoints); + } + + // Initialize memory for projection constraints. + // --------------------------------------------- + int numProjectionConstraints = + m_problem.getNumProjectionConstraintEquations(); + m_constraints.projection = MX( + casadi::Sparsity::dense(numProjectionConstraints, + m_numMeshIntervals)); + m_constraintsLowerBounds.projection = + DM::zeros(numProjectionConstraints, m_numMeshIntervals); + m_constraintsUpperBounds.projection = + DM::zeros(numProjectionConstraints, m_numMeshIntervals); // qdot // ---- const MX u = m_unscaledVars[states](Slice(NQ, NQ + NU), Slice()); m_xdot(Slice(0, NQ), Slice()) = u; + if (m_numProjectionStates) { + const MX u_projection = m_unscaledVars[projection_states] + (Slice(NQ, NQ + NU), Slice()); + m_xdot_projection(Slice(0, NQ), Slice()) = u_projection; + } - if (m_problem.getEnforceConstraintDerivatives() && - m_numMeshInteriorPoints && - !m_problem.isPrescribedKinematics()) { - // In Hermite-Simpson, we must compute a velocity correction at all mesh - // interval midpoints and update qdot. See MocoCasADiVelocityCorrection - // for more details. This function only takes multibody state variables: - // coordinates and speeds. - // TODO: The points at which we apply the velocity correction - // are correct for Trapezoidal (no points) and Hermite-Simpson (mesh - // interval midpoints), but might not be correct in general. Revisit - // this if we add other transcription schemes. - const auto velocityCorrectionOut = evalOnTrajectory( - m_problem.getVelocityCorrection(), {multibody_states, slacks}, - m_meshInteriorIndices); - const auto u_correction = velocityCorrectionOut.at(0); - - m_xdot(Slice(0, NQ), m_meshInteriorIndices) += u_correction; + // projection constraints + // ---------------------- + if (m_problem.getNumKinematicConstraintEquations() && + !m_problem.isPrescribedKinematics() && + m_problem.getEnforceConstraintDerivatives()) { + + OPENSIM_THROW_IF(!m_problem.isKinematicConstraintMethodBordalba2023() && + m_solver.getTranscriptionScheme() != "hermite-simpson", + OpenSim::Exception, + "Expected the 'hermite-simpson' transcription scheme when using " + "the Posa et al. 2016 method for enforcing kinematic constraints, " + "but the '{}' scheme was selected.", + m_solver.getTranscriptionScheme()); + + if (m_solver.getTranscriptionScheme() == "hermite-simpson" && + !m_problem.isKinematicConstraintMethodBordalba2023()) { + // In Hermite-Simpson, we must compute a velocity correction at all + // mesh interval midpoints and update qdot. + // See MocoCasADiVelocityCorrection for more details. This function + // only takes multibody state variables: coordinates and speeds. + const auto velocityCorrectionOut = evalOnTrajectory( + m_problem.getVelocityCorrection(), + {multibody_states, slacks}, + m_meshInteriorIndices); + const auto u_correction = velocityCorrectionOut.at(0); + + m_xdot(Slice(0, NQ), m_meshInteriorIndices) += u_correction; + } + + if (m_numProjectionStates) { + const auto projectionOut = evalOnTrajectory( + m_problem.getStateProjection(), + {multibody_states, slacks}, + m_projectionStateIndices); + const auto x_proj = projectionOut.at(0); + + const MX x = m_unscaledVars[states](Slice(0, NQ + NU), + m_projectionStateIndices); + const MX x_prime = m_unscaledVars[projection_states]; + + // Bordalba et al. (2023) projection equation: x = x' + F_x^T mu. + m_constraints.projection = x - x_prime - x_proj; + } } // udot, zdot, residual, kcerr // --------------------------- if (m_problem.isDynamicsModeImplicit()) { // udot. - const MX w = m_unscaledVars[derivatives](Slice(0, m_problem.getNumSpeeds()), - Slice()); + const MX w = m_unscaledVars[derivatives] + (Slice(0, m_problem.getNumSpeeds()), Slice()); m_xdot(Slice(NQ, NQ + NU), Slice()) = w; + if (m_numProjectionStates) { + const MX w_projection = m_unscaledVars[derivatives] + (Slice(0, m_problem.getNumSpeeds()), + m_projectionStateIndices); + m_xdot_projection(Slice(NQ, NQ + NU), Slice()) = w_projection; + } std::vector inputs{states, controls, multipliers, derivatives}; @@ -398,14 +564,28 @@ void Transcription::transcribe() { m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = out.at(2); - m_constraints.kinematic = out.at(3); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( + Slice(0, nqerr), Slice()); + m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) + = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); + m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = + out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } else { + m_constraints.kinematic = out.at(3); + } } // Points where we ignore algebraic constraints. if (m_numMeshInteriorPoints) { - const auto out = evalOnTrajectory( - m_problem.getImplicitMultibodySystemIgnoringConstraints(), - inputs, m_meshInteriorIndices); + const casadi::Function& implicitMultibodyFunction = + m_problem.isKinematicConstraintMethodBordalba2023() ? + m_problem.getImplicitMultibodySystem() : + m_problem.getImplicitMultibodySystemIgnoringConstraints(); + const auto out = evalOnTrajectory(implicitMultibodyFunction, inputs, + m_meshInteriorIndices); m_constraints.multibody_residuals(Slice(), m_meshInteriorIndices) = out.at(0); // zdot. @@ -413,6 +593,34 @@ void Transcription::transcribe() { out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) + = out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } + + // Points where the multibody residuals depend on the projection states. + if (m_numProjectionStates) { + const casadi::Function& implicitMultibodyFunction = + m_problem.getImplicitMultibodySystem(); + const auto out = evalOnTrajectory(implicitMultibodyFunction, + {projection_states, controls, multipliers, derivatives}, + m_projectionStateIndices); + // This overrides the previous function evaluation assignments for + // `kinematic_udoterr` and `multibody_residuals` at the mesh indices + // (i.e., for "points where we compute algebraic constraints"). We + // still need the function evaluations above since we need state + // derivatives for auxiliary states on the mesh points (note that + // the projection states overlap with all the mesh indices except + // the first mesh point). This also keeps the implementation above + // general when we do not have projection states. + m_constraints.multibody_residuals( + Slice(), m_projectionStateIndices) = out.at(0); + m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) + = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); } } else { // Explicit dynamics mode. @@ -429,13 +637,30 @@ void Transcription::transcribe() { m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = out.at(2); - m_constraints.kinematic = out.at(3); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( + Slice(0, nqerr), Slice()); + m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) + = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); + m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = + out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } else { + m_constraints.kinematic = out.at(3); + } } // Points where we ignore algebraic constraints. if (m_numMeshInteriorPoints) { - const auto out = evalOnTrajectory( - m_problem.getMultibodySystemIgnoringConstraints(), inputs, + // In the Bordalba et al. 2023 method, we need to enforce the + // acceleration-level kinematic constraints at the mesh interval + // interior points. + const casadi::Function& multibodyFunction = + m_problem.isKinematicConstraintMethodBordalba2023() ? + m_problem.getMultibodySystem() : + m_problem.getMultibodySystemIgnoringConstraints(); + const auto out = evalOnTrajectory(multibodyFunction, inputs, m_meshInteriorIndices); m_xdot(Slice(NQ, NQ + NU), m_meshInteriorIndices) = out.at(0); @@ -443,7 +668,69 @@ void Transcription::transcribe() { out.at(1); m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = out.at(2); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) + = out.at(3)( + Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } + + // Points where the state derivatives depend on the projection states. + if (m_numProjectionStates) { + const auto out = evalOnTrajectory(m_problem.getMultibodySystem(), + {projection_states, controls, multipliers, derivatives}, + m_projectionStateIndices); + // This state derivative is not used by Legendre-Gauss collocation + // since there is no collocation point at mesh interval endpoint. + // However, we need this function evaluation, since we still enforce + // the acceleration-level kinematic constraints at the mesh interval + // endpoints. + m_xdot_projection(Slice(NQ, NQ + NU), Slice()) = out.at(0); + // This overrides the previous function evaluation assignments for + // `kinematic_udoterr` at the mesh indices (i.e., for "points where + // we compute algebraic constraints"). We still need the function + // evaluations above since we need state derivatives for auxiliary + // states on the mesh points (note that the projection states + // overlap with all the mesh indices except the first mesh point). + // This also keeps the implementation above general when we do not + // have projection states. + m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) + = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), + Slice()); + } + } + + casadi_int istart = 0; + int numStates = m_problem.getNumStates(); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + casadi_int numPts = m_numPointsPerMeshInterval; + casadi_int iend = istart + numPts - 1; + if (m_numProjectionStates) { + // The state derivatives at all points in the mesh interval except + // the last point are the regular state derivatives. + m_stateDerivativesByMeshInterval[imesh] + (Slice(), Slice(0, numPts-1)) = + m_xdot(Slice(), Slice(istart, iend)); + + // The multibody state derivatives at the last point in the mesh + // interval are the projection state derivatives. + m_stateDerivativesByMeshInterval[imesh] + (Slice(0, m_numProjectionStates), numPts-1) = + m_xdot_projection(Slice(), imesh); + + // The non-multibody state derivatives at the last point (i.e., + // auxiliary state derivatives for muscles) are also the same as the + // regular state derivatives (there are no projection derivatives + // for these variables). + m_stateDerivativesByMeshInterval[imesh]( + Slice(m_numProjectionStates, numStates), numPts-1) = + m_xdot(Slice(m_numProjectionStates, numStates), iend); + } else { + m_stateDerivativesByMeshInterval[imesh](Slice(), Slice()) = + m_xdot(Slice(), Slice(istart, iend+1)); } + istart = iend; } // Calculate defects. @@ -513,6 +800,13 @@ void Transcription::setObjectiveAndEndpointConstraints() { if (minimizeAuxiliaryDerivatives) { m_objectiveTermNames.push_back("auxiliary_derivatives"); } + bool minimizeStateProjection = + m_solver.getMinimizeStateProjection() && + m_numProjectionStates && + m_problem.isKinematicConstraintMethodBordalba2023(); + if (minimizeStateProjection) { + m_objectiveTermNames.push_back("state_projection"); + } m_objectiveTerms = MX::zeros((int)m_objectiveTermNames.size(), 1); int iterm = 0; @@ -547,7 +841,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), - m_unscaledVars[parameters], + m_unscaledVars[parameters], integral}, costOut); m_objectiveTerms(iterm++) = casadi::MX::sum1(costOut.at(0)); @@ -566,7 +860,8 @@ void Transcription::setObjectiveAndEndpointConstraints() { // Minimize generalized accelerations. if (minimizeAccelerations) { const auto& numAccels = m_problem.getNumAccelerations(); - const auto accels = m_scaledVars[derivatives](Slice(0, numAccels), Slice()); + const auto accels = m_scaledVars[derivatives]( + Slice(0, numAccels), Slice()); const double accelWeight = m_solver.getImplicitMultibodyAccelerationsWeight(); MX integrandTraj = MX::sum1(MX::sq(accels)); @@ -587,6 +882,11 @@ void Transcription::setObjectiveAndEndpointConstraints() { dot(quadCoeffs.T(), integrandTraj); } + // Minimize state projection distance. + if (minimizeStateProjection && m_numProjectionStates) { + m_objectiveTerms(iterm++) = m_solver.getStateProjectionWeight() * + MX::sum2(MX::sum1(MX::sq(m_projectionStateDistances))); + } // Endpoint constraints // -------------------- @@ -640,7 +940,9 @@ Solution Transcription::solve(const Iterate& guessOrig) { // ------------------- const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), guessOrig.variables.at(final_time)); - auto guess = guessOrig.resample(guessTimes); + bool appendProjectionStates = + m_problem.getNumProjectionConstraintEquations(); + auto guess = guessOrig.resample(guessTimes, appendProjectionStates); // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). @@ -653,9 +955,18 @@ Solution Transcription::solve(const Iterate& guessOrig) { if (slacks.size2() == m_numGridPoints) { casadi::DM meshIndices = createMeshIndices(); std::vector slackColumnsToRemove; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + slackColumnsToRemove.push_back(0); + } for (int itime = 0; itime < m_numGridPoints; ++itime) { - if (meshIndices(itime).__nonzero__()) { - slackColumnsToRemove.push_back(itime); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + if (!meshIndices(itime).__nonzero__()) { + slackColumnsToRemove.push_back(itime); + } + } else { + if (meshIndices(itime).__nonzero__()) { + slackColumnsToRemove.push_back(itime); + } } } // The first argument is an empty vector since we don't want to @@ -666,11 +977,44 @@ Solution Transcription::solve(const Iterate& guessOrig) { // Check that either that the slack variables provided in the guess // are the correct length, or that the correct number of columns // were removed. - OPENSIM_THROW_IF(slacks.size2() != m_numMeshInteriorPoints, + int slackLength; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + slackLength = m_numMeshIntervals; + } else { + slackLength = m_numMeshInteriorPoints; + } + OPENSIM_THROW_IF(slacks.size2() != slackLength, OpenSim::Exception, "Expected slack variables to be length {}, but they are length " "{}.", - m_numMeshInteriorPoints, slacks.size2()); + slackLength, slacks.size2()); + } + + // Adjust guesses for the projection variables to ensure they are the + // correct length. + if (guess.variables.find(Var::projection_states) != guess.variables.end()) { + auto& projection_states = guess.variables.at(Var::projection_states); + + if (projection_states.size2() == m_numGridPoints) { + casadi::DM meshIndices = createMeshIndices(); + std::vector columnsToRemove; + columnsToRemove.push_back(0); + + for (int itime = 0; itime < m_numGridPoints; ++itime) { + if (!meshIndices(itime).__nonzero__()) { + columnsToRemove.push_back(itime); + } + } + // The first argument is an empty vector since we don't want to + // remove an entire row. + projection_states.remove(std::vector(), columnsToRemove); + } + + OPENSIM_THROW_IF(projection_states.size2() != m_numMeshIntervals, + OpenSim::Exception, + "Expected projection state variables to be length {}, but they " + "are length {}.", + m_numMeshIntervals, projection_states.size2()); } // Create the CasADi NLP function. @@ -1030,19 +1374,25 @@ void Transcription::printConstraintValues(const Iterate& it, ss << "\nKinematic constraints:"; std::vector kinconNames = m_problem.createKinematicConstraintEquationNames(); + int numQErr = m_problem.getNumQErr(); + int numUErr = m_problem.getNumUErr(); + int numUDotErr = m_problem.getNumUDotErr(); + int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? + numQErr + numUErr : numQErr + numUErr + numUDotErr; if (kinconNames.empty()) { ss << " none" << std::endl; } else { maxNameLength = 0; updateMaxNameLength(kinconNames); ss << "\n L2 norm across mesh, max abs value (L1 norm), time of " - "max " - "abs" - << std::endl; + "max " + "abs" + << std::endl; row.resize(1, m_numMeshPoints); { - for (int ikc = 0; ikc < (int)constraints.kinematic.rows(); ++ikc) { - row = constraints.kinematic(ikc, Slice()); + int ikc = 0; + for (int i = 0; i < numKC; ++i) { + row = constraints.kinematic(i, Slice()); const double L2 = casadi::DM::norm_2(row).scalar(); int argmax; double max = calcL1Norm(row, argmax); @@ -1051,15 +1401,35 @@ void Transcription::printConstraintValues(const Iterate& it, std::string label = kinconNames.at(ikc); ss << std::setfill('0') << std::setw(2) << ikc << ":" - << std::setfill(' ') << std::setw(maxNameLength) << label - << spacer << std::setprecision(2) << std::scientific - << std::setw(9) << L2 << spacer << L1 << spacer - << std::setprecision(6) << std::fixed << time_of_max - << std::endl; + << std::setfill(' ') << std::setw(maxNameLength) << label + << spacer << std::setprecision(2) << std::scientific + << std::setw(9) << L2 << spacer << L1 << spacer + << std::setprecision(6) << std::fixed << time_of_max + << std::endl; + ++ikc; + } + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + row = constraints.kinematic_udoterr(iudot, Slice()); + const double L2 = casadi::DM::norm_2(row).scalar(); + int argmax; + double max = calcL1Norm(row, argmax); + const double L1 = max; + const double time_of_max = it.times(argmax).scalar(); + + std::string label = kinconNames.at(ikc); + ss << std::setfill('0') << std::setw(2) << ikc << ":" + << std::setfill(' ') << std::setw(maxNameLength) << label + << spacer << std::setprecision(2) << std::scientific + << std::setw(9) << L2 << spacer << L1 << spacer + << std::setprecision(6) << std::fixed << time_of_max + << std::endl; + ++ikc; + } } } ss << "Kinematic constraint values at each mesh point:" - << std::endl; + << std::endl; ss << " time "; for (int ipc = 0; ipc < (int)kinconNames.size(); ++ipc) { ss << std::setw(9) << ipc << " "; @@ -1069,10 +1439,19 @@ void Transcription::printConstraintValues(const Iterate& it, ss << std::setfill('0') << std::setw(3) << imesh << " "; ss.fill(' '); ss << std::setw(9) << it.times(imesh).scalar() << " "; - for (int ikc = 0; ikc < (int)kinconNames.size(); ++ikc) { - const auto& value = constraints.kinematic(ikc, imesh).scalar(); + for (int i = 0; i < numKC; ++i) { + const auto& value = + constraints.kinematic(i, imesh).scalar(); ss << std::setprecision(2) << std::scientific - << std::setw(9) << value << " "; + << std::setw(9) << value << " "; + } + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + const auto& value = + constraints.kinematic_udoterr(iudot, imesh).scalar(); + ss << std::setprecision(2) << std::scientific + << std::setw(9) << value << " "; + } } ss << std::endl; } @@ -1237,12 +1616,22 @@ casadi::MXVector Transcription::evalOnTrajectory( // Add 1 for time input and 1 for parameters input. MXVector mxIn(inputs.size() + 2); mxIn[0] = m_times(timeIndices); + const auto NQ = m_problem.getNumCoordinates(); + const auto NU = m_problem.getNumSpeeds(); + const auto NS = m_problem.getNumStates(); for (int i = 0; i < (int)inputs.size(); ++i) { if (inputs[i] == multibody_states) { - const auto NQ = m_problem.getNumCoordinates(); - const auto NU = m_problem.getNumSpeeds(); - mxIn[i + 1] = - m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); + mxIn[i + 1] = + m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); + } else if (inputs[i] == projection_states) { + const auto& proj_states = m_unscaledVars.at(projection_states); + OPENSIM_ASSERT(proj_states.size2() == timeIndices.size2()); + mxIn[i + 1] = casadi::MX(NS, timeIndices.size2()); + mxIn[i + 1](Slice(0, NQ + NU), Slice()) = + m_unscaledVars.at(projection_states)(Slice(0, NQ + NU), + Slice()); + mxIn[i + 1](Slice(NQ + NU, NS), Slice()) = + m_unscaledVars.at(states)(Slice(NQ + NU, NS), timeIndices); } else if (inputs[i] == slacks) { mxIn[i + 1] = m_unscaledVars.at(inputs[i]); } else { @@ -1257,6 +1646,8 @@ casadi::MXVector Transcription::evalOnTrajectory( mxIn[mxIn.size() - 1] = m_paramsTrajMeshInterior; } else if (&timeIndices == &m_pathConstraintIndices) { mxIn[mxIn.size() - 1] = m_paramsTrajPathCon; + } else if (&timeIndices == &m_projectionStateIndices) { + mxIn[mxIn.size() - 1] = m_paramsTrajProjState; } else { OPENSIM_THROW(OpenSim::Exception, "Internal error."); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 3e13b38ffb..0cab17732c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -3,9 +3,9 @@ /* -------------------------------------------------------------------------- * * OpenSim: CasOCTranscription.h * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -73,6 +73,7 @@ class Transcription { /// scheme applies constraints between control points. void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numPointsPerMeshInterval, const casadi::DM& pointsForInterpControls = casadi::DM()); /// We assume all functions depend on time and parameters. @@ -127,9 +128,11 @@ class Transcription { T multibody_residuals; T auxiliary_residuals; T kinematic; + T kinematic_udoterr; std::vector endpoint; std::vector path; T interp_controls; + T projection; }; void printConstraintValues(const Iterate& it, const Constraints& constraints, @@ -145,10 +148,13 @@ class Transcription { int m_numMeshIntervals = -1; int m_numMeshInteriorPoints = -1; int m_numDefectsPerMeshInterval = -1; + int m_numPointsPerMeshInterval = -1; + int m_numUDotErrorPoints = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; + int m_numProjectionStates = -1; casadi::DM m_grid; casadi::DM m_pointsForInterpControls; casadi::MX m_times; @@ -161,18 +167,34 @@ class Transcription { casadi::MX m_paramsTrajMesh; casadi::MX m_paramsTrajMeshInterior; casadi::MX m_paramsTrajPathCon; + casadi::MX m_paramsTrajProjState; VariablesDM m_lowerBounds; VariablesDM m_upperBounds; VariablesDM m_shift; VariablesDM m_scale; + // These hold vectors of MX types, where each element of the vector + // contains either the states or state derivatives needed to calculate the + // defect constraints for a single mesh interval. + casadi::MXVector m_statesByMeshInterval; + casadi::MXVector m_stateDerivativesByMeshInterval; casadi::DM m_meshIndicesMap; casadi::Matrix m_gridIndices; casadi::Matrix m_meshIndices; casadi::Matrix m_meshInteriorIndices; casadi::Matrix m_pathConstraintIndices; + casadi::Matrix m_projectionStateIndices; + casadi::Matrix m_notProjectionStateIndices; + + // State derivatives. + casadi::MX m_xdot; + // State derivatives reserved for the Bordalba et al. (2023) kinematic + // constraint method based on coordinate projection. + casadi::MX m_xdot_projection; + // The differences between the true states and the projection states when + // using the Bordalba et al. (2023) kinematic constraint method. + casadi::MX m_projectionStateDistances; - casadi::MX m_xdot; // State derivatives. casadi::MX m_objectiveTerms; std::vector m_objectiveTermNames; @@ -193,8 +215,8 @@ class Transcription { virtual casadi::DM createMeshIndicesImpl() const = 0; /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. - virtual void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const = 0; + virtual void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const = 0; virtual void calcInterpolatingControlsImpl(const casadi::MX& /*controls*/, casadi::MX& /*interpControls*/) const { OPENSIM_THROW_IF(m_pointsForInterpControls.numel(), OpenSim::Exception, @@ -204,8 +226,8 @@ class Transcription { void transcribe(); void setObjectiveAndEndpointConstraints(); void calcDefects() { - calcDefectsImpl( - m_unscaledVars.at(states), m_xdot, m_constraints.defects); + calcDefectsImpl(m_statesByMeshInterval, + m_stateDerivativesByMeshInterval, m_constraints.defects); } void calcInterpolatingControls() { calcInterpolatingControlsImpl( @@ -307,82 +329,135 @@ class Transcription { // Trapezoidal sparsity pattern (mapping between flattened and expanded // constraints) for mesh intervals 0, 1 and 2: Endpoint constraints // depend on all time points through their integral. + // // 0 1 2 3 // endpoint x x x x // path_0 x // kinematic_0 x // residual_0 x // defect_0 x x - // kinematic_1 x + // projection_1 x // path_1 x + // kinematic_1 x // residual_1 x // defect_1 x x - // kinematic_2 x + // projection_2 x // path_2 x + // kinematic_2 x // residual_2 x - // kinematic_3 x + // defect_2 x x + // projection_3 x // path_3 x + // kinematic_3 x // residual_3 x - // Hermite-Simpson sparsity pattern for mesh intervals 0, 1 and 2 - // (* indicates additional non-zero entry when path constraint - // midpoints are enforced): - // 0 0.5 1 1.5 2 2.5 3 - // endpoint x x x x x x x - // path_0 x * - // kinematic_0 x - // residual_0 x - // residual_0.5 x - // defect_0 x x x - // interp_con_0 x x x - // kinematic_1 x - // path_1 x * - // residual_1 x - // residual_1.5 x - // defect_1 x x x - // interp_con_1 x x x - // kinematic_2 x - // path_2 x * - // residual_2 x - // residual_2.5 x - // defect_2 x x x - // interp_con_2 x x x - // kinematic_3 x - // path_3 x - // residual_3 x - // 0 0.5 1 1.5 2 2.5 3 + // Hermite-Simpson sparsity pattern for mesh intervals 0, 1 and 2. + // '*' indicates additional non-zero entry when path constraint + // mesh interior points are enforced. Note that acceleration-level + // kinematic constraints, "kinematic_udoterr_0", are only enforced at + // mesh interior points (e.g., 0.5, 1.5, 2.5) when using the Bordalba + // et al. (2023) kinematic constraint method. This sparsity pattern also + // applies to the Legendre-Gauss and Legendre-Gauss-Radau transcription + // with polynomial degree equal to 1. + // + // 0 0.5 1 1.5 2 2.5 3 + // endpoint x x x x x x x + // path_0 x * + // kinematic_perr_0 x + // kinematic_uerr_0 x + // kinematic_udoterr_0 x x + // residual_0 x x + // defect_0 x x x + // interp_con_0 x x x + // projection_1 x + // path_1 x * + // kinematic_perr_1 x + // kinematic_uerr_1 x + // kinematic_udoterr_1 x x + // residual_1 x x + // defect_1 x x x + // interp_con_1 x x x + // projection_2 x + // path_2 x * + // kinematic_perr_2 x + // kinematic_uerr_2 x + // kinematic_udoterr_2 x x + // residual_2 x x + // defect_2 x x x + // interp_con_2 x x x + // projection_3 x + // path_3 x + // kinematic_perr_3 x + // kinematic_uerr_3 x + // kinematic_udoterr_3 x + // residual_3 x + // 0 0.5 1 1.5 2 2.5 3 for (const auto& endpoint : constraints.endpoint) { copyColumn(endpoint, 0); } - for (int ipc = 0; ipc < m_numPathConstraintPoints; ++ipc) { - for (const auto& path: constraints.path) { - copyColumn(path, ipc); + // Constraints for each mesh interval. + int N = m_numPointsPerMeshInterval - 1; + int icon = 0; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + + // Path constraints. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (int i = 0; i < N; ++i) { + for (const auto& path : constraints.path) { + copyColumn(path, igrid + i); + } + } + } else { + for (const auto& path : constraints.path) { + copyColumn(path, imesh); + } } - } - int igrid = 0; - // Index for pointsForInterpControls. - int icon = 0; - for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { + // Kinematic constraints. copyColumn(constraints.kinematic, imesh); - if (imesh < m_numMeshIntervals) { - while (m_grid(igrid).scalar() < m_solver.getMesh()[imesh + 1]) { - copyColumn(constraints.multibody_residuals, igrid); - copyColumn(constraints.auxiliary_residuals, igrid); - ++igrid; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int i = 0; i < N; ++i) { + copyColumn(constraints.kinematic_udoterr, igrid + i); } - copyColumn(constraints.defects, imesh); - while (icon < m_pointsForInterpControls.numel() && - m_pointsForInterpControls(icon).scalar() < - m_solver.getMesh()[imesh + 1]) { - copyColumn(constraints.interp_controls, icon); - ++icon; + } + + // Multibody and auxiliary residuals. + for (int i = 0; i < N; ++i) { + copyColumn(constraints.multibody_residuals, igrid + i); + copyColumn(constraints.auxiliary_residuals, igrid + i); + } + + // Defect constraints. + copyColumn(constraints.defects, imesh); + + // Interpolating controls. + if (m_pointsForInterpControls.numel()) { + for (int i = 0; i < N-1; ++i) { + copyColumn(constraints.interp_controls, icon++); } } + + // Projection constraints. + copyColumn(constraints.projection, imesh); + } + + // Final grid point. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (const auto& path : constraints.path) { + copyColumn(path, m_numGridPoints - 1); + } + } else { + for (const auto& path : constraints.path) { + copyColumn(path, m_numMeshPoints - 1); + } + } + copyColumn(constraints.kinematic, m_numMeshPoints - 1); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(constraints.kinematic_udoterr, m_numGridPoints - 1); } - // The loop above does not handle the residual at the final grid point. copyColumn(constraints.multibody_residuals, m_numGridPoints - 1); copyColumn(constraints.auxiliary_residuals, m_numGridPoints - 1); @@ -403,12 +478,21 @@ class Transcription { }; Constraints out; out.defects = init(m_numDefectsPerMeshInterval, m_numMeshPoints - 1); - out.multibody_residuals = init(m_numMultibodyResiduals, + out.multibody_residuals = init(m_numMultibodyResiduals, m_numGridPoints); out.auxiliary_residuals = init(m_numAuxiliaryResiduals, m_numGridPoints); - out.kinematic = init(m_problem.getNumKinematicConstraintEquations(), - m_numMeshPoints); + int numQErr = m_problem.getNumQErr(); + int numUErr = m_problem.getNumUErr(); + int numUDotErr = m_problem.getNumUDotErr(); + int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? + numQErr + numUErr : numQErr + numUErr + numUDotErr; + out.kinematic = init(numKC,m_numMeshPoints); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + out.kinematic_udoterr = init(numUDotErr, m_numUDotErrorPoints); + } + out.projection = init(m_problem.getNumProjectionConstraintEquations(), + m_numMeshIntervals); out.endpoint.resize(m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; @@ -436,33 +520,67 @@ class Transcription { copyColumn(endpoint, 0); } - for (int ipc = 0; ipc < m_numPathConstraintPoints; ++ipc) { - for (auto& path : out.path) { - copyColumn(path, ipc); + // Constraints for each mesh interval. + int N = m_numPointsPerMeshInterval - 1; + int icon = 0; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + + // Path constraints. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (int i = 0; i < N; ++i) { + for (auto& path : out.path) { + copyColumn(path, igrid + i); + } + } + } else { + for (auto& path : out.path) { + copyColumn(path, imesh); + } } - } - int igrid = 0; - // Index for pointsForInterpControls. - int icon = 0; - for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { + // Kinematic constraints. copyColumn(out.kinematic, imesh); - if (imesh < m_numMeshIntervals) { - while (m_grid(igrid).scalar() < m_solver.getMesh()[imesh + 1]) { - copyColumn(out.multibody_residuals, igrid); - copyColumn(out.auxiliary_residuals, igrid); - ++igrid; + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + for (int i = 0; i < N; ++i) { + copyColumn(out.kinematic_udoterr, igrid + i); } - copyColumn(out.defects, imesh); - while (icon < m_pointsForInterpControls.numel() && - m_pointsForInterpControls(icon).scalar() < - m_solver.getMesh()[imesh + 1]) { - copyColumn(out.interp_controls, icon); - ++icon; + } + + // Multibody and auxiliary residuals. + for (int i = 0; i < N; ++i) { + copyColumn(out.multibody_residuals, igrid + i); + copyColumn(out.auxiliary_residuals, igrid + i); + } + + // Defect constraints. + copyColumn(out.defects, imesh); + + // Interpolating controls. + if (m_pointsForInterpControls.numel()) { + for (int i = 0; i < N-1; ++i) { + copyColumn(out.interp_controls, icon++); } } + + // Projection constraints. + copyColumn(out.projection, imesh); + } + + // Final grid point. + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + for (auto& path : out.path) { + copyColumn(path, m_numGridPoints - 1); + } + } else { + for (auto& path : out.path) { + copyColumn(path, m_numMeshPoints - 1); + } + } + copyColumn(out.kinematic, m_numMeshPoints - 1); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(out.kinematic_udoterr, m_numGridPoints - 1); } - // The loop above does not handle residuals at the final grid point. copyColumn(out.multibody_residuals, m_numGridPoints - 1); copyColumn(out.auxiliary_residuals, m_numGridPoints - 1); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index a3834c2ea4..4a9a644565 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -40,18 +40,18 @@ DM Trapezoidal::createMeshIndicesImpl() const { return DM::ones(1, m_numGridPoints); } -void Trapezoidal::calcDefectsImpl( - const casadi::MX& x, const casadi::MX& xdot, casadi::MX& defects) const { +void Trapezoidal::calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, casadi::MX& defects) const { // We have arranged the code this way so that all constraints at a given // mesh point are grouped together (organizing the sparsity of the Jacobian // this way might have benefits for sparse linear algebra). for (int itime = 0; itime < m_numMeshIntervals; ++itime) { const auto h = m_times(itime + 1) - m_times(itime); - const auto x_i = x(Slice(), itime); - const auto x_ip1 = x(Slice(), itime + 1); - const auto xdot_i = xdot(Slice(), itime); - const auto xdot_ip1 = xdot(Slice(), itime + 1); + const auto x_i = x[itime](Slice(), 0); + const auto x_ip1 = x[itime](Slice(), 1); + const auto xdot_i = xdot[itime](Slice(), 0); + const auto xdot_ip1 = xdot[itime](Slice(), 1); // Trapezoidal defects. defects(Slice(), itime) = x_ip1 - (x_i + 0.5 * h * (xdot_ip1 + xdot_i)); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index b44367bc24..984790dc61 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -29,21 +29,17 @@ class Trapezoidal : public Transcription { public: Trapezoidal(const Solver& solver, const Problem& problem) : Transcription(solver, problem) { - - OPENSIM_THROW_IF(problem.getEnforceConstraintDerivatives(), - OpenSim::Exception, - "Enforcing kinematic constraint derivatives " - "not supported with trapezoidal transcription."); createVariablesAndSetBounds(m_solver.getMesh(), - m_problem.getNumStates()); + m_problem.getNumStates(), 2); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, + casadi::MX& defects) const override; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 1eca2adc09..d83f461d4c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -54,7 +54,10 @@ void MocoCasADiSolver::constructProperties() { constructProperty_minimize_implicit_auxiliary_derivatives(false); constructProperty_implicit_auxiliary_derivatives_weight(1.0); - constructProperty_enforce_path_constraint_midpoints(false); + constructProperty_enforce_path_constraint_mesh_interior_points(false); + constructProperty_minimize_state_projection_distance(true); + constructProperty_state_projection_distance_weight(1e-6); + constructProperty_projection_slack_variable_bounds({-1e-3, 1e-3}); } bool MocoCasADiSolver::isAvailable() { @@ -79,11 +82,11 @@ MocoTrajectory MocoCasADiSolver::createGuess(const std::string& type) const { auto casProblem = createCasOCProblem(); auto casSolver = createCasOCSolver(*casProblem); - std::vector inputControlIndexes = + std::vector inputControlIndexes = getProblemRep().getInputControlIndexes(); if (type == "bounds") { return convertToMocoTrajectory( - casSolver->createInitialGuessFromBounds(), + casSolver->createInitialGuessFromBounds(), inputControlIndexes); } else if (type == "random") { return convertToMocoTrajectory( @@ -180,8 +183,20 @@ std::unique_ptr MocoCasADiSolver::createCasOCProblem() const { OPENSIM_THROW_IF(!model.getMatterSubsystem().getUseEulerAngles( model.getWorkingState()), Exception, "Quaternions are not supported."); + + if (getProblemRep().getNumKinematicConstraintEquations()) { + checkPropertyValueIsInSet(getProperty_kinematic_constraint_method(), + {"Posa2016", "Bordalba2023"}); + OPENSIM_THROW_IF(get_transcription_scheme() != "hermite-simpson" && + get_kinematic_constraint_method() == "Posa2016", Exception, + "Expected the 'hermite-simpson' transcription scheme when using " + "the Posa et al. 2016 method for enforcing kinematic constraints, " + "but received '{}'.", get_transcription_scheme()); + } + return OpenSim::make_unique(*this, problemRep, - createProblemRepJar(numThreads), get_multibody_dynamics_mode()); + createProblemRepJar(numThreads), get_multibody_dynamics_mode(), + get_kinematic_constraint_method()); #else OPENSIM_THROW(MocoCasADiSolverNotAvailable); #endif @@ -205,22 +220,6 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( "legendre-gauss-radau-4", "legendre-gauss-radau-5", "legendre-gauss-radau-6", "legendre-gauss-radau-7", "legendre-gauss-radau-8", "legendre-gauss-radau-9"}); - OPENSIM_THROW_IF(casProblem.getNumKinematicConstraintEquations() != 0 && - get_transcription_scheme() == "trapezoidal", - OpenSim::Exception, - "Kinematic constraints not supported with " - "trapezoidal transcription."); - // Enforcing constraint derivatives is not supported with trapezoidal - // transcription. - if (casProblem.getNumKinematicConstraintEquations() != 0) { - OPENSIM_THROW_IF(get_transcription_scheme() == "trapezoidal" && - get_enforce_constraint_derivatives(), - Exception, - "The current transcription scheme is '{}', but enforcing " - "kinematic constraint derivatives is not supported with " - "trapezoidal transcription.", - get_transcription_scheme()); - } checkPropertyValueIsInRangeOrSet(getProperty_num_mesh_intervals(), 0, std::numeric_limits::max(), {}); @@ -335,11 +334,15 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( casSolver->setImplicitAuxiliaryDerivativesWeight( get_implicit_auxiliary_derivatives_weight()); + casSolver->setMinimizeStateProjection( + get_minimize_state_projection_distance()); + casSolver->setStateProjectionWeight(get_state_projection_distance_weight()); + casSolver->setOptimSolver(get_optim_solver()); - casSolver->setInterpolateControlMidpoints( - get_interpolate_control_midpoints()); - casSolver->setEnforcePathConstraintMidpoints( - get_enforce_path_constraint_midpoints()); + casSolver->setInterpolateControlMeshInteriorPoints( + get_interpolate_control_mesh_interior_points()); + casSolver->setEnforcePathConstraintMeshInteriorPoints( + get_enforce_path_constraint_mesh_interior_points()); if (casProblem.getJarSize() > 1) { casSolver->setParallelism("thread", casProblem.getJarSize()); } @@ -368,14 +371,22 @@ MocoSolution MocoCasADiSolver::solveImpl() const { log_info("Number of threads: {}", casProblem->getJarSize()); } - std::vector inputControlIndexes = + std::vector inputControlIndexes = getProblemRep().getInputControlIndexes(); MocoTrajectory guess = getGuess(); CasOC::Iterate casGuess; if (guess.empty()) { casGuess = casSolver->createInitialGuessFromBounds(); } else { - casGuess = convertToCasOCIterate(guess, inputControlIndexes); + std::vector expectedSlackNames; + for (const auto& info : casProblem->getSlackInfos()) { + expectedSlackNames.push_back(info.name); + } + // We do not need to append projection states here since they will be + // appended later when the guess is resampled by the solver (if needed). + bool appendProjectionStates = false; + casGuess = convertToCasOCIterate(guess, expectedSlackNames, + appendProjectionStates, inputControlIndexes); } // Temporarily disable printing of negative muscle force warnings so the @@ -409,6 +420,7 @@ MocoSolution MocoCasADiSolver::solveImpl() const { !get_minimize_lagrange_multipliers()) { checkConstraintJacobianRank(mocoSolution); } + checkSlackVariables(mocoSolution); const long long elapsed = stopwatch.getElapsedTimeInNs(); setSolutionStats(mocoSolution, casSolution.stats.at("success"), diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h index d6bba90197..d741671474 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h @@ -172,10 +172,26 @@ class OSIMMOCO_API MocoCasADiSolver : public MocoDirectCollocationSolver { "'minimize_implicit_auxiliary_derivatives' is enabled." "Default: 1.0."); - OpenSim_DECLARE_PROPERTY(enforce_path_constraint_midpoints, bool, - "If the transcription scheme is set to 'hermite-simpson', then " - "enable this property to enforce MocoPathConstraints at mesh " - "interval midpoints. Default: false."); + OpenSim_DECLARE_PROPERTY(enforce_path_constraint_mesh_interior_points, bool, + "If the transcription scheme is set to 'hermite-simpson' or one of " + "the pseudospectral schemes (e.g., 'legendre-gauss-3', " + "'legendre-gauss-radau-3'), then enable this property to enforce " + "MocoPathConstraints at points interior to the mesh interval. " + "Default: false."); + + OpenSim_DECLARE_PROPERTY(minimize_state_projection_distance, bool, + "Minimize the distance between the projection states and the " + "constraint manifold when using the 'Bordalba2023' method for " + "enforcing kinematic constraints. Default: true."); + OpenSim_DECLARE_PROPERTY(state_projection_distance_weight, double, + "The weight on the cost term if " + "'minimize_state_projection_distance' is enabled. " + "Default: 1e-6."); + OpenSim_DECLARE_PROPERTY(projection_slack_variable_bounds, MocoBounds, + "The bounds on the slack variables added to the problem which " + "determine the magnitude of the constraint projection at each " + "mesh interval when using the 'Bordalba2023' method for " + "enforcing kinematic constraints. Default: [-1e-3, 1e-3]."); MocoCasADiSolver(); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 95525a0898..4a752d5cdb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -1,9 +1,9 @@ /* -------------------------------------------------------------------------- * * OpenSim Moco: MocoCasOCProblem.cpp * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -22,6 +22,8 @@ #include +#include + using namespace OpenSim; thread_local SimTK::Vector_ @@ -32,13 +34,14 @@ thread_local SimTK::Vector MocoCasOCProblem::m_pvaerr; MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, const MocoProblemRep& problemRep, std::unique_ptr> jar, - std::string dynamicsMode) + std::string dynamicsMode, std::string kinematicConstraintMethod) : m_jar(std::move(jar)), m_paramsRequireInitSystem( mocoCasADiSolver.get_parameters_require_initsystem()), m_formattedTimeString(getFormattedDateTime(true)) { - setDynamicsMode(dynamicsMode); + setDynamicsMode(std::move(dynamicsMode)); + setKinematicConstraintMethod(std::move(kinematicConstraintMethod)); const auto& model = problemRep.getModelBase(); if (problemRep.isPrescribedKinematics()) { @@ -78,6 +81,7 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // dynamics in implicit form. const auto& implicitRefs = problemRep.getImplicitComponentReferencePtrs(); std::vector derivativeNames; + derivativeNames.reserve(implicitRefs.size()); for (const auto& implicitRef : implicitRefs) { derivativeNames.push_back(implicitRef.second->getAbsolutePathString() + "/" + implicitRef.first); @@ -87,9 +91,8 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // Add any scalar constraints associated with kinematic constraints in // the model as path constraints in the problem. - // Whether or not enabled kinematic constraints exist in the model, - // check that optional solver properties related to constraints are - // set properly. + // Whether enabled kinematic constraints exist in the model, check that + // optional solver properties related to constraints are set properly. const auto kcNames = problemRep.createKinematicConstraintNames(); if (kcNames.empty()) { OPENSIM_THROW_IF(mocoCasADiSolver.get_minimize_lagrange_multipliers(), @@ -112,21 +115,26 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, ma = kc.getNumAccelerationEquations(); kinLevels = kc.getKinematicLevels(); - // TODO only add velocity correction variables for holonomic - // constraint derivatives? For now, disallow enforcing derivatives - // if non-holonomic or acceleration constraints present. - OPENSIM_THROW_IF(enforceConstraintDerivs && mv != 0, Exception, - "Enforcing constraint derivatives is supported only for " - "holonomic (position-level) constraints. There are {} " - "velocity-level scalar constraints associated with the " - "model Constraint at ConstraintIndex {}.", - mv, cid); - OPENSIM_THROW_IF(enforceConstraintDerivs && ma != 0, Exception, - "Enforcing constraint derivatives is supported only for " - "holonomic (position-level) constraints. There are {} " - "acceleration-level scalar constraints associated with the " - "model Constraint at ConstraintIndex {}.", - ma, cid); + if (isKinematicConstraintMethodBordalba2023()) { + OPENSIM_THROW_IF(!enforceConstraintDerivs, Exception, + "The Bordalba et al. 2023 method for enforcing " + "kinematic constraints requires that the solver " + "property 'enforce_constraint_derivatives' be set to " + "true."); + } else { + OPENSIM_THROW_IF(mv != 0, Exception, + "The Posa et al. 2016 method is not compatible with " + "velocity-level kinematic constraints. There are {} " + "velocity-level scalar constraints associated with " + "the model Constraint at ConstraintIndex {}.", + mv, cid); + OPENSIM_THROW_IF(ma != 0, Exception, + "The Posa et al. 2016 method is not compatible with " + "acceleration-level kinematic constraints. There are " + "{} acceleration-level scalar constraints associated " + "with the model Constraint at ConstraintIndex {}.", + ma, cid); + } // Loop through all scalar constraints associated with the model // constraint and corresponding path constraints to the optimal @@ -145,8 +153,8 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, kinLevels[i] == KinematicLevel::Velocity || kinLevels[i] == KinematicLevel::Acceleration) { + // Grab the constraint information from the MocoProblem. const auto& multInfo = multInfos[multIndexThisConstraint]; - CasOC::KinematicLevel kinLevel; if (kinLevels[i] == KinematicLevel::Position) kinLevel = CasOC::KinematicLevel::Position; @@ -156,20 +164,22 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, kinLevel = CasOC::KinematicLevel::Acceleration; else { OPENSIM_THROW(OpenSim::Exception, - "Unrecognized KinematicLevel"); + "Unrecognized KinematicLevel"); } + // This adds a Lagrange multiplier to the current problem, + // and, if kinematics are not prescribed, increments the + // number of scalar kinematic constraint equations added. addKinematicConstraint(multInfo.getName(), - convertBounds(multInfo.getBounds()), - convertBounds(multInfo.getInitialBounds()), - convertBounds(multInfo.getFinalBounds()), kinLevel); - - // Add velocity correction variables if enforcing - // constraint equation derivatives. - if (enforceConstraintDerivs && !isPrescribedKinematics()) { - // TODO this naming convention assumes that the - // associated Lagrange multiplier name begins with - // "lambda", which may change in the future. + convertBounds(multInfo.getBounds()), + convertBounds(multInfo.getInitialBounds()), + convertBounds(multInfo.getFinalBounds()), kinLevel); + + // If kinematics are not prescribed and we are enforcing + // kinematic constraints explicitly, we need to add + // additional "slack" variables to ensure the problem is not + // over constrained. + if (!isPrescribedKinematics()) { OPENSIM_THROW_IF( multInfo.getName().substr(0, 6) != "lambda", Exception, @@ -177,12 +187,39 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, "constraint to begin with 'lambda' but it " "begins with '{}'.", multInfo.getName().substr(0, 6)); - const auto vcBounds = convertBounds( + if (isKinematicConstraintMethodBordalba2023()) { + // Add "mu" variables for the projection method by + // Bordalba et al. (2023). + const auto muBounds = convertBounds( mocoCasADiSolver - .get_velocity_correction_bounds()); - addSlack(std::string(multInfo.getName()) - .replace(0, 6, "gamma"), - vcBounds); + .get_projection_slack_variable_bounds()); + if (kinLevel == CasOC::KinematicLevel::Position) { + // For holonomic constraints, we need to add + // two slack variables: one for the position + // error and one for the derivative of the + // position error. + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "mu"), + muBounds); + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "mu") + "_dot", + muBounds); + } else if (kinLevel == + CasOC::KinematicLevel::Velocity) { + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "mu"), + muBounds); + } + } else if (enforceConstraintDerivs) { + // Add "gamma" variables for the method by + // Posa et al. (2015). + const auto vcBounds = convertBounds( + mocoCasADiSolver + .get_velocity_correction_bounds()); + addSlack(std::string(multInfo.getName()) + .replace(0, 6, "gamma"), + vcBounds); + } } ++multIndexThisConstraint; } @@ -191,6 +228,19 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // Set kinematic constraint information on the CasOC::Problem. setEnforceConstraintDerivatives(enforceConstraintDerivs); + + // Set the kinematic constraint offsets, if not enforcing the + // derivatives of the kinematic constraint equations. + m_uerrOffset = getEnforceConstraintDerivatives() ? 0 : + getNumHolonomicConstraintEquations(); + m_uerrSize = getEnforceConstraintDerivatives() ? getNumUErr() : + getNumNonHolonomicConstraintEquations(); + m_udoterrOffset = getEnforceConstraintDerivatives() ? 0 : + getNumHolonomicConstraintEquations() + + getNumNonHolonomicConstraintEquations(); + m_udoterrSize = getEnforceConstraintDerivatives() ? getNumUDotErr() : + getNumAccelerationConstraintEquations(); + // The bounds are the same for all kinematic constraints in the // MocoProblem, so just grab the bounds from the first constraint. // TODO: This behavior may be unexpected for users. diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 1a3641939c..8032f357fb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -3,9 +3,9 @@ /* -------------------------------------------------------------------------- * * OpenSim: MocoCasOCProblem.h * * -------------------------------------------------------------------------- * - * Copyright (c) 2018 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * - * Author(s): Christopher Dembia * + * Author(s): Christopher Dembia, Nicholas Bianco * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -67,35 +67,37 @@ inline casadi::DM convertToCasADiDM(const SimTK::Vector& simtkVec) { return convertToCasADiDMTemplate(simtkVec); } -/// This resamples the iterate to obtain values that lie on the mesh. -inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, +/// This converts a MocoTrajectory to a CasOC::Iterate. +inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoTraj, + const std::vector& expectedSlackNames, + bool appendProjectionStates = false, std::vector inputControlIndexes = {}) { CasOC::Iterate casIt; CasOC::VariablesDM& casVars = casIt.variables; using CasOC::Var; - casVars[Var::initial_time] = mocoIt.getInitialTime(); - casVars[Var::final_time] = mocoIt.getFinalTime(); + casVars[Var::initial_time] = mocoTraj.getInitialTime(); + casVars[Var::final_time] = mocoTraj.getFinalTime(); casVars[Var::states] = - convertToCasADiDMTranspose(mocoIt.getStatesTrajectory()); - - casadi::DM controls = - convertToCasADiDMTranspose(mocoIt.getControlsTrajectory()); - casadi::DM input_controls = - convertToCasADiDMTranspose(mocoIt.getInputControlsTrajectory()); - std::vector controlNames = mocoIt.getControlNames(); - std::vector inputControlNames = mocoIt.getInputControlNames(); + convertToCasADiDMTranspose(mocoTraj.getStatesTrajectory()); + + casadi::DM controls = + convertToCasADiDMTranspose(mocoTraj.getControlsTrajectory()); + casadi::DM input_controls = + convertToCasADiDMTranspose(mocoTraj.getInputControlsTrajectory()); + std::vector controlNames = mocoTraj.getControlNames(); + std::vector inputControlNames = mocoTraj.getInputControlNames(); std::vector casControlNames; - int numTotalControls = static_cast(controls.rows()) + + int numTotalControls = static_cast(controls.rows()) + static_cast(input_controls.rows()); casadi::DM casControls(numTotalControls, controls.columns()); - + int ic = 0; int iic = 0; std::sort(inputControlIndexes.begin(), inputControlIndexes.end()); int numInputControls = static_cast(inputControlIndexes.size()); for (int i = 0; i < numTotalControls; ++i) { if (iic < numInputControls && inputControlIndexes[iic] == i) { - casControls(i, casadi::Slice()) = + casControls(i, casadi::Slice()) = input_controls(iic, casadi::Slice()); casControlNames.push_back(inputControlNames[iic]); ++iic; @@ -108,24 +110,78 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, casVars[Var::controls] = casControls; casVars[Var::multipliers] = - convertToCasADiDMTranspose(mocoIt.getMultipliersTrajectory()); - if (!mocoIt.getSlackNames().empty()) { - casVars[Var::slacks] = - convertToCasADiDMTranspose(mocoIt.getSlacksTrajectory()); - } - if (!mocoIt.getDerivativeNames().empty()) { + convertToCasADiDMTranspose(mocoTraj.getMultipliersTrajectory()); + + if (!mocoTraj.getDerivativeNames().empty()) { casVars[Var::derivatives] = - convertToCasADiDMTranspose(mocoIt.getDerivativesTrajectory()); + convertToCasADiDMTranspose(mocoTraj.getDerivativesTrajectory()); } casVars[Var::parameters] = - convertToCasADiDMTranspose(mocoIt.getParameters()); - casIt.times = convertToCasADiDMTranspose(mocoIt.getTime()); - casIt.state_names = mocoIt.getStateNames(); + convertToCasADiDMTranspose(mocoTraj.getParameters()); + if (appendProjectionStates) { + casVars[Var::projection_states] = convertToCasADiDMTranspose( + mocoTraj.getMultibodyStatesTrajectory()); + } + casIt.times = convertToCasADiDMTranspose(mocoTraj.getTime()); + casIt.state_names = mocoTraj.getStateNames(); casIt.control_names = casControlNames; - casIt.multiplier_names = mocoIt.getMultiplierNames(); - casIt.slack_names = mocoIt.getSlackNames(); - casIt.derivative_names = mocoIt.getDerivativeNames(); - casIt.parameter_names = mocoIt.getParameterNames(); + casIt.multiplier_names = mocoTraj.getMultiplierNames(); + casIt.derivative_names = mocoTraj.getDerivativeNames(); + casIt.parameter_names = mocoTraj.getParameterNames(); + + // Projection state variables. + // --------------------------- + // Extra variables needed when using the projection method for enforcing + // kinematic constraints from Bordalba et al. (2023). + if (appendProjectionStates) { + auto mbStateNames = mocoTraj.getMultibodyStateNames(); + for (auto name : mbStateNames) { + auto valuepos = name.find("/value"); + if (valuepos != std::string::npos) { + name.replace(valuepos, 6, "/value/projection"); + casIt.projection_state_names.push_back(name); + } + auto speedpos = name.find("/speed"); + if (speedpos != std::string::npos) { + name.replace(speedpos, 6, "/speed/projection"); + casIt.projection_state_names.push_back(name); + } + } + } + // Slack variables. + // ---------------- + // Check that the trajectory has the expected slack names from the + // CasOCProblem. + bool matchedExpectedSlackNames = + mocoTraj.getSlackNames().size() == expectedSlackNames.size(); + if (matchedExpectedSlackNames) { + for (const auto& expectedName : expectedSlackNames) { + if (std::find(mocoTraj.getSlackNames().begin(), + mocoTraj.getSlackNames().end(), expectedName) == + mocoTraj.getSlackNames().end()) { + matchedExpectedSlackNames = false; + break; + } + } + } + + // If the guess matches the expected slack names, use the slack values from + // the guess. If not, create a vector of zeros to use as the initial guess + // for the slack variables. + if (matchedExpectedSlackNames) { + casIt.slack_names = mocoTraj.getSlackNames(); + if (!mocoTraj.getSlackNames().empty()) { + casVars[Var::slacks] = + convertToCasADiDMTranspose(mocoTraj.getSlacksTrajectory()); + } + } else { + casIt.slack_names = expectedSlackNames; + if (!expectedSlackNames.empty()) { + casVars[Var::slacks] = casadi::DM::zeros( + (int)expectedSlackNames.size(), mocoTraj.getNumTimes()); + } + } + return casIt; } @@ -155,7 +211,7 @@ inline SimTK::Matrix convertToSimTKMatrix(const casadi::DM& casMatrix) { } template -TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, +TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, std::vector inputControlIndexes = {}) { SimTK::Matrix simtkStates; const auto& casVars = casIt.variables; @@ -172,7 +228,7 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, std::vector controlNames; std::vector inputControlNames; if (numTotalControls) { - SimTK::Matrix allControls = + SimTK::Matrix allControls = convertToSimTKMatrix(casVars.at(Var::controls)); simtkControls.resize(allControls.nrow(), numControls); simtkInputControls.resize(allControls.nrow(), numInputControls); @@ -217,14 +273,14 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, } SimTK::Vector simtkTimes = convertToSimTKVector(casIt.times); - TOut mocoTraj(simtkTimes, casIt.state_names, controlNames, - inputControlNames, casIt.multiplier_names, derivativeNames, - casIt.parameter_names, simtkStates, simtkControls, - simtkInputControls, simtkMultipliers, simtkDerivatives, + TOut mocoTraj(simtkTimes, casIt.state_names, controlNames, + inputControlNames, casIt.multiplier_names, derivativeNames, + casIt.parameter_names, simtkStates, simtkControls, + simtkInputControls, simtkMultipliers, simtkDerivatives, simtkParameters); // Append slack variables. MocoTrajectory requires the slack variables to be - // the same length as its time vector, but it will not be if the + // the same length as its time vector, but it might not be if the // CasOC::Iterate was generated from a CasOC::Transcription object. // Therefore, slack variables are interpolated as necessary. if (!casIt.slack_names.empty()) { @@ -234,7 +290,8 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, for (int i = 0; i < (int)casIt.slack_names.size(); ++i) { if (simtkSlacksLength != simtkTimes.size()) { mocoTraj.appendSlack(casIt.slack_names[i], - interpolate(slackTime, simtkSlacks.col(i), simtkTimes)); + interpolate(slackTime, simtkSlacks.col(i), simtkTimes, + true, true)); } else { mocoTraj.appendSlack(casIt.slack_names[i], simtkSlacks.col(i)); } @@ -251,7 +308,8 @@ class MocoCasOCProblem : public CasOC::Problem { MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, const MocoProblemRep& mocoProblemRep, std::unique_ptr> jar, - std::string dynamicsMode); + std::string dynamicsMode, + std::string kinematicConstraintMethod); int getJarSize() const { return (int)m_jar->size(); } @@ -277,9 +335,10 @@ class MocoCasOCProblem : public CasOC::Problem { modelDisabledConstraints.realizeAcceleration( simtkStateDisabledConstraints); - // Compute kinematic constraint errors if they exist. + // Compute kinematic constraint errors. if (getNumMultipliers() && calcKCErrors) { - calcKinematicConstraintErrors(modelBase, simtkStateBase, + calcKinematicConstraintErrors( + modelBase, simtkStateBase, simtkStateDisabledConstraints, output.kinematic_constraint_errors); } @@ -308,7 +367,7 @@ class MocoCasOCProblem : public CasOC::Problem { const auto& modelBase = mocoProblemRep->getModelBase(); auto& simtkStateBase = mocoProblemRep->updStateBase(); - // Model with disabled constriants and its associated state. These are + // Model with disabled constraints and its associated state. These are // used to compute the accelerations. const auto& modelDisabledConstraints = mocoProblemRep->getModelDisabledConstraints(); @@ -322,13 +381,10 @@ class MocoCasOCProblem : public CasOC::Problem { modelDisabledConstraints.realizeAcceleration( simtkStateDisabledConstraints); - // Compute kinematic constraint errors if they exist. - // TODO: Do not enforce kinematic constraints if prescribedKinematics, - // but must make sure the prescribedKinematics already obey the - // constraints. This is simple at the q and u level (using assemble()), - // but what do we do for the acceleration level? + // Compute kinematic constraint errors. if (getNumMultipliers() && calcKCErrors) { - calcKinematicConstraintErrors(modelBase, simtkStateBase, + calcKinematicConstraintErrors( + modelBase, simtkStateBase, simtkStateDisabledConstraints, output.kinematic_constraint_errors); } @@ -386,6 +442,50 @@ class MocoCasOCProblem : public CasOC::Problem { m_jar->leave(std::move(mocoProblemRep)); } + void calcStateProjection(const double& time, + const casadi::DM& multibody_states, const casadi::DM& slacks, + const casadi::DM& parameters, + casadi::DM& projection) const override { + if (isPrescribedKinematics()) return; + auto mocoProblemRep = m_jar->take(); + + const auto& modelBase = mocoProblemRep->getModelBase(); + auto& simtkStateBase = mocoProblemRep->updStateBase(); + + // Update the model and state. + applyParametersToModelProperties(parameters, *mocoProblemRep); + convertStatesToSimTKState( + SimTK::Stage::Velocity, time, multibody_states, + modelBase, simtkStateBase, false); + modelBase.realizeVelocity(simtkStateBase); + + // Compute the state projection vector based on the method by Bordalba + // et al. (2023). Our implementation looks slightly different from the + // projection constraints in the manuscript since we compute the + // projections for the coordinate values and coordinate speeds + // separately based on how Simbody's assembler handles coordinate + // projections for kinematic constraints. + const SimTK::SimbodyMatterSubsystem& matterBase = + modelBase.getMatterSubsystem(); + + // Holonomic constraint errors. + const SimTK::Vector mu_p( + getNumHolonomicConstraintEquations(), slacks.ptr(), true); + SimTK::Vector proj_p(getNumCoordinates(), projection.ptr(), true); + matterBase.multiplyByPqTranspose(simtkStateBase, mu_p, proj_p); + + // Derivative of holonomic constraint errors and non-holonomic + // constraint errors. + const SimTK::Vector mu_v( + getNumHolonomicConstraintEquations() + + getNumNonHolonomicConstraintEquations(), + slacks.ptr() + getNumHolonomicConstraintEquations(), true); + SimTK::Vector proj_v(getNumSpeeds(), projection.ptr() + + getNumCoordinates(), true); + matterBase.multiplyByPVTranspose(simtkStateBase, mu_v, proj_v); + + m_jar->leave(std::move(mocoProblemRep)); + } void calcCostIntegrand(int index, const ContinuousInput& input, double& integrand) const override { auto mocoProblemRep = m_jar->take(); @@ -610,8 +710,8 @@ class MocoCasOCProblem : public CasOC::Problem { for (int ic = 0; ic < getNumControls(); ++ic) { simtkControls[ic] = *(controls.ptr() + ic); } - // Updating the Inputs to InputControllers via the - // ControlDistributor does not mark the model controls cache as + // Updating the Inputs to InputControllers via the + // ControlDistributor does not mark the model controls cache as // invalid, so we must do it manually here. model.markControlsAsInvalid(simtkState); } @@ -713,7 +813,8 @@ class MocoCasOCProblem : public CasOC::Problem { m_constraintMobilityForces, m_constraintBodyForces); } - void calcKinematicConstraintErrors(const Model& modelBase, + void calcKinematicConstraintErrors( + const Model& modelBase, const SimTK::State& stateBase, const SimTK::State& simtkStateDisabledConstraints, casadi::DM& kinematic_constraint_errors) const { @@ -723,23 +824,14 @@ class MocoCasOCProblem : public CasOC::Problem { // constraints would be redundant, and we need not enforce them. if (isPrescribedKinematics()) return; - // The total number of scalar holonomic, non-holonomic, and acceleration - // constraint equations enabled in the model. This does not count - // equations for derivatives of holonomic and non-holonomic constraints. - const int total_mp = getNumHolonomicConstraintEquations(); - const int total_mv = getNumNonHolonomicConstraintEquations(); - const int total_ma = getNumAccelerationConstraintEquations(); - - // Position-level errors. - const auto& qerr = stateBase.getQErr(); - - if (getEnforceConstraintDerivatives() || total_ma) { - // Calculate udoterr. We cannot use State::getUDotErr() - // because that uses Simbody's multipliers and UDot, - // whereas we have our own multipliers and UDot. Here, we use - // the udot computed from the model with disabled constraints - // since we cannot use (nor do we have available) udot computed - // from the original model. + // Calculate udoterr. We cannot use State::getUDotErr() + // because that uses Simbody's multipliers and UDot, + // whereas we have our own multipliers and UDot. Here, we use + // the udot computed from the model with disabled constraints + // since we cannot use (nor do we have available) udot computed + // from the original model. + if (getEnforceConstraintDerivatives() || + getNumAccelerationConstraintEquations()) { const auto& matter = modelBase.getMatterSubsystem(); matter.calcConstraintAccelerationErrors(stateBase, simtkStateDisabledConstraints.getUDot(), m_pvaerr); @@ -747,40 +839,23 @@ class MocoCasOCProblem : public CasOC::Problem { m_pvaerr = SimTK::NaN; } + // Position-level errors. + const auto& qerr = stateBase.getQErr(); + // Velocity-level errors. const auto& uerr = stateBase.getUErr(); - int uerrOffset; - int uerrSize; + // Acceleration-level errors. const auto& udoterr = m_pvaerr; - int udoterrOffset; - int udoterrSize; - // TODO These offsets and sizes could be computed once. - if (getEnforceConstraintDerivatives()) { - // Velocity-level errors. - uerrOffset = 0; - uerrSize = uerr.size(); - // Acceleration-level errors. - udoterrOffset = 0; - udoterrSize = udoterr.size(); - } else { - // Velocity-level errors. Skip derivatives of position-level - // constraint equations. - uerrOffset = total_mp; - uerrSize = total_mv; - // Acceleration-level errors. Skip derivatives of velocity- - // and position-level constraint equations. - udoterrOffset = total_mp + total_mv; - udoterrSize = total_ma; - } // This way of copying the data avoids a threadsafety issue in // CasADi related to cached Sparsity objects. std::copy_n(qerr.getContiguousScalarData(), qerr.size(), kinematic_constraint_errors.ptr()); - std::copy_n(uerr.getContiguousScalarData() + uerrOffset, uerrSize, + std::copy_n(uerr.getContiguousScalarData() + m_uerrOffset, m_uerrSize, kinematic_constraint_errors.ptr() + qerr.size()); - std::copy_n(udoterr.getContiguousScalarData() + udoterrOffset, - udoterrSize, - kinematic_constraint_errors.ptr() + qerr.size() + uerrSize); + std::copy_n(udoterr.getContiguousScalarData() + m_udoterrOffset, + m_udoterrSize, + kinematic_constraint_errors.ptr() + qerr.size() + m_uerrSize); + } void copyImplicitResidualsToOutput(const MocoProblemRep& mocoProblemRep, @@ -811,6 +886,12 @@ class MocoCasOCProblem : public CasOC::Problem { // the acceleration-level holonomic, non-holonomic constraint errors and the // acceleration-only constraint errors. static thread_local SimTK::Vector m_pvaerr; + // These offsets are necessary when not enforcing the derivatives of the + // kinematic constraint equations. + int m_uerrOffset; + int m_uerrSize; + int m_udoterrOffset; + int m_udoterrSize; }; } // namespace OpenSim diff --git a/OpenSim/Moco/MocoConstraintInfo.cpp b/OpenSim/Moco/MocoConstraintInfo.cpp index def5a55aaa..a304a7f6c7 100644 --- a/OpenSim/Moco/MocoConstraintInfo.cpp +++ b/OpenSim/Moco/MocoConstraintInfo.cpp @@ -49,7 +49,7 @@ void MocoConstraintInfo::printDescription() const { boundsStr.push_back(ss.str()); } str += fmt::format(". bounds: {}", fmt::join(boundsStr, ", ")); - log_cout(str); + log_info(str); } void MocoConstraintInfo::constructProperties() { diff --git a/OpenSim/Moco/MocoControlBoundConstraint.cpp b/OpenSim/Moco/MocoControlBoundConstraint.cpp index f9318ccf7e..c2086a57be 100644 --- a/OpenSim/Moco/MocoControlBoundConstraint.cpp +++ b/OpenSim/Moco/MocoControlBoundConstraint.cpp @@ -147,7 +147,7 @@ void MocoControlBoundConstraint::calcPathConstraintErrorsImpl( int icontrol = 0; SimTK::Vector time(1); for (const auto& controlIndex : m_controlIndices) { - const auto& control = m_isInputControl[icontrol] ? + const auto& control = m_isInputControl[icontrol] ? input_controls[controlIndex] : controls[controlIndex]; time[0] = state.getTime(); // These if-statements work correctly for either value of diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.cpp b/OpenSim/Moco/MocoDirectCollocationSolver.cpp index b4f4acb34c..5388f91243 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.cpp +++ b/OpenSim/Moco/MocoDirectCollocationSolver.cpp @@ -25,7 +25,7 @@ void MocoDirectCollocationSolver::constructProperties() { constructProperty_mesh(); constructProperty_verbosity(2); constructProperty_transcription_scheme("hermite-simpson"); - constructProperty_interpolate_control_midpoints(true); + constructProperty_interpolate_control_mesh_interior_points(true); constructProperty_enforce_constraint_derivatives(true); constructProperty_multibody_dynamics_mode("explicit"); constructProperty_optim_solver("ipopt"); @@ -40,6 +40,7 @@ void MocoDirectCollocationSolver::constructProperties() { constructProperty_implicit_auxiliary_derivative_bounds({-1000, 1000}); constructProperty_minimize_lagrange_multipliers(false); constructProperty_lagrange_multiplier_weight(1.0); + constructProperty_kinematic_constraint_method("Posa2016"); } void MocoDirectCollocationSolver::setMesh(const std::vector& mesh) { @@ -86,3 +87,42 @@ void MocoDirectCollocationSolver::checkConstraintJacobianRank( log_warn(dashes); } } + +void MocoDirectCollocationSolver::checkSlackVariables( + const MocoSolution& solution) const { + const auto& slacks = solution.getSlacksTrajectory(); + const auto& slackNames = solution.getSlackNames(); + const SimTK::Real threshold = 1e-3; + bool largeSlackDetected = false; + + std::vector slackWarnings; + for (int islack = 0; islack < slacks.ncol(); ++islack) { + if (SimTK::max(SimTK::abs(slacks.col(islack))) > threshold) { + largeSlackDetected = true; + std::stringstream ss; + ss << "Slack variable '" << slackNames[islack] << "' has a maximum " + << "value of " << SimTK::max(SimTK::abs(slacks.col(islack))) + << "."; + slackWarnings.push_back(ss.str()); + } + } + + if (largeSlackDetected) { + const std::string dashes(50, '-'); + log_warn(dashes); + log_warn("Large slack variables detected."); + log_warn(dashes); + for (const auto& warning : slackWarnings) { + log_warn(warning); + } + log_warn(""); + log_warn("Slack variables with values larger than ~{} might", threshold); + log_warn("indicate that the problem is struggling to enforce"); + log_warn("kinematic constraints in the problem. Since slack variables"); + log_warn("interact with defect constraints in the direct collocation "); + log_warn("formulation, large slack values may affect the quality "); + log_warn("of the solution. Consider refining the mesh or adjusting "); + log_warn("the constraint tolerance in the MocoProblem."); + log_warn(dashes); + } +} diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.h b/OpenSim/Moco/MocoDirectCollocationSolver.h index e09f99e391..d671d17b15 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.h +++ b/OpenSim/Moco/MocoDirectCollocationSolver.h @@ -99,13 +99,19 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "0 for silent. 1 for only Moco's own output. " "2 for output from CasADi and the underlying solver (default: 2)."); OpenSim_DECLARE_PROPERTY(transcription_scheme, std::string, - "'trapezoidal' for trapezoidal transcription, or 'hermite-simpson' " - "(default) for separated Hermite-Simpson transcription."); - OpenSim_DECLARE_PROPERTY(interpolate_control_midpoints, bool, - "If the transcription scheme is set to 'hermite-simpson', then " - "enable this property to constrain the control values at mesh " - "interval midpoints to be linearly interpolated from the control " - "values at the mesh interval endpoints. Default: true."); + "'trapezoidal' for trapezoidal transcription, 'hermite-simpson' " + "(default) for separated Hermite-Simpson transcription, " + "'legendre-gauss-#' for Legendre-Gauss transcription (where # is " + "the number of collocation points per mesh interval between 1 and " + "9), and 'legendre-gauss-radau-#' for Legendre-Gauss-Radau " + "transcription."); + OpenSim_DECLARE_PROPERTY(interpolate_control_mesh_interior_points, bool, + "If the transcription scheme is set to 'hermite-simpson' or one of " + "the pseudospectral schemes (e.g., 'legendre-gauss-3', " + "'legendre-gauss-radau-3') then enable this property to constrain " + "the control values at mesh interior points to be linearly " + "interpolated from the control values at the mesh interval " + "endpoints. Default: true."); OpenSim_DECLARE_PROPERTY(multibody_dynamics_mode, std::string, "Multibody dynamics are expressed as 'explicit' (default) or " "'implicit' differential equations."); @@ -147,6 +153,7 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "enforced, set the bounds on the slack variables performing the " "velocity correction to project the model coordinates back onto " "the constraint manifold. Default: [-0.1, 0.1]"); + OpenSim_DECLARE_PROPERTY(implicit_multibody_acceleration_bounds, MocoBounds, "Bounds on acceleration variables in implicit dynamics mode. " "Default: [-1000, 1000]"); @@ -154,6 +161,12 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "Bounds on derivative variables for components with auxiliary " "dynamics in implicit form. Default: [-1000, 1000]"); + OpenSim_DECLARE_PROPERTY(kinematic_constraint_method, std::string, + "The method used to enforce kinematic constraints in the direct " + "collocation problem. 'Posa2016' for the method by Posa et al. " + "2016 (default) or 'Bordalba2023' for the method by Bordalba et " + "al. 2023 (only valid with CasADi)."); + MocoDirectCollocationSolver() { constructProperties(); } /** %Set the mesh to a user-defined list of mesh points to sample. This @@ -170,8 +183,9 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "Takes precedence over uniform mesh with num_mesh_intervals."); void constructProperties(); - // Helper function for post-processing the solution. + // Helper functions for post-processing the solution. void checkConstraintJacobianRank(const MocoSolution& mocoSolution) const; + void checkSlackVariables(const MocoSolution& mocoSolution) const; }; } // namespace OpenSim diff --git a/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp index a74ba15a45..0756adba36 100644 --- a/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoAccelerationTrackingGoal.cpp @@ -136,10 +136,10 @@ void MocoAccelerationTrackingGoal::calcIntegrandImpl( } void MocoAccelerationTrackingGoal::printDescriptionImpl() const { - log_cout(" acceleration reference file: {}", + log_info(" acceleration reference file: {}", get_acceleration_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", i, m_frame_paths[i], + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_acceleration_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp index f09224c8f0..2fe8728b00 100644 --- a/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoAngularVelocityTrackingGoal.cpp @@ -163,10 +163,10 @@ void MocoAngularVelocityTrackingGoal::calcIntegrandImpl( } void MocoAngularVelocityTrackingGoal::printDescriptionImpl() const { - log_cout(" angular velocity reference file: {}", + log_info(" angular velocity reference file: {}", get_angular_velocity_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", i, m_frame_paths[i], + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_angular_velocity_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp index 6201ac9d47..abb6260c8a 100644 --- a/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.cpp @@ -340,15 +340,15 @@ void MocoContactImpulseTrackingGoal::calcIntegrandImpl( } void MocoContactImpulseTrackingGoal::printDescriptionImpl() const { - log_cout(" impulse axis: {}", get_impulse_axis()); + log_info(" impulse axis: {}", get_impulse_axis()); for (int ig = 0; ig < getProperty_contact_groups().size(); ++ig) { const auto& group = get_contact_groups(ig); - log_cout(" group {}: ExternalForce: {}", + log_info(" group {}: ExternalForce: {}", ig, group.get_external_force_name()); - log_cout(" forces:"); + log_info(" forces:"); for (int ic = 0; ic < group.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", group.get_contact_force_paths(ic)); + log_info(" {}", group.get_contact_force_paths(ic)); } } } \ No newline at end of file diff --git a/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp index 17fc6c23d3..1aeb951879 100644 --- a/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.cpp @@ -374,18 +374,18 @@ void MocoContactTrackingGoal::calcIntegrandImpl( } void MocoContactTrackingGoal::printDescriptionImpl() const { - log_cout(" projection type: {}", get_projection()); + log_info(" projection type: {}", get_projection()); if (m_projectionType != ProjectionType::None) { - log_cout(" projection vector: {}", get_projection_vector()); + log_info(" projection vector: {}", get_projection_vector()); } for (int ig = 0; ig < getProperty_contact_groups().size(); ++ig) { const auto& group = get_contact_groups(ig); - log_cout(" group {}: ExternalForce: {}", + log_info(" group {}: ExternalForce: {}", ig, group.get_external_force_name()); - log_cout(" forces:"); + log_info(" forces:"); for (int ic = 0; ic < group.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", group.get_contact_force_paths(ic)); + log_info(" {}", group.get_contact_force_paths(ic)); } } } diff --git a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp index c01ec362a0..334ef0422f 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp @@ -194,11 +194,11 @@ void MocoControlGoal::calcGoalImpl( void MocoControlGoal::printDescriptionImpl() const { for (int i = 0; i < (int) m_controlNames.size(); i++) { - log_cout(" control: {}, weight: {}", m_controlNames[i], + log_info(" control: {}, weight: {}", m_controlNames[i], m_weights[i]); } for (int i = 0; i < (int) m_inputControlNames.size(); i++) { - log_cout(" Input control: {}, weight: {}", + log_info(" Input control: {}, weight: {}", m_inputControlNames[i], m_inputControlWeights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp index ff10037b79..56a4098aee 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp @@ -259,7 +259,7 @@ void MocoControlTrackingGoal::calcIntegrandImpl( void MocoControlTrackingGoal::printDescriptionImpl() const { for (int i = 0; i < (int)m_control_names.size(); i++) { std::string type = m_isInputControl[i] ? "Input control" : "control"; - log_cout(" {}: {}, reference label: {}, weight: {}", + log_info(" {}: {}, reference label: {}, weight: {}", type, m_control_names[i], m_ref_labels[i], m_control_weights[i]); } diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp new file mode 100644 index 0000000000..0e316ecf3b --- /dev/null +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -0,0 +1,124 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: MocoExpressionBasedParameterGoal.cpp * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoExpressionBasedParameterGoal.h" + +#include +#include +#include +#include + +using namespace OpenSim; + +void MocoExpressionBasedParameterGoal::constructProperties() { + constructProperty_expression(""); + constructProperty_parameters(); + constructProperty_variables(); +} + +void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) + const { + OPENSIM_THROW_IF_FRMOBJ(get_expression().empty(), Exception, + "The expression has not been set. Use setExpression().") + m_program = Lepton::Parser::parse(get_expression()).optimize() + .createProgram(); + setRequirements(0, 1, SimTK::Stage::Instance); + + for (int i = 0; i < getProperty_parameters().size(); i++) { + // only taking the first one since they should all be the same value + std::string componentPath = get_parameters(i).getComponentPaths()[0]; + const auto& component = model.getComponent(componentPath); + const auto* ap = &component.getPropertyByName( + get_parameters(i).getPropertyName()); + m_property_refs.emplace_back(ap); + + // get the type and element of the property + if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_double); + } else { + if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_Vec3); + m_indices.emplace_back(get_parameters(i).getPropertyElement()); + } + else if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_Vec6); + m_indices.emplace_back(get_parameters(i).getPropertyElement()); + } + else { + OPENSIM_THROW_FRMOBJ(Exception, + "Data type of specified model property not supported."); + } + } + } + + // test to make sure all variables are there + try { + std::map parameterVars; + for (int i = 0; i < getProperty_variables().size(); ++i) { + parameterVars[get_variables(i)] = getPropertyValue(i); + } + m_program.evaluate(parameterVars); + } catch (Lepton::Exception& ex) { + const std::string msg = ex.what(); + if (msg.compare(0, 30, "No value specified for variable")) { + std::string undefinedVar = msg.substr(32, msg.size() - 32); + OPENSIM_THROW_FRMOBJ(Exception, + fmt::format("Parameter variable '{}' is not defined. Use " + "addParameter() to explicitly define this variable. Or, " + "remove it from the expression.", undefinedVar)); + } else { + OPENSIM_THROW_FRMOBJ(Exception, "Lepton parsing error: {}", msg); + } + } +} + +double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { + OPENSIM_ASSERT_FRMOBJ(m_property_refs.size() > i); + const auto& propRef = m_property_refs[i]; + if (m_data_types[i] == Type_double) { + return static_cast*>(propRef.get())->getValue(); + } + int elt = m_indices[i]; + if (m_data_types[i] == Type_Vec3) { + return static_cast*>(propRef.get()) + ->getValue()[elt]; + } + if (m_data_types[i] == Type_Vec6) { + return static_cast*>(propRef.get()) + ->getValue()[elt]; + } + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Property at index {} is not of" + " a recognized type.")); +} + +void MocoExpressionBasedParameterGoal::calcGoalImpl( + const GoalInput& input, SimTK::Vector& values) const { + std::map parameterVars; + for (int i = 0; i < getProperty_variables().size(); ++i) { + parameterVars[get_variables(i)] = getPropertyValue(i); + } + values[0] = m_program.evaluate(parameterVars); +} + +void MocoExpressionBasedParameterGoal::printDescriptionImpl() const { + log_info(" expression: {}", get_expression()); + for (int i = 0; i < getProperty_parameters().size(); ++i) { + log_cout(" variable {}: {}", get_variables(i), + get_parameters(i).getName()); + } +} diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h new file mode 100644 index 0000000000..2de96dd42d --- /dev/null +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -0,0 +1,133 @@ +#ifndef OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H +#define OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H +/* -------------------------------------------------------------------------- * + * OpenSim: MocoExpressionBasedParameterGoal.h * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoGoal.h" +#include "OpenSim/Moco/MocoParameter.h" +#include +#include + +namespace OpenSim { +class Model; + +/** Minimize or constrain an arithmetic expression of parameters. + +This goal supports both "cost" and "endpoint constraint" modes and can be +defined using any number of MocoParameters. The expression string should match +the Lepton (lightweight expression parser) format. + +# Creating Expressions + +Expressions can be any string that represents a mathematical expression, e.g., +"x*sqrt(y-8)". Expressions can contain variables, constants, operations, +parentheses, commas, spaces, and scientific "e" notation. The full list of +operations is: sqrt, exp, log, sin, cos, sec, csc, tan, cot, asin, acos, atan, +sinh, cosh, tanh, erf, erfc, step, delta, square, cube, recip, min, max, abs, ++, -, *, /, and ^. + +# Examples + +@code +auto* spring1_parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); +auto* spring2_parameter = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); +auto* spring_goal = mp.addGoal(); +double STIFFNESS = 100.0; +// minimum is when p + q = STIFFNESS +spring_goal->setExpression(fmt::format("square(p+q-{})", STIFFNESS)); +spring_goal->addParameter(*spring1_parameter, "p"); +spring_goal->addParameter(*spring2_parameter, "q"); +@endcode + +@ingroup mocogoal */ +class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { + OpenSim_DECLARE_CONCRETE_OBJECT(MocoExpressionBasedParameterGoal, MocoGoal); + +public: + MocoExpressionBasedParameterGoal() { constructProperties(); } + MocoExpressionBasedParameterGoal(std::string name) + : MocoGoal(std::move(name)) { + constructProperties(); + } + MocoExpressionBasedParameterGoal(std::string name, double weight) + : MocoGoal(std::move(name), weight) { + constructProperties(); + } + MocoExpressionBasedParameterGoal(std::string name, double weight, + std::string expression) : MocoGoal(std::move(name), weight) { + constructProperties(); + set_expression(std::move(expression)); + } + + /** Set the arithmetic expression to minimize or constrain. Variable + names should match the names set with addParameter(). See "Creating + Expressions" in the class documentation above for an explanation of how to + create expressions. */ + void setExpression(std::string expression) { + set_expression(std::move(expression)); + } + + /** Add parameters with variable names that match the variables in the + expression string. All variables in the expression must have a corresponding + parameter, but parameters with variables that are not in the expression are + ignored. */ + void addParameter(const MocoParameter& parameter, std::string variable) { + append_parameters(parameter); + append_variables(std::move(variable)); + } + +protected: + void initializeOnModelImpl(const Model& model) const override; + void calcGoalImpl( + const GoalInput& input, SimTK::Vector& cost) const override; + bool getSupportsEndpointConstraintImpl() const override { return true; } + void printDescriptionImpl() const override; + +private: + void constructProperties(); + + /** Get the value of the property from its index in the property_refs vector. + This will use m_data_types to get the type, and if it is a Vec type, it uses + m_indices to get the element to return, both at the same index i. */ + double getPropertyValue(int i) const; + + OpenSim_DECLARE_PROPERTY(expression, std::string, + "The expression string defining this cost or endpoint constraint."); + OpenSim_DECLARE_LIST_PROPERTY(parameters, MocoParameter, + "Parameters included in the expression."); + OpenSim_DECLARE_LIST_PROPERTY(variables, std::string, + "Variables names corresponding to parameters in the expression."); + + mutable Lepton::ExpressionProgram m_program; + // stores references to one property per parameter + mutable std::vector> m_property_refs; + enum DataType { + Type_double, + Type_Vec3, + Type_Vec6 + }; + mutable std::vector m_data_types; + mutable std::vector m_indices; + +}; + +} // namespace OpenSim + +#endif // OPENSIM_MOCOPARAMETEREXPRESSIONGOAL_H diff --git a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp index 32af44d5c7..b96932c8ed 100644 --- a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.cpp @@ -29,11 +29,31 @@ void MocoGeneralizedForceTrackingGoal::constructProperties() { constructProperty_reference(TableProcessor()); constructProperty_force_paths(); constructProperty_generalized_force_weights(MocoWeightSet()); + constructProperty_generalized_force_weights_pattern(MocoWeightSet()); constructProperty_allow_unused_references(false); constructProperty_normalize_tracking_error(false); constructProperty_ignore_constrained_coordinates(true); } +void MocoGeneralizedForceTrackingGoal::setWeightForGeneralizedForce( + const std::string& name, double weight) { + if (get_generalized_force_weights().contains(name)) { + upd_generalized_force_weights().get(name).setWeight(weight); + } else { + upd_generalized_force_weights().cloneAndAppend({name, weight}); + } +} + +void MocoGeneralizedForceTrackingGoal::setWeightForGeneralizedForcePattern( + const std::string& pattern, double weight) { + if (get_generalized_force_weights_pattern().contains(pattern)) { + upd_generalized_force_weights_pattern().get(pattern).setWeight(weight); + } else { + upd_generalized_force_weights_pattern().cloneAndAppend( + {pattern, weight}); + } +} + void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( const Model& model) const { @@ -47,7 +67,7 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( // Create a map between coordinate paths and their indices in the system. SimTK::State state = model.getWorkingState(); const auto& coordinates = model.getCoordinatesInMultibodyTreeOrder(); - std::unordered_map allCoordinateIndices; + std::unordered_map allGeneralizedForceIndexes; for (int i = 0; i < static_cast(coordinates.size()); ++i) { if (get_ignore_constrained_coordinates() && coordinates[i]->isConstrained(state)) { @@ -72,7 +92,7 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( coordinates[i]->getName()); } - allCoordinateIndices[label] = i; + allGeneralizedForceIndexes[label] = i; } // Get the system indexes for specified forces. @@ -100,27 +120,40 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( const SimTK::Force::Gravity& gravity = model.getGravityForce(); m_forceIndexes.push_back(gravity.getForceIndex()); + // Set the regex pattern weights first. + std::map weightsFromPatterns; + for (int i = 0; i < get_generalized_force_weights_pattern().getSize(); ++i) { + const auto& mocoWeight = get_generalized_force_weights_pattern().get(i); + const auto& pattern = mocoWeight.getName(); + const auto regex = std::regex(pattern); + for (const auto& kv : allGeneralizedForceIndexes) { + if (std::regex_match(kv.first, regex)) { + weightsFromPatterns[kv.first] = mocoWeight.getWeight(); + } + } + } + // Validate the coordinate weights. for (int i = 0; i < get_generalized_force_weights().getSize(); ++i) { const auto& weightName = get_generalized_force_weights().get(i).getName(); - if (allCoordinateIndices.count(weightName) == 0) { + if (allGeneralizedForceIndexes.count(weightName) == 0) { OPENSIM_THROW_FRMOBJ(Exception, "Weight provided with name '{}' but this is " - "not a recognized coordinate or it is a constrained " - "coordinate and set to be ignored.", + "not a recognized generalized force or it is associated " + "with a constrained coordinate and set to be ignored.", weightName); } } for (int iref = 0; iref < allSplines.getSize(); ++iref) { const auto& refName = allSplines[iref].getName(); - if (allCoordinateIndices.count(refName) == 0) { + if (allGeneralizedForceIndexes.count(refName) == 0) { if (get_allow_unused_references()) { continue; } OPENSIM_THROW_FRMOBJ(Exception, "Reference provided with name '{}' but this is " - "not a recognized coordinate or it is a constrained " - "coordinate and set to be ignored.", + "not a recognized generalized force or it is associated " + "with a constrained coordinate and set to be ignored.", refName); } @@ -132,12 +165,21 @@ void MocoGeneralizedForceTrackingGoal::initializeOnModelImpl( "Expected coordinate weights to be non-negative, but " "received a negative weight for coordinate '{}'.", refName); - if (refWeight < SimTK::SignificantReal) { continue; } + } else if (weightsFromPatterns.count(refName)) { + refWeight = weightsFromPatterns[refName]; + } + + if (refWeight > SimTK::SignificantReal) { + m_generalizedForceIndexes.push_back( + allGeneralizedForceIndexes.at(refName)); + m_generalizedForceWeights.push_back(refWeight); + m_generalizedForceNames.push_back(refName); + m_refsplines.cloneAndAppend(allSplines[iref]); + } else { + log_info("MocoGeneralizedForceTrackingGoal: Generalized force '{}' " + "has weight 0 (or very close to 0) and will be ignored.", + refName); } - m_coordinateIndexes.push_back(allCoordinateIndices.at(refName)); - m_generalizedForceWeights.push_back(refWeight); - m_generalizedForceNames.push_back(refName); - m_refsplines.cloneAndAppend(allSplines[iref]); // Compute normalization factors. if (get_normalize_tracking_error()) { @@ -185,7 +227,8 @@ void MocoGeneralizedForceTrackingGoal::calcIntegrandImpl( SimTK::Vector timeVec(1, time); integrand = 0; for (int iref = 0; iref < m_refsplines.getSize(); ++iref) { - const auto& modelValue = generalizedForces[m_coordinateIndexes[iref]]; + const auto& modelValue = + generalizedForces[m_generalizedForceIndexes[iref]]; const auto& refValue = m_refsplines[iref].calcValue(timeVec); // Compute the tracking error. @@ -198,7 +241,7 @@ void MocoGeneralizedForceTrackingGoal::calcIntegrandImpl( void MocoGeneralizedForceTrackingGoal::printDescriptionImpl() const { for (int i = 0; i < static_cast(m_generalizedForceNames.size()); i++) { - log_cout(" generalized force: {}, weight: {}", + log_info(" generalized force: {}, weight: {}", m_generalizedForceNames[i], m_generalizedForceWeights[i]); } diff --git a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h index ceb2e05f9a..05677c8d90 100644 --- a/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoGeneralizedForceTrackingGoal.h @@ -93,13 +93,15 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); /// then the provided weight replaces the previous weight. Weight names /// should match the column labels in the reference table (e.g., /// `ankle_angle_r_moment`, `pelvis_tx_force`, etc.). - void setWeightForGeneralizedForce(const std::string& name, double weight) { - if (get_generalized_force_weights().contains(name)) { - upd_generalized_force_weights().get(name).setWeight(weight); - } else { - upd_generalized_force_weights().cloneAndAppend({name, weight}); - } - } + void setWeightForGeneralizedForce(const std::string& name, double weight); + + /// Set the tracking weight for all generalized forces whose names match the + /// regular expression pattern. Multiple pairs of patterns and weights can + /// be provided by calling this function multiple times. If a generalized + /// force matches multiple patterns, the weight associated with the last + /// pattern is used. + void setWeightForGeneralizedForcePattern(const std::string& pattern, + double weight); /// Set the MocoWeightSet to weight the generalized coordinate forces in /// the cost. Replaces the weight set if it already exists. @@ -122,9 +124,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); /// @copydoc setNormalizeTrackingError(bool tf) bool getNormalizeTrackingError() { return get_normalize_tracking_error(); } - /// Whether or not to ignore coordinates that are locked, prescribed, or - /// coupled to other coordinates. This is based on the value returned from - /// `Coordinate::isConstrained()` (default: true). + /// Whether or not to ignore generalized forces for coordinates that are + /// locked, prescribed, or coupled to other coordinates. This is based on + /// the value returned from `Coordinate::isConstrained()` (default: true). void setIgnoreConstrainedCoordinates(bool tf) { set_ignore_constrained_coordinates(tf); } @@ -156,6 +158,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); OpenSim_DECLARE_PROPERTY(generalized_force_weights, MocoWeightSet, "Set of weight objects to weight the tracking of individual " "generalized coordinate forces in the cost."); + OpenSim_DECLARE_PROPERTY(generalized_force_weights_pattern, MocoWeightSet, + "Set weights for all generalized forces matching a regular " + "expression."); OpenSim_DECLARE_PROPERTY(allow_unused_references, bool, "Flag to determine whether or not references contained in the " "reference table are allowed to be ignored by the cost."); @@ -165,14 +170,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoGeneralizedForceTrackingGoal, MocoGoal); "If the peak magnitude of the reference generalized force data is " "close to zero, an exception is thrown (default: false)."); OpenSim_DECLARE_PROPERTY(ignore_constrained_coordinates, bool, - "Flag to determine whether or not to ignore coordinates that are " - "locked, prescribed, or coupled to other coordinates " - "(default: true)."); + "Flag to determine whether or not to ignore generalized forces for " + "coordinates that are locked, prescribed, or coupled to other " + "coordinates (default: true)."); mutable GCVSplineSet m_refsplines; mutable std::vector m_generalizedForceWeights; mutable std::vector m_generalizedForceNames; - mutable std::vector m_coordinateIndexes; + mutable std::vector m_generalizedForceIndexes; mutable std::vector m_normalizationFactors; mutable SimTK::Array_ m_forceIndexes; }; diff --git a/OpenSim/Moco/MocoGoal/MocoGoal.cpp b/OpenSim/Moco/MocoGoal/MocoGoal.cpp index 5a7175cca6..98003bc3b7 100644 --- a/OpenSim/Moco/MocoGoal/MocoGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoGoal.cpp @@ -45,7 +45,7 @@ void MocoGoal::printDescription() const { if (mode == "cost") { str += fmt::format(", weight: {}", get_weight()); } - log_cout(str); + log_info(str); printDescriptionImpl(); } diff --git a/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp b/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp index 40a873d746..604c18f7be 100644 --- a/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.cpp @@ -154,15 +154,15 @@ void MocoJointReactionGoal::calcIntegrandImpl( } void MocoJointReactionGoal::printDescriptionImpl() const { - log_cout(" joint path: ", get_joint_path()); - log_cout(" loads frame: ", get_loads_frame()); - log_cout(" expressed: ", get_expressed_in_frame_path()); + log_info(" joint path: ", get_joint_path()); + log_info(" loads frame: ", get_loads_frame()); + log_info(" expressed: ", get_expressed_in_frame_path()); std::vector measures(getProperty_reaction_measures().size()); for (int i = 0; i < (int)measures.size(); i++) { measures[i] = get_reaction_measures(i); } - log_cout(" reaction measures: {}", fmt::join(measures, ", ")); + log_info(" reaction measures: {}", fmt::join(measures, ", ")); - log_cout(" reaction weights: {}", fmt::join(m_measureWeights, ", ")); + log_info(" reaction weights: {}", fmt::join(m_measureWeights, ", ")); } diff --git a/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.h b/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.h index e8e7f89082..f9ba1fcc60 100644 --- a/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoJointReactionGoal.h @@ -88,11 +88,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoJointReactionGoal, MocoGoal); already set for the requested measure, then the provided weight replaces the previous weight. An exception is thrown during initialization if a weight for an unknown measure is provided. */ - void setWeight(const std::string& stateName, const double& weight) { - if (get_reaction_weights().contains(stateName)) { - upd_reaction_weights().get(stateName).setWeight(weight); + void setWeight(const std::string& measure, const double& weight) { + if (get_reaction_weights().contains(measure)) { + upd_reaction_weights().get(measure).setWeight(weight); } else { - upd_reaction_weights().cloneAndAppend({stateName, weight}); + upd_reaction_weights().cloneAndAppend({measure, weight}); } } /** Provide a MocoWeightSet to weight the reaction measures in the cost. diff --git a/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp b/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp index b19309ebd1..cacf03eb92 100644 --- a/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoMarkerFinalGoal.cpp @@ -40,6 +40,6 @@ void MocoMarkerFinalGoal::constructProperties() { } void MocoMarkerFinalGoal::printDescriptionImpl() const { - log_cout(" point name: {}", get_point_name()); - log_cout(" reference location: {}", get_reference_location()); + log_info(" point name: {}", get_point_name()); + log_info(" reference location: {}", get_reference_location()); } diff --git a/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp index 7489aa1002..0164d7cdc4 100644 --- a/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoMarkerTrackingGoal.cpp @@ -158,12 +158,12 @@ void MocoMarkerTrackingGoal::calcIntegrandImpl( } void MocoMarkerTrackingGoal::printDescriptionImpl() const { - log_cout( + log_info( " allow unused references: ", get_allow_unused_references()); - log_cout(" tracked marker(s):"); + log_info(" tracked marker(s):"); int weightIndex = 0; for (const auto& name : m_marker_names) { - log_cout(" {}, weight: {}", name, + log_info(" {}, weight: {}", name, m_marker_weights[weightIndex]); weightIndex++; } diff --git a/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp index c84aad9538..9fc2cb4c2b 100644 --- a/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoOrientationTrackingGoal.cpp @@ -222,10 +222,10 @@ void MocoOrientationTrackingGoal::calcIntegrandImpl( } void MocoOrientationTrackingGoal::printDescriptionImpl() const { - log_cout(" rotation reference file: {}", + log_info(" rotation reference file: {}", get_rotation_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_rotation_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp b/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp index 99ff8f9f72..87eb56e81c 100644 --- a/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoOutputGoal.cpp @@ -26,6 +26,8 @@ using namespace OpenSim; void MocoOutputBase::constructProperties() { constructProperty_output_path(""); + constructProperty_second_output_path(""); + constructProperty_operation(""); constructProperty_exponent(1); constructProperty_output_index(-1); } @@ -59,8 +61,8 @@ void MocoOutputBase::initializeOnModelBase() const { } else if (dynamic_cast*>(abstractOutput)) { m_data_type = Type_SpatialVec; OPENSIM_THROW_IF_FRMOBJ(get_output_index() > 5, Exception, - "The Output is of type 'SimTK::SpatialVec', but an Output index " - "greater than 5 was provided."); + "The Output is of type 'SimTK::SpatialVec', but an Output index" + " greater than 5 was provided."); if (get_output_index() < 3) { m_index1 = 0; m_index2 = get_output_index(); @@ -94,32 +96,138 @@ void MocoOutputBase::initializeOnModelBase() const { // Set the "depends-on stage", the SimTK::Stage we must realize to // in order to calculate values from this output. m_dependsOnStage = m_output->getDependsOnStage(); + + m_useCompositeOutputValue = false; + // if there's a second output, initialize it + if (get_second_output_path() != "") { + m_useCompositeOutputValue = true; + initializeComposite(); + } else if (get_operation() != "") { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("An operation was provided " + "but a second Output path was not provided. Either provide no " + "operation with a single Output, or provide a value to both " + "setOperation() and setSecondOutputPath().")); + } +} + +void MocoOutputBase::initializeComposite() const { + if (get_operation() == "addition") { + m_operation = Addition; + } else if (get_operation() == "subtraction") { + m_operation = Subtraction; + } else if (get_operation() == "multiplication") { + m_operation = Multiplication; + } else if (get_operation() == "division") { + m_operation = Division; + } else if (get_operation() == "") { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("A second Output path was " + "provided, but no operation was provided. Use setOperation() to" + "provide an operation")); + } else { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Invalid operation: '{}', must " + "be 'addition', 'subtraction', 'multiplication', or 'division'.", + get_operation())); + } + + std::string componentPath, outputName, channelName, alias; + AbstractInput::parseConnecteePath(get_second_output_path(), componentPath, + outputName, channelName, alias); + const auto& component = getModel().getComponent(componentPath); + const auto* abstractOutput = &component.getOutput(outputName); + + if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(getOutputIndex() != -1, Exception, + "An Output index was provided, but the second Output is of type" + " 'double'.") + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_double, Exception, + "Output types do not match. The second Output is of type double" + " but the first is of type {}.", getDataTypeString(m_data_type)); + } else if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_Vec3, Exception, + "Output types do not match. The second Output is of type " + "SimTK::Vec3 but the first is of type {}.", + getDataTypeString(m_data_type)); + } else if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_SpatialVec, Exception, + "Output types do not match. The second Output is of type " + "SimTK::SpatialVec but the first is of type {}.", + getDataTypeString(m_data_type)); + OPENSIM_THROW_IF_FRMOBJ(m_minimizeVectorNorm && + (m_operation == Multiplication || m_operation == Division), + Exception, "Multiplication and division operations are not " + "supported with Output type SimTK::SpatialVec without an index.") + } else { + OPENSIM_THROW_FRMOBJ(Exception, + "Data type of specified second Output not supported."); + } + m_second_output.reset(abstractOutput); + + if (getDependsOnStage() < m_second_output->getDependsOnStage()) { + m_dependsOnStage = m_second_output->getDependsOnStage(); + } } double MocoOutputBase::calcOutputValue(const SimTK::State& state) const { - getModel().getSystem().realize(state, m_output->getDependsOnStage()); + if (m_useCompositeOutputValue) { + return calcCompositeOutputValue(state); + } + return calcSingleOutputValue(state); +} + +double MocoOutputBase::calcSingleOutputValue(const SimTK::State& state) const { + getModel().getSystem().realize(state, getDependsOnStage()); double value = 0; if (m_data_type == Type_double) { - value = static_cast*>(m_output.get()) - ->getValue(state); - + value = getOutput().getValue(state); } else if (m_data_type == Type_Vec3) { if (m_minimizeVectorNorm) { - value = static_cast*>(m_output.get()) - ->getValue(state).norm(); + value = getOutput().getValue(state).norm(); + } else { + value = getOutput().getValue(state)[m_index1]; + } + } else if (m_data_type == Type_SpatialVec) { + if (m_minimizeVectorNorm) { + value = getOutput().getValue(state).norm(); } else { - value = static_cast*>(m_output.get()) - ->getValue(state)[m_index1]; + value = getOutput().getValue(state)[m_index1][m_index2]; } + } + + return value; +} +double MocoOutputBase::calcCompositeOutputValue(const SimTK::State& state) const { + getModel().getSystem().realize(state, getDependsOnStage()); + + double value = 0; + if (m_data_type == Type_double) { + double value1 = getOutput().getValue(state); + double value2 = getSecondOutput().getValue(state); + value = applyOperation(value1, value2); + } else if (m_data_type == Type_Vec3) { + if (m_minimizeVectorNorm) { + const SimTK::Vec3& value1 = getOutput().getValue(state); + const SimTK::Vec3& value2 = getSecondOutput().getValue(state); + value = applyOperation(value1, value2); + } else { + double value1 = getOutput().getValue(state)[m_index1]; + double value2 = getSecondOutput().getValue(state)[m_index1]; + value = applyOperation(value1, value2); + } } else if (m_data_type == Type_SpatialVec) { if (m_minimizeVectorNorm) { - value = static_cast*>(m_output.get()) - ->getValue(state).norm(); + const SimTK::SpatialVec& value1 = getOutput() + .getValue(state); + const SimTK::SpatialVec& value2 = getSecondOutput() + .getValue(state); + value = applyOperation(value1, value2); } else { - value = static_cast*>(m_output.get()) - ->getValue(state)[m_index1][m_index2]; + double value1 = getOutput().getValue(state) + [m_index1][m_index2]; + double value2 = getSecondOutput().getValue(state) + [m_index1][m_index2]; + value = applyOperation(value1, value2); } } @@ -130,12 +238,14 @@ void MocoOutputBase::printDescriptionImpl() const { // Output path. std::string str = fmt::format(" output: {}", getOutputPath()); + if (m_useCompositeOutputValue) { + str += fmt::format("\n second output: {}", getSecondOutputPath()); + // Operation. + str += fmt::format("\n operation: {}", get_operation()); + } + // Output type. - std::string type; - if (m_data_type == Type_double) { type = "double"; } - else if (m_data_type == Type_Vec3) { type = "SimTK::Vec3"; } - else if (m_data_type == Type_SpatialVec) { type = "SimTK::SpatialVec"; } - str += fmt::format(", type: {}", type); + str += fmt::format(", type: {}", getDataTypeString(m_data_type)); // Output index (if relevant). if (getOutputIndex() != -1) { @@ -150,7 +260,7 @@ void MocoOutputBase::printDescriptionImpl() const { str += fmt::format(", bounds: {}", getConstraintInfo().getBounds()[0]); } - log_cout(str); + log_info(str); } // ============================================================================ @@ -255,4 +365,4 @@ void MocoOutputTrackingGoal::calcIntegrandImpl(const IntegrandInput& input, void MocoOutputTrackingGoal::calcGoalImpl(const GoalInput &input, SimTK::Vector &values) const { values[0] = input.integral; -} \ No newline at end of file +} diff --git a/OpenSim/Moco/MocoGoal/MocoOutputGoal.h b/OpenSim/Moco/MocoGoal/MocoOutputGoal.h index 670afeb51d..7353053b19 100644 --- a/OpenSim/Moco/MocoGoal/MocoOutputGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoOutputGoal.h @@ -24,25 +24,38 @@ namespace OpenSim { /** This abstract base class provides convenience methods and common interfaces for all Output-related MocoGoal's. All MocoGoal's deriving from this class -include the 'setOutputPath()', 'setOutputIndex()', and 'setExponent()' methods -and their corresponding Object properties. The convenience method -'initializeOnModelBase()' should be called at the top of -'initializeOnModelImpl()' within each derived class. Similarly, +include the 'setOutputPath()', 'setSecondOutputPath()', 'setOperation()', +'setOutputIndex()', and 'setExponent()' methods and their corresponding Object +properties. The convenience method 'initializeOnModelBase()' should be called at +the top of 'initializeOnModelImpl()' within each derived class. Similarly, 'calcOutputValue()' can be used to retrieve the Output value with 'calcGoalImpl()' and/or 'calcIntegrandImpl()', as needed for each derived class. The method 'getDependsOnStage()' returns the SimTK::Stage that should be realized to to calculate Output values. The method 'setValueToExponent()' can be used to raise a value to the exponent provided via 'setExponent()'. +Goals can be composed of one or two Outputs. The optional second Output can be +included by using the methods 'setSecondOutputPath()' and 'setOperation()'. The +Output values can be combined by addition, subtraction, multiplication, or +division. The first Output is always on the left hand side of the operation and +the second Output on the right hand side. The two Outputs can be different +quantities, but they must be the same type. + We support the following Output types: - double - SimTK::Vec3 - SimTK::SpatialVec -When using vector types, 'setOutputIndex()' may be used to select a specific -element of the Output vector. If not specified, the norm of the vector is -returned when calling 'calcOutputValue()'. +When using SimTK::Vec3 or SimTK::SpatialVec types, 'setOutputIndex()' may be +used to select a specific element of the Output vector. If no index is +specified, the norm of the vector will be used when calling 'calcOutputValue()'. +If using two Outputs, the Output index will be used to select the same element +from both Outputs before the operation. If two Outputs of type SimTK::Vec3 or +SimTK::SpatialVec are provided and no index is specified, the operation will be +applied elementwise before computing the norm. Elementwise +multiplication and division operations are not supported when using two +SimTK::SpatialVec Outputs (i.e., an index must be provided). @ingroup mocogoal */ class OSIMMOCO_API MocoOutputBase : public MocoGoal { OpenSim_DECLARE_ABSTRACT_OBJECT(MocoOutputBase, MocoGoal); @@ -57,11 +70,31 @@ class OSIMMOCO_API MocoOutputBase : public MocoGoal { constructProperties(); } - /** Set the absolute path to the output in the model to use as the integrand - for this goal. The format is "/path/to/component|output_name". */ + /** Set the absolute path to the Output in the model. The format is + "/path/to/component|output_name". */ void setOutputPath(std::string path) { set_output_path(std::move(path)); } const std::string& getOutputPath() const { return get_output_path(); } + /** Set the absolute path to the optional second Output in the model. The + format is "/path/to/component|output_name". This Output should have the same + type as the first Output. If providing a second Output, the user must also + provide an operation via `setOperation()`. */ + void setSecondOutputPath(std::string path) { + set_second_output_path(std::move(path)); + } + const std::string& getSecondOutputPath() const { + return get_second_output_path(); + } + + /** Set the operation that combines Output values where two Outputs are + provided. The supported operations include "addition", "subtraction", + "multiplication", or "division". If providing an operation, the user + must also provide a second Output path. */ + void setOperation(std::string operation) { + set_operation(std::move(operation)); + } + const std::string& getOperation() const { return get_operation(); } + /** Set the exponent applied to the output value in the integrand. This exponent is applied when minimizing the norm of a vector type output. The default exponent is set to 1, meaning that the output can take on negative @@ -82,7 +115,6 @@ class OSIMMOCO_API MocoOutputBase : public MocoGoal { int getOutputIndex() const { return get_output_index(); } protected: - /** Get a reference to the Output at the specified Output path and store its data type. This also creates a function based on the exponent set via 'setExponent()', which can be accessed with 'setValueToExponent()'. Finally, @@ -91,9 +123,7 @@ class OSIMMOCO_API MocoOutputBase : public MocoGoal { 'initializeOnModelImpl()' in each derived class. */ void initializeOnModelBase() const; - /** Calculate the Output value for the provided SimTK::State. If using a - vector Output, either the vector norm or vector element will be returned, - depending on whether an index was provided via 'setOutputIndex()'. Do not + /** Calculate the Output value for the provided SimTK::State. Do not call this function until 'initializeOnModelBase()' has been called. */ double calcOutputValue(const SimTK::State&) const; @@ -111,11 +141,16 @@ class OSIMMOCO_API MocoOutputBase : public MocoGoal { void printDescriptionImpl() const override; - private: OpenSim_DECLARE_PROPERTY(output_path, std::string, - "The absolute path to the output in the model to use as the " - "integrand for this goal."); + "The absolute path to the Output in the model (i.e., " + "'/path/to/component|output_name')."); + OpenSim_DECLARE_PROPERTY(second_output_path, std::string, + "The absolute path to the optional second Output in the model (i.e.," + " '/path/to/component|output_name')."); + OpenSim_DECLARE_PROPERTY(operation, std::string, "The operation to combine " + "the two outputs: 'addition', 'subtraction', 'multiplication', or " + "'divison'."); OpenSim_DECLARE_PROPERTY(exponent, int, "The exponent applied to the output value in the integrand. " "The output can take on negative values in the integrand when the " @@ -132,13 +167,86 @@ class OSIMMOCO_API MocoOutputBase : public MocoGoal { "indicates to minimize the vector norm (default: -1)."); void constructProperties(); + /** Initialize additional information when there are two Outputs: + the second Output, the operation, and the dependsOnStage. */ + void initializeComposite() const; + + /** Calculate the Output value of one Output. */ + double calcSingleOutputValue(const SimTK::State&) const; + /** Calculate the two Output values and apply the operation. */ + double calcCompositeOutputValue(const SimTK::State&) const; + + /** Get a reference to the Output for this goal. */ + template + const Output& getOutput() const { + return static_cast&>(m_output.getRef()); + } + /** Get a reference to the second Output for this goal. */ + template + const Output& getSecondOutput() const { + return static_cast&>(m_second_output.getRef()); + } + + /** Apply the operation to two double values. */ + double applyOperation(double value1, double value2) const { + switch (m_operation) { + case Addition : return value1 + value2; + case Subtraction : return value1 - value2; + case Multiplication : return value1 * value2; + case Division : return value1 / value2; + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "double type Outputs."); + } + } + /** Apply the elementwise operation to two SimTK::Vec3 values. */ + double applyOperation(const SimTK::Vec3& value1, + const SimTK::Vec3& value2) const { + switch (m_operation) { + case Addition : return (value1 + value2).norm(); + case Subtraction : return (value1 - value2).norm(); + case Multiplication : return value1.elementwiseMultiply(value2).norm(); + case Division : return value1.elementwiseDivide(value2).norm(); + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "SimTK::Vec3 type Outputs."); + } + } + /** Apply the elementwise operation to two SimTK::SpatialVec values. + Multiplication and divison operations are not supported for SpatialVec Outputs + without an index. */ + double applyOperation(const SimTK::SpatialVec& value1, + const SimTK::SpatialVec& value2) const { + switch (m_operation) { + case Addition : return (value1 + value2).norm(); + case Subtraction : return (value1 - value2).norm(); + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "SimTK::SpatialVec type Outputs."); + } + } + enum DataType { Type_double, Type_Vec3, Type_SpatialVec, }; + /** Get the string of the data type, for prints and error messages. */ + std::string getDataTypeString(DataType type) const { + switch (type) { + case Type_double: return "double"; + case Type_Vec3: return "SimTK::Vec3"; + case Type_SpatialVec: return "SimTK::SpatialVec"; + default: return "empty Datatype"; + } + } + mutable DataType m_data_type; mutable SimTK::ReferencePtr m_output; + mutable SimTK::ReferencePtr m_second_output; + enum OperationType { Addition, Subtraction, Multiplication, Division }; + mutable OperationType m_operation; + mutable bool m_useCompositeOutputValue; mutable std::function m_power_function; mutable int m_index1; mutable int m_index2; @@ -446,4 +554,4 @@ class OSIMMOCO_API MocoOutputTrackingGoal : public MocoOutputBase { } // namespace OpenSim -#endif // OPENSIM_MOCOOUTPUTGOAL_H \ No newline at end of file +#endif // OPENSIM_MOCOOUTPUTGOAL_H diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp index 31b6e1c531..3cdc4fd894 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp @@ -156,14 +156,14 @@ void MocoPeriodicityGoal::calcGoalImpl( } void MocoPeriodicityGoal::printDescriptionImpl() const { - log_cout(" state periodicity pairs:"); + log_info(" state periodicity pairs:"); for (const auto& pair : m_state_names) { - log_cout(" initial: {}, final: {}", pair.first, + log_info(" initial: {}, final: {}", pair.first, pair.second); } - log_cout(" control periodicity pairs:"); + log_info(" control periodicity pairs:"); for (const auto& pair : m_control_names) { - log_cout(" initial: {}, final: {}", pair.first, + log_info(" initial: {}, final: {}", pair.first, pair.second); } } diff --git a/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp index ed42305a89..ff2f62863b 100644 --- a/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoStateTrackingGoal.cpp @@ -168,8 +168,7 @@ void MocoStateTrackingGoal::calcIntegrandImpl( void MocoStateTrackingGoal::printDescriptionImpl() const { for (int i = 0; i < (int) m_state_names.size(); i++) { - log_cout(" state: {}, weight: {}", m_state_names[i], + log_info(" state: {}, weight: {}", m_state_names[i], m_state_weights[i]); } } - diff --git a/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp b/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp index 972e549c4e..6d04b6d5de 100644 --- a/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoStepLengthAsymmetryGoal.cpp @@ -120,7 +120,7 @@ void MocoStepLengthAsymmetryGoal::calcGoalImpl(const GoalInput& input, } void MocoStepLengthAsymmetryGoal::printDescriptionImpl() const { - log_cout(" target asymmetry: ", get_target_asymmetry()); - log_cout(" left foot frame: ", get_left_foot_frame()); - log_cout(" right foot frame: ", get_right_foot_frame()); + log_info(" target asymmetry: ", get_target_asymmetry()); + log_info(" left foot frame: ", get_left_foot_frame()); + log_info(" right foot frame: ", get_right_foot_frame()); } \ No newline at end of file diff --git a/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp b/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp index dbb00ba796..16737f46c4 100644 --- a/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoStepTimeAsymmetryGoal.cpp @@ -228,22 +228,22 @@ void MocoStepTimeAsymmetryGoal::calcGoalImpl(const GoalInput& input, } void MocoStepTimeAsymmetryGoal::printDescriptionImpl() const { - log_cout(" target asymmetry: {}", get_target_asymmetry()); + log_info(" target asymmetry: {}", get_target_asymmetry()); const auto& leftGroup = get_left_contact_group(); - log_cout(" left forces:"); + log_info(" left forces:"); for (int ic = 0; ic < leftGroup.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", leftGroup.get_contact_force_paths(ic)); + log_info(" {}", leftGroup.get_contact_force_paths(ic)); } - log_cout(" left contact sphere for position: {}", + log_info(" left contact sphere for position: {}", leftGroup.get_foot_position_contact_force_path()); const auto& rightGroup = get_right_contact_group(); - log_cout(" right forces:"); + log_info(" right forces:"); for (int ic = 0; ic < leftGroup.getProperty_contact_force_paths().size(); ++ic) { - log_cout(" {}", rightGroup.get_contact_force_paths(ic)); + log_info(" {}", rightGroup.get_contact_force_paths(ic)); } - log_cout(" right contact sphere for position: {}", + log_info(" right contact sphere for position: {}", rightGroup.get_foot_position_contact_force_path()); } diff --git a/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp b/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp index 2815f7ba9b..ab09830657 100644 --- a/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.cpp @@ -104,7 +104,7 @@ void MocoSumSquaredStateGoal::calcIntegrandImpl( void MocoSumSquaredStateGoal::printDescriptionImpl() const { for (int i = 0; i < (int)m_state_names.size(); i++) { - log_cout(" state: {}, weight: {}", m_state_names[i], + log_info(" state: {}, weight: {}", m_state_names[i], m_state_weights[i]); } } diff --git a/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp index 17089bd48d..6e0efc9066 100644 --- a/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoTranslationTrackingGoal.cpp @@ -162,10 +162,10 @@ void MocoTranslationTrackingGoal::calcIntegrandImpl( } void MocoTranslationTrackingGoal::printDescriptionImpl() const { - log_cout(" translation reference file: {}", + log_info(" translation reference file: {}", get_translation_reference_file()); for (int i = 0; i < (int)m_frame_paths.size(); i++) { - log_cout(" frame {}: {}, weight: {}", i, m_frame_paths[i], + log_info(" frame {}: {}, weight: {}", i, m_frame_paths[i], m_translation_weights[i]); } } diff --git a/OpenSim/Moco/MocoInverse.cpp b/OpenSim/Moco/MocoInverse.cpp index 6ba6bb9ff7..e0d092bec0 100644 --- a/OpenSim/Moco/MocoInverse.cpp +++ b/OpenSim/Moco/MocoInverse.cpp @@ -104,7 +104,7 @@ std::pair MocoInverse::initializeInternal() const { // ------------------------- auto& solver = study.initCasADiSolver(); solver.set_multibody_dynamics_mode("implicit"); - solver.set_interpolate_control_midpoints(false); + solver.set_interpolate_control_mesh_interior_points(false); solver.set_minimize_implicit_auxiliary_derivatives(true); solver.set_implicit_auxiliary_derivatives_weight(0.01); solver.set_optim_convergence_tolerance(get_convergence_tolerance()); diff --git a/OpenSim/Moco/MocoOutputBoundConstraint.cpp b/OpenSim/Moco/MocoOutputBoundConstraint.cpp new file mode 100644 index 0000000000..7e3bbe81bf --- /dev/null +++ b/OpenSim/Moco/MocoOutputBoundConstraint.cpp @@ -0,0 +1,315 @@ +/* -------------------------------------------------------------------------- * +* OpenSim: MocoOutputBoundConstraint.cpp * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoOutputBoundConstraint.h" +#include "MocoProblemInfo.h" +#include +#include + +using namespace OpenSim; + +void MocoOutputBoundConstraint::constructProperties() { + constructProperty_output_path(""); + constructProperty_second_output_path(""); + constructProperty_operation(""); + constructProperty_exponent(1); + constructProperty_output_index(-1); + constructProperty_lower_bound(); + constructProperty_upper_bound(); + constructProperty_equality_with_lower(false); +} + +void MocoOutputBoundConstraint::initializeOnModelImpl( + const Model& model, const MocoProblemInfo& problemInfo) const { + OPENSIM_THROW_IF_FRMOBJ(get_output_path().empty(), Exception, + "No output_path provided."); + std::string componentPath, outputName, channelName, alias; + AbstractInput::parseConnecteePath( + get_output_path(), componentPath, outputName, channelName, alias); + const auto& component = getModel().getComponent(componentPath); + const auto* abstractOutput = &component.getOutput(outputName); + + OPENSIM_THROW_IF_FRMOBJ(get_output_index() < -1, Exception, + "Invalid Output index provided."); + m_boundVectorNorm = (get_output_index() == -1); + + if (dynamic_cast*>(abstractOutput)) { + m_data_type = Type_double; + OPENSIM_THROW_IF_FRMOBJ(get_output_index() != -1, Exception, + "An Output index was provided, but the Output is of type 'double'.") + + } else if (dynamic_cast*>(abstractOutput)) { + m_data_type = Type_Vec3; + OPENSIM_THROW_IF_FRMOBJ(get_output_index() > 2, Exception, + "The Output is of type 'SimTK::Vec3', but an Output index greater " + "than 2 was provided."); + m_index1 = get_output_index(); + + } else if (dynamic_cast*>(abstractOutput)) { + m_data_type = Type_SpatialVec; + OPENSIM_THROW_IF_FRMOBJ(get_output_index() > 5, Exception, + "The Output is of type 'SimTK::SpatialVec', but an Output index " + "greater than 5 was provided."); + if (get_output_index() < 3) { + m_index1 = 0; + m_index2 = get_output_index(); + } else { + m_index1 = 1; + m_index2 = get_output_index() - 3; + } + + } else { + OPENSIM_THROW_FRMOBJ(Exception, + "Data type of specified model output not supported."); + } + m_output.reset(abstractOutput); + + OPENSIM_THROW_IF_FRMOBJ(get_exponent() < 1, Exception, + "Exponent must be 1 or greater."); + int exponent = get_exponent(); + + // The pow() function gives slightly different results than x * x. On Mac, + // using x * x requires fewer solver iterations. + if (exponent == 1) { + m_power_function = [](const double& x) { return x; }; + } else if (exponent == 2) { + m_power_function = [](const double& x) { return x * x; }; + } else { + m_power_function = [exponent](const double& x) { + return pow(std::abs(x), exponent); + }; + } + + // Set the "depends-on stage", the SimTK::Stage we must realize to + // in order to calculate values from this output. + m_dependsOnStage = m_output->getDependsOnStage(); + + // initialize bounds + m_hasLower = !getProperty_lower_bound().empty(); + m_hasUpper = !getProperty_upper_bound().empty(); + if (!m_hasLower && !m_hasUpper) { + log_warn("In MocoOutputBoundConstraint '{}', output path(s) are " + "specified but no bounds are provided.", getName()); + } + OPENSIM_THROW_IF_FRMOBJ(get_equality_with_lower() && m_hasUpper, Exception, + "If equality_with_lower==true, upper bound function must not be " + "set."); + OPENSIM_THROW_IF_FRMOBJ(get_equality_with_lower() && !m_hasLower, Exception, + "If equality_with_lower==true, lower bound function must be set."); + + auto checkTimeRange = [&](const Function& f) { + if (auto* spline = dynamic_cast(&f)) { + OPENSIM_THROW_IF_FRMOBJ( + spline->getMinX() > problemInfo.minInitialTime, Exception, + "The function's minimum domain value ({}) must " + "be less than or equal to the minimum possible " + "initial time ({}).", + spline->getMinX(), problemInfo.minInitialTime); + OPENSIM_THROW_IF_FRMOBJ( + spline->getMaxX() < problemInfo.maxFinalTime, Exception, + "The function's maximum domain value ({}) must " + "be greater than or equal to the maximum possible " + "final time ({}).", + spline->getMaxX(), problemInfo.maxFinalTime); + } + }; + if (m_hasLower) checkTimeRange(get_lower_bound()); + if (m_hasUpper) checkTimeRange(get_upper_bound()); + + if (get_equality_with_lower()) { + setNumEquations(1); + } else { + setNumEquations((int)m_hasLower + (int)m_hasUpper); + } + + // TODO: setConstraintInfo() is not really intended for use here. + MocoConstraintInfo info; + std::vector bounds; + if (get_equality_with_lower()) { + bounds.emplace_back(0, 0); + } else { + // The lower and upper bounds on the path constraint must be + // constants, so we cannot use the lower and upper bound functions + // directly. Therefore, we use a pair of constraints where the + // lower/upper bound functions are part of the path constraint + // functions and the lower/upper bounds for the path constraints are + // -inf, 0, and/or inf. + // If a lower bound function is provided, we enforce + // lower_bound_function <= output + // by creating the constraint + // 0 <= output - lower_bound_function <= inf + if (m_hasLower) { bounds.emplace_back(0, SimTK::Infinity); } + // If an upper bound function is provided, we enforce + // output <= upper_bound_function + // by creating the constraint + // -inf <= output - upper_bound_function <= 0 + if (m_hasUpper) { bounds.emplace_back(-SimTK::Infinity, 0); } + } + info.setBounds(bounds); + const_cast(this)->setConstraintInfo(info); + + m_useCompositeOutputValue = false; + // if there's a second output, initialize it + if (get_second_output_path() != "") { + m_useCompositeOutputValue = true; + initializeComposite(); + } else if (get_operation() != "") { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("An operation was provided " + "but a second Output path was not provided. Either provide no " + "operation with a single Output, or provide a value to both " + "setOperation() and setSecondOutputPath().")); + } +} + +void MocoOutputBoundConstraint::initializeComposite() const { + if (get_operation() == "addition") { + m_operation = Addition; + } else if (get_operation() == "subtraction") { + m_operation = Subtraction; + } else if (get_operation() == "multiplication") { + m_operation = Multiplication; + } else if (get_operation() == "division") { + m_operation = Division; + } else if (get_operation() == "") { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("A second Output path was " + "provided, but no operation was provided. Use setOperation() to" + "provide an operation")); + } else { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Invalid operation: '{}', must " + "be 'addition', 'subtraction', 'multiplication', or 'division'.", + get_operation())); + } + + std::string componentPath, outputName, channelName, alias; + AbstractInput::parseConnecteePath(get_second_output_path(), componentPath, + outputName, channelName, alias); + const auto& component = getModel().getComponent(componentPath); + const auto* abstractOutput = &component.getOutput(outputName); + + if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(getOutputIndex() != -1, Exception, + "An Output index was provided, but the second Output is of type" + " 'double'.") + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_double, Exception, + "Output types do not match. The second Output is of type double" + " but the first is of type {}.", getDataTypeString(m_data_type)); + } else if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_Vec3, Exception, + "Output types do not match. The second Output is of type " + "SimTK::Vec3 but the first is of type {}.", + getDataTypeString(m_data_type)); + } else if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_SpatialVec, Exception, + "Output types do not match. The second Output is of type " + "SimTK::SpatialVec but the first is of type {}.", + getDataTypeString(m_data_type)); + OPENSIM_THROW_IF_FRMOBJ(m_boundVectorNorm && + (m_operation == Multiplication || m_operation == Division), + Exception, "Multiplication and division operations are not " + "supported with Output type SimTK::SpatialVec without an index.") + } else { + OPENSIM_THROW_FRMOBJ(Exception, + "Data type of specified second Output not supported."); + } + m_second_output.reset(abstractOutput); + + if (getDependsOnStage() < m_second_output->getDependsOnStage()) { + m_dependsOnStage = m_second_output->getDependsOnStage(); + } +} + +void MocoOutputBoundConstraint::calcPathConstraintErrorsImpl( + const SimTK::State& state, SimTK::Vector& errors) const { + int iconstr = 0; + SimTK::Vector time(1); + time[0] = state.getTime(); + double output_value = setValueToExponent(calcOutputValue(state)); + if (m_hasLower) { + errors[iconstr++] = output_value - get_lower_bound().calcValue(time); + } + if (m_hasUpper) { + errors[iconstr++] = output_value - get_upper_bound().calcValue(time); + } +} + +double MocoOutputBoundConstraint::calcOutputValue(const SimTK::State& state) const { + if (m_useCompositeOutputValue) { + return calcCompositeOutputValue(state); + } + return calcSingleOutputValue(state); +} + +double MocoOutputBoundConstraint::calcSingleOutputValue(const SimTK::State& state) const { + getModel().getSystem().realize(state, getDependsOnStage()); + + double value = 0; + if (m_data_type == Type_double) { + value = getOutput().getValue(state); + } else if (m_data_type == Type_Vec3) { + if (m_boundVectorNorm) { + value = getOutput().getValue(state).norm(); + } else { + value = getOutput().getValue(state)[m_index1]; + } + } else if (m_data_type == Type_SpatialVec) { + if (m_boundVectorNorm) { + value = getOutput().getValue(state).norm(); + } else { + value = getOutput().getValue(state)[m_index1][m_index2]; + } + } + + return value; +} + +double MocoOutputBoundConstraint::calcCompositeOutputValue(const SimTK::State& state) const { + getModel().getSystem().realize(state, getDependsOnStage()); + + double value = 0; + if (m_data_type == Type_double) { + double value1 = getOutput().getValue(state); + double value2 = getSecondOutput().getValue(state); + value = applyOperation(value1, value2); + } else if (m_data_type == Type_Vec3) { + if (m_boundVectorNorm) { + const SimTK::Vec3& value1 = getOutput().getValue(state); + const SimTK::Vec3& value2 = getSecondOutput().getValue(state); + value = applyOperation(value1, value2); + } else { + double value1 = getOutput().getValue(state)[m_index1]; + double value2 = getSecondOutput().getValue(state)[m_index1]; + value = applyOperation(value1, value2); + } + } else if (m_data_type == Type_SpatialVec) { + if (m_boundVectorNorm) { + const SimTK::SpatialVec& value1 = getOutput() + .getValue(state); + const SimTK::SpatialVec& value2 = getSecondOutput() + .getValue(state); + value = applyOperation(value1, value2); + } else { + double value1 = getOutput() + .getValue(state)[m_index1][m_index2]; + double value2 = getSecondOutput() + .getValue(state)[m_index1][m_index2]; + value = applyOperation(value1, value2); + } + } + + return value; +} diff --git a/OpenSim/Moco/MocoOutputBoundConstraint.h b/OpenSim/Moco/MocoOutputBoundConstraint.h new file mode 100644 index 0000000000..c69460bc43 --- /dev/null +++ b/OpenSim/Moco/MocoOutputBoundConstraint.h @@ -0,0 +1,266 @@ +#ifndef OPENSIM_MOCOOUTPUTBOUNDCONSTRAINT_H +#define OPENSIM_MOCOOUTPUTBOUNDCONSTRAINT_H +/* -------------------------------------------------------------------------- * +* OpenSim: MocoOutputBoundConstraint.h * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoConstraint.h" +#include "osimMocoDLL.h" + +namespace OpenSim { + +class MocoProblemInfo; + +/** This path constraint allows you to constrain a Model Output value to be +between two time-based functions throughout the trajectory. It is +possible to constrain the Output to match the value from a provided function; +see the 'equality_with_lower' property. You can also combine two Outputs +in a constraint by supplying a second output path and an operation to combine +them. The operations are addition, subtraction, multiplication, and division. +The first Output is always on the left hand side of the operation and the second +Output on the right hand side. The two Outputs can be different quantities, but +they must have the same type. + +Outputs of type double, SimTK::Vec3, and SimTK::SpatialVec are supported. +When using SimTK::Vec3 or SimTK::SpatialVec types, 'setOutputIndex()' +may be used to select a specific element of the Output vector. If no index is +specified, the norm of the vector will be used when calling 'calcOutputValue()'. + +If using two Outputs, the Output index will be used to select the same element +from both Outputs before the operation. If two Outputs of type SimTK::Vec3 or +SimTK::SpatialVec are provided and no index is specified, the operation will be +applied elementwise before computing the norm. Elementwise +multiplication and division operations are not supported when using two +SimTK::SpatialVec Outputs (i.e., an index must be provided). + +If a bound function is a GCVSpline, we ensure that the spline covers the entire +possible time range in the problem (using the problem's time bounds). We do +not perform such a check for other types of functions. + +@note If you omit the lower and upper bounds, then this class will not +constrain any Outputs, even if you have provided output paths. +@ingroup mocopathcon */ +class OSIMMOCO_API MocoOutputBoundConstraint : public MocoPathConstraint { + OpenSim_DECLARE_CONCRETE_OBJECT( + MocoOutputBoundConstraint, MocoPathConstraint); + +public: + MocoOutputBoundConstraint() { constructProperties(); } + + /** Set the absolute path to the Output in the model to be used in this path + constraint. The format is "/path/to/component|output_name". */ + void setOutputPath(std::string path) { set_output_path(std::move(path)); } + const std::string& getOutputPath() const { return get_output_path(); } + + /** Set the absolute path to the optional second Output in the model to be + used in this path constraint. The format is "/path/to/component|output_name". + This Output should have the same type as the first Output. If providing a + second Output, the user must also provide an operation via `setOperation()`. + */ + void setSecondOutputPath(std::string path) { + set_second_output_path(std::move(path)); + } + const std::string& getSecondOutputPath() const { + return get_second_output_path(); + } + + /** Set the operation that combines Output values where two Outputs are + provided. The supported operations include "addition", "subtraction", + "multiplication", or "division". If providing an operation, the user + must also provide a second Output path. */ + void setOperation(std::string operation) { + set_operation(std::move(operation)); + } + const std::string& getOperation() const { return get_operation(); } + + /** Set the exponent applied to the output value in the constraint. This + exponent is applied when minimizing the norm of a vector type output. */ + void setExponent(int exponent) { set_exponent(exponent); } + int getExponent() const { return get_exponent(); } + + /** Set the index to the value to be constrained when a vector type Output + is specified. For SpatialVec Outputs, indices 0, 1, and 2 refer to the + rotational components and indices 3, 4, and 5 refer to the translational + components. A value of -1 indicates to constrain the vector norm (which is + the default setting). If an index for a type double Output is provided, an + exception is thrown. */ + void setOutputIndex(int index) { set_output_index(index); } + int getOutputIndex() const { return get_output_index(); } + + + void setLowerBound(const Function& f) { set_lower_bound(f); } + void clearLowerBound() { updProperty_lower_bound().clear(); } + bool hasLowerBound() const { return !getProperty_lower_bound().empty(); } + const Function& getLowerBound() const { return get_lower_bound(); } + + void setUpperBound(const Function& f) { set_upper_bound(f); } + void clearUpperBound() { updProperty_upper_bound().clear(); } + bool hasUpperBound() const { return !getProperty_upper_bound().empty(); } + const Function& getUpperBound() const { return get_upper_bound(); } + + /// Should the Output be constrained to be equal to the lower bound (rather + /// than an inequality constraint)? In this case, the upper bound must be + /// unspecified. + void setEqualityWithLower(bool v) { set_equality_with_lower(v); } + //// @copydoc setEqualityWithLower() + bool getEqualityWithLower() const { return get_equality_with_lower(); } + +protected: + void initializeOnModelImpl( + const Model&, const MocoProblemInfo&) const override; + void calcPathConstraintErrorsImpl( + const SimTK::State&, SimTK::Vector&) const override; + +private: + OpenSim_DECLARE_PROPERTY(output_path, std::string, + "The absolute path to the Output in the model to be used in this " + "path constraint (i.e., '/path/to/component|output_name')."); + OpenSim_DECLARE_PROPERTY(second_output_path, std::string, + "The absolute path to the optional second Output in the model to be used" + " in this path constraint (i.e., '/path/to/component|output_name')."); + OpenSim_DECLARE_PROPERTY(operation, std::string, "The operation to combine " + "the two outputs: 'addition', 'subtraction', 'multiplication', or " + "'divison'."); + OpenSim_DECLARE_PROPERTY(exponent, int, + "The exponent applied to the output value in the constraint " + "(default: 1)."); + OpenSim_DECLARE_PROPERTY(output_index, int, + "The index to the value to be constrained when a vector type " + "Output is specified. For SpatialVec Outputs, indices 0, 1, " + "and 2 refer to the rotational components and indices 3, 4, " + "and 5 refer to the translational components. A value of -1 " + "indicates to constrain the vector norm (default: -1)."); + OpenSim_DECLARE_OPTIONAL_PROPERTY( + lower_bound, Function, "Lower bound as a function of time."); + OpenSim_DECLARE_OPTIONAL_PROPERTY( + upper_bound, Function, "Upper bound as a function of time."); + OpenSim_DECLARE_PROPERTY(equality_with_lower, bool, + "The output must be equal to the lower bound; " + "upper must be unspecified (default: false)."); + + void constructProperties(); + + /** Initialize additional information when there are two Outputs: + the second Output, the operation, and the dependsOnStage. */ + void initializeComposite() const; + + /** Calculate the Output value of one Output. */ + double calcSingleOutputValue(const SimTK::State&) const; + /** Calculate the two Output values and apply the operation. */ + double calcCompositeOutputValue(const SimTK::State&) const; + + /** Calculate the Output value for the provided SimTK::State. Do not + call this function until 'initializeOnModelBase()' has been called. */ + double calcOutputValue(const SimTK::State&) const; + + /** Raise a value to the exponent set via 'setExponent()'. Do not call this + function until 'initializeOnModelBase()' has been called. */ + double setValueToExponent(double value) const { + return m_power_function(value); + } + + /** Get the "depends-on stage", or the SimTK::Stage we need to realize the + system to in order to calculate the Output value. */ + const SimTK::Stage& getDependsOnStage() const { + return m_dependsOnStage; + } + + /** Get a reference to the Output for this path constraint. */ + template + const Output& getOutput() const { + return static_cast&>(m_output.getRef()); + } + /** Get a reference to the second Output for this path constraint. */ + template + const Output& getSecondOutput() const { + return static_cast&>(m_second_output.getRef()); + } + + /** Apply the operation to two double values. */ + double applyOperation(double value1, double value2) const { + switch (m_operation) { + case Addition : return value1 + value2; + case Subtraction : return value1 - value2; + case Multiplication : return value1 * value2; + case Division : return value1 / value2; + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "double type Outputs."); + } + } + /** Apply the elementwise operation to two SimTK::Vec3 values. */ + double applyOperation(const SimTK::Vec3& value1, + const SimTK::Vec3& value2) const { + switch (m_operation) { + case Addition : return (value1 + value2).norm(); + case Subtraction : return (value1 - value2).norm(); + case Multiplication : return value1.elementwiseMultiply(value2).norm(); + case Division : return value1.elementwiseDivide(value2).norm(); + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "SimTK::Vec3 type Outputs."); + } + } + /** Apply the elementwise operation to two SimTK::SpatialVec values. + Multiplication and divison operations are not supported for SpatialVec + Outputs without an index. */ + double applyOperation(const SimTK::SpatialVec& value1, + const SimTK::SpatialVec& value2) const { + switch (m_operation) { + case Addition : return (value1 + value2).norm(); + case Subtraction : return (value1 - value2).norm(); + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "SimTK::SpatialVec type Outputs."); + } + } + + enum DataType { + Type_double, + Type_Vec3, + Type_SpatialVec, + }; + /** Get the string of the data type, for prints and error messages. */ + std::string getDataTypeString(DataType type) const { + switch (type) { + case Type_double : return "double"; + case Type_Vec3 : return "SimTK::Vec3"; + case Type_SpatialVec : return "SimTK::SpatialVec"; + default : return "empty Datatype"; + } + } + + mutable DataType m_data_type; + mutable SimTK::ReferencePtr m_output; + mutable SimTK::ReferencePtr m_second_output; + enum OperationType { Addition, Subtraction, Multiplication, Division }; + mutable OperationType m_operation; + mutable bool m_useCompositeOutputValue; + mutable std::function m_power_function; + mutable int m_index1; + mutable int m_index2; + mutable bool m_boundVectorNorm; + mutable SimTK::Stage m_dependsOnStage = SimTK::Stage::Acceleration; + mutable bool m_hasLower; + mutable bool m_hasUpper; + +}; + +} // namespace OpenSim + +#endif // OPENSIM_MOCOOUTPUTBOUNDCONSTRAINT_H + diff --git a/OpenSim/Moco/MocoOutputConstraint.cpp b/OpenSim/Moco/MocoOutputConstraint.cpp index 3cb64a5088..6a299b3a1c 100644 --- a/OpenSim/Moco/MocoOutputConstraint.cpp +++ b/OpenSim/Moco/MocoOutputConstraint.cpp @@ -22,6 +22,8 @@ using namespace OpenSim; void MocoOutputConstraint::constructProperties() { constructProperty_output_path(""); + constructProperty_second_output_path(""); + constructProperty_operation(""); constructProperty_exponent(1); constructProperty_output_index(-1); } @@ -93,6 +95,75 @@ void MocoOutputConstraint::initializeOnModelImpl(const Model&, // There is only one scalar constraint per Output. setNumEquations(1); + + m_useCompositeOutputValue = false; + // if there's a second output, initialize it + if (get_second_output_path() != "") { + m_useCompositeOutputValue = true; + initializeComposite(); + } else if (get_operation() != "") { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("An operation was provided " + "but a second Output path was not provided. Either provide no " + "operation with a single Output, or provide a value to both " + "setOperation() and setSecondOutputPath().")); + } +} + +void MocoOutputConstraint::initializeComposite() const { + if (get_operation() == "addition") { + m_operation = Addition; + } else if (get_operation() == "subtraction") { + m_operation = Subtraction; + } else if (get_operation() == "multiplication") { + m_operation = Multiplication; + } else if (get_operation() == "division") { + m_operation = Division; + } else if (get_operation() == "") { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("A second Output path was " + "provided, but no operation was provided. Use setOperation() to" + "provide an operation")); + } else { + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Invalid operation: '{}', must " + "be 'addition', 'subtraction', 'multiplication', or 'division'.", + get_operation())); + } + + std::string componentPath, outputName, channelName, alias; + AbstractInput::parseConnecteePath(get_second_output_path(), componentPath, + outputName, channelName, alias); + const auto& component = getModel().getComponent(componentPath); + const auto* abstractOutput = &component.getOutput(outputName); + + if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(getOutputIndex() != -1, Exception, + "An Output index was provided, but the second Output is of type" + " 'double'.") + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_double, Exception, + "Output types do not match. The second Output is of type double" + " but the first is of type {}.", getDataTypeString(m_data_type)); + } else if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_Vec3, Exception, + "Output types do not match. The second Output is of type " + "SimTK::Vec3 but the first is of type {}.", + getDataTypeString(m_data_type)); + } else if (dynamic_cast*>(abstractOutput)) { + OPENSIM_THROW_IF_FRMOBJ(m_data_type != Type_SpatialVec, Exception, + "Output types do not match. The second Output is of type " + "SimTK::SpatialVec but the first is of type {}.", + getDataTypeString(m_data_type)); + OPENSIM_THROW_IF_FRMOBJ(m_minimizeVectorNorm && + (m_operation == Multiplication || m_operation == Division), + Exception, "Multiplication and division operations are not " + "supported with Output type SimTK::SpatialVec without an index.") + } else { + OPENSIM_THROW_FRMOBJ(Exception, + "Data type of specified second Output not supported."); + } + m_second_output.reset(abstractOutput); + + if (getDependsOnStage() < m_second_output->getDependsOnStage()) { + m_dependsOnStage = m_second_output->getDependsOnStage(); + } } void MocoOutputConstraint::calcPathConstraintErrorsImpl( @@ -101,13 +172,19 @@ void MocoOutputConstraint::calcPathConstraintErrorsImpl( } double MocoOutputConstraint::calcOutputValue(const SimTK::State& state) const { + if (m_useCompositeOutputValue) { + return calcCompositeOutputValue(state); + } + return calcSingleOutputValue(state); +} + +double MocoOutputConstraint::calcSingleOutputValue(const SimTK::State& state) const { getModel().getSystem().realize(state, m_output->getDependsOnStage()); double value = 0; if (m_data_type == Type_double) { value = static_cast*>(m_output.get()) ->getValue(state); - } else if (m_data_type == Type_Vec3) { if (m_minimizeVectorNorm) { value = static_cast*>(m_output.get()) @@ -116,7 +193,6 @@ double MocoOutputConstraint::calcOutputValue(const SimTK::State& state) const { value = static_cast*>(m_output.get()) ->getValue(state)[m_index1]; } - } else if (m_data_type == Type_SpatialVec) { if (m_minimizeVectorNorm) { value = static_cast*>(m_output.get()) @@ -130,16 +206,56 @@ double MocoOutputConstraint::calcOutputValue(const SimTK::State& state) const { return value; } +double MocoOutputConstraint::calcCompositeOutputValue(const SimTK::State& state) const { + getModel().getSystem().realize(state, getDependsOnStage()); + + double value = 0; + if (m_data_type == Type_double) { + double value1 = getOutput().getValue(state); + double value2 = getSecondOutput().getValue(state); + value = applyOperation(value1, value2); + } else if (m_data_type == Type_Vec3) { + if (m_minimizeVectorNorm) { + const SimTK::Vec3& value1 = getOutput().getValue(state); + const SimTK::Vec3& value2 = getSecondOutput().getValue(state); + value = applyOperation(value1, value2); + } else { + double value1 = getOutput().getValue(state)[m_index1]; + double value2 = getSecondOutput().getValue(state)[m_index1]; + value = applyOperation(value1, value2); + } + } else if (m_data_type == Type_SpatialVec) { + if (m_minimizeVectorNorm) { + const SimTK::SpatialVec& value1 = getOutput() + .getValue(state); + const SimTK::SpatialVec& value2 = getSecondOutput() + .getValue(state); + value = applyOperation(value1, value2); + } else { + double value1 = getOutput().getValue(state) + [m_index1][m_index2]; + double value2 = getSecondOutput().getValue(state) + [m_index1][m_index2]; + value = applyOperation(value1, value2); + } + } + + return value; +} + + void MocoOutputConstraint::printDescriptionImpl() const { // Output path. std::string str = fmt::format(" output: {}", getOutputPath()); + if (m_useCompositeOutputValue) { + str += fmt::format("\n second output: {}", getSecondOutputPath()); + // Operation. + str += fmt::format("\n operation: {}", get_operation()); + } + // Output type. - std::string type; - if (m_data_type == Type_double) { type = "double"; } - else if (m_data_type == Type_Vec3) { type = "SimTK::Vec3"; } - else if (m_data_type == Type_SpatialVec) { type = "SimTK::SpatialVec"; } - str += fmt::format(", type: {}", type); + str += fmt::format(", type: {}", getDataTypeString(m_data_type)); // Output index (if relevant). if (getOutputIndex() != -1) { @@ -149,5 +265,5 @@ void MocoOutputConstraint::printDescriptionImpl() const { // Exponent. str += fmt::format(", exponent: {}", getExponent()); - log_cout(str); + log_info(str); } \ No newline at end of file diff --git a/OpenSim/Moco/MocoOutputConstraint.h b/OpenSim/Moco/MocoOutputConstraint.h index 546664b66e..769c3ea16d 100644 --- a/OpenSim/Moco/MocoOutputConstraint.h +++ b/OpenSim/Moco/MocoOutputConstraint.h @@ -22,6 +22,25 @@ namespace OpenSim { +/** This path constraint allows you to constrain a Model Output value throughout the +trajectory. You can also combine two Outputs in a constraint by supplying a +second output path and an operation to combine them. The operations are addition, +subtraction, multiplication, and division. The first Output is always on the +left hand side of the operation and the second Output on the right hand side. +The two Outputs can be different quantities, but they must be the same type. + +Outputs of type double, SimTK::Vec3, and SimTK::SpatialVec are supported. +When using SimTK::Vec3 or SimTK::SpatialVec types, 'setOutputIndex()' +may be used to select a specific element of the Output vector. If no index is +specified, the norm of the vector will be used when calling 'calcOutputValue()'. + +If using two Outputs, the Output index will be used to select the same element +from both Outputs before the operation. If two Outputs of type SimTK::Vec3 or +SimTK::SpatialVec are provided and no index is specified, the operation will be +applied elementwise before computing the norm. Elementwise +multiplication and division operations are not supported when using two +SimTK::SpatialVec Outputs (i.e., an index must be provided). +@ingroup mocopathcon */ class OSIMMOCO_API MocoOutputConstraint : public MocoPathConstraint { OpenSim_DECLARE_CONCRETE_OBJECT(MocoOutputConstraint, MocoPathConstraint); @@ -34,6 +53,27 @@ class OSIMMOCO_API MocoOutputConstraint : public MocoPathConstraint { void setOutputPath(std::string path) { set_output_path(std::move(path)); } const std::string& getOutputPath() const { return get_output_path(); } + /** Set the absolute path to the optional second Output in the model to be + used in this path constraint. The format is "/path/to/component|output_name". + This Output should have the same type as the first Output. If providing a + second Output, the user must also provide an operation via `setOperation()`. + */ + void setSecondOutputPath(std::string path) { + set_second_output_path(std::move(path)); + } + const std::string& getSecondOutputPath() const { + return get_second_output_path(); + } + + /** Set the operation that combines Output values where two Outputs are + provided. The supported operations include "addition", "subtraction", + "multiplication", or "division". If providing an operation, the user + must also provide a second Output path. */ + void setOperation(std::string operation) { + set_operation(std::move(operation)); + } + const std::string& getOperation() const { return get_operation(); } + /** Set the exponent applied to the output value in the constraint. This exponent is applied when minimizing the norm of a vector type output. */ void setExponent(int exponent) { set_exponent(exponent); } @@ -55,9 +95,7 @@ class OSIMMOCO_API MocoOutputConstraint : public MocoPathConstraint { const SimTK::State& state, SimTK::Vector& errors) const override; void printDescriptionImpl() const override; - /** Calculate the Output value for the provided SimTK::State. If using a - vector Output, either the vector norm or vector element will be returned, - depending on whether an index was provided via 'setOutputIndex()'. Do not + /** Calculate the Output value for the provided SimTK::State. Do not call this function until 'initializeOnModelBase()' has been called. */ double calcOutputValue(const SimTK::State&) const; @@ -75,8 +113,14 @@ class OSIMMOCO_API MocoOutputConstraint : public MocoPathConstraint { private: OpenSim_DECLARE_PROPERTY(output_path, std::string, - "The absolute path to the output in the model to be used in this " - "path constraint."); + "The absolute path to the Output in the model to be used in this " + "path constraint (i.e., '/path/to/component|output_name')."); + OpenSim_DECLARE_PROPERTY(second_output_path, std::string, + "The absolute path to the optional second Output in the model to be used" + " in this path constraint (i.e., '/path/to/component|output_name')."); + OpenSim_DECLARE_PROPERTY(operation, std::string, "The operation to combine " + "the two outputs: 'addition', 'subtraction', 'multiplication', or " + "'divison'."); OpenSim_DECLARE_PROPERTY(exponent, int, "The exponent applied to the output value in the constraint " "(default: 1)."); @@ -88,13 +132,86 @@ class OSIMMOCO_API MocoOutputConstraint : public MocoPathConstraint { "indicates to constrain the vector norm (default: -1)."); void constructProperties(); + /** Initialize additional information when there are two Outputs: + the second Output, the operation, and the dependsOnStage. */ + void initializeComposite() const; + + /** Calculate the Output value of one Output. */ + double calcSingleOutputValue(const SimTK::State&) const; + /** Calculate the two Output values and apply the operation. */ + double calcCompositeOutputValue(const SimTK::State&) const; + + /** Get a reference to the Output for this path constraint. */ + template + const Output& getOutput() const { + return static_cast&>(m_output.getRef()); + } + /** Get a reference to the second Output for this path constraint. */ + template + const Output& getSecondOutput() const { + return static_cast&>(m_second_output.getRef()); + } + + /** Apply the operation to two double values. */ + double applyOperation(double value1, double value2) const { + switch (m_operation) { + case Addition : return value1 + value2; + case Subtraction : return value1 - value2; + case Multiplication : return value1 * value2; + case Division : return value1 / value2; + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "double type Outputs."); + } + } + /** Apply the elementwise operation to two SimTK::Vec3 values. */ + double applyOperation(const SimTK::Vec3& value1, + const SimTK::Vec3& value2) const { + switch (m_operation) { + case Addition : return (value1 + value2).norm(); + case Subtraction : return (value1 - value2).norm(); + case Multiplication : return value1.elementwiseMultiply(value2).norm(); + case Division : return value1.elementwiseDivide(value2).norm(); + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "SimTK::Vec3 type Outputs."); + } + } + /** Apply the elementwise operation to two SimTK::SpatialVec values. + Multiplication and divison operations are not supported for SpatialVec Outputs + without an index. */ + double applyOperation(const SimTK::SpatialVec& value1, + const SimTK::SpatialVec& value2) const { + switch (m_operation) { + case Addition : return (value1 + value2).norm(); + case Subtraction : return (value1 - value2).norm(); + default : OPENSIM_THROW_FRMOBJ(Exception, + "Internal error: invalid operation type for " + "SimTK::SpatialVec type Outputs."); + } + } + enum DataType { Type_double, Type_Vec3, Type_SpatialVec, }; + /** Get the string of the data type, for prints and error messages. */ + std::string getDataTypeString(DataType type) const { + switch (type) { + case Type_double : return "double"; + case Type_Vec3 : return "SimTK::Vec3"; + case Type_SpatialVec : return "SimTK::SpatialVec"; + default : return "empty Datatype"; + } + } + mutable DataType m_data_type; mutable SimTK::ReferencePtr m_output; + mutable SimTK::ReferencePtr m_second_output; + enum OperationType { Addition, Subtraction, Multiplication, Division }; + mutable OperationType m_operation; + mutable bool m_useCompositeOutputValue; mutable std::function m_power_function; mutable int m_index1; mutable int m_index2; diff --git a/OpenSim/Moco/MocoParameter.cpp b/OpenSim/Moco/MocoParameter.cpp index 9698ecfb97..4a1c4c9a71 100644 --- a/OpenSim/Moco/MocoParameter.cpp +++ b/OpenSim/Moco/MocoParameter.cpp @@ -140,7 +140,7 @@ void MocoParameter::printDescription() const { fmt::format("{}", getProperty_property_element().getValue()); } - log_cout(" {}. model property name: {}. component paths: {}. " + log_info(" {}. model property name: {}. component paths: {}. " "property element: {}. bounds: {}", getName(), getPropertyName(), fmt::join(componentPaths, ", "), propertyElementStr, getBounds()); diff --git a/OpenSim/Moco/MocoParameter.h b/OpenSim/Moco/MocoParameter.h index 60456bd14b..aaa47c3e53 100644 --- a/OpenSim/Moco/MocoParameter.h +++ b/OpenSim/Moco/MocoParameter.h @@ -20,8 +20,8 @@ #include "MocoBounds.h" -#include #include +#include #include namespace OpenSim { @@ -141,6 +141,9 @@ class OSIMMOCO_API MocoParameter : public Object { { set_property_name(propertyName); } void appendComponentPath(const std::string& componentPath) { append_component_paths(componentPath); } + int getPropertyElement() const { + return get_property_element(); + } /** For use by solvers. This performs error checks and caches information about the model that is useful during the optimization. diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index f0e5a2f091..279cf13fa3 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -1043,7 +1043,7 @@ void MocoProblemRep::printDescription() const { } else { ss << "(total: " << size << ")"; } - log_cout(ss.str()); + log_info(ss.str()); }; printHeaderLine("Costs", m_costs.size()); diff --git a/OpenSim/Moco/MocoProblemRep.h b/OpenSim/Moco/MocoProblemRep.h index 2e627884d0..66ee12ebb6 100644 --- a/OpenSim/Moco/MocoProblemRep.h +++ b/OpenSim/Moco/MocoProblemRep.h @@ -266,7 +266,7 @@ class OSIMMOCO_API MocoProblemRep { } /// Print a description of this problem, including costs and variable - /// bounds. Printing is done using OpenSim::log_cout(). + /// bounds. Printing is done using OpenSim::log_info(). void printDescription() const; /// @name Interface for solvers diff --git a/OpenSim/Moco/MocoStateBoundConstraint.cpp b/OpenSim/Moco/MocoStateBoundConstraint.cpp new file mode 100644 index 0000000000..18f9f500f7 --- /dev/null +++ b/OpenSim/Moco/MocoStateBoundConstraint.cpp @@ -0,0 +1,142 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: MocoStateBoundConstraint.cpp * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoStateBoundConstraint.h" + +#include "MocoProblemInfo.h" + +#include +#include + +using namespace OpenSim; + +void MocoStateBoundConstraint::constructProperties() { + constructProperty_state_paths(); + constructProperty_lower_bound(); + constructProperty_upper_bound(); + constructProperty_equality_with_lower(false); +} + +void MocoStateBoundConstraint::initializeOnModelImpl( + const Model& model, const MocoProblemInfo& problemInfo) const { + // Get the state variable index map. + auto systemStateIndexMap = createSystemYIndexMap(model); + + m_hasLower = !getProperty_lower_bound().empty(); + m_hasUpper = !getProperty_upper_bound().empty(); + if (!getProperty_state_paths().empty() && !m_hasLower && !m_hasUpper) { + log_warn("In MocoStateBoundConstraint '{}', state paths are " + "specified but no bounds are provided.", getName()); + } + // Make sure there are no nonexistent states. + if (m_hasLower || m_hasUpper) { + for (int i = 0; i < getProperty_state_paths().size(); ++i) { + const auto& thisName = get_state_paths(i); + OPENSIM_THROW_IF_FRMOBJ(!systemStateIndexMap.count(thisName), + Exception, + "State path '{}' was provided but no such " + "state exists in the model.", + thisName) + m_stateIndices.push_back(systemStateIndexMap[thisName]); + } + } + + OPENSIM_THROW_IF_FRMOBJ(get_equality_with_lower() && m_hasUpper, Exception, + "If equality_with_lower==true, upper bound function must not be " + "set."); + OPENSIM_THROW_IF_FRMOBJ(get_equality_with_lower() && !m_hasLower, Exception, + "If equality_with_lower==true, lower bound function must be set."); + + auto checkTimeRange = [&](const Function& f) { + if (auto* spline = dynamic_cast(&f)) { + OPENSIM_THROW_IF_FRMOBJ( + spline->getMinX() > problemInfo.minInitialTime, Exception, + "The function's minimum domain value ({}) must " + "be less than or equal to the minimum possible " + "initial time ({}).", + spline->getMinX(), problemInfo.minInitialTime); + OPENSIM_THROW_IF_FRMOBJ( + spline->getMaxX() < problemInfo.maxFinalTime, Exception, + "The function's maximum domain value ({}) must " + "be greater than or equal to the maximum possible " + "final time ({}).", + spline->getMaxX(), problemInfo.maxFinalTime); + } + }; + if (m_hasLower) checkTimeRange(get_lower_bound()); + if (m_hasUpper) checkTimeRange(get_upper_bound()); + + int numEqsPerState; + if (get_equality_with_lower()) { + numEqsPerState = 1; + } else { + numEqsPerState = (int)m_hasLower + (int)m_hasUpper; + } + + setNumEquations(numEqsPerState * (int)m_stateIndices.size()); + + // TODO: setConstraintInfo() is not really intended for use here. + MocoConstraintInfo info; + std::vector bounds; + for (int i = 0; i < (int)m_stateIndices.size(); ++i) { + if (get_equality_with_lower()) { + bounds.emplace_back(0, 0); + } else { + // The lower and upper bounds on the path constraint must be + // constants, so we cannot use the lower and upper bound functions + // directly. Therefore, we use a pair of constraints where the + // lower/upper bound functions are part of the path constraint + // functions and the lower/upper bounds for the path constraints are + // -inf, 0, and/or inf. + // If a lower bound function is provided, we enforce + // lower_bound_function <= state + // by creating the constraint + // 0 <= state - lower_bound_function <= inf + if (m_hasLower) { bounds.emplace_back(0, SimTK::Infinity); } + // If an upper bound function is provided, we enforce + // state <= upper_bound_function + // by creating the constraint + // -inf <= state - upper_bound_function <= 0 + if (m_hasUpper) { bounds.emplace_back(-SimTK::Infinity, 0); } + } + } + info.setBounds(bounds); + const_cast(this)->setConstraintInfo(info); +} + +void MocoStateBoundConstraint::calcPathConstraintErrorsImpl( + const SimTK::State& state, SimTK::Vector& errors) const { + const auto& svValues = state.getY(); + int iconstr = 0; + int istate = 0; + SimTK::Vector time(1); + for (const auto& stateIndex : m_stateIndices) { + const auto& stateValue = svValues[stateIndex]; + time[0] = state.getTime(); + // These if-statements work correctly for either value of + // equality_with_lower. + if (m_hasLower) { + errors[iconstr++] = stateValue - get_lower_bound().calcValue(time); + } + if (m_hasUpper) { + errors[iconstr++] = stateValue - get_upper_bound().calcValue(time); + } + ++istate; + } +} + diff --git a/OpenSim/Moco/MocoStateBoundConstraint.h b/OpenSim/Moco/MocoStateBoundConstraint.h new file mode 100644 index 0000000000..11e3821b5d --- /dev/null +++ b/OpenSim/Moco/MocoStateBoundConstraint.h @@ -0,0 +1,109 @@ +#ifndef OPENSIM_MOCOSTATEBOUNDCONSTRAINT_H +#define OPENSIM_MOCOSTATEBOUNDCONSTRAINT_H +/* -------------------------------------------------------------------------- * + * OpenSim: MocoStateBoundConstraint.h * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoConstraint.h" +#include "osimMocoDLL.h" + +/** This path contraint allows you to bound any number of state variables +between two time-based functions. It is possible to constrain the state variable +to match the value from a provided function; see the equality_with_lower +property. + +If a function is a GCVSpline, we ensure that the spline covers the entire +possible time range in the problem (using the problem's time bounds). We do +not perform such a check for other types of functions. + +@note If you omit the lower and upper bounds, then this class will not +constrain any state variable, even if you have provided state paths. + +@ingroup mocopathcon */ +namespace OpenSim { + +class OSIMMOCO_API MocoStateBoundConstraint : public MocoPathConstraint { + OpenSim_DECLARE_CONCRETE_OBJECT( + MocoStateBoundConstraint, MocoPathConstraint); + +public: + MocoStateBoundConstraint() { constructProperties(); } + + /** Add a state path (absolute path to state varaibles in the model) + to be constrained by this class (e.g., "/slider/position/speed"). */ + void addStatePath(std::string statePath) { + append_state_paths(std::move(statePath)); + } + void setStatePaths(const std::vector& statePaths) { + updProperty_state_paths().clear(); + for (const auto& path : statePaths) { append_state_paths(path); } + } + void clearStatePaths() { updProperty_state_paths().clear(); } + std::vector getStatePaths() const { + std::vector paths; + for (int i = 0; i < getProperty_state_paths().size(); ++i) { + paths.push_back(get_state_paths(i)); + } + return paths; + } + + void setLowerBound(const Function& f) { set_lower_bound(f); } + void clearLowerBound() { updProperty_lower_bound().clear(); } + bool hasLowerBound() const { return !getProperty_lower_bound().empty(); } + const Function& getLowerBound() const { return get_lower_bound(); } + + void setUpperBound(const Function& f) { set_upper_bound(f); } + void clearUpperBound() { updProperty_upper_bound().clear(); } + bool hasUpperBound() const { return !getProperty_upper_bound().empty(); } + const Function& getUpperBound() const { return get_upper_bound(); } + + /** Set whether the state should be constrained to be equal to the lower + bound (rather than an inequality constraint). In this case, the upper bound + must be unspecified. */ + void setEqualityWithLower(bool v) { set_equality_with_lower(v); } + //// @copydoc setEqualityWithLower() + bool getEqualityWithLower() const { return get_equality_with_lower(); } + + +protected: + void initializeOnModelImpl( + const Model& model, const MocoProblemInfo&) const override; + + void calcPathConstraintErrorsImpl( + const SimTK::State& state, SimTK::Vector& errors) const override; + +private: + OpenSim_DECLARE_LIST_PROPERTY(state_paths, std::string, + "Constrain the state variables specified by these paths.") + OpenSim_DECLARE_OPTIONAL_PROPERTY( + lower_bound, Function, "Lower bound as a function of time."); + OpenSim_DECLARE_OPTIONAL_PROPERTY( + upper_bound, Function, "Upper bound as a function of time."); + OpenSim_DECLARE_PROPERTY(equality_with_lower, bool, + "The state must be equal to the lower bound; " + "upper must be unspecified (default: false)."); + + void constructProperties(); + + mutable bool m_hasLower; + mutable bool m_hasUpper; + mutable std::vector m_stateIndices; +}; + +} // namespace OpenSim + +#endif // OPENSIM_MOCOSTATEBOUNDCONSTRAINT_H diff --git a/OpenSim/Moco/MocoTrajectory.cpp b/OpenSim/Moco/MocoTrajectory.cpp index e015a4298d..d2f4e3ae77 100644 --- a/OpenSim/Moco/MocoTrajectory.cpp +++ b/OpenSim/Moco/MocoTrajectory.cpp @@ -876,7 +876,7 @@ void MocoTrajectory::resample(SimTK::Vector time) { // does not resize the slacks trajectory. for (int icol = 0; icol < m_slacks.ncol(); ++icol) { m_slacks.updCol(icol) = - interpolate(m_time, m_slacks.col(icol), m_time, true); + interpolate(m_time, m_slacks.col(icol), m_time, true, true); } const TimeSeriesTable table = convertToTable(); diff --git a/OpenSim/Moco/MocoTropterSolver.cpp b/OpenSim/Moco/MocoTropterSolver.cpp index bb3b2ec7cd..0f17c0ba58 100644 --- a/OpenSim/Moco/MocoTropterSolver.cpp +++ b/OpenSim/Moco/MocoTropterSolver.cpp @@ -97,6 +97,8 @@ MocoTropterSolver::createTropterSolver( // is set as the transcription scheme. if (getProblemRep().getNumKinematicConstraintEquations()) { + checkPropertyValueIsInSet( + getProperty_kinematic_constraint_method(), {"Posa2016"}); OPENSIM_THROW_IF(get_transcription_scheme() != "hermite-simpson" && get_enforce_constraint_derivatives(), Exception, @@ -370,6 +372,7 @@ MocoSolution MocoTropterSolver::solveImpl() const { !get_minimize_lagrange_multipliers()) { checkConstraintJacobianRank(mocoSolution); } + checkSlackVariables(mocoSolution); // TODO move this to convert(): const long long elapsed = stopwatch.getElapsedTimeInNs(); diff --git a/OpenSim/Moco/MocoVariableInfo.cpp b/OpenSim/Moco/MocoVariableInfo.cpp index e5835626c7..25db7604ef 100644 --- a/OpenSim/Moco/MocoVariableInfo.cpp +++ b/OpenSim/Moco/MocoVariableInfo.cpp @@ -74,7 +74,7 @@ void MocoVariableInfo::printDescription() const { if (final.isSet()) { str += fmt::format(" final: {}", final); } - log_cout(str); + log_info(str); } void MocoVariableInfo::constructProperties() { diff --git a/OpenSim/Moco/RegisterTypes_osimMoco.cpp b/OpenSim/Moco/RegisterTypes_osimMoco.cpp index 98c385c70c..eba929ae94 100644 --- a/OpenSim/Moco/RegisterTypes_osimMoco.cpp +++ b/OpenSim/Moco/RegisterTypes_osimMoco.cpp @@ -24,6 +24,8 @@ #include "MocoBounds.h" #include "MocoCasADiSolver/MocoCasADiSolver.h" #include "MocoControlBoundConstraint.h" +#include "MocoOutputBoundConstraint.h" +#include "MocoStateBoundConstraint.h" #include "MocoFrameDistanceConstraint.h" #include "MocoGoal/MocoAccelerationTrackingGoal.h" #include "MocoGoal/MocoAngularVelocityTrackingGoal.h" @@ -31,6 +33,7 @@ #include "MocoGoal/MocoContactTrackingGoal.h" #include "MocoGoal/MocoControlGoal.h" #include "MocoGoal/MocoControlTrackingGoal.h" +#include "MocoGoal/MocoExpressionBasedParameterGoal.h" #include "MocoGoal/MocoGoal.h" #include "MocoGoal/MocoInitialActivationGoal.h" #include "MocoGoal/MocoInitialForceEquilibriumDGFGoal.h" @@ -79,6 +82,7 @@ OSIMMOCO_API void RegisterTypes_osimMoco() { Object::registerType(MocoControlGoal()); Object::registerType(MocoSumSquaredStateGoal()); Object::registerType(MocoControlTrackingGoal()); + Object::registerType(MocoExpressionBasedParameterGoal()); Object::registerType(MocoInitialActivationGoal()); Object::registerType(MocoInitialVelocityEquilibriumDGFGoal()); Object::registerType(MocoInitialForceEquilibriumDGFGoal()); @@ -111,6 +115,8 @@ OSIMMOCO_API void RegisterTypes_osimMoco() { Object::registerType(MocoTropterSolver()); Object::registerType(MocoControlBoundConstraint()); + Object::registerType(MocoOutputBoundConstraint()); + Object::registerType(MocoStateBoundConstraint()); Object::registerType(MocoFrameDistanceConstraint()); Object::registerType(MocoFrameDistanceConstraintPair()); diff --git a/OpenSim/Moco/Test/testMocoConstraints.cpp b/OpenSim/Moco/Test/testMocoConstraints.cpp index 3381e24394..ce4bbaf062 100644 --- a/OpenSim/Moco/Test/testMocoConstraints.cpp +++ b/OpenSim/Moco/Test/testMocoConstraints.cpp @@ -17,6 +17,7 @@ * -------------------------------------------------------------------------- */ #include + #include #include #include @@ -29,8 +30,8 @@ #include #include -#include #include "Testing.h" +#include using Catch::Matchers::ContainsSubstring; using Catch::Approx; @@ -52,6 +53,53 @@ static const double ConstraintTol = 1e-10; /// (borrowed from Simbody's 'testConstraints.cpp'). #define MACHINE_TEST(a, b) SimTK_TEST_EQ_SIZE(a, b, 10 * state.getNU()) +/// creates a model with one sliding mass +std::unique_ptr createSlidingMassModel() { + auto model = make_unique(); + model->setName("sliding_mass"); + model->set_gravity(SimTK::Vec3(0, 0, 0)); + auto* body = new Body("body", 10.0, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + body->attachGeometry(new Sphere(0.05)); + + // Allows translation along x. + auto* joint = new SliderJoint("slider", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* actu = new CoordinateActuator(); + actu->setCoordinate(&coord); + actu->setName("actuator"); + actu->setOptimalForce(1); + model->addComponent(actu); + + return model; +} + +/// create a model with two sliding masses +std::unique_ptr createDoubleSlidingMassModel() { + std::unique_ptr model = createSlidingMassModel(); + auto* body = new Body("body2", 10.0, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + body->attachGeometry(new Sphere(0.05)); + + // Allows translation along x. + auto* joint = new SliderJoint("slider2", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* actu = new CoordinateActuator(); + actu->setCoordinate(&coord); + actu->setName("actuator2"); + actu->setOptimalForce(1); + model->addComponent(actu); + + model->finalizeConnections(); + return model; +} + TEST_CASE("(Dummy test to support discovery in Resharper)") { REQUIRE(true); } /// Create a model consisting of a chain of bodies. This model is nearly @@ -532,6 +580,49 @@ MocoTrajectory runForwardSimulation( return forwardSolution; } +// Check that the constraint errors from a MocoSolution are within a specified +// tolerance. Also, check that the slack variables have a reasonable magnitude. +void checkConstraintErrors(const MocoSolution& solution, const Model& model, + bool enforce_constraint_derivatives, const std::string& method) { + StatesTrajectory statesTraj = solution.exportToStatesTrajectory(model); + for (int i = 0; i < (int)statesTraj.getSize(); ++i) { + const auto& s = statesTraj.get(i); + model.realizeAcceleration(s); + const auto& qerr = s.getQErr(); + const auto& uerr = s.getUErr(); + const auto& udoterr = s.getUDotErr(); + + // If we're using the Posa et al. 2016 method and not enforcing + // constraint derivatives, we'll loosen the tolerance for the constraint + // at the midpoint of the mesh intervals, since we do not explicitly + // enforce the constraints at these points. + bool loosen_tol = !enforce_constraint_derivatives && + method == "Posa2016" && + i % 2 != 0; + double tol = loosen_tol ? 1e-3 : 1e-6; + for (int j = 0; j < qerr.size(); ++j) { + REQUIRE_THAT(qerr[j], Catch::Matchers::WithinAbs(0, tol)); + } + if (enforce_constraint_derivatives) { + for (int j = 0; j < uerr.size(); ++j) { + REQUIRE_THAT(uerr[j], Catch::Matchers::WithinAbs(0, tol)); + } + for (int j = 0; j < udoterr.size(); ++j) { + REQUIRE_THAT(udoterr[j], Catch::Matchers::WithinAbs(0, tol)); + } + } + } + + // If problems have converged and all kinematic constraints are satisfied, + // we expect the slack variables to be reasonably small. + const auto& slacks = solution.getSlacksTrajectory(); + for (int icol = 0; icol < slacks.ncol(); ++icol) { + CAPTURE(slacks.col(icol)); + REQUIRE_THAT(SimTK::max(SimTK::abs(slacks.col(icol))), + Catch::Matchers::WithinAbs(0, 1e-5)); + } +} + /// Direct collocation subtests. /// ---------------------------- @@ -541,7 +632,10 @@ MocoTrajectory runForwardSimulation( /// control effort. template void testDoublePendulumPointOnLine( - bool enforce_constraint_derivatives, std::string dynamics_mode) { + bool enforce_constraint_derivatives, std::string dynamics_mode, + const std::string& kinematic_constraint_method = "Posa2016", + const std::string& transcription_scheme = "hermite-simpson", + int num_mesh_intervals = 20) { MocoStudy study; study.setName("double_pendulum_point_on_line"); MocoProblem& mp = study.updProblem(); @@ -577,17 +671,20 @@ void testDoublePendulumPointOnLine( mp.setControlInfo("/tau0", {-100, 100}); mp.setControlInfo("/tau1", {-100, 100}); - mp.addGoal(); + auto* effort = mp.addGoal(); auto& ms = study.initSolver(); - ms.set_num_mesh_intervals(20); + ms.set_num_mesh_intervals(num_mesh_intervals); ms.set_verbosity(2); ms.set_optim_solver("ipopt"); ms.set_optim_convergence_tolerance(1e-3); - ms.set_transcription_scheme("hermite-simpson"); + ms.set_transcription_scheme(transcription_scheme); ms.set_enforce_constraint_derivatives(enforce_constraint_derivatives); - ms.set_minimize_lagrange_multipliers(true); - ms.set_lagrange_multiplier_weight(10); + ms.set_kinematic_constraint_method(kinematic_constraint_method); + if (!enforce_constraint_derivatives) { + ms.set_minimize_lagrange_multipliers(true); + ms.set_lagrange_multiplier_weight(10); + } ms.set_multibody_dynamics_mode(dynamics_mode); ms.setGuess("bounds"); @@ -595,6 +692,13 @@ void testDoublePendulumPointOnLine( solution.write("testConstraints_testDoublePendulumPointOnLine.sto"); // moco.visualize(solution); + // Check that the constraint errors are within a specified tolerance. + model->initSystem(); + checkConstraintErrors(solution, *model, enforce_constraint_derivatives, + kinematic_constraint_method); + + // Check that the end-effector point lies on the vertical line through the + // origin. model->initSystem(); StatesTrajectory states = solution.exportToStatesTrajectory(mp); for (int i = 0; i < (int)states.getSize(); ++i) { @@ -618,8 +722,11 @@ void testDoublePendulumPointOnLine( /// its two coordinates together via a linear relationship and minimizing /// control effort. template -void testDoublePendulumCoordinateCoupler(MocoSolution& solution, - bool enforce_constraint_derivatives, std::string dynamics_mode) { +void testDoublePendulumCoordinateCoupler( + bool enforce_constraint_derivatives, std::string dynamics_mode, + std::string kinematic_constraint_method = "Posa2016", + std::string transcription_scheme = "hermite-simpson", + int num_mesh_intervals = 20) { MocoStudy study; study.setName("double_pendulum_coordinate_coupler"); @@ -657,25 +764,33 @@ void testDoublePendulumCoordinateCoupler(MocoSolution& solution, mp.setStateInfo("/jointset/j1/q1/speed", {-5, 5}, 0, 0); mp.setControlInfo("/tau0", {-50, 50}); mp.setControlInfo("/tau1", {-50, 50}); - mp.addGoal(); + auto* effort = mp.addGoal(); auto& ms = study.initSolver(); - ms.set_num_mesh_intervals(20); + ms.set_num_mesh_intervals(num_mesh_intervals); ms.set_verbosity(2); ms.set_optim_solver("ipopt"); ms.set_optim_convergence_tolerance(1e-3); - ms.set_transcription_scheme("hermite-simpson"); + ms.set_transcription_scheme(transcription_scheme); ms.set_enforce_constraint_derivatives(enforce_constraint_derivatives); - ms.set_minimize_lagrange_multipliers(true); - ms.set_lagrange_multiplier_weight(10); + ms.set_kinematic_constraint_method(kinematic_constraint_method); + if (!enforce_constraint_derivatives) { + ms.set_minimize_lagrange_multipliers(true); + ms.set_lagrange_multiplier_weight(10); + } ms.set_multibody_dynamics_mode(dynamics_mode); ms.setGuess("bounds"); - solution = study.solve(); + MocoSolution solution = study.solve(); solution.write("testConstraints_testDoublePendulumCoordinateCoupler.sto"); - // moco.visualize(solution); + //study.visualize(solution); + // Check that the constraint errors are within a specified tolerance. model->initSystem(); + checkConstraintErrors(solution, *model, enforce_constraint_derivatives, + kinematic_constraint_method); + + // Check that the coordinates are coupled according to the linear function. StatesTrajectory states = solution.exportToStatesTrajectory(mp); for (int i = 0; i < (int)states.getSize(); ++i) { const auto& s = states.get(i); @@ -693,40 +808,36 @@ void testDoublePendulumCoordinateCoupler(MocoSolution& solution, } /// Solve an optimal control problem where a double pendulum must follow a -/// prescribed motion based on the previous test case (see -/// testDoublePendulumCoordinateCoupler). +/// prescribed motion based on sinusoidal functions for each coordinate. template -void testDoublePendulumPrescribedMotion(MocoSolution& couplerSolution, - bool enforce_constraint_derivatives, std::string dynamics_mode) { +void testDoublePendulumPrescribedMotion( + bool enforce_constraint_derivatives, + std::string dynamics_mode, + std::string kinematic_constraint_method = "Posa2016", + std::string transcription_scheme = "hermite-simpson", + int num_mesh_intervals = 20) { MocoStudy study; study.setName("double_pendulum_prescribed_motion"); MocoProblem& mp = study.updProblem(); // Create double pendulum model. auto model = createDoublePendulumModel(); - // Create a spline set for the model states from the previous solution. We - // need to call initSystem() and set the model here in order to convert the - // solution from the previous problem to a StatesTrajectory. model->initSystem(); - mp.setModelAsCopy(*model); - - TimeSeriesTable statesTrajCoupler = - couplerSolution.exportToStatesTrajectory(mp).exportToTable(*model); - GCVSplineSet statesSpline(statesTrajCoupler); // Apply the prescribed motion constraints. + Sine q0func(2.0, 1.0, 0.0); Coordinate& q0 = model->updJointSet().get("j0").updCoordinate(); - q0.setPrescribedFunction(statesSpline.get("/jointset/j0/q0/value")); + q0.setPrescribedFunction(q0func); q0.setDefaultIsPrescribed(true); + Sine q1func(0.5, 2.0, 0.5*SimTK::Pi); Coordinate& q1 = model->updJointSet().get("j1").updCoordinate(); - q1.setPrescribedFunction(statesSpline.get("/jointset/j1/q1/value")); + q1.setPrescribedFunction(q1func); q1.setDefaultIsPrescribed(true); - // Set the model again after implementing the constraints. + + // Set the model after implementing the constraints. mp.setModelAsCopy(*model); mp.setTimeBounds(0, 1); - // No bounds here, since the problem is already highly constrained by the - // prescribed motion constraints on the coordinates. mp.setStateInfo("/jointset/j0/q0/value", {-10, 10}); mp.setStateInfo("/jointset/j0/q0/speed", {-50, 50}); mp.setStateInfo("/jointset/j1/q1/value", {-10, 10}); @@ -734,106 +845,30 @@ void testDoublePendulumPrescribedMotion(MocoSolution& couplerSolution, mp.setControlInfo("/tau0", {-25, 25}); mp.setControlInfo("/tau1", {-25, 25}); - mp.addGoal(); + auto* effort = mp.addGoal(); auto& ms = study.initSolver(); - ms.set_num_mesh_intervals(20); + ms.set_num_mesh_intervals(num_mesh_intervals); ms.set_verbosity(2); ms.set_optim_solver("ipopt"); ms.set_optim_convergence_tolerance(1e-3); - ms.set_transcription_scheme("hermite-simpson"); + ms.set_transcription_scheme(transcription_scheme); ms.set_enforce_constraint_derivatives(enforce_constraint_derivatives); - ms.set_minimize_lagrange_multipliers(true); - ms.set_lagrange_multiplier_weight(10); + ms.set_kinematic_constraint_method(kinematic_constraint_method); + if (!enforce_constraint_derivatives) { + ms.set_minimize_lagrange_multipliers(true); + ms.set_lagrange_multiplier_weight(10); + } ms.set_multibody_dynamics_mode(dynamics_mode); - // Set guess based on coupler solution trajectory. - MocoTrajectory guess(ms.createGuess("bounds")); - guess.setStatesTrajectory(statesTrajCoupler); - ms.setGuess(guess); - - MocoSolution solution = study.solve(); + MocoSolution solution = study.solve().unseal(); solution.write("testConstraints_testDoublePendulumPrescribedMotion.sto"); - // study.visualize(solution); - - // Create a TimeSeriesTable containing the splined state data from - // testDoublePendulumCoordinateCoupler. Since this splined data could be - // somewhat different from the coordinate coupler OCP solution, we use this - // to create a direct comparison between the prescribed motion OCP solution - // states and exactly what the PrescribedMotion constraints should be - // enforcing. - auto statesTraj = solution.exportToStatesTrajectory(mp); - // Initialize data structures to use in the TimeSeriesTable - // convenience constructor. - std::vector indVec((int)statesTraj.getSize()); - SimTK::Matrix depData( - (int)statesTraj.getSize(), (int)solution.getStateNames().size()); - Vector timeVec(1); - for (int i = 0; i < (int)statesTraj.getSize(); ++i) { - const auto& s = statesTraj.get(i); - const SimTK::Real& time = s.getTime(); - indVec[i] = time; - timeVec.updElt(0, 0) = time; - depData.set(i, 0, - statesSpline.get("/jointset/j0/q0/value").calcValue(timeVec)); - depData.set(i, 1, - statesSpline.get("/jointset/j1/q1/value").calcValue(timeVec)); - // The values for the speed states are created from the spline - // derivative values. - depData.set(i, 2, - statesSpline.get("/jointset/j0/q0/value") - .calcDerivative({0}, timeVec)); - depData.set(i, 3, - statesSpline.get("/jointset/j1/q1/value") - .calcDerivative({0}, timeVec)); - } - TimeSeriesTable splineStateValues( - indVec, depData, solution.getStateNames()); - - // Create a MocoTrajectory containing the splined state values. The splined - // state values are also set for the controls and adjuncts as dummy data. - const auto& statesTimes = splineStateValues.getIndependentColumn(); - SimTK::Vector time((int)statesTimes.size(), statesTimes.data(), true); - auto mocoIterSpline = MocoTrajectory(time, - splineStateValues.getColumnLabels(), - splineStateValues.getColumnLabels(), - splineStateValues.getColumnLabels(), {}, - splineStateValues.getMatrix(), splineStateValues.getMatrix(), - splineStateValues.getMatrix(), SimTK::RowVector(0)); - - // Only compare the position-level values between the current solution - // states and the states from the previous test (original and splined). - // These should match well, since position-level values are enforced - // directly via a path constraint in the current problem formulation (see - // MocoTropterSolver for details). - - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(mocoIterSpline, - {{"states", {"/jointset/j0/q0/value", - "/jointset/j1/q1/value"}}}), - 0, 1e-3); - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(couplerSolution, - {{"states", {"/jointset/j0/q0/value", - "/jointset/j1/q1/value"}}}), - 0, 1e-3); - // Only compare the velocity-level values between the current solution - // states and the states from the previous test (original and splined). - // These won't match as well as the position-level values, since velocity- - // level errors are not enforced in the current problem formulation. - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(mocoIterSpline, - {{"states", {"/jointset/j0/q0/speed", - "/jointset/j1/q1/speed"}}}), - 0, 1e-1); - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(couplerSolution, - {{"states", {"/jointset/j0/q0/speed", - "/jointset/j1/q1/speed"}}}), - 0, 1e-1); - // Compare only the actuator controls. These match worse compared to the - // velocity-level states. It is currently unclear to what extent this is - // related to velocity-level states not matching well or the how the model - // constraints are enforced in the current formulation. - SimTK_TEST_EQ_TOL(solution.compareContinuousVariablesRMS(couplerSolution, - {{"controls", {"/tau0", "/tau1"}}}), - 0, 5); + //study.visualize(solution); + + // Check that the constraint errors are within a specified tolerance. + model->initSystem(); + checkConstraintErrors(solution, *model, enforce_constraint_derivatives, + kinematic_constraint_method); // Run a forward simulation using the solution controls in prescribed // controllers for the model actuators and see if we get the correct states @@ -841,64 +876,127 @@ void testDoublePendulumPrescribedMotion(MocoSolution& couplerSolution, runForwardSimulation(*model, solution, 1e-1); } -TEMPLATE_TEST_CASE("DoublePendulum with and without constraint derivatives", - "[explicit]", MocoCasADiSolver, MocoTropterSolver) { - SECTION("DoublePendulum without constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, false, "explicit"); - testDoublePendulumPrescribedMotion( - couplerSol, false, "explicit"); - } - - SECTION("DoublePendulum with constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, true, "explicit"); - testDoublePendulumPrescribedMotion( - couplerSol, true, "explicit"); +TEST_CASE("DoublePendulum tests, Posa2016 method - MocoCasADiSolver", + "[casadi]") { + bool enforce_constraint_derivatives = GENERATE(true, false); + std::string dynamics_mode = GENERATE(as{}, + "explicit", "implicit"); + std::string section = fmt::format( + "enforce derivatives: {}, dynamics_mode: {}", + enforce_constraint_derivatives, dynamics_mode); + + DYNAMIC_SECTION(section) { + SECTION("CoordinateCouplerConstraint") { + testDoublePendulumCoordinateCoupler( + enforce_constraint_derivatives, dynamics_mode, "Posa2016"); + } + SECTION("PrescribedMotion") { + testDoublePendulumPrescribedMotion( + enforce_constraint_derivatives, dynamics_mode, "Posa2016"); + } + SECTION("PointOnLine") { + testDoublePendulumPointOnLine( + enforce_constraint_derivatives, dynamics_mode, "Posa2016"); + } } } -TEST_CASE("DoublePendulum with and without constraint derivatives", - "[implicit][casadi]") { - SECTION("DoublePendulum without constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, false, "implicit"); - testDoublePendulumPrescribedMotion( - couplerSol, false, "implicit"); - } +TEST_CASE("DoublePendulum tests, Posa2016 method - MocoTropterSolver", + "[tropter]") { + bool enforce_constraint_derivatives = GENERATE(true, false); + std::string section = fmt::format( + "enforce derivatives: {}", enforce_constraint_derivatives); - SECTION("DoublePendulum with constraint derivatives") { - MocoSolution couplerSol; - testDoublePendulumCoordinateCoupler( - couplerSol, true, "implicit"); - testDoublePendulumPrescribedMotion( - couplerSol, true, "implicit"); + DYNAMIC_SECTION(section) { + SECTION("CoordinateCouplerConstraint") { + testDoublePendulumCoordinateCoupler( + enforce_constraint_derivatives, "explicit", "Posa2016"); + } + SECTION("PrescribedMotion") { + testDoublePendulumPrescribedMotion( + enforce_constraint_derivatives, "explicit", "Posa2016"); + } + SECTION("PointOnLine") { + testDoublePendulumPointOnLine( + enforce_constraint_derivatives, "explicit", "Posa2016"); + } } } -TEMPLATE_TEST_CASE("DoublePendulumPointOnLine without constraint derivatives", - "[explicit]", MocoCasADiSolver, MocoTropterSolver) { - testDoublePendulumPointOnLine(false, "explicit"); +TEST_CASE("DoublePendulum tests, Bordalba2023 method", "[casadi]") { + std::string scheme = GENERATE(as{}, + "trapezoidal", "hermite-simpson", "legendre-gauss-3", + "legendre-gauss-radau-3"); + std::string dynamics_mode = GENERATE(as{}, + "explicit", "implicit"); + std::string section = fmt::format("scheme: {}, dynamics_mode: {}", + scheme, dynamics_mode); + + // Trapezoidal rule requires more mesh intervals to keep slack variables + // small. + int num_mesh_intervals = scheme == "trapezoidal" ? 50 : 25; + DYNAMIC_SECTION(section) { + SECTION("CoordinateCouplerConstraint") { + testDoublePendulumCoordinateCoupler(true, + dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals); + } + SECTION("PrescribedMotion") { + testDoublePendulumPrescribedMotion(true, + dynamics_mode, "Bordalba2023", scheme, num_mesh_intervals); + } + SECTION("PointOnLine") { + testDoublePendulumPointOnLine( + true, dynamics_mode, "Bordalba2023", scheme, + num_mesh_intervals); + } + } } -TEMPLATE_TEST_CASE("DoublePendulumPointOnLine with constraint derivatives", - "[explicit]", MocoCasADiSolver, MocoTropterSolver) { - testDoublePendulumPointOnLine(true, "explicit"); -} +TEST_CASE("Bad configurations with kinematic constraints") { + MocoStudy study; + study.setName("double_pendulum_coordinate_coupler"); -TEST_CASE("DoublePendulumPointOnLine without constraint derivatives", - "[implicit][casadi]") { - testDoublePendulumPointOnLine(false, "implicit"); -} + // Create a double pendulum model and add a constraint. + auto model = createDoublePendulumModel(); + const Coordinate& q0 = model->getCoordinateSet().get("q0"); + const Coordinate& q1 = model->getCoordinateSet().get("q1"); + CoordinateCouplerConstraint* constraint = new CoordinateCouplerConstraint(); + Array indepCoordNames; + indepCoordNames.append("q0"); + constraint->setIndependentCoordinateNames(indepCoordNames); + constraint->setDependentCoordinateName("q1"); + const SimTK::Real m = -2; + const SimTK::Real b = SimTK::Pi; + LinearFunction linFunc(m, b); + constraint->setFunction(&linFunc); + model->addConstraint(constraint); + model->finalizeConnections(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(*model); + mp.setTimeBounds(0, 1); + + auto& ms = study.initSolver(); + + SECTION("Enforce constraint derivatives") { + ms.set_enforce_constraint_derivatives(false); + ms.set_kinematic_constraint_method("Bordalba2023"); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring( + "The Bordalba et al. 2023 method for enforcing")); + } -TEST_CASE("DoublePendulumPointOnLine with constraint derivatives", - "[implicit][casadi]") { - testDoublePendulumPointOnLine(true, "implicit"); + SECTION("Posa2016 method with Hermite-Simpson only") { + ms.set_enforce_constraint_derivatives(true); + ms.set_kinematic_constraint_method("Posa2016"); + ms.set_transcription_scheme("trapezoidal"); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring( + "Expected the 'hermite-simpson' transcription scheme")); + } } + + class EqualControlConstraint : public MocoPathConstraint { OpenSim_DECLARE_CONCRETE_OBJECT(EqualControlConstraint, MocoPathConstraint); @@ -1020,9 +1118,8 @@ TEMPLATE_TEST_CASE("FailWithPathConstraints", "", // solving for the mass that allows the point mass to obey the constraint // of staying in place. This checks that the parameters are applied to both // ModelBase and ModelDisabledConstraints. -TEMPLATE_TEST_CASE( - "Parameters are set properly for Base and DisabledConstraints", - "[tropter]", MocoTropterSolver /*, too damn slow: MocoCasADiSolver*/) { +TEMPLATE_TEST_CASE("Parameters are set properly", "[tropter]", + MocoTropterSolver /*, too damn slow: MocoCasADiSolver*/) { Model model; auto* body = new Body("b", 0.7, SimTK::Vec3(0), SimTK::Inertia(1)); model.addBody(body); @@ -1131,14 +1228,12 @@ void testDoublePendulumJointReactionGoal(std::string dynamics_mode) { CHECK(solution.getObjective() == Approx(-1. / sqrt(2) * 20).epsilon(1e-2)); } -TEMPLATE_TEST_CASE( - "DoublePendulumPointJointReactionGoal with constraint derivatives", +TEMPLATE_TEST_CASE("Joint reactions w/ constraint derivatives", "[explicit]", MocoCasADiSolver, MocoTropterSolver) { testDoublePendulumJointReactionGoal("explicit"); } -TEMPLATE_TEST_CASE("DoublePendulumJointReactionGoal implicit with " - "constraint derivatives", +TEMPLATE_TEST_CASE("Joint reactions implicit w/ constraint derivatives", "[implicit][casadi]", MocoCasADiSolver) { testDoublePendulumJointReactionGoal("implicit"); } @@ -1207,7 +1302,7 @@ TEST_CASE("Goals use Moco-defined accelerations and multipliers", "[casadi]") { problem.setTimeBounds(0, 1); // This goal is used to populate the testData struct, which will contain - // the accelerations and joint reactions generated by Moco. + // the accelerations and joint reactions generated by Moco. auto testData = std::make_shared(); problem.addGoal("jointset/joint", testData); @@ -1347,6 +1442,8 @@ TEST_CASE("Multipliers are correct", "[casadi]") { SECTION("Body welded to ground") { auto dynamics_mode = GENERATE(as{}, "implicit", "explicit"); + auto kinematic_constraint_method = + GENERATE(as{}, "Posa2016", "Bordalba2023"); Model model; const double mass = 1.3169; @@ -1370,6 +1467,7 @@ TEST_CASE("Multipliers are correct", "[casadi]") { auto& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(5); solver.set_multibody_dynamics_mode(dynamics_mode); + solver.set_kinematic_constraint_method(kinematic_constraint_method); solver.set_transcription_scheme("hermite-simpson"); solver.set_enforce_constraint_derivatives(true); @@ -1398,6 +1496,8 @@ TEST_CASE("Multipliers are correct", "[casadi]") { auto dynamics_mode = GENERATE(as{}, "implicit", "explicit"); + auto kinematic_constraint_method = + GENERATE(as{}, "Posa2016", "Bordalba2023"); Model model = ModelFactory::createPlanarPointMass(); model.set_gravity(Vec3(0)); @@ -1428,6 +1528,7 @@ TEST_CASE("Multipliers are correct", "[casadi]") { solver.set_num_mesh_intervals(10); solver.set_multibody_dynamics_mode(dynamics_mode); solver.set_transcription_scheme("hermite-simpson"); + solver.set_kinematic_constraint_method(kinematic_constraint_method); solver.set_enforce_constraint_derivatives(true); MocoSolution solution = study.solve(); const auto Fx = solution.getControl("/forceset/force_x"); @@ -1473,7 +1574,7 @@ TEST_CASE("Prescribed kinematics with kinematic constraints", "[casadi]") { auto& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(10); solver.set_multibody_dynamics_mode("implicit"); - solver.set_interpolate_control_midpoints(false); + solver.set_interpolate_control_mesh_interior_points(false); MocoSolution solution = study.solve(); const auto Fx = solution.getControl("/forceset/force_x"); const auto Fy = solution.getControl("/forceset/force_y"); @@ -1616,11 +1717,310 @@ TEMPLATE_TEST_CASE("MocoControlBoundConstraint", "", } } +TEMPLATE_TEST_CASE("MocoOutputBoundConstraint", "", + MocoCasADiSolver, MocoTropterSolver) { + SECTION("Two outputs with equality bound") { + MocoSolution solutionControl; + MocoStudy study; + auto& problem = study.updProblem(); + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + + problem.setModelAsCopy(*model); + problem.setTimeBounds(0, 6.283); + + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), + MocoInitialBounds(-1, 1)); + problem.setStateInfo("/slider/position/speed", {-1, 1}); + problem.setStateInfo("/slider2/position/value", MocoBounds(-5, 5), + MocoInitialBounds(-1, 1)); + problem.setStateInfo("/slider2/position/speed", {-1, 1}); + + auto* constr = problem.addPathConstraint(); + Sine lower; + constr->setLowerBound(lower); + constr->setEqualityWithLower(true); + constr->setOutputIndex(0); + constr->setOutputPath("/body|position"); + constr->setSecondOutputPath("/body2|position"); + constr->setOperation("subtraction"); + + auto& solver = study.template initSolver(); + solver.set_num_mesh_intervals(30); + MocoSolution solution = study.solve(); + + auto solutionPosition1 = solution.getState("/slider/position/value"); + auto solutionPosition2 = solution.getState("/slider2/position/value"); + auto times = solution.getTime(); + SimTK::Vector time(1); + for (int i = 0; i < solution.getNumTimes(); ++i) { + double diff = (static_cast(solutionPosition1[i])[0] + - static_cast(solutionPosition2[i])[0]); + time[0] = times[i]; + REQUIRE_THAT(diff, Catch::Matchers::WithinAbs( + lower.calcValue(time), 1e-3)); + } + } + + SECTION("One output with upper bound") { + MocoSolution solutionControl; + MocoStudy study; + auto& problem = study.updProblem(); + auto model = ModelFactory::createSlidingPointMass(); + model.initSystem(); + + problem.setModelAsCopy(model); + problem.setTimeBounds(0, 3); + + problem.setStateInfo("/slider/position/value", MocoBounds(-3, 3)); + problem.setStateInfo("/slider/position/speed", {-3, 3}); + + auto* constr = problem.addPathConstraint(); + PiecewiseLinearFunction upper; + upper.addPoint(0, 0); + upper.addPoint(1, -1); + upper.addPoint(2, -0.4); + upper.addPoint(3, 1); + constr->setUpperBound(upper); + constr->setOutputIndex(0); + constr->setOutputPath("/body|position"); + + // want to minimize position magnitude, but can't reach 0 due to constraint + auto* goal = problem.addGoal(); + goal->setOutputPath("/body|position"); + goal->setExponent(2); + + auto* effort = problem.addGoal(); + effort->setWeight(0.1); + effort->setName("effort"); + + auto& solver = study.template initSolver(); + solver.set_num_mesh_intervals(30); + MocoSolution solution = study.solve(); + + auto solutionPosition = solution.getState("/slider/position/value"); + auto times = solution.getTime(); + SimTK::Vector time(1); + for (int i = 0; i < solution.getNumTimes(); ++i) { + double position = static_cast(solutionPosition[i])[0]; + time[0] = times[i]; + double bound = upper.calcValue(time); + CHECK(Catch::Approx(position).margin(1e-4) <= bound); + } + } + + SECTION("Time range of bounds function is too small.") { + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(ModelFactory::createPendulum()); + problem.setTimeBounds({-31, 0}, {1, 50}); + problem.addGoal(); + GCVSpline violateLower; + violateLower.setDegree(5); + violateLower.addPoint(-30.9999, 0); + violateLower.addPoint(0, 0); + violateLower.addPoint(0.5, 0); + violateLower.addPoint(0.7, 0); + violateLower.addPoint(0.8, 0); + violateLower.addPoint(0.9, 0); + violateLower.addPoint(50, 0.319); + auto* constr = problem.addPathConstraint(); + constr->setOutputPath("|kinetic_energy"); + constr->setLowerBound(violateLower); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring("must be less than or equal to the minimum")); + constr->clearLowerBound(); + GCVSpline violateUpper; + violateUpper.setDegree(5); + violateUpper.addPoint(-31, 0); + violateUpper.addPoint(0, 0); + violateUpper.addPoint(0.5, 0); + violateUpper.addPoint(0.7, 0); + violateUpper.addPoint(0.8, 0); + violateUpper.addPoint(0.9, 0); + violateUpper.addPoint(49.99999, .0319); + constr->setUpperBound(violateUpper); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring( + "must be greater than or equal to the maximum")); + } + + SECTION("Can omit both bounds.") { + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(ModelFactory::createPendulum()); + problem.setTimeBounds(0, 1); + problem.setStateInfo("/jointset/j0/q0/value", {-10, 10}, 0); + problem.setStateInfo("/jointset/j0/q0/speed", {-10, 10}, 0); + problem.setControlInfo("/tau0", {-5, 5}); + problem.addGoal(); + auto* constr = problem.addPathConstraint(); + constr->setOutputPath("|kinetic_energy"); + study.initSolver(); + study.solve(); + } +} + +TEMPLATE_TEST_CASE("MocoStateBoundConstraint", "", + MocoCasADiSolver, MocoTropterSolver) { + SECTION("Upper bounded speed") { + MocoSolution solutionControl; + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(ModelFactory::createSlidingPointMass()); + problem.setTimeBounds(0, 3); + problem.setStateInfo("/slider/position/speed", {-10, 10}, 0); + auto* effort = problem.addGoal(); + effort->setName("effort"); + effort->setWeight(0.001); + // decreasing speed constraint + auto* constr = problem.addPathConstraint(); + constr->addStatePath("/slider/position/speed"); + PiecewiseLinearFunction upperBound; + upperBound.addPoint(0, 2); + upperBound.addPoint(3, -3); + constr->setUpperBound(upperBound); + study.initSolver(); + MocoSolution solution = study.solve(); + // ensure that at every state, the speed is under the bound + auto solutionSpeed = solution.getState("/slider/position/speed"); + auto times = solution.getTime(); + SimTK::Vector time(1); + for (int i = 0; i < solution.getNumTimes(); ++i) { + double speed = solutionSpeed[i]; + time[0] = times[i]; + double max = upperBound.calcValue(time); + CHECK(Catch::Approx(speed).margin(1e-8) <= max); + } + } + + SECTION("Equality bounded speed") { + MocoSolution solutionControl; + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(ModelFactory::createSlidingPointMass()); + problem.setTimeBounds(0, 10); + problem.setStateInfo("/slider/position/speed", {-10, 10}, 0); + // add constraint of changing speed + auto* constr = problem.addPathConstraint(); + constr->addStatePath("/slider/position/speed"); + constr->setEqualityWithLower(true); + Sine lowerBound; + constr->setLowerBound(lowerBound); + // can't have upper bound when set to equal lower bound + constr->setUpperBound(Constant(1)); + CHECK_THROWS(study.initSolver()); + // undo upper bound and solve + constr->clearUpperBound(); + study.initSolver(); + MocoSolution solution = study.solve(); + // check that the speed is always close to the lower bound + auto solutionSpeed = solution.getState("/slider/position/speed"); + auto times = solution.getTime(); + SimTK::Vector time(1); + for (int i = 0; i < solution.getNumTimes(); ++i) { + double speed = solutionSpeed[i]; + time[0] = times[i]; + double max = lowerBound.calcValue(time); + REQUIRE_THAT(speed, Catch::Matchers::WithinAbs(max, 1e-4)); + } + } + + SECTION("Double bounded speed") { + MocoSolution solutionControl; + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(ModelFactory::createSlidingPointMass()); + problem.setTimeBounds(0, 4); + problem.setStateInfo("/slider/position/speed", {-10, 10}); + // add speed constraint with changing upper and lower bounds + auto* constr = problem.addPathConstraint(); + constr->addStatePath("/slider/position/speed"); + PiecewiseLinearFunction lowerBound; + lowerBound.addPoint(0, -2); + lowerBound.addPoint(2, 1); + lowerBound.addPoint(4, -3); + constr->setLowerBound(lowerBound); + PiecewiseLinearFunction upperBound; + upperBound.addPoint(0, -1); + upperBound.addPoint(2, 4); + upperBound.addPoint(4, 0); + constr->setUpperBound(upperBound); + study.initSolver(); + MocoSolution solution = study.solve(); + // check that the speed is between the bounds + auto solutionSpeed = solution.getState("/slider/position/speed"); + auto times = solution.getTime(); + SimTK::Vector time(1); + for (int i = 0; i < solution.getNumTimes(); ++i) { + double speed = solutionSpeed[i]; + time[0] = times[i]; + double max = upperBound.calcValue(time); + CHECK(Catch::Approx(speed).margin(1e-8) <= max); + double min = lowerBound.calcValue(time); + CHECK(Catch::Approx(speed).margin(1e-8) >= min); + } + } + + SECTION("Time range of bounds function is too small.") { + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(ModelFactory::createPendulum()); + problem.setTimeBounds({-31, 0}, {1, 50}); + problem.addGoal(); + GCVSpline violateLower; + violateLower.setDegree(5); + violateLower.addPoint(-30.9999, 0); + violateLower.addPoint(0, 0); + violateLower.addPoint(0.5, 0); + violateLower.addPoint(0.7, 0); + violateLower.addPoint(0.8, 0); + violateLower.addPoint(0.9, 0); + violateLower.addPoint(50, 0.319); + auto* constr = problem.addPathConstraint(); + constr->addStatePath("/jointset/j0/q0/value"); + constr->setLowerBound(violateLower); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring("must be less than or equal to the minimum")); + + constr->clearLowerBound(); + GCVSpline violateUpper; + violateUpper.setDegree(5); + violateUpper.addPoint(-31, 0); + violateUpper.addPoint(0, 0); + violateUpper.addPoint(0.5, 0); + violateUpper.addPoint(0.7, 0); + violateUpper.addPoint(0.8, 0); + violateUpper.addPoint(0.9, 0); + violateUpper.addPoint(49.99999, .0319); + constr->setUpperBound(violateUpper); + CHECK_THROWS_WITH(study.solve(), + ContainsSubstring( + "must be greater than or equal to the maximum")); + } + + SECTION("Can omit both bounds.") { + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(ModelFactory::createPendulum()); + problem.setTimeBounds(0, 1); + problem.setStateInfo("/jointset/j0/q0/value", {-10, 10}, 0); + problem.setStateInfo("/jointset/j0/q0/speed", {-10, 10}, 0); + problem.setControlInfo("/tau0", {-5, 5}); + problem.addGoal(); + auto* constr = problem.addPathConstraint(); + study.initSolver(); + study.solve(); + constr->addStatePath("/jointset/j0/q0/speed"); + study.solve(); + } +} + TEMPLATE_TEST_CASE("MocoFrameDistanceConstraint", "", MocoCasADiSolver, MocoTropterSolver) { using SimTK::Pi; - // Create a 3D pendulum model with a single body and a marker at the + // Create a 3D pendulum model with a single body and a marker at the // end-effector. Model model; auto* body = new Body("body", 1, Vec3(0), SimTK::Inertia(1)); @@ -1661,7 +2061,7 @@ TEMPLATE_TEST_CASE("MocoFrameDistanceConstraint", "", problem.setStateInfo("/jointset/gimbal/qy/speed", {-10, 10}, 0, 0); problem.setStateInfo("/jointset/gimbal/qz/speed", {-10, 10}, 0, 0); - auto* distanceConstraint = + auto* distanceConstraint = problem.addPathConstraint(); const double distance = 0.1; distanceConstraint->addFramePair({"/ground", "/bodyset/body", distance, @@ -1716,3 +2116,104 @@ TEMPLATE_TEST_CASE("Multiple MocoPathConstraints", "", MocoCasADiSolver, study.initSolver(); study.solve(); } + + +class ConstantSpeedConstraint : public Constraint { +OpenSim_DECLARE_CONCRETE_OBJECT(ConstantSpeedConstraint, Constraint); + +public: + OpenSim_DECLARE_SOCKET(body, PhysicalFrame, + "The body participating in this constraint."); + +private: + void extendAddToSystem(SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + const PhysicalFrame& f = getConnectee("body"); + SimTK::MobilizedBody b = f.getMobilizedBody(); + SimTK::Constraint::ConstantSpeed simtkConstantSpeed(b, + SimTK::MobilizerUIndex(0), SimTK::Real(2.34)); + assignConstraintIndex(simtkConstantSpeed.getConstraintIndex()); + } +}; + + +class ConstantAccelerationConstraint : public Constraint { +OpenSim_DECLARE_CONCRETE_OBJECT(ConstantAccelerationConstraint, Constraint); + +public: + OpenSim_DECLARE_SOCKET(body, PhysicalFrame, + "The body participating in this constraint."); + +private: + void extendAddToSystem(SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + const PhysicalFrame& f = getConnectee("body"); + SimTK::MobilizedBody b = f.getMobilizedBody(); + SimTK::Constraint::ConstantAcceleration simtkConstantAcceleration(b, + SimTK::MobilizerUIndex(0), SimTK::Real(1.23)); + assignConstraintIndex(simtkConstantAcceleration.getConstraintIndex()); + } +}; + +MocoStudy createSlidingMassMocoStudy(const Model& model, + const std::string& scheme) { + MocoStudy study; + study.setName("sliding_mass"); + MocoProblem& mp = study.updProblem(); + mp.setModel(make_unique(model)); + mp.setTimeBounds(0, {0, 10}); + mp.setStateInfo("/slider/position/value",{0, 1}, 0, 1); + mp.setStateInfo("/slider/position/speed", {-100, 100}); + mp.addGoal(); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(50); + ms.set_transcription_scheme(scheme); + ms.set_kinematic_constraint_method("Bordalba2023"); + + return study; +} + +TEST_CASE("ConstantSpeedConstraint") { + std::string scheme = GENERATE(as{}, + "trapezoidal", "hermite-simpson", "legendre-gauss-3", + "legendre-gauss-radau-3"); + + Model model = ModelFactory::createSlidingPointMass(); + auto* constraint = new ConstantSpeedConstraint(); + constraint->setName("constant_speed"); + constraint->connectSocket_body(model.getComponent("body")); + model.addConstraint(constraint); + model.initSystem(); + + MocoStudy study = createSlidingMassMocoStudy(model, scheme); + MocoSolution solution = study.solve(); + + const auto& speed = solution.getState("/slider/position/speed"); + for (int itime = 0; itime < solution.getNumTimes(); ++itime) { + CHECK(speed[itime] == Approx(2.34).margin(1e-9)); + } +} + +TEST_CASE("ConstantAccelerationConstraint") { + std::string scheme = GENERATE(as{}, + "trapezoidal", "hermite-simpson", "legendre-gauss-3", + "legendre-gauss-radau-3"); + + Model model = ModelFactory::createSlidingPointMass(); + auto* constraint = new ConstantAccelerationConstraint(); + constraint->setName("constant_acceleration"); + constraint->connectSocket_body(model.getComponent("body")); + model.addConstraint(constraint); + model.initSystem(); + + MocoStudy study = createSlidingMassMocoStudy(model, scheme); + MocoSolution solution = study.solve(); + + const auto& states = solution.exportToStatesTrajectory(model); + for (const auto& state : states) { + model.realizeAcceleration(state); + CHECK(state.getUDot()[0] == Approx(1.23).margin(1e-9)); + } +} + diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index bb155bbcbd..bbcda978ca 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -19,25 +19,29 @@ #include #include #include -#include +#include +#include #include +#include #include #include -#include #include "Testing.h" +#include using Catch::Approx; using Catch::Matchers::ContainsSubstring; using namespace OpenSim; +/// creates a model with one sliding mass std::unique_ptr createSlidingMassModel() { auto model = make_unique(); model->setName("sliding_mass"); model->set_gravity(SimTK::Vec3(0, 0, 0)); auto* body = new Body("body", 10.0, SimTK::Vec3(0), SimTK::Inertia(0)); model->addComponent(body); + body->attachGeometry(new Sphere(0.05)); // Allows translation along x. auto* joint = new SliderJoint("slider", model->getGround(), *body); @@ -54,6 +58,29 @@ std::unique_ptr createSlidingMassModel() { return model; } +/// create a model with two sliding masses +std::unique_ptr createDoubleSlidingMassModel() { + std::unique_ptr model = createSlidingMassModel(); + auto* body = new Body("body2", 10.0, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + body->attachGeometry(new Sphere(0.05)); + + // Allows translation along x. + auto* joint = new SliderJoint("slider2", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* actu = new CoordinateActuator(); + actu->setCoordinate(&coord); + actu->setName("actuator2"); + actu->setOptimalForce(1); + model->addComponent(actu); + + model->finalizeConnections(); + return model; +} + /// Test the result of a sliding mass minimum effort problem. TEMPLATE_TEST_CASE("Test MocoControlGoal", "", MocoCasADiSolver, MocoTropterSolver) { @@ -265,7 +292,7 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, MocoStudy study = setupMocoStudyDoublePendulumMinimizeEffort(); auto solutionEffort = study.solve(); - SECTION ("MocoControlTrackingGoal") { + SECTION("MocoControlTrackingGoal") { // Re-run problem, now setting effort cost function to zero and adding a // control tracking cost. auto& problem = study.updProblem(); @@ -297,7 +324,7 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, solutionTracking.getStatesTrajectory(), 1e-2); } - SECTION ("MocoOrientationTrackingGoal") { + SECTION("MocoOrientationTrackingGoal") { MocoStudy studyOrientationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); @@ -318,21 +345,21 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, } } - SECTION ("MocoTranslationTrackingGoal") { + SECTION("MocoTranslationTrackingGoal") { MocoStudy studyTranslationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); testDoublePendulumTracking( studyTranslationTracking, solutionEffort); } - SECTION ("MocoAngularVelocityTrackingGoal") { + SECTION("MocoAngularVelocityTrackingGoal") { MocoStudy studyAngularVelocityTracking = setupMocoStudyDoublePendulumMinimizeEffort(); testDoublePendulumTracking( studyAngularVelocityTracking, solutionEffort); } - SECTION ("MocoAccelerationTrackingGoal") { + SECTION("MocoAccelerationTrackingGoal") { MocoStudy studyAccelerationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); // Re-run problem, now setting effort cost function to a low weight and @@ -365,7 +392,7 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, solutionTracking.getStatesTrajectory(), 1e-2); } - SECTION ("MocoAccelerationTrackingGoal (IMU tracking)") { + SECTION("MocoAccelerationTrackingGoal (IMU tracking)") { MocoStudy studyAccelerationTracking = setupMocoStudyDoublePendulumMinimizeEffort(); // Re-run problem, now setting effort cost function to a low weight and @@ -411,14 +438,42 @@ TEMPLATE_TEST_CASE("Test tracking goals", "", MocoCasADiSolver, auto* genForceTracking = problem.addGoal("tracking"); genForceTracking->setReference(generalizedForces); - studyGenForceTracking.updSolver().resetProblem(problem); - auto solutionTracking = studyGenForceTracking.solve(); - // The tracking solution should match the effort solution. - SimTK_TEST_EQ_TOL(solutionEffort.getControlsTrajectory(), - solutionTracking.getControlsTrajectory(), 1e-3); - SimTK_TEST_EQ_TOL(solutionEffort.getStatesTrajectory(), - solutionTracking.getStatesTrajectory(), 1e-3); + SECTION("Setting weights") { + Model model = problem.createRep().getModelBase(); + SimTK::State state = model.initSystem(); + // Set acclelerations to zero, so the only model-generated forces + // in the integrand are zero. + state.updUDot() = SimTK::Vector(state.getNU(), 0.0); + MocoGoal::IntegrandInput input{0, state, {}}; + + // Apply weights. + genForceTracking->setWeightForGeneralizedForcePattern("q.*", 5.0); + genForceTracking->setWeightForGeneralizedForce("q1_moment", 10.0); + genForceTracking->initializeOnModel(model); + + // The integrand should be the sum of the squared generalized forces + // multiplied by the weights. + const SimTK::RowVectorView initialGenForces = + generalizedForces.getRowAtIndex(0); + double integrand = + 5.0 * (initialGenForces[0] * initialGenForces[0]) + + 10.0 * (initialGenForces[1] * initialGenForces[1]); + CHECK_THAT(genForceTracking->calcIntegrand(input), + Catch::Matchers::WithinAbs(integrand, 1e-6)); + } + + SECTION("Tracking performance") { + studyGenForceTracking.updSolver().resetProblem(problem); + auto solutionTracking = studyGenForceTracking.solve(); + + // The tracking solution should match the effort solution. + SimTK_TEST_EQ_TOL(solutionEffort.getControlsTrajectory(), + solutionTracking.getControlsTrajectory(), 1e-3); + SimTK_TEST_EQ_TOL(solutionEffort.getStatesTrajectory(), + solutionTracking.getStatesTrajectory(), 1e-3); + } + } } @@ -594,8 +649,7 @@ TEST_CASE("Test MocoSumSquaredStateGoal") { std::string q1_str = q1.getAbsolutePathString() + "/value"; SimTK::State state = model.initSystem(); - SimTK::Vector inputControls; - MocoGoal::IntegrandInput input {0, state, {}}; + MocoGoal::IntegrandInput input{0, state, {}}; q0.setValue(state, 1.0); q1.setValue(state, 0.5); @@ -952,6 +1006,263 @@ TEMPLATE_TEST_CASE("MocoOutputGoal", "", MocoCasADiSolver, } } +TEMPLATE_TEST_CASE("MocoOutputConstraint with two outputs", "", MocoCasADiSolver, + MocoTropterSolver) { + double bound = 2.0; + MocoSolution solutionControl; + MocoStudy study; + auto& problem = study.updProblem(); + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + problem.setModelAsCopy(*model); + problem.setTimeBounds(0, 3); + + // one slider must move, the other doesn't need to + problem.setStateInfo("/slider/position/value", MocoBounds(-50, 50), + MocoInitialBounds(-5,-5), MocoFinalBounds(5, 5.5)); + problem.setStateInfo("/slider2/position/value", MocoBounds(-50, 50), + MocoInitialBounds(-50, 50), MocoFinalBounds(-50, 50)); + problem.setStateInfo("/slider/position/speed", {-10, 10}, {-10, 10}, {-10, 10}); + problem.setStateInfo("/slider2/position/speed", {-10, 10}, {-10, 10}, {-10, 10}); + problem.setControlInfo("/actuator", {-100, 100}); + problem.setControlInfo("/actuator2", {-100, 100}); + + // add constraint: second mass stays near moving mass + auto* pathCon = problem.template addPathConstraint(); + pathCon->setName("velocities_goal"); + pathCon->setOutputPath("/body|position"); + pathCon->setSecondOutputPath("/body2|position"); + pathCon->setOperation("subtraction"); + pathCon->setOutputIndex(0); + pathCon->setExponent(2); + pathCon->updConstraintInfo().setBounds({{0, bound}}); + + auto* effort = problem.template addGoal(); + effort->setName("effort"); + effort->setWeight(0.001); + + auto& solver = study.template initSolver(); + solver.set_num_mesh_intervals(30); + MocoSolution solution = study.solve(); + + auto solutionPositionMoving = solution.getState("/slider/position/value"); + auto solutionPositionFollowing = solution.getState("/slider2/position/value"); + for (int i = 0; i < solution.getNumTimes(); ++i) { + double diff = (static_cast(solutionPositionMoving[i])[0] + - static_cast(solutionPositionFollowing[i])[0]); + CHECK(diff * diff <= bound); + } +} + +TEMPLATE_TEST_CASE("MocoOutputGoal with two outputs", "", MocoCasADiSolver, + MocoTropterSolver) { + MocoSolution solutionControl; + + SECTION("Subtraction of Vec3 (norm)") { + MocoStudy study; + auto& problem = study.updProblem(); + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + + problem.setModelAsCopy(*model); + problem.setTimeBounds(0, 5); + + // set up sliders to have a distance from each other at the beginning + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), + MocoInitialBounds(-2), MocoFinalBounds(-5, 5)); + problem.setStateInfo("/slider2/position/value", MocoBounds(-5, 5), + MocoInitialBounds(2), MocoFinalBounds(-5, 5)); + + problem.setStateInfo("/slider/position/speed", {-10, 10}, 0, 0); + problem.setStateInfo("/slider2/position/speed", {-10, 10}, 0, 0); + problem.setControlInfo("/actuator", {-100, 100}); + problem.setControlInfo("/actuator2", {-100, 100}); + + auto* effort = problem.addGoal(); + effort->setName("effort"); + effort->setWeight(0.001); + + SECTION("MocoOutputGoal") { + // add goal of smallest distance + auto* goal = problem.template addGoal(); + + // check getting the properties before setting them + CHECK_NOTHROW(goal->getOutputPath()); + CHECK_NOTHROW(goal->getOperation()); + + goal->setName("distance2"); + goal->setOutputPath("/body|position"); + goal->setSecondOutputPath("/body2|position"); + goal->setOperation("subtraction"); + goal->setExponent(2); + + auto& solver = study.template initSolver(); + solver.set_num_mesh_intervals(30); + MocoSolution solution = study.solve(); + + // analyze result for ending distance between spheres + StatesTrajectory trajectory = solution.exportToStatesTrajectory(*model); + const SimTK::State& finalState = trajectory.back(); + model->realizePosition(finalState); + const SimTK::Vec3& endPosition1 = model->getComponent("/body") + .getPositionInGround(finalState); + const SimTK::Vec3& endPosition2 = model->getComponent("/body2") + .getPositionInGround(finalState); + + CHECK((endPosition1 - endPosition2).norm() == Approx(0).margin(1e-2)); + } + + SECTION("MocoFinalOutputGoal") { + // add goal of smallest distance + auto* goal = problem.template addGoal(); + goal->setName("distance2"); + goal->setOutputPath("/body|position"); + goal->setSecondOutputPath("/body2|position"); + goal->setOperation("subtraction"); + goal->setExponent(2); + + auto& solver = study.template initSolver(); + solver.set_num_mesh_intervals(30); + MocoSolution solution = study.solve(); + + // analyze result for ending distance between spheres + StatesTrajectory trajectory = solution.exportToStatesTrajectory(*model); + const SimTK::State& finalState = trajectory.back(); + model->realizePosition(finalState); + const SimTK::Vec3& endPosition1 = model->getComponent("/body") + .getPositionInGround(finalState); + const SimTK::Vec3& endPosition2 = model->getComponent("/body2") + .getPositionInGround(finalState); + + CHECK((endPosition1 - endPosition2).norm() == Approx(0).margin(5e-2)); + } + } + + SECTION("Multiplication of SpatialVec (with index)") { + MocoStudy study; + auto& problem = study.updProblem(); + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + + problem.setModelAsCopy(*model); + problem.setTimeBounds(0, 1); + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), + MocoInitialBounds(1), MocoFinalBounds(-5, 5)); + problem.setStateInfo("/slider2/position/value", MocoBounds(-5, 5), + MocoInitialBounds(-1), MocoFinalBounds(-5, 5)); + + // sliders have a starting velocity + problem.setStateInfo("/slider/position/speed", {-50, 50}, 3); + problem.setStateInfo("/slider2/position/speed", {-50, 50}, -1); + problem.setControlInfo("/actuator", {-100, 100}); + problem.setControlInfo("/actuator2", {-100, 100}); + + // add goal of smallest multiplied velocities + auto* goal = problem.template addGoal(); + goal->setName("multiply_velocities"); + goal->setOutputPath("/body|velocity"); + goal->setSecondOutputPath("/body2|velocity"); + goal->setOperation("multiplication"); + goal->setOutputIndex(3); + goal->setExponent(2); + + auto* effort = problem.template addGoal(); + effort->setName("effort"); + effort->setWeight(0.001); + + auto& solver = study.template initSolver(); + solver.set_num_mesh_intervals(10); + MocoSolution solution = study.solve(); + + // analyze result for ending velocities + StatesTrajectory trajectory = solution.exportToStatesTrajectory(*model); + const SimTK::State& finalState = trajectory.back(); + model->realizeAcceleration(finalState); + const SimTK::SpatialVec& endVel1 = model->getComponent("/body") + .getVelocityInGround(finalState); + const SimTK::SpatialVec& endVel2 = model->getComponent("/body2") + .getVelocityInGround(finalState); + + CHECK((endVel1[1][0] * endVel2[1][0]) == Approx(0).margin(1e-1)); + } + + SECTION("MocoInitialOutputGoal") { + MocoStudy study; + auto& problem = study.updProblem(); + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + + problem.setModelAsCopy(*model); + problem.setTimeBounds(0, 3); + + // set up sliders to have a distance from each other at the end + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), + MocoInitialBounds(-5, 5), MocoFinalBounds(-1, 2)); + problem.setStateInfo("/slider2/position/value", MocoBounds(-5, 5), + MocoInitialBounds(-5, 5), MocoFinalBounds(3, 5)); + + problem.setStateInfo("/slider/position/speed", {-10, 10}, 0, 0); + problem.setStateInfo("/slider2/position/speed", {-10, 10}, 0, 0); + problem.setControlInfo("/actuator", {-100, 100}); + problem.setControlInfo("/actuator2", {-100, 100}); + + // add goal of smallest distance at the beginning + auto* goal = problem.template addGoal(); + goal->setName("distance2"); + goal->setOutputPath("/body|position"); + goal->setSecondOutputPath("/body2|position"); + goal->setOperation("subtraction"); + goal->setExponent(2); + + auto* effort = problem.template addGoal(); + effort->setName("effort"); + effort->setWeight(0.001); + + auto& solver = study.template initSolver(); + solver.set_num_mesh_intervals(30); + MocoSolution solution = study.solve(); + + // analyze result for starting distance between spheres + StatesTrajectory trajectory = solution.exportToStatesTrajectory(*model); + const SimTK::State& initialState = trajectory.front(); + model->realizePosition(initialState); + const SimTK::Vec3& startPosition1 = model->getComponent("/body") + .getPositionInGround(initialState); + const SimTK::Vec3& startPosition2 = model->getComponent("/body2") + .getPositionInGround(initialState); + + CHECK(startPosition1 - startPosition2 == Approx(0).margin(5e-2)); + } + + SECTION("Invalid Outputs") { + MocoStudy study; + auto& problem = study.updProblem(); + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + problem.setModelAsCopy(*model); + + SECTION("Invalid Operator") { + auto* goal = problem.template addGoal(); + goal->setName("notDistance"); + goal->setOutputPath("/body|position"); + goal->setSecondOutputPath("/body2|position"); + goal->setOperation("Subtraction"); // instead of subtraction + + REQUIRE_THROWS(study.template initSolver()); + } + + SECTION("Mismatch Type") { + auto* goal = problem.template addGoal(); + goal->setName("badCombo"); + goal->setOutputPath("/body|velocity"); + goal->setSecondOutputPath("/body2|position"); + goal->setOperation("subtraction"); + + REQUIRE_THROWS(study.template initSolver()); + } + } +} + TEST_CASE("MocoOutputPeriodicityGoal", "[casadi]") { // TODO Tropter does not support endpoint constraints. @@ -1131,3 +1442,203 @@ TEST_CASE("MocoFrameDistanceConstraint de/serialization") { "testMocoGoals_MocoFrameDistanceConstraint_study.omoco"); } } + +TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { + SECTION("mass goal") { + MocoStudy study; + Model model = ModelFactory::createSlidingPointMass(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(model); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", + MocoBounds(0, 10)); + auto* mass_goal = mp.addGoal(); + mass_goal->setExpression("(q-4)^2"); + mass_goal->addParameter(*parameter, "q"); + auto* effort_goal = mp.addGoal(); + effort_goal->setWeight(0.001); + effort_goal->setName("effort"); + + auto& ms = study.initTropterSolver(); + ms.set_num_mesh_intervals(25); + MocoSolution sol = study.solve(); + + // 3.998 + CHECK(sol.getParameter("sphere_mass") == Catch::Approx(4).epsilon(1e-2)); + } + + SECTION("two parameter mass goal") { + MocoStudy study; + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(*model); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}); + mp.setStateInfo("/slider/position/speed", {-20, 20}); + mp.setStateInfo("/slider2/position/value", {-5, 5}, 1, {1.2, 1.3}); + mp.setStateInfo("/slider2/position/speed", {-20, 20}); + + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", + MocoBounds(0, 10)); + auto* parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", + MocoBounds(0, 10)); + int total_weight = 7; + auto* mass_goal = mp.addGoal(); + mass_goal->setExpression(fmt::format("(p+q-{})^2", total_weight)); + mass_goal->addParameter(*parameter, "p"); + mass_goal->addParameter(*parameter2, "q"); + + auto& ms = study.initTropterSolver(); + ms.set_num_mesh_intervals(25); + MocoSolution sol = study.solve(); + + // 3.7 and 3.3 + CHECK(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass") + == Catch::Approx(total_weight).epsilon(1e-9)); + } +} + +// from testMocoParameters +const double STIFFNESS = 100.0; // N/m +const double MASS = 5.0; // kg +const double FINAL_TIME = SimTK::Pi * sqrt(MASS / STIFFNESS); +std::unique_ptr createOscillatorTwoSpringsModel() { + auto model = make_unique(); + model->setName("oscillator_two_springs"); + model->set_gravity(SimTK::Vec3(0, 0, 0)); + auto* body = new Body("body", MASS, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + + // Allows translation along x. + auto* joint = new SliderJoint("slider", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* spring1 = new SpringGeneralizedForce(); + spring1->setName("spring1"); + spring1->set_coordinate("position"); + spring1->setRestLength(0.0); + spring1->setStiffness(0.25*STIFFNESS); + spring1->setViscosity(0.0); + model->addComponent(spring1); + + auto* spring2 = new SpringGeneralizedForce(); + spring2->setName("spring2"); + spring2->set_coordinate("position"); + spring2->setRestLength(0.0); + spring2->setStiffness(0.25*STIFFNESS); + spring2->setViscosity(0.0); + model->addComponent(spring2); + + return model; +} + +TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { + MocoStudy study; + study.setName("oscillator_spring_stiffnesses"); + MocoProblem& mp = study.updProblem(); + mp.setModel(createOscillatorTwoSpringsModel()); + mp.setTimeBounds(0, FINAL_TIME); + mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + SECTION("single parameter for two values") { + // create a parameter goal for the stiffness of both strings + std::vector components = {"spring1", "spring2"}; + auto* parameter = mp.addParameter("spring_stiffness", components, + "stiffness", MocoBounds(0, 100)); + auto* spring_goal = mp.addGoal(); + // minimum is when p = 0.5*STIFFNESS + spring_goal->setExpression(fmt::format("(p-{})^2", 0.5*STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") == + Catch::Approx(0.5*STIFFNESS).epsilon(1e-6)); + } + + SECTION("two parameters") { + // create two parameters to include in the goal + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + auto* spring_goal = mp.addGoal(); + // minimum is when p + q = STIFFNESS + spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + spring_goal->addParameter(*parameter2, "q"); + + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") + + sol.getParameter("spring2_stiffness") == + Catch::Approx(STIFFNESS).epsilon(1e-6)); + } + + SECTION("missing parameter") { + // create one parameter but have two variables + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal( + "stiffness", 1, fmt::format("(p+q-{})^2", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + + CHECK_THROWS(study.initCasADiSolver()); // missing q + } + + SECTION("extra parameter") { + // create two parameters for the goal, only one is used + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal( + "stiffness", 1, fmt::format("(p-{})^2", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + // second parameter is ignored + spring_goal->addParameter(*parameter2, "a"); + + CHECK_NOTHROW(study.initCasADiSolver()); + } + + SECTION("endpoint goal") { + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + auto* spring_goal = mp.addGoal(); + spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + spring_goal->addParameter(*parameter2, "q"); + // set as endpoint constraint + spring_goal->setMode("endpoint_constraint"); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") + + sol.getParameter("spring2_stiffness") == + Catch::Approx(STIFFNESS).epsilon(1e-6)); + } +} diff --git a/OpenSim/Moco/Test/testMocoInverse.cpp b/OpenSim/Moco/Test/testMocoInverse.cpp index b7f85318f1..375d674dd6 100644 --- a/OpenSim/Moco/Test/testMocoInverse.cpp +++ b/OpenSim/Moco/Test/testMocoInverse.cpp @@ -194,7 +194,7 @@ TEST_CASE("MocoInverse Rajagopal2016, 18 muscles", "[casadi]") { auto& solver = study.updSolver(); solver.resetProblem(problem); - solver.set_enforce_path_constraint_midpoints(true); + solver.set_enforce_path_constraint_mesh_interior_points(true); MocoSolution solution = study.solve(); diff --git a/OpenSim/Moco/osimMoco.h b/OpenSim/Moco/osimMoco.h index c31b90d431..cc39e2c268 100644 --- a/OpenSim/Moco/osimMoco.h +++ b/OpenSim/Moco/osimMoco.h @@ -26,6 +26,8 @@ #include "MocoCasADiSolver/MocoCasADiSolver.h" #include "MocoConstraint.h" #include "MocoControlBoundConstraint.h" +#include "MocoOutputBoundConstraint.h" +#include "MocoStateBoundConstraint.h" #include "MocoFrameDistanceConstraint.h" #include "MocoGoal/MocoAccelerationTrackingGoal.h" #include "MocoGoal/MocoAngularVelocityTrackingGoal.h" @@ -33,6 +35,7 @@ #include "MocoGoal/MocoContactTrackingGoal.h" #include "MocoGoal/MocoControlGoal.h" #include "MocoGoal/MocoControlTrackingGoal.h" +#include "MocoGoal/MocoExpressionBasedParameterGoal.h" #include "MocoGoal/MocoInitialActivationGoal.h" #include "MocoGoal/MocoInitialForceEquilibriumDGFGoal.h" #include "MocoGoal/MocoInitialVelocityEquilibriumDGFGoal.h" diff --git a/OpenSim/Moco/tropter/TropterProblem.cpp b/OpenSim/Moco/tropter/TropterProblem.cpp index 3e45dd3f0f..19aeae7bc7 100644 --- a/OpenSim/Moco/tropter/TropterProblem.cpp +++ b/OpenSim/Moco/tropter/TropterProblem.cpp @@ -143,9 +143,11 @@ convertIterateTropterToMoco(const tropIterateType& tropSol, input_control_names, multiplier_names, derivative_names, parameter_names, states, controls, input_controls, multipliers, derivatives, parameters); - // Append slack variables. + + // Append slack variables. Interpolate slack variables to remove NaNs. for (int i = 0; i < numSlacks; ++i) { - mocoIter.appendSlack(slack_names[i], slacks.col(i)); + mocoIter.appendSlack(slack_names[i], + interpolate(time, slacks.col(i), time, true, true)); } return mocoIter; } diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 8e30852df9..b17448daa7 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -24,6 +24,5 @@ using namespace OpenSim; int main() { - return EXIT_SUCCESS; } diff --git a/OpenSim/Simulation/Model/CoordinateLimitForce.h b/OpenSim/Simulation/Model/CoordinateLimitForce.h index 1d8574d3de..064baccec7 100644 --- a/OpenSim/Simulation/Model/CoordinateLimitForce.h +++ b/OpenSim/Simulation/Model/CoordinateLimitForce.h @@ -37,10 +37,10 @@ namespace OpenSim { /** * Generate a force that acts to limit the range of motion of a coordinate. * Force is experienced at upper and lower limits of the coordinate value - * according to a constant stiffnesses K_upper and K_lower, with a C2 continuous + * according to constant stiffnesses K_upper and K_lower, with a C2-continuous * transition from 0 to K. The transition parameter defines how far beyond the * limit the stiffness becomes constant. The integrator will like smoother - * (i.e. larger transition regions). + * (i.e. larger) transition regions. * * Damping factor is also phased in through the transition region from 0 to the * value provided. @@ -48,7 +48,7 @@ namespace OpenSim { * Limiting force is guaranteed to be zero within the upper and lower limits. * * The potential energy stored in the spring component of the force is - * accessible as well as the power (nd optionally energy) dissipated. + * accessible as well as the power (and, optionally, energy) dissipated. * The function has the following shape: * * \image html coordinate_limit_force.png diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp index 8bfd4c3b2f..e876f8fb19 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp @@ -269,6 +269,7 @@ void ExpressionBasedBushingForce::setFzExpression(std::string expression) set_Fz_expression(expression); FzProg = Lepton::Parser::parse(expression).optimize().createProgram(); } + //============================================================================= // COMPUTATION //============================================================================= @@ -309,6 +310,11 @@ SimTK::Vec6 ExpressionBasedBushingForce:: return -_dampingMatrix * dqdot; } +SimTK::Vec6 ExpressionBasedBushingForce::calcBushingForce( + const SimTK::State& s) const { + return calcStiffnessForce(s) + calcDampingForce(s); +} + /* Compute the force contribution to the system and add in to appropriate * bodyForce and/or system generalizedForce. */ @@ -316,17 +322,9 @@ void ExpressionBasedBushingForce::computeForce(const SimTK::State& s, SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const { - // stiffness force - Vec6 fk = calcStiffnessForce(s); - // damping force - Vec6 fv = calcDampingForce(s); - - // total bushing force in the internal basis of the deflection (dq) - Vec6 f = fk + fv; - // convert internal forces to spatial and add then add to system // physical (body) forces - addInPhysicalForcesFromInternal(s, f, bodyForces); + addInPhysicalForcesFromInternal(s, calcBushingForce(s), bodyForces); } //============================================================================= @@ -367,7 +365,7 @@ OpenSim::Array ExpressionBasedBushingForce:: SpatialVec F_GF( Vec3(0.0),Vec3(0.0) ); // total bushing force in the internal basis of the deflection (dq) - Vec6 f = calcStiffnessForce(s) + calcDampingForce(s); + Vec6 f = calcBushingForce(s); convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM); @@ -426,7 +424,7 @@ void ExpressionBasedBushingForce::generateDecorations SpatialVec F_GF(Vec3(0.0), Vec3(0.0)); // total bushing force in the internal basis of the deflection (dq) - Vec6 f = calcStiffnessForce(s) + calcDampingForce(s); + Vec6 f = calcBushingForce(s); convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM); diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h index 2c3ace22bb..83d75f86c3 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h @@ -93,6 +93,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); OpenSim_DECLARE_PROPERTY(translational_damping, SimTK::Vec3, "Damping parameters resisting translational deflection (delta_dot) ."); +//============================================================================== +// OUTPUTS +//============================================================================== + OpenSim_DECLARE_OUTPUT(bushing_force, SimTK::Vec6, calcBushingForce, + SimTK::Stage::Dynamics); + //============================================================================== // PUBLIC METHODS //============================================================================== @@ -226,7 +232,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); function of the deflection rate between the bushing frames. It is the force on frame2 from frame1 in the basis of the deflection rate (dqdot).*/ SimTK::Vec6 calcDampingForce(const SimTK::State& state) const; - + + /** Calculate the total bushing force. This is the sum of the stiffness and + damping force contributions. */ + SimTK::Vec6 calcBushingForce(const SimTK::State& state) const; //-------------------------------------------------------------------------- // Reporting diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp index 637622f9f2..c54dd59fdf 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp @@ -53,8 +53,8 @@ ExpressionBasedCoordinateForce::ExpressionBasedCoordinateForce( setExpression(expression); } -// Set the expression for the force function and create it's lepton program -void ExpressionBasedCoordinateForce::setExpression(const string& expression) +// Set the expression for the force function and create its lepton program +void ExpressionBasedCoordinateForce::setExpression(const string& expression) { set_expression(expression); } @@ -68,7 +68,7 @@ void ExpressionBasedCoordinateForce::setExpression(const string& expression) */ void ExpressionBasedCoordinateForce::setNull() { - setAuthors("Nabeel Allana"); + setAuthors("Nabeel Allana"); } //_____________________________________________________________________________ @@ -94,9 +94,9 @@ void ExpressionBasedCoordinateForce::extendConnectToModel(Model& aModel) string& expression = upd_expression(); expression.erase( - remove_if(expression.begin(), expression.end(), ::isspace), + remove_if(expression.begin(), expression.end(), ::isspace), expression.end() ); - + _forceProg = Lepton::Parser::parse(expression).optimize().createProgram(); // Look up the coordinate @@ -105,7 +105,7 @@ void ExpressionBasedCoordinateForce::extendConnectToModel(Model& aModel) throw (Exception(errorMessage.c_str())); } _coord = &_model->updCoordinateSet().get(coordName); - + if(getName() == "") setName("expressionCoordForce_"+coordName); } @@ -124,8 +124,8 @@ void ExpressionBasedCoordinateForce:: // Computing //============================================================================= // Compute and apply the force -void ExpressionBasedCoordinateForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, +void ExpressionBasedCoordinateForce::computeForce(const SimTK::State& s, + SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const { applyGeneralizedForce(s, *_coord, calcExpressionForce(s), generalizedForces); @@ -147,8 +147,7 @@ double ExpressionBasedCoordinateForce::calcExpressionForce(const SimTK::State& s // get the force magnitude that has already been computed const double& ExpressionBasedCoordinateForce:: - getForceMagnitude(const SimTK::State& s) -{ + getForceMagnitude(const SimTK::State& s) const { return getCacheVariableValue(s, _forceMagnitudeCV); } @@ -156,7 +155,7 @@ const double& ExpressionBasedCoordinateForce:: //============================================================================= // Reporting //============================================================================= -// Provide names of the quantities (column labels) of the force value(s) +// Provide names of the quantities (column labels) of the force value(s) // reported. Array ExpressionBasedCoordinateForce::getRecordLabels() const { OpenSim::Array labels(""); diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h index 000cec60ae..e6e0b3313a 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h @@ -42,6 +42,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); "and its time derivative (qdot). Note, expression cannot have any whitespace" "separating characters"); //============================================================================== +// OUTPUTS +//============================================================================== + OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude, + SimTK::Stage::Dynamics); +//============================================================================== // PUBLIC METHODS //============================================================================== /** Default constructor. **/ @@ -58,7 +63,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); /** * Coordinate */ - void setCoordinateName(const std::string& coord) + void setCoordinateName(const std::string& coord) { set_coordinate(coord); } const std::string& getCoordinateName() const {return get_coordinate();} @@ -66,32 +71,32 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); * %Set the mathematical expression that defines the force magnitude of this * coordinate force in terms of the coordinate value (q) and its * time derivative (qdot). Expressions with C-mathematical operations - * such as +,-,*,/ and common functions: exp, pow, sqrt, sin, cos, tan, + * such as +,-,*,/ and common functions: exp, pow, sqrt, sin, cos, tan, * and so on are acceptable. * NOTE: a limitation is that the expression may not contain whitespace * @param expression string containing the mathematical expression that - * defines the coordinate force + * defines the coordinate force */ void setExpression(const std::string& expression); - /** - * Get the computed Force magnitude determined by evaluating the + /** + * Get the computed Force magnitude determined by evaluating the * expression above. Note, computeForce must be evaluated first, * and this is done automatically if the system is realized to Dynamics * @param state const state (reference) for the model * @return const double ref to the force magnitude */ - const double& getForceMagnitude(const SimTK::State& state); + const double& getForceMagnitude(const SimTK::State& state) const; //============================================================================== // COMPUTATION //============================================================================== - /** Compute the coordinate force based on the user-defined expression + /** Compute the coordinate force based on the user-defined expression and apply it to the model */ - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, + void computeForce(const SimTK::State& state, + SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const override; //-------------------------------------------------------------------------- @@ -103,7 +108,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); //============================================================================== // Reporting //============================================================================== - /** + /** * Provide name(s) of the quantities (column labels) of the force value(s) to be reported */ OpenSim::Array getRecordLabels() const override; @@ -112,7 +117,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); */ OpenSim::Array getRecordValues(const SimTK::State& state) const override; - + protected: //============================================================================== diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp index a1157bb332..5560d0d942 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp @@ -202,8 +202,7 @@ void ExpressionBasedPointToPointForce::computeForce(const SimTK::State& s, // get the force magnitude that has already been computed const double& ExpressionBasedPointToPointForce:: - getForceMagnitude(const SimTK::State& s) -{ + getForceMagnitude(const SimTK::State& s) const { return getCacheVariableValue(s, _forceMagnitudeCV); } diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h index 311d1fe504..67a3929d13 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h @@ -71,6 +71,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); "the distance (d) between the points and its time derivative (ddot). " "Note, expression cannot have any whitespace separating characters."); +//============================================================================== +// OUTPUTS +//============================================================================== + OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude, + SimTK::Stage::Dynamics); //============================================================================== // PUBLIC METHODS @@ -132,7 +137,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); * @param state const state (reference) for the model * @return const double ref to the force magnitude */ - const double& getForceMagnitude(const SimTK::State& state); + const double& getForceMagnitude(const SimTK::State& state) const; //-------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Model/PointForceDirection.h b/OpenSim/Simulation/Model/PointForceDirection.h index af47c756fb..4296cfbb3e 100644 --- a/OpenSim/Simulation/Model/PointForceDirection.h +++ b/OpenSim/Simulation/Model/PointForceDirection.h @@ -9,8 +9,8 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2005-2017 Stanford University and the Authors * - * Author(s): Ajay Seth * + * Copyright (c) 2005-2024 Stanford University and the Authors * + * Author(s): Ajay Seth, Adam Kewley * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * * not use this file except in compliance with the License. You may obtain a * @@ -23,70 +23,74 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -// INCLUDES +#include #include namespace OpenSim { class Body; class PhysicalFrame; -//============================================================================= -//============================================================================= -/** Convenience class for a generic representation of geometry of a complex - Force (or any other object) with multiple points of contact through - which forces are applied to bodies. This represents one such point and an - array of these objects defines a complete Force distribution (ie. path). + +/** + * Convenience class for a generic representation of geometry of a complex + * Force (or any other object) with multiple points of contact through + * which forces are applied to bodies. This represents one such point and an + * array of these objects defines a complete Force distribution (i.e., path). * * @author Ajay Seth * @version 1.0 */ +class OSIMSIMULATION_API PointForceDirection final { +public: + PointForceDirection( + SimTK::Vec3 point, + const PhysicalFrame& frame, + SimTK::Vec3 direction) : + + _point(point), _frame(&frame), _direction(direction) + {} + + [[deprecated("the 'scale' functionality should not be used in new code: OpenSim already assumes 'direction' is non-unit-length")]] + PointForceDirection( + SimTK::Vec3 point, + const PhysicalFrame& frame, + SimTK::Vec3 direction, + double scale) : + + _point(point), _frame(&frame), _direction(direction), _scale(scale) + {} + + /** Returns the point of "contact", defined in `frame()` */ + SimTK::Vec3 point() { return _point; } -class OSIMSIMULATION_API PointForceDirection -{ + /** Returns the frame in which `point()` is defined */ + const PhysicalFrame& frame() { return *_frame; } + + /** Returns the (potentially, non-unit-length) direction, defined in ground, of the force at `point()` */ + SimTK::Vec3 direction() { return _direction; } + + /** Returns the scale factor of the force */ + [[deprecated("this functionality should not be used in new code: OpenSim already assumes 'direction' is non-unit-length")]] + double scale() { return _scale; } + + /** Replaces the current direction with `direction + newDirection` */ + void addToDirection(SimTK::Vec3 newDirection) { _direction += newDirection; } -//============================================================================= -// MEMBER VARIABLES -//============================================================================= private: /** Point of "contact" with a body, defined in the body frame */ SimTK::Vec3 _point; + /** The frame in which the point is defined */ - const PhysicalFrame &_frame; + const PhysicalFrame* _frame; + /** Direction of the force at the point, defined in ground */ SimTK::Vec3 _direction; - /** Optional parameter to scale the force that results from a scalar - (tension) multiplies the direction */ - double _scale; -//============================================================================= -// METHODS -//============================================================================= - //-------------------------------------------------------------------------- - // CONSTRUCTION - //-------------------------------------------------------------------------- -public: - virtual ~PointForceDirection() {}; - /** Default constructor takes the point, body, direction and scale - as arguments */ - PointForceDirection(SimTK::Vec3 point, const PhysicalFrame &frame, - SimTK::Vec3 direction, double scale=1): - _point(point), _frame(frame), _direction(direction), _scale(scale) - {} - /** get point of "contact" with on a body defined in the body frame */ - SimTK::Vec3 point() {return _point; } - /** get the body in which the point is defined */ - const PhysicalFrame& frame() {return _frame; } - /** get direction of the force at the point defined in ground */ - SimTK::Vec3 direction() {return _direction; } - /** get the scale factor on the force */ - double scale() {return _scale; } - - /** replace the current direction with the resultant with a new direction */ - void addToDirection(SimTK::Vec3 newDirection) {_direction+=newDirection;} - -//============================================================================= -}; // END of class PointForceDirection -//============================================================================= + /** Deprecated parameter to scale the force that results from a scalar + (tension) multiplies the direction */ + double _scale = 1.0; +}; + } // namespace #endif // __PointForceDirection_h__ diff --git a/OpenSim/Simulation/TableProcessor.h b/OpenSim/Simulation/TableProcessor.h index 21aa4c4f1d..4542f2f2cc 100644 --- a/OpenSim/Simulation/TableProcessor.h +++ b/OpenSim/Simulation/TableProcessor.h @@ -258,7 +258,7 @@ class OSIMSIMULATION_API TabOpAppendCoupledCoordinateValues }; /** Invoke SimulationUtilities::appendCoordinateValueDerivativesAsSpeeds() on -the table */ +the table. */ class OSIMSIMULATION_API TabOpAppendCoordinateValueDerivativesAsSpeeds : public TableOperator { OpenSim_DECLARE_CONCRETE_OBJECT( diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 158371ca0e..66e27a4de5 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -33,8 +33,10 @@ // 7. RotationalCoordinateLimitForce // 8. ExternalForce // 9. PathSpring -// 10. ExpressionBasedPointToPointForce -// 11. Blankevoort1991Ligament +// 10. ExpressionBasedCoordinateForce +// 11. ExpressionBasedPointToPointForce +// 12. ExpressionBasedBushingForce +// 13. Blankevoort1991Ligament // // Add tests here as Forces are added to OpenSim // @@ -139,9 +141,10 @@ TEST_CASE("testExpressionBasedCoordinateForce") { osimModel.setGravity(gravity_vec); // ode for basic mass-spring-dampener system - ExpressionBasedCoordinateForce spring("ball_h", "-10*q-5*qdot"); + ExpressionBasedCoordinateForce* spring = + new ExpressionBasedCoordinateForce("ball_h", "-10*q-5*qdot"); - osimModel.addForce(&spring); + osimModel.addForce(spring); // Create the force reporter ForceReporter* reporter = new ForceReporter(&osimModel); @@ -170,6 +173,7 @@ TEST_CASE("testExpressionBasedCoordinateForce") { osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration); Vec3 pos = ball.findStationLocationInGround(osim_state, Vec3(0)); + // Check ball height against analytical solution. double height = exp(-1 * zeta * omega * osim_state.getTime()) * ((start_h - dh) * @@ -180,14 +184,23 @@ TEST_CASE("testExpressionBasedCoordinateForce") { sin(damp_freq * osim_state.getTime()))) + dh; - ASSERT_EQUAL(height, pos(1), 1e-6); + + // Check that the force reported by spring is correct. + double ball_h = sliderCoord.getValue(osim_state); + double ball_h_dot = sliderCoord.getSpeedValue(osim_state); + double analytical_force = -10*ball_h - 5*ball_h_dot; + double model_force = spring->getForceMagnitude(osim_state); + double output_force = + spring->getOutputValue(osim_state, "force_magnitude"); + ASSERT_EQUAL(analytical_force, model_force, 1e-6); + ASSERT_EQUAL(analytical_force, output_force, 1e-6); } // Test copying - ExpressionBasedCoordinateForce* copyOfSpring = spring.clone(); + ExpressionBasedCoordinateForce* copyOfSpring = spring->clone(); - ASSERT(*copyOfSpring == spring); + ASSERT(*copyOfSpring == *spring); osimModel.print("ExpressionBasedCoordinateForceModel.osim"); @@ -264,6 +277,8 @@ TEST_CASE("testExpressionBasedPointToPointForce") { // Now check that the force reported by spring double model_force = p2pForce->getForceMagnitude(state); + double output_force = + p2pForce->getOutputValue(state, "force_magnitude"); // Save the forces // reporter->getForceStorage().print("path_spring_forces.mot"); @@ -280,6 +295,7 @@ TEST_CASE("testExpressionBasedPointToPointForce") { // something is wrong if the block does not reach equilibrium ASSERT_EQUAL(analytical_force, model_force, 1e-5); + ASSERT_EQUAL(analytical_force, output_force, 1e-5); // Before exiting lets see if copying the P2P force works ExpressionBasedPointToPointForce* copyOfP2pForce = p2pForce->clone(); @@ -915,6 +931,11 @@ TEST_CASE("testExpressionBasedBushingForceTranslational") { // check analytical force corresponds to the force on the ball // in the Y direction, index = 7 ASSERT_EQUAL(analytical_force, model_force[7], 2e-4); + + // check that the force from the Output is correct + SimTK::Vec6 output_force = + spring.getOutputValue(osim_state, "bushing_force"); + ASSERT_EQUAL(analytical_force, output_force[4], 2e-4); } manager.getStateStorage().print( @@ -1046,6 +1067,11 @@ TEST_CASE("testExpressionBasedBushingForceRotational") { // check analytical moment corresponds to the moment on the ball // in the Y direction, index = 4 ASSERT_EQUAL(analytical_moment, model_forces[4], 2e-4); + + // check that the force from the Output is correct + SimTK::Vec6 output_force = + spring.getOutputValue(osim_state, "bushing_force"); + ASSERT_EQUAL(analytical_moment, -output_force[1], 2e-4); } manager.getStateStorage().print( diff --git a/OpenSim/Simulation/Test/testFrames.cpp b/OpenSim/Simulation/Test/testFrames.cpp index 7567b2d690..480cd223cc 100644 --- a/OpenSim/Simulation/Test/testFrames.cpp +++ b/OpenSim/Simulation/Test/testFrames.cpp @@ -39,104 +39,25 @@ Tests Include: #include #include #include +#include using namespace OpenSim; using namespace std; using SimTK::Transform; -void testBody(); -void testPhysicalOffsetFrameOnBody(); -void testPhysicalOffsetFrameOnBodySerialize(); -void testPhysicalOffsetFrameOnPhysicalOffsetFrameAsJointParent(); -void testPhysicalOffsetFrameOnPhysicalOffsetFrameAsJointChild(); -void testPhysicalOffsetFrameOnPhysicalOffsetFrame(); -void testPhysicalOffsetFrameOnPhysicalOffsetFrameOrder(); -void testFilterByFrameType(); -void testVelocityAndAccelerationMethods(); +namespace { + class OrdinaryOffsetFrame : public OffsetFrame < Frame > { + OpenSim_DECLARE_CONCRETE_OBJECT(OrdinaryOffsetFrame, OffsetFrame); + public: + OrdinaryOffsetFrame() : OffsetFrame() {} + virtual ~OrdinaryOffsetFrame() {} -class OrdinaryOffsetFrame : public OffsetFrame < Frame > { - OpenSim_DECLARE_CONCRETE_OBJECT(OrdinaryOffsetFrame, OffsetFrame); -public: - OrdinaryOffsetFrame() : OffsetFrame() {} - virtual ~OrdinaryOffsetFrame() {} - - OrdinaryOffsetFrame(const Frame& parent, const SimTK::Transform& offset) : - OffsetFrame(parent, offset) {} -}; - - -int main() -{ - SimTK::Array_ failures; - - try { testBody(); } - catch (const std::exception& e){ - cout << e.what() < #include #include +#include using namespace OpenSim; using namespace std; @@ -36,38 +37,14 @@ using namespace std; // are updated (after a simulation) that the defaults match the values in the // new state. //============================================================================== -void testStates(const string& modelFile); -static const int MAX_N_TRIES = 100; - -int main() -{ - try { - LoadOpenSimLibrary("osimActuators"); - testStates("arm26.osim"); - } - catch (const Exception& e) { - cout << "testInitState failed: "; - e.print(cout); - return 1; - } - catch (const std::exception& e) { - cout << "testInitState failed: " << e.what() << endl; - return 1; - } - cout << "Done" << endl; - return 0; -} - -//============================================================================== -// Test Cases -//============================================================================== -void testStates(const string& modelFile) +TEST_CASE("testStates") { using namespace SimTK; - + LoadOpenSimLibrary("osimActuators"); //========================================================================== // Setup OpenSim model + std::string modelFile = "arm26.osim"; Model model(modelFile); ControlSetController* controller = new ControlSetController(); controller->setControlSetFileName("arm26_StaticOptimization_controls.xml"); diff --git a/OpenSim/Utilities/CMakeLists.txt b/OpenSim/Utilities/CMakeLists.txt deleted file mode 100644 index d31e6519e8..0000000000 --- a/OpenSim/Utilities/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -set(BUILD_SIMM_TRANSLATOR OFF CACHE BOOL "Build Utilities to support importing/exporting SIMM models") - -if(BUILD_SIMM_TRANSLATOR) - subdirs(simmToOpenSim) -endif() diff --git a/OpenSim/Utilities/convertFiles/CMakeLists.txt b/OpenSim/Utilities/convertFiles/CMakeLists.txt deleted file mode 100644 index ad37d077f5..0000000000 --- a/OpenSim/Utilities/convertFiles/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -include_directories(${OpenSim_SOURCE_DIR} ${OpenSim_SOURCE_DIR}/Vendors ${XERCES_INCLUDE_DIR}) - -file(GLOB SOURCE_FILES *.cpp) -file(GLOB INCLUDE_FILES *.h) - -if(BUILD_STATIC_OSIM_LIBS) - add_definitions(-DSTATIC_OSIM_LIBS) -endif(BUILD_STATIC_OSIM_LIBS) - -link_libraries(debug xerces-c_2D optimized xerces-c_2 - debug osimTools_d optimized osimTools - debug osimSimbodyEngine_d optimized osimSimbodyEngine) - -add_executable(convertFiles ${SOURCE_FILES} ${INCLUDE_FILES}) - -set_target_properties(convertFiles PROPERTIES - ${EXCLUDE_IF_MINIMAL_BUILD} PROJECT_LABEL "Utilities - convertFiles") - -install(TARGETS convertFiles DESTINATION "${CMAKE_INSTALL_BINDIR}") diff --git a/OpenSim/Utilities/convertFiles/convertFiles.cpp b/OpenSim/Utilities/convertFiles/convertFiles.cpp deleted file mode 100644 index 510fc19819..0000000000 --- a/OpenSim/Utilities/convertFiles/convertFiles.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/* -------------------------------------------------------------------------- * - * convertFiles.cpp * - * -------------------------------------------------------------------------- * - * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * - * See http://opensim.stanford.edu and the NOTICE file for more information. * - * OpenSim is developed at Stanford University and supported by the US * - * National Institutes of Health (U54 GM072970, P2C HD065690, U54 EB020405) * - * and by DARPA through the Warrior Web program. * - * * - * Copyright (c) 2005-2017 Stanford University and the Authors * - * Author(s): Ayman Habib * - * * - * Licensed under the Apache License, Version 2.0 (the "License"); you may * - * not use this file except in compliance with the License. You may obtain a * - * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * - * * - * Unless required by applicable law or agreed to in writing, software * - * distributed under the License is distributed on an "AS IS" BASIS, * - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * - * See the License for the specific language governing permissions and * - * limitations under the License. * - * -------------------------------------------------------------------------- */ -#include -#include -#include -#include - -using namespace OpenSim; -using namespace std; - -int main(int argc,char **argv) -{ -#ifndef STATIC_OSIM_LIBS - LoadOpenSimLibrary("osimTools"); - LoadOpenSimLibrary("osimActuators"); - LoadOpenSimLibrary("osimAnalyses"); - LoadOpenSimLibrary("osimSimulation"); - LoadOpenSimLibrary("osimSimbodyEngine"); -#endif - - int offset=0; - if(argc>1 && string(argv[1])=="-offline") offset++; - else IO::SetPrintOfflineDocuments(false); - if(argc<1+offset+2) { - std::cerr << "Not enough arguments: " << std::endl; - exit(1); - } - string fileName = argv[offset+1]; - string outputFileName = argv[offset+2]; - Object *obj = Object::makeObjectFromFile(fileName); - std::cout << fileName << " -> " << outputFileName; - if(!obj) std::cout << " FAILED" << std::endl; - else { - std::cout << std::endl; - //IO::SetGFormatForDoubleOutput(true); - obj->copy()->print(outputFileName); - } -} diff --git a/OpenSim/Utilities/openSimToSimm.cpp b/OpenSim/Utilities/openSimToSimm.cpp deleted file mode 100644 index 888704fbdd..0000000000 --- a/OpenSim/Utilities/openSimToSimm.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* -------------------------------------------------------------------------- * - * openSimToSimm.cpp * - * -------------------------------------------------------------------------- * - * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * - * See http://opensim.stanford.edu and the NOTICE file for more information. * - * OpenSim is developed at Stanford University and supported by the US * - * National Institutes of Health (U54 GM072970, P2C HD065690, U54 EB020405) * - * and by DARPA through the Warrior Web program. * - * * - * Copyright (c) 2005-2017 Stanford University and the Authors * - * Author(s): Eran Guendelman * - * * - * Licensed under the Apache License, Version 2.0 (the "License"); you may * - * not use this file except in compliance with the License. You may obtain a * - * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * - * * - * Unless required by applicable law or agreed to in writing, software * - * distributed under the License is distributed on an "AS IS" BASIS, * - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * - * See the License for the specific language governing permissions and * - * limitations under the License. * - * -------------------------------------------------------------------------- */ - -// INCLUDES -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace OpenSim; - -static void PrintUsage(const char *aProgName, ostream &aOStream); - -//______________________________________________________________________________ -/** - * Program to read an xml file for an openSim Model and generate - * SDFast corresponding code. - * - * @param argc Number of command line arguments (should be 4 or more). - * @param argv Command line arguments: openSimToSimm -x xml_in -j joints_out [ -m muscles_out ] - */ -int main(int argc,char **argv) -{ - std::cout << "openSimToSimm, " << OpenSim::GetVersionAndDate() << std::endl; - - Object::RegisterType(SimbodyEngine()); - SimbodyEngine::registerTypes(); - Object::RegisterType(SimmKinematicsEngine()); - SimmKinematicsEngine::registerTypes(); - Object::RegisterType(Schutte1993Muscle()); - Object::RegisterType(Thelen2003Muscle()); - Object::RegisterType(Delp1990Muscle()); - - // PARSE COMMAND LINE - string inName = ""; - string jntName = ""; - string mslName = ""; - - for(int i=1; iGEFuncOK = yes; - } -} - -/* SOLVE_ALL_LOOPS_AND_CONSTRAINTS: - * Solve all loops and constraints at once. If enforce_constraints = yes, - * solve constraints (if not, do not). Return the status of loops and constraints - */ -void solveAllLoopsAndConstraints(ModelStruct *ms, LoopStatus *loopStatus, - ConstraintStatus *constraintStatus, - SBoolean enforce_constraints) -{ - int i, j, nq, nres, index; - int numLoopQs, numLoopResids, numConstraintQs, numConstraintResids; - double *q = NULL, *saved_q = NULL, *resid = NULL; - LCStruct *solveInfo = NULL; - ReturnCode rc; - SBoolean changed = no; - - *loopStatus = loopUnchanged; - *constraintStatus = constraintUnchanged; - - if ((!loopsToSolve(ms)) && (!constraintsToSolve(ms))) - return; - - solveInfo = (LCStruct *)simm_malloc(sizeof(LCStruct)); - solveInfo->model = ms; - solveInfo->first_iter = yes; - solveInfo->largeGenCoordResids = no; - solveInfo->loopClosed = yes; - solveInfo->constraintClosed = yes; - solveInfo->controlled_gc = NULL; - solveInfo->controlled_value = 0.0; - solveInfo->gencoord_list = NULL; - - /* create structure to hold info about loops being solved */ - solveInfo->loopInfo = (IKStruct *)simm_malloc(sizeof(IKStruct)); - solveInfo->loopInfo->ms = ms; - solveInfo->loopInfo->loopUsed = (SBoolean *)simm_malloc(ms->numclosedloops * sizeof(SBoolean)); - solveInfo->loopInfo->gencoord_list = NULL; - solveInfo->loopInfo->controlled_gc = NULL; - solveInfo->loopInfo->controlled_value = 0.0; - solveInfo->loopInfo->first_iter = yes; - - /* create structure to hold info about constraints being solved */ - solveInfo->constraintInfo = (ConstraintSolverStruct *)simm_malloc(sizeof(ConstraintSolverStruct)); - solveInfo->constraintInfo->model = ms; - solveInfo->constraintInfo->controlled_gc = NULL; - solveInfo->constraintInfo->controlled_value = 0.0; - solveInfo->constraintInfo->gencoord_list = NULL; - solveInfo->constraintInfo->first_iter = yes; - solveInfo->constraintInfo->test = no; -// solveInfo->constraintInfo->tolerance = ms->constraint_tolerance; - solveInfo->constraintInfo->consUsed = (SBoolean *)simm_malloc(ms->num_constraint_objects * sizeof(SBoolean)); - - nq = 0; - nres = 0; - numLoopQs = 0; - numLoopResids = 0; - numConstraintQs = 0; - numConstraintResids = 0; - - /* Calculate the total number of qs and residuals in loops. */ - for (i = 0; i < ms->numclosedloops; i++) - { - numLoopQs += ms->loop[i].num_qs; - numLoopResids += ms->loop[i].num_resids; - solveInfo->loopInfo->loopUsed[i] = yes; - } - - /* Calculate the total number of qs and residuals in constraints. */ - for (i = 0; i < ms->num_constraint_objects; i++) - { - solveInfo->constraintInfo->consUsed[i] = no; - if ((enforce_constraints == yes) && (ms->constraintobj[i].active == yes) - && (ms->constraintobj[i].numPoints > 0)) - { - numConstraintQs += ms->constraintobj[i].num_qs; - numConstraintResids += ms->constraintobj[i].numPoints; - solveInfo->constraintInfo->consUsed[i] = yes; - } - } - - if ((numLoopQs == 0) && (numConstraintQs == 0)) - { - goto cleanup; - } - - /* allocate space for list of gencoords in loops and constraints. */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numLoopQs * sizeof(GeneralizedCoord*)); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numConstraintQs * sizeof(GeneralizedCoord*)); - - /* Make a list of all loop gencoords used. */ - index = 0; - for (i = 0; i < ms->numclosedloops; i++) - { - if (solveInfo->loopInfo->loopUsed[i] == yes) - { - for (j = 0; j < ms->loop[i].num_qs; j++) - { - solveInfo->loopInfo->gencoord_list[index++] = ms->loop[i].qs[j]; - } - } - } - /* Make a list of all constraint gencoords used. */ - index = 0; - for (i = 0; i < ms->num_constraint_objects; i++) - { - if (solveInfo->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < ms->constraintobj[i].num_qs; j++) - { - solveInfo->constraintInfo->gencoord_list[index++] = ms->constraintobj[i].qs[j]; - } - } - } - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords are not included in the list. - * Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &numLoopQs, solveInfo->loopInfo->gencoord_list, NULL); - orderQs(ms, &numConstraintQs, solveInfo->constraintInfo->gencoord_list, NULL); - - nq = numLoopQs + numConstraintQs; - - /* copy both gencoord lists into single list */ - solveInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(nq * sizeof(GeneralizedCoord*)); - for (i = 0; i < numLoopQs; i++) - solveInfo->gencoord_list[i] = solveInfo->loopInfo->gencoord_list[i]; - for (i = numLoopQs; i < nq; i++) - solveInfo->gencoord_list[i] = solveInfo->constraintInfo->gencoord_list[i-numLoopQs]; - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords are not included in the list. - * Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &nq, solveInfo->gencoord_list, NULL); - solveInfo->numQs = nq; -//if nq or nres == 0??? - nres = numLoopResids + numConstraintResids + nq; - - /* reallocate space for arrays given new sizes */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->loopInfo->gencoord_list, - numLoopQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->constraintInfo->gencoord_list, - numConstraintQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->gencoord_list, nq * sizeof(GeneralizedCoord*), &rc); - - /* allocate space for arrays now that sizes are known */ - q = (double *)simm_malloc(nq * sizeof(double)); - saved_q = (double *)simm_malloc(nq * sizeof(double)); - resid = (double *)simm_malloc(nres * sizeof(double)); - - /* Copy gencoord values into q array, and save the original gencoord values. */ - for (i = 0; i < nq; i++) - q[i] = saved_q[i] = solveInfo->gencoord_list[i]->value; - - solveInfo->numLoopQs = numLoopQs; - solveInfo->numLoopResids = numLoopResids; - solveInfo->numConstraintQs = numConstraintQs; - solveInfo->numConstraintResids = numConstraintResids; - - /* solve for new qs using Levenberg-Marquard method of minimizing least squares. */ - if (!LeastSquaresSolver(solveInfo, nq, q, nres, resid)) - { -// printf("lss problem\n"); - } - evaluateLCSolution(solveInfo, nq, q, nres, resid); - updateModel(solveInfo->model, solveInfo, nq, q, saved_q, nres, resid, &changed); - -evaluate: - calculateLCResids(nres, nq, q, resid, &i, solveInfo); - evaluateLCSolution(solveInfo, nq, q, nres, resid); - -cleanup: - if (solveInfo) - { - if (solveInfo->loopClosed == no) - *loopStatus = loopBroken; - if (solveInfo->constraintClosed == no) - *constraintStatus = constraintBroken; - if (changed == yes) - { - *loopStatus = loopChanged; - *constraintStatus = constraintChanged; - } - if (solveInfo->largeGenCoordResids == yes) - { - *loopStatus = largeResidinLoop; - *constraintStatus = gcOutOfRange; - } - - ms->loopsOK = solveInfo->loopClosed; - ms->constraintsOK = solveInfo->constraintClosed; - - FREE_IFNOTNULL(solveInfo->loopInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->loopInfo->loopUsed); - FREE_IFNOTNULL(solveInfo->loopInfo); - FREE_IFNOTNULL(solveInfo->constraintInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->constraintInfo->consUsed); - FREE_IFNOTNULL(solveInfo->constraintInfo); - FREE_IFNOTNULL(solveInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo); - } - FREE_IFNOTNULL(q); - FREE_IFNOTNULL(saved_q); - FREE_IFNOTNULL(resid); -} - -/* Solve any loops and/or constraints that are affected by the given gencoord */ -//try to set to desired gc_value, if works OK, if doesn't work set to orig -SBoolean solveLCAffectedByGC(ModelStruct *ms, GeneralizedCoord* controlled_gc, double *gc_value) -{ - int i, j, index, nq, nres; - int numLoopQs, numLoopResids, numConstraintQs, numConstraintResids; - double orig_value; - double *q = NULL, *saved_q = NULL, *resid = NULL; - LCStruct *solveInfo; - ReturnCode rc; - SBoolean validSolution = yes; - - /* if there are no loops or constraints to solve */ - if ((!loopsToSolve(ms)) && (!constraintsToSolve(ms))) - return validSolution; - - /* store the original value of the controlled gencoord */ - orig_value = controlled_gc->value; - - solveInfo = (LCStruct *)simm_malloc(sizeof(LCStruct)); - solveInfo->model = ms; - solveInfo->first_iter = yes; - solveInfo->largeGenCoordResids = no; - solveInfo->loopClosed = yes; - solveInfo->constraintClosed = yes; - solveInfo->controlled_gc = controlled_gc; - solveInfo->controlled_value = *gc_value; - - /* create structure to hold info about loops being solved */ - solveInfo->loopInfo = (IKStruct *)simm_malloc(sizeof(IKStruct)); - solveInfo->loopInfo->ms = ms; - solveInfo->loopInfo->loopUsed = (SBoolean *)simm_malloc(ms->numclosedloops * sizeof(SBoolean)); - solveInfo->loopInfo->gencoord_list = NULL; - solveInfo->loopInfo->controlled_gc = controlled_gc; - solveInfo->loopInfo->controlled_value = *gc_value; - solveInfo->loopInfo->first_iter = yes; - - /* create structure to hold info about constraints being solved */ - solveInfo->constraintInfo = (ConstraintSolverStruct *)simm_malloc(sizeof(ConstraintSolverStruct)); - solveInfo->constraintInfo->model = ms; - solveInfo->constraintInfo->controlled_gc = controlled_gc; - solveInfo->constraintInfo->controlled_value = *gc_value; - solveInfo->constraintInfo->gencoord_list = NULL; - solveInfo->constraintInfo->first_iter = yes; - solveInfo->constraintInfo->test = no; - solveInfo->constraintInfo->consUsed = (SBoolean *)simm_malloc(ms->num_constraint_objects * sizeof(SBoolean)); - - nq = 0; - nres = 0; - numLoopQs = 0; - numLoopResids = 0; - numConstraintQs = 0; - numConstraintResids = 0; - - /* mark all loops that are affected by a change in the controlled gencoord */ - markAffectedLoopsAndConstraints(solveInfo, controlled_gc); - - /* Calculate the total number of qs and residuals in loops. */ - for (i = 0; i < ms->numclosedloops; i++) - { - if (solveInfo->loopInfo->loopUsed[i]) - { - numLoopQs += ms->loop[i].num_qs; - numLoopResids += ms->loop[i].num_resids; - } - } - - /* Calculate the total number of qs and residuals in constraints. */ - for (i = 0; i < ms->num_constraint_objects; i++) - { - if (solveInfo->constraintInfo->consUsed[i]) - { - numConstraintQs += ms->constraintobj[i].num_qs; - numConstraintResids += ms->constraintobj[i].numPoints; - } - } - - /* allocate space for list of gencoords in loops and constraints. */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numLoopQs * sizeof(GeneralizedCoord*)); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numConstraintQs * sizeof(GeneralizedCoord*)); - - /* Make a list of all loop gencoords used. */ - index = 0; - for (i = 0; i < ms->numclosedloops; i++) - { - if (solveInfo->loopInfo->loopUsed[i] == yes) - { - for (j = 0; j < ms->loop[i].num_qs; j++) - { - solveInfo->loopInfo->gencoord_list[index++] = ms->loop[i].qs[j]; - } - } - } - - /* Make a list of all constraint gencoords used. */ - index = 0; - for (i = 0; i < ms->num_constraint_objects; i++) - { - if (solveInfo->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < ms->constraintobj[i].num_qs; j++) - { - solveInfo->constraintInfo->gencoord_list[index++] = ms->constraintobj[i].qs[j]; - } - } - } - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords and the controlled gencoord are not included in the - * list. Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &numLoopQs, solveInfo->loopInfo->gencoord_list, controlled_gc); - orderQs(ms, &numConstraintQs, solveInfo->constraintInfo->gencoord_list, controlled_gc); - nq = numLoopQs + numConstraintQs; - - /* copy both gencoord lists into single list */ - solveInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(nq * sizeof(GeneralizedCoord*)); - for (i = 0; i < numLoopQs; i++) - solveInfo->gencoord_list[i] = solveInfo->loopInfo->gencoord_list[i]; - for (i = numLoopQs; i < nq; i++) - solveInfo->gencoord_list[i] = solveInfo->constraintInfo->gencoord_list[i-numLoopQs]; - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords are not included in the list. - * Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &nq, solveInfo->gencoord_list, NULL); - solveInfo->numQs = nq; - - if (nq <= 0) //added dkb april 3, 2003 - { - return validSolution; - } - - nres = numLoopResids + numConstraintResids + nq; - - /* reallocate space for arrays given new sizes */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->loopInfo->gencoord_list, - numLoopQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->constraintInfo->gencoord_list, - numConstraintQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->gencoord_list, nq * sizeof(GeneralizedCoord*), &rc); - - /* allocate space for arrays now that sizes are known */ - q = (double *)simm_malloc(nq * sizeof(double)); - saved_q = (double *)simm_malloc(nq * sizeof(double)); - resid = (double *)simm_malloc(nres * sizeof(double)); - - /* Copy gencoord values into q array, and save the original gencoord values. */ - for (i = 0; i < nq; i++) - q[i] = saved_q[i] = solveInfo->gencoord_list[i]->value; - - solveInfo->numLoopQs = numLoopQs; - solveInfo->numLoopResids = numLoopResids; - solveInfo->numConstraintQs = numConstraintQs; - solveInfo->numConstraintResids = numConstraintResids; - - nres = numLoopResids + numConstraintResids + nq; - - /* Set the value of the controlled gencoord to its controlled value. */ - setGencoordValue2(solveInfo->model, solveInfo->controlled_gc, solveInfo->controlled_value); - - /* solve for new qs using Levenberg-Marquard method of minimizing least squares. */ - if (!LeastSquaresSolver(solveInfo, nq, q, nres, resid)) - { -// printf("lss problem\n"); - } - - evaluateLCSolution(solveInfo, nq, q, nres, resid); - - if (numLoopQs > 0) - { - if (solveInfo->loopClosed == yes) - { - /* To ensure the display will update correctly, restore original gencoord values - * in model structure. When setting the gencoords based on the q values, all - * the necessary joint matrices will be calculated. - */ - for (i = 0; i < nq; i++) - { - solveInfo->gencoord_list[i]->value = saved_q[i]; - } - } - else - { - *gc_value = orig_value; - for (i = 0; i < nq; i++) - q[i] = saved_q[i]; - } - } - - if (numConstraintQs > 0) - { - if (solveInfo->constraintClosed == yes) - { - /* To ensure the display will update correctly, restore original gencoord values - * in model structure. When setting the gencoords based on the q values, all - * the necessary joint matrices will be calculated. - */ - for (i = 0; i < nq; i++) - { - // ms->gencoord[solveInfo->gencoord_list[i]].value = saved_q[i]; - set_gencoord_value(ms, solveInfo->gencoord_list[i], saved_q[i], no); - } - - } - else - { - /* Set the gencoord values for all gencoords used in constraints. To prevent - * recursion, set the flag so loops and constraints won't be resolved. */ - // for (i = 0; i < nq; i++) - // { - // set_gencoord_value(ms, solveInfo->gencoord_list[i], saved_q[i], no); - // } - // valid = no; - *gc_value = orig_value; - for (i = 0; i < nq; i++) - q[i] = saved_q[i]; - } - } - - /* If the controlled q is clamped and the new solution is out of range, - * don't reset any q's return with original values. - */ - if ((controlled_gc->clamped == yes) && ((*gc_value < controlled_gc->range.start) || (*gc_value > controlled_gc->range.end))) - { - *gc_value = orig_value; - validSolution = no; - } - else - { - /* Set the gencoord values for all gencoords used in loops. To prevent - * recursion, set the flag so loops won't be resolved. */ - for (i = 0; i < nq; i++) - { - set_gencoord_value(ms, solveInfo->gencoord_list[i], q[i], no); - } - validSolution = yes; - } -//dkb may 15, 2003 - should these be set here? we don't know about status - //of other loops and constraints that weren't solved - //ms->constraintsOK = validSolution; - //ms->loopsOK = validSolution; - FREE_IFNOTNULL(solveInfo->loopInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->loopInfo->loopUsed); - FREE_IFNOTNULL(solveInfo->loopInfo); - FREE_IFNOTNULL(solveInfo->constraintInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->constraintInfo); - FREE_IFNOTNULL(solveInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo); - FREE_IFNOTNULL(q); - FREE_IFNOTNULL(saved_q); - FREE_IFNOTNULL(resid); - - return validSolution; -} - -SBoolean solveLCAffectedByJNT(ModelStruct *ms, int joint, LoopStatus *loopStatus, ConstraintStatus *constraintStatus) -{ - int i, j, index, nq, nres; - int numLoopQs, numLoopResids, numConstraintQs, numConstraintResids; - double *q = NULL, *saved_q = NULL, *resid = NULL; - LCStruct *solveInfo; - ReturnCode rc; - SBoolean validSolution, changed; - int controlled_gc; - - *loopStatus = loopUnchanged; - *constraintStatus = constraintUnchanged; - - /* if there are no loops or constraints to solve */ - if ((!loopsToSolve(ms)) && (!constraintsToSolve(ms))) - return yes; - - solveInfo = (LCStruct *)simm_malloc(sizeof(LCStruct)); - solveInfo->model = ms; - solveInfo->first_iter = yes; - solveInfo->largeGenCoordResids = no; - solveInfo->loopClosed = yes; - solveInfo->constraintClosed = yes; - solveInfo->controlled_gc = NULL; - solveInfo->controlled_value = 0.0; - - /* create structure to hold info about loops being solved */ - solveInfo->loopInfo = (IKStruct *)simm_malloc(sizeof(IKStruct)); - solveInfo->loopInfo->ms = ms; - solveInfo->loopInfo->loopUsed = (SBoolean *)simm_malloc(ms->numclosedloops * sizeof(SBoolean)); - solveInfo->loopInfo->gencoord_list = NULL; - solveInfo->loopInfo->controlled_gc = NULL; - solveInfo->loopInfo->controlled_value = 0.0; - solveInfo->loopInfo->first_iter = yes; - - /* create structure to hold info about constraints being solved */ - solveInfo->constraintInfo = (ConstraintSolverStruct *)simm_malloc(sizeof(ConstraintSolverStruct)); - solveInfo->constraintInfo->model = ms; - solveInfo->constraintInfo->controlled_gc = NULL; - solveInfo->constraintInfo->controlled_value = 0.0; - solveInfo->constraintInfo->gencoord_list = NULL; - solveInfo->constraintInfo->first_iter = yes; - solveInfo->constraintInfo->test = no; -// solveInfo->constraintInfo->tolerance = ms->constraint_tolerance; - solveInfo->constraintInfo->consUsed = (SBoolean *)simm_malloc(ms->num_constraint_objects * sizeof(SBoolean)); - - nq = 0; - nres = 0; - numLoopQs = 0; - numLoopResids = 0; - numConstraintQs = 0; - numConstraintResids = 0; - - /* mark all loops that are affected by a change in the controlled gencoord */ - markLoopsAndConstraintsAffected(solveInfo, joint); - - /* Calculate the total number of qs and residuals in loops. */ - for (i = 0; i < ms->numclosedloops; i++) - { - if (solveInfo->loopInfo->loopUsed[i]) - { - numLoopQs += ms->loop[i].num_qs; - numLoopResids += ms->loop[i].num_resids; - } - } - - /* Calculate the total number of qs and residuals in constraints. */ - for (i = 0; i < ms->num_constraint_objects; i++) - { - if (solveInfo->constraintInfo->consUsed[i]) - { - numConstraintQs += ms->constraintobj[i].num_qs; - numConstraintResids += ms->constraintobj[i].numPoints; - } - } - - /* allocate space for list of gencoords in loops and constraints. */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numLoopQs * sizeof(GeneralizedCoord*)); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numConstraintQs * sizeof(GeneralizedCoord*)); - - /* Make a list of all loop gencoords used. */ - index = 0; - for (i = 0; i < ms->numclosedloops; i++) - { - if (solveInfo->loopInfo->loopUsed[i] == yes) - { - for (j = 0; j < ms->loop[i].num_qs; j++) - { - solveInfo->loopInfo->gencoord_list[index++] = ms->loop[i].qs[j]; - } - } - } - /* Make a list of all constraint gencoords used. */ - index = 0; - for (i = 0; i < ms->num_constraint_objects; i++) - { - if (solveInfo->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < ms->constraintobj[i].num_qs; j++) - { - solveInfo->constraintInfo->gencoord_list[index++] = ms->constraintobj[i].qs[j]; - } - } - } - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords and the controlled gencoord are not included in the - * list. Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &numLoopQs, solveInfo->loopInfo->gencoord_list, NULL); - orderQs(ms, &numConstraintQs, solveInfo->constraintInfo->gencoord_list, NULL); - nq = numLoopQs + numConstraintQs; - - /* copy both gencoord lists into single list */ - solveInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(nq * sizeof(GeneralizedCoord*)); - for (i = 0; i < numLoopQs; i++) - solveInfo->gencoord_list[i] = solveInfo->loopInfo->gencoord_list[i]; - for (i = numLoopQs; i < nq; i++) - solveInfo->gencoord_list[i] = solveInfo->constraintInfo->gencoord_list[i-numLoopQs]; - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords are not included in the list. - * Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &nq, solveInfo->gencoord_list, NULL); - solveInfo->numQs = nq; - - if (nq <= 0) //added dkb april 3, 2003 - { - FREE_IFNOTNULL(solveInfo->loopInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->loopInfo->loopUsed); - FREE_IFNOTNULL(solveInfo->loopInfo); - FREE_IFNOTNULL(solveInfo->constraintInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->constraintInfo); - FREE_IFNOTNULL(solveInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo); - FREE_IFNOTNULL(q); - FREE_IFNOTNULL(saved_q); - FREE_IFNOTNULL(resid); - return yes; - } - - nres = numLoopResids + numConstraintResids + nq; - - /* reallocate space for arrays given new sizes */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->loopInfo->gencoord_list, - numLoopQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->constraintInfo->gencoord_list, - numConstraintQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->gencoord_list, - nq * sizeof(GeneralizedCoord*), &rc); - - /* allocate space for arrays now that sizes are known */ - q = (double *)simm_malloc(nq * sizeof(double)); - saved_q = (double *)simm_malloc(nq * sizeof(double)); - resid = (double *)simm_malloc(nres * sizeof(double)); - - /* Copy gencoord values into q array, and save the original gencoord values. */ - for (i = 0; i < nq; i++) - q[i] = saved_q[i] = solveInfo->gencoord_list[i]->value; - - solveInfo->numLoopQs = numLoopQs; - solveInfo->numLoopResids = numLoopResids; - solveInfo->numConstraintQs = numConstraintQs; - solveInfo->numConstraintResids = numConstraintResids; - - nres = numLoopResids + numConstraintResids + nq; - - /* solve for new qs using Levenberg-Marquard method of minimizing least squares. */ - if (!LeastSquaresSolver(solveInfo, nq, q, nres, resid)) - { -// printf("lss problem\n"); - } - evaluateLCSolution(solveInfo, nq, q, nres, resid); - updateModel(solveInfo->model, solveInfo, nq, q, saved_q, nres, resid, &changed); - - if (solveInfo->loopClosed == no) - *loopStatus = loopBroken; - if (solveInfo->constraintClosed == no) - *constraintStatus = constraintBroken; - if (changed == yes) - { - if (solveInfo->loopClosed == yes) - *loopStatus = loopChanged; - if (solveInfo->constraintClosed == yes) - *constraintStatus = constraintChanged; - } - if (solveInfo->largeGenCoordResids == yes) - { - *loopStatus = largeResidinLoop; - *constraintStatus = gcOutOfRange; - } - - if (solveInfo->loopClosed == yes && solveInfo->constraintClosed == yes) - validSolution = yes; - else - validSolution = no; - - - FREE_IFNOTNULL(solveInfo->loopInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->loopInfo->loopUsed); - FREE_IFNOTNULL(solveInfo->loopInfo); - FREE_IFNOTNULL(solveInfo->constraintInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->constraintInfo); - FREE_IFNOTNULL(solveInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo); - FREE_IFNOTNULL(q); - FREE_IFNOTNULL(saved_q); - FREE_IFNOTNULL(resid); - - return validSolution; -} - -/* SOLVE_ALL_LOOPS_AND_CONSTRAINTS: - * Solve all loops and constraints at once. If enforce_constraints = yes, - * solve constraints (if not, do not). Return the status of loops and constraints - */ -void evaluateLoopsAndConstraintsInCurrentConfiguration(ModelStruct *ms, LoopStatus *loopStatus, - ConstraintStatus *constraintStatus, - SBoolean enforce_constraints) -{ - int i, j, nq, nres, index; - int numLoopQs, numLoopResids, numConstraintQs, numConstraintResids; - double *q = NULL, *saved_q = NULL, *resid = NULL; - LCStruct *solveInfo = NULL; - ReturnCode rc; - SBoolean changed = no; - - if ((!loopsToSolve(ms)) && (!constraintsToSolve(ms))) - goto cleanup; - - solveInfo = (LCStruct *)simm_malloc(sizeof(LCStruct)); - solveInfo->model = ms; - solveInfo->first_iter = yes; - solveInfo->largeGenCoordResids = no; - solveInfo->loopClosed = yes; - solveInfo->constraintClosed = yes; - solveInfo->controlled_gc = NULL; - solveInfo->controlled_value = 0.0; - solveInfo->gencoord_list = NULL; - - /* create structure to hold info about loops being solved */ - solveInfo->loopInfo = (IKStruct *)simm_malloc(sizeof(IKStruct)); - solveInfo->loopInfo->ms = ms; - solveInfo->loopInfo->loopUsed = (SBoolean *)simm_malloc(ms->numclosedloops * sizeof(SBoolean)); - solveInfo->loopInfo->gencoord_list = NULL; - solveInfo->loopInfo->controlled_gc = NULL; - solveInfo->loopInfo->controlled_value = 0.0; - solveInfo->loopInfo->first_iter = yes; - - /* create structure to hold info about constraints being solved */ - solveInfo->constraintInfo = (ConstraintSolverStruct *)simm_malloc(sizeof(ConstraintSolverStruct)); - solveInfo->constraintInfo->model = ms; - solveInfo->constraintInfo->controlled_gc = NULL; - solveInfo->constraintInfo->controlled_value = 0.0; - solveInfo->constraintInfo->gencoord_list = NULL; - solveInfo->constraintInfo->first_iter = yes; - solveInfo->constraintInfo->test = no; -// solveInfo->constraintInfo->tolerance = ms->constraint_tolerance; - solveInfo->constraintInfo->consUsed = (SBoolean *)simm_malloc(ms->num_constraint_objects * sizeof(SBoolean)); - - nq = 0; - nres = 0; - numLoopQs = 0; - numLoopResids = 0; - numConstraintQs = 0; - numConstraintResids = 0; - - /* Calculate the total number of qs and residuals in loops. */ - for (i = 0; i < ms->numclosedloops; i++) - { - numLoopQs += ms->loop[i].num_qs; - numLoopResids += ms->loop[i].num_resids; - solveInfo->loopInfo->loopUsed[i] = yes; - } - - /* Calculate the total number of qs and residuals in constraints. */ - for (i = 0; i < ms->num_constraint_objects; i++) - { - solveInfo->constraintInfo->consUsed[i] = no; - if ((enforce_constraints == yes) && (ms->constraintobj[i].active == yes) - && (ms->constraintobj[i].numPoints > 0)) - { - numConstraintQs += ms->constraintobj[i].num_qs; - numConstraintResids += ms->constraintobj[i].numPoints; - solveInfo->constraintInfo->consUsed[i] = yes; - } - } - - if ((numLoopQs == 0) && (numConstraintQs == 0)) - { - goto cleanup; - } - - /* allocate space for list of gencoords in loops and constraints. */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numLoopQs * sizeof(GeneralizedCoord*)); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(numConstraintQs * sizeof(GeneralizedCoord*)); - - /* Make a list of all loop gencoords used. */ - index = 0; - for (i = 0; i < ms->numclosedloops; i++) - { - if (solveInfo->loopInfo->loopUsed[i] == yes) - { - for (j = 0; j < ms->loop[i].num_qs; j++) - { - solveInfo->loopInfo->gencoord_list[index++] = ms->loop[i].qs[j]; - } - } - } - /* Make a list of all constraint gencoords used. */ - index = 0; - for (i = 0; i < ms->num_constraint_objects; i++) - { - if (solveInfo->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < ms->constraintobj[i].num_qs; j++) - { - solveInfo->constraintInfo->gencoord_list[index++] = ms->constraintobj[i].qs[j]; - } - } - } - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords are not included in the list. - * Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &numLoopQs, solveInfo->loopInfo->gencoord_list, NULL); - orderQs(ms, &numConstraintQs, solveInfo->constraintInfo->gencoord_list, NULL); - - nq = numLoopQs + numConstraintQs; - - /* copy both gencoord lists into single list */ - solveInfo->gencoord_list = (GeneralizedCoord**)simm_malloc(nq * sizeof(GeneralizedCoord*)); - for (i = 0; i < numLoopQs; i++) - solveInfo->gencoord_list[i] = solveInfo->loopInfo->gencoord_list[i]; - for (i = numLoopQs; i < nq; i++) - solveInfo->gencoord_list[i] = solveInfo->constraintInfo->gencoord_list[i-numLoopQs]; - - /* Rearrange the list of gencoords so each gencoord appears only once. - * Locked gencoords are not included in the list. - * Update the number of Qs and residuals (add one residual for each Q). - */ - orderQs(ms, &nq, solveInfo->gencoord_list, NULL); - solveInfo->numQs = nq; -//if nq or nres == 0??? - nres = numLoopResids + numConstraintResids + nq; - - /* reallocate space for arrays given new sizes */ - solveInfo->loopInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->loopInfo->gencoord_list, - numLoopQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->constraintInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->constraintInfo->gencoord_list, - numConstraintQs * sizeof(GeneralizedCoord*), &rc); - solveInfo->gencoord_list = (GeneralizedCoord**)simm_realloc(solveInfo->gencoord_list, nq * sizeof(GeneralizedCoord*), &rc); - - /* allocate space for arrays now that sizes are known */ - q = (double *)simm_malloc(nq * sizeof(double)); - saved_q = (double *)simm_malloc(nq * sizeof(double)); - resid = (double *)simm_malloc(nres * sizeof(double)); - - /* Copy gencoord values into q array, and save the original gencoord values. */ - for (i = 0; i < nq; i++) - q[i] = saved_q[i] = solveInfo->gencoord_list[i]->value; - - solveInfo->numLoopQs = numLoopQs; - solveInfo->numLoopResids = numLoopResids; - solveInfo->numConstraintQs = numConstraintQs; - solveInfo->numConstraintResids = numConstraintResids; - - calculateLCResids(nres, nq, q, resid, &i, solveInfo); - evaluateLCSolution(solveInfo, nq, q, nres, resid); - -cleanup: - *loopStatus = loopUnchanged; - *constraintStatus = constraintUnchanged; - if (solveInfo) - { - if (solveInfo->loopClosed == no) - *loopStatus = loopBroken; - if (solveInfo->constraintClosed == no) - *constraintStatus = constraintBroken; - if (changed == yes) - { - *loopStatus = loopChanged; - *constraintStatus = constraintChanged; - } - if (solveInfo->largeGenCoordResids == yes) - { - *loopStatus = largeResidinLoop; - *constraintStatus = gcOutOfRange; - } - - ms->loopsOK = solveInfo->loopClosed; - ms->constraintsOK = solveInfo->constraintClosed; - - FREE_IFNOTNULL(solveInfo->loopInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->loopInfo->loopUsed); - FREE_IFNOTNULL(solveInfo->loopInfo); - FREE_IFNOTNULL(solveInfo->constraintInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo->constraintInfo->consUsed); - FREE_IFNOTNULL(solveInfo->constraintInfo); - FREE_IFNOTNULL(solveInfo->gencoord_list); - FREE_IFNOTNULL(solveInfo); - } - FREE_IFNOTNULL(q); - FREE_IFNOTNULL(saved_q); - FREE_IFNOTNULL(resid); -} - - -/* Set up the required work arrays and variables and call lmdif - * to solve the least squares problem using the Levenberg-Marquart theory. - * q: solution vector (initially contains estimate of soln in internal units) (x) - * fvec: functions evaluated at output q (final residuals) - * num_resid: number of functions (m) (nres) - * nq: number of variables (n) (ndofinp) - */ -static int LeastSquaresSolver(LCStruct *solveInfo, int numQ, double q[], int numResid, - double resid[]) -{ - int info, callsToCalcResid, ldfjac = numResid; - - /* solution parameters */ - int mode = 1, nprint = 0, max_iter = 500; - double ftol = 1e-4, xtol = 1e-4, gtol = 0.0; - double epsfcn = 0.0, step_factor = 0.2; - - /* work arrays */ - int *ipvt; - double *diag, *qtf, *wa1, *wa2, *wa3, *wa4, *fjac; - - /* allocate space for necessary arrays */ - ipvt = (int *)simm_malloc(numQ * sizeof(int)); - diag = (double *)simm_malloc(numQ * sizeof(double)); - fjac = (double *)simm_malloc(numQ * numResid * sizeof(double)); - qtf = (double *)simm_malloc(numQ * sizeof(double)); /* wa[ndofinp + 1] */ - wa1 = (double *)simm_malloc(numQ * sizeof(double)); /* wa[2 * ndofinp + 1] */ - wa2 = (double *)simm_malloc(numQ * sizeof(double)); /* wa[3 * ndofinp + 1] */ - wa3 = (double *)simm_malloc(numQ * sizeof(double)); /* wa[4 * ndofinp + 1] */ - wa4 = (double *)simm_malloc(numResid * sizeof(double)); /* wa[5 * ndofinp + 1]*/ - -// ftol = xtol = solveInfo->solver_accuracy; -// max_iter = solveInfo->maxIter; - /* if the number of qs is greater than the number of residuals, - * LMDIF will not be able to find a solution and will return an error message - */ - if (numResid < numQ) - { - (void)sprintf(errorbuffer, - "Least Squares Error: num residuals (%d) < num q (%d)\n", numResid, numQ); - error(none, errorbuffer); - } - - lmdif_C(calculateLCResids, numResid, numQ, q, resid, - ftol, xtol, gtol, max_iter, epsfcn, diag, mode, step_factor, - nprint, &info, &callsToCalcResid, fjac, ldfjac, ipvt, qtf, - wa1, wa2, wa3, wa4, solveInfo); - -/* if (info == 0) - printf("improper input parameters\n"); - if (info == 1) - printf("actual and predicted relative reductions in ssq are at most ftol\n"); - if (info == 2) - printf("relative error between two consecutive iterates is at most xtol.\n"); - if (info == 3) - printf("conditions for info = 1 and info = 2 both hold.\n"); - if (info == 4) - printf("the cosine of the angle between fvec and any column of the jacobian is at most gtol in absolute value.\n"); - if (info == 5) - printf("max iter exceeded\n"); - if (info == 6) - printf("ftol is too small. no further reduction in the sum of squares is possible.\n"); - if (info == 7) - printf("xtol is too small. no further improvement in the approximate solution x is possible.\n"); - if (info == 8) - printf("gtol is too small. fvec is orthogonal to the columns of the jacobian to machine precision.\n"); -*/ - FREE_IFNOTNULL(ipvt); - FREE_IFNOTNULL(diag); - FREE_IFNOTNULL(fjac); - FREE_IFNOTNULL(qtf); - FREE_IFNOTNULL(wa1); - FREE_IFNOTNULL(wa2); - FREE_IFNOTNULL(wa3); - FREE_IFNOTNULL(wa4); - - if (info == 0 || info == 5 || info== 6 || info == 7 || info == 8) - return no; - return yes; -} - -/* calculate gencoord, loop and constraint residuals */ -void calculateLCResids(int numResid, int numQ, double q[], double residuals[], - int *iflag, void *data) -{ - int startIndex, endIndex; - double weight = 1.0; - LCStruct *solveInfo = (LCStruct *)data; - - /* initialize residuals array */ - clear_vector(residuals, numResid); - - startIndex = 0; - endIndex = numQ; - calculateGenCoordResiduals(solveInfo, numQ, q, numResid, residuals, startIndex, endIndex, &weight, iflag); - startIndex = endIndex; - endIndex = startIndex + solveInfo->numLoopResids; - calculateLoopResiduals(solveInfo, numQ, q, numResid, residuals, startIndex, endIndex, &weight, iflag); - startIndex = endIndex; - endIndex = startIndex + solveInfo->numConstraintResids; - calculateConstraintResids(solveInfo, numQ, q, numResid, residuals, startIndex, endIndex, iflag); - - solveInfo->first_iter = no; -} - -/* Determine whether loops and constraints are closed, and whether any - * gencoords are out of range (have large residuals) */ -static void evaluateLCSolution(LCStruct *info, int nq, double q[], int nres, double resid[]) -{ - int i, j, index; - double rms_total; - double *loop_resid = NULL, *cons_resid = NULL, *q_resid = NULL; - SBoolean good; - - loop_resid = (double *)simm_malloc(info->numLoopResids * sizeof(double)); - cons_resid = (double *)simm_malloc(info->numConstraintResids * sizeof(double)); - q_resid = (double *)simm_malloc(nq * sizeof(double)); - - for (i = 0; i < nq; i++) - q_resid[i] = resid[i]; - - for (i = nq, j = 0; i < nq + info->numLoopResids; i++) - loop_resid[j++] = resid[i]; - - for (i = nq + info->numLoopResids, j = 0; i < nres; i++) - cons_resid[j++] = resid[i]; - - /* evaluate the loop solution */ - /* calculate the total rms from the 'joint' residuals (not the Q residuals). - * If the total rms is above the threshold, the solution is not valid because - * the loop will break apart. - */ - info->loopClosed = yes; - rms_total = calculateRMS(info->numLoopResids, loop_resid); - if (rms_total > info->loopInfo->ms->loop_tolerance) - info->loopClosed = no; - - /* evaluate the constraint solution */ - index = 0; - good = yes; - for (i = 0; i < info->model->num_constraint_objects; i++) - { - if (info->model->constraintobj[i].active == no) - continue; - if (info->constraintInfo->consUsed[i] == no) - continue; - - for (j = 0; j < info->model->constraintobj[i].numPoints; j++) - { - info->model->constraintobj[i].points[j].broken = no; - if (fabs(cons_resid[index++]) > info->model->constraintobj[i].points[j].tolerance) - { - info->model->constraintobj[i].points[j].broken = yes; - good = no; - } - } - } - - info->constraintClosed = yes; - if (!good) - info->constraintClosed = no; - - info->largeGenCoordResids = no; - /* if any of the gencoord residuals are too high, the gencoord is out of range */ - for (i = 0; i < nq; i++) - if (resid[i] >= GENCOORD_TOLERANCE) - info->largeGenCoordResids = yes; - - FREE_IFNOTNULL(loop_resid); - FREE_IFNOTNULL(cons_resid); - FREE_IFNOTNULL(q_resid); - -} - -static void updateModel(ModelStruct *ms, LCStruct *solveInfo, int nq, double q[], - double saved_q[], int nres, double resid[], SBoolean *changed) -{ - int i; - GeneralizedCoord* gc; - - /* determine whether any gencoords have changed */ - *changed = no; - for (i = 0; i < nq; i++) - { - gc = solveInfo->gencoord_list[i]; - if (DABS(saved_q[i] - q[i]) > GENCOORD_TOLERANCE) // ms->gencoord[gc]->tolerance) - { - *changed = yes; - break; - } - } - - /* if there are no changes, set the gencoords back to their original values */ - if (!(*changed)) - { - for (i = 0; i < nq; i++) - { - gc = solveInfo->gencoord_list[i]; - set_gencoord_value(ms, gc, saved_q[i], no); - } - return; - } - - /* update the values in the model viewer. Reset the original values so - * the changes appear in the model viewer. */ - if ((solveInfo->loopClosed == no) || (solveInfo->constraintClosed == no)) - { - /* If no solution was found, reset the gencoords to their original values. */ - for (i = 0; i < nq; i++) - { - q[i] = saved_q[i]; - *changed = no; - } - } - else - { - for (i = 0; i < nq; i++) - { - solveInfo->gencoord_list[i]->value = saved_q[i]; - } - } - /* set gencoords to new values. Set the flag so loops and constraints are - * not solved to prevent recursion. */ - for (i = 0; i < nq; i++) - { - gc = solveInfo->gencoord_list[i]; - set_gencoord_value(ms, gc, q[i], no); - } -} - -#if ! ENGINE -static void displayLCMessages(ModelStruct *ms, LoopStatus loopStatus, ConstraintStatus constraintStatus) -{ - int i, ans; - char gc_info[255]; - - if ((loopStatus == loopChanged) || (constraintStatus == constraintChanged)) - { - approveNewDefaultGCs(ms); - } - else if ((loopStatus == loopBroken) || (constraintStatus == constraintBroken)) - { - glutMessageBox(GLUT_OK, 1, GLUT_ICON_INFORMATION, - "LOAD MODEL: Constraints and/or Loops not satisfied (closed)", badLoopErrorMsg); - ms->defaultGCApproved = no; - ms->defaultConstraintsOK = no; - ms->constraintsOK = no; - } - else if ((loopStatus == largeResidinLoop) || (constraintStatus == gcOutOfRange)) - { - glutMessageBox(GLUT_OK, 1, GLUT_ICON_INFORMATION, - "LOAD MODEL: Gencoord Out of Range", gencoordResidualErrorMsg); - ms->GEFuncOK = yes; - - /* store new values as defaults (even though loops not closed) */ - for (i = 0; i < ms->numgencoords; i++) - ms->gencoord[i]->default_value = ms->gencoord[i]->value; - ms->defaultGCApproved = yes; - ms->defaultLoopsOK = no; - ms->constraintsOK = no; - sprintf(buffer, "Load Model: Default values do not close loops.\n"); - error(none, buffer); - } -} - -#endif - -/* mark all loops that are affected by a change in the controlled q */ -static void markAffectedLoopsAndConstraints(LCStruct *info, GeneralizedCoord* gencoord) -{ - int i, j, k, numLoopsUsed = 0, numConsUsed = 0; - SBoolean changed; - - for (i = 0; i < info->model->numclosedloops; i++) - info->loopInfo->loopUsed[i] = no; - - for (i = 0; i < info->model->num_constraint_objects; i++) - info->constraintInfo->consUsed[i] = no; - - /* find all loops that use the 'locked' gencoord */ - for (i = 0; i < info->model->numclosedloops; i++) - { - if ((GCUsedInLoop(gencoord, info->model->loop[i]) == yes) && (info->model->useIK == yes)) - { - info->loopInfo->loopUsed[i] = yes; - numLoopsUsed++; - } - } - - /* find all constraints that use the 'locked' gencoord */ - for (i = 0; i < info->model->num_constraint_objects; i++) - { - if (GCUsedInConstraint(gencoord, info->model->constraintobj[i]) == yes) - { - info->constraintInfo->consUsed[i] = yes; - numConsUsed++; - } - } - if ((numLoopsUsed == info->model->numclosedloops) && (numConsUsed == info->model->num_constraint_objects)) - return; - - /* find all loops that are affected by a change in the 'locked' q - * A change in the locked q will cause changes in all the qs in that - * loop. These qs may be part of other loops which will in turn be - * affected. The q's in these affected loops may in turn affect - * other loops. - * Go through all qs in all loops until all affected loops have been - * found. Check all gencoords in each used loop. to see if they belong - * to other loops. If they are, mark those other loops if they haven't - * been marked already. Keep doing this until no changes are made. - */ - while (1) - { - changed = no; - /* see if gencoords are used in any other loops */ - for (i = 0; i < info->model->numclosedloops; i++) - { - if ((info->loopInfo->loopUsed[i] == yes) && (info->model->useIK == yes)) - { - for (j = 0; j < info->model->loop[i].num_qs; j++) - { - for (k = 0; k < info->model->numclosedloops; k++) - { - if (GCUsedInLoop(info->model->loop[i].qs[j], info->model->loop[k]) == yes) - { - if (info->loopInfo->loopUsed[k] == no) - { - info->loopInfo->loopUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - for (i = 0; i < info->model->numclosedloops; i++) - { - if ((info->loopInfo->loopUsed[i] == yes) && (info->model->useIK == yes)) - { - for (j = 0; j < info->model->loop[i].num_qs; j++) - { - for (k = 0; k < info->model->num_constraint_objects; k++) - { - if (GCUsedInConstraint(info->model->loop[i].qs[j], info->model->constraintobj[k]) == yes) - { - if (info->constraintInfo->consUsed[k] == no) - { - info->constraintInfo->consUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - - - /* see if gencoords are used in any other constraints */ - for (i = 0; i < info->model->num_constraint_objects; i++) - { - if (info->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < info->model->constraintobj[i].num_qs; j++) - { - for (k = 0; k < info->model->num_constraint_objects; k++) - { - if (GCUsedInConstraint(info->model->constraintobj[i].qs[j], info->model->constraintobj[k]) == yes) - { - if (info->constraintInfo->consUsed[k] == no) - { - info->constraintInfo->consUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - /* see if gencoords are used in any other loops */ - for (i = 0; i < info->model->num_constraint_objects; i++) - { - if (info->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < info->model->constraintobj[i].num_qs; j++) - { - for (k = 0; k < info->model->numclosedloops; k++) - { - if (GCUsedInLoop(info->model->constraintobj[i].qs[j], info->model->loop[k]) == yes) - { - if (info->model->useIK == yes) - { - if (info->loopInfo->loopUsed[k] == no) - { - info->loopInfo->loopUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - } - if (changed == no) - break; - } -} - -/* mark all loops that are affected by a change in the controlled q */ -static void markLoopsAndConstraintsAffected(LCStruct *info, int jnt) -{ - int i, j, k, numLoopsUsed = 0, numConsUsed = 0, jnt_index; - SBoolean changed; - - for (i = 0; i < info->model->numclosedloops; i++) - info->loopInfo->loopUsed[i] = no; - - for (i = 0; i < info->model->num_constraint_objects; i++) - info->constraintInfo->consUsed[i] = no; - - /* find all loops that use the 'locked' gencoord */ - for (i = 0; i < info->model->numclosedloops; i++) - { - if ((JNTUsedInLoop(jnt, info->model->loop[i])) && (info->model->useIK == yes)) - { - info->loopInfo->loopUsed[i] = yes; - numLoopsUsed++; - } - } - /* find all constraints that use the 'locked' gencoord */ - for (i = 0; i < info->model->num_constraint_objects; i++) - { - if (JNTUsedInConstraint(jnt, info->model->constraintobj[i])) - { - info->constraintInfo->consUsed[i] = yes; - numConsUsed++; - } - } - if ((numLoopsUsed == info->model->numclosedloops) && (numConsUsed == info->model->num_constraint_objects)) - return; - - /* find all loops that are affected by a change in the 'locked' q - * A change in the locked q will cause changes in all the qs in that - * loop. These qs may be part of other loops which will in turn be - * affected. The q's in these affected loops may in turn affect - * other loops. - * Go through all qs in all loops until all affected loops have been - * found. Check all gencoords in each used loop. to see if they belong - * to other loops. If they are, mark those other loops if they haven't - * been marked already. Keep doing this until no changes are made. - */ - while (1) - { - changed = no; - /* see if gencoords are used in any other loops */ - for (i = 0; i < info->model->numclosedloops; i++) - { - if ((info->loopInfo->loopUsed[i] == yes) && (info->model->useIK == yes)) - { - for (j = 0; j < info->model->loop[i].num_jnts; j++) - { - for (k = 0; k < info->model->numclosedloops; k++) - { - jnt_index = fabs(info->model->loop[i].joints[j]) - 1; - if (JNTUsedInLoop(jnt_index, info->model->loop[k]) == yes) - { - if (info->loopInfo->loopUsed[k] == no) - { - info->loopInfo->loopUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - for (i = 0; i < info->model->numclosedloops; i++) - { - if ((info->loopInfo->loopUsed[i] == yes) && (info->model->useIK == yes)) - { - for (j = 0; j < info->model->loop[i].num_jnts; j++) - { - jnt_index = fabs(info->model->loop[i].joints[j]) - 1; - for (k = 0; k < info->model->num_constraint_objects; k++) - { - if (JNTUsedInConstraint(jnt_index, info->model->constraintobj[k]) == yes) - { - if (info->constraintInfo->consUsed[k] == no) - { - info->constraintInfo->consUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - - - /* see if gencoords are used in any other constraints */ - for (i = 0; i < info->model->num_constraint_objects; i++) - { - if (info->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < info->model->constraintobj[i].num_jnts; j++) - { - for (k = 0; k < info->model->num_constraint_objects; k++) - { - if (JNTUsedInConstraint(info->model->constraintobj[i].joints[j], info->model->constraintobj[k]) == yes) - { - if (info->constraintInfo->consUsed[k] == no) - { - info->constraintInfo->consUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - /* see if gencoords are used in any other loops */ - for (i = 0; i < info->model->num_constraint_objects; i++) - { - if (info->constraintInfo->consUsed[i] == yes) - { - for (j = 0; j < info->model->constraintobj[i].num_jnts; j++) - { - for (k = 0; k < info->model->numclosedloops; k++) - { - if (JNTUsedInLoop(info->model->constraintobj[i].joints[j], info->model->loop[k]) == yes) - { - if ((info->loopInfo->loopUsed[k] == no) && (info->model->useIK == yes)) - { - info->loopInfo->loopUsed[k] = yes; - changed = yes; - } - } - } - } - } - } - if (changed == no) - break; - } -} - - -/* CALCULATEGENCOORDRESIDUALS: - * Calculate the residuals for all the gencoords solved (in the q array). - * Set the gencoords to the values in the q array and invalidate the joint - * matrices if the values have changed. - * If a gencoord is clamped and out of range, increase the weight, and set - * the residual. - */ -static void calculateGenCoordResiduals(void *data, int numQ, double q[], int numResid, - double resid[], int startIndex, int endIndex, - double *weight, int *iflag) -{ - int i, index, gc_error; - double rangeError, toMax, toMin, rangeStart, rangeEnd; - LCStruct *solveInfo = (LCStruct *)data; - GeneralizedCoord* gc = NULL; - - *weight = 1.0; - index = startIndex; -//if (!JACOBIAN) printf("GC resids\n"); - - /* Update the gencoord records in the model structure to the new values stored in - * the q array (using setGencoordValue2). Set the values and invalidate the transform matrices so that these - * will be recalculated when necessary. If a clamped gencoord is out of range, - * increase the weight so the loop residuals will be increased. - */ - for (i = 0; i < numQ; i++) - { - if (index > endIndex) - { - *iflag = 10; - break; - } - gc = solveInfo->gencoord_list[i]; - gc_error = setGencoordValue2(solveInfo->model, gc, q[i]); - rangeStart = gc->range.start; - rangeEnd = gc->range.end; - toMax = rangeEnd - q[i]; - toMin = q[i] - rangeStart; - - if (gc->clamped == yes) - { - if (gc->type == rotation_gencoord) - { - if (q[i] > rangeEnd) - { - rangeError = q[i] - rangeEnd + CLAMPING_TOLERANCE; - } - else if (q[i] < rangeStart) - { - rangeError = rangeStart - q[i] + CLAMPING_TOLERANCE; - } - else - { - if ((toMax > CLAMPING_TOLERANCE) && (fabs(toMin) > CLAMPING_TOLERANCE)) - rangeError = 0.0; - else if (toMax < toMin) - rangeError = CLAMPING_TOLERANCE - toMax; - else - rangeError = -1 * (CLAMPING_TOLERANCE - toMin); - } - rangeError *= 0.01; - *weight += rangeError/100.0; - resid[index] = SQR(rangeError)/1000.0; - } - else - { - if (q[i] > rangeEnd) - { - rangeError = q[i] - rangeEnd + CLAMPING_TOL_TRANS; - } - else if (q[i] < rangeStart) - { - rangeError = rangeStart - q[i] + CLAMPING_TOL_TRANS; - } - else - { - if ((toMax >= CLAMPING_TOL_TRANS) && - (fabs(toMin) >= CLAMPING_TOL_TRANS)) - rangeError = 0.0; - else if (toMax < toMin) - rangeError = CLAMPING_TOL_TRANS - toMax; - else - rangeError = -1 * (CLAMPING_TOL_TRANS - toMin); - } - *weight += rangeError; - resid[index] = rangeError; - } - } - else - { - /* if the gencoord is unclamped and has an active function, - * use the restraint function to calculate the weight and - * residual */ - if (gc->restraint_function && gc->restraintFuncActive == yes) - { - rangeError = fabs(interpolate_function(q[i], gc->restraint_function, zeroth, 0.0, 0.0)); - rangeError *= 0.1; - *weight += rangeError / 100.0; - resid[index] = rangeError / 1000.0; - } - else - { -#if 0 // JPL 01/23/06: if gc is unclamped and no restraint function, residual should be 0.0, right? - /* original method */ - if (q[i] < rangeStart) - rangeError = (rangeStart - q[i]); - else if (q[i] > rangeEnd) - rangeError = (q[i] - rangeEnd); - else - rangeError = 0.0; - resid[index] = rangeError; -#endif - resid[index] = 0.0; - } //added dkb apr 2003 - } -//if (!JACOBIAN) printf("resid[%d] (gc%s) = %f\n", index, gc->name, resid[index]); - index++; - } - solveInfo->first_iter = no; -} - - -/* CALCULATELOOPRESIDUALS: - * Calculate the residuals for the loops solved. Assume the gencoords have been - * set to the new values and the joint matrices have been invalidated. - * The loop residuals are calculated by breaking each joint in the loop and - * determining the coordinates of the point in the ground frame using two paths - * from the "first" segment to the joint. The coordinates of the points are - * then subtracted and scaled by the given weight to find the residual. - */ -static void calculateLoopResiduals(void *data, int numQ, double q[], int numResid, double resid[], - int startIndex, int endIndex, double *weight, int *iflag) -{ - int i, j, loop, seg, index; - int *pathA= NULL, *pathB = NULL, pathLengthA, pathLengthB; - double coordsA[3], coordsB[3]; - LCStruct *solveInfo = (LCStruct *)data; -//if (!JACOBIAN) printf("loop resid\n"); - index = startIndex; - for (loop = 0; loop < solveInfo->model->numclosedloops; loop++) - { - if (index > endIndex) - { -// printf("problem\n"); - break; - } - if (solveInfo->loopInfo->loopUsed[loop] == yes) - { - /* allocate paths */ - pathA = (int *)simm_malloc(solveInfo->model->loop[loop].num_jnts * sizeof(int)); - pathB = (int *)simm_malloc(solveInfo->model->loop[loop].num_jnts * sizeof(int)); - - /* calculate residuals - * Find residuals for each joint in the loop, as though that joint were the - * loop joint. For each joint, "break" the loop and determine forward and inverse - * paths from each segment to the "first" segment in the loop. Transform the coords - * of the two points to the "first" segment frame then to the ground frame. - * The residual is the difference in the coordinates multiplied by the weight factor. - */ - for (seg = 1; seg < solveInfo->model->loop[loop].num_segs - 1; seg++) - { - /* initially, coordinates in each frame are 0.0 0.0 0.0 */ - clear_vector(coordsA, 3); - clear_vector(coordsB, 3); - - /* create paths going both ways from the segment to the "first" segment */ - pathLengthA = getLoopPath(seg, pathA, INVERSE, &solveInfo->model->loop[loop]); - pathLengthB = getLoopPath(seg, pathB, FORWARD, &solveInfo->model->loop[loop]); - - /* convert point to "first" segment in loop via INVERSE path, then to ground */ - convertNEW(solveInfo->model, coordsA, pathA, pathLengthA); - convert(solveInfo->model, coordsA, solveInfo->model->loop[loop].segs[0], - solveInfo->model->ground_segment); - - /* convert point to "first" segment in loop via FORWARD path, then to ground */ - convertNEW(solveInfo->model, coordsB, pathB, pathLengthB); - convert(solveInfo->model, coordsB, solveInfo->model->loop[loop].segs[0], - solveInfo->model->ground_segment); - - for (i = 0; i < 3; i++) - { - resid[index] = (coordsA[i] - coordsB[i]) * *weight; - index++; - } - } - FREE_IFNOTNULL(pathA); - FREE_IFNOTNULL(pathB); - } - } - solveInfo->first_iter = no; - -} - -/* Reorganize the gencoord list so that a gencoord index appears only once. Do not - * include the controlled gencoord or any locked gencoords in the list. Update the - * number of qs needed (number of gencoords in the new list) - */ -static void orderQs(ModelStruct *ms, int *nq, GeneralizedCoord* gc_list[], GeneralizedCoord* controlled_gc) -{ - SBoolean *gc_used = NULL; - int i; - - gc_used = (SBoolean *)simm_malloc(ms->numgencoords * sizeof(SBoolean)); - - for (i = 0; i < ms->numgencoords; i++) - gc_used[i] = no; - - /* mark which gencoords are used in the affected loops */ - for (i = 0; i < *nq; i++) - gc_used[getGencoordIndex(ms, gc_list[i])] = yes; - - if (controlled_gc) - gc_used[getGencoordIndex(ms, controlled_gc)] = no; - - for (i = 0; i < ms->numgencoords; i++) - if (ms->gencoord[i]->locked == yes) - gc_used[i] = no; - - /* create a new q array with each gencoord appearing only once, and count the number - * of qs used. */ - *nq = 0; - for (i = 0; i < ms->numgencoords; i++) - { - if (gc_used[i] == yes) - gc_list[(*nq)++] = ms->gencoord[i]; - } - - FREE_IFNOTNULL(gc_used); -} - -/* calculate RMS values for loop joint residuals as well as total rms */ -static double calculateRMS(int n, double fvec[]) -{ - int i, j; - double sum, *rms = NULL, temp, rms_all; - - sum = 0.0; - rms = (double *)simm_malloc(n * sizeof(double)); - for (i = 0; i < n/3; i++) - { - temp = 0.0; - for (j = 0; j < 3; j++) - temp += SQR(fvec[3*i + j]); - rms[i] = sqrt(temp/3); - sum += temp; - } - rms_all = sqrt(sum/n); - - FREE_IFNOTNULL(rms); - return rms_all; -} - - -/* determine whether the given gencoord is used in the given constraint object */ -static SBoolean GCUsedInConstraint(GeneralizedCoord* gencoord, ConstraintObject co) -{ - int i; - - for (i = 0; i < co.num_qs; i++) - { - if (gencoord == co.qs[i]) - return yes; - } - return no; -} - -/* determine whether the given joint is used in the given constraint object */ -static SBoolean JNTUsedInConstraint(int jnt, ConstraintObject co) -{ - int i; - - for (i = 0; i < co.num_jnts; i++) - { - if (jnt == co.joints[i]) - return yes; - } - return no; -} - -static SBoolean GCUsedInLoop(GeneralizedCoord* gencoord, LoopStruct loop) -{ - int i; - - for (i = 0; i < loop.num_qs; i++) - { - if (gencoord == loop.qs[i]) - return yes; - } - return no; -} - -static SBoolean JNTUsedInLoop(int jnt, LoopStruct loop) -{ - int i; - - for (i = 0; i < loop.num_jnts; i++) - { - if (jnt == fabs(loop.joints[i]) - 1) - return(yes); - } - return (no); -} - - -SBoolean loopsToSolve(ModelStruct *model) -{ - if ((model->numclosedloops > 0) && (model->useIK == yes)) - return yes; - return no; -} - -SBoolean constraintsToSolve(ModelStruct *model) -{ - int i; - - for (i = 0; i < model->num_constraint_objects; i++) - if ((model->constraintobj[i].active == yes) && (model->constraintobj[i].numPoints > 0)) - return yes; - return no; -} - -void approveNewDefaultGCs(ModelStruct *ms) -{ - int i, ans; - char gc_info[255]; - -#if ! ENGINE - /* in all cases, defaults have been approved (whether or not they're accepted) */ - ms->defaultGCApproved = yes; - sprintf(buffer, - "The set of default values for the gencoords did not\n" - "satisfy the constraints and/or loops. SIMM calculated\n" - "a new solution closing all loops/satisfying all constraints.\n" - "Do you want to accept the new default values?\n\n"); - for (i = 0; i < ms->numgencoords; i++) - { - if ((ms->gencoord[i]->used_in_model == yes) - && (NOT_EQUAL_WITHIN_ERROR(ms->gencoord[i]->value, - ms->gencoord[i]->default_value))) - { - sprintf(gc_info, " %-20s: % 10.5f -> % 10.5f\n", - ms->gencoord[i]->name, ms->gencoord[i]->default_value, - ms->gencoord[i]->value); - strcat(buffer, gc_info); - } - } - strcat(buffer, "\nClick \"YES\" to display and save new values.\n\n"); - strcat(buffer, "Click \"NO\" to display new values without saving\n" - "them as default values\n\n"); - strcat(buffer, "Click \"Cancel\" to ignore new values."); - - ans = glutMessageBox(GLUT_YES_NO_CANCEL, 1, GLUT_ICON_INFORMATION, - "LOAD MODEL: Default Configuration", buffer); - if (ans == GLUT_MSG_YES) - { - /* accept new default values and display them*/ - for (i = 0; i < ms->numgencoords; i++) - { - ms->gencoord[i]->default_value = ms->gencoord[i]->value; - ms->gencoord[i]->default_value_specified = yes; - } - ms->defaultLoopsOK = yes; - ms->defaultConstraintsOK = yes; - sprintf(buffer, "New default gencoord values saved for %s.\n", ms->name); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); - } - else if (ans == GLUT_MSG_CANCEL) - { - LoopStatus loopStatus; - ConstraintStatus constraintStatus; - - /* keep and display original default values. Set values without solving - * loops and constraints */ - //evaluate current situation to set values - ms->defaultLoopsOK = no; - ms->defaultConstraintsOK = no; - ms->loopsOK = no; - ms->constraintsOK = no; - for (i = 0; i < ms->numgencoords; i++) - set_gencoord_value(ms, ms->gencoord[i], ms->gencoord[i]->default_value, no); - evaluateLoopsAndConstraintsInCurrentConfiguration(ms, &loopStatus, &constraintStatus, - yes); - if (!ms->defaultLoopsOK || !ms->defaultConstraintsOK) - { - sprintf(buffer, "Default values may not close loops or satisfy constraints.\n"); - error(none, buffer); - } - } - else - { - /* display new values, but do not change defaults */ - ms->defaultLoopsOK = no; - ms->defaultConstraintsOK = no; - sprintf(buffer, "Default values may not close loops or satisfy constraints.\n"); - error(none, buffer); - } -#else - for (i = 0; i < ms->numgencoords; i++) - { - ms->gencoord[i]->default_value = ms->gencoord[i]->value; - ms->gencoord[i]->default_value_specified = yes; - } - ms->defaultLoopsOK = yes; - ms->defaultConstraintsOK = yes; -#endif -} diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/CMakeLists.txt b/OpenSim/Utilities/simmToOpenSim/acpp/CMakeLists.txt deleted file mode 100644 index 46b5a71c58..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -#----------------------------------------------------------------------------- -subdirs(src) diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/CMakeLists.txt b/OpenSim/Utilities/simmToOpenSim/acpp/src/CMakeLists.txt deleted file mode 100644 index 8360299b8e..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -set(KIT acpp) - -if(WIN32) -file(GLOB SOURCE_FILES alloca.c cccp.c obstack.c version.c y.tab.c) -else(WIN32) -file(GLOB SOURCE_FILES cccp.c obstack.c version.c y_linux.tab.c) -endif(WIN32) - -file(GLOB INCLUDE_FILES *.h) - -add_library(acpp ${SOURCE_FILES} ${INCLUDE_FILES}) - -set_target_properties(acpp PROPERTIES PROJECT_LABEL "Vendor Libraries - acpp") - -if(WIN32) - install_targets(/sdk/lib/ acpp) -else(WIN32) - install_targets(/bin/ acpp) -endif(WIN32) diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/Makefile.n32 b/OpenSim/Utilities/simmToOpenSim/acpp/src/Makefile.n32 deleted file mode 100644 index 7dfc7c5991..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/Makefile.n32 +++ /dev/null @@ -1,12 +0,0 @@ -COPTS = -n32 -all: - cc -c $(COPTS) cccp.c -o cccp.o - cc -c $(COPTS) alloca.c -o alloca.o - yacc cexp.y - cc -c $(COPTS) y.tab.c -o y.tab.o - cc -c $(COPTS) obstack.c -o obstack.o - cc -c $(COPTS) version.c -o version.o - ar q libacppn32.a *.o - -clean: - rm libacppn32.a *.o diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/README.txt b/OpenSim/Utilities/simmToOpenSim/acpp/src/README.txt deleted file mode 100644 index f3abe98310..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/README.txt +++ /dev/null @@ -1,14 +0,0 @@ -README.txt - -This directory contains a version of the GNU C-preprocessor modified by -MusculoGraphics, Inc. for use with SIMM, SIMM Gait, and the Dynamics Pipeline. - -Distribution of the code in this directory is governed by the GNU General -Public License (http://www.gnu.org/copyleft/gpl.html). - -Up-to-date versions of the MusculoGraphics-modified version of GNU acpp can be -downloaded at the following web addresses: - - http://www.musculographics.com/acpp.zip (PC zip archive) - - http://www.musculographics.com/acpp.tar.gz (Unix tar/gzip archive) diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/alloca.c b/OpenSim/Utilities/simmToOpenSim/acpp/src/alloca.c deleted file mode 100644 index ffdd61a01c..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/alloca.c +++ /dev/null @@ -1,193 +0,0 @@ -/* - alloca -- (mostly) portable public-domain implementation -- D A Gwyn - - last edit: 86/05/30 rms - include config.h, since on VMS it renames some symbols. - Use xmalloc instead of malloc. - - This implementation of the PWB library alloca() function, - which is used to allocate space off the run-time stack so - that it is automatically reclaimed upon procedure exit, - was inspired by discussions with J. Q. Johnson of Cornell. - - It should work under any C implementation that uses an - actual procedure stack (as opposed to a linked list of - frames). There are some preprocessor constants that can - be defined when compiling for your specific system, for - improved efficiency; however, the defaults should be okay. - - The general concept of this implementation is to keep - track of all alloca()-allocated blocks, and reclaim any - that are found to be deeper in the stack than the current - invocation. This heuristic does not reclaim storage as - soon as it becomes invalid, but it will do so eventually. - - As a special case, alloca(0) reclaims storage without - allocating any. It is a good idea to use alloca(0) in - your main control loop, etc. to force garbage collection. -*/ -#ifndef lint -static char SCCSid[] = "@(#)alloca.c 1.1"; /* for the "what" utility */ -#endif - -#ifdef emacs -#include "config.h" -#ifdef static -/* actually, only want this if static is defined as "" - -- this is for usg, in which emacs must undefine static - in order to make unexec workable - */ -#ifndef STACK_DIRECTION -you -lose --- must know STACK_DIRECTION at compile-time -#endif /* STACK_DIRECTION undefined */ -#endif static -#endif emacs - -#ifdef X3J11 -typedef void *pointer; /* generic pointer type */ -#else -typedef char *pointer; /* generic pointer type */ -#endif - -#ifndef NULL -#define NULL 0 /* null pointer constant */ -#endif - -extern void free(); -extern pointer xmalloc(); - -/* - Define STACK_DIRECTION if you know the direction of stack - growth for your system; otherwise it will be automatically - deduced at run-time. - - STACK_DIRECTION > 0 => grows toward higher addresses - STACK_DIRECTION < 0 => grows toward lower addresses - STACK_DIRECTION = 0 => direction of growth unknown -*/ - -#ifndef STACK_DIRECTION -#define STACK_DIRECTION 0 /* direction unknown */ -#endif - -#if STACK_DIRECTION != 0 - -#define STACK_DIR STACK_DIRECTION /* known at compile-time */ - -#else /* STACK_DIRECTION == 0; need run-time code */ - -static int stack_dir; /* 1 or -1 once known */ -#define STACK_DIR stack_dir - -static void -find_stack_direction (/* void */) -{ - static char *addr = NULL; /* address of first - `dummy', once known */ - auto char dummy; /* to get stack address */ - - if (addr == NULL) - { /* initial entry */ - addr = &dummy; - - find_stack_direction (); /* recurse once */ - } - else /* second entry */ - if (&dummy > addr) - stack_dir = 1; /* stack grew upward */ - else - stack_dir = -1; /* stack grew downward */ -} - -#endif /* STACK_DIRECTION == 0 */ - -/* - An "alloca header" is used to: - (a) chain together all alloca()ed blocks; - (b) keep track of stack depth. - - It is very important that sizeof(header) agree with malloc() - alignment chunk size. The following default should work okay. -*/ - -#ifndef ALIGN_SIZE -#define ALIGN_SIZE sizeof(double) -#endif - -typedef union hdr -{ - char align[ALIGN_SIZE]; /* to force sizeof(header) */ - struct - { - union hdr *next; /* for chaining headers */ - char *deep; /* for stack depth measure */ - } h; -} header; - -/* - alloca( size ) returns a pointer to at least `size' bytes of - storage which will be automatically reclaimed upon exit from - the procedure that called alloca(). Originally, this space - was supposed to be taken from the current stack frame of the - caller, but that method cannot be made to work for some - implementations of C, for example under Gould's UTX/32. -*/ - -static header *last_alloca_header = NULL; /* -> last alloca header */ - -pointer -alloca (size) /* returns pointer to storage */ - unsigned size; /* # bytes to allocate */ -{ - auto char probe; /* probes stack depth: */ - register char *depth = &probe; - -#if STACK_DIRECTION == 0 - if (STACK_DIR == 0) /* unknown growth direction */ - find_stack_direction (); -#endif - - /* Reclaim garbage, defined as all alloca()ed storage that - was allocated from deeper in the stack than currently. */ - - { - register header *hp; /* traverses linked list */ - - for (hp = last_alloca_header; hp != NULL;) - if (STACK_DIR > 0 && hp->h.deep > depth - || STACK_DIR < 0 && hp->h.deep < depth) - { - register header *np = hp->h.next; - - free ((pointer) hp); /* collect garbage */ - - hp = np; /* -> next header */ - } - else - break; /* rest are not deeper */ - - last_alloca_header = hp; /* -> last valid storage */ - } - - if (size == 0) - return NULL; /* no allocation required */ - - /* Allocate combined header + user data storage. */ - - { - register pointer new = xmalloc (sizeof (header) + size); - /* address of header */ - - ((header *)new)->h.next = last_alloca_header; - ((header *)new)->h.deep = depth; - - last_alloca_header = (header *)new; - - /* User storage begins just after header. */ - - return (pointer)((char *)new + sizeof(header)); - } -} - diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/cccp.c b/OpenSim/Utilities/simmToOpenSim/acpp/src/cccp.c deleted file mode 100644 index 19588010ee..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/cccp.c +++ /dev/null @@ -1,6647 +0,0 @@ -/* C Compatible Compiler Preprocessor (CCCP) -Copyright (C) 1986, 1987, 1989 Free Software Foundation, Inc. - Written by Paul Rubin, June 1986 - Adapted to ANSI C, Richard Stallman, Jan 1987 - -This program is free software; you can redistribute it and/or modify it -under the terms of the GNU General Public License as published by the -Free Software Foundation; either version 1, or (at your option) any -later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. - - In other words, you are welcome to use, share and improve this program. - You are forbidden to forbid anyone else to use, share and improve - what you give them. Help stamp out software-hoarding! */ - - -/*JPL*/ -#define GCC_INCLUDE_DIR "/usr/include" -#define GPLUSPLUS_INCLUDE_DIR "/usr/include" - -#ifdef sgi -/* do not compile with -signed!! Only use default unsigned char */ -typedef char U_CHAR; -#else -typedef unsigned char U_CHAR; -#endif - -#ifdef DWARF_DM -#include "dm.h" -#endif - -#ifdef EMACS -#define NO_SHORTNAMES -#include "../src/config.h" -#ifdef open -#undef open -#undef read -#undef write -#endif /* open */ -#endif /* EMACS */ - -#ifndef EMACS -#include "config.h" -#endif /* not EMACS */ - -/* In case config.h defines these. */ -#undef bcopy -#undef bzero -#undef bcmp - -#include - -#ifdef __MWERKS__ - /*#include */ - #include - - #define _OPEN(_FD, _MODE, _PERM) open(_FD, _MODE) -#else - #include - - #define _OPEN(_FD, _MODE, _PERM) open(_FD, _MODE, _PERM) -#endif - -#include -#include -#include -#include -#include -#include - -#ifndef WIN32 -#include -#endif - -#ifndef VMS -#ifdef WIN32 -#include -#else -#include -#endif - -#ifndef USG -#ifdef WIN32 -#include /* for __DATE__ and __TIME__ */ -#define index strchr -#define rindex strrchr -#else -#include /* for __DATE__ and __TIME__ */ -#include -#endif -#else -#define index strchr -#define rindex strrchr -#include -#include -#endif /* USG */ -#endif /* not VMS */ - -/* VMS-specific definitions */ -#ifdef VMS -#include -#include /* This defines "errno" properly */ -#include /* This defines sys_errlist/sys_nerr properly */ -#define O_RDONLY 0 /* Open arg for Read/Only */ -#define O_WRONLY 1 /* Open arg for Write/Only */ -#define read(fd,buf,size) VAX11_C_read(fd,buf,size) -#define write(fd,buf,size) VAX11_C_write(fd,buf,size) -#ifdef __GNUC__ -#define BSTRING /* VMS/GCC supplies the bstring routines */ -#endif /* __GNUC__ */ -#endif /* VMS */ - -#ifdef __linux__ -#include -#ifdef __GNUC__ -#define BSTRING /* linux/GCC supplies the bstring routines */ -#endif /* __GNUC__ */ -#endif /* linux */ - -#ifdef WIN32 - #include - #include - - - #ifdef __MWERKS__ - #define fileno(F) ((F)->handle) - #endif - - #ifndef F_DUPFD - #define F_DUPFD 0x0 - #endif -#endif - -#ifdef max -#undef max -#endif - -#define max(a,b) ((a) > (b) ? (a) : (b)) - -/* External declarations. */ - -void bcopy (), bzero (); -int bcmp (); -extern char *getenv (); -extern char *version_string; -extern int inside_math; - -/* Forward declarations. */ - -struct directive; -struct file_buf; -struct arglist; -struct argdata; - -int do_define (), do_undef (), do_error (), - do_pragma (), do_if (), do_xifdef (), - do_endif (), do_sccs (), do_ident(), do_once (), - do_include(), finclude(), do_line(), do_elif(), do_else(); - -struct hashnode *install (); -struct hashnode *lookup (); - -char *xmalloc (), *xrealloc (), *xcalloc (), *savestring (); -void fatal (), pfatal_with_name (), perror_with_name (); - -void macroexpand (); -void dump_all_macros (); -void conditional_skip (); -void skip_if_group (); -void output_line_command (); -/* Last arg to output_line_command. */ -enum file_change_code {same_file, enter_file, leave_file}; - -int grow_outbuf (); -int handle_directive (); -int handle_c_expression (); -double parse_c_math_expression (); -void memory_full (); - -U_CHAR *macarg1 (); -char *macarg (); - -U_CHAR *skip_to_end_of_comment (); -U_CHAR *skip_quoted_string (); - -#ifndef FATAL_EXIT_CODE -#define FATAL_EXIT_CODE 33 /* gnu cc command understands this */ -#endif - -#ifndef SUCCESS_EXIT_CODE -#define SUCCESS_EXIT_CODE 0 /* 0 means success on Unix. */ -#endif - -/* Name under which this program was invoked. */ - -char *acpp_progname; - -/* Nonzero means handle C++ comment syntax and use - extra default include directories for C++. */ - -int cplusplus; - -/* Current maximum length of directory names in the search path - for include files. (Altered as we get more of them.) */ - -int max_include_len; - -/* Nonzero means copy comments into the output file. */ - -int put_out_comments = 0; - -/* Nonzero means turn backslash-newline into newline when outputting - defines, but don't consider them to end the macro definition. */ -int no_newline_splicing = 0; - -/* Nonzero means don't process the ANSI trigraph sequences. */ - -int no_trigraphs = 0; - -/* Nonzero means print the names of included files rather than - the preprocessed output. 1 means just the #include "...", - 2 means #include <...> as well. */ - -int print_deps = 0; -#ifdef sgi -/* deps_prefix is non-null if we wish to output dependencies in traditional - cpp format. It is a pointer to the prefix string. - deps_file is a handle for -MD's argument, which names the make dependency - file to update. */ -#include -#include "make_depend.h" -char *deps_prefix; -MDhandle deps_file; -#endif - -/* Nonzero means don't output line number information. */ - -int no_line_commands; - -/* Nonzero means inhibit output of the preprocessed text - and instead output the definitions of all user-defined macros - in a form suitable for use as input to cccp. */ - -int dump_macros; - -/* Nonzero means give all the error messages the ANSI standard requires. */ -#ifdef sgi - /* value > 1 means add warnings about #ident, #sccs, which are in - our headers, so we do not want to warn too much.... - */ - -#endif - -int pedantic; -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ -/* value of > 0 means turn warnings off */ -int warnings_off; -/* - If showdefines is non-zero, put out extra stuff to show where - the define is coming from. - For -showdefines option for debugging include files. -*/ -int showdefines; -#endif - -/* Nonzero means warn if slash-star appears in a comment. */ - -int warn_comments; - -/* Nonzero means warn if there are any trigraphs. */ - -int warn_trigraphs; - -/* Nonzero means try to imitate old fashioned non-ANSI preprocessor. */ - -int traditional; - -/* Nonzero causes output not to be done, - but directives such as #define that have side effects - are still obeyed. */ - -int no_output; - -/* I/O buffer structure. - The `fname' field is nonzero for source files and #include files - and for the dummy text used for -D and -U. - It is zero for rescanning results of macro expansion - and for expanding macro arguments. */ -#define INPUT_STACK_MAX 200 -struct file_buf { - char *fname; - int lineno; - int length; -#ifdef sgi - int endif_cruft_message_issued; -#endif - U_CHAR *buf; - U_CHAR *bufp; - /* Macro that this level is the expansion of. - Included so that we can reenable the macro - at the end of this level. */ - struct hashnode *macro; - /* Value of if_stack at start of this file. - Used to prohibit unmatched #endif (etc) in an include file. */ - struct if_stack *if_stack; - /* Object to be freed at end of input at this level. */ - U_CHAR *free_ptr; -} instack[INPUT_STACK_MAX]; - -#ifdef sgi -#define ONE_TIME_ENDIF(s) {\ - if(instack[indepth].endif_cruft_message_issued == 0) {\ - s;\ - } instack[indepth].endif_cruft_message_issued += 1; } - -#endif - -/* Current nesting level of input sources. - `instack[indepth]' is the level currently being read. */ -int indepth = -1; -#define CHECK_DEPTH(code) \ - if (indepth >= (INPUT_STACK_MAX - 1)) \ - { \ - error_with_line (line_for_error (instack[indepth].lineno), \ - "macro or #include recursion too deep"); \ - code; \ - } - -/* Current depth in #include directives that use <...>. */ -int system_include_depth = 0; - -typedef struct file_buf FILE_BUF; - -/* The output buffer. Its LENGTH field is the amount of room allocated - for the buffer, not the number of chars actually present. To get - that, subtract outbuf.buf from outbuf.bufp. */ - -#define OUTBUF_SIZE 10 /* initial size of output buffer */ -FILE_BUF outbuf; - -/* Grow output buffer OBUF points at - so it can hold at least NEEDED more chars. */ - -#define check_expand(OBUF, NEEDED) \ - (((OBUF)->length - ((OBUF)->bufp - (OBUF)->buf) <= (NEEDED)) \ - ? grow_outbuf ((OBUF), (NEEDED)) : 0) - -struct file_name_list - { - struct file_name_list *next; - char *fname; - }; - -/* #include "file" looks in source file dir, then stack. */ -/* #include just looks in the stack. */ -/* -I directories are added to the end, then the defaults are added. */ -struct file_name_list include_defaults[] = - { -#ifndef VMS - { &include_defaults[1], GCC_INCLUDE_DIR }, - { &include_defaults[2], "/usr/include" }, - { 0, "/usr/local/include" } -#else - { &include_defaults[1], "GNU_CC_INCLUDE:" }, /* GNU includes */ - { &include_defaults[2], "SYS$SYSROOT:[SYSLIB.]" }, /* VAX-11 "C" includes */ - { 0, "" }, /* This makes normal VMS filespecs work OK */ -#endif /* VMS */ - }; - -/* These are used instead of the above, for C++. */ -struct file_name_list cplusplus_include_defaults[] = - { -#ifndef VMS - /* Pick up GNU C++ specific include files. */ - { &cplusplus_include_defaults[1], GPLUSPLUS_INCLUDE_DIR }, - /* Use GNU CC specific header files. */ - { &cplusplus_include_defaults[2], GCC_INCLUDE_DIR }, - { 0, "/usr/include" } -#else - { &cplusplus_include_defaults[1], "GNU_CC_INCLUDE:" }, - /* VAX-11 C includes */ - { &cplusplus_include_defaults[2], "SYS$SYSROOT:[SYSLIB.]" }, - { 0, "" }, /* This makes normal VMS filespecs work OK */ -#endif /* VMS */ - }; - -struct file_name_list *include = 0; /* First dir to search */ - /* First dir to search for */ -struct file_name_list *first_bracket_include = 0; -struct file_name_list *last_include = 0; /* Last in chain */ - -/* List of included files that contained #once. */ -struct file_name_list *dont_repeat_files = 0; - -/* List of other included files. */ -struct file_name_list *all_include_files = 0; - -/* Structure allocated for every #define. For a simple replacement - such as - #define foo bar , - nargs = -1, the `pattern' list is null, and the expansion is just - the replacement text. Nargs = 0 means a functionlike macro with no args, - e.g., - #define getchar() getc (stdin) . - When there are args, the expansion is the replacement text with the - args squashed out, and the reflist is a list describing how to - build the output from the input: e.g., "3 chars, then the 1st arg, - then 9 chars, then the 3rd arg, then 0 chars, then the 2nd arg". - The chars here come from the expansion. Whatever is left of the - expansion after the last arg-occurrence is copied after that arg. - Note that the reflist can be arbitrarily long--- - its length depends on the number of times the arguments appear in - the replacement text, not how many args there are. Example: - #define f(x) x+x+x+x+x+x+x would have replacement text "++++++" and - pattern list - { (0, 1), (1, 1), (1, 1), ..., (1, 1), NULL } - where (x, y) means (nchars, argno). */ - -typedef struct definition DEFINITION; -struct definition { - int nargs; - int length; /* length of expansion string */ - U_CHAR *expansion; - struct reflist { - struct reflist *next; - char stringify; /* nonzero if this arg was preceded by a - # operator. */ - char raw_before; /* Nonzero if a ## operator before arg. */ - char raw_after; /* Nonzero if a ## operator after arg. */ - int nchars; /* Number of literal chars to copy before - this arg occurrence. */ - int argno; /* Number of arg to substitute (origin-0) */ - } *pattern; - /* Names of macro args, concatenated in reverse order - with comma-space between them. - The only use of this is that we warn on redefinition - if this differs between the old and new definitions. */ - U_CHAR *argnames; -}; - -/* different kinds of things that can appear in the value field - of a hash node. Actually, this may be useless now. */ -union hashval { - int ival; - char *cpval; - DEFINITION *defn; -}; - - -/* The structure of a node in the hash table. The hash table - has entries for all tokens defined by #define commands (type T_MACRO), - plus some special tokens like __LINE__ (these each have their own - type, and the appropriate code is run when that type of node is seen. - It does not contain control words like "#define", which are recognized - by a separate piece of code. */ - -/* different flavors of hash nodes --- also used in keyword table */ -enum node_type { - T_DEFINE = 1, /* the `#define' keyword */ - T_INCLUDE, /* the `#include' keyword */ - T_IFDEF, /* the `#ifdef' keyword */ - T_IFNDEF, /* the `#ifndef' keyword */ - T_IF, /* the `#if' keyword */ - T_ELSE, /* `#else' */ - T_PRAGMA, /* `#pragma' */ - T_ELIF, /* `#else' */ - T_UNDEF, /* `#undef' */ - T_LINE, /* `#line' */ - T_ERROR, /* `#error' */ - T_ENDIF, /* `#endif' */ - T_SCCS, /* `#sccs', used on system V. */ - T_IDENT, /* `#ident', used on system V. */ - T_SPECLINE, /* special symbol `__LINE__' */ - T_DATE, /* `__DATE__' */ - T_FILE, /* `__FILE__' */ - T_BASE_FILE, /* `__BASE_FILE__' */ - T_INCLUDE_LEVEL, /* `__INCLUDE_LEVEL__' */ - T_VERSION, /* `__VERSION__' */ - T_TIME, /* `__TIME__' */ - T_CONST, /* Constant value, used by `__STDC__' */ - T_MACRO, /* macro defined by `#define' */ - T_DISABLED, /* macro temporarily turned off for rescan */ - T_SPEC_DEFINED, /* special `defined' macro for use in #if statements */ - T_UNUSED /* Used for something not defined. */ - }; - -struct hashnode { - struct hashnode *next; /* double links for easy deletion */ - struct hashnode *prev; - struct hashnode **bucket_hdr; /* also, a back pointer to this node's hash - chain is kept, in case the node is the head - of the chain and gets deleted. */ - enum node_type type; /* type of special token */ - int length; /* length of token, for quick comparison */ - U_CHAR *name; /* the actual name */ - union hashval value; /* pointer to expansion, or whatever */ -}; - -typedef struct hashnode HASHNODE; - -/* Some definitions for the hash table. The hash function MUST be - computed as shown in hashf () below. That is because the rescan - loop computes the hash value `on the fly' for most tokens, - in order to avoid the overhead of a lot of procedure calls to - the hashf () function. Hashf () only exists for the sake of - politeness, for use when speed isn't so important. */ - -#define HASHSIZE 1403 -HASHNODE *hashtab[HASHSIZE]; -#define HASHSTEP(old, c) ((old << 2) + c) -#define MAKE_POS(v) (v & ~0x80000000) /* make number positive */ - -/* Symbols to predefine. */ - -#ifdef CPP_PREDEFINES -char *predefs = CPP_PREDEFINES; -#else -char *predefs = ""; -#endif - -/* `struct directive' defines one #-directive, including how to handle it. */ - -struct directive { - int length; /* Length of name */ - int (*func)(); /* Function to handle directive */ - char *name; /* Name of directive */ - enum node_type type; /* Code which describes which directive. */ - char angle_brackets; /* Nonzero => <...> is special. */ - char traditional_comments; /* Nonzero: keep comments if -traditional. */ - char pass_thru; /* Copy preprocessed directive to output file. */ -}; - -/* Here is the actual list of #-directives, most-often-used first. */ - -struct directive directive_table[] = { - { 6, do_define, "define", T_DEFINE, 0, 1}, - { 2, do_if, "if", T_IF}, - { 5, do_xifdef, "ifdef", T_IFDEF}, - { 6, do_xifdef, "ifndef", T_IFNDEF}, - { 5, do_endif, "endif", T_ENDIF}, - { 4, do_else, "else", T_ELSE}, - { 4, do_elif, "elif", T_ELIF}, - { 4, do_line, "line", T_LINE}, - { 7, do_include, "include", T_INCLUDE, 1}, - { 5, do_undef, "undef", T_UNDEF}, - { 5, do_error, "error", T_ERROR}, -#ifdef SCCS_DIRECTIVE - { 4, do_sccs, "sccs", T_SCCS}, - { 5, do_ident, "ident", T_IDENT}, -#endif - { 6, do_pragma, "pragma", T_PRAGMA, 0, 0, 1}, - { -1, 0, "", T_UNUSED}, -}; - -/* table to tell if char can be part of a C identifier. */ -U_CHAR is_idchar[256]; -/* table to tell if char can be first char of a c identifier. */ -U_CHAR is_idstart[256]; -/* table to tell if c is horizontal space. */ -extern U_CHAR is_hor_space[256]; -/* table to tell if c is horizontal or vertical space. */ -U_CHAR is_space[256]; - -#define SKIP_WHITE_SPACE(p) do { while (is_hor_space[*p]) p++; } while (0) -#define SKIP_ALL_WHITE_SPACE(p) do { while (is_space[*p]) p++; } while (0) - -int errors = 0; /* Error counter for exit code */ - -/* Zero means dollar signs are punctuation. - -$ stores 0; -traditional, stores 1. Default is 1 for VMS, 0 otherwise. - This must be 0 for correct processing of this ANSI C program: - #define foo(a) #a - #define lose(b) foo(b) - #define test$ - lose(test) */ -#ifndef DOLLARS_IN_IDENTIFIERS -#define DOLLARS_IN_IDENTIFIERS 0 -#endif -int dollars_in_ident = DOLLARS_IN_IDENTIFIERS; - -FILE_BUF expand_to_temp_buffer (); - -DEFINITION *collect_expansion (); - -/* Stack of conditionals currently in progress - (including both successful and failing conditionals). */ - -struct if_stack { - struct if_stack *next; /* for chaining to the next stack frame */ - char *fname; /* copied from input when frame is made */ - int lineno; /* similarly */ - int if_succeeded; /* true if a leg of this if-group - has been passed through rescan */ - enum node_type type; /* type of last directive seen in this group */ -}; -typedef struct if_stack IF_STACK_FRAME; -IF_STACK_FRAME *if_stack = NULL; - -/* Buffer of -M output. */ - -char *deps_buffer; - -/* Number of bytes allocated in above. */ -int deps_allocated_size; - -/* Number of bytes used. */ -int deps_size; - -/* Number of bytes since the last newline. */ -int deps_column; - -/* Nonzero means -I- has been seen, - so don't look for #include "foo" the source-file directory. */ -int ignore_srcdir; - -/* Handler for SIGPIPE. */ - -static void -pipe_closed () -{ - fatal ("output pipe has been closed"); -} - -#ifdef sgi -/* This is for output on names like b.p or a.c++ etc etc - Generates the same sort of output as cpp does - for the dependency (.o) name. 16 is too much extra, but - we do need room for the .o (sometimes) and the :space ... -*/ -char * -make_deps_target(char *p) -{ - char *q, *target, *dot, *slash; - int len; - - /* discard all directory prefixes from P */ - for (q = p; *q; q++) { - if (*q == '/' && q[1]) - p = q + 1; - } - len = q - p; - target = (char *)xmalloc(len + 16); - strcpy(target,p); - dot = strrchr(target,'.'); - slash = strrchr(target,'/'); - if (dot && ((slash == 0) || (dot > slash)) && dot <= target+len-2) { - dot[1] = 'o'; - dot[2] = '\0'; - } else { - /* cpp says -missing component name- rather than doing this */ - strcat(target,".o"); - } - return target; -} -#endif - - -/* ------------------------------------------------------------------------- - PARAMETERIZED USER MESSAGES - allows SIMM to display acpp message in the - SIMM message window. ----------------------------------------------------------------------------- */ -static int default_message_proc (const char* format, ...) -{ - - va_list ap; - int n; - - va_start(ap, format); - n = vfprintf(stderr, format, ap); - va_end(ap); - - return n; - -} - -typedef int (*_msg_proc)(const char* format, ...); - -static _msg_proc sMessageProc = default_message_proc; - -#define USER_MESSAGE sMessageProc - -/* ------------------------------------------------------------------------- - reset_acpp - this routine was added to allow accp_main() to be called - multiple times during a program's execution without residual effect - from one call to the next. This routine resets acpp's state. -- KMS 8/24/99 ----------------------------------------------------------------------------- */ -static void reset_acpp () -{ - int i, n = 0; - - for (i = 0; i < HASHSIZE; i++) - { - if (hashtab[i]) - { - HASHNODE* node = hashtab[i]; - - while (node) - { - HASHNODE* next = node->next; - - free(node); - - /* NOTE: If we want to be ambitious here we can also determine - * when to free the hash node's "value" (i.e. cpval, or defn). - * This seemed tricky however, and I decided that a small - * memory leak was better than a possible invalid free(). - * -- KMS 8/24/99 - */ - - node = next; - - n++; - } - hashtab[i] = NULL; - } - } -#if 0 - if (n > 0) - USER_MESSAGE("Freed %d hash nodes in acpp.\n", n); -#endif -} - - -FILE* fp_out = NULL; - - -int acpp_main(int argc, char** argv, _msg_proc msg) -{ - int st_mode; - long st_size; - char *in_fname, *out_fname; - int f, i; - FILE_BUF *fp; - char **pend_files = (char **) xmalloc (argc * sizeof (char *)); - char **pend_defs = (char **) xmalloc (argc * sizeof (char *)); - char **pend_undefs = (char **) xmalloc (argc * sizeof (char *)); - int inhibit_predefs = 0; - int no_standard_includes = 0; - - /* Non-0 means don't output the preprocessed program. */ - int inhibit_output = 0; - - /* Stream on which to print the dependency information. */ - FILE *deps_stream = 0; - /* Target-name to write with the dependency information. */ - char *deps_target = 0; -#ifdef sgi - /* name of the make-dependencies file to update */ - char *deps_update = 0; -#endif - - /* KMS: allow caller to specify message procedure */ - - sMessageProc = msg ? msg : default_message_proc; - - /* JPL: re-initialize global variables */ - - include = 0; - first_bracket_include = 0; - last_include = 0; - errors = 0; - indepth = -1; - system_include_depth = 0; - if_stack = NULL; - - /* KMS: empty residual hash-table entries from previous call to acpp_main() */ - - reset_acpp(); - -#ifdef RLIMIT_STACK - /* Get rid of any avoidable limit on stack size. */ - { - struct rlimit rlim; - - /* Set the stack limit huge so that alloca (particularly stringtab - * in dbxread.c) does not fail. */ - getrlimit (RLIMIT_STACK, &rlim); - rlim.rlim_cur = rlim.rlim_max; - setrlimit (RLIMIT_STACK, &rlim); - } -#endif /* RLIMIT_STACK defined */ - - acpp_progname = argv[0]; - in_fname = NULL; - out_fname = NULL; - - /* Initialize is_idchar to allow $. */ - dollars_in_ident = 1; - initialize_char_syntax (); - dollars_in_ident = DOLLARS_IN_IDENTIFIERS; - - no_line_commands = 0; - no_trigraphs = 1; - dump_macros = 0; - no_output = 0; - cplusplus = 0; -#ifdef CPLUSPLUS - cplusplus = 1; -#endif - -#ifndef WIN32 - signal (SIGPIPE, pipe_closed); -#endif - -#ifndef VMS - max_include_len - = max (max (sizeof (GCC_INCLUDE_DIR), - sizeof (GPLUSPLUS_INCLUDE_DIR)), - sizeof ("/usr/include/CC")); -#else /* VMS */ - max_include_len - = sizeof("SYS$SYSROOT:[SYSLIB.]"); -#endif /* VMS */ - - bzero (pend_files, argc * sizeof (char *)); - bzero (pend_defs, argc * sizeof (char *)); - bzero (pend_undefs, argc * sizeof (char *)); - - /* Process switches and find input file name. */ - - for (i = 1; i < argc; i++) { - if (argv[i][0] != '-') { - if (out_fname != NULL) - fatal ("Usage: %s [switches] input output\n", argv[0]); - else if (in_fname != NULL) - out_fname = argv[i]; - else - in_fname = argv[i]; - } else { - switch (argv[i][1]) { - - case 'i': - if (argv[i][2] != 0) - pend_files[i] = argv[i] + 2; - else - pend_files[i] = argv[i+1], i++; - break; - - case 'o': - if (out_fname != NULL) - fatal ("Output filename specified twice\n"); - out_fname = argv[++i]; - if (!strcmp (out_fname, "-")) - out_fname = ""; - break; - - case 'p': -#ifdef sgi - pedantic += 1; -#else - pedantic = 1; -#endif - break; - - case 't': - if (!strcmp (argv[i], "-traditional")) { - traditional = 1; - dollars_in_ident = 1; - } else if (!strcmp (argv[i], "-trigraphs")) { - no_trigraphs = 0; - } - break; - -#ifdef sgi - case 'B': -#endif - case '+': - cplusplus = 1; - break; - - case 'W': - if (!strcmp (argv[i], "-Wtrigraphs")) { - warn_trigraphs = 1; - } - if (!strcmp (argv[i], "-Wcomments")) - warn_comments = 1; - if (!strcmp (argv[i], "-Wcomment")) - warn_comments = 1; - if (!strcmp (argv[i], "-Wall")) { - warn_trigraphs = 1; - warn_comments = 1; - } - break; - - case 'M': -#ifdef sgi - if (argv[i][2] == 'D') { - char **cpp; - if (!strcmp(&argv[i][3], "target")) - cpp = &deps_target; - else if (!strcmp(&argv[i][3], "update")) - cpp = &deps_update; - else - goto default_illegal_switch; - if (++i == argc) - fatal("%s option requires an argument", argv[i-1]); - *cpp = argv[i]; - break; - } -#endif - if (!strcmp (argv[i], "-M")) - print_deps = 2; - else if (!strcmp (argv[i], "-MM")) - print_deps = 1; -#ifdef sgi - else - goto default_illegal_switch; -#endif - inhibit_output = 1; - break; - - case 'd': - dump_macros = 1; - no_output = 1; - break; - - case 'v': - USER_MESSAGE("GNU CPP version %s\n", version_string); - break; - - case 'D': - { - char *p, *p1; - - if (argv[i][2] != 0) - p = argv[i] + 2; - else - p = argv[++i]; - - if ((p1 = (char *) index (p, '=')) != NULL) - *p1 = ' '; - pend_defs[i] = p; - } - break; - - case 'U': /* JF #undef something */ - if (argv[i][2] != 0) - pend_undefs[i] = argv[i] + 2; - else - pend_undefs[i] = argv[i+1], i++; - break; - - case 'C': - put_out_comments = 1; - break; - - case 'E': /* -E comes from cc -E; ignore it. */ - break; - - case 'P': - no_line_commands = 1; - break; - - case '$': /* Don't include $ in identifiers. */ -#ifdef sgi - /* we disallow by default, but allow with -$ */ - dollars_in_ident = 1; -#else - dollars_in_ident = 0; -#endif - break; - - case 'I': /* Add directory to path for includes. */ - { - struct file_name_list *dirtmp; - - if (argv[i][2] == 0 && argv[i+1][0] == '-') { - /* handle SGI brain-damaged -I hackery; -I with - * no arg means -nostdinc. YUCK */ - no_standard_includes = 1; - break; - } - - if (! ignore_srcdir && !strcmp (argv[i] + 2, "-")) - { - ignore_srcdir = 1; - } - else - { - dirtmp = (struct file_name_list *) - xmalloc (sizeof (struct file_name_list)); - dirtmp->next = 0; /* New one goes on the end */ - if (include == 0) - { - include = dirtmp; - last_include = dirtmp; - } - else - { - last_include->next = dirtmp; - } - last_include = dirtmp; /* Tail follows the last one */ - if (argv[i][2] != 0) - dirtmp->fname = argv[i] + 2; - else - dirtmp->fname = argv[++i]; - if (strlen (dirtmp->fname) > max_include_len) - max_include_len = strlen (dirtmp->fname); - if (ignore_srcdir && first_bracket_include == 0) - first_bracket_include = dirtmp; - } - } - break; -#ifdef sgi - case 'w': - warnings_off = 1; - break; - case 's': - if(strcmp(argv[i],"-showdefines") == 0) { - showdefines++; - } else { - goto default_illegal_switch; - } - break; -#endif - - case 'n': -#if sgi - if (!strcmp (argv[i], "-nosplice")) { - no_newline_splicing = 1; - break; - } -#endif - /* -nostdinc causes no default include directories. - You must specify all include-file directories with -I. */ - no_standard_includes = 1; - break; - - case 'u': - /* Sun compiler passes undocumented switch "-undef". - Let's assume it means to inhibit the predefined symbols. */ - inhibit_predefs = 1; - break; - - case '\0': /* JF handle '-' as file name meaning stdin or stdout */ - if (in_fname == NULL) { - in_fname = ""; - break; - } else if (out_fname == NULL) { - out_fname = ""; - break; - } /* else fall through into error */ - - default_illegal_switch: ; /* fall thru */ - default: - fatal ("Invalid option `%s'\n", argv[i]); - } - } - } - - /* Now that dollars_in_ident is known, initialize is_idchar. */ - initialize_char_syntax (); - -#ifdef sgi - /* Initialize output buffer */ - outbuf.buf = (U_CHAR *) xmalloc (OUTBUF_SIZE); - outbuf.bufp = outbuf.buf; - outbuf.length = OUTBUF_SIZE; -#endif - /* Install __LINE__, etc. Must follow initialize_char_syntax - and option processing. */ -#ifdef DWARF_DM - dm_push_new(in_fname,0); -#endif - initialize_builtins (); - - /* Do standard #defines that identify processor type. */ - -#ifdef DWARF_DM - dm_push_new(DWARF_INIT_FNAME,1); -#endif - if (!inhibit_predefs) { - char *p = (char *) alloca (strlen (predefs) + 1); - strcpy (p, predefs); - while (*p) { - char *q; - if (p[0] != '-' || p[1] != 'D') - abort (); - q = &p[2]; - while (*p && *p != ' ') p++; - if (*p != 0) - *p++= 0; - make_definition (q); - } - } - /* Do defines specified with -D. */ - for (i = 1; i < argc; i++) - if (pend_defs[i]) - make_definition (pend_defs[i]); - - /* Do undefines specified with -U. */ - for (i = 1; i < argc; i++) - if (pend_undefs[i]) - make_undef (pend_undefs[i]); -#ifdef DWARF_DM - dm_finish_current(); -#endif - - /* Unless -fnostdinc, - tack on the standard include file dirs to the specified list */ - if (!no_standard_includes) - { - if (include == 0) - { - include = (cplusplus ? cplusplus_include_defaults : include_defaults); - } - else - { - last_include->next - = (cplusplus ? cplusplus_include_defaults : include_defaults); - } - /* Make sure the list for #include <...> also has the standard dirs. */ - if (ignore_srcdir && first_bracket_include == 0) - { - first_bracket_include - = (cplusplus ? cplusplus_include_defaults : include_defaults); - } - } - -#ifndef sgi - /* Initialize output buffer */ - - outbuf.buf = (U_CHAR *) xmalloc (OUTBUF_SIZE); - outbuf.bufp = outbuf.buf; - outbuf.length = OUTBUF_SIZE; -#endif - - /* Scan the -i files before the main input. - Much like #including them, but with no_output set - so that only their macro definitions matter. */ - - no_output++; - for (i = 1; i < argc; i++) - if (pend_files[i]) { - int fd = _OPEN (pend_files[i], O_RDONLY, 0666); - if (fd < 0) { - perror_with_name (pend_files[i]); - reset_acpp(); - return FATAL_EXIT_CODE; - } - finclude (fd, pend_files[i], &outbuf); - } - no_output--; - - /* Create an input stack level for the main input file - and copy the entire contents of the file into it. */ - - fp = &instack[++indepth]; - - /* JF check for stdin */ - if (in_fname == NULL || *in_fname == 0) { - in_fname = ""; - f = 0; - } else if ((f = _OPEN (in_fname, O_RDONLY, 0666)) < 0) - goto perror; - - /* Either of two environment variables can specify output of deps. - Its value is either "OUTPUT_FILE" or "OUTPUT_FILE DEPS_TARGET", - where OUTPUT_FILE is the file to write deps info to - and DEPS_TARGET is the target to mention in the deps. */ - - if (print_deps == 0 - && (getenv ("SUNPRO_DEPENDENCIES") != 0 - || getenv ("DEPENDENCIES_OUTPUT") != 0)) - { - char *spec = getenv ("DEPENDENCIES_OUTPUT"); - char *s; - char *output_file; - - if (spec == 0) - { - spec = getenv ("SUNPRO_DEPENDENCIES"); - print_deps = 2; - } - else - print_deps = 1; - - s = spec; - /* Find the space before the DEPS_TARGET, if there is one. */ - /* Don't use `index'; that causes trouble on USG. */ - while (*s != 0 && *s != ' ') s++; - if (*s != 0) - { - deps_target = s + 1; - output_file = (char *) xmalloc (s - spec + 1); - bcopy (spec, output_file, s - spec); - output_file[s - spec] = 0; - } - else - { - deps_target = 0; - output_file = spec; - } - - deps_stream = fopen (output_file, "a"); - if (deps_stream == 0) - pfatal_with_name (output_file); - } - /* If the -M option was used, output the deps to standard output. */ - else if (print_deps) - deps_stream = stdout; - - /* For -M, print the expected object file name - as the target of this Make-rule. */ - if (print_deps) { - deps_allocated_size = 200; - deps_buffer = (char *) xmalloc (deps_allocated_size); - deps_buffer[0] = 0; - deps_size = 0; - deps_column = 0; - - if (deps_target) { - deps_output (deps_target, 0); - deps_output (":", 0); - } else if (*in_fname == 0) - deps_output ("-: ", 0); - else { -#ifdef sgi - deps_prefix = make_deps_target(in_fname); - strcat(deps_prefix,": "); - deps_output (deps_prefix, 0); -#else - int len; - char *p = in_fname; - char *p1 = p; - /* Discard all directory prefixes from P. */ - while (*p1) { - if (*p1 == '/') - p = p1 + 1; - p1++; - } - /* Output P, but remove known suffixes. */ - len = strlen (p); - if (p[len - 2] == '.' && p[len - 1] == 'c') { - deps_output (p, len - 2); - } else if (p[len - 3] == '.' - && p[len - 2] == 'c' - && p[len - 1] == 'c') { - deps_output (p, len - 3); - } else { - deps_output (p, 0); - } - /* Supply our own suffix. */ - deps_output (".o : ", 0); -#endif - deps_output (in_fname, 0); - deps_output (" ", 0); - } - } -#ifdef sgi - if (deps_update) { - if (*in_fname == '\0') - fatal("No input filename specified with -MDupdate option"); - if (deps_target == NULL) - deps_target = make_deps_target(in_fname); - printf("*************No MDopen************\n"); -/* deps_file = MDopen("cpp", deps_update, deps_target, fatal);*/ - } -#endif - - file_size_and_mode (f, &st_mode, &st_size); - fp->fname = in_fname; - fp->lineno = 1; - /* JF all this is mine about reading pipes and ttys */ - if ((st_mode & S_IFMT) != S_IFREG) { - /* Read input from a file that is not a normal disk file. - We cannot preallocate a buffer with the correct size, - so we must read in the file a piece at the time and make it bigger. */ - int size; - int bsize; - int cnt; - U_CHAR *bufp; - - bsize = 2000; - size = 0; - fp->buf = (U_CHAR *) xmalloc (bsize + 2); - bufp = fp->buf; - for (;;) { - cnt = read (f, bufp, bsize - size); - if (cnt < 0) goto perror; /* error! */ - if (cnt == 0) break; /* End of file */ - size += cnt; - bufp += cnt; - if (bsize == size) { /* Buffer is full! */ - bsize *= 2; - fp->buf = (U_CHAR *) xrealloc (fp->buf, bsize + 2); - bufp = fp->buf + size; /* May have moved */ - } - } - fp->length = size; - } else { - /* Read a file whose size we can determine in advance. - For the sake of VMS, st_size is just an upper bound. */ - long i; - fp->length = 0; - fp->buf = (U_CHAR *) xmalloc (st_size + 2); - - while (st_size > 0) { - i = read (f, fp->buf + fp->length, st_size); - if (i <= 0) { - if (i == 0) break; - goto perror; - } - fp->length += i; - st_size -= i; - } - } - fp->bufp = fp->buf; - fp->if_stack = if_stack; - - /* Unless inhibited, convert trigraphs in the input. */ - - if (!no_trigraphs) - trigraph_pcp (fp); - - /* Make sure data ends with a newline. And put a null after it. */ - - if (fp->length > 0 && fp->buf[fp->length-1] != '\n') - fp->buf[fp->length++] = '\n'; - fp->buf[fp->length] = '\0'; - - /* Now that we know the input file is valid, open the output. */ - if ( ! out_fname || ! strcmp (out_fname, "")) - out_fname = "stdout"; - else if ( ! (fp_out = fopen (out_fname, "w"))) - pfatal_with_name (out_fname); - - output_line_command (fp, &outbuf, 0, same_file); - - /* Scan the input, processing macros and directives. */ - - rescan (&outbuf, 0); - - /* Now we have processed the entire input - Write whichever kind of output has been requested. */ - - if (dump_macros) - dump_all_macros (); - else if (! inhibit_output && deps_stream != stdout) { - if (write (fileno (fp_out), outbuf.buf, outbuf.bufp - outbuf.buf) < 0) - fatal ("I/O error on output"); - } - - if (print_deps) { - fputs (deps_buffer, deps_stream); - putc ('\n', deps_stream); - if (deps_stream != stdout) { - fclose (deps_stream); - if (ferror (deps_stream)) - fatal ("I/O error on output"); - } - } -#ifdef sgi - if (deps_file) - { - printf("**************No MDclose***************\n"); -/* MDclose(deps_file, deps_target);*/ - } -#endif -#ifdef DWARF_DM - dm_finish_current(); - dm_write_file(); -#endif - - fclose(fp_out); - - if (f != 0) - close(f); - -/*JPL: is this still needed? - if (ferror (stdout)) - fatal ("I/O error on output"); -*/ - reset_acpp(); - - if (errors) - return (FATAL_EXIT_CODE); - return (SUCCESS_EXIT_CODE); - - perror: - reset_acpp(); - pfatal_with_name (in_fname); -} -#ifdef sgi -static void -print_def_undef(FILE_BUF *op,U_CHAR *type, U_CHAR *name,int namelen) -{ - U_CHAR *lbuf; - U_CHAR *lname; - int fnamelen; - U_CHAR *fname; - FILE_BUF *fp; - int len; - - fp = &instack[indepth]; - fname = fp->fname; - fnamelen = strlen(fname); - lbuf = xmalloc(namelen + 100 + fnamelen); - lname = xmalloc(namelen + 100 + fnamelen); - strncpy(lname,name,namelen); - lname[namelen] = 0; - sprintf(lbuf,"#%s %s #line %d %s\n",type,lname,fp->lineno,fname); - len = strlen(lbuf); - if(op == 0) - op = &outbuf; - check_expand(op,len+1); - if (op->buf > op->buf && op->bufp[-1] != '\n') - *op->bufp++ = '\n'; - bcopy(lbuf,op->bufp,len); - op->bufp+= len; - free(lbuf); - free(lname); -} -#endif - -/* Pre-C-Preprocessor to translate ANSI trigraph idiocy in BUF - before main CCCP processing. Name `pcp' is also in honor of the - drugs the trigraph designers must have been on. - - Using an extra pass through the buffer takes a little extra time, - but is infinitely less hairy than trying to handle ??/" inside - strings, etc. everywhere, and also makes sure that trigraphs are - only translated in the top level of processing. */ - -trigraph_pcp (buf) - FILE_BUF *buf; -{ - register U_CHAR c, *fptr, *bptr, *sptr; - int len; - - fptr = bptr = sptr = buf->buf; - while ((sptr = (U_CHAR *) index (sptr, '?')) != NULL) { - if (*++sptr != '?') - continue; - switch (*++sptr) { - case '=': - c = '#'; - break; - case '(': - c = '['; - break; - case '/': - c = '\\'; - break; - case ')': - c = ']'; - break; - case '\'': - c = '^'; - break; - case '<': - c = '{'; - break; - case '!': - c = '|'; - break; - case '>': - c = '}'; - break; - case '-': - c = '~'; - break; - case '?': - sptr--; - continue; - default: - continue; - } - len = sptr - fptr - 2; - if (bptr != fptr && len > 0) - bcopy (fptr, bptr, len); /* BSD doc says bcopy () works right - for overlapping strings. In ANSI - C, this will be memmove (). */ - bptr += len; - *bptr++ = c; - fptr = ++sptr; - } - len = buf->length - (fptr - buf->buf); - if (bptr != fptr && len > 0) - bcopy (fptr, bptr, len); - buf->length -= fptr - bptr; - buf->buf[buf->length] = '\0'; - if (warn_trigraphs && fptr != bptr -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - && warnings_off == 0 -#endif - ) - warning ("%d trigraph(s) encountered", (fptr - bptr) / 2); -} - -/* Move all backslash-newline pairs out of embarrassing places. - Exchange all such pairs following BP - with any potentially-embarrasing characters that follow them. - Potentially-embarrassing characters are / and * - (because a backslash-newline inside a comment delimiter - would cause it not to be recognized). */ - -void newline_fix (bp) - U_CHAR *bp; -{ - register U_CHAR *p = bp; - register int count = 0; - - /* First count the backslash-newline pairs here. */ - - while (*p++ == '\\' && *p++ == '\n') - count++; - - p = bp + count * 2; - - /* What follows the backslash-newlines is not embarrassing. */ - - if (count == 0 || (*p != '/' && *p != '*')) - return; - - /* Copy all potentially embarrassing characters - that follow the backslash-newline pairs - down to where the pairs originally started. */ - - while (*p == '*' || *p == '/') - *bp++ = *p++; - - /* Now write the same number of pairs after the embarrassing chars. */ - while (count-- > 0) { - *bp++ = '\\'; - *bp++ = '\n'; - } -} - -/* Like newline_fix but for use within a directive-name. - Move any backslash-newlines up past any following symbol constituents. */ - -void name_newline_fix (bp) - U_CHAR *bp; -{ - register U_CHAR *p = bp; - register int count = 0; - - /* First count the backslash-newline pairs here. */ - - while (*p++ == '\\' && *p++ == '\n') - count++; - - p = bp + count * 2; - - /* What follows the backslash-newlines is not embarrassing. */ - - if (count == 0 || !is_idchar[*p]) - return; - - /* Copy all potentially embarrassing characters - that follow the backslash-newline pairs - down to where the pairs originally started. */ - - while (is_idchar[*p]) - *bp++ = *p++; - - /* Now write the same number of pairs after the embarrassing chars. */ - while (count-- > 0) { - *bp++ = '\\'; - *bp++ = '\n'; - } -} - -/* - * The main loop of the program. - * - * Read characters from the input stack, transferring them to the - * output buffer OP. - * - * Macros are expanded and push levels on the input stack. - * At the end of such a level it is popped off and we keep reading. - * At the end of any other kind of level, we return. - * #-directives are handled, except within macros. - * - * If OUTPUT_MARKS is nonzero, keep Newline markers found in the input - * and insert them when appropriate. This is set while scanning macro - * arguments before substitution. It is zero when scanning for final output. - * There are three types of Newline markers: - * * Newline - follows a macro name that was not expanded - * because it appeared inside an expansion of the same macro. - * This marker prevents future expansion of that identifier. - * When the input is rescanned into the final output, these are deleted. - * These are also deleted by ## concatenation. - * * Newline Space (or Newline and any other whitespace character) - * stands for a place that tokens must be separated or whitespace - * is otherwise desirable, but where the ANSI standard specifies there - * is no whitespace. This marker turns into a Space (or whichever other - * whitespace char appears in the marker) in the final output, - * but it turns into nothing in an argument that is stringified with #. - * Such stringified arguments are the only place where the ANSI standard - * specifies with precision that whitespace may not appear. - * - * During this function, IP->bufp is kept cached in IBP for speed of access. - * Likewise, OP->bufp is kept in OBP. Before calling a subroutine - * IBP, IP and OBP must be copied back to memory. IP and IBP are - * copied back with the RECACHE macro. OBP must be copied back from OP->bufp - * explicitly, and before RECACHE, since RECACHE uses OBP. - */ - -rescan (op, output_marks) - FILE_BUF *op; - int output_marks; -{ - /* Character being scanned in main loop. */ - register U_CHAR c; - - /* Length of pending accumulated identifier. */ - register int ident_length = 0; - - /* Hash code of pending accumulated identifier. */ - register int hash = 0; - - /* Current input level (&instack[indepth]). */ - FILE_BUF *ip; - - /* Pointer for scanning input. */ - register U_CHAR *ibp; - - /* Pointer to end of input. End of scan is controlled by LIMIT. */ - register U_CHAR *limit; - - /* Pointer for storing output. */ - register U_CHAR *obp; - - /* REDO_CHAR is nonzero if we are processing an identifier - after backing up over the terminating character. - Sometimes we process an identifier without backing up over - the terminating character, if the terminating character - is not special. Backing up is done so that the terminating character - will be dispatched on again once the identifier is dealt with. */ - int redo_char = 0; - - /* 1 if within an identifier inside of which a concatenation - marker (Newline -) has been seen. */ - int concatenated = 0; - - /* While scanning a comment or a string constant, - this records the line it started on, for error messages. */ - int start_line; - - /* Record position of last `real' newline. */ - U_CHAR *beg_of_line; - -/* Pop the innermost input stack level, assuming it is a macro expansion. */ - -#define POPMACRO \ -do { ip->macro->type = T_MACRO; \ - if (ip->free_ptr) free (ip->free_ptr); \ - --indepth; } while (0) - -/* Reload `rescan's local variables that describe the current - level of the input stack. */ - -#define RECACHE \ -do { ip = &instack[indepth]; \ - ibp = ip->bufp; \ - limit = ip->buf + ip->length; \ - op->bufp = obp; \ - check_expand (op, limit - ibp); \ - beg_of_line = 0; \ - obp = op->bufp; } while (0) - - if (no_output && instack[indepth].fname != 0) - skip_if_group (&instack[indepth], 1); - - obp = op->bufp; - RECACHE; - beg_of_line = ibp; - - /* Our caller must always put a null after the end of - the input at each input stack level. */ - if (*limit != 0) - abort (); - - while (1) { - c = *ibp++; - *obp++ = c; - - switch (c) - { - case '\\': - if (ibp >= limit) - break; - if (*ibp == '\n') { - /* Always merge lines ending with backslash-newline, - even in middle of identifier. */ - ++ibp; - ++ip->lineno; - --obp; /* remove backslash from obuf */ - break; - } - /* Otherwise, backslash suppresses specialness of following char, - so copy it here to prevent the switch from seeing it. - But first get any pending identifier processed. */ - if (ident_length > 0) - goto specialchar; - *obp++ = *ibp++; - break; - - case '#': - /* If this is expanding a macro definition, don't recognize - preprocessor directives. */ - if (ip->macro != 0) - goto randomchar; - if (ident_length) - goto specialchar; - - /* # keyword: a # must be first nonblank char on the line */ - if (beg_of_line == 0) - goto randomchar; - { - U_CHAR *bp; - - /* Scan from start of line, skipping whitespace, comments - and backslash-newlines, and see if we reach this #. - If not, this # is not special. */ - bp = beg_of_line; - while (1) { - if (is_hor_space[*bp]) - bp++; - else if (*bp == '\\' && bp[1] == '\n') - bp += 2; - else if (*bp == '/' && bp[1] == '*') { - bp += 2; - while (!(*bp == '*' && bp[1] == '/')) - bp++; - bp += 2; - } - else if (cplusplus && *bp == '/' && bp[1] == '/') { - bp += 2; - while (*bp++ != '\n') ; - } - else break; - } - if (bp + 1 != ibp) - goto randomchar; - } - - /* This # can start a directive. */ - - --obp; /* Don't copy the '#' */ - - ip->bufp = ibp; - op->bufp = obp; - if (! handle_directive (ip, op)) { -#ifdef USE_C_ALLOCA - alloca (0); -#endif - /* Not a known directive: treat it as ordinary text. - IP, OP, IBP, etc. have not been changed. */ - if (no_output && instack[indepth].fname) { - /* If not generating expanded output, - what we do with ordinary text is skip it. - Discard everything until next # directive. */ - skip_if_group (&instack[indepth], 1); - RECACHE; - beg_of_line = ibp; - break; - } - ++obp; /* Copy the '#' after all */ - goto randomchar; - } -#ifdef USE_C_ALLOCA - alloca (0); -#endif - /* A # directive has been successfully processed. */ - /* If not generating expanded output, ignore everything until - next # directive. */ - if (no_output && instack[indepth].fname) - skip_if_group (&instack[indepth], 1); - obp = op->bufp; - RECACHE; - beg_of_line = ibp; - break; - - case '{': - //if (beg_of_line == 0) - //goto randomchar; - { - int len; - U_CHAR *bp; -#if 0 - /* Scan from start of line, skipping whitespace, comments - and backslash-newlines, and see if we reach this {. - If not, this { is not special. */ - bp = beg_of_line; - while (1) - { - if (is_hor_space[*bp]) - bp++; - else if (*bp == '\\' && bp[1] == '\n') - bp += 2; - else if (*bp == '/' && bp[1] == '*') - { - bp += 2; - while (!(*bp == '*' && bp[1] == '/')) - bp++; - bp += 2; - } - else if (cplusplus && *bp == '/' && bp[1] == '/') - { - bp += 2; - while (*bp++ != '\n') ; - } - else if (*bp == '{') - break; - else - bp++; - } - if (bp + 1 != ibp) - goto randomchar; -#endif - /* This { can start an expression. */ - - --obp; /* Don't copy the '{' */ - - ip->bufp = ibp; - op->bufp = obp; - inside_math++; - len = handle_c_expression(ip, op); - inside_math--; - ibp = ++ip->bufp; - obp = op->bufp; - } - break; - - case '\"': /* skip quoted string */ - case '\'': - /* A single quoted string is treated like a double -- some - programs (e.g., troff) are perverse this way */ - - if (ident_length) - goto specialchar; - - start_line = ip->lineno; - - /* Skip ahead to a matching quote. */ - - while (1) { - if (ibp >= limit) { - if (traditional) { - if (ip->macro != 0) { - /* try harder: this string crosses a macro expansion boundary */ - POPMACRO; - RECACHE; - continue; - } - } else - error_with_line (line_for_error (start_line), - "unterminated string or character constant"); - break; - } - *obp++ = *ibp; - switch (*ibp++) { - case '\n': - ++ip->lineno; - ++op->lineno; - if (traditional) - goto while2end; - if (pedantic || c == '\'') { - error_with_line (line_for_error (start_line), - "unterminated string or character constant"); - goto while2end; - } - break; - - case '\\': - if (ibp >= limit) - break; - if (*ibp == '\n') { - /* Backslash newline is replaced by nothing at all, - but keep the line counts correct. */ - --obp; - ++ibp; - ++ip->lineno; - } else { - /* ANSI stupidly requires that in \\ the second \ - is *not* prevented from combining with a newline. */ - while (*ibp == '\\' && ibp[1] == '\n') { - ibp += 2; - ++ip->lineno; - } - *obp++ = *ibp++; - } - break; - - case '\"': - case '\'': - if (ibp[-1] == c) - goto while2end; - break; - } - } -while2end: - break; - - case '/': - if (*ibp == '\\' && ibp[1] == '\n') - newline_fix (ibp); - if (cplusplus && *ibp == '/') { - /* C++ style comment... */ - start_line = ip->lineno; - - --ibp; /* Back over the slash */ - --obp; - - /* Comments are equivalent to spaces. */ - if (! put_out_comments) - *obp++ = ' '; - else { - /* must fake up a comment here */ - *obp++ = '/'; - *obp++ = '/'; - } - { - U_CHAR *before_bp = ibp+2; - - while (ibp < limit) { - if (*ibp++ == '\n') { - ibp--; - if (put_out_comments) { - bcopy (before_bp, obp, ibp - before_bp); - obp += ibp - before_bp; - } - break; - } - } - break; - } - } - if (*ibp != '*') - goto randomchar; - if (ip->macro != 0) - goto randomchar; - if (ident_length) - goto specialchar; - - /* We have a comment. Skip it, optionally copying it to output. */ - - start_line = ip->lineno; - - ++ibp; /* Skip the star. */ - - /* Comments are equivalent to spaces. - Note that we already output the slash; we might not want it. - For -traditional, a comment is equivalent to nothing. */ - if (! put_out_comments) { - if (traditional) - obp--; - else - obp[-1] = ' '; - } - else - *obp++ = '*'; - - { - U_CHAR *before_bp = ibp; - - while (ibp < limit) { - switch (*ibp++) { - case '/': - if (warn_comments && ibp < limit && *ibp == '*' -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - && warnings_off == 0 -#endif - ) - warning("`/*' within comment"); - break; - case '*': - if (*ibp == '\\' && ibp[1] == '\n') - newline_fix (ibp); - if (ibp >= limit || *ibp == '/') - goto comment_end; - break; - case '\n': - ++ip->lineno; - /* Copy the newline into the output buffer, in order to - avoid the pain of a #line every time a multiline comment - is seen. */ - if (!put_out_comments) - *obp++ = '\n'; - ++op->lineno; - } - } -comment_end: - - if (ibp >= limit) - error_with_line (line_for_error (start_line), - "unterminated comment"); - else { - ibp++; - if (put_out_comments) { - bcopy (before_bp, obp, ibp - before_bp); - obp += ibp - before_bp; - } - } - } - break; - - case '$': - if (!dollars_in_ident) - goto randomchar; - goto letter; - - case '0': case '1': case '2': case '3': case '4': - case '5': case '6': case '7': case '8': case '9': - /* If digit is not part of identifier, it starts a number, - which means that following letters are not an identifier. - "0x5" does not refer to an identifier "x5". - So copy all alphanumerics that follow without accumulating - as an identifier. Periods also, for sake of "3.e7". */ - - if (ident_length == 0) { - while (ibp < limit) { - c = *ibp++; - if (!isalnum (c) && c != '.' && c != '_') { - --ibp; - break; - } - *obp++ = c; - /* A sign can be part of a preprocessing number - if it follows an e. */ - if (c == 'e' || c == 'E') { - if (ibp < limit && (*ibp == '+' || *ibp == '-')) - *obp++ = *ibp++; - } - } - break; - } - /* fall through */ - - case '_': - case 'a': case 'b': case 'c': case 'd': case 'e': case 'f': - case 'g': case 'h': case 'i': case 'j': case 'k': case 'l': - case 'm': case 'n': case 'o': case 'p': case 'q': case 'r': - case 's': case 't': case 'u': case 'v': case 'w': case 'x': - case 'y': case 'z': - case 'A': case 'B': case 'C': case 'D': case 'E': case 'F': - case 'G': case 'H': case 'I': case 'J': case 'K': case 'L': - case 'M': case 'N': case 'O': case 'P': case 'Q': case 'R': - case 'S': case 'T': case 'U': case 'V': case 'W': case 'X': - case 'Y': case 'Z': - letter: - ident_length++; - /* Compute step of hash function, to avoid a proc call on every token */ - hash = HASHSTEP (hash, c); - break; - - case '\n': - /* If reprocessing a macro expansion, newline is a special marker. */ - if (ip->macro != 0) { - /* Newline White is a "funny space" to separate tokens that are - supposed to be separate but without space between. - Here White means any horizontal whitespace character. - Newline - marks a recursive macro use that is not - supposed to be expandable. */ - - if (*ibp == '-') { - /* Newline - inhibits expansion of preceding token. - If expanding a macro arg, we keep the newline -. - In final output, it is deleted. */ - if (! concatenated) { - ident_length = 0; - hash = 0; - } - ibp++; - if (!output_marks) { - obp--; - } else { - /* If expanding a macro arg, keep the newline -. */ - *obp++ = '-'; - } - } else if (is_space[*ibp]) { - /* Newline Space does not prevent expansion of preceding token - so expand the preceding token and then come back. */ - if (ident_length > 0) - goto specialchar; - - /* If generating final output, newline space makes a space. */ - if (!output_marks) { - obp[-1] = *ibp++; - /* And Newline Newline makes a newline, so count it. */ - if (obp[-1] == '\n') - op->lineno++; - } else { - /* If expanding a macro arg, keep the newline space. - If the arg gets stringified, newline space makes nothing. */ - *obp++ = *ibp++; - } - } else abort (); /* Newline followed by something random? */ - break; - } - - /* If there is a pending identifier, handle it and come back here. */ - if (ident_length > 0) - goto specialchar; - - beg_of_line = ibp; - - /* Update the line counts and output a #line if necessary. */ - ++ip->lineno; - ++op->lineno; - if (ip->lineno != op->lineno) { - op->bufp = obp; - output_line_command (ip, op, 1, same_file); - check_expand (op, ip->length - (ip->bufp - ip->buf)); - obp = op->bufp; - } - break; - - /* Come here either after (1) a null character that is part of the input - or (2) at the end of the input, because there is a null there. */ - case 0: - if (ibp <= limit) - /* Our input really contains a null character. */ - goto randomchar; - - /* At end of a macro-expansion level, pop it and read next level. */ - if (ip->macro != 0) { - obp--; - ibp--; - /* If traditional, and we have an identifier that ends here, - process it now, so we get the right error for recursion. */ - if (traditional && ident_length - && ! is_idchar[*instack[indepth - 1].bufp]) { - redo_char = 1; - goto randomchar; - } - POPMACRO; - RECACHE; - break; - } - - /* If we don't have a pending identifier, - return at end of input. */ - if (ident_length == 0) { - obp--; - ibp--; - op->bufp = obp; - ip->bufp = ibp; - goto ending; - } - - /* If we do have a pending identifier, just consider this null - a special character and arrange to dispatch on it again. - The second time, IDENT_LENGTH will be zero so we will return. */ - - /* Fall through */ - -specialchar: - - /* Handle the case of a character such as /, ', " or null - seen following an identifier. Back over it so that - after the identifier is processed the special char - will be dispatched on again. */ - - ibp--; - obp--; - redo_char = 1; - - default: - -randomchar: - - if (ident_length > 0) { - register HASHNODE *hp; - - /* We have just seen an identifier end. If it's a macro, expand it. - - IDENT_LENGTH is the length of the identifier - and HASH is its hash code. - - The identifier has already been copied to the output, - so if it is a macro we must remove it. - - If REDO_CHAR is 0, the char that terminated the identifier - has been skipped in the output and the input. - OBP-IDENT_LENGTH-1 points to the identifier. - If the identifier is a macro, we must back over the terminator. - - If REDO_CHAR is 1, the terminating char has already been - backed over. OBP-IDENT_LENGTH points to the identifier. */ - - for (hp = hashtab[MAKE_POS (hash) % HASHSIZE]; hp != NULL; - hp = hp->next) { - - if (hp->length == ident_length) { - U_CHAR *obufp_before_macroname; - int op_lineno_before_macroname; - register int i = ident_length; - register U_CHAR *p = hp->name; - register U_CHAR *q = obp - i; - int disabled; - - if (! redo_char) - q--; - - do { /* All this to avoid a strncmp () */ - if (*p++ != *q++) - goto hashcollision; - } while (--i); - - /* We found a use of a macro name. - see if the context shows it is a macro call. */ - - /* Back up over terminating character if not already done. */ - if (! redo_char) { - ibp--; - obp--; - } - - obufp_before_macroname = obp - ident_length; - op_lineno_before_macroname = op->lineno; - - /* Record whether the macro is disabled. */ - disabled = hp->type == T_DISABLED; - - /* This looks like a macro ref, but if the macro was disabled, - just copy its name and put in a marker if requested. */ - - if (disabled) { -#if 0 - /* This error check caught useful cases such as - #define foo(x,y) bar(x(y,0), y) - foo(foo, baz) */ - if (traditional) - error ("recursive use of macro `%s'", hp->name); -#endif - - if (output_marks) { - check_expand (op, limit - ibp + 2); - *obp++ = '\n'; - *obp++ = '-'; - } - break; - } - - /* If macro wants an arglist, verify that a '(' follows. - first skip all whitespace, copying it to the output - after the macro name. Then, if there is no '(', - decide this is not a macro call and leave things that way. */ - if ((hp->type == T_MACRO || hp->type == T_DISABLED) - && hp->value.defn->nargs >= 0) - { - while (1) { - /* Scan forward over whitespace, copying it to the output. */ - if (ibp == limit && ip->macro != 0) { - POPMACRO; - RECACHE; - } - /* A comment: copy it to the output unchanged. */ - else if (*ibp == '/' && ibp+1 != limit && ibp[1] == '*') { - *obp++ = '/'; - *obp++ = '*'; - ibp += 2; - while (ibp + 1 != limit - && !(ibp[0] == '*' && ibp[1] == '/')) { - /* We need not worry about newline-marks, - since they are never found in comments. */ - if (*ibp == '\n') { - /* Newline in a file. Count it. */ - ++ip->lineno; - ++op->lineno; - } - *obp++ = *ibp++; - } - ibp += 2; - *obp++ = '*'; - *obp++ = '/'; - } - else if (is_space[*ibp]) { - *obp++ = *ibp++; - if (ibp[-1] == '\n') { - if (ip->macro == 0) { - /* Newline in a file. Count it. */ - ++ip->lineno; - ++op->lineno; - } else if (!output_marks) { - /* A newline mark, and we don't want marks - in the output. If it is newline-hyphen, - discard it entirely. Otherwise, it is - newline-whitechar, so keep the whitechar. */ - obp--; - if (*ibp == '-') - ibp++; - else { - if (*ibp == '\n') - ++op->lineno; - *obp++ = *ibp++; - } - } else { - /* A newline mark; copy both chars to the output. */ - *obp++ = *ibp++; - } - } - } - else break; - } - if (*ibp != '(') - break; - } - - /* This is now known to be a macro call. - Discard the macro name from the output, - along with any following whitespace just copied. */ - obp = obufp_before_macroname; - op->lineno = op_lineno_before_macroname; - - /* Expand the macro, reading arguments as needed, - and push the expansion on the input stack. */ - ip->bufp = ibp; - op->bufp = obp; - macroexpand (hp, op); - - /* Reexamine input stack, since macroexpand has pushed - a new level on it. */ - obp = op->bufp; - RECACHE; - break; - } -hashcollision: - ; - } /* End hash-table-search loop */ - ident_length = hash = 0; /* Stop collecting identifier */ - redo_char = 0; - concatenated = 0; - } /* End if (ident_length > 0) */ - } /* End switch */ - } /* End per-char loop */ - - /* Come here to return -- but first give an error message - if there was an unterminated successful conditional. */ - ending: - if (if_stack != ip->if_stack) { - char *str; - switch (if_stack->type) { - case T_IF: - str = "if"; - break; - case T_IFDEF: - str = "ifdef"; - break; - case T_IFNDEF: - str = "ifndef"; - break; - case T_ELSE: - str = "else"; - break; - case T_ELIF: - str = "elif"; - break; - } - error_with_line (line_for_error (if_stack->lineno), - "unterminated #%s conditional", str); - } - if_stack = ip->if_stack; -} - -/* - * Rescan a string into a temporary buffer and return the result - * as a FILE_BUF. Note this function returns a struct, not a pointer. - * - * OUTPUT_MARKS nonzero means keep Newline markers found in the input - * and insert such markers when appropriate. See `rescan' for details. - * OUTPUT_MARKS is 1 for macroexpanding a macro argument separately - * before substitution; it is 0 for other uses. - */ -FILE_BUF -expand_to_temp_buffer (buf, limit, output_marks) - U_CHAR *buf, *limit; - int output_marks; -{ - register FILE_BUF *ip; - FILE_BUF obuf; - int length = limit - buf; - U_CHAR *buf1; - int odepth = indepth; - - if (length < 0) - abort (); - - /* Set up the input on the input stack. */ - - buf1 = (U_CHAR *) alloca (length + 1); - { - register U_CHAR *p1 = buf; - register U_CHAR *p2 = buf1; - - while (p1 != limit) - *p2++ = *p1++; - } - buf1[length] = 0; - - /* Set up to receive the output. */ - - obuf.length = length * 2 + 100; /* Usually enough. Why be stingy? */ - obuf.bufp = obuf.buf = (U_CHAR *) xmalloc (obuf.length); - obuf.fname = 0; - obuf.macro = 0; - obuf.free_ptr = 0; - - CHECK_DEPTH ({return obuf;}); - - ++indepth; - - ip = &instack[indepth]; -#ifdef sgi - ip->endif_cruft_message_issued = 0; -#endif - ip->fname = 0; - ip->macro = 0; - ip->free_ptr = 0; - ip->length = length; - ip->buf = ip->bufp = buf1; - ip->if_stack = if_stack; - - ip->lineno = obuf.lineno = 1; - - /* Scan the input, create the output. */ - - rescan (&obuf, output_marks); - - /* Pop input stack to original state. */ - --indepth; - - if (indepth != odepth) - abort (); - - /* Record the output. */ - obuf.length = obuf.bufp - obuf.buf; - - return obuf; -} - -/* - * Process a # directive. Expects IP->bufp to point to the '#', as in - * `#define foo bar'. Passes to the command handler - * (do_define, do_include, etc.): the addresses of the 1st and - * last chars of the command (starting immediately after the # - * keyword), plus op and the keyword table pointer. If the command - * contains comments it is copied into a temporary buffer sans comments - * and the temporary buffer is passed to the command handler instead. - * Likewise for backslash-newlines. - * - * Returns nonzero if this was a known # directive. - * Otherwise, returns zero, without advancing the input pointer. - */ - -int -handle_directive (ip, op) - FILE_BUF *ip, *op; -{ - register U_CHAR *bp, *cp; - register struct directive *kt; - register int ident_length; - U_CHAR *resume_p; - - /* Nonzero means we must copy the entire command - to get rid of comments or backslash-newlines. */ - int copy_command = 0; - - U_CHAR *ident, *after_ident; - - bp = ip->bufp; - /* Skip whitespace and \-newline. */ - while (1) { - if (is_hor_space[*bp]) - bp++; - else if (*bp == '/' && bp[1] == '*') { - ip->bufp = bp; - skip_to_end_of_comment (ip, &ip->lineno); - bp = ip->bufp; - } else if (*bp == '\\' && bp[1] == '\n') { - bp += 2; ip->lineno++; - } else break; - } - - /* Now find end of directive name. - If we encounter a backslash-newline, exchange it with any following - symbol-constituents so that we end up with a contiguous name. */ - - cp = bp; - while (1) { - if (is_idchar[*cp]) - cp++; - else { - if (*cp == '\\' && cp[1] == '\n') - name_newline_fix (cp); - if (is_idchar[*cp]) - cp++; - else break; - } - } - ident_length = cp - bp; - ident = bp; - after_ident = cp; - - /* A line of just `#' becomes blank. */ - - if (traditional && ident_length == 0 && *after_ident == '\n') { - ip->bufp = after_ident; - return 1; - } -#ifdef sgi - /* A line of just `#' becomes blank. */ - if (ident_length == 0 && *after_ident == '\n') { - ip->bufp = after_ident; - return 1; - } -#endif - - /* - * Decode the keyword and call the appropriate expansion - * routine, after moving the input pointer up to the next line. - */ - - for (kt = directive_table; kt->length > 0; kt++) { - if (kt->length == ident_length && !strncmp (kt->name, ident, ident_length)) { - register U_CHAR *buf; - register U_CHAR *limit = ip->buf + ip->length; - int unterminated = 0; - - /* Nonzero means do not delete comments within the directive. - #define needs this when -traditional. */ - int keep_comments = traditional && kt->traditional_comments; - - /* Find the end of this command (first newline not backslashed - and not in a string or comment). - Set COPY_COMMAND if the command must be copied - (it contains a backslash-newline or a comment). */ - - buf = bp = after_ident; - while (bp < limit) - { - register U_CHAR c = *bp++; - switch (c) { - case '\\': - if (bp < limit) { - if (*bp == '\n') { - ip->lineno++; - copy_command = 1; - } - bp++; - } - break; - - case '\'': - case '\"': - bp = skip_quoted_string (bp - 1, limit, ip->lineno, &ip->lineno, ©_command, &unterminated); - /* Don't bother calling the directive if we already got an error - message due to unterminated string. Skip everything and pretend - we called the directive. */ - if (unterminated) { - if (traditional) { - /* Traditional preprocessing permits unterminated strings. */ - --bp; - ip->bufp = bp; - goto endloop1; - } - ip->bufp = bp; - return 1; - } - break; - - /* <...> is special for #include. */ - case '<': - if (!kt->angle_brackets) - break; - while (*bp && *bp != '>') bp++; - break; - - case '/': - if (*bp == '\\' && bp[1] == '\n') - newline_fix (bp); - if (*bp == '*' - || (cplusplus && *bp == '/')) { - U_CHAR *obp = bp - 1; - ip->bufp = bp + 1; - skip_to_end_of_comment (ip, &ip->lineno); - bp = ip->bufp; - /* No need to copy the command because of a comment at the end; - just don't include the comment in the directive. */ - if (bp == limit || *bp == '\n') { - bp = obp; - goto endloop1; - } - /* Don't remove the comments if -traditional. */ - if (! keep_comments) - copy_command++; - } - break; - - case '\n': - --bp; /* Point to the newline */ - ip->bufp = bp; - goto endloop1; - } - } - ip->bufp = bp; - - endloop1: - resume_p = ip->bufp; - /* BP is the end of the directive. - RESUME_P is the next interesting data after the directive. - A comment may come between. */ - - if (copy_command) { - register U_CHAR *xp = buf; - /* Need to copy entire command into temp buffer before dispatching */ - - cp = (U_CHAR *) alloca (bp - buf + 5); /* room for cmd plus - some slop */ - buf = cp; - - /* Copy to the new buffer, deleting comments - and backslash-newlines (and whitespace surrounding the latter). */ - - while (xp < bp) { - register U_CHAR c = *xp++; - *cp++ = c; - - switch (c) { - case '\n': - break; - - /* <...> is special for #include. */ - case '<': - if (!kt->angle_brackets) - break; - while (xp < bp && c != '>') { - c = *xp++; - *cp++ = c; - } - break; - - case '\\': - if (*xp == '\n') { - xp++; - cp--; -#if sgi - if (no_newline_splicing) { - *cp++ = '\n'; - *cp++ = '\n'; - break; - } -#endif - if (cp != buf && is_space[cp[-1]]) { - while (cp != buf && is_space[cp[-1]]) cp--; - cp++; - SKIP_WHITE_SPACE (xp); - } else if (is_space[*xp]) { - *cp++ = *xp++; - SKIP_WHITE_SPACE (xp); - } - } - break; - - case '\'': - case '\"': - if (!traditional) { - register U_CHAR *bp1 - = skip_quoted_string (xp - 1, limit, ip->lineno, 0, 0, 0); - while (xp != bp1) - *cp++ = *xp++; - } - break; - - case '/': - if (keep_comments) - break; - if (*xp == '*' - || (cplusplus && *xp == '/')) { - if (traditional) - cp--; - else - cp[-1] = ' '; - ip->bufp = xp + 1; - skip_to_end_of_comment (ip, 0); - xp = ip->bufp; - } - } - } - - /* Null-terminate the copy. */ - - *cp = 0; - } - else - cp = bp; - - ip->bufp = resume_p; - - /* Some directives should be written out for cc1 to process, - just as if they were not defined. */ - - if (kt->pass_thru) { - int len; - - /* Output directive name. */ - check_expand (op, kt->length+1); - *op->bufp++ = '#'; - bcopy (kt->name, op->bufp, kt->length); - op->bufp += kt->length; - - /* Output arguments. */ - len = (cp - buf); - check_expand (op, len); - bcopy (buf, op->bufp, len); - op->bufp += len; - } - - /* Call the appropriate command handler. buf now points to - either the appropriate place in the input buffer, or to - the temp buffer if it was necessary to make one. cp - points to the first char after the contents of the (possibly - copied) command, in either case. */ - (*kt->func) (buf, cp, op, kt); - check_expand (op, ip->length - (ip->bufp - ip->buf)); - - return 1; - } - } -#ifdef sgi - if(pedantic && warnings_off == 0) - warning("Not a legal preprocessing directive; inappropriate non-whitespace characters are present (ANSI 3.8)"); -#endif - return 0; -} - -/* - * Process a # directive. Expects IP->bufp to point to the '#', as in - * `#define foo bar'. Passes to the command handler - * (do_define, do_include, etc.): the addresses of the 1st and - * last chars of the command (starting immediately after the # - * keyword), plus op and the keyword table pointer. If the command - * contains comments it is copied into a temporary buffer sans comments - * and the temporary buffer is passed to the command handler instead. - * Likewise for backslash-newlines. - * - * Returns nonzero if this was a known # directive. - * Otherwise, returns zero, without advancing the input pointer. - */ - -int -handle_c_expression (ip, op) - FILE_BUF *ip, *op; -{ - register U_CHAR *bp, *cp; - register int ident_length; - U_CHAR *resume_p; - - /* Nonzero means we must copy the entire command - to get rid of comments or backslash-newlines. */ - int copy_command = 1; - - U_CHAR *ident, *after_ident; - - bp = ip->bufp; - /* Skip whitespace and \-newline. */ - while (1) { - if (is_hor_space[*bp]) - bp++; - else if (*bp == '/' && bp[1] == '*') { - ip->bufp = bp; - skip_to_end_of_comment (ip, &ip->lineno); - bp = ip->bufp; - } else if (*bp == '\\' && bp[1] == '\n') { - bp += 2; ip->lineno++; - } else break; - } - - /* Now find end of directive name. - If we encounter a backslash-newline, exchange it with any following - symbol-constituents so that we end up with a contiguous name. */ - - cp = bp; -#if 0 - while (1) { - if (is_idchar[*cp]) - cp++; - else { - if (*cp == '\\' && cp[1] == '\n') - name_newline_fix (cp); - if (is_idchar[*cp]) - cp++; - else break; - } - } -#endif - - ident_length = cp - bp; - ident = bp; - after_ident = cp; - -#if 0 - /* A line of just `#' becomes blank. */ - - if (traditional && ident_length == 0 && *after_ident == '\n') { - ip->bufp = after_ident; - return 1; - } -#ifdef sgi - /* A line of just `#' becomes blank. */ - if (ident_length == 0 && *after_ident == '\n') { - ip->bufp = after_ident; - return 1; - } -#endif - -#endif - - /* - * Decode the keyword and call the appropriate expansion - * routine, after moving the input pointer up to the next line. - */ - - { - register U_CHAR *buf; - register U_CHAR *limit = ip->buf + ip->length; - int unterminated = 0; - - /* Nonzero means do not delete comments within the directive. - #define needs this when -traditional. */ - int keep_comments = traditional; - - /* Find the end of this command (first newline not backslashed - and not in a string or comment). - Set COPY_COMMAND if the command must be copied - (it contains a backslash-newline or a comment). */ - - buf = bp = after_ident; - while (bp < limit) - { - register U_CHAR c = *bp++; - switch (c) - { - case '\\': - if (bp < limit) { - if (*bp == '\n') { - ip->lineno++; - copy_command = 1; - } - bp++; - } - break; - - case '\'': - case '\"': - bp = skip_quoted_string (bp - 1, limit, ip->lineno, &ip->lineno, ©_command, &unterminated); - /* Don't bother calling the directive if we already got an error - message due to unterminated string. Skip everything and pretend - we called the directive. */ - if (unterminated) { - if (traditional) { - /* Traditional preprocessing permits unterminated strings. */ - --bp; - ip->bufp = bp; - goto endloop1; - } - ip->bufp = bp; - return 1; - } - break; - - case '/': - if (*bp == '\\' && bp[1] == '\n') - newline_fix (bp); - if (*bp == '*' - || (cplusplus && *bp == '/')) { - U_CHAR *obp = bp - 1; - ip->bufp = bp + 1; - skip_to_end_of_comment (ip, &ip->lineno); - bp = ip->bufp; - /* No need to copy the command because of a comment at the end; - just don't include the comment in the directive. */ - if (bp == limit || *bp == '\n') { - bp = obp; - goto endloop1; - } - /* Don't remove the comments if -traditional. */ - if (! keep_comments) - copy_command++; - } - break; - - case '}': - --bp; /* Point to the newline */ - ip->bufp = bp; - goto endloop1; - } - } - ip->bufp = bp; - -endloop1: - resume_p = ip->bufp; - /* BP is the end of the directive. - RESUME_P is the next interesting data after the directive. - A comment may come between. */ - - if (copy_command) - { - register U_CHAR *xp = buf; - /* Need to copy entire command into temp buffer before dispatching */ - - cp = (U_CHAR *) alloca (bp - buf + 5); /* room for cmd plus - some slop */ - buf = cp; - - /* Copy to the new buffer, deleting comments - and backslash-newlines (and whitespace surrounding the latter). */ - - while (xp < bp) - { - register U_CHAR c = *xp++; - *cp++ = c; - - switch (c) { - case '\n': - break; - - case '\\': - if (*xp == '\n') { - xp++; - cp--; -#if sgi - if (no_newline_splicing) { - *cp++ = '\n'; - *cp++ = '\n'; - break; - } -#endif - if (cp != buf && is_space[cp[-1]]) { - while (cp != buf && is_space[cp[-1]]) cp--; - cp++; - SKIP_WHITE_SPACE (xp); - } else if (is_space[*xp]) { - *cp++ = *xp++; - SKIP_WHITE_SPACE (xp); - } - } - break; - - case '\'': - case '\"': - if (!traditional) { - register U_CHAR *bp1 - = skip_quoted_string (xp - 1, limit, ip->lineno, 0, 0, 0); - while (xp != bp1) - *cp++ = *xp++; - } - break; - - case '/': - if (keep_comments) - break; - if (*xp == '*' - || (cplusplus && *xp == '/')) { - if (traditional) - cp--; - else - cp[-1] = ' '; - ip->bufp = xp + 1; - skip_to_end_of_comment (ip, 0); - xp = ip->bufp; - } - } - } - - /* Null-terminate the copy. */ - - *cp = 0; - } - else - { - cp = bp; - } - - ip->bufp = resume_p; - - /* Some directives should be written out for cc1 to process, - just as if they were not defined. */ - - /* buf now points to the beginning of the expression, - * and is null terminated after the end of the expression. - */ - { - int len; - double val; - char obuf[512]; - FILE_BUF temp_obuf; - - len = strlen(buf); - temp_obuf = expand_to_temp_buffer (buf, buf + len, 0); - val = parse_c_math_expression(temp_obuf.buf); - sprintf(obuf, "%lf", val); - len = strlen(obuf); - bcopy(obuf, op->bufp, len); - op->bufp += len; - } - check_expand (op, ip->length - (ip->bufp - ip->buf)); - - return 1; - } - -#ifdef sgi - if(pedantic && warnings_off == 0) - warning("Not a legal preprocessing directive; inappropriate non-whitespace characters are present (ANSI 3.8)"); -#endif - - return 0; -} - - -static char *monthnames[] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", - "Jul", "Aug", "Sep", "Oct", "Nov", "Dec", - }; - -static const char* _get_filename_without_path (const char* filename) -{ -#if defined WIN32 || defined _WIN32 || defined __WIN32 || defined __WIN32__ - #define DIR_SEP_CHAR '\\' -#else - #define DIR_SEP_CHAR '/' -#endif - - const char* p = strrchr(filename, DIR_SEP_CHAR); - - return p ? ++p : filename; -} - -/* - * error - print error message and increment count of errors. - */ -static error (msg, arg1, arg2, arg3) - char *msg; -{ - int i; - FILE_BUF *ip = NULL; - - for (i = indepth; i >= 0; i--) - if (instack[i].fname != NULL) { - ip = &instack[i]; - break; - } - - if (ip != NULL) - USER_MESSAGE("%s:%d: ", _get_filename_without_path(ip->fname), ip->lineno); - USER_MESSAGE(msg, arg1, arg2, arg3); - USER_MESSAGE("\n"); - errors++; - return 0; -} - -/* - * expand things like __FILE__. Place the expansion into the output - * buffer *without* rescanning. - */ -void special_symbol (hp, op) - HASHNODE *hp; - FILE_BUF *op; -{ - char *buf; - long t; - int i, len; - int true_indepth; - FILE_BUF *ip = NULL; - static struct tm *timebuf = NULL; -#ifndef sgi - struct tm *localtime (); -#endif - - int paren = 0; /* For special `defined' keyword */ - - for (i = indepth; i >= 0; i--) - if (instack[i].fname != NULL) { - ip = &instack[i]; - break; - } - if (ip == NULL) { - error ("cccp error: not in any file?!"); - return; /* the show must go on */ - } - - switch (hp->type) { - case T_FILE: - case T_BASE_FILE: - { - char *string; - if (hp->type == T_FILE) - string = ip->fname; - else - string = instack[0].fname; - - if (string) - { - buf = (char *) alloca (3 + strlen (string)); - sprintf (buf, "\"%s\"", string); - } - else - buf = "\"\""; - - break; - } - - case T_INCLUDE_LEVEL: - true_indepth = 0; - for (i = indepth; i >= 0; i--) - if (instack[i].fname != NULL) - true_indepth++; - - buf = (char *) alloca (8); /* Eigth bytes ought to be more than enough */ - sprintf (buf, "%d", true_indepth - 1); - break; - - case T_VERSION: - buf = (char *) alloca (3 + strlen (version_string)); - sprintf (buf, "\"%s\"", version_string); - break; - - case T_CONST: - buf = (char *) alloca (4 * sizeof (int)); - sprintf (buf, "%d", hp->value.ival); - break; - - case T_SPECLINE: - buf = (char *) alloca (10); - sprintf (buf, "%d", ip->lineno); - break; - - case T_DATE: - case T_TIME: - if (timebuf == NULL) { - t = time (0); - timebuf = localtime (&t); - } - buf = (char *) alloca (20); - if (hp->type == T_DATE) - sprintf (buf, "\"%s %2d %4d\"", monthnames[timebuf->tm_mon], - timebuf->tm_mday, timebuf->tm_year + 1900); - else - sprintf (buf, "\"%02d:%02d:%02d\"", timebuf->tm_hour, timebuf->tm_min, - timebuf->tm_sec); - break; - - case T_SPEC_DEFINED: - buf = " 0 "; /* Assume symbol is not defined */ - ip = &instack[indepth]; - SKIP_WHITE_SPACE (ip->bufp); - if (*ip->bufp == '(') { - paren++; - ip->bufp++; /* Skip over the paren */ - SKIP_WHITE_SPACE (ip->bufp); - } - - if (!is_idstart[*ip->bufp]) - goto oops; - if (lookup (ip->bufp, -1, -1)) - buf = " 1 "; - while (is_idchar[*ip->bufp]) - ++ip->bufp; - SKIP_WHITE_SPACE (ip->bufp); - if (paren) { - if (*ip->bufp != ')') - goto oops; - ++ip->bufp; - } - break; - -oops: - - error ("`defined' must be followed by ident or (ident)"); - break; - - default: - error ("cccp error: invalid special hash type"); /* time for gdb */ - abort (); - } - len = strlen (buf); - check_expand (op, len); - bcopy (buf, op->bufp, len); - op->bufp += len; - - return; -} - - -/* Routines to handle #directives */ - -/* - * Process include file by reading it in and calling rescan. - * Expects to see "fname" or on the input. - */ - -do_include (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - char *fname; /* Dynamically allocated fname buffer */ - U_CHAR *fbeg, *fend; /* Beginning and end of fname */ - - struct file_name_list *stackp = include; /* Chain of dirs to search */ - struct file_name_list dsp[1]; /* First in chain, if #include "..." */ - int flen; - - int f; /* file number */ - - int retried = 0; /* Have already tried macro - expanding the include line*/ - FILE_BUF trybuf; /* It got expanded into here */ - int system_header_p = 0; /* 0 for "...", 1 for <...> */ - - f= -1; /* JF we iz paranoid! */ - -get_filename: - - fbeg = buf; - SKIP_WHITE_SPACE (fbeg); - /* Discard trailing whitespace so we can easily see - if we have parsed all the significant chars we were given. */ - while (limit != fbeg && is_hor_space[limit[-1]]) limit--; - - switch (*fbeg++) { - case '\"': - fend = fbeg; - while (fend != limit && *fend != '\"') - fend++; - if (*fend == '\"' && fend + 1 == limit) { - FILE_BUF *fp; - - /* We have "filename". Figure out directory this source - file is coming from and put it on the front of the list. */ - - /* If -I- was specified, don't search current dir, only spec'd ones. */ - if (ignore_srcdir) - break; - - for (fp = &instack[indepth]; fp >= instack; fp--) - { - int n; - char *ep,*nam; -#ifndef sgi - extern char *rindex (); -#endif - - if ((nam = fp->fname) != NULL) { - /* Found a named file. Figure out dir of the file, - and put it in front of the search list. */ - dsp[0].next = stackp; - stackp = dsp; -#ifndef VMS - ep = (char*)rindex (nam, '/'); -#else /* VMS */ - ep = rindex (nam, ']'); - if (ep == NULL) ep = rindex (nam, '>'); - if (ep == NULL) ep = rindex (nam, ':'); - if (ep != NULL) ep++; -#endif /* VMS */ - if (ep != NULL) { - n = ep - nam; - dsp[0].fname = (char *) alloca (n + 1); - strncpy (dsp[0].fname, nam, n); - dsp[0].fname[n] = '\0'; - if (n > max_include_len) max_include_len = n; - } else { - dsp[0].fname = 0; /* Current directory */ - } - break; - } - } - break; - } - goto fail; - - case '<': - fend = fbeg; - while (fend != limit && *fend != '>') fend++; - if (*fend == '>' && fend + 1 == limit) { - system_header_p = 1; - /* If -I-, start with the first -I dir after the -I-. */ - if (first_bracket_include) - stackp = first_bracket_include; - break; - } - goto fail; - - default: - fail: - if (retried) { - error ("#include expects \"fname\" or "); - return 1; - } else { - trybuf = expand_to_temp_buffer (buf, limit, 0); - buf = (U_CHAR *) alloca (trybuf.bufp - trybuf.buf + 1); - bcopy (trybuf.buf, buf, trybuf.bufp - trybuf.buf); - limit = buf + (trybuf.bufp - trybuf.buf); - free (trybuf.buf); - retried++; - goto get_filename; - } - } - - flen = fend - fbeg; - fname = (char *) alloca (max_include_len + flen + 2); - /* + 2 above for slash and terminating null. */ - - /* If specified file name is absolute, just open it. */ - - if (*fbeg == '/') { - strncpy (fname, fbeg, flen); - fname[flen] = 0; - f = _OPEN (fname, O_RDONLY, 0666); - } else { - /* Search directory path, trying to open the file. - Copy each filename tried into FNAME. */ - - for (; stackp; stackp = stackp->next) { - if (stackp->fname) { - strcpy (fname, stackp->fname); -#ifdef WIN32 - if (fname[strlen(fname) - 1] != '\\') -#endif - strcat (fname, "/"); - fname[strlen (fname) + flen] = 0; - } else { - fname[0] = 0; - } - strncat (fname, fbeg, flen); -#ifdef VMS - /* Change this 1/2 Unix 1/2 VMS file specification into a - full VMS file specification */ - if (stackp->fname && (stackp->fname[0] != 0)) { - /* Fix up the filename */ - hack_vms_include_specification (fname); - } else { - /* This is a normal VMS filespec, so use it unchanged. */ - strncpy (fname, fbeg, flen); - fname[flen] = 0; - } -#endif /* VMS */ -#if 0 - USER_MESSAGE("acpp include: %s\n", fname); -#endif - if ((f = _OPEN (fname, O_RDONLY, 0666)) >= 0) - break; - } - } - - if (f < 0) { - strncpy (fname, fbeg, flen); - fname[flen] = 0; - error_from_errno (fname); - /* For -M, add this file to the dependencies. */ - if (print_deps > (system_header_p || (system_include_depth > 0))) { - if (system_header_p) { -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - if(warnings_off == 0) -#endif - warning ("nonexistent file <%.*s> omitted from dependency output", - fend - fbeg, fbeg); - } - else - { -#ifdef sgi - if(deps_prefix) { - deps_output("\n",1); - deps_output(deps_prefix,strlen(deps_prefix)); - } -#endif - deps_output (fbeg, fend - fbeg); - deps_output (" ", 0); - } - } - } else { - - /* Check to see if this include file is a once-only include file. - If so, give up. */ - - struct file_name_list* ptr; - - for (ptr = dont_repeat_files; ptr; ptr = ptr->next) { - if (!strcmp (ptr->fname, fname)) { - close (f); - return 1; /* This file was once'd. */ - } - } - - for (ptr = all_include_files; ptr; ptr = ptr->next) { - if (!strcmp (ptr->fname, fname)) - break; /* This file was included before. */ - } - - if (ptr == 0) { - /* This is the first time for this file. */ - /* Add it to list of files included. */ - - ptr = (struct file_name_list *) xmalloc (sizeof (struct file_name_list)); - ptr->next = all_include_files; - all_include_files = ptr; - ptr->fname = savestring (fname); - - /* For -M, add this file to the dependencies. */ - if (print_deps > (system_header_p || (system_include_depth > 0))) { -#ifdef sgi - if(deps_prefix) { - deps_output("\n",1); - deps_output(deps_prefix,strlen(deps_prefix)); - } -#endif - deps_output (fname, strlen (fname)); - deps_output (" ", 0); - } -#ifdef sgi - if (deps_file) - { - printf("****************No MDupdate***************\n"); -/* MDupdate(deps_file, ptr->fname);*/ - } -#endif - } - - if (system_header_p) - system_include_depth++; - - /* Actually process the file. */ - finclude (f, fname, op); - - if (system_header_p) - system_include_depth--; - - close (f); - } -} - -/* Process the contents of include file FNAME, already open on descriptor F, - with output to OP. */ - -finclude (f, fname, op) - int f; - char *fname; - FILE_BUF *op; -{ - int st_mode; - long st_size; - long i; - FILE_BUF *fp; /* For input stack frame */ - int success = 0; - - CHECK_DEPTH (return 1;); - -#ifdef DWARF_DM - { - FILE_BUF *ip; - ip = &instack[indepth]; - dm_push_new(fname,ip->lineno); - } -#endif - if (file_size_and_mode (f, &st_mode, &st_size) < 0) - goto nope; /* Impossible? */ - - fp = &instack[indepth + 1]; - bzero (fp, sizeof (FILE_BUF)); - fp->fname = fname; - fp->length = 0; - fp->lineno = 1; - fp->if_stack = if_stack; - - if (st_mode & S_IFREG) { - fp->buf = (U_CHAR *) alloca (st_size + 2); - fp->bufp = fp->buf; - - /* Read the file contents, knowing that st_size is an upper bound - on the number of bytes we can read. */ - while (st_size > 0) { - i = read (f, fp->buf + fp->length, st_size); - if (i <= 0) { - if (i == 0) break; - goto nope; - } - fp->length += i; - st_size -= i; - } - } - else { - /* Cannot count its file size before reading. - First read the entire file into heap and - copy them into buffer on stack. */ - - U_CHAR *bufp; - U_CHAR *basep; - int bsize = 2000; - - st_size = 0; - basep = (U_CHAR *) xmalloc (bsize + 2); - bufp = basep; - - for (;;) { - i = read (f, bufp, bsize - st_size); - if (i < 0) - goto nope; /* error! */ - if (i == 0) - break; /* End of file */ - st_size += i; - bufp += i; - if (bsize == st_size) { /* Buffer is full! */ - bsize *= 2; - basep = (U_CHAR *) xrealloc (basep, bsize + 2); - bufp = basep + st_size; /* May have moved */ - } - } - fp->buf = (U_CHAR *) alloca (st_size + 2); - fp->bufp = fp->buf; - bcopy (basep, fp->buf, st_size); - fp->length = st_size; - free (basep); - } - - if (!no_trigraphs) - trigraph_pcp (fp); - - if (fp->length > 0 && fp->buf[fp->length-1] != '\n') - fp->buf[fp->length++] = '\n'; - fp->buf[fp->length] = '\0'; - - success = 1; - indepth++; - - output_line_command (fp, op, 0, enter_file); - rescan (op, 0); - indepth--; - output_line_command (&instack[indepth], op, 0, leave_file); - -nope: -#ifdef DWARF_DM - { - dm_finish_current(); - } -#endif - - if (!success) - perror_with_name (fname); - - close (f); -} - -/* The arglist structure is built by do_define to tell - collect_definition where the argument names begin. That - is, for a define like "#define f(x,y,z) foo+x-bar*y", the arglist - would contain pointers to the strings x, y, and z. - Collect_definition would then build a DEFINITION node, - with reflist nodes pointing to the places x, y, and z had - appeared. So the arglist is just convenience data passed - between these two routines. It is not kept around after - the current #define has been processed and entered into the - hash table. */ - -struct arglist { - struct arglist *next; - U_CHAR *name; - int length; - int argno; -#ifdef sgi - int duplicate_warned; -#endif -}; - -/* Process a #define command. -BUF points to the contents of the #define command, as a continguous string. -LIMIT points to the first character past the end of the definition. -KEYWORD is the keyword-table entry for #define. */ - -do_define (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - U_CHAR *bp; /* temp ptr into input buffer */ - U_CHAR *symname; /* remember where symbol name starts */ - int sym_length; /* and how long it is */ -#ifdef DWARF_DM - U_CHAR *end_name; - FILE_BUF *ip; -#endif - - DEFINITION *defn; - int arglengths = 0; /* Accumulate lengths of arg names - plus number of args. */ - int hashcode; - - bp = buf; - - while (is_hor_space[*bp]) - bp++; - if (!is_idstart[*bp]) -#ifdef sgi - if(*bp == 0 || *bp == '\n') { - error ("#define has no identifier (no macro name is present)"); - } else { - if(warnings_off == 0) - warning ("macro name in #define starts with an inappropriate character"); - } -#else - warning ("macro name starts with a digit"); -#endif - - symname = bp; /* remember where it starts */ - while (is_idchar[*bp] && bp < limit) { - bp++; - } - sym_length = bp - symname; - -#ifdef sgi - if(showdefines) { - print_def_undef(op,"define",symname,sym_length); - } -#endif - - /* lossage will occur if identifiers or control keywords are broken - across lines using backslash. This is not the right place to take - care of that. */ - - if (*bp == '(') { - struct arglist *arg_ptrs = NULL; - int argno = 0; - - bp++; /* skip '(' */ - SKIP_WHITE_SPACE (bp); - - /* Loop over macro argument names. */ - while (*bp != ')') { - struct arglist *temp; - - temp = (struct arglist *) alloca (sizeof (struct arglist)); - temp->name = bp; - temp->next = arg_ptrs; - temp->argno = argno++; -#ifdef sgi - temp->duplicate_warned = 0; -#endif - arg_ptrs = temp; - - if (!is_idstart[*bp] -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - && warnings_off == 0 -#endif - ) - warning ("parameter name starts with a digit in #define"); - - /* Find the end of the arg name. */ - while (is_idchar[*bp]) { - bp++; - } - temp->length = bp - temp->name; - arglengths += temp->length + 2; - SKIP_WHITE_SPACE (bp); - if (temp->length == 0 || (*bp != ',' && *bp != ')')) { - error ("badly punctuated parameter list in #define"); - goto nope; - } - if (*bp == ',') { - bp++; - SKIP_WHITE_SPACE (bp); - } - if (bp >= limit) { - error ("unterminated parameter list in #define"); - goto nope; - } -#ifdef sgi - /* determine if a name is repeated. 3.8.3 page 90 line 16.*/ - /* This is n**2 in number of macro paramaters. Ugh. */ - { - struct arglist *atemp; - if(pedantic) - for(atemp = arg_ptrs->next; atemp; atemp = atemp->next) { - if(arg_ptrs->length == atemp->length & - atemp->duplicate_warned == 0 && - strncmp(arg_ptrs->name,atemp->name,atemp->length) == 0) { - U_CHAR *str; - str = (U_CHAR *)xmalloc(atemp->length+1); - strncpy(str,atemp->name,atemp->length); - str[atemp->length] = 0; - if(warnings_off == 0) - warning("duplicate macro parameter (%s) is illegal (ANSI 3.8.3)",str); - free(str); - atemp->duplicate_warned=1; - } - } - } -#endif - } -#ifdef DWARF_DM - end_name = bp; -#endif - - ++bp; /* skip paren */ - /* Skip exactly one space or tab if any. */ -#ifdef DWARF_DM - ip = &instack[indepth]; - dm_add_define(symname,bp - symname, bp,limit - bp,ip->lineno); -#endif - if (bp < limit && (*bp == ' ' || *bp == '\t')) ++bp; - /* now everything from bp before limit is the definition. */ - defn = collect_expansion (bp, limit, argno, arg_ptrs); - - /* Now set defn->argnames to the result of concatenating - the argument names in reverse order - with comma-space between them. */ - defn->argnames = (U_CHAR *) xmalloc (arglengths + 1); - { - struct arglist *temp; - int i = 0; - for (temp = arg_ptrs; temp; temp = temp->next) { - bcopy (temp->name, &defn->argnames[i], temp->length); - i += temp->length; - if (temp->next != 0) { - defn->argnames[i++] = ','; - defn->argnames[i++] = ' '; - } - } - defn->argnames[i] = 0; - } - } else { - /* simple expansion or empty definition; gobble it */ - if (is_hor_space[*bp]) - ++bp; /* skip exactly one blank/tab char */ - /* now everything from bp before limit is the definition. */ -#ifdef DWARF_DM - ip = &instack[indepth]; - dm_add_define(symname,sym_length, bp,limit - bp,ip->lineno); -#endif - defn = collect_expansion (bp, limit, -1, 0); - defn->argnames = (U_CHAR *) ""; - } - - hashcode = hashf (symname, sym_length, HASHSIZE); - - { - HASHNODE *hp; - if ((hp = lookup (symname, sym_length, hashcode)) != NULL) { - if (hp->type != T_MACRO - || compare_defs (defn, hp->value.defn)) { - U_CHAR *msg; /* what pain... */ - msg = (U_CHAR *) alloca (sym_length + 20); - bcopy (symname, msg, sym_length); - strcpy (msg + sym_length, " redefined"); -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - if(warnings_off == 0) -#endif - warning (msg); - } - /* Replace the old definition. */ - hp->type = T_MACRO; - hp->value.defn = defn; - } else - install (symname, sym_length, T_MACRO, defn, hashcode); - } - - return 0; - -nope: - - return 1; -} - -/* - * return zero if two DEFINITIONs are isomorphic - */ -int -compare_defs (d1, d2) - DEFINITION *d1, *d2; -{ - register struct reflist *a1, *a2; - register U_CHAR *p1 = d1->expansion; - register U_CHAR *p2 = d2->expansion; - int first = 1; - - if (d1->nargs != d2->nargs) - return 1; - if (strcmp (d1->argnames, d2->argnames)) - return 1; - for (a1 = d1->pattern, a2 = d2->pattern; a1 && a2; - a1 = a1->next, a2 = a2->next) { - if (!((a1->nchars == a2->nchars && ! strncmp (p1, p2, a1->nchars)) - || ! comp_def_part (first, p1, a1->nchars, p2, a2->nchars, 0)) - || a1->argno != a2->argno - || a1->stringify != a2->stringify - || a1->raw_before != a2->raw_before - || a1->raw_after != a2->raw_after) - return 1; - first = 0; - p1 += a1->nchars; - p2 += a2->nchars; - } - if (a1 != a2) - return 1; - if (comp_def_part (first, p1, d1->length - (p1 - d1->expansion), - p2, d2->length - (p2 - d2->expansion), 1)) - return 1; - return 0; -} - -/* Return 1 if two parts of two macro definitions are effectively different. - One of the parts starts at BEG1 and has LEN1 chars; - the other has LEN2 chars at BEG2. - Any sequence of whitespace matches any other sequence of whitespace. - FIRST means these parts are the first of a macro definition; - so ignore leading whitespace entirely. - LAST means these parts are the last of a macro definition; - so ignore trailing whitespace entirely. */ - -comp_def_part (first, beg1, len1, beg2, len2, last) - int first; - U_CHAR *beg1, *beg2; - int len1, len2; - int last; -{ - register U_CHAR *end1 = beg1 + len1; - register U_CHAR *end2 = beg2 + len2; - if (first) { - while (beg1 != end1 && is_space[*beg1]) beg1++; - while (beg2 != end2 && is_space[*beg2]) beg2++; - } - if (last) { - while (beg1 != end1 && is_space[end1[-1]]) end1--; - while (beg2 != end2 && is_space[end2[-1]]) end2--; - } - while (beg1 != end1 && beg2 != end2) { - if (is_space[*beg1] && is_space[*beg2]) { - while (beg1 != end1 && is_space[*beg1]) beg1++; - while (beg2 != end2 && is_space[*beg2]) beg2++; - } else if (*beg1 == *beg2) { - beg1++; beg2++; - } else break; - } - return (beg1 != end1) || (beg2 != end2); -} - -/* Read a replacement list for a macro with parameters. - Build the DEFINITION structure. - Reads characters of text starting at BUF until LIMIT. - ARGLIST specifies the formal parameters to look for - in the text of the definition; NARGS is the number of args - in that list, or -1 for a macro name that wants no argument list. - MACRONAME is the macro name itself (so we can avoid recursive expansion) - and NAMELEN is its length in characters. - -Note that comments and backslash-newlines have already been deleted -from the argument. */ - -/* Leading and trailing Space, Tab, etc. are converted to markers - Newline Space, Newline Tab, etc. - Newline Space makes a space in the final output - but is discarded if stringified. (Newline Tab is similar but - makes a Tab instead.) - - If there is no trailing whitespace, a Newline Space is added at the end - to prevent concatenation that would be contrary to the standard. */ - -DEFINITION * -collect_expansion (buf, end, nargs, arglist) - U_CHAR *buf, *end; - int nargs; - struct arglist *arglist; -{ - DEFINITION *defn; - register U_CHAR *p, *limit, *lastp, *exp_p; - struct reflist *endpat = NULL; - /* Pointer to first nonspace after last ## seen. */ - U_CHAR *concat = 0; - /* Pointer to first nonspace after last single-# seen. */ - U_CHAR *stringify = 0; - int maxsize; - int expected_delimiter = '\0'; -#ifdef sgi - int token_count = 0; - int last_token_stringify = 0; -#endif - - /* Scan thru the replacement list, ignoring comments and quoted - strings, picking up on the macro calls. It does a linear search - thru the arg list on every potential symbol. Profiling might say - that something smarter should happen. */ - - if (end < buf) - abort (); - - /* Find the beginning of the trailing whitespace. */ - /* Find end of leading whitespace. */ - limit = end; - p = buf; - while (p < limit && is_space[limit[-1]]) limit--; - while (p < limit && is_space[*p]) p++; - - /* Allocate space for the text in the macro definition. - Leading and trailing whitespace chars need 2 bytes each. - Each other input char may or may not need 1 byte, - so this is an upper bound. - The extra 2 are for invented trailing newline-marker and final null. */ - maxsize = (sizeof (DEFINITION) - + 2 * (end - limit) + 2 * (p - buf) - + (limit - p) + 3); - defn = (DEFINITION *) xcalloc (1, maxsize); - - defn->nargs = nargs; - exp_p = defn->expansion = (U_CHAR *) defn + sizeof (DEFINITION); - lastp = exp_p; - - p = buf; - - /* Convert leading whitespace to Newline-markers. */ - while (p < limit && is_space[*p]) { - *exp_p++ = '\n'; - *exp_p++ = *p++; - } - - /* Process the main body of the definition. */ - while (p < limit) { - int skipped_arg = 0; - register U_CHAR c = *p++; - -#ifdef sgi - token_count++; - last_token_stringify = 0; -#endif - *exp_p++ = c; - - if (!traditional) { - switch (c) { - case '\'': - case '\"': - for (; p < limit && *p != c; p++) { - *exp_p++ = *p; - if (*p == '\\') { - *exp_p++ = *++p; - } - } - *exp_p++ = *p++; - break; - - /* Special hack: if a \# is written in the #define - include a # in the definition. This is useless for C code - but useful for preprocessing other things. */ - - case '\\': - if (p < limit && *p == '#') { - /* Pass through this # */ - exp_p--; - *exp_p++ = *p++; - } else if (p < limit) { - /* Otherwise backslash goes through but makes next char ordinary. */ - *exp_p++ = *p++; - } - break; - - case '#': - if (p < limit && *p == '#') { - /* ##: concatenate preceding and following tokens. */ - /* Take out the first #, discard preceding whitespace. */ - exp_p--; - while (exp_p > lastp && is_hor_space[exp_p[-1]]) - --exp_p; - /* Skip the second #. */ - p++; - /* Discard following whitespace. */ - SKIP_WHITE_SPACE (p); - concat = p; -#ifdef sgi - last_token_stringify = 1; - if(token_count == 1 && pedantic) { - if(warnings_off == 0) - warning("A ## preprocessing token must not be at the beginning of a replacement list (ANSI 3.8.3.3)"); - } -#endif - } else { - /* Single #: stringify following argument ref. - Don't leave the # in the expansion. */ - exp_p--; - SKIP_WHITE_SPACE (p); - if (p == limit || ! is_idstart[*p] || nargs <= 0) - error ("# operator should be followed by a macro argument name\n"); - else - stringify = p; - } - break; - } - } else { - /* In -traditional mode, recognize arguments inside strings and - and character constants, and ignore special properties of #. - Arguments inside strings are considered "stringified", but no - extra quote marks are supplied. */ - switch (c) { - case '\'': - case '\"': - if (expected_delimiter != '\0') { - if (c == expected_delimiter) - expected_delimiter = '\0'; - } else - expected_delimiter = c; - break; - - case '\\': - /* Backslash quotes delimiters and itself, but not macro args. */ - if (expected_delimiter != 0 && p < limit - && (*p == expected_delimiter || *p == '\\')) { - *exp_p++ = *p++; - continue; - } - break; - - case '/': - if (expected_delimiter != '\0') /* No comments inside strings. */ - break; - if (*p == '*') { - /* If we find a comment that wasn't removed by handle_directive, - this must be -traditional. So replace the comment with - nothing at all. */ - exp_p--; - p += 1; - while (p < limit && !(p[-2] == '*' && p[-1] == '/')) - p++; - /* Mark this as a concatenation-point, as if it had been ##. */ - concat = p; - } - break; - } - } - - if (is_idchar[c] && nargs > 0) { - U_CHAR *id_beg = p - 1; - int id_len; - - --exp_p; - while (p != limit && is_idchar[*p]) p++; - id_len = p - id_beg; - - if (is_idstart[c]) { - register struct arglist *arg; - - for (arg = arglist; arg != NULL; arg = arg->next) { - struct reflist *tpat; - - if (arg->name[0] == c - && arg->length == id_len - && strncmp (arg->name, id_beg, id_len) == 0) { - /* make a pat node for this arg and append it to the end of - the pat list */ - tpat = (struct reflist *) xmalloc (sizeof (struct reflist)); - tpat->next = NULL; - tpat->raw_before = concat == id_beg; - tpat->raw_after = 0; - tpat->stringify = (traditional ? expected_delimiter != '\0' - : stringify == id_beg); - - if (endpat == NULL) - defn->pattern = tpat; - else - endpat->next = tpat; - endpat = tpat; - - tpat->argno = arg->argno; - tpat->nchars = exp_p - lastp; - { - register U_CHAR *p1 = p; - SKIP_WHITE_SPACE (p1); - if (p1 + 2 <= limit && p1[0] == '#' && p1[1] == '#') - tpat->raw_after = 1; - } - lastp = exp_p; /* place to start copying from next time */ - skipped_arg = 1; - break; - } - } - } - - /* If this was not a macro arg, copy it into the expansion. */ - if (! skipped_arg) { - register U_CHAR *lim1 = p; - p = id_beg; - while (p != lim1) - *exp_p++ = *p++; - if (stringify == id_beg) - error ("# operator should be followed by a macro argument name\n"); - } - } - } -#ifdef sgi - if(last_token_stringify && pedantic) - if(warnings_off == 0) - warning("A ## preprocessing token must not be at the end of a replacement list (ANSI 3.8.3.3)"); -#endif - - if (limit < end) { - /* Convert trailing whitespace to Newline-markers. */ - while (limit < end && is_space[*limit]) { - *exp_p++ = '\n'; - *exp_p++ = *limit++; - } - } else if (!traditional) { - /* There is no trailing whitespace, so invent some. */ - *exp_p++ = '\n'; - *exp_p++ = ' '; - } - - *exp_p = '\0'; - - defn->length = exp_p - defn->expansion; - - /* Crash now if we overrun the allocated size. */ - if (defn->length + 1 > maxsize) - abort (); - -#if 0 -/* This isn't worth the time it takes. */ - /* give back excess storage */ - defn->expansion = (U_CHAR *) xrealloc (defn->expansion, defn->length + 1); -#endif - - return defn; -} - -/* - * interpret #line command. Remembers previously seen fnames - * in its very own hash table. - */ -#define FNAME_HASHSIZE 37 - -do_line (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - register U_CHAR *bp; - FILE_BUF *ip = &instack[indepth]; - FILE_BUF tem; - int new_lineno; - enum file_change_code file_change = same_file; - - /* Expand any macros. */ - tem = expand_to_temp_buffer (buf, limit, 0); - - /* Point to macroexpanded line, which is null-terminated now. */ - bp = tem.buf; - SKIP_WHITE_SPACE (bp); - - if (!isdigit (*bp)) { - error ("invalid format #line command"); - return 1; - } - - /* The Newline at the end of this line remains to be processed. - To put the next line at the specified line number, - we must store a line number now that is one less. */ - new_lineno = atoi (bp) - 1; - - /* skip over the line number. */ - while (isdigit (*bp)) - bp++; - if (*bp && !is_space[*bp]) { - error ("invalid format #line command"); - return 1; - } - - SKIP_WHITE_SPACE (bp); - - if (*bp == '\"') { - static HASHNODE *fname_table[FNAME_HASHSIZE]; - HASHNODE *hp, **hash_bucket; - U_CHAR *fname; - int fname_length; - - fname = ++bp; - - while (*bp && *bp != '\"') - bp++; - if (*bp != '\"') { - error ("invalid format #line command"); - return 1; - } - - fname_length = bp - fname; - - bp++; - SKIP_WHITE_SPACE (bp); - if (*bp) { - if (*bp == '1') - file_change = enter_file; - else if (*bp == '2') - file_change = leave_file; - else { - error ("invalid format #line command"); - return 1; - } - - bp++; - SKIP_WHITE_SPACE (bp); - if (*bp) { - error ("invalid format #line command"); - return 1; - } - } - - hash_bucket = - &fname_table[hashf (fname, fname_length, FNAME_HASHSIZE)]; - for (hp = *hash_bucket; hp != NULL; hp = hp->next) - if (hp->length == fname_length && - strncmp (hp->value.cpval, fname, fname_length) == 0) { - ip->fname = hp->value.cpval; - break; - } - if (hp == 0) { - /* Didn't find it; cons up a new one. */ - hp = (HASHNODE *) xcalloc (1, sizeof (HASHNODE) + fname_length + 1); - hp->next = *hash_bucket; - *hash_bucket = hp; - - hp->length = fname_length; - ip->fname = hp->value.cpval = ((char *) hp) + sizeof (HASHNODE); - bcopy (fname, hp->value.cpval, fname_length); - } - } else if (*bp) { - error ("invalid format #line command"); - return 1; - } - - ip->lineno = new_lineno; - output_line_command (ip, op, 0, file_change); - check_expand (op, ip->length - (ip->bufp - ip->buf)); -} - -/* - * remove all definitions of symbol from symbol table. - * according to un*x /lib/cpp, it is not an error to undef - * something that has no definitions, so it isn't one here either. - */ -do_undef (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - HASHNODE *hp; -#ifdef sgi - U_CHAR *start_name; -#endif - - SKIP_WHITE_SPACE (buf); -#ifdef sgi - if (!is_idstart[*buf]) { - if(*buf == 0 || *buf == '\n') { - error ("#undef has no identifier (no macro name is present)"); - } else { - if(warnings_off == 0) - warning ("macro name in #undef starts with an inappropriate character"); - } - } -#endif - -#ifndef sgi - while ((hp = lookup (buf, -1, -1)) != NULL) { - if (hp->type != T_MACRO) - error ("undefining `%s'", hp->name); - delete_macro (hp); - } -#else - while ((hp = lookup (buf, -1, -1)) != NULL) { - if (hp->type != T_MACRO) - error ("undefining `%s'", hp->name); - delete_macro (hp); - } /* silent about not finding identifier */ - start_name = buf; - while (is_idchar[*buf] && buf < limit) { - buf++; - } -#ifdef DWARF_DM -{ - FILE_BUF *ip; - ip = &instack[indepth]; - dm_add_undef(start_name, buf - start_name,ip->lineno); -} -#endif - if(showdefines) { - print_def_undef(op,"undef",start_name,buf - start_name); - } - if (pedantic) { - SKIP_WHITE_SPACE (buf); - if (buf != limit) - if(warnings_off == 0) - warning ("text following #undef identifier violates ANSI standard"); - } -#endif -} - -/* - * Report a fatal error detected by the program we are processing. - * Use the text of the line in the error message, then terminate. - * (We use error() because it prints the filename & line#.) - */ -do_error (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - int length = limit - buf; - char *copy = (char *) xmalloc (length + 1); - bcopy (buf, copy, length); - copy[length] = 0; - SKIP_WHITE_SPACE (copy); - error ("#error %s", copy); - return (FATAL_EXIT_CODE); -} - -/* Remember the name of the current file being read from so that we can - avoid ever including it again. */ - -do_once () -{ - int i; - FILE_BUF *ip = NULL; - - for (i = indepth; i >= 0; i--) - if (instack[i].fname != NULL) { - ip = &instack[i]; - break; - } - - if (ip != NULL) - { - struct file_name_list *new; - - new = (struct file_name_list *) xmalloc (sizeof (struct file_name_list)); - new->next = dont_repeat_files; - dont_repeat_files = new; - new->fname = savestring (ip->fname); - } -} - -/* #pragma and its argument line have already been copied to the output file. - Here just check for recognized pragmas. */ - -do_pragma (buf, limit) - U_CHAR *buf, *limit; -{ - while (*buf == ' ' || *buf == '\t') - buf++; - if (!strncmp (buf, "once", 4)) - do_once (); -} - -#if 0 -/* This was a fun hack, but #pragma seems to start to be useful. - By failing to recognize it, we pass it through unchanged to cc1. */ - -/* - * the behavior of the #pragma directive is implementation defined. - * this implementation defines it as follows. - */ -do_pragma () -{ - close (0); - if (_OPEN ("/dev/tty", O_RDONLY, 0666) != 0) - goto nope; - close (1); - if (_OPEN ("/dev/tty", O_WRONLY, 0666) != 1) - goto nope; - execl ("/usr/games/hack", "#pragma", 0); - execl ("/usr/games/rogue", "#pragma", 0); - execl ("/usr/new/emacs", "-f", "hanoi", "9", "-kill", 0); - execl ("/usr/local/emacs", "-f", "hanoi", "9", "-kill", 0); -nope: - fatal ("You are in a maze of twisty compiler features, all different"); -} -#endif - -/* Just ignore #ident, on systems where we define it at all. */ -do_ident () -{ -#ifdef sgi - if (pedantic > 1 ) - if(warnings_off == 0) - warning ("ANSI C does not allow #ident; #ident ignored "); -#else - if (pedantic ) - error ("ANSI C does not allow #ident"); -#endif -} - -/* Just ignore #sccs, on systems where we define it at all. */ -do_sccs () -{ - if (pedantic) - error ("ANSI C does not allow #sccs"); -} - -/* - * handle #if command by - * 1) inserting special `defined' keyword into the hash table - * that gets turned into 0 or 1 by special_symbol (thus, - * if the luser has a symbol called `defined' already, it won't - * work inside the #if command) - * 2) rescan the input into a temporary output buffer - * 3) pass the output buffer to the yacc parser and collect a value - * 4) clean up the mess left from steps 1 and 2. - * 5) call conditional_skip to skip til the next #endif (etc.), - * or not, depending on the value from step 3. - */ - -do_if (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - int value; - FILE_BUF *ip = &instack[indepth]; - - value = eval_if_expression (buf, limit - buf); - conditional_skip (ip, value == 0, T_IF); -} - -/* - * handle a #elif directive by not changing if_stack either. - * see the comment above do_else. - */ - -do_elif (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - int value; - FILE_BUF *ip = &instack[indepth]; - - if (if_stack == instack[indepth].if_stack) { - error ("#elif not within a conditional"); - return 1; - } else { - if (if_stack->type != T_IF && if_stack->type != T_ELIF) { - error ("#elif after #else"); - USER_MESSAGE(" (matches line %d", if_stack->lineno); - if (if_stack->fname != NULL && ip->fname != NULL && - strcmp (if_stack->fname, ip->fname) != 0) - USER_MESSAGE(", file %s", if_stack->fname); - USER_MESSAGE(")\n"); - } - if_stack->type = T_ELIF; - } - - if (if_stack->if_succeeded) - skip_if_group (ip, 0); - else { - value = eval_if_expression (buf, limit - buf); - if (value == 0) - skip_if_group (ip, 0); - else { - ++if_stack->if_succeeded; /* continue processing input */ - output_line_command (ip, op, 1, same_file); - } - } -} - -#if 0 -/* THIS DOESN'T EVEN BEGIN TO WORK (yet) */ -/* - * return true if what we are looking at is an assertion expression, - */ -int -is_assertion(buf) - U_CHAR *buf; -{ - int c; - do { - c = *buf++; - } while(isspace(c) || c == '!'); /* can negate an arbitrary number of - times */ - - return (c == '#'); -} -#endif - -/* - * evaluate a #if expression in BUF, of length LENGTH, - * then parse the result as a C expression and return the value as an int. - */ -int -eval_if_expression (buf, length) - U_CHAR *buf; - int length; -{ - FILE_BUF temp_obuf; - HASHNODE *save_defined; - int value; - - save_defined = install ("defined", -1, T_SPEC_DEFINED, 0, -1); - temp_obuf = expand_to_temp_buffer (buf, buf + length, 0); - delete_macro (save_defined); /* clean up special symbol */ - - /* the AT&T preprocessor made an extension to ANSI which proves to - * be sufficiently useful that I have decided to go ahead and add - * it eventually. It works by: - * - * # assert assertion-name ( preprocessor tokens ) - * # unassert assertion-name - * - * Having made an assertion of this form, one can use the assertion - * in a preprocessor conditional by saying - * - * # if #assertion-name(preprocessor-tokens) - * - * If the assertion has been made with the preprocessor tokens in - * question, the result is to return true. If the preprocessor - * tokens don't match, or the assertion hasn't been made, the value is - * considered to be false. - */ - -#if 0 - value = (is_assertion(temp_obuf.buf) ? - parse_assertion_extension (temp_obuf.buf) : - parse_c_expression (temp_obuf.buf)); -#else - value = parse_c_expression (temp_obuf.buf); -#endif - - free (temp_obuf.buf); - - return value; -} - -/* - * routine to handle ifdef/ifndef. Try to look up the symbol, - * then do or don't skip to the #endif/#else/#elif depending - * on what directive is actually being processed. - */ -do_xifdef (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - int skip; - FILE_BUF *ip = &instack[indepth]; - U_CHAR *end; - - /* Discard leading and trailing whitespace. */ - SKIP_WHITE_SPACE (buf); - while (limit != buf && is_hor_space[limit[-1]]) limit--; - - /* Find the end of the identifier at the beginning. */ - for (end = buf; is_idchar[*end]; end++); - - if (end == buf) { - skip = (keyword->type == T_IFDEF); - if (! traditional) -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - if(warnings_off == 0) -#endif - warning (end == limit ? "#%s with no argument" - : "#%s argument starts with punctuation", - keyword->name); - } else { - if (pedantic && buf[0] >= '0' && buf[0] <= '9') { -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - if(warnings_off == 0) -#endif - warning ("#%s argument starts with a digit", keyword->name); - } else if (end != limit && !traditional) { -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ - if(warnings_off == 0) -#endif - warning ("garbage at end of #%s argument", keyword->name); - } - - skip = (lookup (buf, end-buf, -1) == NULL) ^ (keyword->type == T_IFNDEF); - } - - conditional_skip (ip, skip, T_IF); -} - -/* - * push TYPE on stack; then, if SKIP is nonzero, skip ahead. - */ -void -conditional_skip (ip, skip, type) - FILE_BUF *ip; - int skip; - enum node_type type; -{ - IF_STACK_FRAME *temp; - - temp = (IF_STACK_FRAME *) xcalloc (1, sizeof (IF_STACK_FRAME)); - temp->fname = ip->fname; - temp->lineno = ip->lineno; - temp->next = if_stack; - if_stack = temp; - - if_stack->type = type; - - if (skip != 0) { - skip_if_group (ip, 0); - return; - } else { - ++if_stack->if_succeeded; - output_line_command (ip, &outbuf, 1, same_file); - } -} - -/* - * skip to #endif, #else, or #elif. adjust line numbers, etc. - * leaves input ptr at the sharp sign found. - * If ANY is nonzero, return at next directive of any sort. - */ -void -skip_if_group (ip, any) - FILE_BUF *ip; - int any; -{ - register U_CHAR *bp = ip->bufp, *cp; - register U_CHAR *endb = ip->buf + ip->length; - struct directive *kt; - IF_STACK_FRAME *save_if_stack = if_stack; /* don't pop past here */ - U_CHAR *beg_of_line = bp; - - while (bp < endb) { - switch (*bp++) { - case '/': /* possible comment */ - if (*bp == '\\' && bp[1] == '\n') - newline_fix (bp); - if (*bp == '*' - || (cplusplus && *bp == '/')) { - ip->bufp = ++bp; - bp = skip_to_end_of_comment (ip, &ip->lineno); - } - break; - case '\"': - case '\'': - if (!traditional) - bp = skip_quoted_string (bp - 1, endb, ip->lineno, &ip->lineno, 0, 0); - break; - case '\\': - /* Char after backslash loses its special meaning. */ - if (bp < endb) { - if (*bp == '\n') - ++ip->lineno; /* But do update the line-count. */ - bp++; - } - break; - case '\n': - ++ip->lineno; - beg_of_line = bp; - break; - case '#': - ip->bufp = bp - 1; - - /* # keyword: a # must be first nonblank char on the line */ - if (beg_of_line == 0) - break; - /* Scan from start of line, skipping whitespace, comments - and backslash-newlines, and see if we reach this #. - If not, this # is not special. */ - bp = beg_of_line; - while (1) { - if (is_hor_space[*bp]) - bp++; - else if (*bp == '\\' && bp[1] == '\n') - bp += 2; - else if (*bp == '/' && bp[1] == '*') { - bp += 2; - while (!(*bp == '*' && bp[1] == '/')) - bp++; - bp += 2; - } - else if (cplusplus && *bp == '/' && bp[1] == '/') { - bp += 2; - while (*bp++ != '\n') ; - } - else break; - } - if (bp != ip->bufp) { - bp = ip->bufp + 1; /* Reset bp to after the #. */ - break; - } - - bp = ip->bufp + 1; /* point at '#' */ - - /* Skip whitespace and \-newline. */ - while (1) { - if (is_hor_space[*bp]) - bp++; - else if (*bp == '\\' && bp[1] == '\n') - bp += 2; -#ifdef sgi - /* cypress 66662. - In false if, comment after # before endif not handled before - */ - else if (*bp == '/' && bp[1] == '*') { - bp += 2; - while (!(*bp == '*' && bp[1] == '/')) - bp++; - bp += 2; - } -#endif - else break; - } - - cp = bp; - - /* Now find end of directive name. - If we encounter a backslash-newline, exchange it with any following - symbol-constituents so that we end up with a contiguous name. */ - - while (1) { - if (is_idchar[*bp]) - bp++; - else { - if (*bp == '\\' && bp[1] == '\n') - name_newline_fix (bp); - if (is_idchar[*bp]) - bp++; - else break; - } - } - - for (kt = directive_table; kt->length >= 0; kt++) { - IF_STACK_FRAME *temp; - if (strncmp (cp, kt->name, kt->length) == 0 - && !is_idchar[cp[kt->length]]) { - - /* If we are asked to return on next directive, - do so now. */ - if (any) - return; - - switch (kt->type) { - case T_IF: - case T_IFDEF: - case T_IFNDEF: - temp = (IF_STACK_FRAME *) xcalloc (1, sizeof (IF_STACK_FRAME)); - temp->next = if_stack; - if_stack = temp; - temp->lineno = ip->lineno; - temp->fname = ip->fname; - temp->type = kt->type; - break; - case T_ELSE: - case T_ENDIF: - if (pedantic && if_stack != save_if_stack) - validate_else (bp); - case T_ELIF: - if (if_stack == instack[indepth].if_stack) { - error ("#%s not within a conditional", kt->name); - break; - } - else if (if_stack == save_if_stack) - return; /* found what we came for */ - - if (kt->type != T_ENDIF) { - if (if_stack->type == T_ELSE) - error ("#else or #elif after #else"); - if_stack->type = kt->type; - break; - } - - temp = if_stack; - if_stack = if_stack->next; - free (temp); - break; - } - break; - } - } - } - } - ip->bufp = bp; - /* after this returns, rescan will exit because ip->bufp - now points to the end of the buffer. - rescan is responsible for the error message also. */ -} - -/* - * handle a #else directive. Do this by just continuing processing - * without changing if_stack ; this is so that the error message - * for missing #endif's etc. will point to the original #if. It - * is possible that something different would be better. - */ -do_else (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - FILE_BUF *ip = &instack[indepth]; - - if (pedantic) { - SKIP_WHITE_SPACE (buf); - if (buf != limit) -#ifdef sgi - if(warnings_off == 0) { - ONE_TIME_ENDIF( - warning ("text following #else violates ANSI standard") - ); - } -#else - warning ("text following #else violates ANSI standard"); -#endif - } - - if (if_stack == instack[indepth].if_stack) { - error ("#else not within a conditional"); - return 1; - } else { - if (if_stack->type != T_IF && if_stack->type != T_ELIF) { - error ("#else after #else"); - USER_MESSAGE(" (matches line %d", if_stack->lineno); - if (strcmp (if_stack->fname, ip->fname) != 0) - USER_MESSAGE(", file %s", if_stack->fname); - USER_MESSAGE(")\n"); - } - if_stack->type = T_ELSE; - } - - if (if_stack->if_succeeded) - skip_if_group (ip, 0); - else { - ++if_stack->if_succeeded; /* continue processing input */ - output_line_command (ip, op, 1, same_file); - } -} - -/* - * unstack after #endif command - */ -do_endif (buf, limit, op, keyword) - U_CHAR *buf, *limit; - FILE_BUF *op; - struct directive *keyword; -{ - if (pedantic) { - SKIP_WHITE_SPACE (buf); - if (buf != limit) -#ifdef sgi - if(warnings_off == 0) { - ONE_TIME_ENDIF( - warning ("text following #endif violates ANSI standard") - ); - } -#else - warning ("text following #endif violates ANSI standard"); -#endif - } - - if (if_stack == instack[indepth].if_stack) - error ("unbalanced #endif"); - else { - IF_STACK_FRAME *temp = if_stack; - if_stack = if_stack->next; - free (temp); - output_line_command (&instack[indepth], op, 1, same_file); - } -} - -/* When an #else or #endif is found while skipping failed conditional, - if -pedantic was specified, this is called to warn about text after - the command name. P points to the first char after the command name. */ - -validate_else (p) - register U_CHAR *p; -{ - /* Advance P over whitespace and comments. */ - while (1) { - if (*p == '\\' && p[1] == '\n') - p += 2; - if (is_hor_space[*p]) - p++; - else if (*p == '/') { - if (p[1] == '\\' && p[2] == '\n') - newline_fix (p + 1); - if (p[1] == '*') { - p += 2; - /* Don't bother warning about unterminated comments - since that will happen later. Just be sure to exit. */ - while (*p) { - if (p[1] == '\\' && p[2] == '\n') - newline_fix (p + 1); - if (*p == '*' && p[1] == '/') { - p += 2; - break; - } - p++; - } - } - else if (cplusplus && p[1] == '/') { - p += 2; - while (*p && *p++ != '\n') ; - } - } else break; - } - if (*p && *p != '\n') -#ifdef sgi - if(warnings_off == 0) { - ONE_TIME_ENDIF( - warning ("text following #else or #endif violates ANSI standard") - ); - } -#else - warning ("text following #else or #endif violates ANSI standard"); -#endif -} - -/* - * Skip a comment, assuming the input ptr immediately follows the - * initial slash-star. Bump line counter as necessary. - * (The canonical line counter is &ip->lineno). - * Don't use this routine (or the next one) if bumping the line - * counter is not sufficient to deal with newlines in the string. - */ -U_CHAR * -skip_to_end_of_comment (ip, line_counter) - register FILE_BUF *ip; - int *line_counter; /* place to remember newlines, or NULL */ -{ - register U_CHAR *limit = ip->buf + ip->length; - register U_CHAR *bp = ip->bufp; - FILE_BUF *op = &outbuf; /* JF */ - int output = put_out_comments && !line_counter; - - /* JF this line_counter stuff is a crock to make sure the - comment is only put out once, no matter how many times - the comment is skipped. It almost works */ - if (output) { - *op->bufp++ = '/'; - *op->bufp++ = '*'; - } - if (cplusplus && bp[-1] == '/') { - if (output) { - while (bp < limit) - if ((*op->bufp++ = *bp++) == '\n') { - bp--; - break; - } - op->bufp[-1] = '*'; - *op->bufp++ = '/'; - *op->bufp++ = '\n'; - } else { - while (bp < limit) { - if (*bp++ == '\n') { - bp--; - break; - } - } - } - ip->bufp = bp; - return bp; - } - while (bp < limit) { - if (output) - *op->bufp++ = *bp; - switch (*bp++) { - case '\n': - if (line_counter != NULL) - ++*line_counter; - if (output) - ++op->lineno; - break; - case '*': - if (*bp == '\\' && bp[1] == '\n') - newline_fix (bp); - if (*bp == '/') { - if (output) - *op->bufp++ = '/'; - ip->bufp = ++bp; - return bp; - } - break; - } - } - ip->bufp = bp; - return bp; -} - -/* - * Skip over a quoted string. BP points to the opening quote. - * Returns a pointer after the closing quote. Don't go past LIMIT. - * START_LINE is the line number of the starting point (but it need - * not be valid if the starting point is inside a macro expansion). - * - * The input stack state is not changed. - * - * If COUNT_NEWLINES is nonzero, it points to an int to increment - * for each newline passed. - * - * If BACKSLASH_NEWLINES_P is nonzero, store 1 thru it - * if we pass a backslash-newline. - * - * If EOFP is nonzero, set *EOFP to 1 if the string is unterminated. - */ -U_CHAR * -skip_quoted_string (bp, limit, start_line, count_newlines, backslash_newlines_p, eofp) - register U_CHAR *bp; - register U_CHAR *limit; - int start_line; - int *count_newlines; - int *backslash_newlines_p; - int *eofp; -{ - register U_CHAR c, match; - - match = *bp++; - while (1) { - if (bp >= limit) { - error_with_line (line_for_error (start_line), - "unterminated string or character constant"); - if (eofp) - *eofp = 1; - break; - } - c = *bp++; - if (c == '\\') { - while (*bp == '\\' && bp[1] == '\n') { - if (backslash_newlines_p) - *backslash_newlines_p = 1; - if (count_newlines) - ++*count_newlines; - bp += 2; - } - if (*bp == '\n' && count_newlines) { - if (backslash_newlines_p) - *backslash_newlines_p = 1; - ++*count_newlines; - } - bp++; - } else if (c == '\n') { - if (match == '\'') { - error_with_line (line_for_error (start_line), - "unterminated character constant"); - bp--; - if (eofp) - *eofp = 1; - break; - } - if (traditional) { /* Unterminated strings are 'legal'. */ - if (eofp) - *eofp = 1; - break; - } - if (count_newlines) - ++*count_newlines; - } else if (c == match) - break; - } - return bp; -} - -/* - * write out a #line command, for instance, after an #include file. - * If CONDITIONAL is nonzero, we can omit the #line if it would - * appear to be a no-op, and we can output a few newlines instead - * if we want to increase the line number by a small amount. - * FILE_CHANGE says whether we are entering a file, leaving, or neither. - */ - -void -output_line_command (ip, op, conditional, file_change) - FILE_BUF *ip, *op; - int conditional; - enum file_change_code file_change; -{ - int len; - char line_cmd_buf[500]; - - if (no_line_commands - || ip->fname == NULL - || no_output) { - op->lineno = ip->lineno; - return; - } - - if (conditional) { - if (ip->lineno == op->lineno) - return; - - /* If the inherited line number is a little too small, - output some newlines instead of a #line command. */ - if (ip->lineno > op->lineno && ip->lineno < op->lineno + 8) { - check_expand (op, 10); - while (ip->lineno > op->lineno) { - *op->bufp++ = '\n'; - op->lineno++; - } - return; - } - } - -#ifdef OUTPUT_LINE_COMMANDS - sprintf (line_cmd_buf, "#line %d \"%s\"", ip->lineno, ip->fname); -#else - sprintf (line_cmd_buf, "# %d \"%s\"", ip->lineno, ip->fname); -#endif -#if 0 - /* on sgi machines this gives the compiler indigestion, which in turn - upsets dbx... */ - if (file_change != same_file) - strcat (line_cmd_buf, file_change == enter_file ? " 1" : " 2"); -#endif - len = strlen (line_cmd_buf); - line_cmd_buf[len++] = '\n'; - check_expand (op, len + 1); - if (op->bufp > op->buf && op->bufp[-1] != '\n') - *op->bufp++ = '\n'; - bcopy (line_cmd_buf, op->bufp, len); - op->bufp += len; - op->lineno = ip->lineno; -} - -/* This structure represents one parsed argument in a macro call. - `raw' points to the argument text as written (`raw_length' is its length). - `expanded' points to the argument's macro-expansion - (its length is `expand_length'). - `stringified_length' is the length the argument would have - if stringified. - `free1' and `free2', if nonzero, point to blocks to be freed - when the macro argument data is no longer needed. */ - -struct argdata { - U_CHAR *raw, *expanded; - int raw_length, expand_length; - int stringified_length; - U_CHAR *free1, *free2; - char newlines; - char comments; -}; - -/* Expand a macro call. - HP points to the symbol that is the macro being called. - Put the result of expansion onto the input stack - so that subsequent input by our caller will use it. - - If macro wants arguments, caller has already verified that - an argument list follows; arguments come from the input stack. */ - -void -macroexpand (hp, op) - HASHNODE *hp; - FILE_BUF *op; -{ - int nargs; - DEFINITION *defn = hp->value.defn; - register U_CHAR *xbuf; - int xbuf_len; - int start_line = instack[indepth].lineno; - - CHECK_DEPTH (return;); - - /* it might not actually be a macro. */ - if (hp->type != T_MACRO) { - special_symbol (hp, op); - return; - } - - nargs = defn->nargs; - - if (nargs >= 0) { - register int i; - struct argdata *args; - char *parse_error = 0; - - args = (struct argdata *) alloca ((nargs + 1) * sizeof (struct argdata)); - - for (i = 0; i < nargs; i++) { - args[i].raw = args[i].expanded = (U_CHAR *) ""; - args[i].raw_length = args[i].expand_length - = args[i].stringified_length = 0; - args[i].free1 = args[i].free2 = 0; - } - - /* Parse all the macro args that are supplied. I counts them. - The first NARGS args are stored in ARGS. - The rest are discarded. */ - i = 0; - do { - /* Discard the open-parenthesis or comma before the next arg. */ - ++instack[indepth].bufp; - if (parse_error = macarg ((i < nargs || (nargs == 0 && i == 0)) ? &args[i] : 0)) - { - error_with_line (line_for_error (start_line), parse_error); - break; - } - i++; - } while (*instack[indepth].bufp != ')'); - - if (nargs == 0 && i == 1) { - register U_CHAR *bp = args[0].raw; - register U_CHAR *lim = bp + args[0].raw_length; - while (bp != lim && is_space[*bp]) bp++; - if (bp != lim) - error ("arguments given to macro `%s'", hp->name); - } else if (i < nargs) - error ("only %d args to macro `%s'", i, hp->name); - else if (i > nargs) - error ("too many (%d) args to macro `%s'", i, hp->name); - - /* Swallow the closeparen. */ - ++instack[indepth].bufp; - - /* If macro wants zero args, we parsed the arglist for checking only. - Read directly from the macro definition. */ - if (nargs == 0) { - xbuf = defn->expansion; - xbuf_len = defn->length; - } else { - register U_CHAR *exp = defn->expansion; - register int offset; /* offset in expansion, - copied a piece at a time */ - register int totlen; /* total amount of exp buffer filled so far */ - - register struct reflist *ap; - - /* Macro really takes args. Compute the expansion of this call. */ - - /* Compute length in characters of the macro's expansion. */ - xbuf_len = defn->length; - for (ap = defn->pattern; ap != NULL; ap = ap->next) { - if (ap->stringify) - xbuf_len += args[ap->argno].stringified_length; - else if (ap->raw_before || ap->raw_after) - xbuf_len += args[ap->argno].raw_length; - else - xbuf_len += args[ap->argno].expand_length; - } - - xbuf = (U_CHAR *) xmalloc (xbuf_len + 1); - - /* Generate in XBUF the complete expansion - with arguments substituted in. - TOTLEN is the total size generated so far. - OFFSET is the index in the definition - of where we are copying from. */ - offset = totlen = 0; - for (ap = defn->pattern; ap != NULL; ap = ap->next) { - register struct argdata *arg = &args[ap->argno]; - - for (i = 0; i < ap->nchars; i++) - xbuf[totlen++] = exp[offset++]; - - if (ap->stringify != 0) { - int arglen = arg->raw_length; - int escaped = 0; - int in_string = 0; - int c; - i = 0; - while (i < arglen - && (c = arg->raw[i], is_space[c])) - i++; - while (i < arglen - && (c = arg->raw[arglen - 1], is_space[c])) - arglen--; - if (!traditional) - xbuf[totlen++] = '\"'; /* insert beginning quote */ - for (; i < arglen; i++) { - c = arg->raw[i]; - - /* Special markers Newline Space - generate nothing for a stringified argument. */ - if (c == '\n' && arg->raw[i+1] != '\n') { - i++; - continue; - } - - /* Internal sequences of whitespace are replaced by one space. */ -#ifdef sgi - if(in_string && is_hor_space[c] ) { - /* ok as is: take in-string unchanged */ - } else -#endif - if (c == '\n' ? arg->raw[i+1] == '\n' : is_space[c]) { - while (1) { - if (c == '\n' && arg->raw[i+1] == '\n') - i += 2; - else if (c != '\n' && is_space[c]) - i++; - else break; - c = arg->raw[i]; - } - i--; - c = ' '; - } - - if (escaped) - escaped = 0; - else { - if (c == '\\') - escaped = 1; - if (in_string && c == in_string) - in_string = 0; - else if (c == '\"' || c == '\'') - in_string = c; - } - - /* Escape these chars */ - if (c == '\"' || (in_string && c == '\\')) - xbuf[totlen++] = '\\'; - if (isprint (c)) - xbuf[totlen++] = c; - else { - sprintf ((char *) &xbuf[totlen], "\\%03o", (unsigned int) c); - totlen += 4; - } - } - if (!traditional) - xbuf[totlen++] = '\"'; /* insert ending quote */ - } else if (ap->raw_before || ap->raw_after) { - U_CHAR *p1 = arg->raw; - U_CHAR *l1 = p1 + arg->raw_length; - if (ap->raw_before) { - while (p1 != l1 && is_space[*p1]) p1++; - while (p1 != l1 && is_idchar[*p1]) - xbuf[totlen++] = *p1++; - /* Delete any no-reexpansion marker that follows - an identifier at the beginning of the argument - if the argument is concatenated with what precedes it. */ - if (p1[0] == '\n' && p1[1] == '-') - p1 += 2; - } - if (ap->raw_after) { - /* Arg is concatenated after: delete trailing whitespace, - whitespace markers, and no-reexpansion markers. */ - while (p1 != l1) { - if (is_space[l1[-1]]) l1--; - else if (l1[-1] == '-') { - U_CHAR *p2 = l1 - 1; - /* If a `-' is preceded by an odd number of newlines then it - and the last newline are a no-reexpansion marker. */ - while (p2 != p1 && p2[-1] == '\n') p2--; - if ((l1 - 1 - p2) & 1) { - l1 -= 2; - } - else break; - } - else break; - } - } - bcopy (p1, xbuf + totlen, l1 - p1); - totlen += l1 - p1; - } else { - bcopy (arg->expanded, xbuf + totlen, arg->expand_length); - totlen += arg->expand_length; - } - - if (totlen > xbuf_len) - abort (); - } - - /* if there is anything left of the definition - after handling the arg list, copy that in too. */ - - for (i = offset; i < defn->length; i++) - xbuf[totlen++] = exp[i]; - - xbuf[totlen] = 0; - xbuf_len = totlen; - - for (i = 0; i < nargs; i++) { - if (args[i].free1 != 0) - free (args[i].free1); - if (args[i].free2 != 0) - free (args[i].free2); - } - } - } else { - xbuf = defn->expansion; - xbuf_len = defn->length; - } - - /* Now put the expansion on the input stack - so our caller will commence reading from it. */ - { - register FILE_BUF *ip2; - - ip2 = &instack[++indepth]; - - ip2->fname = 0; - ip2->lineno = 0; - ip2->buf = xbuf; - ip2->length = xbuf_len; - ip2->bufp = xbuf; - ip2->free_ptr = (nargs > 0) ? xbuf : 0; - ip2->macro = hp; - ip2->if_stack = if_stack; - - /* Recursive macro use sometimes works traditionally. - #define foo(x,y) bar(x(y,0), y) - foo(foo, baz) */ - - if (!traditional) - hp->type = T_DISABLED; - } -} - -/* - * Parse a macro argument and store the info on it into *ARGPTR. - * Return nonzero to indicate a syntax error. - */ - -char * -macarg (argptr) - register struct argdata *argptr; -{ - FILE_BUF *ip = &instack[indepth]; - int paren = 0; - int newlines = 0; - int comments = 0; - - /* Try to parse as much of the argument as exists at this - input stack level. */ - U_CHAR *bp = macarg1 (ip->bufp, ip->buf + ip->length, - &paren, &newlines, &comments); - - /* If we find the end of the argument at this level, - set up *ARGPTR to point at it in the input stack. */ - if (!(ip->fname != 0 && (newlines != 0 || comments != 0)) - && bp != ip->buf + ip->length) { - if (argptr != 0) { - argptr->raw = ip->bufp; - argptr->raw_length = bp - ip->bufp; - } - ip->bufp = bp; - } else { - /* This input stack level ends before the macro argument does. - We must pop levels and keep parsing. - Therefore, we must allocate a temporary buffer and copy - the macro argument into it. */ - int bufsize = bp - ip->bufp; - int extra = newlines; - U_CHAR *buffer = (U_CHAR *) xmalloc (bufsize + extra + 1); - int final_start = 0; - - bcopy (ip->bufp, buffer, bufsize); - ip->bufp = bp; - ip->lineno += newlines; - - while (bp == ip->buf + ip->length) { - if (instack[indepth].macro == 0) { - free (buffer); - return "unterminated macro call"; - } - ip->macro->type = T_MACRO; - free (ip->buf); - ip = &instack[--indepth]; - newlines = 0; - comments = 0; - bp = macarg1 (ip->bufp, ip->buf + ip->length, &paren, - &newlines, &comments); - final_start = bufsize; - bufsize += bp - ip->bufp; - extra += newlines; - buffer = (U_CHAR *) xrealloc (buffer, bufsize + extra + 1); - bcopy (ip->bufp, buffer + bufsize - (bp - ip->bufp), bp - ip->bufp); - ip->bufp = bp; - ip->lineno += newlines; - } - - /* Now, if arg is actually wanted, record its raw form, - discarding comments and duplicating newlines in whatever - part of it did not come from a macro expansion. - EXTRA space has been preallocated for duplicating the newlines. - FINAL_START is the index of the start of that part. */ - if (argptr != 0) { - argptr->raw = buffer; - argptr->raw_length = bufsize; - argptr->free1 = buffer; - argptr->newlines = newlines; - argptr->comments = comments; - if ((newlines || comments) && ip->fname != 0) - argptr->raw_length - = final_start + - discard_comments (argptr->raw + final_start, - argptr->raw_length - final_start, - newlines); - argptr->raw[argptr->raw_length] = 0; - if (argptr->raw_length > bufsize + extra) - abort (); - } - } - - /* If we are not discarding this argument, - macroexpand it and compute its length as stringified. - All this info goes into *ARGPTR. */ - - if (argptr != 0) { - FILE_BUF obuf; - register U_CHAR *buf, *lim; - register int totlen; -#ifdef sgi - int escaped = 0; - int in_string = 0; -#endif - - obuf = expand_to_temp_buffer (argptr->raw, - argptr->raw + argptr->raw_length, - 1); - - argptr->expanded = obuf.buf; - argptr->expand_length = obuf.length; - argptr->free2 = obuf.buf; - - buf = argptr->raw; - lim = buf + argptr->raw_length; - - while (buf != lim && is_space[*buf]) - buf++; - while (buf != lim && is_space[lim[-1]]) - lim--; - totlen = traditional ? 0 : 2; /* Count opening and closing quote. */ -#ifdef sgi - /* need the same logic as in real stringify code: must not - reduce stuff inside "" or '' */ - while (buf != lim) { - register U_CHAR c = *buf++; - totlen++; - /* Internal sequences of whitespace are replaced by one space. */ - if (escaped) - escaped = 0; - else { - if (c == '\\') - escaped = 1; - if (in_string && c == in_string) - in_string = 0; - else if (c == '\"' || c == '\'') - in_string = c; - } - if (is_space[c] && !in_string) { - SKIP_ALL_WHITE_SPACE (buf); - } - else if (c == '\"' || c == '\\') /* escape these chars */ - totlen++; - else if (!isprint (c)) - totlen += 3; - } -#else - while (buf != lim) { - register U_CHAR c = *buf++; - totlen++; - /* Internal sequences of whitespace are replaced by one space. */ - if (is_space[c]) { - SKIP_ALL_WHITE_SPACE (buf); - } - else if (c == '\"' || c == '\\') /* escape these chars */ - totlen++; - else if (!isprint (c)) - totlen += 3; - } -#endif - argptr->stringified_length = totlen; - } - return 0; -} - -/* Scan text from START (inclusive) up to LIMIT (exclusive), - counting parens in *DEPTHPTR, - and return if reach LIMIT - or before a `)' that would make *DEPTHPTR negative - or before a comma when *DEPTHPTR is zero. - Single and double quotes are matched and termination - is inhibited within them. Comments also inhibit it. - Value returned is pointer to stopping place. - - Increment *NEWLINES each time a newline is passed. - Set *COMMENTS to 1 if a comment is seen. */ - -U_CHAR * -macarg1 (start, limit, depthptr, newlines, comments) - U_CHAR *start; - register U_CHAR *limit; - int *depthptr, *newlines, *comments; -{ - register U_CHAR *bp = start; - - while (bp < limit) { - switch (*bp) { - case '(': - (*depthptr)++; - break; - case ')': - if (--(*depthptr) < 0) - return bp; - break; - case '\\': - /* Backslash makes following char not special. */ - if (bp + 1 < limit) bp++; - break; - case '\n': - ++*newlines; - break; - case '/': - if (bp[1] == '\\' && bp[2] == '\n') - newline_fix (bp + 1); - if (cplusplus && bp[1] == '/') { - *comments = 1; - bp += 2; - while (bp < limit && *bp++ != '\n') ; - ++*newlines; - break; - } - if (bp[1] != '*' || bp + 1 >= limit) - break; - *comments = 1; - bp += 2; - while (bp + 1 < limit) { - if (bp[0] == '*' - && bp[1] == '\\' && bp[2] == '\n') - newline_fix (bp + 1); - if (bp[0] == '*' && bp[1] == '/') - break; - if (*bp == '\n') ++*newlines; - bp++; - } - break; - case '\'': - case '\"': - { - int quotec; - for (quotec = *bp++; bp + 1 < limit && *bp != quotec; bp++) { - if (*bp == '\\') { - bp++; - if (*bp == '\n') - ++*newlines; - while (*bp == '\\' && bp[1] == '\n') { - bp += 2; - } - } else if (*bp == '\n') { - ++*newlines; - if (quotec == '\'') - break; - } - } - } - break; - case ',': - if ((*depthptr) == 0) - return bp; - break; - } - bp++; - } - - return bp; -} - -/* Discard comments and duplicate newlines - in the string of length LENGTH at START, - except inside of string constants. - The string is copied into itself with its beginning staying fixed. - - NEWLINES is the number of newlines that must be duplicated. - We assume that that much extra space is available past the end - of the string. */ - -int -discard_comments (start, length, newlines) - U_CHAR *start; - int length; - int newlines; -{ - register U_CHAR *ibp; - register U_CHAR *obp; - register U_CHAR *limit; - register int c; - - /* If we have newlines to duplicate, copy everything - that many characters up. Then, in the second part, - we will have room to insert the newlines - while copying down. - NEWLINES may actually be too large, because it counts - newlines in string constants, and we don't duplicate those. - But that does no harm. */ - if (newlines > 0) { - ibp = start + length; - obp = ibp + newlines; - limit = start; - while (limit != ibp) - *--obp = *--ibp; - } - - ibp = start + newlines; - limit = start + length + newlines; - obp = start; - - while (ibp < limit) { - *obp++ = c = *ibp++; - switch (c) { - case '\n': - /* Duplicate the newline. */ - *obp++ = '\n'; - break; - - case '/': - if (*ibp == '\\' && ibp[1] == '\n') - newline_fix (ibp); - /* Delete any comment. */ - if (cplusplus && ibp[0] == '/') { - obp--; - ibp++; - while (ibp < limit && *ibp++ != '\n') ; - break; - } - if (ibp[0] != '*' || ibp + 1 >= limit) - break; - obp--; - ibp++; - while (ibp + 1 < limit) { - if (ibp[0] == '*' - && ibp[1] == '\\' && ibp[2] == '\n') - newline_fix (ibp + 1); - if (ibp[0] == '*' && ibp[1] == '/') - break; - ibp++; - } - ibp += 2; - break; - - case '\'': - case '\"': - /* Notice and skip strings, so that we don't - think that comments start inside them, - and so we don't duplicate newlines in them. */ - { - int quotec = c; - while (ibp < limit) { - *obp++ = c = *ibp++; - if (c == quotec) - break; - if (c == '\n' && quotec == '\'') - break; - if (c == '\\' && ibp < limit) { - while (*ibp == '\\' && ibp[1] == '\n') - ibp += 2; - *obp++ = *ibp++; - } - } - } - break; - } - } - - return obp - start; -} - - -/* Error including a message from `errno'. */ - -error_from_errno (name) - char *name; -{ - int i; - FILE_BUF *ip = NULL; -#if !defined WIN32 && !defined __linux__ && !defined __APPLE__ - extern int errno, sys_nerr; - extern char *sys_errlist[]; -#endif - - for (i = indepth; i >= 0; i--) - if (instack[i].fname != NULL) { - ip = &instack[i]; - break; - } - - if (ip != NULL) - USER_MESSAGE("%s:%d: ", _get_filename_without_path(ip->fname), ip->lineno); - -#if defined WIN32 || defined __linux__ || defined __APPLE__ - perror(name); -#else - if (errno < sys_nerr) - USER_MESSAGE("%s: %s\n", name, sys_errlist[errno]); - else - USER_MESSAGE("%s: undocumented I/O error\n", name); -#endif - - errors++; - return 0; -} - -/* Print error message but don't count it. */ - -warning (msg, arg1, arg2, arg3) - char *msg; -{ - int i; - FILE_BUF *ip = NULL; - -#if 0 - fprintf(fp_out, "warning\n"); -#endif - - for (i = indepth; i >= 0; i--) - if (instack[i].fname != NULL) { - ip = &instack[i]; - break; - } - - if (ip != NULL) - USER_MESSAGE("%s:%d: warning: ", _get_filename_without_path(ip->fname), ip->lineno); - - USER_MESSAGE(msg, arg1, arg2, arg3); - USER_MESSAGE("\n"); - return 0; -} - -error_with_line (line, msg, arg1, arg2, arg3) - int line; - char *msg; -{ - int i; - FILE_BUF *ip = NULL; - - fprintf(fp_out, "error with line\n"); - - for (i = indepth; i >= 0; i--) - if (instack[i].fname != NULL) { - ip = &instack[i]; - break; - } - - if (ip != NULL) - USER_MESSAGE("%s:%d: ", _get_filename_without_path(ip->fname), line); - - USER_MESSAGE(msg, arg1, arg2, arg3); - USER_MESSAGE("\n"); - errors++; - return 0; -} - -/* Return the line at which an error occurred. - The error is not necessarily associated with the current spot - in the input stack, so LINE says where. LINE will have been - copied from ip->lineno for the current input level. - If the current level is for a file, we return LINE. - But if the current level is not for a file, LINE is meaningless. - In that case, we return the lineno of the innermost file. */ -int -line_for_error (line) - int line; -{ - int i; - int line1 = line; - - fprintf(fp_out, "line for error\n"); - - for (i = indepth; i >= 0; ) { - if (instack[i].fname != 0) - return line1; - i--; - if (i < 0) - return 0; - line1 = instack[i].lineno; - } -} - -/* - * If OBUF doesn't have NEEDED bytes after OPTR, make it bigger. - * - * As things stand, nothing is ever placed in the output buffer to be - * removed again except when it's KNOWN to be part of an identifier, - * so flushing and moving down everything left, instead of expanding, - * should work ok. - */ - -int -grow_outbuf (obuf, needed) - register FILE_BUF *obuf; - register int needed; -{ - register U_CHAR *p; - int minsize; - - if (obuf->length - (obuf->bufp - obuf->buf) > needed) - return 1; - - /* Make it at least twice as big as it is now. */ - obuf->length *= 2; - /* Make it have at least 150% of the free space we will need. */ - minsize = (3 * needed) / 2 + (obuf->bufp - obuf->buf); - if (minsize > obuf->length) - obuf->length = minsize; - - if ((p = (U_CHAR *) xrealloc (obuf->buf, obuf->length)) == NULL) - memory_full (); - - obuf->bufp = p + (obuf->bufp - obuf->buf); - obuf->buf = p; -} - -/* Symbol table for macro names and special symbols */ - -/* - * install a name in the main hash table, even if it is already there. - * name stops with first non alphanumeric, except leading '#'. - * caller must check against redefinition if that is desired. - * delete_macro () removes things installed by install () in fifo order. - * this is important because of the `defined' special symbol used - * in #if, and also if pushdef/popdef directives are ever implemented. - * - * If LEN is >= 0, it is the length of the name. - * Otherwise, compute the length by scanning the entire name. - * - * If HASH is >= 0, it is the precomputed hash code. - * Otherwise, compute the hash code. - */ -HASHNODE * -install (name, len, type, value, hash) - U_CHAR *name; - int len; - enum node_type type; - int value; - int hash; - /* watch out here if sizeof (U_CHAR *) != sizeof (int) */ -{ - register HASHNODE *hp; - register int i, bucket; - register U_CHAR *p, *q; - - if (len < 0) { - p = name; - while (is_idchar[*p]) - p++; - len = p - name; - } - - if (hash < 0) - hash = hashf (name, len, HASHSIZE); - - i = sizeof (HASHNODE) + len + 1; - hp = (HASHNODE *) xmalloc (i); - bucket = hash; - hp->bucket_hdr = &hashtab[bucket]; - hp->next = hashtab[bucket]; - hashtab[bucket] = hp; - hp->prev = NULL; - if (hp->next != NULL) - hp->next->prev = hp; - hp->type = type; - hp->length = len; - hp->value.ival = value; - hp->name = ((U_CHAR *) hp) + sizeof (HASHNODE); - p = hp->name; - q = name; - for (i = 0; i < len; i++) - *p++ = *q++; - hp->name[len] = 0; - return hp; -} - -/* - * find the most recent hash node for name name (ending with first - * non-identifier char) installed by install - * - * If LEN is >= 0, it is the length of the name. - * Otherwise, compute the length by scanning the entire name. - * - * If HASH is >= 0, it is the precomputed hash code. - * Otherwise, compute the hash code. - */ -HASHNODE * -lookup (name, len, hash) - U_CHAR *name; - int len; - int hash; -{ - register U_CHAR *bp; - register HASHNODE *bucket; - - if (len < 0) { - for (bp = name; is_idchar[*bp]; bp++) ; - len = bp - name; - } - - if (hash < 0) - hash = hashf (name, len, HASHSIZE); - - bucket = hashtab[hash]; - while (bucket) { - if (bucket->length == len && strncmp (bucket->name, name, len) == 0) - return bucket; - bucket = bucket->next; - } - return NULL; -} - -/* - * Delete a hash node. Some weirdness to free junk from macros. - * More such weirdness will have to be added if you define more hash - * types that need it. - */ - -/* Note that the DEFINITION of a macro is removed from the hash table - but its storage is not freed. This would be a storage leak - except that it is not reasonable to keep undefining and redefining - large numbers of macros many times. - In any case, this is necessary, because a macro can be #undef'd - in the middle of reading the arguments to a call to it. - If #undef freed the DEFINITION, that would crash. */ - -delete_macro (hp) - HASHNODE *hp; -{ - - if (hp->prev != NULL) - hp->prev->next = hp->next; - if (hp->next != NULL) - hp->next->prev = hp->prev; - - /* make sure that the bucket chain header that - the deleted guy was on points to the right thing afterwards. */ - if (hp == *hp->bucket_hdr) - *hp->bucket_hdr = hp->next; - -#if 0 - if (hp->type == T_MACRO) { - DEFINITION *d = hp->value.defn; - struct reflist *ap, *nextap; - - for (ap = d->pattern; ap != NULL; ap = nextap) { - nextap = ap->next; - free (ap); - } - free (d); - } -#endif - free (hp); -} - -/* - * return hash function on name. must be compatible with the one - * computed a step at a time, elsewhere - */ -int -hashf (name, len, hashsize) - register U_CHAR *name; - register int len; - int hashsize; -{ - register int r = 0; - - while (len--) - r = HASHSTEP (r, *name++); - - return MAKE_POS (r) % hashsize; -} - -/* Dump all macro definitions as #defines to stdout. */ - -void -dump_all_macros () -{ - int bucket; - - for (bucket = 0; bucket < HASHSIZE; bucket++) { - register HASHNODE *hp; - - for (hp = hashtab[bucket]; hp; hp= hp->next) { - if (hp->type == T_MACRO) { - register DEFINITION *defn = hp->value.defn; - struct reflist *ap; - int offset; - int concat; - - - /* Print the definition of the macro HP. */ - - fprintf(fp_out, "#define %s", hp->name); - if (defn->nargs >= 0) { - int i; - - fprintf(fp_out, "("); - for (i = 0; i < defn->nargs; i++) { - dump_arg_n (defn, i); - if (i + 1 < defn->nargs) - fprintf(fp_out, ", "); - } - fprintf(fp_out, ")"); - } - - fprintf(fp_out, " "); - - offset = 0; - concat = 0; - for (ap = defn->pattern; ap != NULL; ap = ap->next) { - dump_defn_1 (defn->expansion, offset, ap->nchars); - if (ap->nchars != 0) - concat = 0; - offset += ap->nchars; - if (ap->stringify) - fprintf(fp_out, " #"); - if (ap->raw_before && !concat) - fprintf(fp_out, " ## "); - concat = 0; - dump_arg_n (defn, ap->argno); - if (ap->raw_after) { - fprintf(fp_out, " ## "); - concat = 1; - } - } - dump_defn_1 (defn->expansion, offset, defn->length - offset); - fprintf(fp_out, "\n"); - } - } - } -} - -/* Output to stdout a substring of a macro definition. - BASE is the beginning of the definition. - Output characters START thru LENGTH. - Discard newlines outside of strings, thus - converting funny-space markers to ordinary spaces. */ - -dump_defn_1 (base, start, length) - U_CHAR *base; - int start; - int length; -{ - U_CHAR *p = base + start; - U_CHAR *limit = base + start + length; - - while (p < limit) { - if (*p != '\n') - putc (*p, fp_out ? fp_out : stdout); - else if (*p == '\"' || *p =='\'') { - U_CHAR *p1 = skip_quoted_string (p, limit, 0, 0, 0, 0); - fwrite (p, p1 - p, 1, stdout); - p = p1 - 1; - } - p++; - } -} - -/* Print the name of argument number ARGNUM of macro definition DEFN. - Recall that DEFN->argnames contains all the arg names - concatenated in reverse order with comma-space in between. */ - -dump_arg_n (defn, argnum) - DEFINITION *defn; - int argnum; -{ - register U_CHAR *p = defn->argnames; - while (argnum + 1 < defn->nargs) { - p = (U_CHAR *) index (p, ' ') + 1; - argnum++; - } - - while (*p && *p != ',') { - putc (*p, fp_out ? fp_out : stdout); - p++; - } -} - -/* Initialize syntactic classifications of characters. */ - -initialize_char_syntax () -{ - register int i; - - /* - * Set up is_idchar and is_idstart tables. These should be - * faster than saying (is_alpha (c) || c == '_'), etc. - * Must do set up these things before calling any routines tthat - * refer to them. - */ - for (i = 'a'; i <= 'z'; i++) { - is_idchar[i - 'a' + 'A'] = 1; - is_idchar[i] = 1; - is_idstart[i - 'a' + 'A'] = 1; - is_idstart[i] = 1; - } - for (i = '0'; i <= '9'; i++) - is_idchar[i] = 1; - is_idchar['_'] = 1; - is_idstart['_'] = 1; - is_idchar['$'] = dollars_in_ident; - is_idstart['$'] = dollars_in_ident; - - /* horizontal space table */ - is_hor_space[' '] = 1; - is_hor_space['\t'] = 1; - is_hor_space['\v'] = 1; - is_hor_space['\f'] = 1; - - is_space[' '] = 1; - is_space['\t'] = 1; - is_space['\v'] = 1; - is_space['\f'] = 1; - is_space['\n'] = 1; -} - -/* Initialize the built-in macros. */ - -initialize_builtins () -{ - install ("__LINE__", -1, T_SPECLINE, 0, -1); - install ("__DATE__", -1, T_DATE, 0, -1); - install ("__FILE__", -1, T_FILE, 0, -1); - install ("__BASE_FILE__", -1, T_BASE_FILE, 0, -1); - install ("__INCLUDE_LEVEL__", -1, T_INCLUDE_LEVEL, 0, -1); - install ("__VERSION__", -1, T_VERSION, 0, -1); - install ("__TIME__", -1, T_TIME, 0, -1); - install ("__ANSI_CPP__", -1, T_CONST, 1, -1); -/*if (!traditional) - install ("__STDC__", -1, T_CONST, 1, -1); */ -/* install ("__GNU__", -1, T_CONST, 1, -1); */ -/* This is supplied using a -D by the compiler driver - so that it is present only when truly compiling with GNU C. */ -} - -/* - * process a given definition string, for initialization - * If STR is just an identifier, define it with value 1. - * If STR has anything after the identifier, then it should - * be identifier-space-definition. - */ -make_definition (str) - U_CHAR *str; -{ - FILE_BUF *ip; - struct directive *kt; - U_CHAR *buf, *p; - - buf = str; - p = str; - while (is_idchar[*p]) p++; - if (*p == 0) { - buf = (U_CHAR *) alloca (p - buf + 4); - strcpy (buf, str); - strcat (buf, " 1"); - } - - ip = &instack[++indepth]; - ip->fname = "*Initialization*"; - - ip->buf = ip->bufp = buf; - ip->length = strlen (buf); - ip->lineno = 1; - ip->macro = 0; - ip->free_ptr = 0; - ip->if_stack = if_stack; - - for (kt = directive_table; kt->type != T_DEFINE; kt++) - ; - - /* pass NULL as output ptr to do_define since we KNOW it never - does any output.... */ - do_define (buf, buf + strlen (buf) , NULL, kt); - --indepth; -} - -/* JF, this does the work for the -U option */ -make_undef (str) - U_CHAR *str; -{ - FILE_BUF *ip; - struct directive *kt; - - ip = &instack[++indepth]; - ip->fname = "*undef*"; - - ip->buf = ip->bufp = str; - ip->length = strlen (str); - ip->lineno = 1; - ip->macro = 0; - ip->free_ptr = 0; - ip->if_stack = if_stack; - - for (kt = directive_table; kt->type != T_UNDEF; kt++) - ; - - do_undef (str,str + strlen (str) - 1, NULL, kt); - --indepth; -} - -/* Add output to `deps_buffer' for the -M switch. - STRING points to the text to be output. - SIZE is the number of bytes, or 0 meaning output until a null. - If SIZE is nonzero, we break the line first, if it is long enough. */ - -deps_output (string, size) - char *string; - int size; -{ -#ifndef MAX_OUTPUT_COLUMNS -#define MAX_OUTPUT_COLUMNS 75 -#endif -#ifndef sgi - if (size != 0 && deps_column != 0 - && size + deps_column > MAX_OUTPUT_COLUMNS) { - deps_output ("\\\n ", 0); - deps_column = 0; - } -#endif - if (size == 0) - size = strlen (string); - - if (deps_size + size + 1 > deps_allocated_size) { - deps_allocated_size = deps_size + size + 50; - deps_allocated_size *= 2; - deps_buffer = (char *) xrealloc (deps_buffer, deps_allocated_size); - } - bcopy (string, &deps_buffer[deps_size], size); - deps_size += size; - deps_column += size; - deps_buffer[deps_size] = 0; -} - -#if !defined BSD && !defined __APPLE__ -#ifndef BSTRING - -void -bzero (b, length) - register char *b; - register int length; -{ -#ifdef VMS - short zero = 0; - long max_str = 65535; - - while (length > max_str) { - (void) LIB$MOVC5 (&zero, &zero, &zero, &max_str, b); - length -= max_str; - b += max_str; - } - (void) LIB$MOVC5 (&zero, &zero, &zero, &length, b); -#else - while (length-- > 0) - *b++ = 0; -#endif /* not VMS */ -} - -void -bcopy (b1, b2, length) - register char *b1; - register char *b2; - register int length; -{ -#ifdef VMS - long max_str = 65535; - - while (length > max_str) { - (void) LIB$MOVC3 (&max_str, b1, b2); - length -= max_str; - b1 += max_str; - b2 += max_str; - } - (void) LIB$MOVC3 (&length, b1, b2); -#else - while (length-- > 0) - *b2++ = *b1++; -#endif /* not VMS */ -} - -int -bcmp (b1, b2, length) /* This could be a macro! */ - register char *b1; - register char *b2; - register int length; -{ -#ifdef VMS - struct dsc$descriptor_s src1 = {length, DSC$K_DTYPE_T, DSC$K_CLASS_S, b1}; - struct dsc$descriptor_s src2 = {length, DSC$K_DTYPE_T, DSC$K_CLASS_S, b2}; - - return STR$COMPARE (&src1, &src2); -#else - while (length-- > 0) - if (*b1++ != *b2++) - return 1; - - return 0; -#endif /* not VMS */ -} -#endif /* not BSTRING */ -#endif /* not BSD */ - - -void -fatal (str, arg) - char *str, *arg; -{ - USER_MESSAGE("%s: ", acpp_progname); - USER_MESSAGE(str, arg); - USER_MESSAGE("\n"); -/* return (FATAL_EXIT_CODE);*/ -} - -void -perror_with_name (name) - char *name; -{ -#if !defined WIN32 && !defined __linux__ && !defined __APPLE__ - extern int errno, sys_nerr; - extern char *sys_errlist[]; -#endif - - USER_MESSAGE("%s: ", acpp_progname); - -#if defined WIN32 || defined __linux__ || defined __APPLE__ - perror(name); -#else - if (errno < sys_nerr) - USER_MESSAGE("%s: %s\n", name, sys_errlist[errno]); - else - USER_MESSAGE("%s: undocumented I/O error\n", name); -#endif - errors++; - -} - -void -pfatal_with_name (name) - char *name; -{ - perror_with_name (name); -/* return (FATAL_EXIT_CODE);*/ -} - - -void -memory_full () -{ - fatal ("Memory exhausted."); -} - - -char * -xmalloc (size) - int size; -{ - /*extern char *malloc ();*/ - register char *ptr = malloc (size); - if (ptr != 0) return (ptr); - memory_full (); - /*NOTREACHED*/ -} - -char * -xrealloc (old, size) - char *old; - int size; -{ - /*extern char *realloc ();*/ - register char *ptr = realloc (old, size); - if (ptr != 0) return (ptr); - memory_full (); - /*NOTREACHED*/ -} - -char * -xcalloc (number, size) - int number, size; -{ - /*extern char *malloc ();*/ - register int total = number * size; - register char *ptr = malloc (total); - if (ptr != 0) { - if (total > 100) - bzero (ptr, total); - else { - /* It's not too long, so loop, zeroing by longs. - It must be safe because malloc values are always well aligned. */ - register long *zp = (long *) ptr; - register long *zl = (long *) (ptr + total - 4); - register int i = total - 4; - while (zp < zl) - *zp++ = 0; - if (i < 0) - i = 0; - while (i < total) - ptr[i++] = 0; - } - return ptr; - } - memory_full (); - /*NOTREACHED*/ -} - -char * -savestring (input) - char *input; -{ - int size = strlen (input); - char *output = xmalloc (size + 1); - strcpy (output, input); - return output; -} - -/* Get the file-mode and data size of the file open on FD - and store them in *MODE_POINTER and *SIZE_POINTER. */ - -int -file_size_and_mode (fd, mode_pointer, size_pointer) - int fd; - int *mode_pointer; - long int *size_pointer; -{ - struct stat sbuf; - - if (fstat (fd, &sbuf) < 0) return (-1); - if (mode_pointer) *mode_pointer = sbuf.st_mode; - if (size_pointer) *size_pointer = sbuf.st_size; - return 0; -} - -#ifdef VMS - -/* Under VMS we need to fix up the "include" specification - filename so that everything following the 1st slash is - changed into its correct VMS file specification. */ - -hack_vms_include_specification (fname) - char *fname; -{ - register char *cp, *cp1, *cp2; - char Local[512]; - extern char *index (), *rindex (); - - /* Ignore leading "./"s */ - while (fname[0] == '.' && fname[1] == '/') - strcpy (fname, fname+2); - /* Look for the boundary between the VMS and UNIX filespecs */ - cp = rindex (fname, ']'); /* Look for end of dirspec. */ - if (cp == 0) cp == rindex (fname, '>'); /* ... Ditto */ - if (cp == 0) cp == rindex (fname, ':'); /* Look for end of devspec. */ - if (cp) { - cp++; - } else { - cp = index (fname, '/'); /* Look for the "/" */ - } - /* See if we found that 1st slash */ - if (cp == 0) return; /* Nothing to do!!! */ - if (*cp != '/') return; /* Nothing to do!!! */ - /* Point to the UNIX filename part (which needs to be fixed!) */ - cp1 = cp+1; - /* If the directory spec is not rooted, we can just copy - the UNIX filename part and we are done */ - if (((cp - fname) > 2) - && ((cp[-1] == ']') || (cp[-1] == '>')) - && (cp[-2] != '.')) { - strcpy (cp, cp1); - return; - } - /* If there are no other slashes then the filename will be - in the "root" directory. Otherwise, we need to add - directory specifications. */ - if (index (cp1, '/') == 0) { - /* Just add "[000000]" as the directory string */ - strcpy (Local, "[000000]"); - cp2 = Local + strlen (Local); - } else { - /* Open the directory specification */ - cp2 = Local; - *cp2++ = '['; - /* As long as there are still subdirectories to add, do them. */ - while (index (cp1, '/') != 0) { - /* If this token is "." we can ignore it */ - if ((cp1[0] == '.') && (cp1[1] == '/')) { - cp1 += 2; - continue; - } - /* Add a subdirectory spec. */ - if (cp2 != Local+1) *cp2++ = '.'; - /* If this is ".." then the spec becomes "-" */ - if ((cp1[0] == '.') && (cp1[1] == '.') && (cp[2] == '/')) { - /* Add "-" and skip the ".." */ - *cp2++ = '-'; - cp1 += 3; - continue; - } - /* Copy the subdirectory */ - while (*cp1 != '/') *cp2++= *cp1++; - cp1++; /* Skip the "/" */ - } - /* Close the directory specification */ - *cp2++ = ']'; - } - /* Now add the filename */ - while (*cp1) *cp2++ = *cp1++; - *cp2 = 0; - /* Now append it to the original VMS spec. */ - strcpy (cp, Local); - return; -} -#endif /* VMS */ - -#ifdef VMS - -/* These are the read/write replacement routines for - VAX-11 "C". They make read/write behave enough - like their UNIX counterparts that CCCP will work */ - -int -read (fd, buf, size) - int fd; - char *buf; - int size; -{ -#undef read /* Get back the REAL read routine */ - register int i; - register int total = 0; - - /* Read until the buffer is exhausted */ - while (size > 0) { - /* Limit each read to 32KB */ - i = (size > (32*1024)) ? (32*1024) : size; - i = read (fd, buf, i); - if (i <= 0) { - if (i == 0) return (total); - return(i); - } - /* Account for this read */ - total += i; - buf += i; - size -= i; - } - return (total); -} - -int -write (fd, buf, size) - int fd; - char *buf; - int size; -{ -#undef write /* Get back the REAL write routine */ - int i; - int j; - - /* Limit individual writes to 32Kb */ - i = size; - while (i > 0) { - j = (i > (32*1024)) ? (32*1024) : i; - if (write (fd, buf, j) < 0) return (-1); - /* Account for the data written */ - buf += j; - i -= j; - } - return (size); -} - -#endif /* VMS */ diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/cexp.y b/OpenSim/Utilities/simmToOpenSim/acpp/src/cexp.y deleted file mode 100644 index 7c38dc1765..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/cexp.y +++ /dev/null @@ -1,839 +0,0 @@ -/* Parse C expressions for CCCP. - Copyright (C) 1987 Free Software Foundation. - -This program is free software; you can redistribute it and/or modify it -under the terms of the GNU General Public License as published by the -Free Software Foundation; either version 1, or (at your option) any -later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. - - In other words, you are welcome to use, share and improve this program. - You are forbidden to forbid anyone else to use, share and improve - what you give them. Help stamp out software-hoarding! - - Adapted from expread.y of GDB by Paul Rubin, July 1986. - -/* Parse a C expression from text in a string */ - -%{ -#include "stdio.h" -#include "stdlib.h" -#include "math.h" -#include "config.h" -#ifdef __linux__ -#include "strings.h" -#endif -#include -#include -/* #define YYDEBUG 1 */ - - int yylex (); - void yyerror (); - int inside_math = 0; - int expression_value; - double math_value; - - extern FILE* fp_out; - - static jmp_buf parse_return_error; - - /* some external tables of character types */ - extern unsigned char is_idstart[], is_idchar[]; -#if defined sgi || defined WIN32 || defined __linux__ -/* table to tell if c is horizontal space. isspace () thinks that - newline is space; this is not a good idea for this program. */ -char is_hor_space[256]; -#endif - -#if defined WIN32 || defined __linux__ -#define gettxt(S1,S2) (S1 ## S2) -#endif - -#ifndef CHAR_TYPE_SIZE -#define CHAR_TYPE_SIZE BITS_PER_UNIT -#endif - -acpp_error (msg) -{ -#ifdef INSIDE_SIMM - error(0, msg); // 0 = SIMM error action of "recover" -#else - fprintf (stderr, "error: %s\n", msg); -#endif -} - -acpp_warning (msg) -{ -#ifdef INSIDE_SIMM - error(0, msg); // 0 = SIMM error action of "recover" -#else - fprintf (stderr, "warning: %s\n", msg); -#endif -} -%} - -%union { - struct constant {long value; int unsignedp;} integer; - int voidval; - char *sval; - double dval; -} - -%type exp exp1 start -%type exp3 -%token INT CHAR -%token NAME -%token ERROR -%token DVALUE - -%right '?' ':' -%left ',' -%left OR -%left AND -%left '|' -%left '^' -%left '&' -%left EQUAL NOTEQUAL -%left '<' '>' LEQ GEQ -%left LSH RSH -%left '+' '-' -%left '*' '/' '%' -%right UNARY - -/* %expect 40 */ - -%% - -start : exp1 - { expression_value = $1.value; math_value = 0.0; } - | exp3 - { math_value = $1; expression_value = 0; } - ; - -/* Expressions, including the comma operator. */ -exp1 : exp - | exp1 ',' exp - { $$ = $3; } - ; - -/* Expressions, not including the comma operator. */ -exp : '-' exp %prec UNARY - { $$.value = - $2.value; - $$.unsignedp = $2.unsignedp; } - | '!' exp %prec UNARY - { $$.value = ! $2.value; - $$.unsignedp = 0; } - | '~' exp %prec UNARY - { $$.value = ~ $2.value; - $$.unsignedp = $2.unsignedp; } - | '(' exp1 ')' - { $$ = $2; } - ; - -/* Unary floating point expressions */ -exp3 : '-' exp3 %prec UNARY - { $$ = - $2; } - | '(' exp3 ')' - { $$ = $2; } - | '{' exp3 '}' - { $$ = $2; } - ; - -/* Binary floating point operators in order of decreasing precedence. */ -exp3 : exp3 '*' exp3 - { $$ = $1 * $3; } - | exp3 '/' exp3 - { if ($3 == 0.0) - { - acpp_error ("division by zero in expression"); - $3 = 1.0; - } - $$ = $1 / $3; } - | exp3 '+' exp3 - { $$ = $1 + $3; } - | exp3 '-' exp3 - { $$ = $1 - $3; } - | DVALUE - { $$ = yylval.dval; } - ; - -/* Binary operators in order of decreasing precedence. */ -exp : exp '*' exp - { $$.unsignedp = $1.unsignedp || $3.unsignedp; - if ($$.unsignedp) - $$.value = (unsigned) $1.value * $3.value; - else - $$.value = $1.value * $3.value; } - | exp '/' exp - { if ($3.value == 0) - { - acpp_error ("division by zero in #if"); - $3.value = 1; - } - $$.unsignedp = $1.unsignedp || $3.unsignedp; - if ($$.unsignedp) - $$.value = (unsigned) $1.value / $3.value; - else - $$.value = $1.value / $3.value; } - | exp '%' exp - { if ($3.value == 0) - { - acpp_error ("division by zero in #if"); - $3.value = 1; - } - $$.unsignedp = $1.unsignedp || $3.unsignedp; - if ($$.unsignedp) - $$.value = (unsigned) $1.value % $3.value; - else - $$.value = $1.value % $3.value; } - | exp '+' exp - { $$.value = $1.value + $3.value; - $$.unsignedp = $1.unsignedp || $3.unsignedp; } - | exp '-' exp - { $$.value = $1.value - $3.value; - $$.unsignedp = $1.unsignedp || $3.unsignedp; } - | exp LSH exp - { $$.unsignedp = $1.unsignedp; - if ($$.unsignedp) - $$.value = (unsigned) $1.value << $3.value; - else - $$.value = $1.value << $3.value; } - | exp RSH exp - { $$.unsignedp = $1.unsignedp; - if ($$.unsignedp) - $$.value = (unsigned) $1.value >> $3.value; - else - $$.value = $1.value >> $3.value; } - | exp EQUAL exp - { $$.value = ($1.value == $3.value); - $$.unsignedp = 0; } - | exp NOTEQUAL exp - { $$.value = ($1.value != $3.value); - $$.unsignedp = 0; } - | exp LEQ exp - { $$.unsignedp = 0; - if ($1.unsignedp || $3.unsignedp) - $$.value = (unsigned) $1.value <= $3.value; - else - $$.value = $1.value <= $3.value; } - | exp GEQ exp - { $$.unsignedp = 0; - if ($1.unsignedp || $3.unsignedp) - $$.value = (unsigned) $1.value >= $3.value; - else - $$.value = $1.value >= $3.value; } - | exp '<' exp - { $$.unsignedp = 0; - if ($1.unsignedp || $3.unsignedp) - $$.value = (unsigned) $1.value < $3.value; - else - $$.value = $1.value < $3.value; } - | exp '>' exp - { $$.unsignedp = 0; - if ($1.unsignedp || $3.unsignedp) - $$.value = (unsigned) $1.value > $3.value; - else - $$.value = $1.value > $3.value; } - | exp '&' exp - { $$.value = $1.value & $3.value; - $$.unsignedp = $1.unsignedp || $3.unsignedp; } - | exp '^' exp - { $$.value = $1.value ^ $3.value; - $$.unsignedp = $1.unsignedp || $3.unsignedp; } - | exp '|' exp - { $$.value = $1.value | $3.value; - $$.unsignedp = $1.unsignedp || $3.unsignedp; } - | exp AND exp - { $$.value = ($1.value && $3.value); - $$.unsignedp = 0; } - | exp OR exp - { $$.value = ($1.value || $3.value); - $$.unsignedp = 0; } - | exp '?' exp ':' exp - { $$.value = $1.value ? $3.value : $5.value; - $$.unsignedp = $3.unsignedp || $5.unsignedp; } - | INT - { $$ = yylval.integer; } - | CHAR - { $$ = yylval.integer; } - | NAME - { $$.value = 0; - $$.unsignedp = 0; } - ; -%% - -/* During parsing of a C expression, the pointer to the next character - is in this variable. */ - -static char *lexptr; - -/* Take care of parsing a number (anything that starts with a digit). - Set yylval and return the token type; update lexptr. - LEN is the number of characters in it. */ - -int -parse_number (olen) - int olen; -{ - register char *p = lexptr; - register long n = 0; - register int c; - register int base = 10; - register int len = olen; - - /* Check to see if this is a floating point number or not */ - for (c = 0; c < len; c++) - { - if (p[c] == '.') - { - if (inside_math == 0) - { - yyerror ("floating point numbers not allowed in #if expressions"); - return ERROR; - } - else - { - char *np = (char*)alloca(len + 1); - bcopy(p, np, len); - np[len] = '\0'; - yylval.dval = atof(np); - lexptr += len; - return DVALUE; - } - } - } - - yylval.integer.unsignedp = 0; - - if (len >= 3 && (!strncmp (p, "0x", 2) || !strncmp (p, "0X", 2))) { - p += 2; - base = 16; - len -= 2; - } - else if (*p == '0') - base = 8; - - while (len > 0) { - c = *p++; - len--; - if (c >= 'A' && c <= 'Z') c += 'a' - 'A'; - - if (c >= '0' && c <= '9') { - n *= base; - n += c - '0'; - } else if (base == 16 && c >= 'a' && c <= 'f') { - n *= base; - n += c - 'a' + 10; - } else { - /* `l' means long, and `u' means unsigned. */ - while (1) { - if (c == 'l' || c == 'L') - ; - else if (c == 'u' || c == 'U') - yylval.integer.unsignedp = 1; - else - break; - - if (len == 0) - break; - c = *p++; - len--; - } - /* Don't look for any more digits after the suffixes. */ - break; - } - } - - if (len != 0) { - yyerror ("Invalid number in #if expression"); - return ERROR; - } - - lexptr = p; - - if (inside_math > 0) - { - yylval.dval = (double)n; - return DVALUE; - } - else - { - /* If too big to be signed, consider it unsigned. */ - if (n < 0) - yylval.integer.unsignedp = 1; - - yylval.integer.value = n; - return INT; - } - - return INT; -} - -struct token { - char *operator; - int token; -}; - -#undef NULL -#define NULL 0 - -static struct token tokentab2[] = { - {"&&", AND}, - {"||", OR}, - {"<<", LSH}, - {">>", RSH}, - {"==", EQUAL}, - {"!=", NOTEQUAL}, - {"<=", LEQ}, - {">=", GEQ}, - {NULL, ERROR} -}; - -/* Read one token, getting characters through lexptr. */ - -int -yylex () -{ - register int c; - register int namelen; - register char *tokstart; - register struct token *toktab; - - retry: - - tokstart = lexptr; - c = *tokstart; - /* See if it is a special token of length 2. */ - for (toktab = tokentab2; toktab->operator != NULL; toktab++) - if (c == *toktab->operator && tokstart[1] == toktab->operator[1]) { - lexptr += 2; - return toktab->token; - } - - switch (c) { - case 0: - return 0; - - case ' ': - case '\t': - case '\n': - lexptr++; - goto retry; - -#ifdef sgi - case 'L': - { - /* look for wide char constants */ - int c2; - int i; - for(i = 1, c2 = *(lexptr+i); is_hor_space[c2]; - ++i,c2 = *(lexptr+i)) - /* do nothing on horiz space stuff */ { } - if(c2 != '\'') { - /* Not a wide char const. An identifier */ - break; - } - lexptr += i ; /* points to the quote now - Is wide char constant */ - tokstart = lexptr; - c = c2; /* c now a quote */ - } -#endif - case '\'': - lexptr++; - c = *lexptr++; - if (c == '\\') - c = parse_escape (&lexptr); - - /* Sign-extend the constant if chars are signed on target machine. */ - { - if (lookup ("__CHAR_UNSIGNED__", sizeof ("__CHAR_UNSIGNED__")-1, -1) - || ((c >> (CHAR_TYPE_SIZE - 1)) & 1) == 0) - yylval.integer.value = c & ((1 << CHAR_TYPE_SIZE) - 1); - else - yylval.integer.value = c | ~((1 << CHAR_TYPE_SIZE) - 1); - } - - yylval.integer.unsignedp = 0; - c = *lexptr++; - if (c != '\'') { - yyerror ("Invalid character constant in #if"); - return ERROR; - } - - return CHAR; - - /* some of these chars are invalid in constant expressions; - maybe do something about them later */ - case '/': - case '+': - case '-': - case '*': - case '%': - case '|': - case '&': - case '^': - case '~': - case '!': - case '@': - case '<': - case '>': - case '(': - case ')': - case '[': - case ']': - case '.': - case '?': - case ':': - case '=': - case '{': - case '}': - case ',': - lexptr++; - return c; - - case '"': - yyerror ("double quoted strings not allowed in #if expressions"); - return ERROR; - } - - /* If you are not inside a math expression, then numbers have - * to be integers. Thus they should contain only numerals from 0 to 9. - * However, a decimal point is allowed because it is checked for - * later (inside parse_number). - * If you are inside a math expression, then the number can start with - * a numeral or decimal point, and can contain a trailing exponent. - */ - if ((c >= '0' && c <= '9') || c == '.') - { - if (inside_math == 0) - { - for (namelen = 0; - c = tokstart[namelen], is_idchar[c] || c == '.'; - namelen++) - ; - } - else - { - int exp = 0; - for (namelen = 0; - c = tokstart[namelen], is_idchar[c] || c == '.' || (exp == 1 && (c == '-' || c == '+')); - namelen++) - { - if (exp == 0 && (c == 'e' || c == 'E' || c == 'g' || c == 'G')) - exp = 1; - else if (exp == 1 && (c == '-' || c == '+')) - exp = 0; - } - } - - return parse_number (namelen); - } - - if (!is_idstart[c]) { - yyerror ("Invalid token in expression"); - return ERROR; - } - - /* It is a name. See how long it is. */ - - for (namelen = 0; is_idchar[tokstart[namelen]]; namelen++) - ; - - lexptr += namelen; - return NAME; -} - - -/* Parse a C escape sequence. STRING_PTR points to a variable - containing a pointer to the string to parse. That pointer - is updated past the characters we use. The value of the - escape sequence is returned. - - A negative value means the sequence \ newline was seen, - which is supposed to be equivalent to nothing at all. - - If \ is followed by a null character, we return a negative - value and leave the string pointer pointing at the null character. - - If \ is followed by 000, we return 0 and leave the string pointer - after the zeros. A value of 0 does not mean end of string. */ - -int -parse_escape (string_ptr) - char **string_ptr; -{ - register int c = *(*string_ptr)++; - switch (c) - { - case 'a': - return TARGET_BELL; - case 'b': - return TARGET_BS; - case 'e': - return 033; - case 'f': - return TARGET_FF; - case 'n': - return TARGET_NEWLINE; - case 'r': - return TARGET_CR; - case 't': - return TARGET_TAB; - case 'v': - return TARGET_VT; - case '\n': - return -2; - case 0: - (*string_ptr)--; - return 0; - case '^': - c = *(*string_ptr)++; - if (c == '\\') - c = parse_escape (string_ptr); - if (c == '?') - return 0177; - return (c & 0200) | (c & 037); - - case '0': - case '1': - case '2': - case '3': - case '4': - case '5': - case '6': - case '7': - { - register int i = c - '0'; - register int count = 0; - while (++count < 3) - { - c = *(*string_ptr)++; - if (c >= '0' && c <= '7') - i = (i << 3) + c - '0'; - else - { - (*string_ptr)--; - break; - } - } - if ((i & ~((1 << CHAR_TYPE_SIZE) - 1)) != 0) - { - i &= (1 << CHAR_TYPE_SIZE) - 1; - acpp_warning ("octal character constant does not fit in a byte"); - } - return i; - } - case 'x': - { - register int i = 0; - register int count = 0; - for (;;) - { - c = *(*string_ptr)++; - if (c >= '0' && c <= '9') - i = (i << 4) + c - '0'; - else if (c >= 'a' && c <= 'f') - i = (i << 4) + c - 'a' + 10; - else if (c >= 'A' && c <= 'F') - i = (i << 4) + c - 'A' + 10; - else - { - (*string_ptr)--; - break; - } - } - if ((i & ~((1 << BITS_PER_UNIT) - 1)) != 0) - { - i &= (1 << BITS_PER_UNIT) - 1; - acpp_warning ("hex character constant does not fit in a byte"); - } - return i; - } - default: - return c; - } -} - -void -yyerror (s) - char *s; -{ - acpp_error (s); - longjmp (parse_return_error, 1); -} - -/* This page contains the entry point to this file. */ - -#if 0 -/* THIS DOESN'T EVEN BEGIN TO WORK */ -/* Parse STRING as an assertion (the AT&T ANSI cpp extension), and complain if - * this fails to use up all of the contents of STRING */ -int -parse_assertion_extension (buf) - char *buf; -{ - int token; - int c; - int negate = 0; - int value = 0; - - for(c = *buf; c != '#'; c = *buf++) { - if (c == '!') - negate = !negate; - else if(!isspace(c)) - acpp_error("Unexpected character in assertion expression\n"); - } - - /* buf is now one past the '#' character */ - lexptr = buf; - - if((token = yylex()) != NAME) - acpp_error("Syntax for assertion conditionals is '#if #assertion-name(tokens)'\n"); - - /* for the moment - check the syntax, but don't actually *do* anything. */ - return (negate ? !value : value); -} -#endif - -/* Parse STRING as an expression, and complain if this fails - to use up all of the contents of STRING. */ -/* We do not support C comments. They should be removed before - this function is called. */ - -int -parse_c_expression (string) - char *string; -{ - lexptr = string; - - if (lexptr == 0 || *lexptr == 0) { - acpp_error ("empty #if expression"); - return 0; /* don't include the #if group */ - } - - /* if there is some sort of scanning error, just return 0 and assume - the parsing routine has printed an error message somewhere. - there is surely a better thing to do than this. */ - if (setjmp (parse_return_error)) - return 0; - - if (yyparse ()) - return 0; /* actually this is never reached - the way things stand. */ - if (*lexptr) - acpp_error ("Junk after end of expression."); - - return expression_value; /* set by yyparse () */ -} - -double -parse_c_math_expression (string) - char *string; -{ - lexptr = string; - - if (lexptr == 0 || *lexptr == 0) { - acpp_error ("empty #if expression"); - return 0; /* don't include the #if group */ - } - - /* if there is some sort of scanning error, just return 0 and assume - the parsing routine has printed an error message somewhere. - there is surely a better thing to do than this. */ - if (setjmp (parse_return_error)) - return 0; - - if (yyparse ()) - return 0; /* actually this is never reached - the way things stand. */ - if (*lexptr) - acpp_error ("Junk after end of expression."); - - return math_value + (double)expression_value; /* set by yyparse () */ -} - -#ifdef TEST_EXP_READER -/* main program, for testing purposes. */ -main () -{ - int n, c; - char buf[1024]; - extern int yydebug; -/* - yydebug = 1; -*/ - initialize_random_junk (); - - for (;;) { - printf ("enter expression: "); - n = 0; - while ((buf[n] = getchar ()) != '\n' && buf[n] != EOF) - n++; - if (buf[n] == EOF) - break; - buf[n] = '\0'; - printf ("parser returned %d\n", parse_c_expression (buf)); - } -} - -/* table to tell if char can be part of a C identifier. */ -unsigned char is_idchar[256]; -/* table to tell if char can be first char of a c identifier. */ -unsigned char is_idstart[256]; -#ifndef sgi -/* table to tell if c is horizontal space. isspace () thinks that - newline is space; this is not a good idea for this program. */ -char is_hor_space[256]; -#endif - -/* - * initialize random junk in the hash table and maybe other places - */ -initialize_random_junk () -{ - register int i; - - /* - * Set up is_idchar and is_idstart tables. These should be - * faster than saying (is_alpha (c) || c == '_'), etc. - * Must do set up these things before calling any routines tthat - * refer to them. - */ - for (i = 'a'; i <= 'z'; i++) { - ++is_idchar[i - 'a' + 'A']; - ++is_idchar[i]; - ++is_idstart[i - 'a' + 'A']; - ++is_idstart[i]; - } - for (i = '0'; i <= '9'; i++) - ++is_idchar[i]; - ++is_idchar['_']; - ++is_idstart['_']; -#ifdef DOLLARS_IN_IDENTIFIERS - ++is_idchar['$']; - ++is_idstart['$']; -#endif - - /* horizontal space table */ - ++is_hor_space[' ']; - ++is_hor_space['\t']; -} - -struct hashnode * -lookup (name, len, hash) - char *name; - int len; - int hash; -{ - return (DEFAULT_SIGNED_CHAR) ? 0 : ((struct hashnode *) -1); -} -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/config.h b/OpenSim/Utilities/simmToOpenSim/acpp/src/config.h deleted file mode 100644 index f83cebf586..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/config.h +++ /dev/null @@ -1,37 +0,0 @@ -/* Configuration for GNU C-compiler for MIPS Rx000 family - Copyright (C) 1987 Free Software Foundation, Inc. - -This file is part of GNU CC. - -GNU CC is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 1, or (at your option) -any later version. - -GNU CC is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with GNU CC; see the file COPYING. If not, write to -the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. */ - - -/* #defines that need visibility everywhere. */ -#define FALSE 0 -#define TRUE 1 - -/* This describes the machine the compiler is hosted on. */ -#define HOST_BITS_PER_CHAR 8 -#define HOST_BITS_PER_SHORT 16 -#define HOST_BITS_PER_INT 32 -#define HOST_BITS_PER_LONG 32 - -/* target machine dependencies. - tm.h is a symbolic link to the actual target specific file. */ -#include "tm.h" - -/* Arguments to use with `exit'. */ -#define SUCCESS_EXIT_CODE 0 -#define FATAL_EXIT_CODE 33 diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/make_depend.h b/OpenSim/Utilities/simmToOpenSim/acpp/src/make_depend.h deleted file mode 100644 index 10762ff024..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/make_depend.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef __MAKE_DEPEND_H__ -#define __MAKE_DEPEND_H__ -/* - * Copyright 1991 Silicon Graphics, Inc. All rights reserved. - * - * Integrated 'make depend' support. - */ - -/* - * An opaque handle on the structure used to update a single target's make - * dependencies rule. Each target built in a directory has one rule in an - * automatically-generated make-dependencies file, usually named Makedepend - * and sincluded by the directory's makefile. Each rule relates the target - * to all source files that it #includes and all libraries/runtimes that it - * links against. - */ -typedef struct mdrule *MDhandle; - -/* - * Call MDopen(toolname, filename, target, errorfunc) to open a make rule - * for target in filename. Target may be null if the calling tool does not - * yet know its output file's name. Errorfunc is called for malloc failure - * and system call errors. Toolname is used to distinguish link-editor and - * include-file dependencies for the same target (e.g., a command made using - * a null-suffix rule). - * - * MDupdate(handle, dependency) adds the named dependency to the right part - * of target's rule. Dependency must be in persistent store. - * - * MDclose(handle, target) updates the dependencies file named by MDopen's - * filename argument. If a null target was passed to MDopen, a non-null one - * must be passed to MDclose. MDclose locks filename, updates target's rule - * in it or adds a new rule if target had none, and unlocks filename. Lines - * in filename not matching '^target *:' are left alone. - */ -extern MDhandle MDopen(char *, char *, char *, void (*)(char *, ...)); -extern void MDupdate(MDhandle, char *); -extern void MDclose(MDhandle, char *); - -#endif /* __MAKE_DEPEND_H__ */ diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/obstack.c b/OpenSim/Utilities/simmToOpenSim/acpp/src/obstack.c deleted file mode 100644 index 40accd496d..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/obstack.c +++ /dev/null @@ -1,329 +0,0 @@ -/* obstack.c - subroutines used implicitly by object stack macros - Copyright (C) 1988 Free Software Foundation, Inc. - -This program is free software; you can redistribute it and/or modify it -under the terms of the GNU General Public License as published by the -Free Software Foundation; either version 1, or (at your option) any -later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. - - -In other words, you are welcome to use, share and improve this program. -You are forbidden to forbid anyone else to use, share and improve -what you give them. Help stamp out software-hoarding! */ - - -#include "obstack.h" -#ifdef __linux__ -#include "stdlib.h" -#endif - -#ifdef __STDC__ -#define POINTER void * -#else -#define POINTER char * -#endif - -/* Determine default alignment. */ -struct fooalign {char x; double d;}; -#define DEFAULT_ALIGNMENT ((char *)&((struct fooalign *) 0)->d - (char *)0) -/* If malloc were really smart, it would round addresses to DEFAULT_ALIGNMENT. - But in fact it might be less smart and round addresses to as much as - DEFAULT_ROUNDING. So we prepare for it to do that. */ -union fooround {long x; double d;}; -#define DEFAULT_ROUNDING (sizeof (union fooround)) - -/* When we copy a long block of data, this is the unit to do it with. - On some machines, copying successive ints does not work; - in such a case, redefine COPYING_UNIT to `long' (if that works) - or `char' as a last resort. */ -#ifndef COPYING_UNIT -#define COPYING_UNIT int -#endif - -/* The non-GNU-C macros copy the obstack into this global variable - to avoid multiple evaluation. */ - -struct obstack *_obstack; - -/* Initialize an obstack H for use. Specify chunk size SIZE (0 means default). - Objects start on multiples of ALIGNMENT (0 means use default). - CHUNKFUN is the function to use to allocate chunks, - and FREEFUN the function to free them. */ - -void -_obstack_begin (h, size, alignment, chunkfun, freefun) - struct obstack *h; - int size; - int alignment; - POINTER (*chunkfun) (); - void (*freefun) (); -{ - register struct _obstack_chunk* chunk; /* points to new chunk */ - - if (alignment == 0) - alignment = DEFAULT_ALIGNMENT; - if (size == 0) - /* Default size is what GNU malloc can fit in a 4096-byte block. - Pick a number small enough that when rounded up to DEFAULT_ROUNDING - it is still smaller than 4096 - 4. */ - { - int extra = 4; - if (extra < DEFAULT_ROUNDING) - extra = DEFAULT_ROUNDING; - size = 4096 - extra; - } - - h->chunkfun = (struct _obstack_chunk * (*)()) chunkfun; - h->freefun = freefun; - h->chunk_size = size; - h->alignment_mask = alignment - 1; - - chunk = h->chunk = (*h->chunkfun) (h->chunk_size); - h->next_free = h->object_base = chunk->contents; - h->chunk_limit = chunk->limit - = (char *) chunk + h->chunk_size; - chunk->prev = 0; -} - -/* Allocate a new current chunk for the obstack *H - on the assumption that LENGTH bytes need to be added - to the current object, or a new object of length LENGTH allocated. - Copies any partial object from the end of the old chunk - to the beginning of the new one. */ - -void -_obstack_newchunk (h, length) - struct obstack *h; - int length; -{ - register struct _obstack_chunk* old_chunk = h->chunk; - register struct _obstack_chunk* new_chunk; - register long new_size; - register int obj_size = h->next_free - h->object_base; - register int i; - int already; - - /* Compute size for new chunk. */ - new_size = (obj_size + length) + (obj_size >> 3) + 100; - if (new_size < h->chunk_size) - new_size = h->chunk_size; - - /* Allocate and initialize the new chunk. */ - new_chunk = h->chunk = (*h->chunkfun) (new_size); - new_chunk->prev = old_chunk; - new_chunk->limit = h->chunk_limit = (char *) new_chunk + new_size; - - /* Move the existing object to the new chunk. - Word at a time is fast and is safe if the object - is sufficiently aligned. */ - if (h->alignment_mask + 1 >= DEFAULT_ALIGNMENT) - { - for (i = obj_size / sizeof (COPYING_UNIT) - 1; - i >= 0; i--) - ((COPYING_UNIT *)new_chunk->contents)[i] - = ((COPYING_UNIT *)h->object_base)[i]; - /* We used to copy the odd few remaining bytes as one extra COPYING_UNIT, - but that can cross a page boundary on a machine - which does not do strict alignment for COPYING_UNITS. */ - already = obj_size / sizeof (COPYING_UNIT) * sizeof (COPYING_UNIT); - } - else - already = 0; - /* Copy remaining bytes one by one. */ - for (i = already; i < obj_size; i++) - new_chunk->contents[i] = h->object_base[i]; - - h->object_base = new_chunk->contents; - h->next_free = h->object_base + obj_size; -} - -/* Return nonzero if object OBJ has been allocated from obstack H. - This is here for debugging. - If you use it in a program, you are probably losing. */ - -int -_obstack_allocated_p (h, obj) - struct obstack *h; - POINTER obj; -{ - register struct _obstack_chunk* lp; /* below addr of any objects in this chunk */ - register struct _obstack_chunk* plp; /* point to previous chunk if any */ - - lp = (h)->chunk; - while (lp != 0 && ((POINTER)lp > obj || (POINTER)(lp)->limit < obj)) - { - plp = lp -> prev; - lp = plp; - } - return lp != 0; -} - -/* Free objects in obstack H, including OBJ and everything allocate - more recently than OBJ. If OBJ is zero, free everything in H. */ - -void -#ifdef __STDC__ -#undef obstack_free -obstack_free (struct obstack *h, POINTER obj) -#else -_obstack_free (h, obj) - struct obstack *h; - POINTER obj; -#endif -{ - register struct _obstack_chunk* lp; /* below addr of any objects in this chunk */ - register struct _obstack_chunk* plp; /* point to previous chunk if any */ - - lp = (h)->chunk; - while (lp != 0 && ((POINTER)lp > obj || (POINTER)(lp)->limit < obj)) - { - plp = lp -> prev; - (*h->freefun) (lp); - lp = plp; - } - if (lp) - { - (h)->object_base = (h)->next_free = (char *)(obj); - (h)->chunk_limit = lp->limit; - (h)->chunk = lp; - } - else if (obj != 0) - /* obj is not in any of the chunks! */ - abort (); -} - -/* Let same .o link with output of gcc and other compilers. */ - -#ifdef __STDC__ -void -_obstack_free (h, obj) - struct obstack *h; - POINTER obj; -{ - obstack_free (h, obj); -} -#endif - -#if 0 -/* These are now turned off because the applications do not use it - and it uses bcopy via obstack_grow, which causes trouble on sysV. */ - -/* Now define the functional versions of the obstack macros. - Define them to simply use the corresponding macros to do the job. */ - -#ifdef __STDC__ -/* These function definitions do not work with non-ANSI preprocessors; - they won't pass through the macro names in parentheses. */ - -/* The function names appear in parentheses in order to prevent - the macro-definitions of the names from being expanded there. */ - -POINTER (obstack_base) (obstack) - struct obstack *obstack; -{ - return obstack_base (obstack); -} - -POINTER (obstack_next_free) (obstack) - struct obstack *obstack; -{ - return obstack_next_free (obstack); -} - -int (obstack_object_size) (obstack) - struct obstack *obstack; -{ - return obstack_object_size (obstack); -} - -int (obstack_room) (obstack) - struct obstack *obstack; -{ - return obstack_room (obstack); -} - -void (obstack_grow) (obstack, pointer, length) - struct obstack *obstack; - POINTER pointer; - int length; -{ - obstack_grow (obstack, pointer, length); -} - -void (obstack_grow0) (obstack, pointer, length) - struct obstack *obstack; - POINTER pointer; - int length; -{ - obstack_grow0 (obstack, pointer, length); -} - -void (obstack_1grow) (obstack, character) - struct obstack *obstack; - int character; -{ - obstack_1grow (obstack, character); -} - -void (obstack_blank) (obstack, length) - struct obstack *obstack; - int length; -{ - obstack_blank (obstack, length); -} - -void (obstack_1grow_fast) (obstack, character) - struct obstack *obstack; - int character; -{ - obstack_1grow_fast (obstack, character); -} - -void (obstack_blank_fast) (obstack, length) - struct obstack *obstack; - int length; -{ - obstack_blank_fast (obstack, length); -} - -POINTER (obstack_finish) (obstack) - struct obstack *obstack; -{ - return obstack_finish (obstack); -} - -POINTER (obstack_alloc) (obstack, length) - struct obstack *obstack; - int length; -{ - return obstack_alloc (obstack, length); -} - -POINTER (obstack_copy) (obstack, pointer, length) - struct obstack *obstack; - POINTER pointer; - int length; -{ - return obstack_copy (obstack, pointer, length); -} - -POINTER (obstack_copy0) (obstack, pointer, length) - struct obstack *obstack; - POINTER pointer; - int length; -{ - return obstack_copy0 (obstack, pointer, length); -} - -#endif /* __STDC__ */ - -#endif /* 0 */ diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/obstack.h b/OpenSim/Utilities/simmToOpenSim/acpp/src/obstack.h deleted file mode 100644 index 052a249fee..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/obstack.h +++ /dev/null @@ -1,410 +0,0 @@ -/* obstack.h - object stack macros - Copyright (C) 1988 Free Software Foundation, Inc. - -This program is free software; you can redistribute it and/or modify it -under the terms of the GNU General Public License as published by the -Free Software Foundation; either version 1, or (at your option) any -later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. - - -In other words, you are welcome to use, share and improve this program. -You are forbidden to forbid anyone else to use, share and improve -what you give them. Help stamp out software-hoarding! */ - - -/* Summary: - -All the apparent functions defined here are macros. The idea -is that you would use these pre-tested macros to solve a -very specific set of problems, and they would run fast. -Caution: no side-effects in arguments please!! They may be -evaluated MANY times!! - -These macros operate a stack of objects. Each object starts life -small, and may grow to maturity. (Consider building a word syllable -by syllable.) An object can move while it is growing. Once it has -been "finished" it never changes address again. So the "top of the -stack" is typically an immature growing object, while the rest of the -stack is of mature, fixed size and fixed address objects. - -These routines grab large chunks of memory, using a function you -supply, called `obstack_chunk_alloc'. On occasion, they free chunks, -by calling `obstack_chunk_free'. You must define them and declare -them before using any obstack macros. - -Each independent stack is represented by a `struct obstack'. -Each of the obstack macros expects a pointer to such a structure -as the first argument. - -One motivation for this package is the problem of growing char strings -in symbol tables. Unless you are "fascist pig with a read-only mind" -[Gosper's immortal quote from HAKMEM item 154, out of context] you -would not like to put any arbitrary upper limit on the length of your -symbols. - -In practice this often means you will build many short symbols and a -few long symbols. At the time you are reading a symbol you don't know -how long it is. One traditional method is to read a symbol into a -buffer, realloc()ating the buffer every time you try to read a symbol -that is longer than the buffer. This is beaut, but you still will -want to copy the symbol from the buffer to a more permanent -symbol-table entry say about half the time. - -With obstacks, you can work differently. Use one obstack for all symbol -names. As you read a symbol, grow the name in the obstack gradually. -When the name is complete, finalize it. Then, if the symbol exists already, -free the newly read name. - -The way we do this is to take a large chunk, allocating memory from -low addresses. When you want to build a symbol in the chunk you just -add chars above the current "high water mark" in the chunk. When you -have finished adding chars, because you got to the end of the symbol, -you know how long the chars are, and you can create a new object. -Mostly the chars will not burst over the highest address of the chunk, -because you would typically expect a chunk to be (say) 100 times as -long as an average object. - -In case that isn't clear, when we have enough chars to make up -the object, THEY ARE ALREADY CONTIGUOUS IN THE CHUNK (guaranteed) -so we just point to it where it lies. No moving of chars is -needed and this is the second win: potentially long strings need -never be explicitly shuffled. Once an object is formed, it does not -change its address during its lifetime. - -When the chars burst over a chunk boundary, we allocate a larger -chunk, and then copy the partly formed object from the end of the old -chunk to the beginning of the new larger chunk. We then carry on -accreting characters to the end of the object as we normally would. - -A special macro is provided to add a single char at a time to a -growing object. This allows the use of register variables, which -break the ordinary 'growth' macro. - -Summary: - We allocate large chunks. - We carve out one object at a time from the current chunk. - Once carved, an object never moves. - We are free to append data of any size to the currently - growing object. - Exactly one object is growing in an obstack at any one time. - You can run one obstack per control block. - You may have as many control blocks as you dare. - Because of the way we do it, you can `unwind' a obstack - back to a previous state. (You may remove objects much - as you would with a stack.) -*/ - - -/* Don't do the contents of this file more than once. */ - -#ifndef __OBSTACKS__ -#define __OBSTACKS__ - -/* We use subtraction of (char *)0 instead of casting to int - because on word-addressable machines a simple cast to int - may ignore the byte-within-word field of the pointer. */ - -#ifndef __PTR_TO_INT -#define __PTR_TO_INT(P) ((P) - (char *)0) -#endif - -#ifndef __INT_TO_PTR -#define __INT_TO_PTR(P) ((P) + (char *)0) -#endif - -struct _obstack_chunk /* Lives at front of each chunk. */ -{ - char *limit; /* 1 past end of this chunk */ - struct _obstack_chunk *prev; /* address of prior chunk or NULL */ - char contents[4]; /* objects begin here */ -}; - -struct obstack /* control current object in current chunk */ -{ - long chunk_size; /* preferred size to allocate chunks in */ - struct _obstack_chunk* chunk; /* address of current struct obstack_chunk */ - char *object_base; /* address of object we are building */ - char *next_free; /* where to add next char to current object */ - char *chunk_limit; /* address of char after current chunk */ - int temp; /* Temporary for some macros. */ - int alignment_mask; /* Mask of alignment for each object. */ - struct _obstack_chunk *(*chunkfun) (); /* User's fcn to allocate a chunk. */ - void (*freefun) (); /* User's function to free a chunk. */ -}; - -#ifdef __STDC__ - -/* Do the function-declarations after the structs - but before defining the macros. */ - -void obstack_init (struct obstack *obstack); - -void * obstack_alloc (struct obstack *obstack, int size); - -void * obstack_copy (struct obstack *obstack, void *address, int size); -void * obstack_copy0 (struct obstack *obstack, void *address, int size); - -void obstack_free (struct obstack *obstack, void *block); - -void obstack_blank (struct obstack *obstack, int size); - -void obstack_grow (struct obstack *obstack, void *data, int size); -void obstack_grow0 (struct obstack *obstack, void *data, int size); - -void obstack_1grow (struct obstack *obstack, int data_char); -void obstack_ptr_grow (struct obstack *obstack, void *data); -void obstack_int_grow (struct obstack *obstack, int data); - -void * obstack_finish (struct obstack *obstack); - -int obstack_object_size (struct obstack *obstack); - -int obstack_room (struct obstack *obstack); -void obstack_1grow_fast (struct obstack *obstack, int data_char); -void obstack_ptr_grow_fast (struct obstack *obstack, void *data); -void obstack_int_grow_fast (struct obstack *obstack, int data); -void obstack_blank_fast (struct obstack *obstack, int size); - -void * obstack_base (struct obstack *obstack); -void * obstack_next_free (struct obstack *obstack); -int obstack_alignment_mask (struct obstack *obstack); -int obstack_chunk_size (struct obstack *obstack); - -#endif /* __STDC__ */ - -/* Non-ANSI C cannot really support alternative functions for these macros, - so we do not declare them. */ - -/* Pointer to beginning of object being allocated or to be allocated next. - Note that this might not be the final address of the object - because a new chunk might be needed to hold the final size. */ - -#define obstack_base(h) ((h)->object_base) - -/* Size for allocating ordinary chunks. */ - -#define obstack_chunk_size(h) ((h)->chunk_size) - -/* Pointer to next byte not yet allocated in current chunk. */ - -#define obstack_next_free(h) ((h)->next_free) - -/* Mask specifying low bits that should be clear in address of an object. */ - -#define obstack_alignment_mask(h) ((h)->alignment_mask) - -#define obstack_init(h) \ - _obstack_begin ((h), 0, 0, obstack_chunk_alloc, obstack_chunk_free) - -#define obstack_begin(h, size) \ - _obstack_begin ((h), (size), 0, obstack_chunk_alloc, obstack_chunk_free) - -#define obstack_1grow_fast(h,achar) (*((h)->next_free)++ = achar) - -#define obstack_blank_fast(h,n) ((h)->next_free += (n)) - -#if defined (__GNUC__) && defined (__STDC__) - -/* For GNU C, if not -traditional, - we can define these macros to compute all args only once - without using a global variable. - Also, we can avoid using the `temp' slot, to make faster code. */ - -#define obstack_object_size(OBSTACK) \ - ({ struct obstack *__o = (OBSTACK); \ - (unsigned) (__o->next_free - __o->object_base); }) - -#define obstack_room(OBSTACK) \ - ({ struct obstack *__o = (OBSTACK); \ - (unsigned) (__o->chunk_limit - __o->next_free); }) - -#define obstack_grow(OBSTACK,where,length) \ -({ struct obstack *__o = (OBSTACK); \ - int __len = (length); \ - ((__o->next_free + __len > __o->chunk_limit) \ - ? _obstack_newchunk (__o, __len) : 0); \ - bcopy (where, __o->next_free, __len); \ - __o->next_free += __len; \ - (void) 0; }) - -#define obstack_grow0(OBSTACK,where,length) \ -({ struct obstack *__o = (OBSTACK); \ - int __len = (length); \ - ((__o->next_free + __len + 1 > __o->chunk_limit) \ - ? _obstack_newchunk (__o, __len + 1) : 0), \ - bcopy (where, __o->next_free, __len), \ - __o->next_free += __len, \ - *(__o->next_free)++ = 0; \ - (void) 0; }) - -#define obstack_1grow(OBSTACK,datum) \ -({ struct obstack *__o = (OBSTACK); \ - ((__o->next_free + 1 > __o->chunk_limit) \ - ? _obstack_newchunk (__o, 1) : 0), \ - *(__o->next_free)++ = (datum); \ - (void) 0; }) - -/* These assume that the obstack alignment is good enough for pointers or ints, - and that the data added so far to the current object - shares that much alignment. */ - -#define obstack_ptr_grow(OBSTACK,datum) \ -({ struct obstack *__o = (OBSTACK); \ - ((__o->next_free + sizeof (void *) > __o->chunk_limit) \ - ? _obstack_newchunk (__o, sizeof (void *)) : 0), \ - *((void **)__o->next_free)++ = ((void *)datum); \ - (void) 0; }) - -#define obstack_int_grow(OBSTACK,datum) \ -({ struct obstack *__o = (OBSTACK); \ - ((__o->next_free + sizeof (int) > __o->chunk_limit) \ - ? _obstack_newchunk (__o, sizeof (int)) : 0), \ - *((int *)__o->next_free)++ = ((int)datum); \ - (void) 0; }) - -#define obstack_ptr_grow_fast(h,aptr) (*((void **)(h)->next_free)++ = (void *)aptr) -#define obstack_int_grow_fast(h,aint) (*((int *)(h)->next_free)++ = (int)aint) - -#define obstack_blank(OBSTACK,length) \ -({ struct obstack *__o = (OBSTACK); \ - int __len = (length); \ - ((__o->next_free + __len > __o->chunk_limit) \ - ? _obstack_newchunk (__o, __len) : 0); \ - __o->next_free += __len; \ - (void) 0; }) - -#define obstack_alloc(OBSTACK,length) \ -({ struct obstack *__h = (OBSTACK); \ - obstack_blank (__h, (length)); \ - obstack_finish (__h); }) - -#define obstack_copy(OBSTACK,where,length) \ -({ struct obstack *__h = (OBSTACK); \ - obstack_grow (__h, (where), (length)); \ - obstack_finish (__h); }) - -#define obstack_copy0(OBSTACK,where,length) \ -({ struct obstack *__h = (OBSTACK); \ - obstack_grow0 (__h, (where), (length)); \ - obstack_finish (__h); }) - -#define obstack_finish(OBSTACK) \ -({ struct obstack *__o = (OBSTACK); \ - void *value = (void *) __o->object_base; \ - __o->next_free \ - = __INT_TO_PTR ((__PTR_TO_INT (__o->next_free)+__o->alignment_mask)\ - & ~ (__o->alignment_mask)); \ - ((__o->next_free - (char *)__o->chunk \ - > __o->chunk_limit - (char *)__o->chunk) \ - ? (__o->next_free = __o->chunk_limit) : 0); \ - __o->object_base = __o->next_free; \ - value; }) - -#define obstack_free(OBSTACK, OBJ) \ -({ struct obstack *__o = (OBSTACK); \ - void *__obj = (OBJ); \ - if (__obj >= (void *)__o->chunk && __obj < (void *)__o->chunk_limit) \ - __o->next_free = __o->object_base = __obj; \ - else (obstack_free) (__o, __obj); }) - -#else /* not __GNUC__ or not __STDC__ */ - -/* The non-GNU macros copy the obstack-pointer into this global variable - to avoid multiple evaluation. */ - -extern struct obstack *_obstack; - -#define obstack_object_size(h) \ - (unsigned) (_obstack = (h), (h)->next_free - (h)->object_base) - -#define obstack_room(h) \ - (unsigned) (_obstack = (h), (h)->chunk_limit - (h)->next_free) - -#define obstack_grow(h,where,length) \ -( (h)->temp = (length), \ - (((h)->next_free + (h)->temp > (h)->chunk_limit) \ - ? _obstack_newchunk ((h), (h)->temp) : 0), \ - bcopy (where, (h)->next_free, (h)->temp), \ - (h)->next_free += (h)->temp) - -#define obstack_grow0(h,where,length) \ -( (h)->temp = (length), \ - (((h)->next_free + (h)->temp + 1 > (h)->chunk_limit) \ - ? _obstack_newchunk ((h), (h)->temp + 1) : 0), \ - bcopy (where, (h)->next_free, (h)->temp), \ - (h)->next_free += (h)->temp, \ - *((h)->next_free)++ = 0) - -#define obstack_1grow(h,datum) \ -( (((h)->next_free + 1 > (h)->chunk_limit) \ - ? _obstack_newchunk ((h), 1) : 0), \ - *((h)->next_free)++ = (datum)) - -#define obstack_ptr_grow(h,datum) \ -( (((h)->next_free + sizeof (char *) > (h)->chunk_limit) \ - ? _obstack_newchunk ((h), sizeof (char *)) : 0), \ - *((char **)(h)->next_free)++ = ((char *)datum)) - -#define obstack_int_grow(h,datum) \ -( (((h)->next_free + sizeof (int) > (h)->chunk_limit) \ - ? _obstack_newchunk ((h), sizeof (int)) : 0), \ - *((int *)(h)->next_free)++ = ((int)datum)) - -#define obstack_ptr_grow_fast(h,aptr) (*((char **)(h)->next_free)++ = (char *)aptr) -#define obstack_int_grow_fast(h,aint) (*((int *)(h)->next_free)++ = (int)aint) - -#define obstack_blank(h,length) \ -( (h)->temp = (length), \ - (((h)->next_free + (h)->temp > (h)->chunk_limit) \ - ? _obstack_newchunk ((h), (h)->temp) : 0), \ - (h)->next_free += (h)->temp) - -#define obstack_alloc(h,length) \ - (obstack_blank ((h), (length)), obstack_finish ((h))) - -#define obstack_copy(h,where,length) \ - (obstack_grow ((h), (where), (length)), obstack_finish ((h))) - -#define obstack_copy0(h,where,length) \ - (obstack_grow0 ((h), (where), (length)), obstack_finish ((h))) - -#define obstack_finish(h) \ -( (h)->temp = __PTR_TO_INT ((h)->object_base), \ - (h)->next_free \ - = __INT_TO_PTR ((__PTR_TO_INT ((h)->next_free)+(h)->alignment_mask) \ - & ~ ((h)->alignment_mask)), \ - (((h)->next_free - (char *)(h)->chunk \ - > (h)->chunk_limit - (char *)(h)->chunk) \ - ? ((h)->next_free = (h)->chunk_limit) : 0), \ - (h)->object_base = (h)->next_free, \ - __INT_TO_PTR ((h)->temp)) - -#ifdef __STDC__ -#define obstack_free(h,obj) \ -( (h)->temp = (char *)(obj) - (char *) (h)->chunk, \ - (((h)->temp >= 0 && (h)->temp < (h)->chunk_limit - (char *) (h)->chunk)\ - ? (int) ((h)->next_free = (h)->object_base \ - = (h)->temp + (char *) (h)->chunk) \ - : ((obstack_free) ((h), (h)->temp + (char *) (h)->chunk), 0))) -#else -#define obstack_free(h,obj) \ -( (h)->temp = (char *)(obj) - (char *) (h)->chunk, \ - (((h)->temp >= 0 && (h)->temp < (h)->chunk_limit - (char *) (h)->chunk)\ - ? (int) ((h)->next_free = (h)->object_base \ - = (h)->temp + (char *) (h)->chunk) \ - : (int) _obstack_free ((h), (h)->temp + (char *) (h)->chunk))) -#endif - -#endif /* not __GNUC__ or not __STDC__ */ - -#endif /* not __OBSTACKS__ */ - diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/tm.h b/OpenSim/Utilities/simmToOpenSim/acpp/src/tm.h deleted file mode 100644 index 49c5202813..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/tm.h +++ /dev/null @@ -1,1829 +0,0 @@ -/* Definitions of target machine for GNU compiler. MIPS version. - Contributed by A. Lichnewsky, lich@inria.inria.fr - Copyright (C) 1989 Free Software Foundation, Inc. - -This file is part of GNU CC. - -GNU CC is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 1, or (at your option) -any later version. - -GNU CC is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with GNU CC; see the file COPYING. If not, write to -the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. */ - -/* ??? This file needs to be reformatted so that it looks like the - rest of GCC. ??? */ - -/* Currently we know of 2 MIPS machines: MIPS- M series, and DECStations. */ -/* Therefore we discriminate by declaring DECStations with */ -/* #define DECSTATION */ - -/* Names to predefine in the preprocessor for this target machine. */ - -#define CPP_PREDEFINES "-Dmips -Dunix" -#ifdef DECSTATION -#define CPP_SPEC "-DR3000 -DLANGUAGE_C -DMIPSEL -DSYSTYPE_BSD " -#else -#define CPP_SPEC "-DR3000 -Dhost_mips -DMIPSEB -DSYSTYPE_BSD -DLANGUAGE_C " -#endif - -/*---------------------------------------------------------------------- - -SWITCHES: - - -O optimization. Implies -fstrength-reduce -fomit-frame-pointer - -O2 optimization. Implies -O - - Tries to make use of short displacements using the - Sdata and Sbss sections. This uses the -G switches of as and ld. - - -G - Pass size to as and ld. Default -G 8. - - -mG0 -mG1 -mG2 - Construct a size to be passed to GCC for Data / Sdata selection. - - Value is ( (i=G0 + 2 G1 + 4 G2) , (i < 6) ? ( 1<\n"); - /* Depends on MIPS ASM. */ -#else -#define TARGET_VERSION printf (" (AL-MIPS 1.10) \n"); - /* Depends on MIPS ASM. */ -#endif -#define TARGET_VERSNUM "1 10" - -/* Do not Generate DBX debugging information. */ - -/* #define DBX_DEBUGGING_INFO */ - -/* Run-time compilation parameters selecting different hardware subsets. */ - -extern int target_flags; - -/* Macros used in the machine description to test the flags. */ - -/* Nonzero if compiling code that Unix assembler can assemble. */ -#define TARGET_UNIX_ASM (target_flags & 1) - /* Debug Mode */ -#define TARGET_DEBUG_MODE (target_flags & 2) -#define TARGET_DEBUGA_MODE (target_flags & 4) -#define TARGET_DEBUGB_MODE (target_flags & 16) -#define TARGET_DEBUGC_MODE (target_flags & 32) -#define TARGET_DEBUGD_MODE (target_flags & 64) - /* Register Naming in .s ($21 vs. $a0) */ -#define TARGET_NAME_REGS (target_flags & 8) - /* Use addu / subbu or get FIXED_OVFL TRAPS */ -#define TARGET_NOFIXED_OVFL (target_flags & 128) - /* Optimize for Sdata/Sbss */ -#define TARGET_GP_OPT (target_flags & 4096) -#define TARGET_GVALUE ((target_flags >> 8 ) & 0xf) - - - -/* Macro to define tables used to set the flags. - This is a list in braces of pairs in braces, - each pair being { "NAME", VALUE } - where VALUE is the bits to set or minus the bits to clear. - An empty string NAME is used to identify the default VALUE. */ - -#define TARGET_SWITCHES \ - { {"unix", 1}, \ - {"gnu", -1}, \ - {"debug", 2 }, /* RELOAD and CONSTRAINTS Related DEBUG */\ - {"nodebug", -2 }, \ - {"debuga", 4 }, /* CALLING SEQUENCE RELATED DEBUG */ \ - {"nodebuga", -4 }, \ - {"debugb", 16 }, /* GLOBAL/LOCAL ALLOC DEBUG */ \ - {"nodebugb", -16 }, \ - {"debugc", 32 }, /* SPILL/RELOAD REGISTER ALLOCATOR DEBUG */\ - {"nodebugc", -32 }, \ - {"debugd", 64 }, /* CSE DEBUG */ \ - {"nodebugd", -64 }, \ - {"rnames", 8 }, /* Output register names like $a0 */ \ - {"nornames", -8 }, /* Output register numbers like $21 */ \ - {"nofixed-ovfl",128}, /* use addu and subu */ \ - {"fixed-ovfl", -128}, /* use add and sub */ \ - /* Following used to support the data/sdata */\ - /* feature */ \ - {"G0",256}, \ - {"nG0",-256}, \ - {"G1",512}, \ - {"nG1",-512}, \ - {"G2",1024}, \ - {"nG2",-1024}, \ - {"gpOPT", 4096}, /* DO the full GP optimization data/sdata.. */\ - {"ngpOPT", -4096},\ - { "", TARGET_DEFAULT}} - -/* Default target_flags if no switches specified. */ - -#define TARGET_DEFAULT 897 - -/* Default GVALUE (data item size threshold for selection of Sdata/data) - is computed : GVALUE == ( ((i=G0+2*G1+4*G2) < 6) - ? 1<= 32) ? FP_REGS : GR_REGS) - -/* Define a table that lets us find quickly all the reg classes - containing a given one. This is the initializer for an - N_REG_CLASSES x N_REG_CLASSES array of reg class codes. - Row N is a sequence containing all the class codes for - classes that contain all the regs in class N. Each row - contains no duplicates, and is terminated by LIM_REG_CLASSES. */ - -/* We give just a dummy for the first element, which is for NO_REGS. */ -/* #define REG_CLASS_SUPERCLASSES {{LIM_REG_CLASSES}, \ - {GR_REGS,ALL_REGS,LIM_REG_CLASSES}, \ - {FP_REGS,ALL_REGS,LIM_REG_CLASSES}, \ - {ALL_REGS,LIM_REG_CLASSES} \ -} -*/ -/* We give just a dummy for the first element, which is for NO_REGS. */ -#define REG_CLASS_SUPERCLASSES {{LIM_REG_CLASSES}, \ - {ALL_REGS,LIM_REG_CLASSES}, \ - {ALL_REGS,LIM_REG_CLASSES}, \ - {LIM_REG_CLASSES} \ -} - -/* The inverse relationship: - for each class, a list of all reg classes contained in it. */ -#define REG_CLASS_SUBCLASSES \ -{{LIM_REG_CLASSES}, \ - {GR_REGS,LIM_REG_CLASSES}, \ - {FP_REGS,LIM_REG_CLASSES},\ - {GR_REGS, FP_REGS, ALL_REGS, LIM_REG_CLASSES}\ -} - -/* Define a table that lets us find quickly the class - for the subunion of any two classes. - - We say "subunion" because the result need not be exactly - the union; it may instead be a subclass of the union - (though the closer to the union, the better). - But if it contains anything beyond union of the two classes, - you will lose! - - This is an initializer for an N_REG_CLASSES x N_REG_CLASSES - array of reg class codes. The subunion of classes C1 and C2 - is just element [C1, C2]. */ - -#define REG_CLASS_SUBUNION {{NO_REGS, GR_REGS, FP_REGS, ALL_REGS}, \ - {GR_REGS, GR_REGS, ALL_REGS, ALL_REGS}, \ - {FP_REGS, ALL_REGS, FP_REGS, ALL_REGS}, \ - {ALL_REGS, ALL_REGS, ALL_REGS, ALL_REGS}} - -/* The class value for index registers, and the one for base regs. */ - -#define INDEX_REG_CLASS GR_REGS -#define BASE_REG_CLASS GR_REGS - - - /* REGISTER AND CONSTANT CLASSES - */ - -/* Get reg_class from a letter such as appears in the machine -description. */ - /* DEFINED REGISTER CLASSES: - ** - ** 'f' : Floating point registers - ** 'y' : General register when used to - ** transfer chunks of Floating point - ** with mfc1 mtc1 insn - */ - -#define REG_CLASS_FROM_LETTER(C) \ - ((C) == 'f' ? FP_REGS: \ - (C) == 'y' ? GR_REGS:NO_REGS) - -/* The letters I, J, K, L and M in a register constraint string - can be used to stand for particular ranges of immediate operands. - This macro defines what the ranges are. - C is the letter, and VALUE is a constant value. - Return 1 if VALUE is in the range specified by C. */ - -/* For MIPS, `I' is used for the range of constants an insn - can actually contain (16 bits signed integers). - `J' is used for the range which is just zero (since that is - available as $R0). -*/ - -#define SMALL_INT(X) ((unsigned) (INTVAL (X) + 0x10000) < 0x20000) - -#define CONST_OK_FOR_LETTER_P(VALUE, C) \ - ((C) == 'I' ? (unsigned) ((VALUE) + 0x10000) < 0x20000 \ - : (C) == 'J' ? (VALUE) == 0 \ - : 0) - -/* Similar, but for floating constants, and defining letters G and H. - Here VALUE is the CONST_DOUBLE rtx itself. */ - - /* DEFINED FLOATING CONSTANT CLASSES: - ** - ** 'G' : Floating point 0 - */ -#define CONST_DOUBLE_OK_FOR_LETTER_P(VALUE, C) \ - ((C) == 'G' && XINT (VALUE, 0) == 0 && XINT (VALUE, 1) == 0) - -/* Given an rtx X being reloaded into a reg required to be - in class CLASS, return the class of reg to actually use. - In general this is just CLASS; but on some machines - in some cases it is preferable to use a more restrictive class. */ - -#define PREFERRED_RELOAD_CLASS(X,CLASS) \ - (((GET_MODE(X) == SFmode) || (GET_MODE(X) == DFmode))? FP_REGS : \ - ((GET_MODE(X) == VOIDmode) ? GR_REGS :(CLASS))) - -/* Same but Mode has been extracted already -*/ - -#define PREFERRED_RELOAD_CLASS_FM(X,CLASS) \ - ((((X) == SFmode) || ((X) == DFmode))? FP_REGS : \ - (((X) == VOIDmode) ? GR_REGS :(CLASS))) - -/* Return the maximum number of consecutive registers - needed to represent mode MODE in a register of class CLASS. */ - -#define CLASS_MAX_NREGS(CLASS, MODE) \ - ((((MODE) == DFmode) || ((MODE) == SFmode)) ? 2 \ - : ((MODE) == VOIDmode)? ((CLASS) == FP_REGS ? 2 :1) \ - : ((GET_MODE_SIZE (MODE) + UNITS_PER_WORD - 1) / UNITS_PER_WORD)) - - -/* Stack layout; function entry, exit and calling. */ - -/* Define this if pushing a word on the stack - makes the stack pointer a smaller address. */ -#define STACK_GROWS_DOWNWARD - -/* Define this if the nominal address of the stack frame - is at the high-address end of the local variables; - that is, each additional local variable allocated - goes at a more negative offset in the frame. */ -#define FRAME_GROWS_DOWNWARD - -/* Offset within stack frame to start allocating local variables at. - If FRAME_GROWS_DOWNWARD, this is the offset to the END of the - first local allocated. Otherwise, it is the offset to the BEGINNING - of the first local allocated. */ -#define STARTING_FRAME_OFFSET -4 - -/* If we generate an insn to push BYTES bytes, - this says how many the stack pointer really advances by. - On the vax, sp@- in a byte insn really pushes a word. */ - -/* #define PUSH_ROUNDING(BYTES) 0 */ - - -/* Offset of first parameter from the argument pointer register value. */ -#define FIRST_PARM_OFFSET(FNDECL) 0 - -/* Offset from top-of-stack address to location to store the - function parameter if it can't go in a register. - Addresses for following parameters are computed relative to this one. */ -#define FIRST_PARM_CALLER_OFFSET(FNDECL) 0 - -/* When a parameter is passed in a register, stack space is still - allocated for it. */ -/* For the MIPS, stack space must be allocated, cf Asm Lang Prog Guide - page 7-8 - - BEWARE that some space is also allocated for non existing arguments - in register. In case an argument list is of form - GF used registers are a0 (a2,a3), but we should push over a1... not - used.. -*/ -#define REG_PARM_STACK_SPACE - /* Align stack frames on 64 bits (Double Word ) - */ - -#define STACK_BOUNDARY 64 - -/* For the MIPS, there seems to be a minimum to the amount of stack space - used... for varargs using functions. - evidence comes from the dis-assembled version of printf: - - cc (cc) - Mips Computer Systems 1.31 - /usr/lib/cpp1.31 - - -printf: - [printf.c: 14] 0x400510: 27bdffe8 addiu sp,sp,-24 - [printf.c: 14] 0x400514: afbf0014 sw ra,20(sp) - [printf.c: 14] 0x400518: afa5001c sw a1,28(sp) - [printf.c: 14] 0x40051c: afa60020 sw a2,32(sp) - [printf.c: 14] 0x400520: afa70024 sw a3,36(sp) - [printf.c: 18] 0x400524: 27a5001c addiu a1,sp,28 - - it is however OK for functions that do not take arguments to have 0 size - frames. - -*/ - -#define STACK_ARGS_ADJUST(SIZE) \ -{ \ - SIZE.constant += 4; \ - if (SIZE.var) \ - { \ - rtx size1 = ARGS_SIZE_RTX (SIZE); \ - rtx rounded = gen_reg_rtx (SImode); \ - rtx label = gen_label_rtx (); \ - emit_move_insn (rounded, size1); \ - /* Needed: insns to jump to LABEL if ROUNDED is < 16. */ \ - abort (); \ - emit_move_insn (rounded, gen_rtx (CONST_INT, VOIDmode, 16)); \ - emit_label (label); \ - SIZE.constant = 0; \ - SIZE.var = (tree) rounded; \ - } \ - else if (SIZE.constant < 16) \ - SIZE.constant = 16; \ -} - -/* Value is 1 if returning from a function call automatically - pops the arguments described by the number-of-args field in the call. - FUNTYPE is the data type of the function (as a tree), - or for a library call it is an identifier node for the subroutine name. */ - -#define RETURN_POPS_ARGS(FUNTYPE) 0 - - -/* Define how to find the value returned by a function. - VALTYPE is the data type of the value (as a tree). - If the precise function being called is known, FUNC is its FUNCTION_DECL; - otherwise, FUNC is 0. */ - -#define FUNCTION_VALUE(VALTYPE, FUNC) \ - gen_rtx (REG, TYPE_MODE (VALTYPE), \ - (TYPE_MODE (VALTYPE) == SFmode) ||(TYPE_MODE (VALTYPE) == DFmode)?32 : 2) - -/* Define how to find the value returned by a library function - assuming the value has mode MODE. */ - - -#define LIBCALL_VALUE(MODE) gen_rtx (REG, MODE, \ - ((MODE) == DFmode || ( MODE) == SFmode) ? 32 : 2) - -/* 1 if N is a possible register number for a function value. - On the MIPS, R2 R3 and F0 F2 are the only register thus used. */ -/* Currently, R2 and F0 are only implemented here ( C has no complex type) -*/ - -#define FUNCTION_VALUE_REGNO_P(N) ((N) == 2 || (N) == 32) - -/* 1 if N is a possible register number for function argument passing. - */ - -#define FUNCTION_ARG_REGNO_P(N) (((N) < 8 && (N) > 3) \ - ||((N) < 48 && (N) > 44 && (0 == (N) % 2))) - -/* Define a data type for recording info about an argument list - during the scan of that argument list. This data type should - hold all necessary information about the function itself - and about the args processed so far, enough to enable macros - such as FUNCTION_ARG to determine where the next arg should go. -*/ - /* On MIPS the following automaton decides */ - /* where to put things. */ - /* If you dont believe it, look at Gerry Kane*/ - /* 's book page D-22 */ - -#define CUMULATIVE_ARGS struct { enum arg_state arg_rec_state;int restype,arg_num;} - -enum arg_state { ARG_STA_INIT =0, - ARG_STA_F =1, /* $f12 */ - ARG_STA_FF =2, /* $f12 $f14 */ - ARG_STA_FG =3, /* $f12 $6 */ - ARG_STA_FGG =4, /* $f12 $6 $7 */ - ARG_STA_FGF =5, /* $f12 $6 STACK */ - ARG_STA_G =6, /* $4 */ - ARG_STA_GF =7, /* $4 ($6,$7) */ - ARG_STA_GG =8, /* $4 $5 */ - ARG_STA_GGF =9, /* $4 $5 ($6,$7) */ - ARG_STA_GGG =10,/* $4 $5 $6 */ - ARG_STA_GGGF =11,/* $4 $5 $6 STACK */ - ARG_STA_GGGG =12 /* $4 $5 $6 $7 */ - }; -#define ARG_STA_AUTOMA \ -{ \ - {ARG_STA_F,ARG_STA_G,44,4 }, /* ARG_STA_INIT */ \ - {ARG_STA_FF,ARG_STA_FG,46,6 }, /* ARG_STA_F */ \ - {ARG_STA_FF,ARG_STA_FF,-1,-1 }, /* ARG_STA_FF */ \ - {ARG_STA_FGF,ARG_STA_FGG,-1,7 }, /* ARG_STA_FG */ \ - {ARG_STA_FGG,ARG_STA_FGG,-1,-1 }, /* ARG_STA_FGG */ \ - {ARG_STA_FGF,ARG_STA_FGF,-1,-1 }, /* ARG_STA_FGF */ \ - {ARG_STA_GF,ARG_STA_GG,-2,5 }, /* ARG_STA_G */ \ - {ARG_STA_GF,ARG_STA_GF,-1,-1 }, /* ARG_STA_GF */ \ - {ARG_STA_GGF,ARG_STA_GGG,-2,6 }, /* ARG_STA_GG */ \ - {ARG_STA_GGF,ARG_STA_GGF,-1,-1 }, /* ARG_STA_GGF */ \ - {ARG_STA_GGGF,ARG_STA_GGGG,-1,7 }, /* ARG_STA_GGG */ \ - {ARG_STA_GGGF,ARG_STA_GGGF,-1,-1 }, /* ARG_STA_GGGF */ \ - {ARG_STA_GGGG,ARG_STA_GGGG,-1,-1 } /* ARG_STA_GGGG */ \ -} - -/* Initialize a variable CUM of type CUMULATIVE_ARGS - for a call to a function whose data type is FNTYPE. - For a library call, FNTYPE is 0. - -*/ - -#define INIT_CUMULATIVE_ARGS(CUM,FNTYPE) ((CUM.arg_rec_state) = ARG_STA_INIT,\ - (CUM.arg_num) = 0, (CUM.restype = (int)VOIDmode)) - -/* Update the data in CUM to advance over an argument - of mode MODE and data type TYPE. - (TYPE is null for libcalls where that information may not be available.) */ - -#define FUNCTION_ARG_ADVANCE(CUM, MODE, TYPE, NAMED) \ - ( function_arg_advance(&CUM,MODE,TYPE)); - -extern enum arg_state function_arg_advance(); - -/* Determine where to put an argument to a function. - Value is zero to push the argument on the stack, - or a hard register in which to store the argument. - - MODE is the argument's machine mode. - TYPE is the data type of the argument (as a tree). - This is null for libcalls where that information may - not be available. - CUM is a variable of type CUMULATIVE_ARGS which gives info about - the preceding args and about the function being called. - NAMED is nonzero if this argument is a named parameter - (otherwise it is an extra parameter matching an ellipsis). */ - - - -#define FUNCTION_ARG(CUM, MODE, TYPE, NAMED) \ - ( (rtx) function_arg(&CUM,MODE,TYPE,NAMED)) - -/* For an arg passed partly in registers and partly in memory, - this is the number of registers used. - For args passed entirely in registers or entirely in memory, zero. -*/ - -#define FUNCTION_ARG_PARTIAL_NREGS(CUM, MODE, TYPE, NAMED) (0) - - -/* This macro generates the assembly code for function entry. - FILE is a stdio stream to output the code to. - SIZE is an int: how many units of temporary storage to allocate. - Refer to the array `regs_ever_live' to determine which registers - to save; `regs_ever_live[I]' is nonzero if register number I - is ever used in the function. This macro is responsible for - knowing which registers should not be saved even if used. */ - - -/* ALIGN FRAMES on double word boundaries */ - -#define AL_ADJUST_ALIGN(LOC) (((LOC)+7) & 0xfffffff8) - - -/* The problem of Varargs comes from the register passing conventions - for Floating Point data. There is a conflict when we send registers - back to stack between registers $4,$5 $6,$7 and $f12, $f14. - - The current implementation: - a/ tries to figure out if the current routines uses varargs.(It becomes - ``suspect''.) This is currently done by looking for a special - static character string constant. - - b/when a function is suspected of using varags, a larger reg - save_area is allocated which will hold regs f12 and f14. The varargs - macros then have to find where is the argument they are looking for. - This is made easier by a modification in stack frame layout for - these functions:the stack frame-size is accessible on stack at - location 4($30). - - Total overhead in PROLOGUE: 2 inns to put stacksize on stack - 2 sw.d to save floating registers. - (Only when Varargs suspected) - - The only problem with ``thinking'', is that when functions are - thought using varargs and dont do it, they get the above entry - overhead.However the current method is quite precise, and is *safe*. - - - See va-mips.h for more information on varargs - -*/ -extern int varargs_suspect; -extern int this_varargs_suspect ; - -#define VARARGS_SUSPECT(COND) varargs_suspect |= (COND) -#define VARARGS_NOTSUSPECT varargs_suspect = 0 -#define VARARGS_SUSPECTED (varargs_suspect) - -#define THIS_VARARGS_SUSPECT(COND) this_varargs_suspect |= (COND) -#define THIS_VARARGS_NOTSUSPECT this_varargs_suspect = 0 -#define THIS_VARARGS_SUSPECTED (this_varargs_suspect) - - -#define FUNCTION_PROLOGUE(FILE, SIZE) \ -{ register int regno; \ - register int mask = 0, fmask=0; \ - static char dont_save_regs[] = CALL_USED_REGISTERS; \ - register int push_loc = 0,tsize = SIZE+4; \ - char *fp_str; \ - extern char *reg_numchar[]; \ - extern int current_function_total_framesize; \ - this_varargs_suspect = VARARGS_SUSPECTED ; \ - fp_str = TARGET_NAME_REGS ? reg_names[STACK_POINTER_REGNUM] \ - : reg_numchar[STACK_POINTER_REGNUM]; \ - for (regno = 0; regno < 32; regno++) \ - if ( MUST_SAVE_REG_LOGUES \ - || (regs_ever_live[regno] && !dont_save_regs[regno])) \ - {tsize += 4; mask |= 1 << regno;} \ - for (regno = 32; regno < FIRST_PSEUDO_REGISTER; regno += 2) \ - if (regs_ever_live[regno] && !dont_save_regs[regno]) \ - {tsize += 8; fmask |= 1 << (regno-32);} \ - if (THIS_VARARGS_SUSPECTED) tsize += 16; \ - fprintf (FILE," #PROLOGUE\n"); \ - regno = STACK_POINTER_REGNUM; \ - tsize = AL_ADJUST_ALIGN (tsize); \ - \ - if (!frame_pointer_needed) \ - fprintf (FILE,"#define __0__gcc %d\n", \ - (!( regs_ever_live[29] || regs_ever_live[30] \ - || fmask || mask \ - || (SIZE > 0))) \ - ? 0:tsize); \ - \ - push_loc = 0; current_function_total_framesize = tsize; \ - fprintf (FILE, " #\t.mask\t0x%x\n", mask); \ - if (frame_pointer_needed || regs_ever_live[29] || regs_ever_live[30] \ - || fmask || mask \ - || (SIZE > 0)) \ - fprintf (FILE,"\tsubu\t%s,%d\t#temp=%5d,saveregs=%5d, sfo=%5d\n", \ - TARGET_NAME_REGS ? reg_names[29] \ - :reg_numchar[29],tsize,SIZE,tsize-SIZE, \ - STARTING_FRAME_OFFSET); \ - else fprintf (FILE," #NO STACK PUSH:\tSP %sused, FP %sused, FP %sneeded\n",\ - regs_ever_live[29]? "":"un", \ - regs_ever_live[30]? "":"un", \ - frame_pointer_needed ?"" : "not "); \ - for (regno = 31; regno >= 30; regno--) \ - { \ - if (MUST_SAVE_REG_LOGUES \ - || (regs_ever_live[regno] && !dont_save_regs[regno])) \ - { \ - push_loc += 4; \ - fprintf (FILE,"\tsw\t%s,%d(%s)\n", \ - TARGET_NAME_REGS ? reg_names[regno] \ - : reg_numchar[regno],push_loc,fp_str); \ - } \ - } \ - if (THIS_VARARGS_SUSPECTED) \ - { int fregno; \ - fprintf (FILE,"\taddi\t%s,$0,%d\t#Varargs suspicion\n", \ - TARGET_NAME_REGS ? reg_names[9] \ - : reg_numchar[9],tsize); \ - fprintf (FILE,"\tsw\t%s,-4(%s)\t#Varargs suspicion\n", \ - TARGET_NAME_REGS ? reg_names[9] \ - : reg_numchar[9], \ - TARGET_NAME_REGS ? reg_names[29] \ - : reg_numchar[29]); \ - for (fregno = 44; fregno< 48; fregno += 2) \ - {push_loc += 8; \ - fprintf (FILE,"\ts.d\t%s,%d(%s)\t#Varargs Suspicion\n", \ - ( (TARGET_NAME_REGS) \ - ?reg_names[fregno]: reg_numchar[fregno]), \ - push_loc,fp_str);} \ - } \ - for (regno = 29; regno >= 0; regno--) \ - { \ - if ( MUST_SAVE_REG_LOGUES \ - || (regs_ever_live[regno] && !dont_save_regs[regno])) \ - { \ - push_loc += 4; \ - fprintf (FILE, "\tsw\t%s,%d(%s)\n", \ - TARGET_NAME_REGS ? reg_names[regno] \ - : reg_numchar[regno],push_loc,fp_str); \ - } \ - } \ - fprintf (FILE, " #\t.fmask\t0x%x\n", fmask); \ - for (regno = 32; regno < FIRST_PSEUDO_REGISTER; regno += 2) \ - if (regs_ever_live[regno] && !dont_save_regs[regno]) \ - {push_loc += 8; \ - fprintf (FILE,"\ts.d\t%s,%d(%s)\n", \ - ( (TARGET_NAME_REGS) ? reg_names[regno] \ - : reg_numchar[regno]),push_loc,fp_str); \ - } \ - if (frame_pointer_needed) \ - fprintf (FILE,"\taddiu\t%s,%s,%d\t#Establish FramePTR\n", \ - (TARGET_NAME_REGS ? reg_names[FRAME_POINTER_REGNUM] : \ - reg_numchar[FRAME_POINTER_REGNUM]), \ - (TARGET_NAME_REGS ? reg_names[29] : reg_numchar[29]), \ - tsize); \ - fprintf (FILE," #END PROLOGUE\n"); \ -} - -/* Output assembler code to FILE to increment profiler label # LABELNO - for profiling a function entry. */ - -#define FUNCTION_PROFILER(FILE, LABELNO) \ - fprintf (FILE, "ERROR\t profiler LP%d,r0\n", (LABELNO)); - -/* EXIT_IGNORE_STACK should be nonzero if, when returning from a function, - the stack pointer does not matter. The value is tested only in - functions that have frame pointers. - No definition is equivalent to always zero. */ - -extern int may_call_alloca; -extern int current_function_pretend_args_size; - -#define EXIT_IGNORE_STACK 0 - - -/* This declaration is needed due to traditional/ANSI - incompatibilities which cannot be #ifdefed away - because they occur inside of macros. Sigh. */ - - -extern union tree_node *current_function_decl; -extern char *current_function_name; - -/* Tell prologue and epilogue if Register containing return - address should be saved / restored -*/ - -#define MUST_SAVE_REG_LOGUES (( frame_pointer_needed && (regno == 30)) \ - ||( (regno == 31) && regs_ever_live[31]) \ - ) - - -/* This macro generates the assembly code for function exit, - on machines that need it. If FUNCTION_EPILOGUE is not defined - then individual return instructions are generated for each - return statement. Args are same as for FUNCTION_PROLOGUE. */ - - -#define FUNCTION_EPILOGUE(FILE, SIZE) \ -{ register int regno; \ - register int mask = 0; \ - register int fmask = 0; \ - char *fp_str; \ - char *sp_str; \ - static char dont_save_regs[] = CALL_USED_REGISTERS; \ - register int push_loc ; \ - extern char *reg_numchar[]; \ - extern char *current_function_name; \ - extern int current_function_total_framesize; \ - push_loc = 0; \ - regno = STACK_POINTER_REGNUM; \ - sp_str = TARGET_NAME_REGS ? reg_names[STACK_POINTER_REGNUM] \ - : reg_numchar[STACK_POINTER_REGNUM]; \ - fp_str = TARGET_NAME_REGS ? reg_names[8] \ - :reg_numchar[8]; \ - fprintf (FILE," #EPILOGUE\n"); \ - if (!frame_pointer_needed) \ - fprintf (FILE,"#undef __0__gcc\n"); \ - else \ - fprintf (FILE,"\taddu\t%s,$0,%s\t# sp not trusted here \n", \ - fp_str, \ - TARGET_NAME_REGS ? reg_names[FRAME_POINTER_REGNUM] \ - :reg_numchar[FRAME_POINTER_REGNUM] \ - ); \ - for (regno = 0; regno < 32; regno++) \ - if ( MUST_SAVE_REG_LOGUES \ - || (regs_ever_live[regno] && !dont_save_regs[regno])) \ - mask |= 1 << regno; \ - fprintf (FILE, " #\t.mask\t0x%x\n", mask); \ - for (regno = 31; regno >= 0; regno--) \ - { if ( MUST_SAVE_REG_LOGUES \ - || (regs_ever_live[regno] && !dont_save_regs[regno])) \ - { \ - push_loc += 4; \ - fprintf (FILE,"\tlw\t%s,%d(%s)\n", \ - TARGET_NAME_REGS ? reg_names[regno] \ - : reg_numchar[regno], \ - (frame_pointer_needed ? \ - push_loc - current_function_total_framesize: \ - push_loc), \ - (frame_pointer_needed ? fp_str :sp_str)); \ - } \ - if ( THIS_VARARGS_SUSPECTED && (regno == 30)) push_loc += 16; \ - } \ - for (regno = 32; regno < FIRST_PSEUDO_REGISTER; regno += 2) \ - if (regs_ever_live[regno] && !dont_save_regs[regno]) \ - fmask |= 1 << (regno-32); \ - fprintf (FILE, " #\t.fmask\t0x%x\n", fmask); \ - for (regno = 32; regno < FIRST_PSEUDO_REGISTER; regno += 2) \ - { \ - if (regs_ever_live[regno] && !dont_save_regs[regno]) \ - { \ - push_loc += 8; \ - fprintf (FILE,"\tl.d\t%s,%d(%s)\n", \ - ( ( TARGET_NAME_REGS) ? reg_names[regno] \ - : reg_numchar[regno]), \ - (frame_pointer_needed ? \ - push_loc - current_function_total_framesize \ - : push_loc), \ - (frame_pointer_needed ? fp_str :sp_str)); \ - } \ - } \ - if (frame_pointer_needed) \ - fprintf (FILE,"\taddu\t%s,$0,%s\t# sp not trusted here \n", \ - TARGET_NAME_REGS ? reg_names[STACK_POINTER_REGNUM] \ - :reg_numchar[STACK_POINTER_REGNUM], \ - TARGET_NAME_REGS ? reg_names[8] \ - :reg_numchar[8] \ - ); \ - else \ - if (regs_ever_live[29]|| regs_ever_live[30] \ - || fmask || mask \ - || (SIZE > 0)) \ - fprintf (FILE,"\taddu\t%s,%d\t\n",TARGET_NAME_REGS ? reg_names[29]\ - :reg_numchar[29],current_function_total_framesize); \ - fprintf (FILE,"\tj\t$31\n"); \ - fprintf (FILE," #END EPILOGUE\n"); \ - fprintf (FILE," \t.end\t%s\n",current_function_name); \ - THIS_VARARGS_NOTSUSPECT; VARARGS_NOTSUSPECT;} - -/* If the memory Address ADDR is relative to the frame pointer, - correct it to be relative to the stack pointer. This is for - when we don't use a frame pointer. - ADDR should be a variable name. */ - -#define FIX_FRAME_POINTER_ADDRESS(ADDR,DEPTH) \ -{ rtx newaddr; \ - int frame_offset = -1; \ - /* fprintf(stderr,"FIX_FRAME depth=%d\n",DEPTH); */ \ - if(ADDR == frame_pointer_rtx) \ - frame_offset = 0; \ - else \ - if (GET_CODE(ADDR) == PLUS) \ - if(XEXP(ADDR,0) == frame_pointer_rtx) \ - if(GET_CODE(XEXP(ADDR,1)) == CONST_INT) \ - frame_offset = INTVAL(XEXP(ADDR,1)); \ - else abort_with_insn(ADDR,"Unable to FIX"); \ - else if (XEXP(ADDR,1) == frame_pointer_rtx) \ - if(GET_CODE(XEXP(ADDR,0)) == CONST_INT) \ - frame_offset = INTVAL(XEXP(ADDR,0)); \ - else abort_with_insn(ADDR,"Unable to FIX"); \ - else; \ - if (frame_offset >= 0) \ - { newaddr=gen_rtx(PLUS,Pmode,stack_pointer_rtx, \ - gen_rtx(PLUS,Pmode, \ - gen_rtx(CONST_INT,VOIDmode,frame_offset+(DEPTH)),\ - gen_rtx(SYMBOL_REF,SImode,"__0__gcc"))); \ - ADDR = newaddr; \ - } \ - } - - - -/* Addressing modes, and classification of registers for them. */ - -/* #define HAVE_POST_INCREMENT */ -/* #define HAVE_POST_DECREMENT */ - -/* #define HAVE_PRE_DECREMENT */ -/* #define HAVE_PRE_INCREMENT */ - -/* These assume that REGNO is a hard or pseudo reg number. - They give nonzero only if REGNO is a hard reg of the suitable class - or a pseudo reg currently allocated to a suitable hard reg. - These definitions are NOT overridden anywhere. */ - -#define REGNO_OK_FOR_INDEX_P(regno) \ -((regno) < FIRST_PSEUDO_REGISTER || reg_renumber[regno] >= 0) -#define REGNO_OK_FOR_BASE_P(regno) \ -((regno) < FIRST_PSEUDO_REGISTER || reg_renumber[regno] >= 0) -#define REGNO_OK_FOR_FP_P(REGNO) \ -(((REGNO) ^ 0x20) < 32 || (unsigned) (reg_renumber[REGNO] ^ 0x20) < 32) - -/* The macros REG_OK_FOR..._P assume that the arg is a REG rtx - and check its validity for a certain class. - We have two alternate definitions for each of them. - The usual definition accepts all pseudo regs; the other rejects them all. - The symbol REG_OK_STRICT causes the latter definition to be used. - - Most source files want to accept pseudo regs in the hope that - they will get allocated to the class that the insn wants them to be in. - Some source files that are used after register allocation - need to be strict. */ - -#ifndef REG_OK_STRICT - -/* Nonzero if X is a hard reg that can be used as an index or if - it is a pseudo reg. */ -#define REG_OK_FOR_INDEX_P(X) 1 -/* Nonzero if X is a hard reg that can be used as a base reg - of if it is a pseudo reg. */ -#define REG_OK_FOR_BASE_P(X) 1 - -#else - -/* Nonzero if X is a hard reg that can be used as an index. */ -#define REG_OK_FOR_INDEX_P(X) REGNO_OK_FOR_INDEX_P (REGNO (X)) -/* Nonzero if X is a hard reg that can be used as a base reg. */ -#define REG_OK_FOR_BASE_P(X) REGNO_OK_FOR_BASE_P (REGNO (X)) - -#endif - -#define REG_OK_FOR_CLASS_P(X, C) 0 - -#define REGNO_OK_FOR_CLASS_P(X, C) 0 - -#define ADDRESS_REG_P(X) \ - (GET_CODE (X) == REG ) - -/* 1 if X is an fp register. */ - -#define FP_REG_P(X) (REG_P (X) && REGNO_OK_FOR_FP_P (REGNO (X))) - -/* Maximum number of registers that can appear in a valid memory address. */ - -#define MAX_REGS_PER_ADDRESS 1 - -/* GO_IF_LEGITIMATE_ADDRESS recognizes an RTL expression - that is a valid memory address for an instruction. - The MODE argument is the machine mode for the MEM expression - that wants to use this address. - - The other macros defined here are used only in GO_IF_LEGITIMATE_ADDRESS, - except for CONSTANT_ADDRESS_P which is actually machine-independent. */ - -/* 1 if X is an address that we could indirect through. */ -#define INDIRECTABLE_ADDRESS_P(X) \ - (CONSTANT_ADDRESS_P (X) \ - || (GET_CODE (X) == REG && REG_OK_FOR_BASE_P (X)) \ - || (GET_CODE (X) == PLUS \ - && GET_CODE (XEXP (X, 0)) == REG \ - && REG_OK_FOR_BASE_P (XEXP (X, 0)) \ - && CONSTANT_ADDRESS_P (XEXP (X, 1)))) - - - -/* 1 if X is an address which is (+ (reg) (+ (const_int) (symbol_ref) )) */ -#define FIXED_FRAME_PTR_REL_P(X) \ - ( (GET_CODE(X) == PLUS) \ - && (GET_CODE(XEXP((X),0)) == REG) \ - && (GET_CODE(XEXP((X),1)) == PLUS) \ - && (GET_CODE(XEXP(XEXP((X),1),0)) == CONST_INT) \ - && (GET_CODE(XEXP(XEXP((X),1),1)) == SYMBOL_REF)) - -/* Go to ADDR if X is a valid address not using indexing. - (This much is the easy part.) */ -#define GO_IF_LEGITIMATE_ADDRESS(MODE, X, ADDR) \ -{ register rtx xfoob = (X); \ - if (GET_CODE (xfoob) == REG) goto ADDR; \ - if (INDIRECTABLE_ADDRESS_P (xfoob)) goto ADDR; \ - if (FIXED_FRAME_PTR_REL_P (xfoob)) goto ADDR; \ - } - - - - -#define CONSTANT_ADDRESS_P(X) \ - (GET_CODE (X) == LABEL_REF || GET_CODE (X) == SYMBOL_REF \ - || GET_CODE (X) == CONST_INT \ - || GET_CODE (X) == CONST) - -/* Nonzero if the constant value X is a legitimate general operand. - It is given that X satisfies CONSTANT_P or is a CONST_DOUBLE. - - Anything but a CONST_DOUBLE can be made to work. */ - -#define LEGITIMATE_CONSTANT_P(X) \ - (GET_CODE (X) != CONST_DOUBLE) - -/* Try machine-dependent ways of modifying an illegitimate address - to be legitimate. If we find one, return the new, valid address. - This macro is used in only one place: `memory_address' in explow.c. - - OLDX is the address as it was before break_out_memory_refs was called. - In some cases it is useful to look at this to decide what needs to be done. - - MODE and WIN are passed so that this macro can use - GO_IF_LEGITIMATE_ADDRESS. - - It is always safe for this macro to do nothing. It exists to recognize - opportunities to optimize the output. - - For the MIPS (so far ..), nothing needs to be done. - - ACHTUNG this is actually used by the FLOW analysis to get rid - of statements.... - -*/ - -#define LEGITIMIZE_ADDRESS(X,OLDX,MODE,WIN) {} - -/* Go to LABEL if ADDR (a legitimate address expression) - has an effect that depends on the machine mode it is used for. -*/ - - /* See if this is of any use here */ - -#define GO_IF_MODE_DEPENDENT_ADDRESS(ADDR,LABEL) \ -{ } - - -/* Specify the machine mode that this machine uses - for the index in the tablejump instruction. */ -#define CASE_VECTOR_MODE SImode - -/* Define this if the tablejump instruction expects the table - to contain offsets from the address of the table. - Do not define this if the table should contain absolute addresses. */ -/* #define CASE_VECTOR_PC_RELATIVE */ - -/* Specify the tree operation to be used to convert reals to integers. */ -#define IMPLICIT_FIX_EXPR FIX_ROUND_EXPR - -/* This is the kind of divide that is easiest to do in the general case. */ -#define EASY_DIV_EXPR TRUNC_DIV_EXPR - -/* Define this as 1 if `char' should by default be signed; else as 0. */ -#define DEFAULT_SIGNED_CHAR 1 - -/* Max number of bytes we can move from memory to memory - in one reasonably fast instruction. */ -#define MOVE_MAX 4 - -/* Nonzero if access to memory by bytes is slow and undesirable. */ -#define SLOW_BYTE_ACCESS 0 - -/* On Sun 4, this limit is 2048. We use 1500 to be safe, - since the length can run past this up to a continuation point. */ -#define DBX_CONTIN_LENGTH 1500 - -/* We assume that the store-condition-codes instructions store 0 for false - and some other value for true. This is the value stored for true. */ - -#define STORE_FLAG_VALUE 1 - -/* Define this if zero-extension is slow (more than one real instruction). */ -#define SLOW_ZERO_EXTEND - -/* Define if shifts truncate the shift count - which implies one can omit a sign-extension or zero-extension - of a shift count. - - Only 5 bits are used in SLLV and SRLV -*/ -#define SHIFT_COUNT_TRUNCATED - - -/* Value is 1 if truncating an integer of INPREC bits to OUTPREC bits - is done just by pretending it is already truncated. */ -#define TRULY_NOOP_TRUNCATION(OUTPREC, INPREC) 1 - -/* Specify the machine mode that pointers have. - After generation of rtl, the compiler makes no further distinction - between pointers and any other objects of this machine mode. */ -#define Pmode SImode - -/* A function address in a call instruction - is a word address (for indexing purposes) - so give the MEM rtx a words's mode. */ - -#define FUNCTION_MODE SImode - -/* Compute the cost of computing a constant rtl expression RTX - whose rtx-code is CODE. The body of this macro is a portion - of a switch statement. If the code is computed here, - return it with a return statement. Otherwise, break from the switch. */ - -#define CONST_COSTS(RTX,CODE) \ - case CONST_INT: \ - /* Constant zero is super cheap due to register 0. */ \ - if (RTX == const0_rtx) return 0; \ - if ((INTVAL (RTX) < 0x7fff) && (- INTVAL(RTX) < 0x7fff)) return 1; \ - case CONST: \ - case LABEL_REF: \ - case SYMBOL_REF: \ - return 3; \ - case CONST_DOUBLE: \ - return 5; - -/* Tell final.c how to eliminate redundant test instructions. */ - -/* Here we define machine-dependent flags and fields in cc_status - (see `conditions.h'). No extra ones are needed for the vax. */ -/* Tell final.c how to eliminate redundant test instructions. */ - -/* Tell final.c how to eliminate redundant test instructions. */ - -/* Here we define machine-dependent flags and fields in cc_status - (see `conditions.h'). No extra ones are needed for the vax. */ - -/* Store in cc_status the expressions - that the condition codes will describe - after execution of an instruction whose pattern is EXP. - Do not alter them if the instruction would not alter the cc's. */ - -#define NOTICE_UPDATE_CC(EXP, INSN) \ - CC_STATUS_INIT; - - -/* Here we define machine-dependent flags and fields in cc_status - (see `conditions.h'). */ - - -/* Control the assembler format that we output. */ - -/* Output at beginning of assembler file. */ - -#define ASM_FILE_START(FILE) \ -{ \ - if (TARGET_NAME_REGS) \ - fprintf (FILE, "#include \n\t.verstamp\t%s\n", TARGET_VERSNUM);\ - else fprintf (FILE, " #\t.verstamp\t%s\n", TARGET_VERSNUM); \ -/* print_options(FILE); */ \ - if (TARGET_GP_OPT) \ - fprintf (FILE, "#ifdef %sRESCAN_GCC\n", "__x_"); \ -} - -/* Output to assembler file text saying following lines - may contain character constants, extra white space, comments, etc. */ - -#define ASM_APP_ON " #APP\n" - -/* Output to assembler file text saying following lines - no longer contain unusual constructs. */ - -#define ASM_APP_OFF " #NO_APP\n" - -/* Output before read-only data. */ - -#define TEXT_SECTION_ASM_OP ".text" - -/* Output before writable data. */ - -#define DATA_SECTION_ASM_OP ".data" - -#define ASM_OUTPUT_MIPS_SECTIONS -#define OUTPUT_MIPS_SECTION_THRESHOLD ((mips_section_threshold >= 0 )?\ - mips_section_threshold : mips_section_get()) - -/* Output before writable short data. */ - -#define SDATA_SECTION_ASM_OP ".sdata" - -/* How to refer to registers in assembler output. - This sequence is indexed by compiler's hard-register-number (see above). */ - -#define REGISTER_NAMES \ -{"$0", "at", "v0", "v1", "a0", "a1", "a2", "a3", "t0", \ - "t1", "t2", "t3", "t4", "t5", "t6", "t7","s0", \ - "s1","s2","s3","s4","s5","s6","s7","t8","t9", \ - "k0","k1","gp","sp","fp","ra", \ - "$f0","$f1","$f2","$f3","$f4","$f5","$f6","$f7","$f8","$f9", \ -"$f10","$f11","$f12","$f13","$f14","$f15","$f16","$f17","$f18","$f19", \ -"$f20","$f21","$f22","$f23","$f24","$f25","$f26","$f27","$f28","$f29", \ -"$f30","$f31" \ -} -#define REGISTER_NUMCHAR \ -{ \ -"$0","$1","$2","$3","$4","$5","$6","$7","$8","$9", \ -"$10","$11","$12","$13","$14","$15","$16","$17","$18","$19", \ -"$20","$21","$22","$23","$24","$25","$26","$27","$28","$29", \ -"$30","$31", \ -"$f0","$f1","$f2","$f3","$f4","$f5","$f6","$f7","$f8","$f9", \ -"$f10","$f11","$f12","$f13","$f14","$f15","$f16","$f17","$f18","$f19", \ -"$f20","$f21","$f22","$f23","$f24","$f25","$f26","$f27","$f28","$f29", \ -"$f30","$f31" \ -} - - -/* How to renumber registers for dbx and gdb. - MIPS needs no change in the numeration. */ - -#define DBX_REGISTER_NUMBER(REGNO) (REGNO) - -/* Define results of standard character escape sequences. */ -#define TARGET_BELL 007 -#define TARGET_BS 010 -#define TARGET_TAB 011 -#define TARGET_NEWLINE 012 -#define TARGET_VT 013 -#define TARGET_FF 014 -#define TARGET_CR 015 - - /* LIST OF PRINT OPERAND CODES - - - ** 'x' X is CONST_INT, prints 16 bits in - ** Hexadecimal format = "0x%4x", - ** 'd' output integer constant in decimal, - ** 'u' Prints an 'u' if flag -mnofixed-ovfl - ** has been set, thus selecting addu - ** instruction instead of add. - */ - - -/* Print an instruction operand X on file FILE. - CODE is the code from the %-spec that requested printing this operand; - if `%z3' was used to print operand 3, then CODE is 'z'. - CODE is used as follows: - - LIST OF PRINT OPERAND CODES - - - 'x' X is CONST_INT, prints 16 bits in - ** Hexadecimal format = "0x%4x", - ** 'd' output integer constant in decimal, - ** ':' Prints an 'u' if flag -mnofixed-ovfl - ** has been set, thus selecting addu - ** instruction instead of add. - */ - -#define PRINT_OPERAND_PUNCT_VALID_P(CODE) \ - ((CODE) == ':') - -#define PRINT_OPERAND(FILE, X, CODE) \ -{ if ((CODE) == ':') \ - {if (TARGET_NOFIXED_OVFL)fprintf(FILE,"u");} \ - else if (GET_CODE (X) == REG) \ - { extern char *reg_numchar[]; \ - fprintf (FILE, "%s", TARGET_NAME_REGS ?reg_names[REGNO (X)] \ - :reg_numchar[REGNO (X) ]); \ - } \ - else \ - { \ - if (GET_CODE (X) == MEM) \ - output_address (XEXP (X, 0)); \ - else if (GET_CODE (X) == CONST_DOUBLE) \ - { union { double d; int i[2]; } u; \ - union { float f; int i; } u1; \ - u.i[0] = CONST_DOUBLE_LOW (X); \ - u.i[1] = CONST_DOUBLE_HIGH (X); \ - u1.f = u.d; \ - if (GET_MODE (X) == SFmode) \ - u.d = u1.f; \ - fprintf (FILE, "%.20e", u.d); } \ - else \ - { if ((CODE == 'x') && (GET_CODE(X) == CONST_INT)) \ - fprintf(FILE,"0x%x",0xffff & (INTVAL(X))); \ - else { if ((CODE == 'd') && (GET_CODE(X) == CONST_INT)) \ - fprintf(FILE,"%d",(INTVAL(X))); \ - else \ - { \ - if ((CODE) == 'd') abort(); \ - else output_addr_const (FILE, X);} \ - }}}} - -/* Print a memory operand whose address is X, on file FILE. */ - -#define PRINT_OPERAND_ADDRESS(FILE, ADDR) \ -{ register rtx reg1, reg2, breg, ireg; \ - register rtx addr = ADDR; \ - rtx offset; \ - extern char *reg_numchar[]; \ -/* my_print_rtx(addr);*/ \ - retry: \ - switch (GET_CODE (addr)) \ - { \ - case REG: \ - fprintf (FILE, "0(%s)", TARGET_NAME_REGS ? reg_names [REGNO (addr)]\ - : reg_numchar[REGNO(addr)]); \ - break; \ - case MEM: \ - case PRE_DEC: \ - case POST_INC: \ - abort(); \ - break; \ - case PLUS: \ - if( (GET_CODE (XEXP(addr,0)) == REG) \ - && (GET_CODE (XEXP(addr,1)) == PLUS) \ - && (GET_CODE (XEXP(XEXP(addr,1),1)) == SYMBOL_REF) \ - && (GET_CODE (XEXP(XEXP(addr,1),0)) == CONST_INT)) \ - {output_address(XEXP(XEXP(addr,1),0)); \ - fprintf(FILE,"+"); \ - output_address(XEXP(XEXP(addr,1),1)); \ - breg = XEXP(addr,0); \ - fprintf(FILE,"(%s)", TARGET_NAME_REGS ? \ - reg_names[REGNO (breg)]: reg_numchar[REGNO(breg)]); \ - break; \ - } \ - \ - reg1 = 0; reg2 = 0; \ - ireg = 0; breg = 0; \ - offset = 0; \ - /*fprintf(stderr,"PRINT_OPERAND_ADDRESS"); */ \ - if (CONSTANT_ADDRESS_P (XEXP (addr, 0)) \ - || GET_CODE (XEXP (addr, 0)) == MEM) \ - { \ - offset = XEXP (addr, 0); \ - addr = XEXP (addr, 1); \ - } \ - else if (CONSTANT_ADDRESS_P (XEXP (addr, 1)) \ - || GET_CODE (XEXP (addr, 1)) == MEM) \ - { \ - offset = XEXP (addr, 1); \ - addr = XEXP (addr, 0); \ - } \ - if (GET_CODE (addr) != PLUS) ; \ - else if (GET_CODE (XEXP (addr, 0)) == MULT) \ - { \ - reg1 = XEXP (addr, 0); \ - addr = XEXP (addr, 1); \ - } \ - else if (GET_CODE (XEXP (addr, 1)) == MULT) \ - { \ - reg1 = XEXP (addr, 1); \ - addr = XEXP (addr, 0); \ - } \ - else if (GET_CODE (XEXP (addr, 0)) == REG) \ - { \ - reg1 = XEXP (addr, 0); \ - addr = XEXP (addr, 1); \ - } \ - else if (GET_CODE (XEXP (addr, 1)) == REG) \ - { \ - reg1 = XEXP (addr, 1); \ - addr = XEXP (addr, 0); \ - } \ - if (GET_CODE (addr) == REG || GET_CODE (addr) == MULT) \ - { if (reg1 == 0) reg1 = addr; else reg2 = addr; addr = 0; } \ - if (offset != 0) { if (addr != 0) abort (); addr = offset; } \ - if (reg1 != 0 && GET_CODE (reg1) == MULT) \ - { breg = reg2; ireg = reg1; } \ - else if (reg2 != 0 && GET_CODE (reg2) == MULT) \ - { breg = reg1; ireg = reg2; } \ - else if (reg2 != 0 || GET_CODE (addr) == MEM) \ - { breg = reg2; ireg = reg1; } \ - else \ - { breg = reg1; ireg = reg2; } \ - if (addr != 0) \ - output_address (offset); \ - if (breg != 0) \ - { if (GET_CODE (breg) != REG) abort (); \ - fprintf (FILE, "(%s)", TARGET_NAME_REGS ? \ - reg_names[REGNO (breg)]: reg_numchar[REGNO(breg)]); }\ - if (ireg != 0) \ - { if (GET_CODE (ireg) == MULT) ireg = XEXP (ireg, 0); \ - if (GET_CODE (ireg) != REG) abort (); \ - fprintf (FILE, "[%s]", TARGET_NAME_REGS ? \ - reg_names[REGNO (ireg)]: reg_numchar[REGNO(ireg)]); }\ - break; \ - default: \ - output_addr_const (FILE, addr); \ - }} - - -/* This is how to output a note to DBX telling it the line number - to which the following sequence of instructions corresponds. - - This is needed for SunOS 4.0, and should not hurt for 3.2 - versions either. */ -#define ASM_OUTPUT_SOURCE_LINE(file, line) \ - { static int sym_lineno = 1; \ - fprintf (file, " #.stabn 68,0,%d,LM%d\nLM%d:\n", \ - line, sym_lineno, sym_lineno); \ - sym_lineno += 1; } - -/* This is how to output the definition of a user-level label named NAME, - such as the label on a static function or variable NAME. */ - -#define ASM_OUTPUT_LABEL(FILE,NAME) \ - do { assemble_name (FILE, NAME); fputs (":\n", FILE); } while (0) - -/* This is how to output a command to make the user-level label named NAME - defined for reference from other files. */ - -#define ASM_GLOBALIZE_LABEL(FILE,NAME) \ - do { fputs ("\t.globl ", FILE); assemble_name (FILE, NAME); \ - fputs ("\n", FILE); \ - if(TARGET_GP_OPT) {fputs ("#define _gccx__",FILE); \ - assemble_name(FILE,NAME); \ - fputs ("\n", FILE); \ - } \ - } while (0) - -#define ASM_DECLARE_FUNCTION_NAME(FILE,NAME,DECL) \ - fprintf(FILE,"\t.ent\t%s\n",NAME); \ - current_function_name = NAME; \ - ASM_OUTPUT_LABEL(FILE,NAME); - -/* This is how to output a reference to a user-level label named NAME. - `assemble_name' uses this. */ - -#define ASM_OUTPUT_LABELREF(FILE,NAME) \ - fprintf (FILE, "%s", NAME) - -/* This is how to output an internal numbered label where - PREFIX is the class of label and NUM is the number within the class. */ - -#define ASM_OUTPUT_INTERNAL_LABEL(FILE,PREFIX,NUM) \ - fprintf (FILE, "%s%d:\n", PREFIX, NUM) - -/* This is how to store into the string LABEL - the symbol_ref name of an internal numbered label where - PREFIX is the class of label and NUM is the number within the class. - This is suitable for output with `assemble_name'. */ - -#define ASM_GENERATE_INTERNAL_LABEL(LABEL,PREFIX,NUM) \ - sprintf (LABEL, "*%s%d", PREFIX, NUM) - -/* This is how to output an assembler line defining a `double' constant. */ - -#define ASM_OUTPUT_DOUBLE(FILE,VALUE) \ - fprintf (FILE, "\t.double %.20e\n", (VALUE)) - -/* This is how to output an assembler line defining a `float' constant. */ - -#define ASM_OUTPUT_FLOAT(FILE,VALUE) \ - fprintf (FILE, "\t.float %.12e\n", (VALUE)) - -/* This is how to output an assembler line defining an `int' constant. */ - -#define ASM_OUTPUT_INT(FILE,VALUE) \ -( fprintf (FILE, "\t.word "), \ - output_addr_const (FILE, (VALUE)), \ - fprintf (FILE, "\n")) - -/* Likewise for `char' and `short' constants. */ - -#define ASM_OUTPUT_SHORT(FILE,VALUE) \ -( fprintf (FILE, "\t.half "), \ - output_addr_const (FILE, (VALUE)), \ - fprintf (FILE, "\n")) - -#define ASM_OUTPUT_CHAR(FILE,VALUE) \ -( fprintf (FILE, "\t.byte "), \ - output_addr_const (FILE, (VALUE)), \ - fprintf (FILE, "\n")) - -/* This is how to output an assembler line for a numeric constant byte. */ - -#define ASM_OUTPUT_BYTE(FILE,VALUE) \ - fprintf (FILE, "\t.byte 0x%x\n", (VALUE)) - -/* This is how to output an element of a case-vector that is absolute. */ - -#define ASM_OUTPUT_ADDR_VEC_ELT(FILE, VALUE) \ - fprintf (FILE, "\t.word L%d\n", VALUE) - -/* This is how to output an element of a case-vector that is relative. - (We do not use such vectors, - but we must define this macro anyway.) */ - -#define ASM_OUTPUT_ADDR_DIFF_ELT(FILE, VALUE, REL) \ - fprintf (FILE, "\t.word L%d-L%d\n", VALUE, REL) - -/* This is how to output an assembler line - that says to advance the location counter - to a multiple of 2**LOG bytes. */ - -#define ASM_OUTPUT_ALIGN(FILE,LOG) \ - fprintf (FILE, "\t.align %d\n", (LOG)) - -#define ASM_OUTPUT_SKIP(FILE,SIZE) \ - fprintf (FILE, "\t.space %d\n", (SIZE)) - -/* The support of .comm and .extern below permits to take advantage - of the SDATA/SBSS sections supported by the MIPS ASSEMBLER and LOADER - However some problems have to be solved - a/ externs should be included ONCE - b/ the same external cannot appear both on an extern and .comm stmt - in the same assembly - c/ for the whole scheme to bring some benefit, .comm should appear - in front of the source asm -- whereas GCC put them at the end -*/ - - - /* ALL THESE PROBLEMS ARE PRESENTLY SOLVED */ - /* USING CONDITIONAL ASSEMBLY + FILE RESCAN */ - -#define EXTRA_SECTIONS in_sdata - -/* Define the additional functions to select our additional sections. */ - - /* on the MIPS it is not a good idea to put constants in the - text section, since this defeats the sdata/data mechanism. This - is especially true when -O2 is used. In this case an effort is - made to address with faster (gp) register relative addressing, - which can only get at sdata and sbss items (there is no stext !!) - */ -#define EXTRA_SECTION_FUNCTIONS \ -void \ -sdata_section () \ -{ \ - if (in_section != in_sdata) \ - { \ - fprintf (asm_out_file, "%s\n", SDATA_SECTION_ASM_OP); \ - in_section = in_sdata; \ - } \ -} - -/* Given a decl node or constant node, choose the section to output it in - and select that section. */ - - /* following takes care of constants emitted from - the hash table entries (see above comment) - */ -#define SELECT_SECTION_MODE(MODE,RTX) \ -{ \ - extern int mips_section_threshold; \ - if (( GET_MODE_SIZE(MODE)/ BITS_PER_UNIT) \ - <= OUTPUT_MIPS_SECTION_THRESHOLD) \ - sdata_section(); \ - else \ - data_section (); \ -} \ - -#define SELECT_SECTION(DECL) \ -{ \ - extern int mips_section_threshold; \ - if (int_size_in_bytes (TREE_TYPE (DECL)) \ - <= OUTPUT_MIPS_SECTION_THRESHOLD) \ - sdata_section (); \ - else \ - data_section (); \ -} - -/* This says how to output an assembler line - to define a global common symbol. */ - -#define ASM_OUTPUT_COMMON(FILE, NAME, SIZE, ROUNDED) \ -( ((TARGET_GP_OPT)? \ - fprintf((FILE),"\n#else"),0 :0), \ - fputs ("\n\t.comm ", (FILE)), \ - assemble_name ((FILE), (NAME)), \ - fprintf ((FILE), ",%d\n", (ROUNDED)), \ - (TARGET_GP_OPT ? (fputs("\n#define _gccx__",(FILE)), \ - assemble_name((FILE),NAME),0):0), \ - ((TARGET_GP_OPT)? \ - fprintf((FILE),"\n#endif\n#ifdef %sRESCAN_GCC","__x_"),0 :0) \ -) - - -/* This says how to output an external */ -/* It would be possible not to output anything and let undefined */ -/* symbol become external. However the assembler uses length information on*/ -/* externals to allocate in data/sdata bss/sbss, thereby saving exec time */ - -#define ASM_OUTPUT_EXTERNAL(FILE,DECL,NAME) \ - mips_output_external(FILE,DECL,NAME) - - -/* This says how to output an assembler line - to define a local common symbol. */ - -#define ASM_OUTPUT_LOCAL(FILE, NAME, SIZE, ROUNDED) \ -( fputs ("\n\t.lcomm\t", (FILE)), \ - assemble_name ((FILE), (NAME)), \ - fprintf ((FILE), ",%d\n", (ROUNDED))) - -/* This says what to print at the end of the assembly file */ -#define ASM_FILE_END(FILE) \ - mips_asm_file_end(FILE) - -/* Store in OUTPUT a string (made with alloca) containing - an assembler-name for a local static variable named NAME. - LABELNO is an integer which is different for each call. */ - -#define ASM_FORMAT_PRIVATE_NAME(OUTPUT, NAME, LABELNO) \ -( (OUTPUT) = (char *) alloca (strlen ((NAME)) + 10), \ - sprintf ((OUTPUT), "%s.%d", (NAME), (LABELNO))) - -#define ASM_OUTPUT_REG_POP(FILE,REGNO) \ - (fprintf (FILE,"ERROR: ASM_OUTPUT_REG_POP\n")) -#define ASM_OUTPUT_REG_PUSH(FILE,REGNO) \ - (fprintf (FILE,"ERROR: ASM_OUTPUT_REG_PUSH\n")) - - /* The following macro is taken from the */ - /* C-text of varasm.c. It has been modified */ - /* to handle the VARARG_SUSPECTED hack */ -#define ASM_OUTPUT_ASCII(FILE, P , SIZE) \ -{ int i; \ - fprintf ((FILE), "\t.ascii \""); \ - VARARGS_SUSPECT( 0 == strncmp((P),"__%%VARARGS",11)); \ - for (i = 0; i < (SIZE); i++) \ - { \ - register int c = (P)[i]; \ - if (i != 0 && (i / 200) * 200 == i) \ - fprintf ((FILE), "\"\n\t.ascii \""); \ - if (c == '\"' || c == '\\') \ - putc ('\\', (FILE)); \ - if (c >= ' ' && c < 0177) \ - putc (c, (FILE)); \ - else \ - { \ - fprintf ((FILE), "\\%o", c); \ - /* After an octal-escape, if a digit follows, \ - terminate one string constant and start another. \ - The Vax assembler fails to stop reading the escape \ - after three digits, so this is the only way we \ - can get it to parse the data properly. */ \ - if (i < (SIZE) - 1 && (P)[i + 1] >= '0' && (P)[i + 1] <= '9')\ - fprintf ((FILE), "\"\n\t.ascii \""); \ - } \ - } \ - fprintf ((FILE), "\"\n"); \ - } - - - -/* Define the parentheses used to group arithmetic operations - in assembler code. */ - -#define ASM_OPEN_PAREN "(" -#define ASM_CLOSE_PAREN ")" - -/* Specify what to precede various sizes of constant with - in the output file. */ - -#define ASM_INT_OP ".word " -#define ASM_SHORT_OP ".half " -#define ASM_CHAR_OP ".byte " - - -#define DEBUG_LOG_INSN(X) { \ - extern rtx al_log_insn_debug; \ - al_log_insn_debug=(X); } diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/version.c b/OpenSim/Utilities/simmToOpenSim/acpp/src/version.c deleted file mode 100644 index e6bea26d18..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/version.c +++ /dev/null @@ -1 +0,0 @@ -char *version_string = "1.35.95x"; diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/y.tab.c b/OpenSim/Utilities/simmToOpenSim/acpp/src/y.tab.c deleted file mode 100644 index 9291608ed3..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/y.tab.c +++ /dev/null @@ -1,2164 +0,0 @@ -/* A Bison parser, made by GNU Bison 1.875b. */ - -/* Skeleton parser for Yacc-like parsing with Bison, - Copyright (C) 1984, 1989, 1990, 2000, 2001, 2002, 2003 Free Software Foundation, Inc. - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2, or (at your option) - any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 59 Temple Place - Suite 330, - Boston, MA 02111-1307, USA. */ - -/* As a special exception, when this file is copied by Bison into a - Bison output file, you may use that output file without restriction. - This special exception was added by the Free Software Foundation - in version 1.24 of Bison. */ - -/* Written by Richard Stallman by simplifying the original so called - ``semantic'' parser. */ - -/* All symbols defined below should begin with yy or YY, to avoid - infringing on user name space. This should be done even for local - variables, as they might otherwise be expanded by user macros. - There are some unavoidable exceptions within include files to - define necessary library symbols; they are noted "INFRINGES ON - USER NAME SPACE" below. */ - -/* Identify Bison output. */ -#define YYBISON 1 - -/* Skeleton name. */ -#define YYSKELETON_NAME "yacc.c" - -/* Pure parsers. */ -#define YYPURE 0 - -/* Using locations. */ -#define YYLSP_NEEDED 0 - - - -/* Tokens. */ -#ifndef YYTOKENTYPE -# define YYTOKENTYPE - /* Put the tokens into the symbol table, so that GDB and other debuggers - know about them. */ - enum yytokentype { - INT = 258, - CHAR = 259, - NAME = 260, - ERROR = 261, - DVALUE = 262, - OR = 263, - AND = 264, - NOTEQUAL = 265, - EQUAL = 266, - GEQ = 267, - LEQ = 268, - RSH = 269, - LSH = 270, - UNARY = 271 - }; -#endif -#define INT 258 -#define CHAR 259 -#define NAME 260 -#define ERROR 261 -#define DVALUE 262 -#define OR 263 -#define AND 264 -#define NOTEQUAL 265 -#define EQUAL 266 -#define GEQ 267 -#define LEQ 268 -#define RSH 269 -#define LSH 270 -#define UNARY 271 - - - - -/* Copy the first part of user declarations. */ -#line 26 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - -#include "stdio.h" -#include "stdlib.h" -#include "math.h" -#include "config.h" -#include -#include -/* #define YYDEBUG 1 */ - - int yylex (); - void yyerror (); - int inside_math = 0; - int expression_value; - double math_value; - - extern FILE* fp_out; - - static jmp_buf parse_return_error; - - /* some external tables of character types */ - extern unsigned char is_idstart[], is_idchar[]; -#if defined sgi || defined WIN32 -/* table to tell if c is horizontal space. isspace () thinks that - newline is space; this is not a good idea for this program. */ -char is_hor_space[256]; -#endif - -#ifdef WIN32 -#define gettxt(S1,S2) (S1 ## S2) -#endif - -#ifndef CHAR_TYPE_SIZE -#define CHAR_TYPE_SIZE BITS_PER_UNIT -#endif - -acpp_error (msg) -{ -#ifdef INSIDE_SIMM - error(0, msg); // 0 = SIMM error action of "recover" -#else - fprintf (stderr, "error: %s\n", msg); -#endif -} - -acpp_warning (msg) -{ -#ifdef INSIDE_SIMM - error(0, msg); // 0 = SIMM error action of "recover" -#else - fprintf (stderr, "warning: %s\n", msg); -#endif -} - -/* Enabling traces. */ -#ifndef YYDEBUG -# define YYDEBUG 0 -#endif - -/* Enabling verbose error messages. */ -#ifdef YYERROR_VERBOSE -# undef YYERROR_VERBOSE -# define YYERROR_VERBOSE 1 -#else -# define YYERROR_VERBOSE 0 -#endif - -#if ! defined (YYSTYPE) && ! defined (YYSTYPE_IS_DECLARED) -#line 62 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" -typedef union YYSTYPE { - struct constant {long value; int unsignedp;} integer; - int voidval; - char *sval; - double dval; -} YYSTYPE; -/* Line 191 of yacc.c. */ -#line 151 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.tab.c" -# define yystype YYSTYPE /* obsolescent; will be withdrawn */ -# define YYSTYPE_IS_DECLARED 1 -# define YYSTYPE_IS_TRIVIAL 1 -#endif - - - -/* Copy the second part of user declarations. */ - - -/* Line 214 of yacc.c. */ -#line 163 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.tab.c" - -#if ! defined (yyoverflow) || YYERROR_VERBOSE - -/* The parser invokes alloca or malloc; define the necessary symbols. */ - -# if YYSTACK_USE_ALLOCA -# define YYSTACK_ALLOC alloca -# else -# ifndef YYSTACK_USE_ALLOCA -# if defined (alloca) || defined (_ALLOCA_H) -# define YYSTACK_ALLOC alloca -# else -# ifdef __GNUC__ -# define YYSTACK_ALLOC __builtin_alloca -# endif -# endif -# endif -# endif - -# ifdef YYSTACK_ALLOC - /* Pacify GCC's `empty if-body' warning. */ -# define YYSTACK_FREE(Ptr) do { /* empty */; } while (0) -# else -# if defined (__STDC__) || defined (__cplusplus) -# include /* INFRINGES ON USER NAME SPACE */ -# define YYSIZE_T size_t -# endif -# define YYSTACK_ALLOC malloc -# define YYSTACK_FREE free -# endif -#endif /* ! defined (yyoverflow) || YYERROR_VERBOSE */ - - -#if (! defined (yyoverflow) \ - && (! defined (__cplusplus) \ - || (YYSTYPE_IS_TRIVIAL))) - -/* A type that is properly aligned for any stack member. */ -union yyalloc -{ - short yyss; - YYSTYPE yyvs; - }; - -/* The size of the maximum gap between one aligned stack and the next. */ -# define YYSTACK_GAP_MAXIMUM (sizeof (union yyalloc) - 1) - -/* The size of an array large to enough to hold all stacks, each with - N elements. */ -# define YYSTACK_BYTES(N) \ - ((N) * (sizeof (short) + sizeof (YYSTYPE)) \ - + YYSTACK_GAP_MAXIMUM) - -/* Copy COUNT objects from FROM to TO. The source and destination do - not overlap. */ -# ifndef YYCOPY -# if 1 < __GNUC__ -# define YYCOPY(To, From, Count) \ - __builtin_memcpy (To, From, (Count) * sizeof (*(From))) -# else -# define YYCOPY(To, From, Count) \ - do \ - { \ - register YYSIZE_T yyi; \ - for (yyi = 0; yyi < (Count); yyi++) \ - (To)[yyi] = (From)[yyi]; \ - } \ - while (0) -# endif -# endif - -/* Relocate STACK from its old location to the new one. The - local variables YYSIZE and YYSTACKSIZE give the old and new number of - elements in the stack, and YYPTR gives the new location of the - stack. Advance YYPTR to a properly aligned location for the next - stack. */ -# define YYSTACK_RELOCATE(Stack) \ - do \ - { \ - YYSIZE_T yynewbytes; \ - YYCOPY (&yyptr->Stack, Stack, yysize); \ - Stack = &yyptr->Stack; \ - yynewbytes = yystacksize * sizeof (*Stack) + YYSTACK_GAP_MAXIMUM; \ - yyptr += yynewbytes / sizeof (*yyptr); \ - } \ - while (0) - -#endif - -#if defined (__STDC__) || defined (__cplusplus) - typedef signed char yysigned_char; -#else - typedef short yysigned_char; -#endif - -/* YYFINAL -- State number of the termination state. */ -#define YYFINAL 25 -/* YYLAST -- Last index in YYTABLE. */ -#define YYLAST 226 - -/* YYNTOKENS -- Number of terminals. */ -#define YYNTOKENS 36 -/* YYNNTS -- Number of nonterminals. */ -#define YYNNTS 5 -/* YYNRULES -- Number of rules. */ -#define YYNRULES 39 -/* YYNRULES -- Number of states. */ -#define YYNSTATES 79 - -/* YYTRANSLATE(YYLEX) -- Bison symbol number corresponding to YYLEX. */ -#define YYUNDEFTOK 2 -#define YYMAXUTOK 271 - -#define YYTRANSLATE(YYX) \ - ((unsigned int) (YYX) <= YYMAXUTOK ? yytranslate[YYX] : YYUNDEFTOK) - -/* YYTRANSLATE[YYLEX] -- Bison symbol number corresponding to YYLEX. */ -static const unsigned char yytranslate[] = -{ - 0, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 30, 2, 2, 2, 28, 15, 2, - 32, 33, 26, 24, 10, 25, 2, 27, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 9, 2, - 18, 2, 19, 8, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 14, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 34, 13, 35, 31, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 1, 2, 3, 4, - 5, 6, 7, 11, 12, 16, 17, 20, 21, 22, - 23, 29 -}; - -#if YYDEBUG -/* YYPRHS[YYN] -- Index of the first RHS symbol of rule number YYN in - YYRHS. */ -static const unsigned char yyprhs[] = -{ - 0, 0, 3, 5, 7, 9, 13, 16, 19, 22, - 26, 29, 33, 37, 41, 45, 49, 53, 55, 59, - 63, 67, 71, 75, 79, 83, 87, 91, 95, 99, - 103, 107, 111, 115, 119, 123, 127, 133, 135, 137 -}; - -/* YYRHS -- A `-1'-separated list of the rules' RHS. */ -static const yysigned_char yyrhs[] = -{ - 37, 0, -1, 38, -1, 40, -1, 39, -1, 38, - 10, 39, -1, 25, 39, -1, 30, 39, -1, 31, - 39, -1, 32, 38, 33, -1, 25, 40, -1, 32, - 40, 33, -1, 34, 40, 35, -1, 40, 26, 40, - -1, 40, 27, 40, -1, 40, 24, 40, -1, 40, - 25, 40, -1, 7, -1, 39, 26, 39, -1, 39, - 27, 39, -1, 39, 28, 39, -1, 39, 24, 39, - -1, 39, 25, 39, -1, 39, 23, 39, -1, 39, - 22, 39, -1, 39, 17, 39, -1, 39, 16, 39, - -1, 39, 21, 39, -1, 39, 20, 39, -1, 39, - 18, 39, -1, 39, 19, 39, -1, 39, 15, 39, - -1, 39, 14, 39, -1, 39, 13, 39, -1, 39, - 12, 39, -1, 39, 11, 39, -1, 39, 8, 39, - 9, 39, -1, 3, -1, 4, -1, 5, -1 -}; - -/* YYRLINE[YYN] -- source line where rule number YYN was defined. */ -static const unsigned char yyrline[] = -{ - 0, 94, 94, 96, 101, 102, 107, 110, 113, 116, - 121, 123, 125, 130, 132, 139, 141, 143, 148, 154, - 165, 176, 179, 182, 188, 194, 197, 200, 206, 212, - 218, 224, 227, 230, 233, 236, 239, 242, 244, 246 -}; -#endif - -#if YYDEBUG || YYERROR_VERBOSE -/* YYTNME[SYMBOL-NUM] -- String name of the symbol SYMBOL-NUM. - First, the terminals, then, starting at YYNTOKENS, nonterminals. */ -static const char *const yytname[] = -{ - "$end", "error", "$undefined", "INT", "CHAR", "NAME", "ERROR", "DVALUE", - "'?'", "':'", "','", "OR", "AND", "'|'", "'^'", "'&'", "NOTEQUAL", - "EQUAL", "'<'", "'>'", "GEQ", "LEQ", "RSH", "LSH", "'+'", "'-'", "'*'", - "'/'", "'%'", "UNARY", "'!'", "'~'", "'('", "')'", "'{'", "'}'", - "$accept", "start", "exp1", "exp", "exp3", 0 -}; -#endif - -# ifdef YYPRINT -/* YYTOKNUM[YYLEX-NUM] -- Internal token number corresponding to - token YYLEX-NUM. */ -static const unsigned short yytoknum[] = -{ - 0, 256, 257, 258, 259, 260, 261, 262, 63, 58, - 44, 263, 264, 124, 94, 38, 265, 266, 60, 62, - 267, 268, 269, 270, 43, 45, 42, 47, 37, 271, - 33, 126, 40, 41, 123, 125 -}; -# endif - -/* YYR1[YYN] -- Symbol number of symbol that rule YYN derives. */ -static const unsigned char yyr1[] = -{ - 0, 36, 37, 37, 38, 38, 39, 39, 39, 39, - 40, 40, 40, 40, 40, 40, 40, 40, 39, 39, - 39, 39, 39, 39, 39, 39, 39, 39, 39, 39, - 39, 39, 39, 39, 39, 39, 39, 39, 39, 39 -}; - -/* YYR2[YYN] -- Number of symbols composing right hand side of rule YYN. */ -static const unsigned char yyr2[] = -{ - 0, 2, 1, 1, 1, 3, 2, 2, 2, 3, - 2, 3, 3, 3, 3, 3, 3, 1, 3, 3, - 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, - 3, 3, 3, 3, 3, 3, 5, 1, 1, 1 -}; - -/* YYDEFACT[STATE-NAME] -- Default rule to reduce with in state - STATE-NUM when YYTABLE doesn't specify something else to do. Zero - means the default is an error. */ -static const unsigned char yydefact[] = -{ - 0, 37, 38, 39, 17, 0, 0, 0, 0, 0, - 0, 2, 4, 3, 6, 10, 0, 0, 7, 8, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 9, 11, 12, 5, 0, 35, 34, 33, 32, 31, - 26, 25, 29, 30, 28, 27, 24, 23, 21, 22, - 18, 19, 20, 15, 16, 13, 14, 0, 36 -}; - -/* YYDEFGOTO[NTERM-NUM]. */ -static const yysigned_char yydefgoto[] = -{ - -1, 10, 20, 12, 15 -}; - -/* YYPACT[STATE-NUM] -- Index in YYTABLE of the portion describing - STATE-NUM. */ -#define YYPACT_NINF -22 -static const short yypact[] = -{ - 48, -22, -22, -22, -22, 48, 66, 66, 48, 49, - 15, 7, 112, 68, -22, -22, 66, 66, -22, -22, - 9, -17, 49, 49, -21, -22, 66, 66, 66, 66, - 66, 66, 66, 66, 66, 66, 66, 66, 66, 66, - 66, 66, 66, 66, 66, 66, 49, 49, 49, 49, - -22, -22, -22, 112, 91, 129, 145, 160, 174, 187, - 198, 198, 35, 35, 35, 35, 19, 19, 39, 39, - -22, -22, -22, -14, -14, -22, -22, 66, 112 -}; - -/* YYPGOTO[NTERM-NUM]. */ -static const yysigned_char yypgoto[] = -{ - -22, -22, 18, -5, 41 -}; - -/* YYTABLE[YYPACT[STATE-NUM]]. What to do in state STATE-NUM. If - positive, shift that token. If negative, reduce the rule which - number is the opposite. If zero, do what YYDEFACT says. - If YYTABLE_NINF, syntax error. */ -#define YYTABLE_NINF -1 -static const unsigned char yytable[] = -{ - 14, 18, 19, 46, 47, 48, 49, 46, 47, 48, - 49, 14, 48, 49, 52, 25, 51, 26, 11, 26, - 0, 53, 54, 55, 56, 57, 58, 59, 60, 61, - 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, - 72, 13, 50, 41, 42, 43, 44, 45, 0, 21, - 24, 1, 2, 3, 0, 4, 4, 39, 40, 41, - 42, 43, 44, 45, 21, 43, 44, 45, 0, 1, - 2, 3, 78, 5, 22, 0, 0, 0, 6, 7, - 8, 23, 9, 9, 0, 0, 0, 73, 74, 75, - 76, 16, 46, 47, 48, 49, 6, 7, 17, 27, - 77, 0, 28, 29, 30, 31, 32, 33, 34, 35, - 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, - 27, 0, 0, 28, 29, 30, 31, 32, 33, 34, - 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, - 45, 29, 30, 31, 32, 33, 34, 35, 36, 37, - 38, 39, 40, 41, 42, 43, 44, 45, 30, 31, - 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, - 42, 43, 44, 45, 31, 32, 33, 34, 35, 36, - 37, 38, 39, 40, 41, 42, 43, 44, 45, 32, - 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, - 43, 44, 45, 33, 34, 35, 36, 37, 38, 39, - 40, 41, 42, 43, 44, 45, 35, 36, 37, 38, - 39, 40, 41, 42, 43, 44, 45 -}; - -static const yysigned_char yycheck[] = -{ - 5, 6, 7, 24, 25, 26, 27, 24, 25, 26, - 27, 16, 26, 27, 35, 0, 33, 10, 0, 10, - -1, 26, 27, 28, 29, 30, 31, 32, 33, 34, - 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, - 45, 0, 33, 24, 25, 26, 27, 28, -1, 8, - 9, 3, 4, 5, -1, 7, 7, 22, 23, 24, - 25, 26, 27, 28, 23, 26, 27, 28, -1, 3, - 4, 5, 77, 25, 25, -1, -1, -1, 30, 31, - 32, 32, 34, 34, -1, -1, -1, 46, 47, 48, - 49, 25, 24, 25, 26, 27, 30, 31, 32, 8, - 9, -1, 11, 12, 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, - 8, -1, -1, 11, 12, 13, 14, 15, 16, 17, - 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, - 28, 12, 13, 14, 15, 16, 17, 18, 19, 20, - 21, 22, 23, 24, 25, 26, 27, 28, 13, 14, - 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 14, 15, 16, 17, 18, 19, - 20, 21, 22, 23, 24, 25, 26, 27, 28, 15, - 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, - 26, 27, 28, 16, 17, 18, 19, 20, 21, 22, - 23, 24, 25, 26, 27, 28, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, 28 -}; - -/* YYSTOS[STATE-NUM] -- The (internal number of the) accessing - symbol of state STATE-NUM. */ -static const unsigned char yystos[] = -{ - 0, 3, 4, 5, 7, 25, 30, 31, 32, 34, - 37, 38, 39, 40, 39, 40, 25, 32, 39, 39, - 38, 40, 25, 32, 40, 0, 10, 8, 11, 12, - 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, - 23, 24, 25, 26, 27, 28, 24, 25, 26, 27, - 33, 33, 35, 39, 39, 39, 39, 39, 39, 39, - 39, 39, 39, 39, 39, 39, 39, 39, 39, 39, - 39, 39, 39, 40, 40, 40, 40, 9, 39 -}; - -#if ! defined (YYSIZE_T) && defined (__SIZE_TYPE__) -# define YYSIZE_T __SIZE_TYPE__ -#endif -#if ! defined (YYSIZE_T) && defined (size_t) -# define YYSIZE_T size_t -#endif -#if ! defined (YYSIZE_T) -# if defined (__STDC__) || defined (__cplusplus) -# include /* INFRINGES ON USER NAME SPACE */ -# define YYSIZE_T size_t -# endif -#endif -#if ! defined (YYSIZE_T) -# define YYSIZE_T unsigned int -#endif - -#define yyerrok (yyerrstatus = 0) -#define yyclearin (yychar = YYEMPTY) -#define YYEMPTY (-2) -#define YYEOF 0 - -#define YYACCEPT goto yyacceptlab -#define YYABORT goto yyabortlab -#define YYERROR goto yyerrlab1 - - -/* Like YYERROR except do call yyerror. This remains here temporarily - to ease the transition to the new meaning of YYERROR, for GCC. - Once GCC version 2 has supplanted version 1, this can go. */ - -#define YYFAIL goto yyerrlab - -#define YYRECOVERING() (!!yyerrstatus) - -#define YYBACKUP(Token, Value) \ -do \ - if (yychar == YYEMPTY && yylen == 1) \ - { \ - yychar = (Token); \ - yylval = (Value); \ - yytoken = YYTRANSLATE (yychar); \ - YYPOPSTACK; \ - goto yybackup; \ - } \ - else \ - { \ - yyerror ("syntax error: cannot back up");\ - YYERROR; \ - } \ -while (0) - -#define YYTERROR 1 -#define YYERRCODE 256 - -/* YYLLOC_DEFAULT -- Compute the default location (before the actions - are run). */ - -#ifndef YYLLOC_DEFAULT -# define YYLLOC_DEFAULT(Current, Rhs, N) \ - Current.first_line = Rhs[1].first_line; \ - Current.first_column = Rhs[1].first_column; \ - Current.last_line = Rhs[N].last_line; \ - Current.last_column = Rhs[N].last_column; -#endif - -/* YYLEX -- calling `yylex' with the right arguments. */ - -#ifdef YYLEX_PARAM -# define YYLEX yylex (YYLEX_PARAM) -#else -# define YYLEX yylex () -#endif - -/* Enable debugging if requested. */ -#if YYDEBUG - -# ifndef YYFPRINTF -# include /* INFRINGES ON USER NAME SPACE */ -# define YYFPRINTF fprintf -# endif - -# define YYDPRINTF(Args) \ -do { \ - if (yydebug) \ - YYFPRINTF Args; \ -} while (0) - -# define YYDSYMPRINT(Args) \ -do { \ - if (yydebug) \ - yysymprint Args; \ -} while (0) - -# define YYDSYMPRINTF(Title, Token, Value, Location) \ -do { \ - if (yydebug) \ - { \ - YYFPRINTF (stderr, "%s ", Title); \ - yysymprint (stderr, \ - Token, Value); \ - YYFPRINTF (stderr, "\n"); \ - } \ -} while (0) - -/*------------------------------------------------------------------. -| yy_stack_print -- Print the state stack from its BOTTOM up to its | -| TOP (cinluded). | -`------------------------------------------------------------------*/ - -#if defined (__STDC__) || defined (__cplusplus) -static void -yy_stack_print (short *bottom, short *top) -#else -static void -yy_stack_print (bottom, top) - short *bottom; - short *top; -#endif -{ - YYFPRINTF (stderr, "Stack now"); - for (/* Nothing. */; bottom <= top; ++bottom) - YYFPRINTF (stderr, " %d", *bottom); - YYFPRINTF (stderr, "\n"); -} - -# define YY_STACK_PRINT(Bottom, Top) \ -do { \ - if (yydebug) \ - yy_stack_print ((Bottom), (Top)); \ -} while (0) - - -/*------------------------------------------------. -| Report that the YYRULE is going to be reduced. | -`------------------------------------------------*/ - -#if defined (__STDC__) || defined (__cplusplus) -static void -yy_reduce_print (int yyrule) -#else -static void -yy_reduce_print (yyrule) - int yyrule; -#endif -{ - int yyi; - unsigned int yylno = yyrline[yyrule]; - YYFPRINTF (stderr, "Reducing stack by rule %d (line %u), ", - yyrule - 1, yylno); - /* Print the symbols being reduced, and their result. */ - for (yyi = yyprhs[yyrule]; 0 <= yyrhs[yyi]; yyi++) - YYFPRINTF (stderr, "%s ", yytname [yyrhs[yyi]]); - YYFPRINTF (stderr, "-> %s\n", yytname [yyr1[yyrule]]); -} - -# define YY_REDUCE_PRINT(Rule) \ -do { \ - if (yydebug) \ - yy_reduce_print (Rule); \ -} while (0) - -/* Nonzero means print parse trace. It is left uninitialized so that - multiple parsers can coexist. */ -int yydebug; -#else /* !YYDEBUG */ -# define YYDPRINTF(Args) -# define YYDSYMPRINT(Args) -# define YYDSYMPRINTF(Title, Token, Value, Location) -# define YY_STACK_PRINT(Bottom, Top) -# define YY_REDUCE_PRINT(Rule) -#endif /* !YYDEBUG */ - - -/* YYINITDEPTH -- initial size of the parser's stacks. */ -#ifndef YYINITDEPTH -# define YYINITDEPTH 200 -#endif - -/* YYMAXDEPTH -- maximum size the stacks can grow to (effective only - if the built-in stack extension method is used). - - Do not make this value too large; the results are undefined if - SIZE_MAX < YYSTACK_BYTES (YYMAXDEPTH) - evaluated with infinite-precision integer arithmetic. */ - -#if YYMAXDEPTH == 0 -# undef YYMAXDEPTH -#endif - -#ifndef YYMAXDEPTH -# define YYMAXDEPTH 10000 -#endif - - - -#if YYERROR_VERBOSE - -# ifndef yystrlen -# if defined (__GLIBC__) && defined (_STRING_H) -# define yystrlen strlen -# else -/* Return the length of YYSTR. */ -static YYSIZE_T -# if defined (__STDC__) || defined (__cplusplus) -yystrlen (const char *yystr) -# else -yystrlen (yystr) - const char *yystr; -# endif -{ - register const char *yys = yystr; - - while (*yys++ != '\0') - continue; - - return yys - yystr - 1; -} -# endif -# endif - -# ifndef yystpcpy -# if defined (__GLIBC__) && defined (_STRING_H) && defined (_GNU_SOURCE) -# define yystpcpy stpcpy -# else -/* Copy YYSRC to YYDEST, returning the address of the terminating '\0' in - YYDEST. */ -static char * -# if defined (__STDC__) || defined (__cplusplus) -yystpcpy (char *yydest, const char *yysrc) -# else -yystpcpy (yydest, yysrc) - char *yydest; - const char *yysrc; -# endif -{ - register char *yyd = yydest; - register const char *yys = yysrc; - - while ((*yyd++ = *yys++) != '\0') - continue; - - return yyd - 1; -} -# endif -# endif - -#endif /* !YYERROR_VERBOSE */ - - - -#if YYDEBUG -/*--------------------------------. -| Print this symbol on YYOUTPUT. | -`--------------------------------*/ - -#if defined (__STDC__) || defined (__cplusplus) -static void -yysymprint (FILE *yyoutput, int yytype, YYSTYPE *yyvaluep) -#else -static void -yysymprint (yyoutput, yytype, yyvaluep) - FILE *yyoutput; - int yytype; - YYSTYPE *yyvaluep; -#endif -{ - /* Pacify ``unused variable'' warnings. */ - (void) yyvaluep; - - if (yytype < YYNTOKENS) - { - YYFPRINTF (yyoutput, "token %s (", yytname[yytype]); -# ifdef YYPRINT - YYPRINT (yyoutput, yytoknum[yytype], *yyvaluep); -# endif - } - else - YYFPRINTF (yyoutput, "nterm %s (", yytname[yytype]); - - switch (yytype) - { - default: - break; - } - YYFPRINTF (yyoutput, ")"); -} - -#endif /* ! YYDEBUG */ -/*-----------------------------------------------. -| Release the memory associated to this symbol. | -`-----------------------------------------------*/ - -#if defined (__STDC__) || defined (__cplusplus) -static void -yydestruct (int yytype, YYSTYPE *yyvaluep) -#else -static void -yydestruct (yytype, yyvaluep) - int yytype; - YYSTYPE *yyvaluep; -#endif -{ - /* Pacify ``unused variable'' warnings. */ - (void) yyvaluep; - - switch (yytype) - { - - default: - break; - } -} - - -/* Prevent warnings from -Wmissing-prototypes. */ - -#ifdef YYPARSE_PARAM -# if defined (__STDC__) || defined (__cplusplus) -int yyparse (void *YYPARSE_PARAM); -# else -int yyparse (); -# endif -#else /* ! YYPARSE_PARAM */ -#if defined (__STDC__) || defined (__cplusplus) -int yyparse (void); -#else -int yyparse (); -#endif -#endif /* ! YYPARSE_PARAM */ - - - -/* The lookahead symbol. */ -int yychar; - -/* The semantic value of the lookahead symbol. */ -YYSTYPE yylval; - -/* Number of syntax errors so far. */ -int yynerrs; - - - -/*----------. -| yyparse. | -`----------*/ - -#ifdef YYPARSE_PARAM -# if defined (__STDC__) || defined (__cplusplus) -int yyparse (void *YYPARSE_PARAM) -# else -int yyparse (YYPARSE_PARAM) - void *YYPARSE_PARAM; -# endif -#else /* ! YYPARSE_PARAM */ -#if defined (__STDC__) || defined (__cplusplus) -int -yyparse (void) -#else -int -yyparse () - -#endif -#endif -{ - - register int yystate; - register int yyn; - int yyresult; - /* Number of tokens to shift before error messages enabled. */ - int yyerrstatus; - /* Lookahead token as an internal (translated) token number. */ - int yytoken = 0; - - /* Three stacks and their tools: - `yyss': related to states, - `yyvs': related to semantic values, - `yyls': related to locations. - - Refer to the stacks thru separate pointers, to allow yyoverflow - to reallocate them elsewhere. */ - - /* The state stack. */ - short yyssa[YYINITDEPTH]; - short *yyss = yyssa; - register short *yyssp; - - /* The semantic value stack. */ - YYSTYPE yyvsa[YYINITDEPTH]; - YYSTYPE *yyvs = yyvsa; - register YYSTYPE *yyvsp; - - - -#define YYPOPSTACK (yyvsp--, yyssp--) - - YYSIZE_T yystacksize = YYINITDEPTH; - - /* The variables used to return semantic value and location from the - action routines. */ - YYSTYPE yyval; - - - /* When reducing, the number of symbols on the RHS of the reduced - rule. */ - int yylen; - - YYDPRINTF ((stderr, "Starting parse\n")); - - yystate = 0; - yyerrstatus = 0; - yynerrs = 0; - yychar = YYEMPTY; /* Cause a token to be read. */ - - /* Initialize stack pointers. - Waste one element of value and location stack - so that they stay on the same level as the state stack. - The wasted elements are never initialized. */ - - yyssp = yyss; - yyvsp = yyvs; - - goto yysetstate; - -/*------------------------------------------------------------. -| yynewstate -- Push a new state, which is found in yystate. | -`------------------------------------------------------------*/ - yynewstate: - /* In all cases, when you get here, the value and location stacks - have just been pushed. so pushing a state here evens the stacks. - */ - yyssp++; - - yysetstate: - *yyssp = yystate; - - if (yyss + yystacksize - 1 <= yyssp) - { - /* Get the current used size of the three stacks, in elements. */ - YYSIZE_T yysize = yyssp - yyss + 1; - -#ifdef yyoverflow - { - /* Give user a chance to reallocate the stack. Use copies of - these so that the &'s don't force the real ones into - memory. */ - YYSTYPE *yyvs1 = yyvs; - short *yyss1 = yyss; - - - /* Each stack pointer address is followed by the size of the - data in use in that stack, in bytes. This used to be a - conditional around just the two extra args, but that might - be undefined if yyoverflow is a macro. */ - yyoverflow ("parser stack overflow", - &yyss1, yysize * sizeof (*yyssp), - &yyvs1, yysize * sizeof (*yyvsp), - - &yystacksize); - - yyss = yyss1; - yyvs = yyvs1; - } -#else /* no yyoverflow */ -# ifndef YYSTACK_RELOCATE - goto yyoverflowlab; -# else - /* Extend the stack our own way. */ - if (YYMAXDEPTH <= yystacksize) - goto yyoverflowlab; - yystacksize *= 2; - if (YYMAXDEPTH < yystacksize) - yystacksize = YYMAXDEPTH; - - { - short *yyss1 = yyss; - union yyalloc *yyptr = - (union yyalloc *) YYSTACK_ALLOC (YYSTACK_BYTES (yystacksize)); - if (! yyptr) - goto yyoverflowlab; - YYSTACK_RELOCATE (yyss); - YYSTACK_RELOCATE (yyvs); - -# undef YYSTACK_RELOCATE - if (yyss1 != yyssa) - YYSTACK_FREE (yyss1); - } -# endif -#endif /* no yyoverflow */ - - yyssp = yyss + yysize - 1; - yyvsp = yyvs + yysize - 1; - - - YYDPRINTF ((stderr, "Stack size increased to %lu\n", - (unsigned long int) yystacksize)); - - if (yyss + yystacksize - 1 <= yyssp) - YYABORT; - } - - YYDPRINTF ((stderr, "Entering state %d\n", yystate)); - - goto yybackup; - -/*-----------. -| yybackup. | -`-----------*/ -yybackup: - -/* Do appropriate processing given the current state. */ -/* Read a lookahead token if we need one and don't already have one. */ -/* yyresume: */ - - /* First try to decide what to do without reference to lookahead token. */ - - yyn = yypact[yystate]; - if (yyn == YYPACT_NINF) - goto yydefault; - - /* Not known => get a lookahead token if don't already have one. */ - - /* YYCHAR is either YYEMPTY or YYEOF or a valid lookahead symbol. */ - if (yychar == YYEMPTY) - { - YYDPRINTF ((stderr, "Reading a token: ")); - yychar = YYLEX; - } - - if (yychar <= YYEOF) - { - yychar = yytoken = YYEOF; - YYDPRINTF ((stderr, "Now at end of input.\n")); - } - else - { - yytoken = YYTRANSLATE (yychar); - YYDSYMPRINTF ("Next token is", yytoken, &yylval, &yylloc); - } - - /* If the proper action on seeing token YYTOKEN is to reduce or to - detect an error, take that action. */ - yyn += yytoken; - if (yyn < 0 || YYLAST < yyn || yycheck[yyn] != yytoken) - goto yydefault; - yyn = yytable[yyn]; - if (yyn <= 0) - { - if (yyn == 0 || yyn == YYTABLE_NINF) - goto yyerrlab; - yyn = -yyn; - goto yyreduce; - } - - if (yyn == YYFINAL) - YYACCEPT; - - /* Shift the lookahead token. */ - YYDPRINTF ((stderr, "Shifting token %s, ", yytname[yytoken])); - - /* Discard the token being shifted unless it is eof. */ - if (yychar != YYEOF) - yychar = YYEMPTY; - - *++yyvsp = yylval; - - - /* Count tokens shifted since error; after three, turn off error - status. */ - if (yyerrstatus) - yyerrstatus--; - - yystate = yyn; - goto yynewstate; - - -/*-----------------------------------------------------------. -| yydefault -- do the default action for the current state. | -`-----------------------------------------------------------*/ -yydefault: - yyn = yydefact[yystate]; - if (yyn == 0) - goto yyerrlab; - goto yyreduce; - - -/*-----------------------------. -| yyreduce -- Do a reduction. | -`-----------------------------*/ -yyreduce: - /* yyn is the number of a rule to reduce with. */ - yylen = yyr2[yyn]; - - /* If YYLEN is nonzero, implement the default value of the action: - `$$ = $1'. - - Otherwise, the following line sets YYVAL to garbage. - This behavior is undocumented and Bison - users should not rely upon it. Assigning to YYVAL - unconditionally makes the parser a bit smaller, and it avoids a - GCC warning that YYVAL may be used uninitialized. */ - yyval = yyvsp[1-yylen]; - - - YY_REDUCE_PRINT (yyn); - switch (yyn) - { - case 2: -#line 95 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { expression_value = yyvsp[0].integer.value; math_value = 0.0; ;} - break; - - case 3: -#line 97 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { math_value = yyvsp[0].dval; expression_value = 0; ;} - break; - - case 5: -#line 103 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer = yyvsp[0].integer; ;} - break; - - case 6: -#line 108 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = - yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[0].integer.unsignedp; ;} - break; - - case 7: -#line 111 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = ! yyvsp[0].integer.value; - yyval.integer.unsignedp = 0; ;} - break; - - case 8: -#line 114 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = ~ yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[0].integer.unsignedp; ;} - break; - - case 9: -#line 117 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer = yyvsp[-1].integer; ;} - break; - - case 10: -#line 122 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.dval = - yyvsp[0].dval; ;} - break; - - case 11: -#line 124 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.dval = yyvsp[-1].dval; ;} - break; - - case 12: -#line 126 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.dval = yyvsp[-1].dval; ;} - break; - - case 13: -#line 131 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.dval = yyvsp[-2].dval * yyvsp[0].dval; ;} - break; - - case 14: -#line 133 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { if (yyvsp[0].dval == 0.0) - { - acpp_error("division by zero in expression"); - yyvsp[0].dval = 1.0; - } - yyval.dval = yyvsp[-2].dval / yyvsp[0].dval; ;} - break; - - case 15: -#line 140 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.dval = yyvsp[-2].dval + yyvsp[0].dval; ;} - break; - - case 16: -#line 142 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.dval = yyvsp[-2].dval - yyvsp[0].dval; ;} - break; - - case 17: -#line 144 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.dval = yylval.dval; ;} - break; - - case 18: -#line 149 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; - if (yyval.integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value * yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value * yyvsp[0].integer.value; ;} - break; - - case 19: -#line 155 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { if (yyvsp[0].integer.value == 0) - { - acpp_error("division by zero in #if"); - yyvsp[0].integer.value = 1; - } - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; - if (yyval.integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value / yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value / yyvsp[0].integer.value; ;} - break; - - case 20: -#line 166 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { if (yyvsp[0].integer.value == 0) - { - acpp_error("division by zero in #if"); - yyvsp[0].integer.value = 1; - } - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; - if (yyval.integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value % yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value % yyvsp[0].integer.value; ;} - break; - - case 21: -#line 177 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = yyvsp[-2].integer.value + yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; ;} - break; - - case 22: -#line 180 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = yyvsp[-2].integer.value - yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; ;} - break; - - case 23: -#line 183 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp; - if (yyval.integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value << yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value << yyvsp[0].integer.value; ;} - break; - - case 24: -#line 189 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp; - if (yyval.integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value >> yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value >> yyvsp[0].integer.value; ;} - break; - - case 25: -#line 195 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = (yyvsp[-2].integer.value == yyvsp[0].integer.value); - yyval.integer.unsignedp = 0; ;} - break; - - case 26: -#line 198 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = (yyvsp[-2].integer.value != yyvsp[0].integer.value); - yyval.integer.unsignedp = 0; ;} - break; - - case 27: -#line 201 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.unsignedp = 0; - if (yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value <= yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value <= yyvsp[0].integer.value; ;} - break; - - case 28: -#line 207 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.unsignedp = 0; - if (yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value >= yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value >= yyvsp[0].integer.value; ;} - break; - - case 29: -#line 213 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.unsignedp = 0; - if (yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value < yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value < yyvsp[0].integer.value; ;} - break; - - case 30: -#line 219 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.unsignedp = 0; - if (yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp) - yyval.integer.value = (unsigned) yyvsp[-2].integer.value > yyvsp[0].integer.value; - else - yyval.integer.value = yyvsp[-2].integer.value > yyvsp[0].integer.value; ;} - break; - - case 31: -#line 225 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = yyvsp[-2].integer.value & yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; ;} - break; - - case 32: -#line 228 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = yyvsp[-2].integer.value ^ yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; ;} - break; - - case 33: -#line 231 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = yyvsp[-2].integer.value | yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; ;} - break; - - case 34: -#line 234 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = (yyvsp[-2].integer.value && yyvsp[0].integer.value); - yyval.integer.unsignedp = 0; ;} - break; - - case 35: -#line 237 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = (yyvsp[-2].integer.value || yyvsp[0].integer.value); - yyval.integer.unsignedp = 0; ;} - break; - - case 36: -#line 240 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = yyvsp[-4].integer.value ? yyvsp[-2].integer.value : yyvsp[0].integer.value; - yyval.integer.unsignedp = yyvsp[-2].integer.unsignedp || yyvsp[0].integer.unsignedp; ;} - break; - - case 37: -#line 243 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer = yylval.integer; ;} - break; - - case 38: -#line 245 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer = yylval.integer; ;} - break; - - case 39: -#line 247 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - { yyval.integer.value = 0; - yyval.integer.unsignedp = 0; ;} - break; - - - } - -/* Line 999 of yacc.c. */ -#line 1372 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.tab.c" - - yyvsp -= yylen; - yyssp -= yylen; - - - YY_STACK_PRINT (yyss, yyssp); - - *++yyvsp = yyval; - - - /* Now `shift' the result of the reduction. Determine what state - that goes to, based on the state we popped back to and the rule - number reduced by. */ - - yyn = yyr1[yyn]; - - yystate = yypgoto[yyn - YYNTOKENS] + *yyssp; - if (0 <= yystate && yystate <= YYLAST && yycheck[yystate] == *yyssp) - yystate = yytable[yystate]; - else - yystate = yydefgoto[yyn - YYNTOKENS]; - - goto yynewstate; - - -/*------------------------------------. -| yyerrlab -- here on detecting error | -`------------------------------------*/ -yyerrlab: - /* If not already recovering from an error, report this error. */ - if (!yyerrstatus) - { - ++yynerrs; -#if YYERROR_VERBOSE - yyn = yypact[yystate]; - - if (YYPACT_NINF < yyn && yyn < YYLAST) - { - YYSIZE_T yysize = 0; - int yytype = YYTRANSLATE (yychar); - const char* yyprefix; - char *yymsg; - int yyx; - - /* Start YYX at -YYN if negative to avoid negative indexes in - YYCHECK. */ - int yyxbegin = yyn < 0 ? -yyn : 0; - - /* Stay within bounds of both yycheck and yytname. */ - int yychecklim = YYLAST - yyn; - int yyxend = yychecklim < YYNTOKENS ? yychecklim : YYNTOKENS; - int yycount = 0; - - yyprefix = ", expecting "; - for (yyx = yyxbegin; yyx < yyxend; ++yyx) - if (yycheck[yyx + yyn] == yyx && yyx != YYTERROR) - { - yysize += yystrlen (yyprefix) + yystrlen (yytname [yyx]); - yycount += 1; - if (yycount == 5) - { - yysize = 0; - break; - } - } - yysize += (sizeof ("syntax error, unexpected ") - + yystrlen (yytname[yytype])); - yymsg = (char *) YYSTACK_ALLOC (yysize); - if (yymsg != 0) - { - char *yyp = yystpcpy (yymsg, "syntax error, unexpected "); - yyp = yystpcpy (yyp, yytname[yytype]); - - if (yycount < 5) - { - yyprefix = ", expecting "; - for (yyx = yyxbegin; yyx < yyxend; ++yyx) - if (yycheck[yyx + yyn] == yyx && yyx != YYTERROR) - { - yyp = yystpcpy (yyp, yyprefix); - yyp = yystpcpy (yyp, yytname[yyx]); - yyprefix = " or "; - } - } - yyerror (yymsg); - YYSTACK_FREE (yymsg); - } - else - yyerror ("syntax error; also virtual memory exhausted"); - } - else -#endif /* YYERROR_VERBOSE */ - yyerror ("syntax error"); - } - - - - if (yyerrstatus == 3) - { - /* If just tried and failed to reuse lookahead token after an - error, discard it. */ - - /* Return failure if at end of input. */ - if (yychar == YYEOF) - { - /* Pop the error token. */ - YYPOPSTACK; - /* Pop the rest of the stack. */ - while (yyss < yyssp) - { - YYDSYMPRINTF ("Error: popping", yystos[*yyssp], yyvsp, yylsp); - yydestruct (yystos[*yyssp], yyvsp); - YYPOPSTACK; - } - YYABORT; - } - - YYDSYMPRINTF ("Error: discarding", yytoken, &yylval, &yylloc); - yydestruct (yytoken, &yylval); - yychar = YYEMPTY; - - } - - /* Else will try to reuse lookahead token after shifting the error - token. */ - goto yyerrlab1; - - -/*----------------------------------------------------. -| yyerrlab1 -- error raised explicitly by an action. | -`----------------------------------------------------*/ -yyerrlab1: - yyerrstatus = 3; /* Each real token shifted decrements this. */ - - for (;;) - { - yyn = yypact[yystate]; - if (yyn != YYPACT_NINF) - { - yyn += YYTERROR; - if (0 <= yyn && yyn <= YYLAST && yycheck[yyn] == YYTERROR) - { - yyn = yytable[yyn]; - if (0 < yyn) - break; - } - } - - /* Pop the current state because it cannot handle the error token. */ - if (yyssp == yyss) - YYABORT; - - YYDSYMPRINTF ("Error: popping", yystos[*yyssp], yyvsp, yylsp); - yydestruct (yystos[yystate], yyvsp); - yyvsp--; - yystate = *--yyssp; - - YY_STACK_PRINT (yyss, yyssp); - } - - if (yyn == YYFINAL) - YYACCEPT; - - YYDPRINTF ((stderr, "Shifting error token, ")); - - *++yyvsp = yylval; - - - yystate = yyn; - goto yynewstate; - - -/*-------------------------------------. -| yyacceptlab -- YYACCEPT comes here. | -`-------------------------------------*/ -yyacceptlab: - yyresult = 0; - goto yyreturn; - -/*-----------------------------------. -| yyabortlab -- YYABORT comes here. | -`-----------------------------------*/ -yyabortlab: - yyresult = 1; - goto yyreturn; - -#ifndef yyoverflow -/*----------------------------------------------. -| yyoverflowlab -- parser overflow comes here. | -`----------------------------------------------*/ -yyoverflowlab: - yyerror ("parser stack overflow"); - yyresult = 2; - /* Fall through. */ -#endif - -yyreturn: -#ifndef yyoverflow - if (yyss != yyssa) - YYSTACK_FREE (yyss); -#endif - return yyresult; -} - - -#line 250 "C:\\Users\\Simm\\acppTest\\acpp\\src\\cexp.y" - - -/* During parsing of a C expression, the pointer to the next character - is in this variable. */ - -static char *lexptr; - -/* Take care of parsing a number (anything that starts with a digit). - Set yylval and return the token type; update lexptr. - LEN is the number of characters in it. */ - -int -parse_number (olen) - int olen; -{ - register char *p = lexptr; - register long n = 0; - register int c; - register int base = 10; - register int len = olen; - - /* Check to see if this is a floating point number or not */ - for (c = 0; c < len; c++) - { - if (p[c] == '.') - { - if (inside_math == 0) - { - yyerror ("floating point numbers not allowed in #if expressions"); - return ERROR; - } - else - { - char *np = (char*)alloca(len + 1); - bcopy(p, np, len); - np[len] = '\0'; - yylval.dval = atof(np); - lexptr += len; - return DVALUE; - } - } - } - - yylval.integer.unsignedp = 0; - - if (len >= 3 && (!strncmp (p, "0x", 2) || !strncmp (p, "0X", 2))) { - p += 2; - base = 16; - len -= 2; - } - else if (*p == '0') - base = 8; - - while (len > 0) { - c = *p++; - len--; - if (c >= 'A' && c <= 'Z') c += 'a' - 'A'; - - if (c >= '0' && c <= '9') { - n *= base; - n += c - '0'; - } else if (base == 16 && c >= 'a' && c <= 'f') { - n *= base; - n += c - 'a' + 10; - } else { - /* `l' means long, and `u' means unsigned. */ - while (1) { - if (c == 'l' || c == 'L') - ; - else if (c == 'u' || c == 'U') - yylval.integer.unsignedp = 1; - else - break; - - if (len == 0) - break; - c = *p++; - len--; - } - /* Don't look for any more digits after the suffixes. */ - break; - } - } - - if (len != 0) { - yyerror ("Invalid number in #if expression"); - return ERROR; - } - - lexptr = p; - - if (inside_math > 0) - { - yylval.dval = (double)n; - return DVALUE; - } - else - { - /* If too big to be signed, consider it unsigned. */ - if (n < 0) - yylval.integer.unsignedp = 1; - - yylval.integer.value = n; - return INT; - } - - return INT; -} - -struct token { - char *operator; - int token; -}; - -#define NULL 0 - -static struct token tokentab2[] = { - {"&&", AND}, - {"||", OR}, - {"<<", LSH}, - {">>", RSH}, - {"==", EQUAL}, - {"!=", NOTEQUAL}, - {"<=", LEQ}, - {">=", GEQ}, - {NULL, ERROR} -}; - -/* Read one token, getting characters through lexptr. */ - -int -yylex () -{ - register int c; - register int namelen; - register char *tokstart; - register struct token *toktab; - - retry: - - tokstart = lexptr; - c = *tokstart; - /* See if it is a special token of length 2. */ - for (toktab = tokentab2; toktab->operator != NULL; toktab++) - if (c == *toktab->operator && tokstart[1] == toktab->operator[1]) { - lexptr += 2; - return toktab->token; - } - - switch (c) { - case 0: - return 0; - - case ' ': - case '\t': - case '\n': - lexptr++; - goto retry; - -#ifdef sgi - case 'L': - { - /* look for wide char constants */ - int c2; - int i; - for(i = 1, c2 = *(lexptr+i); is_hor_space[c2]; - ++i,c2 = *(lexptr+i)) - /* do nothing on horiz space stuff */ { } - if(c2 != '\'') { - /* Not a wide char const. An identifier */ - break; - } - lexptr += i ; /* points to the quote now - Is wide char constant */ - tokstart = lexptr; - c = c2; /* c now a quote */ - } -#endif - case '\'': - lexptr++; - c = *lexptr++; - if (c == '\\') - c = parse_escape (&lexptr); - - /* Sign-extend the constant if chars are signed on target machine. */ - { - if (lookup ("__CHAR_UNSIGNED__", sizeof ("__CHAR_UNSIGNED__")-1, -1) - || ((c >> (CHAR_TYPE_SIZE - 1)) & 1) == 0) - yylval.integer.value = c & ((1 << CHAR_TYPE_SIZE) - 1); - else - yylval.integer.value = c | ~((1 << CHAR_TYPE_SIZE) - 1); - } - - yylval.integer.unsignedp = 0; - c = *lexptr++; - if (c != '\'') { - yyerror ("Invalid character constant in #if"); - return ERROR; - } - - return CHAR; - - /* some of these chars are invalid in constant expressions; - maybe do something about them later */ - case '/': - case '+': - case '-': - case '*': - case '%': - case '|': - case '&': - case '^': - case '~': - case '!': - case '@': - case '<': - case '>': - case '(': - case ')': - case '[': - case ']': - case '.': - case '?': - case ':': - case '=': - case '{': - case '}': - case ',': - lexptr++; - return c; - - case '"': - yyerror ("double quoted strings not allowed in #if expressions"); - return ERROR; - } - - /* If you are not inside a math expression, then numbers have - * to be integers. Thus they should contain only numerals from 0 to 9. - * However, a decimal point is allowed because it is checked for - * later (inside parse_number). - * If you are inside a math expression, then the number can start with - * a numeral or decimal point, and can contain a trailing exponent. - */ - if ((c >= '0' && c <= '9') || c == '.') - { - if (inside_math == 0) - { - for (namelen = 0; - c = tokstart[namelen], is_idchar[c] || c == '.'; - namelen++) - ; - } - else - { - int exp = 0; - for (namelen = 0; - c = tokstart[namelen], is_idchar[c] || c == '.' || (exp == 1 && (c == '-' || c == '+')); - namelen++) - { - if (exp == 0 && (c == 'e' || c == 'E' || c == 'g' || c == 'G')) - exp = 1; - else if (exp == 1 && (c == '-' || c == '+')) - exp = 0; - } - } - - return parse_number (namelen); - } - - if (!is_idstart[c]) { - yyerror ("Invalid token in expression"); - return ERROR; - } - - /* It is a name. See how long it is. */ - - for (namelen = 0; is_idchar[tokstart[namelen]]; namelen++) - ; - - lexptr += namelen; - return NAME; -} - - -/* Parse a C escape sequence. STRING_PTR points to a variable - containing a pointer to the string to parse. That pointer - is updated past the characters we use. The value of the - escape sequence is returned. - - A negative value means the sequence \ newline was seen, - which is supposed to be equivalent to nothing at all. - - If \ is followed by a null character, we return a negative - value and leave the string pointer pointing at the null character. - - If \ is followed by 000, we return 0 and leave the string pointer - after the zeros. A value of 0 does not mean end of string. */ - -int -parse_escape (string_ptr) - char **string_ptr; -{ - register int c = *(*string_ptr)++; - switch (c) - { - case 'a': - return TARGET_BELL; - case 'b': - return TARGET_BS; - case 'e': - return 033; - case 'f': - return TARGET_FF; - case 'n': - return TARGET_NEWLINE; - case 'r': - return TARGET_CR; - case 't': - return TARGET_TAB; - case 'v': - return TARGET_VT; - case '\n': - return -2; - case 0: - (*string_ptr)--; - return 0; - case '^': - c = *(*string_ptr)++; - if (c == '\\') - c = parse_escape (string_ptr); - if (c == '?') - return 0177; - return (c & 0200) | (c & 037); - - case '0': - case '1': - case '2': - case '3': - case '4': - case '5': - case '6': - case '7': - { - register int i = c - '0'; - register int count = 0; - while (++count < 3) - { - c = *(*string_ptr)++; - if (c >= '0' && c <= '7') - i = (i << 3) + c - '0'; - else - { - (*string_ptr)--; - break; - } - } - if ((i & ~((1 << CHAR_TYPE_SIZE) - 1)) != 0) - { - i &= (1 << CHAR_TYPE_SIZE) - 1; - acpp_warning ("octal character constant does not fit in a byte"); - } - return i; - } - case 'x': - { - register int i = 0; - register int count = 0; - for (;;) - { - c = *(*string_ptr)++; - if (c >= '0' && c <= '9') - i = (i << 4) + c - '0'; - else if (c >= 'a' && c <= 'f') - i = (i << 4) + c - 'a' + 10; - else if (c >= 'A' && c <= 'F') - i = (i << 4) + c - 'A' + 10; - else - { - (*string_ptr)--; - break; - } - } - if ((i & ~((1 << BITS_PER_UNIT) - 1)) != 0) - { - i &= (1 << BITS_PER_UNIT) - 1; - acpp_warning ("hex character constant does not fit in a byte"); - } - return i; - } - default: - return c; - } -} - -void -yyerror (s) - char *s; -{ - acpp_error (s); - longjmp (parse_return_error, 1); -} - -/* This page contains the entry point to this file. */ - -#if 0 -/* THIS DOESN'T EVEN BEGIN TO WORK */ -/* Parse STRING as an assertion (the AT&T ANSI cpp extension), and complain if - * this fails to use up all of the contents of STRING */ -int -parse_assertion_extension (buf) - char *buf; -{ - int token; - int c; - int negate = 0; - int value = 0; - - for(c = *buf; c != '#'; c = *buf++) { - if (c == '!') - negate = !negate; - else if(!isspace(c)) - acpp_error("Unexpected character in assertion expression\n"); - } - - /* buf is now one past the '#' character */ - lexptr = buf; - - if((token = yylex()) != NAME) - acpp_error("Syntax for assertion conditionals is '#if #assertion-name(tokens)'\n"); - - /* for the moment - check the syntax, but don't actually *do* anything. */ - return (negate ? !value : value); -} -#endif - -/* Parse STRING as an expression, and complain if this fails - to use up all of the contents of STRING. */ -/* We do not support C comments. They should be removed before - this function is called. */ - -int -parse_c_expression (string) - char *string; -{ - lexptr = string; - - if (lexptr == 0 || *lexptr == 0) { - acpp_error("empty #if expression"); - return 0; /* don't include the #if group */ - } - - /* if there is some sort of scanning error, just return 0 and assume - the parsing routine has printed an error message somewhere. - there is surely a better thing to do than this. */ - if (setjmp (parse_return_error)) - return 0; - - if (yyparse ()) - return 0; /* actually this is never reached - the way things stand. */ - if (*lexptr) - acpp_error("Junk after end of expression."); - - return expression_value; /* set by yyparse () */ -} - -double -parse_c_math_expression (string) - char *string; -{ - lexptr = string; - - if (lexptr == 0 || *lexptr == 0) { - acpp_error("empty #if expression"); - return 0; /* don't include the #if group */ - } - - /* if there is some sort of scanning error, just return 0 and assume - the parsing routine has printed an error message somewhere. - there is surely a better thing to do than this. */ - if (setjmp (parse_return_error)) - return 0; - - if (yyparse ()) - return 0; /* actually this is never reached - the way things stand. */ - if (*lexptr) - acpp_error("Junk after end of expression."); - - return math_value + (double)expression_value; /* set by yyparse () */ -} - -#ifdef TEST_EXP_READER -/* main program, for testing purposes. */ -main () -{ - int n, c; - char buf[1024]; - extern int yydebug; -/* - yydebug = 1; -*/ - initialize_random_junk (); - - for (;;) { - printf ("enter expression: "); - n = 0; - while ((buf[n] = getchar ()) != '\n' && buf[n] != EOF) - n++; - if (buf[n] == EOF) - break; - buf[n] = '\0'; - printf ("parser returned %d\n", parse_c_expression (buf)); - } -} - -/* table to tell if char can be part of a C identifier. */ -unsigned char is_idchar[256]; -/* table to tell if char can be first char of a c identifier. */ -unsigned char is_idstart[256]; -#ifndef sgi -/* table to tell if c is horizontal space. isspace () thinks that - newline is space; this is not a good idea for this program. */ -char is_hor_space[256]; -#endif - -/* - * initialize random junk in the hash table and maybe other places - */ -initialize_random_junk () -{ - register int i; - - /* - * Set up is_idchar and is_idstart tables. These should be - * faster than saying (is_alpha (c) || c == '_'), etc. - * Must do set up these things before calling any routines tthat - * refer to them. - */ - for (i = 'a'; i <= 'z'; i++) { - ++is_idchar[i - 'a' + 'A']; - ++is_idchar[i]; - ++is_idstart[i - 'a' + 'A']; - ++is_idstart[i]; - } - for (i = '0'; i <= '9'; i++) - ++is_idchar[i]; - ++is_idchar['_']; - ++is_idstart['_']; -#ifdef DOLLARS_IN_IDENTIFIERS - ++is_idchar['$']; - ++is_idstart['$']; -#endif - - /* horizontal space table */ - ++is_hor_space[' ']; - ++is_hor_space['\t']; -} - -struct hashnode * -lookup (name, len, hash) - char *name; - int len; - int hash; -{ - return (DEFAULT_SIGNED_CHAR) ? 0 : ((struct hashnode *) -1); -} -#endif - - diff --git a/OpenSim/Utilities/simmToOpenSim/acpp/src/y_linux.tab.c b/OpenSim/Utilities/simmToOpenSim/acpp/src/y_linux.tab.c deleted file mode 100644 index 113b148d20..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/acpp/src/y_linux.tab.c +++ /dev/null @@ -1,2515 +0,0 @@ -/* A Bison parser, made by GNU Bison 2.3. */ - -/* Skeleton implementation for Bison's Yacc-like parsers in C - - Copyright (C) 1984, 1989, 1990, 2000, 2001, 2002, 2003, 2004, 2005, 2006 - Free Software Foundation, Inc. - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2, or (at your option) - any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, - Boston, MA 02110-1301, USA. */ - -/* As a special exception, you may create a larger work that contains - part or all of the Bison parser skeleton and distribute that work - under terms of your choice, so long as that work isn't itself a - parser generator using the skeleton or a modified version thereof - as a parser skeleton. Alternatively, if you modify or redistribute - the parser skeleton itself, you may (at your option) remove this - special exception, which will cause the skeleton and the resulting - Bison output files to be licensed under the GNU General Public - License without this special exception. - - This special exception was added by the Free Software Foundation in - version 2.2 of Bison. */ - -/* C LALR(1) parser skeleton written by Richard Stallman, by - simplifying the original so-called "semantic" parser. */ - -/* All symbols defined below should begin with yy or YY, to avoid - infringing on user name space. This should be done even for local - variables, as they might otherwise be expanded by user macros. - There are some unavoidable exceptions within include files to - define necessary library symbols; they are noted "INFRINGES ON - USER NAME SPACE" below. */ - -/* Identify Bison output. */ -#define YYBISON 1 - -/* Bison version. */ -#define YYBISON_VERSION "2.3" - -/* Skeleton name. */ -#define YYSKELETON_NAME "yacc.c" - -/* Pure parsers. */ -#define YYPURE 0 - -/* Using locations. */ -#define YYLSP_NEEDED 0 - - - -/* Tokens. */ -#ifndef YYTOKENTYPE -# define YYTOKENTYPE - /* Put the tokens into the symbol table, so that GDB and other debuggers - know about them. */ - enum yytokentype { - INT = 258, - CHAR = 259, - NAME = 260, - ERROR = 261, - DVALUE = 262, - OR = 263, - AND = 264, - NOTEQUAL = 265, - EQUAL = 266, - GEQ = 267, - LEQ = 268, - RSH = 269, - LSH = 270, - UNARY = 271 - }; -#endif -/* Tokens. */ -#define INT 258 -#define CHAR 259 -#define NAME 260 -#define ERROR 261 -#define DVALUE 262 -#define OR 263 -#define AND 264 -#define NOTEQUAL 265 -#define EQUAL 266 -#define GEQ 267 -#define LEQ 268 -#define RSH 269 -#define LSH 270 -#define UNARY 271 - - - - -/* Copy the first part of user declarations. */ -#line 26 "cexp.y" - -#include "stdio.h" -#include "stdlib.h" -#include "math.h" -#include "config.h" -#ifdef __linux__ -#include "strings.h" -#endif -#include -#include -/* #define YYDEBUG 1 */ - - int yylex (); - void yyerror (); - int inside_math = 0; - int expression_value; - double math_value; - - extern FILE* fp_out; - - static jmp_buf parse_return_error; - - /* some external tables of character types */ - extern unsigned char is_idstart[], is_idchar[]; -#if defined sgi || defined WIN32 || defined __linux__ || defined __APPLE__ -/* table to tell if c is horizontal space. isspace () thinks that - newline is space; this is not a good idea for this program. */ -char is_hor_space[256]; -#endif - -#if defined WIN32 || defined __linux__ || defined __APPLE__ -#define gettxt(S1,S2) (S1 ## S2) -#endif - -#ifndef CHAR_TYPE_SIZE -#define CHAR_TYPE_SIZE BITS_PER_UNIT -#endif - -acpp_error (msg) -{ -#ifdef INSIDE_SIMM - error(0, msg); // 0 = SIMM error action of "recover" -#else - fprintf (stderr, "error: %s\n", msg); -#endif -} - -acpp_warning (msg) -{ -#ifdef INSIDE_SIMM - error(0, msg); // 0 = SIMM error action of "recover" -#else - fprintf (stderr, "warning: %s\n", msg); -#endif -} - - -/* Enabling traces. */ -#ifndef YYDEBUG -# define YYDEBUG 0 -#endif - -/* Enabling verbose error messages. */ -#ifdef YYERROR_VERBOSE -# undef YYERROR_VERBOSE -# define YYERROR_VERBOSE 1 -#else -# define YYERROR_VERBOSE 0 -#endif - -/* Enabling the token table. */ -#ifndef YYTOKEN_TABLE -# define YYTOKEN_TABLE 0 -#endif - -#if ! defined YYSTYPE && ! defined YYSTYPE_IS_DECLARED -typedef union YYSTYPE -#line 83 "cexp.y" -{ - struct constant {long value; int unsignedp;} integer; - int voidval; - char *sval; - double dval; -} -/* Line 193 of yacc.c. */ -#line 192 "y_linux.tab.c" - YYSTYPE; -# define yystype YYSTYPE /* obsolescent; will be withdrawn */ -# define YYSTYPE_IS_DECLARED 1 -# define YYSTYPE_IS_TRIVIAL 1 -#endif - - - -/* Copy the second part of user declarations. */ - - -/* Line 216 of yacc.c. */ -#line 205 "y_linux.tab.c" - -#ifdef short -# undef short -#endif - -#ifdef YYTYPE_UINT8 -typedef YYTYPE_UINT8 yytype_uint8; -#else -typedef unsigned char yytype_uint8; -#endif - -#ifdef YYTYPE_INT8 -typedef YYTYPE_INT8 yytype_int8; -#elif (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -typedef signed char yytype_int8; -#else -typedef short int yytype_int8; -#endif - -#ifdef YYTYPE_UINT16 -typedef YYTYPE_UINT16 yytype_uint16; -#else -typedef unsigned short int yytype_uint16; -#endif - -#ifdef YYTYPE_INT16 -typedef YYTYPE_INT16 yytype_int16; -#else -typedef short int yytype_int16; -#endif - -#ifndef YYSIZE_T -# ifdef __SIZE_TYPE__ -# define YYSIZE_T __SIZE_TYPE__ -# elif defined size_t -# define YYSIZE_T size_t -# elif ! defined YYSIZE_T && (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -# include /* INFRINGES ON USER NAME SPACE */ -# define YYSIZE_T size_t -# else -# define YYSIZE_T unsigned int -# endif -#endif - -#define YYSIZE_MAXIMUM ((YYSIZE_T) -1) - -#ifndef YY_ -# if YYENABLE_NLS -# if ENABLE_NLS -# include /* INFRINGES ON USER NAME SPACE */ -# define YY_(msgid) dgettext ("bison-runtime", msgid) -# endif -# endif -# ifndef YY_ -# define YY_(msgid) msgid -# endif -#endif - -/* Suppress unused-variable warnings by "using" E. */ -#if ! defined lint || defined __GNUC__ -# define YYUSE(e) ((void) (e)) -#else -# define YYUSE(e) /* empty */ -#endif - -/* Identity function, used to suppress warnings about constant conditions. */ -#ifndef lint -# define YYID(n) (n) -#else -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static int -YYID (int i) -#else -static int -YYID (i) - int i; -#endif -{ - return i; -} -#endif - -#if ! defined yyoverflow || YYERROR_VERBOSE - -/* The parser invokes alloca or malloc; define the necessary symbols. */ - -# ifdef YYSTACK_USE_ALLOCA -# if YYSTACK_USE_ALLOCA -# ifdef __GNUC__ -# define YYSTACK_ALLOC __builtin_alloca -# elif defined __BUILTIN_VA_ARG_INCR -# include /* INFRINGES ON USER NAME SPACE */ -# elif defined _AIX -# define YYSTACK_ALLOC __alloca -# elif defined _MSC_VER -# include /* INFRINGES ON USER NAME SPACE */ -# define alloca _alloca -# else -# define YYSTACK_ALLOC alloca -# if ! defined _ALLOCA_H && ! defined _STDLIB_H && (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -# include /* INFRINGES ON USER NAME SPACE */ -# ifndef _STDLIB_H -# define _STDLIB_H 1 -# endif -# endif -# endif -# endif -# endif - -# ifdef YYSTACK_ALLOC - /* Pacify GCC's `empty if-body' warning. */ -# define YYSTACK_FREE(Ptr) do { /* empty */; } while (YYID (0)) -# ifndef YYSTACK_ALLOC_MAXIMUM - /* The OS might guarantee only one guard page at the bottom of the stack, - and a page size can be as small as 4096 bytes. So we cannot safely - invoke alloca (N) if N exceeds 4096. Use a slightly smaller number - to allow for a few compiler-allocated temporary stack slots. */ -# define YYSTACK_ALLOC_MAXIMUM 4032 /* reasonable circa 2006 */ -# endif -# else -# define YYSTACK_ALLOC YYMALLOC -# define YYSTACK_FREE YYFREE -# ifndef YYSTACK_ALLOC_MAXIMUM -# define YYSTACK_ALLOC_MAXIMUM YYSIZE_MAXIMUM -# endif -# if (defined __cplusplus && ! defined _STDLIB_H \ - && ! ((defined YYMALLOC || defined malloc) \ - && (defined YYFREE || defined free))) -# include /* INFRINGES ON USER NAME SPACE */ -# ifndef _STDLIB_H -# define _STDLIB_H 1 -# endif -# endif -# ifndef YYMALLOC -# define YYMALLOC malloc -# if ! defined malloc && ! defined _STDLIB_H && (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -void *malloc (YYSIZE_T); /* INFRINGES ON USER NAME SPACE */ -# endif -# endif -# ifndef YYFREE -# define YYFREE free -# if ! defined free && ! defined _STDLIB_H && (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -void free (void *); /* INFRINGES ON USER NAME SPACE */ -# endif -# endif -# endif -#endif /* ! defined yyoverflow || YYERROR_VERBOSE */ - - -#if (! defined yyoverflow \ - && (! defined __cplusplus \ - || (defined YYSTYPE_IS_TRIVIAL && YYSTYPE_IS_TRIVIAL))) - -/* A type that is properly aligned for any stack member. */ -union yyalloc -{ - yytype_int16 yyss; - YYSTYPE yyvs; - }; - -/* The size of the maximum gap between one aligned stack and the next. */ -# define YYSTACK_GAP_MAXIMUM (sizeof (union yyalloc) - 1) - -/* The size of an array large to enough to hold all stacks, each with - N elements. */ -# define YYSTACK_BYTES(N) \ - ((N) * (sizeof (yytype_int16) + sizeof (YYSTYPE)) \ - + YYSTACK_GAP_MAXIMUM) - -/* Copy COUNT objects from FROM to TO. The source and destination do - not overlap. */ -# ifndef YYCOPY -# if defined __GNUC__ && 1 < __GNUC__ -# define YYCOPY(To, From, Count) \ - __builtin_memcpy (To, From, (Count) * sizeof (*(From))) -# else -# define YYCOPY(To, From, Count) \ - do \ - { \ - YYSIZE_T yyi; \ - for (yyi = 0; yyi < (Count); yyi++) \ - (To)[yyi] = (From)[yyi]; \ - } \ - while (YYID (0)) -# endif -# endif - -/* Relocate STACK from its old location to the new one. The - local variables YYSIZE and YYSTACKSIZE give the old and new number of - elements in the stack, and YYPTR gives the new location of the - stack. Advance YYPTR to a properly aligned location for the next - stack. */ -# define YYSTACK_RELOCATE(Stack) \ - do \ - { \ - YYSIZE_T yynewbytes; \ - YYCOPY (&yyptr->Stack, Stack, yysize); \ - Stack = &yyptr->Stack; \ - yynewbytes = yystacksize * sizeof (*Stack) + YYSTACK_GAP_MAXIMUM; \ - yyptr += yynewbytes / sizeof (*yyptr); \ - } \ - while (YYID (0)) - -#endif - -/* YYFINAL -- State number of the termination state. */ -#define YYFINAL 25 -/* YYLAST -- Last index in YYTABLE. */ -#define YYLAST 226 - -/* YYNTOKENS -- Number of terminals. */ -#define YYNTOKENS 36 -/* YYNNTS -- Number of nonterminals. */ -#define YYNNTS 5 -/* YYNRULES -- Number of rules. */ -#define YYNRULES 39 -/* YYNRULES -- Number of states. */ -#define YYNSTATES 79 - -/* YYTRANSLATE(YYLEX) -- Bison symbol number corresponding to YYLEX. */ -#define YYUNDEFTOK 2 -#define YYMAXUTOK 271 - -#define YYTRANSLATE(YYX) \ - ((unsigned int) (YYX) <= YYMAXUTOK ? yytranslate[YYX] : YYUNDEFTOK) - -/* YYTRANSLATE[YYLEX] -- Bison symbol number corresponding to YYLEX. */ -static const yytype_uint8 yytranslate[] = -{ - 0, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 30, 2, 2, 2, 28, 15, 2, - 32, 33, 26, 24, 10, 25, 2, 27, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 9, 2, - 18, 2, 19, 8, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 14, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 34, 13, 35, 31, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, - 2, 2, 2, 2, 2, 2, 1, 2, 3, 4, - 5, 6, 7, 11, 12, 16, 17, 20, 21, 22, - 23, 29 -}; - -#if YYDEBUG -/* YYPRHS[YYN] -- Index of the first RHS symbol of rule number YYN in - YYRHS. */ -static const yytype_uint8 yyprhs[] = -{ - 0, 0, 3, 5, 7, 9, 13, 16, 19, 22, - 26, 29, 33, 37, 41, 45, 49, 53, 55, 59, - 63, 67, 71, 75, 79, 83, 87, 91, 95, 99, - 103, 107, 111, 115, 119, 123, 127, 133, 135, 137 -}; - -/* YYRHS -- A `-1'-separated list of the rules' RHS. */ -static const yytype_int8 yyrhs[] = -{ - 37, 0, -1, 38, -1, 40, -1, 39, -1, 38, - 10, 39, -1, 25, 39, -1, 30, 39, -1, 31, - 39, -1, 32, 38, 33, -1, 25, 40, -1, 32, - 40, 33, -1, 34, 40, 35, -1, 40, 26, 40, - -1, 40, 27, 40, -1, 40, 24, 40, -1, 40, - 25, 40, -1, 7, -1, 39, 26, 39, -1, 39, - 27, 39, -1, 39, 28, 39, -1, 39, 24, 39, - -1, 39, 25, 39, -1, 39, 23, 39, -1, 39, - 22, 39, -1, 39, 17, 39, -1, 39, 16, 39, - -1, 39, 21, 39, -1, 39, 20, 39, -1, 39, - 18, 39, -1, 39, 19, 39, -1, 39, 15, 39, - -1, 39, 14, 39, -1, 39, 13, 39, -1, 39, - 12, 39, -1, 39, 11, 39, -1, 39, 8, 39, - 9, 39, -1, 3, -1, 4, -1, 5, -1 -}; - -/* YYRLINE[YYN] -- source line where rule number YYN was defined. */ -static const yytype_uint16 yyrline[] = -{ - 0, 115, 115, 117, 122, 123, 128, 131, 134, 137, - 142, 144, 146, 151, 153, 160, 162, 164, 169, 175, - 186, 197, 200, 203, 209, 215, 218, 221, 227, 233, - 239, 245, 248, 251, 254, 257, 260, 263, 265, 267 -}; -#endif - -#if YYDEBUG || YYERROR_VERBOSE || YYTOKEN_TABLE -/* YYTNAME[SYMBOL-NUM] -- String name of the symbol SYMBOL-NUM. - First, the terminals, then, starting at YYNTOKENS, nonterminals. */ -static const char *const yytname[] = -{ - "$end", "error", "$undefined", "INT", "CHAR", "NAME", "ERROR", "DVALUE", - "'?'", "':'", "','", "OR", "AND", "'|'", "'^'", "'&'", "NOTEQUAL", - "EQUAL", "'<'", "'>'", "GEQ", "LEQ", "RSH", "LSH", "'+'", "'-'", "'*'", - "'/'", "'%'", "UNARY", "'!'", "'~'", "'('", "')'", "'{'", "'}'", - "$accept", "start", "exp1", "exp", "exp3", 0 -}; -#endif - -# ifdef YYPRINT -/* YYTOKNUM[YYLEX-NUM] -- Internal token number corresponding to - token YYLEX-NUM. */ -static const yytype_uint16 yytoknum[] = -{ - 0, 256, 257, 258, 259, 260, 261, 262, 63, 58, - 44, 263, 264, 124, 94, 38, 265, 266, 60, 62, - 267, 268, 269, 270, 43, 45, 42, 47, 37, 271, - 33, 126, 40, 41, 123, 125 -}; -# endif - -/* YYR1[YYN] -- Symbol number of symbol that rule YYN derives. */ -static const yytype_uint8 yyr1[] = -{ - 0, 36, 37, 37, 38, 38, 39, 39, 39, 39, - 40, 40, 40, 40, 40, 40, 40, 40, 39, 39, - 39, 39, 39, 39, 39, 39, 39, 39, 39, 39, - 39, 39, 39, 39, 39, 39, 39, 39, 39, 39 -}; - -/* YYR2[YYN] -- Number of symbols composing right hand side of rule YYN. */ -static const yytype_uint8 yyr2[] = -{ - 0, 2, 1, 1, 1, 3, 2, 2, 2, 3, - 2, 3, 3, 3, 3, 3, 3, 1, 3, 3, - 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, - 3, 3, 3, 3, 3, 3, 5, 1, 1, 1 -}; - -/* YYDEFACT[STATE-NAME] -- Default rule to reduce with in state - STATE-NUM when YYTABLE doesn't specify something else to do. Zero - means the default is an error. */ -static const yytype_uint8 yydefact[] = -{ - 0, 37, 38, 39, 17, 0, 0, 0, 0, 0, - 0, 2, 4, 3, 6, 10, 0, 0, 7, 8, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 9, 11, 12, 5, 0, 35, 34, 33, 32, 31, - 26, 25, 29, 30, 28, 27, 24, 23, 21, 22, - 18, 19, 20, 15, 16, 13, 14, 0, 36 -}; - -/* YYDEFGOTO[NTERM-NUM]. */ -static const yytype_int8 yydefgoto[] = -{ - -1, 10, 20, 12, 15 -}; - -/* YYPACT[STATE-NUM] -- Index in YYTABLE of the portion describing - STATE-NUM. */ -#define YYPACT_NINF -22 -static const yytype_int16 yypact[] = -{ - 48, -22, -22, -22, -22, 48, 66, 66, 48, 49, - 15, 7, 112, 68, -22, -22, 66, 66, -22, -22, - 9, -17, 49, 49, -21, -22, 66, 66, 66, 66, - 66, 66, 66, 66, 66, 66, 66, 66, 66, 66, - 66, 66, 66, 66, 66, 66, 49, 49, 49, 49, - -22, -22, -22, 112, 91, 129, 145, 160, 174, 187, - 198, 198, 35, 35, 35, 35, 19, 19, 39, 39, - -22, -22, -22, -14, -14, -22, -22, 66, 112 -}; - -/* YYPGOTO[NTERM-NUM]. */ -static const yytype_int8 yypgoto[] = -{ - -22, -22, 18, -5, 41 -}; - -/* YYTABLE[YYPACT[STATE-NUM]]. What to do in state STATE-NUM. If - positive, shift that token. If negative, reduce the rule which - number is the opposite. If zero, do what YYDEFACT says. - If YYTABLE_NINF, syntax error. */ -#define YYTABLE_NINF -1 -static const yytype_uint8 yytable[] = -{ - 14, 18, 19, 46, 47, 48, 49, 46, 47, 48, - 49, 14, 48, 49, 52, 25, 51, 26, 11, 26, - 0, 53, 54, 55, 56, 57, 58, 59, 60, 61, - 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, - 72, 13, 50, 41, 42, 43, 44, 45, 0, 21, - 24, 1, 2, 3, 0, 4, 4, 39, 40, 41, - 42, 43, 44, 45, 21, 43, 44, 45, 0, 1, - 2, 3, 78, 5, 22, 0, 0, 0, 6, 7, - 8, 23, 9, 9, 0, 0, 0, 73, 74, 75, - 76, 16, 46, 47, 48, 49, 6, 7, 17, 27, - 77, 0, 28, 29, 30, 31, 32, 33, 34, 35, - 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, - 27, 0, 0, 28, 29, 30, 31, 32, 33, 34, - 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, - 45, 29, 30, 31, 32, 33, 34, 35, 36, 37, - 38, 39, 40, 41, 42, 43, 44, 45, 30, 31, - 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, - 42, 43, 44, 45, 31, 32, 33, 34, 35, 36, - 37, 38, 39, 40, 41, 42, 43, 44, 45, 32, - 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, - 43, 44, 45, 33, 34, 35, 36, 37, 38, 39, - 40, 41, 42, 43, 44, 45, 35, 36, 37, 38, - 39, 40, 41, 42, 43, 44, 45 -}; - -static const yytype_int8 yycheck[] = -{ - 5, 6, 7, 24, 25, 26, 27, 24, 25, 26, - 27, 16, 26, 27, 35, 0, 33, 10, 0, 10, - -1, 26, 27, 28, 29, 30, 31, 32, 33, 34, - 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, - 45, 0, 33, 24, 25, 26, 27, 28, -1, 8, - 9, 3, 4, 5, -1, 7, 7, 22, 23, 24, - 25, 26, 27, 28, 23, 26, 27, 28, -1, 3, - 4, 5, 77, 25, 25, -1, -1, -1, 30, 31, - 32, 32, 34, 34, -1, -1, -1, 46, 47, 48, - 49, 25, 24, 25, 26, 27, 30, 31, 32, 8, - 9, -1, 11, 12, 13, 14, 15, 16, 17, 18, - 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, - 8, -1, -1, 11, 12, 13, 14, 15, 16, 17, - 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, - 28, 12, 13, 14, 15, 16, 17, 18, 19, 20, - 21, 22, 23, 24, 25, 26, 27, 28, 13, 14, - 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, - 25, 26, 27, 28, 14, 15, 16, 17, 18, 19, - 20, 21, 22, 23, 24, 25, 26, 27, 28, 15, - 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, - 26, 27, 28, 16, 17, 18, 19, 20, 21, 22, - 23, 24, 25, 26, 27, 28, 18, 19, 20, 21, - 22, 23, 24, 25, 26, 27, 28 -}; - -/* YYSTOS[STATE-NUM] -- The (internal number of the) accessing - symbol of state STATE-NUM. */ -static const yytype_uint8 yystos[] = -{ - 0, 3, 4, 5, 7, 25, 30, 31, 32, 34, - 37, 38, 39, 40, 39, 40, 25, 32, 39, 39, - 38, 40, 25, 32, 40, 0, 10, 8, 11, 12, - 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, - 23, 24, 25, 26, 27, 28, 24, 25, 26, 27, - 33, 33, 35, 39, 39, 39, 39, 39, 39, 39, - 39, 39, 39, 39, 39, 39, 39, 39, 39, 39, - 39, 39, 39, 40, 40, 40, 40, 9, 39 -}; - -#define yyerrok (yyerrstatus = 0) -#define yyclearin (yychar = YYEMPTY) -#define YYEMPTY (-2) -#define YYEOF 0 - -#define YYACCEPT goto yyacceptlab -#define YYABORT goto yyabortlab -#define YYERROR goto yyerrorlab - - -/* Like YYERROR except do call yyerror. This remains here temporarily - to ease the transition to the new meaning of YYERROR, for GCC. - Once GCC version 2 has supplanted version 1, this can go. */ - -#define YYFAIL goto yyerrlab - -#define YYRECOVERING() (!!yyerrstatus) - -#define YYBACKUP(Token, Value) \ -do \ - if (yychar == YYEMPTY && yylen == 1) \ - { \ - yychar = (Token); \ - yylval = (Value); \ - yytoken = YYTRANSLATE (yychar); \ - YYPOPSTACK (1); \ - goto yybackup; \ - } \ - else \ - { \ - yyerror (YY_("syntax error: cannot back up")); \ - YYERROR; \ - } \ -while (YYID (0)) - - -#define YYTERROR 1 -#define YYERRCODE 256 - - -/* YYLLOC_DEFAULT -- Set CURRENT to span from RHS[1] to RHS[N]. - If N is 0, then set CURRENT to the empty location which ends - the previous symbol: RHS[0] (always defined). */ - -#define YYRHSLOC(Rhs, K) ((Rhs)[K]) -#ifndef YYLLOC_DEFAULT -# define YYLLOC_DEFAULT(Current, Rhs, N) \ - do \ - if (YYID (N)) \ - { \ - (Current).first_line = YYRHSLOC (Rhs, 1).first_line; \ - (Current).first_column = YYRHSLOC (Rhs, 1).first_column; \ - (Current).last_line = YYRHSLOC (Rhs, N).last_line; \ - (Current).last_column = YYRHSLOC (Rhs, N).last_column; \ - } \ - else \ - { \ - (Current).first_line = (Current).last_line = \ - YYRHSLOC (Rhs, 0).last_line; \ - (Current).first_column = (Current).last_column = \ - YYRHSLOC (Rhs, 0).last_column; \ - } \ - while (YYID (0)) -#endif - - -/* YY_LOCATION_PRINT -- Print the location on the stream. - This macro was not mandated originally: define only if we know - we won't break user code: when these are the locations we know. */ - -#ifndef YY_LOCATION_PRINT -# if YYLTYPE_IS_TRIVIAL -# define YY_LOCATION_PRINT(File, Loc) \ - fprintf (File, "%d.%d-%d.%d", \ - (Loc).first_line, (Loc).first_column, \ - (Loc).last_line, (Loc).last_column) -# else -# define YY_LOCATION_PRINT(File, Loc) ((void) 0) -# endif -#endif - - -/* YYLEX -- calling `yylex' with the right arguments. */ - -#ifdef YYLEX_PARAM -# define YYLEX yylex (YYLEX_PARAM) -#else -# define YYLEX yylex () -#endif - -/* Enable debugging if requested. */ -#if YYDEBUG - -# ifndef YYFPRINTF -# include /* INFRINGES ON USER NAME SPACE */ -# define YYFPRINTF fprintf -# endif - -# define YYDPRINTF(Args) \ -do { \ - if (yydebug) \ - YYFPRINTF Args; \ -} while (YYID (0)) - -# define YY_SYMBOL_PRINT(Title, Type, Value, Location) \ -do { \ - if (yydebug) \ - { \ - YYFPRINTF (stderr, "%s ", Title); \ - yy_symbol_print (stderr, \ - Type, Value); \ - YYFPRINTF (stderr, "\n"); \ - } \ -} while (YYID (0)) - - -/*--------------------------------. -| Print this symbol on YYOUTPUT. | -`--------------------------------*/ - -/*ARGSUSED*/ -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static void -yy_symbol_value_print (FILE *yyoutput, int yytype, YYSTYPE const * const yyvaluep) -#else -static void -yy_symbol_value_print (yyoutput, yytype, yyvaluep) - FILE *yyoutput; - int yytype; - YYSTYPE const * const yyvaluep; -#endif -{ - if (!yyvaluep) - return; -# ifdef YYPRINT - if (yytype < YYNTOKENS) - YYPRINT (yyoutput, yytoknum[yytype], *yyvaluep); -# else - YYUSE (yyoutput); -# endif - switch (yytype) - { - default: - break; - } -} - - -/*--------------------------------. -| Print this symbol on YYOUTPUT. | -`--------------------------------*/ - -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static void -yy_symbol_print (FILE *yyoutput, int yytype, YYSTYPE const * const yyvaluep) -#else -static void -yy_symbol_print (yyoutput, yytype, yyvaluep) - FILE *yyoutput; - int yytype; - YYSTYPE const * const yyvaluep; -#endif -{ - if (yytype < YYNTOKENS) - YYFPRINTF (yyoutput, "token %s (", yytname[yytype]); - else - YYFPRINTF (yyoutput, "nterm %s (", yytname[yytype]); - - yy_symbol_value_print (yyoutput, yytype, yyvaluep); - YYFPRINTF (yyoutput, ")"); -} - -/*------------------------------------------------------------------. -| yy_stack_print -- Print the state stack from its BOTTOM up to its | -| TOP (included). | -`------------------------------------------------------------------*/ - -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static void -yy_stack_print (yytype_int16 *bottom, yytype_int16 *top) -#else -static void -yy_stack_print (bottom, top) - yytype_int16 *bottom; - yytype_int16 *top; -#endif -{ - YYFPRINTF (stderr, "Stack now"); - for (; bottom <= top; ++bottom) - YYFPRINTF (stderr, " %d", *bottom); - YYFPRINTF (stderr, "\n"); -} - -# define YY_STACK_PRINT(Bottom, Top) \ -do { \ - if (yydebug) \ - yy_stack_print ((Bottom), (Top)); \ -} while (YYID (0)) - - -/*------------------------------------------------. -| Report that the YYRULE is going to be reduced. | -`------------------------------------------------*/ - -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static void -yy_reduce_print (YYSTYPE *yyvsp, int yyrule) -#else -static void -yy_reduce_print (yyvsp, yyrule) - YYSTYPE *yyvsp; - int yyrule; -#endif -{ - int yynrhs = yyr2[yyrule]; - int yyi; - unsigned long int yylno = yyrline[yyrule]; - YYFPRINTF (stderr, "Reducing stack by rule %d (line %lu):\n", - yyrule - 1, yylno); - /* The symbols being reduced. */ - for (yyi = 0; yyi < yynrhs; yyi++) - { - fprintf (stderr, " $%d = ", yyi + 1); - yy_symbol_print (stderr, yyrhs[yyprhs[yyrule] + yyi], - &(yyvsp[(yyi + 1) - (yynrhs)]) - ); - fprintf (stderr, "\n"); - } -} - -# define YY_REDUCE_PRINT(Rule) \ -do { \ - if (yydebug) \ - yy_reduce_print (yyvsp, Rule); \ -} while (YYID (0)) - -/* Nonzero means print parse trace. It is left uninitialized so that - multiple parsers can coexist. */ -int yydebug; -#else /* !YYDEBUG */ -# define YYDPRINTF(Args) -# define YY_SYMBOL_PRINT(Title, Type, Value, Location) -# define YY_STACK_PRINT(Bottom, Top) -# define YY_REDUCE_PRINT(Rule) -#endif /* !YYDEBUG */ - - -/* YYINITDEPTH -- initial size of the parser's stacks. */ -#ifndef YYINITDEPTH -# define YYINITDEPTH 200 -#endif - -/* YYMAXDEPTH -- maximum size the stacks can grow to (effective only - if the built-in stack extension method is used). - - Do not make this value too large; the results are undefined if - YYSTACK_ALLOC_MAXIMUM < YYSTACK_BYTES (YYMAXDEPTH) - evaluated with infinite-precision integer arithmetic. */ - -#ifndef YYMAXDEPTH -# define YYMAXDEPTH 10000 -#endif - - - -#if YYERROR_VERBOSE - -# ifndef yystrlen -# if defined __GLIBC__ && defined _STRING_H -# define yystrlen strlen -# else -/* Return the length of YYSTR. */ -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static YYSIZE_T -yystrlen (const char *yystr) -#else -static YYSIZE_T -yystrlen (yystr) - const char *yystr; -#endif -{ - YYSIZE_T yylen; - for (yylen = 0; yystr[yylen]; yylen++) - continue; - return yylen; -} -# endif -# endif - -# ifndef yystpcpy -# if defined __GLIBC__ && defined _STRING_H && defined _GNU_SOURCE -# define yystpcpy stpcpy -# else -/* Copy YYSRC to YYDEST, returning the address of the terminating '\0' in - YYDEST. */ -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static char * -yystpcpy (char *yydest, const char *yysrc) -#else -static char * -yystpcpy (yydest, yysrc) - char *yydest; - const char *yysrc; -#endif -{ - char *yyd = yydest; - const char *yys = yysrc; - - while ((*yyd++ = *yys++) != '\0') - continue; - - return yyd - 1; -} -# endif -# endif - -# ifndef yytnamerr -/* Copy to YYRES the contents of YYSTR after stripping away unnecessary - quotes and backslashes, so that it's suitable for yyerror. The - heuristic is that double-quoting is unnecessary unless the string - contains an apostrophe, a comma, or backslash (other than - backslash-backslash). YYSTR is taken from yytname. If YYRES is - null, do not copy; instead, return the length of what the result - would have been. */ -static YYSIZE_T -yytnamerr (char *yyres, const char *yystr) -{ - if (*yystr == '"') - { - YYSIZE_T yyn = 0; - char const *yyp = yystr; - - for (;;) - switch (*++yyp) - { - case '\'': - case ',': - goto do_not_strip_quotes; - - case '\\': - if (*++yyp != '\\') - goto do_not_strip_quotes; - /* Fall through. */ - default: - if (yyres) - yyres[yyn] = *yyp; - yyn++; - break; - - case '"': - if (yyres) - yyres[yyn] = '\0'; - return yyn; - } - do_not_strip_quotes: ; - } - - if (! yyres) - return yystrlen (yystr); - - return yystpcpy (yyres, yystr) - yyres; -} -# endif - -/* Copy into YYRESULT an error message about the unexpected token - YYCHAR while in state YYSTATE. Return the number of bytes copied, - including the terminating null byte. If YYRESULT is null, do not - copy anything; just return the number of bytes that would be - copied. As a special case, return 0 if an ordinary "syntax error" - message will do. Return YYSIZE_MAXIMUM if overflow occurs during - size calculation. */ -static YYSIZE_T -yysyntax_error (char *yyresult, int yystate, int yychar) -{ - int yyn = yypact[yystate]; - - if (! (YYPACT_NINF < yyn && yyn <= YYLAST)) - return 0; - else - { - int yytype = YYTRANSLATE (yychar); - YYSIZE_T yysize0 = yytnamerr (0, yytname[yytype]); - YYSIZE_T yysize = yysize0; - YYSIZE_T yysize1; - int yysize_overflow = 0; - enum { YYERROR_VERBOSE_ARGS_MAXIMUM = 5 }; - char const *yyarg[YYERROR_VERBOSE_ARGS_MAXIMUM]; - int yyx; - -# if 0 - /* This is so xgettext sees the translatable formats that are - constructed on the fly. */ - YY_("syntax error, unexpected %s"); - YY_("syntax error, unexpected %s, expecting %s"); - YY_("syntax error, unexpected %s, expecting %s or %s"); - YY_("syntax error, unexpected %s, expecting %s or %s or %s"); - YY_("syntax error, unexpected %s, expecting %s or %s or %s or %s"); -# endif - char *yyfmt; - char const *yyf; - static char const yyunexpected[] = "syntax error, unexpected %s"; - static char const yyexpecting[] = ", expecting %s"; - static char const yyor[] = " or %s"; - char yyformat[sizeof yyunexpected - + sizeof yyexpecting - 1 - + ((YYERROR_VERBOSE_ARGS_MAXIMUM - 2) - * (sizeof yyor - 1))]; - char const *yyprefix = yyexpecting; - - /* Start YYX at -YYN if negative to avoid negative indexes in - YYCHECK. */ - int yyxbegin = yyn < 0 ? -yyn : 0; - - /* Stay within bounds of both yycheck and yytname. */ - int yychecklim = YYLAST - yyn + 1; - int yyxend = yychecklim < YYNTOKENS ? yychecklim : YYNTOKENS; - int yycount = 1; - - yyarg[0] = yytname[yytype]; - yyfmt = yystpcpy (yyformat, yyunexpected); - - for (yyx = yyxbegin; yyx < yyxend; ++yyx) - if (yycheck[yyx + yyn] == yyx && yyx != YYTERROR) - { - if (yycount == YYERROR_VERBOSE_ARGS_MAXIMUM) - { - yycount = 1; - yysize = yysize0; - yyformat[sizeof yyunexpected - 1] = '\0'; - break; - } - yyarg[yycount++] = yytname[yyx]; - yysize1 = yysize + yytnamerr (0, yytname[yyx]); - yysize_overflow |= (yysize1 < yysize); - yysize = yysize1; - yyfmt = yystpcpy (yyfmt, yyprefix); - yyprefix = yyor; - } - - yyf = YY_(yyformat); - yysize1 = yysize + yystrlen (yyf); - yysize_overflow |= (yysize1 < yysize); - yysize = yysize1; - - if (yysize_overflow) - return YYSIZE_MAXIMUM; - - if (yyresult) - { - /* Avoid sprintf, as that infringes on the user's name space. - Don't have undefined behavior even if the translation - produced a string with the wrong number of "%s"s. */ - char *yyp = yyresult; - int yyi = 0; - while ((*yyp = *yyf) != '\0') - { - if (*yyp == '%' && yyf[1] == 's' && yyi < yycount) - { - yyp += yytnamerr (yyp, yyarg[yyi++]); - yyf += 2; - } - else - { - yyp++; - yyf++; - } - } - } - return yysize; - } -} -#endif /* YYERROR_VERBOSE */ - - -/*-----------------------------------------------. -| Release the memory associated to this symbol. | -`-----------------------------------------------*/ - -/*ARGSUSED*/ -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -static void -yydestruct (const char *yymsg, int yytype, YYSTYPE *yyvaluep) -#else -static void -yydestruct (yymsg, yytype, yyvaluep) - const char *yymsg; - int yytype; - YYSTYPE *yyvaluep; -#endif -{ - YYUSE (yyvaluep); - - if (!yymsg) - yymsg = "Deleting"; - YY_SYMBOL_PRINT (yymsg, yytype, yyvaluep, yylocationp); - - switch (yytype) - { - - default: - break; - } -} - - -/* Prevent warnings from -Wmissing-prototypes. */ - -#ifdef YYPARSE_PARAM -#if defined __STDC__ || defined __cplusplus -int yyparse (void *YYPARSE_PARAM); -#else -int yyparse (); -#endif -#else /* ! YYPARSE_PARAM */ -#if defined __STDC__ || defined __cplusplus -int yyparse (void); -#else -int yyparse (); -#endif -#endif /* ! YYPARSE_PARAM */ - - - -/* The look-ahead symbol. */ -int yychar; - -/* The semantic value of the look-ahead symbol. */ -YYSTYPE yylval; - -/* Number of syntax errors so far. */ -int yynerrs; - - - -/*----------. -| yyparse. | -`----------*/ - -#ifdef YYPARSE_PARAM -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -int -yyparse (void *YYPARSE_PARAM) -#else -int -yyparse (YYPARSE_PARAM) - void *YYPARSE_PARAM; -#endif -#else /* ! YYPARSE_PARAM */ -#if (defined __STDC__ || defined __C99__FUNC__ \ - || defined __cplusplus || defined _MSC_VER) -int -yyparse (void) -#else -int -yyparse () - -#endif -#endif -{ - - int yystate; - int yyn; - int yyresult; - /* Number of tokens to shift before error messages enabled. */ - int yyerrstatus; - /* Look-ahead token as an internal (translated) token number. */ - int yytoken = 0; -#if YYERROR_VERBOSE - /* Buffer for error messages, and its allocated size. */ - char yymsgbuf[128]; - char *yymsg = yymsgbuf; - YYSIZE_T yymsg_alloc = sizeof yymsgbuf; -#endif - - /* Three stacks and their tools: - `yyss': related to states, - `yyvs': related to semantic values, - `yyls': related to locations. - - Refer to the stacks thru separate pointers, to allow yyoverflow - to reallocate them elsewhere. */ - - /* The state stack. */ - yytype_int16 yyssa[YYINITDEPTH]; - yytype_int16 *yyss = yyssa; - yytype_int16 *yyssp; - - /* The semantic value stack. */ - YYSTYPE yyvsa[YYINITDEPTH]; - YYSTYPE *yyvs = yyvsa; - YYSTYPE *yyvsp; - - - -#define YYPOPSTACK(N) (yyvsp -= (N), yyssp -= (N)) - - YYSIZE_T yystacksize = YYINITDEPTH; - - /* The variables used to return semantic value and location from the - action routines. */ - YYSTYPE yyval; - - - /* The number of symbols on the RHS of the reduced rule. - Keep to zero when no symbol should be popped. */ - int yylen = 0; - - YYDPRINTF ((stderr, "Starting parse\n")); - - yystate = 0; - yyerrstatus = 0; - yynerrs = 0; - yychar = YYEMPTY; /* Cause a token to be read. */ - - /* Initialize stack pointers. - Waste one element of value and location stack - so that they stay on the same level as the state stack. - The wasted elements are never initialized. */ - - yyssp = yyss; - yyvsp = yyvs; - - goto yysetstate; - -/*------------------------------------------------------------. -| yynewstate -- Push a new state, which is found in yystate. | -`------------------------------------------------------------*/ - yynewstate: - /* In all cases, when you get here, the value and location stacks - have just been pushed. So pushing a state here evens the stacks. */ - yyssp++; - - yysetstate: - *yyssp = yystate; - - if (yyss + yystacksize - 1 <= yyssp) - { - /* Get the current used size of the three stacks, in elements. */ - YYSIZE_T yysize = yyssp - yyss + 1; - -#ifdef yyoverflow - { - /* Give user a chance to reallocate the stack. Use copies of - these so that the &'s don't force the real ones into - memory. */ - YYSTYPE *yyvs1 = yyvs; - yytype_int16 *yyss1 = yyss; - - - /* Each stack pointer address is followed by the size of the - data in use in that stack, in bytes. This used to be a - conditional around just the two extra args, but that might - be undefined if yyoverflow is a macro. */ - yyoverflow (YY_("memory exhausted"), - &yyss1, yysize * sizeof (*yyssp), - &yyvs1, yysize * sizeof (*yyvsp), - - &yystacksize); - - yyss = yyss1; - yyvs = yyvs1; - } -#else /* no yyoverflow */ -# ifndef YYSTACK_RELOCATE - goto yyexhaustedlab; -# else - /* Extend the stack our own way. */ - if (YYMAXDEPTH <= yystacksize) - goto yyexhaustedlab; - yystacksize *= 2; - if (YYMAXDEPTH < yystacksize) - yystacksize = YYMAXDEPTH; - - { - yytype_int16 *yyss1 = yyss; - union yyalloc *yyptr = - (union yyalloc *) YYSTACK_ALLOC (YYSTACK_BYTES (yystacksize)); - if (! yyptr) - goto yyexhaustedlab; - YYSTACK_RELOCATE (yyss); - YYSTACK_RELOCATE (yyvs); - -# undef YYSTACK_RELOCATE - if (yyss1 != yyssa) - YYSTACK_FREE (yyss1); - } -# endif -#endif /* no yyoverflow */ - - yyssp = yyss + yysize - 1; - yyvsp = yyvs + yysize - 1; - - - YYDPRINTF ((stderr, "Stack size increased to %lu\n", - (unsigned long int) yystacksize)); - - if (yyss + yystacksize - 1 <= yyssp) - YYABORT; - } - - YYDPRINTF ((stderr, "Entering state %d\n", yystate)); - - goto yybackup; - -/*-----------. -| yybackup. | -`-----------*/ -yybackup: - - /* Do appropriate processing given the current state. Read a - look-ahead token if we need one and don't already have one. */ - - /* First try to decide what to do without reference to look-ahead token. */ - yyn = yypact[yystate]; - if (yyn == YYPACT_NINF) - goto yydefault; - - /* Not known => get a look-ahead token if don't already have one. */ - - /* YYCHAR is either YYEMPTY or YYEOF or a valid look-ahead symbol. */ - if (yychar == YYEMPTY) - { - YYDPRINTF ((stderr, "Reading a token: ")); - yychar = YYLEX; - } - - if (yychar <= YYEOF) - { - yychar = yytoken = YYEOF; - YYDPRINTF ((stderr, "Now at end of input.\n")); - } - else - { - yytoken = YYTRANSLATE (yychar); - YY_SYMBOL_PRINT ("Next token is", yytoken, &yylval, &yylloc); - } - - /* If the proper action on seeing token YYTOKEN is to reduce or to - detect an error, take that action. */ - yyn += yytoken; - if (yyn < 0 || YYLAST < yyn || yycheck[yyn] != yytoken) - goto yydefault; - yyn = yytable[yyn]; - if (yyn <= 0) - { - if (yyn == 0 || yyn == YYTABLE_NINF) - goto yyerrlab; - yyn = -yyn; - goto yyreduce; - } - - if (yyn == YYFINAL) - YYACCEPT; - - /* Count tokens shifted since error; after three, turn off error - status. */ - if (yyerrstatus) - yyerrstatus--; - - /* Shift the look-ahead token. */ - YY_SYMBOL_PRINT ("Shifting", yytoken, &yylval, &yylloc); - - /* Discard the shifted token unless it is eof. */ - if (yychar != YYEOF) - yychar = YYEMPTY; - - yystate = yyn; - *++yyvsp = yylval; - - goto yynewstate; - - -/*-----------------------------------------------------------. -| yydefault -- do the default action for the current state. | -`-----------------------------------------------------------*/ -yydefault: - yyn = yydefact[yystate]; - if (yyn == 0) - goto yyerrlab; - goto yyreduce; - - -/*-----------------------------. -| yyreduce -- Do a reduction. | -`-----------------------------*/ -yyreduce: - /* yyn is the number of a rule to reduce with. */ - yylen = yyr2[yyn]; - - /* If YYLEN is nonzero, implement the default value of the action: - `$$ = $1'. - - Otherwise, the following line sets YYVAL to garbage. - This behavior is undocumented and Bison - users should not rely upon it. Assigning to YYVAL - unconditionally makes the parser a bit smaller, and it avoids a - GCC warning that YYVAL may be used uninitialized. */ - yyval = yyvsp[1-yylen]; - - - YY_REDUCE_PRINT (yyn); - switch (yyn) - { - case 2: -#line 116 "cexp.y" - { expression_value = (yyvsp[(1) - (1)].integer).value; math_value = 0.0; } - break; - - case 3: -#line 118 "cexp.y" - { math_value = (yyvsp[(1) - (1)].dval); expression_value = 0; } - break; - - case 5: -#line 124 "cexp.y" - { (yyval.integer) = (yyvsp[(3) - (3)].integer); } - break; - - case 6: -#line 129 "cexp.y" - { (yyval.integer).value = - (yyvsp[(2) - (2)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(2) - (2)].integer).unsignedp; } - break; - - case 7: -#line 132 "cexp.y" - { (yyval.integer).value = ! (yyvsp[(2) - (2)].integer).value; - (yyval.integer).unsignedp = 0; } - break; - - case 8: -#line 135 "cexp.y" - { (yyval.integer).value = ~ (yyvsp[(2) - (2)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(2) - (2)].integer).unsignedp; } - break; - - case 9: -#line 138 "cexp.y" - { (yyval.integer) = (yyvsp[(2) - (3)].integer); } - break; - - case 10: -#line 143 "cexp.y" - { (yyval.dval) = - (yyvsp[(2) - (2)].dval); } - break; - - case 11: -#line 145 "cexp.y" - { (yyval.dval) = (yyvsp[(2) - (3)].dval); } - break; - - case 12: -#line 147 "cexp.y" - { (yyval.dval) = (yyvsp[(2) - (3)].dval); } - break; - - case 13: -#line 152 "cexp.y" - { (yyval.dval) = (yyvsp[(1) - (3)].dval) * (yyvsp[(3) - (3)].dval); } - break; - - case 14: -#line 154 "cexp.y" - { if ((yyvsp[(3) - (3)].dval) == 0.0) - { - acpp_error ("division by zero in expression"); - (yyvsp[(3) - (3)].dval) = 1.0; - } - (yyval.dval) = (yyvsp[(1) - (3)].dval) / (yyvsp[(3) - (3)].dval); } - break; - - case 15: -#line 161 "cexp.y" - { (yyval.dval) = (yyvsp[(1) - (3)].dval) + (yyvsp[(3) - (3)].dval); } - break; - - case 16: -#line 163 "cexp.y" - { (yyval.dval) = (yyvsp[(1) - (3)].dval) - (yyvsp[(3) - (3)].dval); } - break; - - case 17: -#line 165 "cexp.y" - { (yyval.dval) = yylval.dval; } - break; - - case 18: -#line 170 "cexp.y" - { (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; - if ((yyval.integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value * (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value * (yyvsp[(3) - (3)].integer).value; } - break; - - case 19: -#line 176 "cexp.y" - { if ((yyvsp[(3) - (3)].integer).value == 0) - { - acpp_error ("division by zero in #if"); - (yyvsp[(3) - (3)].integer).value = 1; - } - (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; - if ((yyval.integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value / (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value / (yyvsp[(3) - (3)].integer).value; } - break; - - case 20: -#line 187 "cexp.y" - { if ((yyvsp[(3) - (3)].integer).value == 0) - { - acpp_error ("division by zero in #if"); - (yyvsp[(3) - (3)].integer).value = 1; - } - (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; - if ((yyval.integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value % (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value % (yyvsp[(3) - (3)].integer).value; } - break; - - case 21: -#line 198 "cexp.y" - { (yyval.integer).value = (yyvsp[(1) - (3)].integer).value + (yyvsp[(3) - (3)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; } - break; - - case 22: -#line 201 "cexp.y" - { (yyval.integer).value = (yyvsp[(1) - (3)].integer).value - (yyvsp[(3) - (3)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; } - break; - - case 23: -#line 204 "cexp.y" - { (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp; - if ((yyval.integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value << (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value << (yyvsp[(3) - (3)].integer).value; } - break; - - case 24: -#line 210 "cexp.y" - { (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp; - if ((yyval.integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value >> (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value >> (yyvsp[(3) - (3)].integer).value; } - break; - - case 25: -#line 216 "cexp.y" - { (yyval.integer).value = ((yyvsp[(1) - (3)].integer).value == (yyvsp[(3) - (3)].integer).value); - (yyval.integer).unsignedp = 0; } - break; - - case 26: -#line 219 "cexp.y" - { (yyval.integer).value = ((yyvsp[(1) - (3)].integer).value != (yyvsp[(3) - (3)].integer).value); - (yyval.integer).unsignedp = 0; } - break; - - case 27: -#line 222 "cexp.y" - { (yyval.integer).unsignedp = 0; - if ((yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value <= (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value <= (yyvsp[(3) - (3)].integer).value; } - break; - - case 28: -#line 228 "cexp.y" - { (yyval.integer).unsignedp = 0; - if ((yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value >= (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value >= (yyvsp[(3) - (3)].integer).value; } - break; - - case 29: -#line 234 "cexp.y" - { (yyval.integer).unsignedp = 0; - if ((yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value < (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value < (yyvsp[(3) - (3)].integer).value; } - break; - - case 30: -#line 240 "cexp.y" - { (yyval.integer).unsignedp = 0; - if ((yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp) - (yyval.integer).value = (unsigned) (yyvsp[(1) - (3)].integer).value > (yyvsp[(3) - (3)].integer).value; - else - (yyval.integer).value = (yyvsp[(1) - (3)].integer).value > (yyvsp[(3) - (3)].integer).value; } - break; - - case 31: -#line 246 "cexp.y" - { (yyval.integer).value = (yyvsp[(1) - (3)].integer).value & (yyvsp[(3) - (3)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; } - break; - - case 32: -#line 249 "cexp.y" - { (yyval.integer).value = (yyvsp[(1) - (3)].integer).value ^ (yyvsp[(3) - (3)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; } - break; - - case 33: -#line 252 "cexp.y" - { (yyval.integer).value = (yyvsp[(1) - (3)].integer).value | (yyvsp[(3) - (3)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(1) - (3)].integer).unsignedp || (yyvsp[(3) - (3)].integer).unsignedp; } - break; - - case 34: -#line 255 "cexp.y" - { (yyval.integer).value = ((yyvsp[(1) - (3)].integer).value && (yyvsp[(3) - (3)].integer).value); - (yyval.integer).unsignedp = 0; } - break; - - case 35: -#line 258 "cexp.y" - { (yyval.integer).value = ((yyvsp[(1) - (3)].integer).value || (yyvsp[(3) - (3)].integer).value); - (yyval.integer).unsignedp = 0; } - break; - - case 36: -#line 261 "cexp.y" - { (yyval.integer).value = (yyvsp[(1) - (5)].integer).value ? (yyvsp[(3) - (5)].integer).value : (yyvsp[(5) - (5)].integer).value; - (yyval.integer).unsignedp = (yyvsp[(3) - (5)].integer).unsignedp || (yyvsp[(5) - (5)].integer).unsignedp; } - break; - - case 37: -#line 264 "cexp.y" - { (yyval.integer) = yylval.integer; } - break; - - case 38: -#line 266 "cexp.y" - { (yyval.integer) = yylval.integer; } - break; - - case 39: -#line 268 "cexp.y" - { (yyval.integer).value = 0; - (yyval.integer).unsignedp = 0; } - break; - - -/* Line 1267 of yacc.c. */ -#line 1732 "y_linux.tab.c" - default: break; - } - YY_SYMBOL_PRINT ("-> $$ =", yyr1[yyn], &yyval, &yyloc); - - YYPOPSTACK (yylen); - yylen = 0; - YY_STACK_PRINT (yyss, yyssp); - - *++yyvsp = yyval; - - - /* Now `shift' the result of the reduction. Determine what state - that goes to, based on the state we popped back to and the rule - number reduced by. */ - - yyn = yyr1[yyn]; - - yystate = yypgoto[yyn - YYNTOKENS] + *yyssp; - if (0 <= yystate && yystate <= YYLAST && yycheck[yystate] == *yyssp) - yystate = yytable[yystate]; - else - yystate = yydefgoto[yyn - YYNTOKENS]; - - goto yynewstate; - - -/*------------------------------------. -| yyerrlab -- here on detecting error | -`------------------------------------*/ -yyerrlab: - /* If not already recovering from an error, report this error. */ - if (!yyerrstatus) - { - ++yynerrs; -#if ! YYERROR_VERBOSE - yyerror (YY_("syntax error")); -#else - { - YYSIZE_T yysize = yysyntax_error (0, yystate, yychar); - if (yymsg_alloc < yysize && yymsg_alloc < YYSTACK_ALLOC_MAXIMUM) - { - YYSIZE_T yyalloc = 2 * yysize; - if (! (yysize <= yyalloc && yyalloc <= YYSTACK_ALLOC_MAXIMUM)) - yyalloc = YYSTACK_ALLOC_MAXIMUM; - if (yymsg != yymsgbuf) - YYSTACK_FREE (yymsg); - yymsg = (char *) YYSTACK_ALLOC (yyalloc); - if (yymsg) - yymsg_alloc = yyalloc; - else - { - yymsg = yymsgbuf; - yymsg_alloc = sizeof yymsgbuf; - } - } - - if (0 < yysize && yysize <= yymsg_alloc) - { - (void) yysyntax_error (yymsg, yystate, yychar); - yyerror (yymsg); - } - else - { - yyerror (YY_("syntax error")); - if (yysize != 0) - goto yyexhaustedlab; - } - } -#endif - } - - - - if (yyerrstatus == 3) - { - /* If just tried and failed to reuse look-ahead token after an - error, discard it. */ - - if (yychar <= YYEOF) - { - /* Return failure if at end of input. */ - if (yychar == YYEOF) - YYABORT; - } - else - { - yydestruct ("Error: discarding", - yytoken, &yylval); - yychar = YYEMPTY; - } - } - - /* Else will try to reuse look-ahead token after shifting the error - token. */ - goto yyerrlab1; - - -/*---------------------------------------------------. -| yyerrorlab -- error raised explicitly by YYERROR. | -`---------------------------------------------------*/ -yyerrorlab: - - /* Pacify compilers like GCC when the user code never invokes - YYERROR and the label yyerrorlab therefore never appears in user - code. */ - if (/*CONSTCOND*/ 0) - goto yyerrorlab; - - /* Do not reclaim the symbols of the rule which action triggered - this YYERROR. */ - YYPOPSTACK (yylen); - yylen = 0; - YY_STACK_PRINT (yyss, yyssp); - yystate = *yyssp; - goto yyerrlab1; - - -/*-------------------------------------------------------------. -| yyerrlab1 -- common code for both syntax error and YYERROR. | -`-------------------------------------------------------------*/ -yyerrlab1: - yyerrstatus = 3; /* Each real token shifted decrements this. */ - - for (;;) - { - yyn = yypact[yystate]; - if (yyn != YYPACT_NINF) - { - yyn += YYTERROR; - if (0 <= yyn && yyn <= YYLAST && yycheck[yyn] == YYTERROR) - { - yyn = yytable[yyn]; - if (0 < yyn) - break; - } - } - - /* Pop the current state because it cannot handle the error token. */ - if (yyssp == yyss) - YYABORT; - - - yydestruct ("Error: popping", - yystos[yystate], yyvsp); - YYPOPSTACK (1); - yystate = *yyssp; - YY_STACK_PRINT (yyss, yyssp); - } - - if (yyn == YYFINAL) - YYACCEPT; - - *++yyvsp = yylval; - - - /* Shift the error token. */ - YY_SYMBOL_PRINT ("Shifting", yystos[yyn], yyvsp, yylsp); - - yystate = yyn; - goto yynewstate; - - -/*-------------------------------------. -| yyacceptlab -- YYACCEPT comes here. | -`-------------------------------------*/ -yyacceptlab: - yyresult = 0; - goto yyreturn; - -/*-----------------------------------. -| yyabortlab -- YYABORT comes here. | -`-----------------------------------*/ -yyabortlab: - yyresult = 1; - goto yyreturn; - -#ifndef yyoverflow -/*-------------------------------------------------. -| yyexhaustedlab -- memory exhaustion comes here. | -`-------------------------------------------------*/ -yyexhaustedlab: - yyerror (YY_("memory exhausted")); - yyresult = 2; - /* Fall through. */ -#endif - -yyreturn: - if (yychar != YYEOF && yychar != YYEMPTY) - yydestruct ("Cleanup: discarding lookahead", - yytoken, &yylval); - /* Do not reclaim the symbols of the rule which action triggered - this YYABORT or YYACCEPT. */ - YYPOPSTACK (yylen); - YY_STACK_PRINT (yyss, yyssp); - while (yyssp != yyss) - { - yydestruct ("Cleanup: popping", - yystos[*yyssp], yyvsp); - YYPOPSTACK (1); - } -#ifndef yyoverflow - if (yyss != yyssa) - YYSTACK_FREE (yyss); -#endif -#if YYERROR_VERBOSE - if (yymsg != yymsgbuf) - YYSTACK_FREE (yymsg); -#endif - /* Make sure YYID is used. */ - return YYID (yyresult); -} - - -#line 271 "cexp.y" - - -/* During parsing of a C expression, the pointer to the next character - is in this variable. */ - -static char *lexptr; - -/* Take care of parsing a number (anything that starts with a digit). - Set yylval and return the token type; update lexptr. - LEN is the number of characters in it. */ - -int -parse_number (olen) - int olen; -{ - register char *p = lexptr; - register long n = 0; - register int c; - register int base = 10; - register int len = olen; - - /* Check to see if this is a floating point number or not */ - for (c = 0; c < len; c++) - { - if (p[c] == '.') - { - if (inside_math == 0) - { - yyerror ("floating point numbers not allowed in #if expressions"); - return ERROR; - } - else - { - char *np = (char*)alloca(len + 1); - bcopy(p, np, len); - np[len] = '\0'; - yylval.dval = atof(np); - lexptr += len; - return DVALUE; - } - } - } - - yylval.integer.unsignedp = 0; - - if (len >= 3 && (!strncmp (p, "0x", 2) || !strncmp (p, "0X", 2))) { - p += 2; - base = 16; - len -= 2; - } - else if (*p == '0') - base = 8; - - while (len > 0) { - c = *p++; - len--; - if (c >= 'A' && c <= 'Z') c += 'a' - 'A'; - - if (c >= '0' && c <= '9') { - n *= base; - n += c - '0'; - } else if (base == 16 && c >= 'a' && c <= 'f') { - n *= base; - n += c - 'a' + 10; - } else { - /* `l' means long, and `u' means unsigned. */ - while (1) { - if (c == 'l' || c == 'L') - ; - else if (c == 'u' || c == 'U') - yylval.integer.unsignedp = 1; - else - break; - - if (len == 0) - break; - c = *p++; - len--; - } - /* Don't look for any more digits after the suffixes. */ - break; - } - } - - if (len != 0) { - yyerror ("Invalid number in #if expression"); - return ERROR; - } - - lexptr = p; - - if (inside_math > 0) - { - yylval.dval = (double)n; - return DVALUE; - } - else - { - /* If too big to be signed, consider it unsigned. */ - if (n < 0) - yylval.integer.unsignedp = 1; - - yylval.integer.value = n; - return INT; - } - - return INT; -} - -struct token { - char *operator; - int token; -}; - -#undef NULL -#define NULL 0 - -static struct token tokentab2[] = { - {"&&", AND}, - {"||", OR}, - {"<<", LSH}, - {">>", RSH}, - {"==", EQUAL}, - {"!=", NOTEQUAL}, - {"<=", LEQ}, - {">=", GEQ}, - {NULL, ERROR} -}; - -/* Read one token, getting characters through lexptr. */ - -int -yylex () -{ - register int c; - register int namelen; - register char *tokstart; - register struct token *toktab; - - retry: - - tokstart = lexptr; - c = *tokstart; - /* See if it is a special token of length 2. */ - for (toktab = tokentab2; toktab->operator != NULL; toktab++) - if (c == *toktab->operator && tokstart[1] == toktab->operator[1]) { - lexptr += 2; - return toktab->token; - } - - switch (c) { - case 0: - return 0; - - case ' ': - case '\t': - case '\n': - lexptr++; - goto retry; - -#ifdef sgi - case 'L': - { - /* look for wide char constants */ - int c2; - int i; - for(i = 1, c2 = *(lexptr+i); is_hor_space[c2]; - ++i,c2 = *(lexptr+i)) - /* do nothing on horiz space stuff */ { } - if(c2 != '\'') { - /* Not a wide char const. An identifier */ - break; - } - lexptr += i ; /* points to the quote now - Is wide char constant */ - tokstart = lexptr; - c = c2; /* c now a quote */ - } -#endif - case '\'': - lexptr++; - c = *lexptr++; - if (c == '\\') - c = parse_escape (&lexptr); - - /* Sign-extend the constant if chars are signed on target machine. */ - { - if (lookup ("__CHAR_UNSIGNED__", sizeof ("__CHAR_UNSIGNED__")-1, -1) - || ((c >> (CHAR_TYPE_SIZE - 1)) & 1) == 0) - yylval.integer.value = c & ((1 << CHAR_TYPE_SIZE) - 1); - else - yylval.integer.value = c | ~((1 << CHAR_TYPE_SIZE) - 1); - } - - yylval.integer.unsignedp = 0; - c = *lexptr++; - if (c != '\'') { - yyerror ("Invalid character constant in #if"); - return ERROR; - } - - return CHAR; - - /* some of these chars are invalid in constant expressions; - maybe do something about them later */ - case '/': - case '+': - case '-': - case '*': - case '%': - case '|': - case '&': - case '^': - case '~': - case '!': - case '@': - case '<': - case '>': - case '(': - case ')': - case '[': - case ']': - case '.': - case '?': - case ':': - case '=': - case '{': - case '}': - case ',': - lexptr++; - return c; - - case '"': - yyerror ("double quoted strings not allowed in #if expressions"); - return ERROR; - } - - /* If you are not inside a math expression, then numbers have - * to be integers. Thus they should contain only numerals from 0 to 9. - * However, a decimal point is allowed because it is checked for - * later (inside parse_number). - * If you are inside a math expression, then the number can start with - * a numeral or decimal point, and can contain a trailing exponent. - */ - if ((c >= '0' && c <= '9') || c == '.') - { - if (inside_math == 0) - { - for (namelen = 0; - c = tokstart[namelen], is_idchar[c] || c == '.'; - namelen++) - ; - } - else - { - int exp = 0; - for (namelen = 0; - c = tokstart[namelen], is_idchar[c] || c == '.' || (exp == 1 && (c == '-' || c == '+')); - namelen++) - { - if (exp == 0 && (c == 'e' || c == 'E' || c == 'g' || c == 'G')) - exp = 1; - else if (exp == 1 && (c == '-' || c == '+')) - exp = 0; - } - } - - return parse_number (namelen); - } - - if (!is_idstart[c]) { - yyerror ("Invalid token in expression"); - return ERROR; - } - - /* It is a name. See how long it is. */ - - for (namelen = 0; is_idchar[tokstart[namelen]]; namelen++) - ; - - lexptr += namelen; - return NAME; -} - - -/* Parse a C escape sequence. STRING_PTR points to a variable - containing a pointer to the string to parse. That pointer - is updated past the characters we use. The value of the - escape sequence is returned. - - A negative value means the sequence \ newline was seen, - which is supposed to be equivalent to nothing at all. - - If \ is followed by a null character, we return a negative - value and leave the string pointer pointing at the null character. - - If \ is followed by 000, we return 0 and leave the string pointer - after the zeros. A value of 0 does not mean end of string. */ - -int -parse_escape (string_ptr) - char **string_ptr; -{ - register int c = *(*string_ptr)++; - switch (c) - { - case 'a': - return TARGET_BELL; - case 'b': - return TARGET_BS; - case 'e': - return 033; - case 'f': - return TARGET_FF; - case 'n': - return TARGET_NEWLINE; - case 'r': - return TARGET_CR; - case 't': - return TARGET_TAB; - case 'v': - return TARGET_VT; - case '\n': - return -2; - case 0: - (*string_ptr)--; - return 0; - case '^': - c = *(*string_ptr)++; - if (c == '\\') - c = parse_escape (string_ptr); - if (c == '?') - return 0177; - return (c & 0200) | (c & 037); - - case '0': - case '1': - case '2': - case '3': - case '4': - case '5': - case '6': - case '7': - { - register int i = c - '0'; - register int count = 0; - while (++count < 3) - { - c = *(*string_ptr)++; - if (c >= '0' && c <= '7') - i = (i << 3) + c - '0'; - else - { - (*string_ptr)--; - break; - } - } - if ((i & ~((1 << CHAR_TYPE_SIZE) - 1)) != 0) - { - i &= (1 << CHAR_TYPE_SIZE) - 1; - acpp_warning ("octal character constant does not fit in a byte"); - } - return i; - } - case 'x': - { - register int i = 0; - register int count = 0; - for (;;) - { - c = *(*string_ptr)++; - if (c >= '0' && c <= '9') - i = (i << 4) + c - '0'; - else if (c >= 'a' && c <= 'f') - i = (i << 4) + c - 'a' + 10; - else if (c >= 'A' && c <= 'F') - i = (i << 4) + c - 'A' + 10; - else - { - (*string_ptr)--; - break; - } - } - if ((i & ~((1 << BITS_PER_UNIT) - 1)) != 0) - { - i &= (1 << BITS_PER_UNIT) - 1; - acpp_warning ("hex character constant does not fit in a byte"); - } - return i; - } - default: - return c; - } -} - -void -yyerror (s) - char *s; -{ - acpp_error (s); - longjmp (parse_return_error, 1); -} - -/* This page contains the entry point to this file. */ - -#if 0 -/* THIS DOESN'T EVEN BEGIN TO WORK */ -/* Parse STRING as an assertion (the AT&T ANSI cpp extension), and complain if - * this fails to use up all of the contents of STRING */ -int -parse_assertion_extension (buf) - char *buf; -{ - int token; - int c; - int negate = 0; - int value = 0; - - for(c = *buf; c != '#'; c = *buf++) { - if (c == '!') - negate = !negate; - else if(!isspace(c)) - acpp_error("Unexpected character in assertion expression\n"); - } - - /* buf is now one past the '#' character */ - lexptr = buf; - - if((token = yylex()) != NAME) - acpp_error("Syntax for assertion conditionals is '#if #assertion-name(tokens)'\n"); - - /* for the moment - check the syntax, but don't actually *do* anything. */ - return (negate ? !value : value); -} -#endif - -/* Parse STRING as an expression, and complain if this fails - to use up all of the contents of STRING. */ -/* We do not support C comments. They should be removed before - this function is called. */ - -int -parse_c_expression (string) - char *string; -{ - lexptr = string; - - if (lexptr == 0 || *lexptr == 0) { - acpp_error ("empty #if expression"); - return 0; /* don't include the #if group */ - } - - /* if there is some sort of scanning error, just return 0 and assume - the parsing routine has printed an error message somewhere. - there is surely a better thing to do than this. */ - if (setjmp (parse_return_error)) - return 0; - - if (yyparse ()) - return 0; /* actually this is never reached - the way things stand. */ - if (*lexptr) - acpp_error ("Junk after end of expression."); - - return expression_value; /* set by yyparse () */ -} - -double -parse_c_math_expression (string) - char *string; -{ - lexptr = string; - - if (lexptr == 0 || *lexptr == 0) { - acpp_error ("empty #if expression"); - return 0; /* don't include the #if group */ - } - - /* if there is some sort of scanning error, just return 0 and assume - the parsing routine has printed an error message somewhere. - there is surely a better thing to do than this. */ - if (setjmp (parse_return_error)) - return 0; - - if (yyparse ()) - return 0; /* actually this is never reached - the way things stand. */ - if (*lexptr) - acpp_error ("Junk after end of expression."); - - return math_value + (double)expression_value; /* set by yyparse () */ -} - -#ifdef TEST_EXP_READER -/* main program, for testing purposes. */ -main () -{ - int n, c; - char buf[1024]; - extern int yydebug; -/* - yydebug = 1; -*/ - initialize_random_junk (); - - for (;;) { - printf ("enter expression: "); - n = 0; - while ((buf[n] = getchar ()) != '\n' && buf[n] != EOF) - n++; - if (buf[n] == EOF) - break; - buf[n] = '\0'; - printf ("parser returned %d\n", parse_c_expression (buf)); - } -} - -/* table to tell if char can be part of a C identifier. */ -unsigned char is_idchar[256]; -/* table to tell if char can be first char of a c identifier. */ -unsigned char is_idstart[256]; -#ifndef sgi -/* table to tell if c is horizontal space. isspace () thinks that - newline is space; this is not a good idea for this program. */ -char is_hor_space[256]; -#endif - -/* - * initialize random junk in the hash table and maybe other places - */ -initialize_random_junk () -{ - register int i; - - /* - * Set up is_idchar and is_idstart tables. These should be - * faster than saying (is_alpha (c) || c == '_'), etc. - * Must do set up these things before calling any routines tthat - * refer to them. - */ - for (i = 'a'; i <= 'z'; i++) { - ++is_idchar[i - 'a' + 'A']; - ++is_idchar[i]; - ++is_idstart[i - 'a' + 'A']; - ++is_idstart[i]; - } - for (i = '0'; i <= '9'; i++) - ++is_idchar[i]; - ++is_idchar['_']; - ++is_idstart['_']; -#ifdef DOLLARS_IN_IDENTIFIERS - ++is_idchar['$']; - ++is_idstart['$']; -#endif - - /* horizontal space table */ - ++is_hor_space[' ']; - ++is_hor_space['\t']; -} - -struct hashnode * -lookup (name, len, hash) - char *name; - int len; - int hash; -{ - return (DEFAULT_SIGNED_CHAR) ? 0 : ((struct hashnode *) -1); -} -#endif - diff --git a/OpenSim/Utilities/simmToOpenSim/basic.h b/OpenSim/Utilities/simmToOpenSim/basic.h deleted file mode 100644 index c1d3ab2d3d..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/basic.h +++ /dev/null @@ -1,588 +0,0 @@ -/******************************************************************************* - - BASIC.H - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Basic #defines, enums, and structures needed by most/all ".c" files. - -*******************************************************************************/ - -#ifndef BASIC_H -#define BASIC_H - -#define STRUCT typedef struct -#define UNION typedef union -#define ENUM typedef enum - -#define public - -#define SELECT_BUTTON RIGHTMOUSE - -#define DUMMY_LONG 0 -#define DUMMY_INT 0 - -#define ZERO 0 /* used like NULL, but is an int */ -#define NO 0 -#define YES 1 - -#define SPACE ' ' -#define TAB '\t' -#define CARRIAGE_RETURN '\n' -#define STRING_TERMINATOR '\0' -#define LINE_FEED 13 - -#ifdef WIN32 - #define DIR_SEP_CHAR '\\' - #define DIR_SEP_STRING "\\" - - #define COPY_COMMAND "copy" -#else - #define DIR_SEP_CHAR '/' - #define DIR_SEP_STRING "/" - - #define COPY_COMMAND "cp" -#endif - -#define COMMAND_BUFFER 500 - -#define PIXELSPERINCH 96 -#define PIXELSPERINCHF 96.0 - -#define MENU_ITEM_HEIGHT 24 -#define FORM_FIELD_HEIGHT 21 -#define FORM_FIELD_YSPACING 27 -#define COMBO_ITEM_HEIGHT 20 -#define COMBO_ITEM_SIZE 135 - -#define MAX_MGROUP_MENU_HEIGHT 540 -#define MGROUP_MENU_SPACER 20 - -#define MAX_CHAR_WIDTH 13 - -#define MAX_NUM_MATERIALS 64 - -#define FORMITEM_XMARGIN 4 -#define FORMITEM_YMARGIN 6 - -#define FORM_ITEM_STRING_MAX 256 -#define FORM_BEGINNING 0 -#define FORM_END 1 -#define FORM_CURSOR 2 -#define FORM_INSERT 3 -#define FORM_NAVIGATE 4 - -#define MODEL_SELECTOR_X 20 -#define MODEL_SELECTOR_Y -40 - -#define END_OF_ARRAY -1 - -#define NO_WINDOW -1 -#define NO_SELECTION -1 - -#define FILLED 1 -#define EMPTY 0 -#define DID_NOT_PLACE_MENU 0 -#define PLACED_MENU 1 - -#define NOITEM -1 -#define TITLEBOX -2 - -#define TITLE_BAR -2 -#define TITLE_AREA_HEIGHT 65 - -#define FUNCTION_ARRAY_INCREMENT 24 // might cause crashing, setting to 1 fixes this -#define MOTION_ARRAY_INCREMENT 4 -#define DEFAULT_ARRAY_INCREMENT 10 - -#define CHARBUFFER 4096 -#if ENGINE || CORTEX_PLUGIN - #define MODELBUFFER 4 - #define PLOTBUFFER 1 - #define TOOLBUFFER 1 - #define SCENEBUFFER 4 -#else - #define MODELBUFFER 32 - #define PLOTBUFFER 24 - #define TOOLBUFFER 16 - #define SCENEBUFFER 32 -#endif -#define COLORBUFFER 256 - -#define GENBUFFER 64 /* used only for gencoord help text in MV */ -#define GROUPBUFFER 96 /* used by JO and PM model options structs */ -#define COLUMNBUFFER 50 /* used by tools which place muscle menus */ - -#define FIXED_SEGMENT 0 -#define MAX_MUSCLE_POINTS 200 -#define MAX_PLOT_CURVES 60 -#define MAXMDOUBLE 99999999.999 -#define MINMDOUBLE -99999999.999 -#define TINY_NUMBER 0.0000001 -#define ERROR_DOUBLE -999999.3 -#define ROUNDOFF_ERROR 0.000000001 -#define KEEPOLDDOUBLE -999999.4 -#define DONT_CHECK_DOUBLE -999999.5 -#define ERRORINT -32760 -#define MAXIMUMERROR 0.01 -#define DEG_TO_RAD 0.017453292519943 -#define RAD_TO_DEG 57.295779513082323 -#define DTOR DEG_TO_RAD -#define RTOD RAD_TO_DEG -#define DOUBLE_NOT_DONE -99999.73 -#define MAX_MATRIX_SIZE 20 -#define MAX_UINT ((unsigned int)~((unsigned int)0)) -#define MAX_INT ((int)(MAX_UINT >> 1)) - -#define UNDEFINED_USERFUNCNUM -9999 -#define INVALID_FUNCTION -1 -#define INVALID_GENCOORD -1 - -#define STRING_DONE 0 -#define STRING_NOT_DONE 1 -#define KEEP_OLD_STRING 2 -#define NULL_STRING_ENTERED 3 - -#define CHECKBOX_XSIZE 18 -#define CHECKBOX_YSIZE 18 - -#define UPDATE_WINDOW 1 -#define DONT_UPDATE_WINDOW 0 - -#define HELP_WINDOW_TEXT_Y_SPACING 20 -#define HELP_WINDOW_X_MARGIN 40 - -#define HIGHLIGHT_TEXT 1 -#define CENTER_JUSTIFY 2 -#define NO_NEW_LINE 4 -#define OVERWRITE_LAST_LINE 8 -#define OVERWRITABLE 16 -#define SPECIAL_COLOR 32 -#define DEFAULT_MESSAGE_X_OFFSET 10 - -#define SHOW_MODEL 0x00000001 -#define SHOW_PLOT 0x00000002 -#define SHOW_HELP 0x00000004 - -#define NULL_SELECTED 0 -#define MODEL_SELECTED 1 -#define PLOT_SELECTED 2 -#define HELP_SELECTED 3 - -#define NUM_DRAW_MODES 7 - -#ifndef FALSE - enum {FALSE, TRUE}; -#endif -enum {OFF, ON}; - -enum {XX, YY, ZZ, WW}; -enum {RD, GR, BL}; - -/* to go along with "typedef float Matrix[4][4]" in gl.h */ -typedef double DMatrix[4][4]; - -typedef double DCoord[3]; -typedef double Quat[4]; - -typedef unsigned __int64 PickIndex; - -STRUCT { - double x1; /* minimum x */ - double y1; /* minimum y */ - double x2; /* maximum x */ - double y2; /* maximum y */ -} Ortho; /* values for ortho() commands */ - - -STRUCT { - double x; /* x coordinate */ - double y; /* y coordinate */ -} XYCoord; /* an (x,y) coordinate pair */ - - -STRUCT { - int x; /* x coordinate */ - int y; /* y coordinate */ -} XYIntCoord; /* an (x,y) integer coordinate pair */ - - -STRUCT { - int x1; /* minimum x */ - int y1; /* minimum y */ - int x2; /* maximum x */ - int y2; /* maximum y */ -} IntBox; /* a box in integer coordinates */ - - -STRUCT { - double x1; - double x2; - double y1; - double y2; - double z1; - double z2; -} BoundingCube; - - -STRUCT { - long x1; /* minimum x */ - long y1; /* minimum y */ - long x2; /* maximum x */ - long y2; /* maximum y */ -} LongBox; /* a box in long coordinates */ - -STRUCT { - GLfloat rgb[3]; -} ColorRGB; /* an rgb triplet defining a color */ - -ENUM { - defining_element, /* defining an element's properties */ - declaring_element, /* using the element name, not defining it */ - just_checking_element /* just checking to see if already defined */ -} EnterMode; /* modes when entering/defining model elements */ - -ENUM { - type_int, /* integer */ - type_double, /* double */ - type_char, /* single character */ - type_string /* string */ -} VariableType; /* variable types that can be read from string */ - - -ENUM { - up_obj, /* object appears to come out of screen */ - down_obj /* object appears to go down into screen */ -} GUIObjectMode; - - -ENUM { - code_fine, /* fine, no error was encountered */ - code_bad /* bad, an error was encountered */ -} ReturnCode; /* error condition values */ - - -ENUM { - recover, /* recover from error */ - abort_action, /* abort action; critical error */ - exit_program, /* exit program; fatal error */ - none /* no action; no error */ -} ErrorAction; /* error recovery actions */ - - -ENUM { - horizontal_slider, /* */ - vertical_slider /* */ -} SliderType; /* */ - - -ENUM { - zeroth, /* zeroth derivative */ - first, /* first derivative */ - second /* second derivative */ -} Derivative; /* function derivative values */ - - -ENUM { - FORWARD, /* forward for converting a frame/point */ - INVERSE /* inverse for converting a frame/point */ -} Direction; /* directions used in joint traversal */ - - -ENUM { - from_ground, - to_ground -} GroundDirection; - -ENUM { - no, /* no */ - yes /* yes */ -} SBoolean; /* conventional SBooleanean */ - - -ENUM -{ - off, - on -} OnOffSwitch; - - -ENUM -{ - right, - left -} Justification; - - -ENUM { - valid, /* clean, still valid */ - invalid /* dirty, no longer valid */ -} Condition; /* conditions for calculated values */ - - -ENUM { - gouraud_shading=1, - flat_shading, - solid_fill, - wireframe, - outlined_polygons, - bounding_box, - no_surface -} DrawingMode; - - -ENUM { - normal_menu, - toggle_menu -} MenuType; - - -ENUM { - normal_checkbox, - radio_checkbox -} CheckBoxType; - -ENUM { - no_field_action, - goto_previous_field, - goto_next_field -} TextFieldAction; - -ENUM { - left_arrow, - up_arrow, - right_arrow, - down_arrow -} ArrowDirection; - -STRUCT { - ArrowDirection direction; - SBoolean pressed; - int color; - int pressed_color; - IntBox bounding_box; - XYIntCoord tip; - XYIntCoord base1; - XYIntCoord base2; -} ArrowButton; - -STRUCT { - SBoolean visible; - SBoolean active; - SliderType type; - char label[3]; - double value; - double min_value; - double max_value; - double arrow_step; - int thumb_thickness; - int thumb_dist; - IntBox shaft; - ArrowButton decrease_arrow; - ArrowButton increase_arrow; - SBoolean thumb_pressed; - int thumb_color; - int background_color; - int border_color; - int pressed_thumb_color; - void* data; /* user-supplied data to identify slider */ -} Slider; - -STRUCT { - int numsliders; - Slider* sl; - XYIntCoord origin; -} SliderArray; - -STRUCT { - SBoolean visible; - SBoolean active; - char start_label[10]; - char end_label[10]; - double start_value; - double end_value; - double min_value; - double max_value; - double arrow_step; - int thumb_thickness; - int thumb_dist; - int thumb_x1; - int thumb_x2; - IntBox bounding_box; - IntBox shaft; - ArrowButton start_increase_arrow; - ArrowButton start_decrease_arrow; - ArrowButton end_increase_arrow; - ArrowButton end_decrease_arrow; - SBoolean thumb_pressed; - int thumb_color; - int background_color; - int border_color; - int pressed_thumb_color; - void* data; /* user-supplied data to identify slider */ - XYIntCoord origin; -} CropSlider; - - -STRUCT { - IntBox box; /* position of option box */ - char* name; /* name of option */ - SBoolean active; /* is this menu item active? */ - SBoolean visible; -} MenuItem; /* an Menu menu option */ - - -STRUCT { - MenuType type; /* toggle menu or normal menu */ - int numoptions; /* number of options */ - IntBox titlebox; /* position of title box */ - char* title; /* title of menu */ - IntBox bbox; /* bounding box of entire menu */ - MenuItem *option; /* list of menu-option structures */ - XYIntCoord origin; /* origin of menu */ -} Menu; /* properties of an Menu menu */ - - -STRUCT { - IntBox box; /* position of option box */ - char* name; /* name of option */ - char valuestr[FORM_ITEM_STRING_MAX]; /* string to hold current value */ - SBoolean justify; /* used for lining-up numbers */ - SBoolean active; /* whether or not this field is active */ - SBoolean visible; /* whether of not this field is visible */ - SBoolean editable; /* whether or not this field can be edited */ - SBoolean use_alternate_colors; /* use alternate colors for display? */ - int decimal_places; /* how many places to right of number */ - void* data; /* user-supplied data to identify form item */ - int firstVisible; /* first character that is visible in the box */ - int lastVisible; /* last character that is visible in the box */ -} FormItem; /* a Form menu option */ - - -STRUCT { - int numoptions; /* number of options */ - char* title; /* title of form */ - IntBox bbox; /* bounding box of entire form */ - FormItem *option; /* list of form-option structures */ - XYIntCoord origin; /* origin of menu */ - int selected_item; /* the selected option, if any */ - int cursor_position; /* where the cursor is in the selected string */ - int highlight_start; /* first character of selected text in string */ -} Form; /* properties of a Form */ - - -STRUCT { - IntBox box; /* position of option box */ - char* name; /* name of option */ - OnOffSwitch state; /* state of checkbox (yes,no) */ - Justification just; /* left or right justified text */ - SBoolean active; /* whether or not this checkbox is active */ - SBoolean visible; - SBoolean use_alternate_colors;//dkb -} CheckBox; /* a checkbox */ - - -STRUCT { - CheckBoxType type; /* normal (boxes) or radio button (diamonds) */ - int numoptions; /* number of options */ - char* title; /* title of menu */ - IntBox bbox; /* bounding box of entire checkbox region */ - CheckBox *checkbox; /* list of checkboxes */ - XYIntCoord origin; /* origin of menu */ -} CheckBoxPanel; /* holds panel of checkboxes */ - - -STRUCT { - IntBox box; /* position of combobox */ - char* defaultName; /* name to show when no selected option */ - const char* currentName; /* name of currently selected option */ - int currentMenuIndex; /* index in menu of currently selected option */ - SBoolean active; /* is this combobox active? */ - SBoolean visible; /* is this combobox visible? */ - long popupMenu; /* the popup menu of options to choose from */ - void (*menuCallback)(int menuValue, void* userData); /* callback for popup menu */ - void* userData; /* user data for popup menu callback */ -} ComboBox; /* a combobox for choosing one of several options */ - -STRUCT { - char *title; - int numoptions; - IntBox bbox; /* bounding box of entire combobox region */ - ComboBox *combobox; /* list of combo boxes */ - XYIntCoord origin; /* origin of menu */ - int yPosition; -} ComboBoxPanel; - -STRUCT { - OnOffSwitch state; /* whether this menu is on or off */ - int xo; /* x coordinate of menu origin */ - int yo; /* y coordinate of menu origin */ - int starting_column; /* first column this menu occupied, when on */ -} MuscleMenu; /* state and position of group menus */ - - -STRUCT { - int block_size; /* */ - int free_ptr; /* */ - void* memory; /* */ -} MEMORYBLOCK; /* */ - - -STRUCT { - double xyz[3]; /* x, y, and z coordinates */ - float normal[3]; /* vertex normal (float because of n3f()*/ -} Vertex; /* description of a polygon vertex */ - - -STRUCT { - int numpoints; /* number of vertices in polygon */ - int pts[25]; /* list of vertices in polygon */ - float normal[3]; /* polygon normal */ -} SimmPolygon; /* description of a polygon */ - - -STRUCT { - char* text; - int format; - int xoffset; -} TextLine; - - -STRUCT { - TextLine* line; - int num_lines_malloced; - int num_lines; - int window_id; - int window_width; - int window_height; - Slider sl; - int lines_per_page; - int starting_line; - int background_color; -} HelpStruct; - - -STRUCT { - void* defaultfont; /* */ - void* largefont; /* */ - void* smallfont; /* */ - void* italics; /* */ - void* bold; /* */ -} GlobalFonts; /* */ - - -/* TOOL QUERIES (sent to a tools query_handler routine) */ -ENUM -{ - GET_TOOL_MODEL, - GET_TOOL_PLOT -} QueryType; - -#endif /*BASIC_H*/ - diff --git a/OpenSim/Utilities/simmToOpenSim/cefunctions.h b/OpenSim/Utilities/simmToOpenSim/cefunctions.h deleted file mode 100644 index e4935ca87d..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/cefunctions.h +++ /dev/null @@ -1,53 +0,0 @@ -/******************************************************************************* - - CEFUNCTIONS.H - this header declares the Constraint Editor's public functions - - Author: Krystyne Blaikie (based on wefunctions.h by Kenny Smith) - - Date: 8-APR-02 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef CEFUNCTIONS_H -#define CEFUNCTIONS_H - -void makeconstrainteditormenus(void); -void ce_entervalue(SimmEvent se); -void update_ce_forms(void); -void move_ce_help_text(int dummy_int, double slider_value, double delta); - -void ce_track_cb(void* data, SimmEvent se); - -void ce_recalc_loops_and_constraints(ModelStruct* ms, int constraint_object, - SBoolean displayErrorMsg); - - -int is_current_constraint_object(ModelStruct*, ConstraintObject*); -int is_current_constraint_point(ModelStruct* ms, ConstraintObject* co, ConstraintPoint *pt); -void select_constraint_object(int constraintobj, SBoolean redisplay); -void select_constraint_point(int coIndex, int ptIndex, SBoolean redisplay); - -void convert_to_constraint_object_frame(ConstraintObject*, double* pt); -void convert_from_constraint_object_frame(ConstraintObject*, double* pt); - -void reset_constraintobj_xform(); - -void apply_xform_to_constraintobj(double factor); -void clear_ce_xform_form(); -void make_constraint_cylinder (ConstraintObject* co); -void save_all_constraint_objects(ModelStruct* ms); -int save_constraint_points(ModelStruct* ms, int constraint_obj); -void restore_all_constraint_objects(ModelStruct* ms); -int restore_constraint_points(ModelStruct* ms); -void delete_constraint_object(ModelStruct* ms, int constraintobj, SBoolean queue_event); -void delete_constraint_point(ModelStruct* ms, int constraintpt, SBoolean queue_event); - -void update_ce_win_status(); - -double calc_distance_to_ellipsoid(double p1[], double radii[], double projpt[]); - -#endif /* CEFUNCTIONS_H */ diff --git a/OpenSim/Utilities/simmToOpenSim/color.h b/OpenSim/Utilities/simmToOpenSim/color.h deleted file mode 100644 index 814e88abcf..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/color.h +++ /dev/null @@ -1,131 +0,0 @@ -/******************************************************************************* - - COLOR.H - - Author: Peter Loan - - Date: 25-JUN-91 - - Copyright (c) 1992-4 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#define BLACK 0 -#define RED 1 -#define GREEN 2 -#define YELLOW 3 -#define BLUE 4 -#define MAGENTA 5 -#define CYAN 6 -#define WHITE 7 -#define DESKTOP_BACKGROUND 8 -#define DESKTOP_BORDER 9 -#define DESKTOP_TEXT 10 -#define TOOL_TITLE_AREA_BACKGROUND 11 -#define TOOL_TITLE_AREA_BORDER 12 -#define TOOL_TITLE_AREA_TEXT 13 -#define FORM_ITEM_BACKGROUND 14 -#define FORM_ITEM_BORDER 15 -#define FORM_ITEM_TEXT 16 -#define FORM_ITEM_SELECTED_BACKGROUND 17 -#define FORM_ITEM_SELECTED_BORDER 18 -#define FORM_ITEM_SELECTED_TEXT 19 -#define FORM_TITLE_BOX_BACKGROUND 20 -#define FORM_TITLE_BOX_BORDER 21 -#define FORM_TITLE_BOX_TEXT 22 -#define MENU_ITEM_BACKGROUND 23 -#define MENU_ITEM_BORDER 24 -#define MENU_ITEM_TEXT 25 -#define MENU_ITEM_SELECTED_BACKGROUND 26 -#define MENU_ITEM_SELECTED_BORDER 27 -#define MENU_ITEM_SELECTED_TEXT 28 -#define MENU_TITLE_BOX_BACKGROUND 29 -#define MENU_TITLE_BOX_BORDER 30 -#define MENU_TITLE_BOX_TEXT 31 -#define HELP_WINDOW_BACKGROUND 32 -#define HELP_WINDOW_BORDER 33 -#define HELP_WINDOW_TEXT 34 -#define CONFIRM_WINDOW_BACKGROUND 35 -#define CONFIRM_WINDOW_BORDER 36 -#define CONFIRM_WINDOW_TEXT 37 -#define TOOL_WINDOW_BACKGROUND 38 -#define TOOL_WINDOW_BORDER 39 -#define TOOL_WINDOW_TEXT 40 -#define MISC_TEXTPORT 41 -#define MISC_PLOT_BACKGROUND 42 -#define MISC_PLOT_POINT 43 -#define MISC_PLOT_LABELS 44 -#define MISC_CURVE_NAME 45 -#define MISC_CURVE_NAME_SELECTED 46 -#define MISC_CURVE_BACKGROUND_SELECTED 47 -#define MISC_CONFIRM_BORDER1 48 -#define MISC_CONFIRM_BORDER2 49 -#define MISC_CONFIRM_BORDER3 50 -#define MISC_MUSCLE_POINT 51 -#define MISC_MUSCLE_POINT_SELECTED 52 -#define MISC_MUSCLE_LINE20 53 -#define MISC_MUSCLE_LINE19 54 -#define MISC_MUSCLE_LINE18 55 -#define MISC_MUSCLE_LINE17 56 -#define MISC_MUSCLE_LINE16 57 -#define MISC_MUSCLE_LINE15 58 -#define MISC_MUSCLE_LINE14 59 -#define MISC_MUSCLE_LINE13 60 -#define MISC_MUSCLE_LINE12 61 -#define MISC_MUSCLE_LINE11 62 -#define MISC_MUSCLE_LINE10 63 -#define MISC_MUSCLE_LINE9 64 -#define MISC_MUSCLE_LINE8 65 -#define MISC_MUSCLE_LINE7 66 -#define MISC_MUSCLE_LINE6 67 -#define MISC_MUSCLE_LINE5 68 -#define MISC_MUSCLE_LINE4 69 -#define MISC_MUSCLE_LINE3 70 -#define MISC_MUSCLE_LINE2 71 -#define MISC_MUSCLE_LINE1 72 -#define MISC_MUSCLE_LINE0 73 -#define MISC_WIREFRAME_BONES 74 -#define MISC_MODEL_WINDOW_BACKGROUND 75 -#define MISC_HELP_TEXT_BOLD 76 -#define MISC_GUI_SIDE1 77 -#define MISC_GUI_SIDE2 78 -#define MISC_GUI_SIDE3 79 -#define MISC_GUI_SIDE4 80 -#define MISC_TOGGLE_BUTTON_ON 81 -#define MISC_TOGGLE_BUTTON_OFF 82 -#define MISC_GUI_BORDER 83 -#define MISC_BONE_POLYGON 84 -#define MISC_BONE_POLYGON_VERTEX_LABELS 85 -#define MISC_SLIDER_BACKGROUND 86 -#define MISC_SLIDER_THUMB 87 -#define MISC_SLIDER_THUMB_PRESSED 88 -#define MISC_FORM_TEXT_HIGHLIGHT 89 -#define MISC_FORM_CURSOR 90 -#define MISC_PLOT_CURVE1 91 -#define MISC_PLOT_CURVE2 92 -#define MISC_PLOT_CURVE3 93 -#define MISC_PLOT_CURVE4 94 -#define MISC_PLOT_CURVE5 95 -#define MISC_PLOT_CURVE6 96 -#define MISC_PLOT_CURVE7 97 -#define MISC_PLOT_CURVE8 98 -#define MISC_PLOT_CURVE9 99 -#define MISC_PLOT_CURVE10 100 -#define MISC_PLOT_CURVE11 101 -#define MISC_PLOT_CURVE12 102 -#define MISC_PLOT_CURVE13 103 -#define MISC_PLOT_CURVE14 104 -#define MISC_PLOT_CURVE15 105 -#define MISC_PLOT_CURVE16 106 -#define MISC_PLOT_CURVE17 107 -#define MISC_PLOT_CURVE18 108 -#define MISC_PLOT_CURVE19 109 -#define MISC_PLOT_CURVE20 110 -#define MISC_SEGMENT_AXES 111 -#define MISC_ROTATION_AXES 112 -#define MISC_CROSSHAIRS 113 -#define MISC_MOTION_EVENTS 114 -#define FORM_ITEM_TEXT_ALT 115 - diff --git a/OpenSim/Utilities/simmToOpenSim/constraint.c b/OpenSim/Utilities/simmToOpenSim/constraint.c deleted file mode 100644 index 9cbad1ec92..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/constraint.c +++ /dev/null @@ -1,420 +0,0 @@ -/******************************************************************************* - - CONSTRAINT.C - - Author: Krystyne Blaikie - - Date: 20-DEC-01 - - Copyright (c) 2000 MusculoGraphics, Inc. - All rights reserved. - - Description: - - Routines: - markAffectedGencoords: - calculateConstraintResids: - updateConstraintInfo: - -*******************************************************************************/ -#include "universal.h" -#include "functions.h" -#include "globals.h" -#include "cefunctions.h" - -/*************** DEFINES (for this file only) *********************************/ -#if EXPERIMENTAL -#define DEBUG_LEVEL 1 -#endif -#if OPENSMAC -#undef ENGINE -#define ENGINE 1 -#endif - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ -extern int JACOBIAN; - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ -extern char badConstraintErrorMsg[]; -extern char badGencoordErrorMsg2[]; - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ - - -/************************* static function prototypes ***********************************/ - - - -/* mark and store all gencoords and joints that are found in loops - * created by constraints */ -void markAffectedGencoords(ModelStruct *ms) -{ - int i, j, k, parent, child, jnt; - int *path, nq, nj, *jnts; - GeneralizedCoord **qs, *gc; - - qs = (GeneralizedCoord**)simm_malloc(6 * ms->numjoints * sizeof(GeneralizedCoord*)); - jnts = (int *)simm_malloc(ms->num_constraint_objects * ms->numjoints * sizeof(int)); - for (i = 0; i < ms->num_constraint_objects; i++) - { - /* Free the joints and qs arrays, if they exist. They will either - * be left NULL, or re-malloced later. - */ - FREE_IFNOTNULL(ms->constraintobj[i].joints); - FREE_IFNOTNULL(ms->constraintobj[i].qs); - - if (ms->constraintobj[i].active == no) - { - ms->constraintobj[i].num_qs = 0; - ms->constraintobj[i].num_jnts = 0; - continue; - } - nq = 0; - nj = 0; - - if (!ms->constraintobj[i].points) - { - continue; - } - parent = ms->constraintobj[i].segment; - child = ms->constraintobj[i].points[0].segment; - - path = GET_PATH(ms->modelnum, parent, child); - for (j = 0; path[j] != ms->numjoints + 1; j++) - { - jnt = ABS(path[j]) - 1; - jnts[nj] = jnt; - nj++; - for (k = 0; k < 6; k++) - { - gc = ms->joint[jnt].dofs[k].gencoord; - if (gc && gc->used_in_model == yes) - { - gc->used_in_constraint = yes; - qs[nq] = gc; - nq++; - } - } - } - - ms->constraintobj[i].num_qs = nq; - ms->constraintobj[i].num_jnts = nj; - ms->constraintobj[i].joints = (int *)simm_malloc(nj * sizeof(int)); - ms->constraintobj[i].qs = (GeneralizedCoord**)simm_malloc(nq * sizeof(GeneralizedCoord*)); - for (j = 0; j < nj; j++) - ms->constraintobj[i].joints[j] = jnts[j]; - for (j = 0; j < nq; j++) - ms->constraintobj[i].qs[j] = qs[j]; - -#if EXPERIMENTAL - printf("Constraint %d\n", i); - printf("%s -> %s:\n", ms->segment[parent].name, ms->segment[child].name); - printf("joints: "); - for (j = 0; j < ms->constraintobj[i].num_jnts; j++) - printf("%s ", ms->joint[ms->constraintobj[i].joints[j]].name); - printf("\ngencoords: "); - for (j = 0; j < ms->constraintobj[i].num_qs; j++) - printf("%s ", ms->constraintobj[i].qs[j]->name); - printf("\n"); -#endif - } - - free(qs); - free(jnts); -} - - -/* CALCULATE CONSTRAINT RESIDUALS: - * assume gencoords have already been set to the values in the Q array. - * Determine the residuals for each constraint point in each constraint - * object that is being solved. The residual for each point equals the - * shortest distance from the point to its constraint object. If the - * projected point is not within the active quadrant, residuals are increased. - */ -void calculateConstraintResids(void *data, int numQ, double q[], int numResid, double resid[], - int startIndex, int endIndex, - int *iflag) -{ - int i, j, index, seg_index, constraintAxis, constraintSign; - double pt[3], projpt[3], vec[3], distance; - LCStruct *solveInfo = (LCStruct *)data; - -//if (!JACOBIAN) printf("cons resid\n"); - index = startIndex; - for (i = 0; i < solveInfo->model->num_constraint_objects; i++) - { - if (index > endIndex) - { -// printf("problem\n"); - break; - } - if (solveInfo->model->constraintobj[i].active == no) - continue; - if (solveInfo->constraintInfo->consUsed[i] == no) - continue; - if (solveInfo->model->constraintobj[i].constraintType == constraint_plane) - { - PlaneStruct plane; - double planeLength, planeWidth; - - if (solveInfo->model->constraintobj[i].xforms_valid == no) - recalc_constraint_xforms(&solveInfo->model->constraintobj[i]); - - plane.a = solveInfo->model->constraintobj[i].plane.a; - plane.b = solveInfo->model->constraintobj[i].plane.b; - plane.c = solveInfo->model->constraintobj[i].plane.c; - plane.d = solveInfo->model->constraintobj[i].plane.d; - - for (j = 0; j < solveInfo->model->constraintobj[i].numPoints; j++) - { - COPY_1X3VECTOR(solveInfo->model->constraintobj[i].points[j].offset, pt); - convert(solveInfo->model, pt, solveInfo->model->constraintobj[i].points[j].segment, - solveInfo->model->constraintobj[i].segment); - project_point_onto_plane(pt, &plane, projpt); - SUB_VECTORS(projpt, pt, vec); - distance = VECTOR_MAGNITUDE(vec); - - /* determine whether projected point is within plane limits */ - planeLength = solveInfo->model->constraintobj[i].radius.xyz[0]; - planeWidth = solveInfo->model->constraintobj[i].radius.xyz[1]; - if ((fabs(projpt[0]) > planeLength) || (fabs(projpt[2]) > planeWidth)) - { - distance = 10.0; - } - - resid[index++] = distance; -//if (!JACOBIAN) printf("resid[%d] (cons %d) = %f\n", index-1, i, resid[index-1]); - } - } - else - { - constraintAxis = solveInfo->model->constraintobj[i].constraintAxis; - constraintSign = solveInfo->model->constraintobj[i].constraintSign; - for (j = 0; j < solveInfo->model->constraintobj[i].numPoints; j++) - { - seg_index = solveInfo->model->constraintobj[i].points[j].segment; - COPY_1X3VECTOR(solveInfo->model->constraintobj[i].points[j].offset, pt); - /* convert point from point_segment to constraint_segment frame */ - convert(solveInfo->model, pt, seg_index, solveInfo->model->constraintobj[i].segment); - /* convert from constraint_segment frame to constraint object frame */ - convert_to_constraint_object_frame(&solveInfo->model->constraintobj[i], pt); - /* calculate distance */ - if (solveInfo->model->constraintobj[i].constraintType == constraint_sphere) - { - double radius = solveInfo->model->constraintobj[i].radius.xyz[0]; - distance = VECTOR_MAGNITUDE(pt) - radius; - /* check that point is in active quadrant */ - if (solveInfo->model->constraintobj[i].constraintSign != 0) - { - double vec[3]; - - /* find projected point */ - vec[0] = pt[0]; - vec[1] = pt[1]; - vec[2] = pt[2]; - normalize_vector(vec, vec); - projpt[0] = vec[0] * radius; - projpt[1] = vec[1] * radius; - projpt[2] = vec[2] * radius; - /* point on inactive quadrant if SIGN(pt[axis]) != constraint_sign */ - if (DSIGN(projpt[constraintAxis]) != constraintSign) - distance = 100.0; - } - } - else if (solveInfo->model->constraintobj[i].constraintType == constraint_ellipsoid) - { - distance = calc_distance_to_ellipsoid(pt, solveInfo->model->constraintobj[i].radius.xyz, projpt); - /* check whether projpt is in the active quadrant */ - if (constraintSign != 0) - { - /* point on inactive quadrant if SIGN(pt[axis]) != constraint_sign */ - if (DSIGN(projpt[constraintAxis]) != constraintSign) - distance = 100.0; - } - } - else if (solveInfo->model->constraintobj[i].constraintType == constraint_cylinder) - { - double radius = solveInfo->model->constraintobj[i].radius.xyz[0]; - double axis[] = {0.0, 0.0, 1.0}, origin[] = {0.0, 0.0, 0.0}; - distance = sqrt(get_distsqr_point_line(pt, origin, axis)) - radius; - if (solveInfo->model->constraintobj[i].constraintSign != 0) - { - /* find projected point on cylinder axis */ - get_point_from_point_line(pt, origin, axis, projpt); - /* find projected point on cylinder */ - vec[0] = pt[0] - projpt[0]; - vec[1] = pt[1] - projpt[1]; - vec[2] = pt[2] - projpt[2]; - normalize_vector(vec, vec); - projpt[0] = vec[0] * radius; - projpt[1] = vec[1] * radius; - projpt[2] = vec[2] * radius; - /* point on inactive quadrant if SIGN(pt[axis]) != constraint_sign */ - if (DSIGN(projpt[constraintAxis]) != constraintSign) - distance = 100.0; - } - } - else - distance = 0.0; - resid[index] = distance * solveInfo->model->constraintobj[i].points[j].weight; -//if (!JACOBIAN) printf("resid[%d] (cons %d) = %f\n", index, i, resid[index]); - index++; - } - } - } -} - - - - -/* check if any of the qs in the constraints have changed */ -void updateConstraintInfo(ModelStruct *ms) -{ - int i, j, k, nq, jnt; - GeneralizedCoord* gc; - ReturnCode rc; - - for (i = 0; i < ms->numgencoords; i++) - ms->gencoord[i]->used_in_constraint = no; - for (i = 0; i < ms->num_constraint_objects; i++) - { - nq = 0; - ms->constraintobj[i].qs = (GeneralizedCoord**)simm_malloc(6 * ms->constraintobj[i].num_jnts * sizeof(GeneralizedCoord*)); - for (j = 0; j < ms->constraintobj[i].num_jnts; j++) - { - jnt = ms->constraintobj[i].joints[j]; - for (k = 0; k < 6; k++) - { - gc = ms->joint[jnt].dofs[k].gencoord; - if (gc && gc->used_in_model == yes) - { - gc->used_in_constraint = yes; - ms->constraintobj[i].qs[nq] = gc; - nq++; - } - } - } - ms->constraintobj[i].num_qs = nq; - /* reallocate space for arrays given new sizes */ - ms->constraintobj[i].qs = (GeneralizedCoord**)simm_realloc(ms->constraintobj[i].qs, nq * sizeof(GeneralizedCoord*), &rc); - } -} - -static void transformConstraintPlane(ConstraintObject *co) -{ - double norm[3], dotProd; - - norm[0] = norm[1] = 0.0; - norm[2] = 1.0; - transform_vec(co->from_local_xform, norm); - co->plane.a = norm[0]; - co->plane.b = norm[1]; - co->plane.c = norm[2]; - /* determine "d" value if translation changed */ - dotProd = DOT_VECTORS(norm, co->translation.xyz); - co->plane.d = -1 * dotProd; -} - -/* recalculate constraint transforms if the transforms were invalidated */ -void recalc_constraint_xforms(ConstraintObject* co) -{ - - if ( ! co->xforms_valid) - { - identity_matrix(co->from_local_xform); - - if (co->rotationAngle != 0.0) - rotate_matrix_axis_angle(co->from_local_xform, co->rotationAxis.xyz, co->rotationAngle); - - translate_matrix(co->from_local_xform, co->translation.xyz); - - invert_4x4transform(co->from_local_xform, co->to_local_xform); - - /* if have a plane, transform the plane normal and its offset - translation from - * the origin along the normal */ - //dkb check - if the normal was previously transformed should you - // transform again? or should you transform default normal (001)? - if (co->constraintType == constraint_plane) - { - transformConstraintPlane(co); - } - - co->xforms_valid = yes; - } -} - -/* ============================================================================== - * ==== CONSTRAINT OBJECT TRANSFORMATION SUPPORT - */ - -#define CHECK_CO_XFORMS(_CO) { if ( ! (_CO)->xforms_valid) recalc_constraint_xforms(_CO); } - - -void convert_to_constraint_object_frame(ConstraintObject* co, double* pt) -{ - if (co && pt) - { - CHECK_CO_XFORMS(co); - - transform_pt(co->to_local_xform, pt); - } -} - -void convert_from_constraint_object_frame(ConstraintObject* co, double* pt) -{ - if (co && pt) - { - CHECK_CO_XFORMS(co); - - transform_pt(co->from_local_xform, pt); - } -} - -/* recalculate the constraint functions if constraint objects have changed. If errorMsg is - * set to yes, display any error messages if they occur. */ -void ce_recalc_loops_and_constraints(ModelStruct* ms, int constraint_object, - SBoolean displayErrorMsg) -{ - int i, j; - - if (ms == NULL || constraint_object < 0) - return; - - /* if there are constraints in the system -- if the co is used in a constraint - * resolve the constraints. - */ - if (ms->num_constraint_objects > 0) - { - ConstraintStatus constraintStatus; - LoopStatus ls; - - solveAllLoopsAndConstraints(ms, &ls, &constraintStatus, yes); - - /* after joints are changed, the default values may no longer satisfy the constraints */ - ms->defaultConstraintsOK = no; - -#if ! ENGINE - if (displayErrorMsg == yes) - { - if (constraintStatus == constraintBroken) - { - glutMessageBox(GLUT_OK, 1, GLUT_ICON_INFORMATION, - "Constraint Solver: Unable to Satisfy Constraints.", badConstraintErrorMsg); - } - else if (constraintStatus == gcOutOfRange) - { - glutMessageBox(GLUT_OK, 1, GLUT_ICON_INFORMATION, - "Constraint Solver: Check Restraint Functions", badGencoordErrorMsg2); - } - } -#endif - } - -#if ! ENGINE - queue_model_redraw(ms); -#endif -} diff --git a/OpenSim/Utilities/simmToOpenSim/convert.c b/OpenSim/Utilities/simmToOpenSim/convert.c deleted file mode 100644 index 7d45237dbd..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/convert.c +++ /dev/null @@ -1,820 +0,0 @@ -/******************************************************************************* - - CONVERT.C - - Authors: Peter Loan, Scott Delp - - Date: 6-APR-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - - Description: This file contains routines which define the transformations - between model segments. That is, they make and control - the 4x4 matrices needed to travel across each joint. - - See the Programmer's Manual for a description of how body-fixed - transformations work. Basically, when you do a transformation (rotation - or translation), you move the coordinate system along with the - object, so the next transformation is done with respect to the new - coordinate system. - In SIMM a joint is defined by one translation and three rotations. - The user is free to do these transformations in any order he chooses, - so the final joint transformation matrix is made by multiplying together - the four transformation matrices in the user-defined order. When a given - rotation matrix is made, the X, Y, and Z axes are rotated too, so the - next transformation matrix can be built properly. The translation - matrix is a bit special, because it typically moves the object without - moving the coordinate system. - Consider a square centered on the origin of its local coordinate system. - If you wanted to move it along the X axis, and then spin it around its - Z axis (its own center), the appropriate GL transformation commands would be - translate(x,0,0) and then rotate(angle,'Z'). You could then change the - value of "angle" to spin the object about its center. If you tried to - do the rotation first, it would rotate the X axis, so the subsequent - translation would be along this new axis, not the original X axis. - If you tried to duplicate the desired transformation by creating your own - 4x4 matrix and putting it on the stack (i.e. without using the GL - commands translate() and rotate()), you would encounter some problems. - If you generated the translation matrix, and then multiplied it by the - rotation matrix, the object would seem to spin around the original - (world) Z axis, not the object's local Z axis. This is because the - translation matrix effectively moves the points in the object away - from its coordianate system, so the subsequent rotation is different - (than what you want). When you want to rotate the object, what you - need to do is translate the object back to the origin, do the rotation, - then translate back to where you were. So your matrices look like - [T] [INV(T) R T*], instead of just T [R]. The translation after the - rotation is T* because it is a translation within the newly rotated - coordinate system, not just the negative of the previous translation. - If you were to do more than one rotation, your matrix string might be: - [T] [INV(T) R1 T*] [INV(T*) R2 T**], which is equal to [R1] [R2] [T**]. - Thus to do the final translation, you need to use T**, the translation - in the twice-rotated coordinate system. In this way, all the transformations - are body-fixed. Equivalently, you could make the final translation not - body-fixed so you do not need to compute T**. Just use T instead. - So you always want to do the translation last. But if the translation is - after some rotations (in the user-defined order), you need to translate - along a rotated axis, so you want to compute the translation matrix in - order, but then save it until the end to add it onto the transformation - stack. - - Routines: - convert : converts a 3-D point from one ref. frame to another - get_conversion : finds the matrix needed to travel across a joint - make_conversion : governs the making of the transformation matrices - make_body_fixed_transform : makes a body-fixed transformation matrix - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" - -#include "defunctions.h" - -#if INCLUDE_SKIN_EDITOR - #include "skfunctions.h" -#endif - -/*************** DEFINES (for this file only) *********************************/ - - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static double world_x[] = {1.0, 0.0, 0.0, 0.0}; -static double world_y[] = {0.0, 1.0, 0.0, 0.0}; -static double world_z[] = {0.0, 0.0, 1.0, 0.0}; - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void make_joint_conversion(ModelStruct* ms, int joint); - -/* CONVERT: */ - -int convert(ModelStruct* ms, double p[], int n1, int n2) -{ - int i; - int* path; - double result[3]; - DMatrix* mat; - - if (n1 == n2) - return (0); - - if (n1 == ms->ground_segment) - { - mat = get_ground_conversion(ms,n2,from_ground); - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - p[0] = result[0]; - p[1] = result[1]; - p[2] = result[2]; - return (0); - } - else if (n2 == ms->ground_segment) - { - mat = get_ground_conversion(ms,n1,to_ground); - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - p[0] = result[0]; - p[1] = result[1]; - p[2] = result[2]; - return (0); - } - - // Performance enhancements for See System 9/8/93: - // This routine used to copy p[] to a 1x4 vector, so that it can be multiplied - // by the 4x4 transformation matrices. Since the last column of the matrices - // is always [0 0 0 1], the code was changed to use the original 1x3 vector - // and ignore the last row and column of the 4x4 transformation matrices. - - path = GET_PATH(ms, n1, n2); - - for (i=0; path[i] != ms->numjoints+1; i++) - { - /* If you want to convert a point in A to B, and the joint is defined A -> B, - * then you need to use the inverse transformation matrix. - */ - - if (path[i] > 0) - mat = get_conversion(ms,path[i]-1,INVERSE); - else - mat = get_conversion(ms,-path[i]-1,FORWARD); - - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - - p[0] = result[0]; - p[1] = result[1]; - p[2] = result[2]; - } - - return (i); -} - -int convertNEW(ModelStruct* ms, double p[], int path[], int len) -{ - int i; - double result[3]; - DMatrix* mat; - - for (i=0; i < len; i++) - { - if (path[i] > 0) - mat = get_conversion(ms, path[i]-1, INVERSE); - else - mat = get_conversion(ms, -path[i]-1, FORWARD); - - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - - p[0] = result[0]; - p[1] = result[1]; - p[2] = result[2]; - } - - return i; -} - - - -/* Converts a vector from one reference frame to another */ - -int convert_vector(ModelStruct* ms, double p[], int n1, int n2) -{ - int i; - int* path; - double origin[3], result[3], new_origin[3]; - DMatrix* mat; - - if (n1 == n2) - return (0); - - origin[0] = origin[1] = origin[2] = 0.0; - - if (n1 == ms->ground_segment) - { - mat = get_ground_conversion(ms,n2,from_ground); - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - new_origin[0] = origin[0]*(*mat)[0][0] + origin[1]*(*mat)[1][0] + - origin[2]*(*mat)[2][0] + (*mat)[3][0]; - new_origin[1] = origin[0]*(*mat)[0][1] + origin[1]*(*mat)[1][1] + - origin[2]*(*mat)[2][1] + (*mat)[3][1]; - new_origin[2] = origin[0]*(*mat)[0][2] + origin[1]*(*mat)[1][2] + - origin[2]*(*mat)[2][2] + (*mat)[3][2]; - p[0] = result[0] - new_origin[0]; - p[1] = result[1] - new_origin[1]; - p[2] = result[2] - new_origin[2]; - return (0); - } - else if (n2 == ms->ground_segment) - { - mat = get_ground_conversion(ms,n1,to_ground); - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - new_origin[0] = origin[0]*(*mat)[0][0] + origin[1]*(*mat)[1][0] + - origin[2]*(*mat)[2][0] + (*mat)[3][0]; - new_origin[1] = origin[0]*(*mat)[0][1] + origin[1]*(*mat)[1][1] + - origin[2]*(*mat)[2][1] + (*mat)[3][1]; - new_origin[2] = origin[0]*(*mat)[0][2] + origin[1]*(*mat)[1][2] + - origin[2]*(*mat)[2][2] + (*mat)[3][2]; - p[0] = result[0] - new_origin[0]; - p[1] = result[1] - new_origin[1]; - p[2] = result[2] - new_origin[2]; - return (0); - } - - /* Performance enhancements for See System 9/8/93: - * This routine used to copy p[] to a 1x4 vector, so that it can be multiplied - * by the 4x4 transformation matrices. Since the last column of the matrices - * is always [0 0 0 1], the code was changed to use the original 1x3 vector - * and ignore the last row and column of the 4x4 transformation matrices. - */ - - path = GET_PATH(ms,n1,n2); - - for (i=0; path[i] != ms->numjoints+1; i++) - { - /* If you want to convert a point in A to B, and the joint is defined A -> B, - * then you need to use the inverse transformation matrix. - */ - - if (path[i] > 0) - mat = get_conversion(ms,path[i]-1,INVERSE); - else - mat = get_conversion(ms,-path[i]-1,FORWARD); - - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - new_origin[0] = origin[0]*(*mat)[0][0] + origin[1]*(*mat)[1][0] + - origin[2]*(*mat)[2][0] + (*mat)[3][0]; - new_origin[1] = origin[0]*(*mat)[0][1] + origin[1]*(*mat)[1][1] + - origin[2]*(*mat)[2][1] + (*mat)[3][1]; - new_origin[2] = origin[0]*(*mat)[0][2] + origin[1]*(*mat)[1][2] + - origin[2]*(*mat)[2][2] + (*mat)[3][2]; - p[0] = result[0] - new_origin[0]; - p[1] = result[1] - new_origin[1]; - p[2] = result[2] - new_origin[2]; - } - - return (i); - -} - - -/* GET_TRANSFORM_BETWEEN_SEGS: This routine returns the 4x4 transform - * matrix between segment n1 and n2. - */ - -void get_transform_between_segs(ModelStruct* ms, double tmat[][4], int n1, int n2) -{ - - if (n1 == n2) - { - reset_4x4matrix(tmat); - } - else if (n1 == ms->ground_segment) - { - memcpy(tmat, *get_ground_conversion(ms, n2, from_ground), 16 * sizeof(double)); - } - else if (n2 == ms->ground_segment) - { - memcpy(tmat, *get_ground_conversion(ms, n1, to_ground), 16 * sizeof(double)); - } - else - { - memcpy(tmat, *get_ground_conversion(ms, n1, to_ground), 16 * sizeof(double)); - - append_matrix(tmat, *get_ground_conversion(ms, n2, from_ground)); - } - -} - - -void invalidate_joint_matrix(ModelStruct* model, JointStruct* joint) -{ - int i; - - // Make the joint matrix invalid. - joint->conversion.condition = invalid; - - // Now invalidate the ground-to-segment transforms of every segment that - // uses this joint to get to ground. - for (i=0; inumsegments; i++) - { -#if INCLUDE_SKIN_EDITOR - sk_invalidate_bone_xform(model, i); -#endif - if (joint->in_seg_ground_path[i] == yes) - model->segment[i].ground_condition = invalid; - } - - // Now set all of the muscle wrap flags to invalid (wrap_calced = no) - // so that the wrapping points (implicit and explicit) will be updated. -#if ! ENGINE - for (i=0; inummuscles; i++) - model->muscle[i]->wrap_calced = no; -#endif -} - - -void invalidate_joints_using_func(ModelStruct* ms, dpFunction* function) -{ - int i, j; - - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - if (ms->joint[i].dofs[j].type == function_dof && ms->joint[i].dofs[j].function == function) - { - invalidate_joint_matrix(ms, &ms->joint[i]); - break; - } - } - } -} - - -/* GET_CONVERSION: this routine returns the 4x4 transformation matrix needed - * to travel down the specified joint in the specified direction. - */ -DMatrix* get_conversion(ModelStruct* ms, int joint, Direction dir) -{ - if (joint > ms->numjoints) - { - (void)sprintf(errorbuffer, "Fatal error: joint %d not valid for model %s", joint, ms->name); - error(exit_program, errorbuffer); - } - - make_conversion(ms,joint); - - if (dir == FORWARD) - return (&ms->joint[joint].conversion.forward); - else /* if (dir == INVERSE) */ - return (&ms->joint[joint].conversion.inverse); -} - -#if ! ENGINE -GLfloat* get_float_conversion(ModelStruct* ms, int joint, Direction dir) -{ - if (joint > ms->numjoints) - { - (void)sprintf(errorbuffer, "Fatal error: joint %d not valid for model %s", joint, ms->name); - error(exit_program, errorbuffer); - } - - make_conversion(ms,joint); - - if (dir == FORWARD) - return (ms->joint[joint].conversion.gl_forward); - else /* if (dir == INVERSE) */ - return (ms->joint[joint].conversion.gl_inverse); -} -#endif - - -/* GET_GROUND_CONVERSION: this routine returns the 4x4 transformation matrix needed - * to travel between the specified segment and the ground segment. - */ - -DMatrix* get_ground_conversion(ModelStruct* ms, int seg, GroundDirection gd) -{ - if (ms->segment[seg].ground_condition == invalid) - make_ground_conversion(ms,seg); - - if (gd == from_ground) - return (&ms->segment[seg].from_ground); - else /* if (gd == to_ground) */ - return (&ms->segment[seg].to_ground); - -} - - - -/* GET_FLOAT_GROUND_CONVERSION: this routine returns the 4x4 transformation - * matrix needed to travel between the specified segment and the ground segment. - */ -#if ! ENGINE -GLfloat* get_float_ground_conversion(ModelStruct* ms, int seg, GroundDirection gd) -{ - if (ms->segment[seg].ground_condition == invalid) - make_ground_conversion(ms,seg); - - if (gd == from_ground) - return (ms->segment[seg].float_from_ground); - else /* if (gd == to_ground) */ - return (ms->segment[seg].float_to_ground); - -} -#endif - - -void make_ground_conversion(ModelStruct* ms, int seg) -{ - - int i, j, index; - - calc_transformation(ms,ms->ground_segment, seg, ms->segment[seg].from_ground); - invert_4x4transform(ms->segment[seg].from_ground, ms->segment[seg].to_ground); - -#if ! ENGINE - for (i=0,index=0; i<4; i++) - { - for (j=0; j<4; j++) - { - ms->segment[seg].float_from_ground[index] = ms->segment[seg].from_ground[i][j]; - ms->segment[seg].float_to_ground[index++] = ms->segment[seg].to_ground[i][j]; - } - } -#endif - - ms->segment[seg].ground_condition = valid; - -} - - - -/* MAKECONVERSION: this guy forms the conversion matrices which - * define the transformation between the segments in a joint. Each joint - * has a forward and an inverse matrix, so you can "travel" both ways - * across a joint. - */ - -void make_conversion(ModelStruct* ms, int joint) -{ - JointStruct* jnt = &ms->joint[joint]; - int i, j, index; - - if (jnt->conversion.condition == valid && - (jnt->pretransform_active == no || jnt->pretransform_condition == valid)) - { - return; - } - - /* if the kinematic transform is invalid and the pretransform is active, - * then we need to invalidate the pretransform as well because the change - * in kinematic transform may move the joint origin to a new location, - * thereby affecting the amount that it is deformed. - */ - if (jnt->conversion.condition == invalid && jnt->pretransform_active) - jnt->pretransform_condition = invalid; - - if (jnt->conversion.condition == invalid) - { - make_joint_conversion(ms, joint); - - if (jnt->pretransform_active) - { - /* if this segment has any deformed ancestors, the prepend their - * deforming transforms, if any: - */ - DMatrix m; - - if (jnt->pretransform_condition == invalid) - { - calc_joint_pretransform(ms, jnt); - - jnt->pretransform_condition = valid; - } - - copy_4x4matrix(jnt->conversion.forward, m); - mult_4x4matrices(jnt->pretransform, m, jnt->conversion.forward); - - copy_4x4matrix(jnt->conversion.inverse, m); - mult_4x4matrices(m, jnt->pretransform_inverse, jnt->conversion.inverse); - } - } - /* Now copy the forward and inverse matrices to float versions so they can - * be sent down the graphics pipeline via glMultMatrixf(). TODO: double - * check this code. OpenGL wants column-major matrices and IRIS GL wanted - * row-major. - */ -#if ! ENGINE - for (i=0, index=0; i<4; i++) - for (j=0; j<4; j++) - { - ms->joint[joint].conversion.gl_forward[index] = - ms->joint[joint].conversion.forward[i][j]; - ms->joint[joint].conversion.gl_inverse[index++] = - ms->joint[joint].conversion.inverse[i][j]; - } -#endif -} - - -static void make_joint_conversion(ModelStruct* ms, int joint) -{ - int i; - int order[4]; - double dofvalue[6]; - double x[4], y[4], z[4], ra1[4], ra2[4], ra3[4]; - - /* calculate the values of the 6 dof variables for this joint */ - for (i=0; i<6; i++) - dofvalue[i] = evaluate_dof(ms,&ms->joint[joint].dofs[i]); - - /* initialize the [parent] x, y, and z axes */ - COPY_1X4VECTOR(world_x,ms->joint[joint].parentframe[XX]); - COPY_1X4VECTOR(world_y,ms->joint[joint].parentframe[YY]); - COPY_1X4VECTOR(world_z,ms->joint[joint].parentframe[ZZ]); - - /* call calc_joint_transform() to make the matrices */ - - /***************** FORWARD **********************/ - - /* set the order, copy the parent frame and parent axes to other vectors */ - order[TRANS] = ms->joint[joint].order[TRANS]+1; - order[ROT1] = ms->joint[joint].order[ROT1]+1; - order[ROT2] = ms->joint[joint].order[ROT2]+1; - order[ROT3] = ms->joint[joint].order[ROT3]+1; - COPY_1X4VECTOR(ms->joint[joint].parentframe[XX],x); - COPY_1X4VECTOR(ms->joint[joint].parentframe[YY],y); - COPY_1X4VECTOR(ms->joint[joint].parentframe[ZZ],z); - COPY_1X4VECTOR(ms->joint[joint].parentrotaxes[R1],ra1); - normalize_vector(ra1, ra1); - COPY_1X4VECTOR(ms->joint[joint].parentrotaxes[R2],ra2); - normalize_vector(ra2, ra2); - COPY_1X4VECTOR(ms->joint[joint].parentrotaxes[R3],ra3); - normalize_vector(ra3, ra3); - - calc_joint_transform(order,dofvalue,ms->joint[joint].conversion.forward, - x,y,z,ra1,ra2,ra3,BF,FORWARD,&ms->joint[joint]); - - /* When the axes come back, they are transformed into the child axes, - * so store them in the child elements in the joint structure. - */ - COPY_1X4VECTOR(x,ms->joint[joint].childframe[XX]); - COPY_1X4VECTOR(y,ms->joint[joint].childframe[YY]); - COPY_1X4VECTOR(z,ms->joint[joint].childframe[ZZ]); - COPY_1X4VECTOR(ra1,ms->joint[joint].childrotaxes[R1]); - COPY_1X4VECTOR(ra2,ms->joint[joint].childrotaxes[R2]); - COPY_1X4VECTOR(ra3,ms->joint[joint].childrotaxes[R3]); - - - /***************** INVERSE **********************/ - - /* You cannot just invert the forward matrix to get the inverse matrix - * because you also need to compute the intermediate axes to store in - * the child portion of the joint structure (so the partial velocity - * routines can get them). - */ - order[TRANS] = ms->joint[joint].order[TRANS]-4; - order[ROT1] = ms->joint[joint].order[ROT1]-4; - order[ROT2] = ms->joint[joint].order[ROT2]-4; - order[ROT3] = ms->joint[joint].order[ROT3]-4; - - calc_joint_transform(order,dofvalue,ms->joint[joint].conversion.inverse, - x,y,z,ra1,ra2,ra3,BF,INVERSE,&ms->joint[joint]); - - /* and now the condition of the conversion matrices is valid */ - ms->joint[joint].conversion.condition = valid; -} - - - -/* CALC_JOINT_TRANSFORM: Makes a joint transformation matrix by concatenating four transformation - * matrices, in either a body-fixed format or space-fixed. - */ - -void calc_joint_transform(int order[], double dofs[], double rmat[][4], double xaxis[], - double yaxis[], double zaxis[], double axis1[], double axis2[], - double axis3[], int mode, Direction dir, JointStruct* joint) -{ - int i; - int absorder[4], ordersigns[4]; - double cl, sl, omc, rvec[4]; - double axis[4]; - double mat[4][4], wmat[4][4], trans_mat[4][4]; - - absorder[0] = ABS(order[0]) - 1; - absorder[1] = ABS(order[1]) - 1; - absorder[2] = ABS(order[2]) - 1; - absorder[3] = ABS(order[3]) - 1; - - ordersigns[TRANS] = SIGN(order[TRANS]); - ordersigns[ROT1] = SIGN(order[ROT1]); - ordersigns[ROT2] = SIGN(order[ROT2]); - ordersigns[ROT3] = SIGN(order[ROT3]); - - MAKE_IDENTITY_MATRIX(rmat); - MAKE_IDENTITY_MATRIX(mat); - - /* We do this loop four times, once for each transformation (one translation, - * three rotations). - */ - for (i=0; i<4; i++) - { - MAKE_IDENTITY_MATRIX(mat); - - if (i == absorder[TRANS]) - { - mat[3][0] = dofs[TX]*ordersigns[TRANS]*xaxis[0] + - dofs[TY]*ordersigns[TRANS]*yaxis[0] + - dofs[TZ]*ordersigns[TRANS]*zaxis[0]; - mat[3][1] = dofs[TX]*ordersigns[TRANS]*xaxis[1] + - dofs[TY]*ordersigns[TRANS]*yaxis[1] + - dofs[TZ]*ordersigns[TRANS]*zaxis[1]; - mat[3][2] = dofs[TX]*ordersigns[TRANS]*xaxis[2] + - dofs[TY]*ordersigns[TRANS]*yaxis[2] + - dofs[TZ]*ordersigns[TRANS]*zaxis[2]; - } - else - { - if (i == absorder[ROT1]) - { - cl = cos(DTOR*dofs[R1]*ordersigns[ROT1]); - sl = sin(DTOR*dofs[R1]*ordersigns[ROT1]); - axis[0] = axis1[0]; - axis[1] = axis1[1]; - axis[2] = axis1[2]; - axis[3] = 0.0; - } - else if (i == absorder[ROT2]) - { - cl = cos(DTOR*dofs[R2]*ordersigns[ROT2]); - sl = sin(DTOR*dofs[R2]*ordersigns[ROT2]); - axis[0] = axis2[0]; - axis[1] = axis2[1]; - axis[2] = axis2[2]; - axis[3] = 0.0; - } - else if (i == absorder[ROT3]) - { - cl = cos(DTOR*dofs[R3]*ordersigns[ROT3]); - sl = sin(DTOR*dofs[R3]*ordersigns[ROT3]); - axis[0] = axis3[0]; - axis[1] = axis3[1]; - axis[2] = axis3[2]; - axis[3] = 0.0; - } - - /* the following matrix is taken from Kane's 'Spacecraft Dynamics,' pp 6-7 */ - omc = 1.0 - cl; - mat[0][0] = cl + axis[0]*axis[0]*omc; - mat[1][0] = -axis[2]*sl + axis[0]*axis[1]*omc; - mat[2][0] = axis[1]*sl + axis[2]*axis[0]*omc; - mat[0][1] = axis[2]*sl + axis[0]*axis[1]*omc; - mat[1][1] = cl + axis[1]*axis[1]*omc; - mat[2][1] = -axis[0]*sl + axis[1]*axis[2]*omc; - mat[0][2] = -axis[1]*sl + axis[2]*axis[0]*omc; - mat[1][2] = axis[0]*sl + axis[1]*axis[2]*omc; - mat[2][2] = cl + axis[2]*axis[2]*omc; - } - - /* If the transformation is the translation, do not multiply in into - * the current working matrix (i.e. do not add it onto the stack of - * transformation matrices). Since subsequent rotations should be - * done with respect to the non-translated coordinate system, save - * the translation matrix for last, so that it is added onto the - * stack after all the rotations have been added. You want to compute - * the translation matrix in the correct order, though, so that it - * uses the "new" axes after any previous rotations. So compute it - * in order, but add it onto the end of the transformation stack. - */ - if (i != absorder[TRANS]) - { - mult_4x4matrices(rmat,mat,wmat); - copy_4x4matrix(wmat,rmat); - /* save rmat in right rotation structure (e.g., parentinterrotmat) */ - } - else - { - copy_4x4matrix(mat,trans_mat); - } - - if (mode == NBF) - continue; - - /* Now that we've done another transformation, transform the X, Y, Z, and - * lambda (rotation) axes so that the next transformation uses the new - * transformed axes (so transformations are body-fixed). - */ - mult_4x4matrix_by_vector(mat,xaxis,rvec); - COPY_1X4VECTOR(rvec,xaxis); - mult_4x4matrix_by_vector(mat,yaxis,rvec); - COPY_1X4VECTOR(rvec,yaxis); - mult_4x4matrix_by_vector(mat,zaxis,rvec); - COPY_1X4VECTOR(rvec,zaxis); - mult_4x4matrix_by_vector(mat,axis1,rvec); - COPY_1X4VECTOR(rvec,axis1); - mult_4x4matrix_by_vector(mat,axis2,rvec); - COPY_1X4VECTOR(rvec,axis2); - mult_4x4matrix_by_vector(mat,axis3,rvec); - COPY_1X4VECTOR(rvec,axis3); - - /* If we did a translation in this iteration, save the X, Y, and Z axes along - * which we translated. Save them in the joint structure for which we are - * computing the transformation matrices. If we are computing a forward matrix, - * save them in the parentinterframe element of the structure. If an inverse - * matrix, save them in the childinterframe element. - * If we did a rotation, save the lambda axis about which we just rotated. - * If we are computing a forward matrix, save it in the parentinterrotaxes - * element in the joint structure. If an inverse matrix, save it in the - * childinterrotaxes element. - */ - if (dir == FORWARD) - { - if (i == absorder[TRANS]) - { - COPY_1X4VECTOR(xaxis,joint->parentinterframe[0]); - COPY_1X4VECTOR(yaxis,joint->parentinterframe[1]); - COPY_1X4VECTOR(zaxis,joint->parentinterframe[2]); - } - else if (i == absorder[ROT1]) - COPY_1X4VECTOR(axis1,joint->parentinterrotaxes[0]) - else if (i == absorder[ROT2]) - COPY_1X4VECTOR(axis2,joint->parentinterrotaxes[1]) - else if (i == absorder[ROT3]) - COPY_1X4VECTOR(axis3,joint->parentinterrotaxes[2]) - - } - else if (dir == INVERSE) - { - if (i == absorder[TRANS]) - { - COPY_1X4VECTOR(xaxis,joint->childinterframe[0]); - COPY_1X4VECTOR(yaxis,joint->childinterframe[1]); - COPY_1X4VECTOR(zaxis,joint->childinterframe[2]); - } - else if (i == absorder[ROT1]) - COPY_1X4VECTOR(axis1,joint->childinterrotaxes[0]) - else if (i == absorder[ROT2]) - COPY_1X4VECTOR(axis2,joint->childinterrotaxes[1]) - else if (i == absorder[ROT3]) - COPY_1X4VECTOR(axis3,joint->childinterrotaxes[2]) - } - } - - /* You can now add the translation matrix onto the end (or beginning if INVERSE) */ - if (dir == FORWARD) - mult_4x4matrices(rmat,trans_mat,wmat); - else - mult_4x4matrices(trans_mat,rmat,wmat); - - copy_4x4matrix(wmat,rmat); -} - - - -void calc_transformation(ModelStruct* ms, int from, int to, DMatrix mat) -{ - - int i; - int* path; - DMatrix* jmat; - DMatrix workmat1, workmat2; - - path = GET_PATH(ms->modelnum,from,to); - - reset_4x4matrix(workmat1); - - for (i=0; path[i] != ms->numjoints+1; i++) - { - if (path[i] > 0) - jmat = get_conversion(ms,path[i]-1,INVERSE); - else - jmat = get_conversion(ms,-path[i]-1,FORWARD); - mult_4x4matrices(workmat1,*jmat,workmat2); - copy_4x4matrix(workmat2,workmat1); - } - - copy_4x4matrix(workmat2,mat); - -} diff --git a/OpenSim/Utilities/simmToOpenSim/copy.c b/OpenSim/Utilities/simmToOpenSim/copy.c deleted file mode 100644 index a1c5e01048..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/copy.c +++ /dev/null @@ -1,1950 +0,0 @@ -/******************************************************************************* - - COPY.C - - Author: Peter Loan - - Date: 17-MAY-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines that copy values - in some structure or array to another structure or array. - - Routines: - copy_muscle : copies an individual muscle structure - copy_default_muscle : copies one default muscle structure to another - copy_4x4matrix : copies a 4x4 matrix - copy_1x4vector : copies a 1x4 array (vector) - copy_dof : copies a REFEQ structure which holds a dof - copy_function : copies a function's points and coefficients - copy_mps : copies a dpMusclePoint array - copy_nndouble : mallocs and copies a double if 'from' is not null - copy_nddouble : mallocs and copies a double if 'from' is not default - copy_nnint : mallocs and copies a int if 'from' is not null - copy_ndint : mallocs and copies a int if 'from' is not default - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "defunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static ReturnCode copy_nondefault_dyn_params(dpMuscleStruct* from, dpMuscleStruct* to, - dpMuscleStruct* deffrom, dpMuscleStruct* defto); -static ReturnCode copy_nonnull_dyn_params(dpMuscleStruct* from, dpMuscleStruct* to); -static MotionSequence* copy_motion(MotionSequence* from); -static dpQStruct* get_q_from_gencoord_dp(dpModelStruct* model, GeneralizedCoord *genc); -static ReturnCode copy_muscle_path_dp(dpMusclePathStruct *from, dpMusclePathStruct *to, dpModelStruct *modelTo); -static ReturnCode copy_muscle_point_dp(dpMusclePoint *from, dpMusclePoint *to, dpModelStruct *modelTo); - - -void copy_1x4vector(double from[], double to[]) -{ - to[0] = from[0]; - to[1] = from[1]; - to[2] = from[2]; - to[3] = from[3]; -} - - -void copy_point(dpCoord3D *from, dpCoord3D *to) -{ - to->xyz[0] = from->xyz[0]; - to->xyz[1] = from->xyz[1]; - to->xyz[2] = from->xyz[2]; -} - -/* COPY_DOF: copies a dof. Used to save and restore dofs in the joint - * editor and to copy dofs from the model-file-reading buffer to a - * model structure. - */ -void copy_dof(DofStruct* from, DofStruct* to) -{ - to->type = from->type; - to->value = from->value; - to->function = from->function; - to->gencoord = from->gencoord; -} - - -void copy_gencoord_info(GeneralizedCoord *from, SaveGencoords *to) -{ - to->value = from->value; - to->velocity = from->velocity; - to->tolerance = from->tolerance; - to->pd_stiffness = from->pd_stiffness; - to->default_value = from->default_value; - to->clamped = from->clamped; - to->locked = from->locked; - to->restraintFuncActive = from->restraintFuncActive; - to->range = from->range; - to->restraint_function = from->restraint_function; - to->used_in_model = from->used_in_model; -} - - -/* COPY_FUNCTION: copies a function structure. This structure contains - * the original (x,y) data points and the coefficients (b,c,d) of - * the function interpolants. It assumes that space for the x, y, a, b, c - * arrays has already been allocated, and the size of this space - * is stored in to->coefficient_array_size. - */ -void copy_function(dpFunction* from, dpFunction* to) -{ - int numToCopy = _MIN(to->coefficient_array_size, from->coefficient_array_size); - - to->type = from->type; - to->numpoints = from->numpoints; - to->source = from->source; - to->status = from->status; - to->used = from->used; - to->usernum = from->usernum; - to->cutoff_frequency = from->cutoff_frequency; - - memcpy(to->x, from->x, numToCopy * sizeof(double)); - memcpy(to->y, from->y, numToCopy * sizeof(double)); - memcpy(to->b, from->b, numToCopy * sizeof(double)); - memcpy(to->c, from->c, numToCopy * sizeof(double)); - memcpy(to->d, from->d, numToCopy * sizeof(double)); -} - -/* COPY_MUSCLE_PATH: copy the muscle paths - all the original points (mp_orig), - * and the active points (mp), from one path struct to another. - */ -ReturnCode copy_muscle_path(dpMusclePathStruct *from, dpMusclePathStruct *to) -{ - int i; - - if (from == NULL || to == NULL) - return code_bad; - - to->num_orig_points = from->num_orig_points; - to->mp_orig_array_size = to->num_orig_points; - to->mp_orig = (dpMusclePoint*)simm_malloc(to->mp_orig_array_size * sizeof(dpMusclePoint)); - for (i=0; inum_orig_points; i++) - { - if (copy_muscle_point(&from->mp_orig[i], &to->mp_orig[i]) == code_bad) - return code_bad; - } - - to->num_points = 0; - to->mp_array_size = from->mp_array_size; - to->mp = (dpMusclePoint**)simm_calloc(to->mp_array_size, sizeof(dpMusclePoint*)); - if (to->mp == NULL) - return code_bad; - - return code_fine; -} - -/* COPY_MUSCLE_POINT: copy the individual point from a path - * Allocate space for the wrapping points - */ -ReturnCode copy_muscle_point(dpMusclePoint* from, dpMusclePoint* to) -{ - if (from == NULL || to == NULL) - return code_bad; - - memcpy(to, from, sizeof(dpMusclePoint)); - - if (to->num_wrap_pts > 0) - { - to->wrap_pts = (double*)simm_malloc(to->num_wrap_pts*3*sizeof(double)); - if (to->wrap_pts == NULL) - return code_bad; - memcpy(to->wrap_pts, from->wrap_pts, to->num_wrap_pts * 3 * sizeof(double)); - } - - return code_fine; -} - -/* COPY_MUSCLE_PATH_DP: copy the muscle paths - all the original points (mp_orig), - * and the active points (mp), from one path struct to another that will be used by - * a Dynamics Pipeline model. - */ -static ReturnCode copy_muscle_path_dp(dpMusclePathStruct *from, dpMusclePathStruct *to, dpModelStruct *modelTo) -{ - int i; - - if (from == NULL || to == NULL) - return code_bad; - - to->num_orig_points = from->num_orig_points; - to->mp_orig_array_size = to->num_orig_points; - to->mp_orig = (dpMusclePoint*)simm_malloc(to->mp_orig_array_size * sizeof(dpMusclePoint)); - for (i=0; inum_orig_points; i++) - { - if (copy_muscle_point_dp(&from->mp_orig[i], &to->mp_orig[i], modelTo) == code_bad) - return code_bad; - } - - to->num_points = 0; - to->mp_array_size = from->mp_array_size; - to->mp = (dpMusclePoint**)simm_calloc(to->mp_array_size, sizeof(dpMusclePoint*)); - if (to->mp == NULL) - return code_bad; - - return code_fine; -} - -/* COPY_MUSCLE_POINT_DP: copy the individual point from a path. Since destination - * model is a dynamics Pipeline model, don't want to store gencoord information in - * GeneralizedCoord struct. Find the equivalent QStruct and store that. - * Allocate space for the wrapping points. - */ -static ReturnCode copy_muscle_point_dp(dpMusclePoint* from, dpMusclePoint* to, dpModelStruct *modelTo) -{ - int i; - - if (from == NULL || to == NULL) - return code_bad; - - memcpy(to, from, sizeof(dpMusclePoint)); - - if (to->num_wrap_pts > 0) - { - to->wrap_pts = (double*)simm_malloc(to->num_wrap_pts*3*sizeof(double)); - if (to->wrap_pts == NULL) - return code_bad; - memcpy(to->wrap_pts, from->wrap_pts, to->num_wrap_pts * 3 * sizeof(double)); - } - - if (to->isVia) - to->viaRange.gencoord = (void *)get_q_from_gencoord_dp(modelTo, (GeneralizedCoord*)from->viaRange.gencoord); - - if (to->isMovingPoint) - { - for (i = 0; i < 3; i++) - { - if (from->gencoord[i] != NULL) - to->gencoord[i] = (void *)get_q_from_gencoord_dp(modelTo, (GeneralizedCoord*)from->gencoord[i]); - } - } - return code_fine; -} - -/* COPY_DEFAULT_MUSCLE: copy the default muscle to another structure used by a SIMM model. */ -ReturnCode copy_default_muscle(dpMuscleStruct* from, dpMuscleStruct* to, ModelStruct* modelTo) -{ - int i; - - if (from == NULL || to == NULL) - return code_fine; - - if (from->name == NULL) - to->name = NULL; - else - mstrcpy(&to->name,from->name); - - to->index = from->index; - - // The default muscle cannot contain path information. - to->path = NULL; - - to->nummomentarms = 0; - to->momentarms = NULL; - - if (copy_nonnull_dyn_params(from, to) == code_bad) - return code_bad; - - to->numgroups = from->numgroups; - if (to->numgroups == 0) - to->group = NULL; - else - { - if ((to->group = (int*)simm_malloc((to->numgroups)*sizeof(int))) == NULL) - return code_bad; - for (i=0; i<(to->numgroups); i++) - to->group[i] = from->group[i]; - } - - if (copy_nndouble(from->max_isometric_force,&to->max_isometric_force) == code_bad) - return code_bad; - if (copy_nndouble(from->pennation_angle,&to->pennation_angle) == code_bad) - return code_bad; - if (copy_nndouble(from->min_thickness,&to->min_thickness) == code_bad) - return code_bad; - if (copy_nndouble(from->max_thickness,&to->max_thickness) == code_bad) - return code_bad; - if (copy_nndouble(from->max_contraction_vel,&to->max_contraction_vel) == code_bad) - return code_bad; - - if (copy_nndouble(from->optimal_fiber_length,&to->optimal_fiber_length) == code_bad) - return code_bad; - if (copy_nndouble(from->resting_tendon_length,&to->resting_tendon_length) == code_bad) - return code_bad; - - if (copy_nnint(from->min_material,&to->min_material) == code_bad) - return code_bad; - if (copy_nnint(from->max_material,&to->max_material) == code_bad) - return code_bad; - - to->dynamic_activation = from->dynamic_activation; - - if (from->active_force_len_func == NULL) - to->active_force_len_func = NULL; - else - { - to->active_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->active_force_len_func == NULL) - return code_bad; - *to->active_force_len_func = *from->active_force_len_func; - } - - if (from->passive_force_len_func == NULL) - to->passive_force_len_func = NULL; - else - { - to->passive_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->passive_force_len_func == NULL) - return code_bad; - *to->passive_force_len_func = *from->passive_force_len_func; - } - - if (from->tendon_force_len_func == NULL) - to->tendon_force_len_func = NULL; - else - { - to->tendon_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->tendon_force_len_func == NULL) - return code_bad; - *to->tendon_force_len_func = *from->tendon_force_len_func; - } - - if (from->force_vel_func == NULL) - to->force_vel_func = NULL; - else - { - to->force_vel_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->force_vel_func == NULL) - return code_bad; - *to->force_vel_func = *from->force_vel_func; - } - - if (from->excitation_abscissa == NULL) - { - to->excitation_abscissa = from->excitation_abscissa; - } - else - { - to->excitation_abscissa = (void**)simm_malloc(sizeof(void*)); - if (to->excitation_abscissa == NULL) - return code_bad; - if (*from->excitation_abscissa == TIME) - *to->excitation_abscissa = TIME; - else - { - *to->excitation_abscissa = (void*)enter_gencoord(modelTo, ((GeneralizedCoord*)(*from->excitation_abscissa))->name, no); - // If you got a NULL in the destination, but the source has an abscissa, something went wrong - if (*to->excitation_abscissa == NULL && *from->excitation_abscissa != NULL) - return code_bad; - } - - } - - if (from->excitation_func == NULL) - to->excitation_func = NULL; - else - { - to->excitation_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->excitation_func == NULL) - return code_bad; - *to->excitation_func = *from->excitation_func; - } - - if (copy_nnint(from->muscle_model_index, &to->muscle_model_index) == code_bad) - return code_bad; - - // Not used in SIMM. - to->numStateParams = 0; - to->stateParams = NULL; - - return code_fine; -} - -/* COPY_DEFAULT_MUSCLE_DP : copy the default muscle to another muscle structure that - * will be used by a Dynamics Pipeline model. If the default muscle has an excitation - * function defined with an abscissa other than time, the gencoord should not be stored - * as a GeneralizedCoord struct, but as a QStruct. - */ -ReturnCode copy_default_muscle_dp(dpMuscleStruct* from, dpMuscleStruct* to, dpModelStruct* modelTo) -{ - int i; - - if (from == NULL || to == NULL) - return code_fine; - - if (from->name == NULL) - to->name = NULL; - else - mstrcpy(&to->name,from->name); - - to->index = from->index; - - // The default muscle cannot contain path information. - to->path = NULL; - - to->nummomentarms = 0; - to->momentarms = NULL; - - if (copy_nonnull_dyn_params(from, to) == code_bad) - return code_bad; - - to->numgroups = from->numgroups; - if (to->numgroups == 0) - to->group = NULL; - else - { - if ((to->group = (int*)simm_malloc((to->numgroups)*sizeof(int))) == NULL) - return code_bad; - for (i=0; i<(to->numgroups); i++) - to->group[i] = from->group[i]; - } - - if (copy_nndouble(from->max_isometric_force,&to->max_isometric_force) == code_bad) - return code_bad; - if (copy_nndouble(from->pennation_angle,&to->pennation_angle) == code_bad) - return code_bad; - if (copy_nndouble(from->min_thickness,&to->min_thickness) == code_bad) - return code_bad; - if (copy_nndouble(from->max_thickness,&to->max_thickness) == code_bad) - return code_bad; - if (copy_nndouble(from->max_contraction_vel,&to->max_contraction_vel) == code_bad) - return code_bad; - - if (copy_nndouble(from->optimal_fiber_length,&to->optimal_fiber_length) == code_bad) - return code_bad; - if (copy_nndouble(from->resting_tendon_length,&to->resting_tendon_length) == code_bad) - return code_bad; - - if (copy_nnint(from->min_material,&to->min_material) == code_bad) - return code_bad; - if (copy_nnint(from->max_material,&to->max_material) == code_bad) - return code_bad; - - to->dynamic_activation = from->dynamic_activation; - - if (from->active_force_len_func == NULL) - to->active_force_len_func = NULL; - else - { - to->active_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->active_force_len_func == NULL) - return code_bad; - *to->active_force_len_func = *from->active_force_len_func; - } - - if (from->passive_force_len_func == NULL) - to->passive_force_len_func = NULL; - else - { - to->passive_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->passive_force_len_func == NULL) - return code_bad; - *to->passive_force_len_func = *from->passive_force_len_func; - } - - if (from->tendon_force_len_func == NULL) - to->tendon_force_len_func = NULL; - else - { - to->tendon_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->tendon_force_len_func == NULL) - return code_bad; - *to->tendon_force_len_func = *from->tendon_force_len_func; - } - - if (from->force_vel_func == NULL) - to->force_vel_func = NULL; - else - { - to->force_vel_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->force_vel_func == NULL) - return code_bad; - *to->force_vel_func = *from->force_vel_func; - } - - if (from->excitation_abscissa == NULL) - { - to->excitation_abscissa = from->excitation_abscissa; - } - else - { - to->excitation_abscissa = (void**)simm_malloc(sizeof(void*)); - if (to->excitation_abscissa == NULL) - return code_bad; - if (*from->excitation_abscissa == TIME) - *to->excitation_abscissa = TIME; - else - { - *to->excitation_abscissa = (void*)get_q_from_gencoord_dp(modelTo, (GeneralizedCoord*)(*from->excitation_abscissa)); - // If you got a NULL in the destination, but the source has an abscissa, something went wrong - if (*to->excitation_abscissa == NULL && *from->excitation_abscissa != NULL) - return code_bad; - } - } - - if (from->excitation_func == NULL) - to->excitation_func = NULL; - else - { - to->excitation_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (to->excitation_func == NULL) - return code_bad; - *to->excitation_func = *from->excitation_func; - } - - if (copy_nnint(from->muscle_model_index, &to->muscle_model_index) == code_bad) - return code_bad; - - // Not used in SIMM. - to->numStateParams = 0; - to->stateParams = NULL; - - return code_fine; -} - -/* COPY_MUSCLE: copy a muscle to another struct used by a SIMM model. */ -ReturnCode copy_muscle(dpMuscleStruct* from, dpMuscleStruct* to, dpMuscleStruct* deffrom, dpMuscleStruct *defto, ModelStruct* modelTo) -{ - int i, j, mem_size; - - memcpy(to, from, sizeof(dpMuscleStruct)); - -#if MEMORY_LEAK - if (from->name == deffrom->name) - to->name = deffrom->name; - else - { - mstrcpy(&to->name,from->name); - if (to->name == NULL) - return code_bad; - } -#else - if (from->name == deffrom->name) - to->name = deffrom->name; - else if (mstrcpy(&to->name,from->name) == code_bad) - return code_bad; -#endif - - if (to->numWrapStructs > 0) - { - if ((to->wrapStruct = (dpMuscleWrapStruct**)simm_malloc(to->numWrapStructs * sizeof(dpMuscleWrapStruct*))) == NULL) - return code_bad; - for (i = 0; i < to->numWrapStructs; i++) - { - if ((to->wrapStruct[i] = (dpMuscleWrapStruct*)simm_malloc(sizeof(dpMuscleWrapStruct))) == NULL) - return code_bad; - memcpy(to->wrapStruct[i], from->wrapStruct[i], sizeof(dpMuscleWrapStruct)); - for (j = 0; j < 2; j++) - { - if (from->wrapStruct[i]->mp_wrap[j].num_wrap_pts > 0) - { - to->wrapStruct[i]->mp_wrap[j].num_wrap_pts = from->wrapStruct[i]->mp_wrap[j].num_wrap_pts; - mem_size = to->wrapStruct[i]->mp_wrap[j].num_wrap_pts * 3 * sizeof(double); - to->wrapStruct[i]->mp_wrap[j].wrap_pts = (double*)simm_malloc(mem_size); - memcpy(to->wrapStruct[i]->mp_wrap[j].wrap_pts, from->wrapStruct[i]->mp_wrap[j].wrap_pts, mem_size); - } - else - { - to->wrapStruct[i]->mp_wrap[j].num_wrap_pts = 0; - to->wrapStruct[i]->mp_wrap[j].wrap_pts = NULL; - } - to->wrapStruct[i]->mp_wrap[j].isVia = from->wrapStruct[i]->mp_wrap[j].isVia; - to->wrapStruct[i]->mp_wrap[j].viaRange = from->wrapStruct[i]->mp_wrap[j].viaRange; - - } - } - } - - if (from->path) - { - to->path = (dpMusclePathStruct*)simm_calloc(1, sizeof(dpMusclePathStruct)); - //free_muscle(to, defto); // just to test! - - if (copy_muscle_path(from->path, to->path) == code_bad) - return code_bad; - } - to->wrap_calced = no; - - if (to->numgroups == 0) - to->group = NULL; - else - { - if ((to->group = (int*)simm_malloc(to->numgroups*sizeof(int))) == NULL) - return code_bad; - for (i=0; inumgroups; i++) - to->group[i] = from->group[i]; - } - - if (copy_nddouble(from->max_isometric_force,&to->max_isometric_force,deffrom->max_isometric_force,defto->max_isometric_force) == code_bad) - return code_bad; - if (copy_nddouble(from->pennation_angle,&to->pennation_angle, - deffrom->pennation_angle,defto->pennation_angle) == code_bad) - return code_bad; - if (copy_nddouble(from->min_thickness,&to->min_thickness, - deffrom->min_thickness,defto->min_thickness) == code_bad) - return code_bad; - if (copy_nddouble(from->max_thickness,&to->max_thickness, - deffrom->max_thickness,defto->max_thickness) == code_bad) - return code_bad; - if (copy_nddouble(from->optimal_fiber_length,&to->optimal_fiber_length, - deffrom->optimal_fiber_length,defto->optimal_fiber_length) == code_bad) - return code_bad; - if (copy_nddouble(from->resting_tendon_length,&to->resting_tendon_length, - deffrom->resting_tendon_length,defto->resting_tendon_length) == code_bad) - return code_bad; - - if (copy_ndint(from->min_material, &to->min_material, deffrom->min_material, defto->min_material) == code_bad) - return code_bad; - - if (copy_ndint(from->max_material, &to->max_material, deffrom->max_material, defto->max_material) == code_bad) - return code_bad; - - if (copy_nddouble(from->max_contraction_vel,&to->max_contraction_vel, - deffrom->max_contraction_vel,defto->max_contraction_vel) == code_bad) - return code_bad; - - if (copy_nondefault_dyn_params(from,to,deffrom,defto) == code_bad) - return code_bad; - - if ((to->momentarms = (double*)simm_malloc(to->nummomentarms*sizeof(double))) == NULL) - return code_bad; - for (i=0; inummomentarms; i++) - to->momentarms[i] = from->momentarms[i]; - - if (from->active_force_len_func == deffrom->active_force_len_func) - to->active_force_len_func = defto->active_force_len_func; - else - { - if (from->active_force_len_func == NULL) - to->active_force_len_func = NULL; - else - { - if ((to->active_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->active_force_len_func = *from->active_force_len_func; - } - } - - if (from->passive_force_len_func == deffrom->passive_force_len_func) - to->passive_force_len_func = defto->passive_force_len_func; - else - { - if (from->passive_force_len_func == NULL) - to->passive_force_len_func = NULL; - else - { - if ((to->passive_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->passive_force_len_func = *from->passive_force_len_func; - } - } - - if (from->tendon_force_len_func == deffrom->tendon_force_len_func) - to->tendon_force_len_func = defto->tendon_force_len_func; - else - { - if (from->tendon_force_len_func == NULL) - to->tendon_force_len_func = NULL; - else - { - if ((to->tendon_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->tendon_force_len_func = *from->tendon_force_len_func; - } - } - - if (from->force_vel_func == deffrom->force_vel_func) - to->force_vel_func = defto->force_vel_func; - else - { - if (from->force_vel_func == NULL) - to->force_vel_func = NULL; - else - { - if ((to->force_vel_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->force_vel_func = *from->force_vel_func; - } - } - - if (from->excitation_abscissa == deffrom->excitation_abscissa) - to->excitation_abscissa = defto->excitation_abscissa; - else - { - if (from->excitation_abscissa == NULL) - { - to->excitation_abscissa = NULL; - } - else - { - if ((to->excitation_abscissa = (void**)simm_malloc(sizeof(void*))) == NULL) - return code_bad; - if (*from->excitation_abscissa == TIME) - *to->excitation_abscissa = TIME; - else - { - *to->excitation_abscissa = (void*)enter_gencoord(modelTo, ((GeneralizedCoord*)(*from->excitation_abscissa))->name, no); - // If you got a NULL in the destination, but the source has an abscissa, something went wron - if (*to->excitation_abscissa == NULL && *from->excitation_abscissa != NULL) - return code_bad; - } - } - } - - if (from->excitation_func == deffrom->excitation_func) - to->excitation_func = defto->excitation_func; - else - { - if (from->excitation_func == NULL) - to->excitation_func = NULL; - else - { - if ((to->excitation_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->excitation_func = *from->excitation_func; - } - } - - if (copy_ndint(from->muscle_model_index,&to->muscle_model_index, - deffrom->muscle_model_index,defto->muscle_model_index) == code_bad) - return code_bad; - - to->saved = NULL; - - // Not used in SIMM. - to->numStateParams = 0; - to->stateParams = NULL; - - return code_fine; -} - -/* COPY_MUSCLE_DP: copy the muscle into a structure that will be used by a Dynamics Pipeline model. - * All references to gencoords in the muscle (via points, moving muscle points, excitation abscissae), - * should refer to the QSTruct, not the GeneralizedCoord struct. - */ -ReturnCode copy_muscle_dp(dpMuscleStruct* from, dpMuscleStruct* to, dpMuscleStruct* deffrom, dpMuscleStruct *defto, dpModelStruct* modelTo) -{ - int i, j, mem_size; - - memcpy(to, from, sizeof(dpMuscleStruct)); - -#if MEMORY_LEAK - if (from->name == deffrom->name) - to->name = deffrom->name; - else - { - mstrcpy(&to->name,from->name); - if (to->name == NULL) - return code_bad; - } -#else - if (from->name == deffrom->name) - to->name = deffrom->name; - else if (mstrcpy(&to->name,from->name) == code_bad) - return code_bad; -#endif - - if (to->numWrapStructs > 0) - { - if ((to->wrapStruct = (dpMuscleWrapStruct**)simm_malloc(to->numWrapStructs * sizeof(dpMuscleWrapStruct*))) == NULL) - return code_bad; - for (i = 0; i < to->numWrapStructs; i++) - { - if ((to->wrapStruct[i] = (dpMuscleWrapStruct*)simm_malloc(sizeof(dpMuscleWrapStruct))) == NULL) - return code_bad; - memcpy(to->wrapStruct[i], from->wrapStruct[i], sizeof(dpMuscleWrapStruct)); - // need to allocate space for wrap_object otherwise element in DP model points to element - // in SIMM model. Changes to DP model (adjusting segment index for SD segments) - // cause unwanted changes in SIMM model - // find the wrap object in dp model... -// to->wrapStruct[i]->wrap_object = (dpWrapObject *) simm_malloc(sizeof(dpWrapObject)); - for (j = 0; j < modelTo->num_wrap_objects; j++) - { - if (STRINGS_ARE_EQUAL(from->wrapStruct[i]->wrap_object->name, modelTo->wrap_object[j].name)) - { - to->wrapStruct[i]->wrap_object = &modelTo->wrap_object[j]; - } - } - for (j = 0; j < 2; j++) - { - if (from->wrapStruct[i]->mp_wrap[j].num_wrap_pts > 0) - { - to->wrapStruct[i]->mp_wrap[j].num_wrap_pts = from->wrapStruct[i]->mp_wrap[j].num_wrap_pts; - mem_size = to->wrapStruct[i]->mp_wrap[j].num_wrap_pts * 3 * sizeof(double); - to->wrapStruct[i]->mp_wrap[j].wrap_pts = (double*)simm_malloc(mem_size); - memcpy(to->wrapStruct[i]->mp_wrap[j].wrap_pts, from->wrapStruct[i]->mp_wrap[j].wrap_pts, mem_size); - } - else - { - to->wrapStruct[i]->mp_wrap[j].num_wrap_pts = 0; - to->wrapStruct[i]->mp_wrap[j].wrap_pts = NULL; - } - to->wrapStruct[i]->mp_wrap[j].isVia = from->wrapStruct[i]->mp_wrap[j].isVia; - to->wrapStruct[i]->mp_wrap[j].viaRange = from->wrapStruct[i]->mp_wrap[j].viaRange; - } - } - } - - if (from->path) - { - to->path = (dpMusclePathStruct*)simm_calloc(1, sizeof(dpMusclePathStruct)); - //free_muscle(to, defto); // just to test! - - if (copy_muscle_path_dp(from->path, to->path, modelTo) == code_bad) - return code_bad; - } - to->wrap_calced = no; - - if (to->numgroups == 0) - to->group = NULL; - else - { - if ((to->group = (int*)simm_malloc(to->numgroups*sizeof(int))) == NULL) - return code_bad; - for (i=0; inumgroups; i++) - to->group[i] = from->group[i]; - } - - if (copy_nddouble(from->max_isometric_force,&to->max_isometric_force,deffrom->max_isometric_force,defto->max_isometric_force) == code_bad) - return code_bad; - if (copy_nddouble(from->pennation_angle,&to->pennation_angle, - deffrom->pennation_angle,defto->pennation_angle) == code_bad) - return code_bad; - if (copy_nddouble(from->min_thickness,&to->min_thickness, - deffrom->min_thickness,defto->min_thickness) == code_bad) - return code_bad; - if (copy_nddouble(from->max_thickness,&to->max_thickness, - deffrom->max_thickness,defto->max_thickness) == code_bad) - return code_bad; - if (copy_nddouble(from->optimal_fiber_length,&to->optimal_fiber_length, - deffrom->optimal_fiber_length,defto->optimal_fiber_length) == code_bad) - return code_bad; - if (copy_nddouble(from->resting_tendon_length,&to->resting_tendon_length, - deffrom->resting_tendon_length,defto->resting_tendon_length) == code_bad) - return code_bad; - - if (copy_ndint(from->min_material, &to->min_material, deffrom->min_material, defto->min_material) == code_bad) - return code_bad; - - if (copy_ndint(from->max_material, &to->max_material, deffrom->max_material, defto->max_material) == code_bad) - return code_bad; - - if (copy_nddouble(from->max_contraction_vel,&to->max_contraction_vel, - deffrom->max_contraction_vel,defto->max_contraction_vel) == code_bad) - return code_bad; - - if (copy_nondefault_dyn_params(from,to,deffrom,defto) == code_bad) - return code_bad; - - if ((to->momentarms = (double*)simm_malloc(to->nummomentarms*sizeof(double))) == NULL) - return code_bad; - for (i=0; inummomentarms; i++) - to->momentarms[i] = from->momentarms[i]; - - if (from->active_force_len_func == deffrom->active_force_len_func) - to->active_force_len_func = defto->active_force_len_func; - else - { - if (from->active_force_len_func == NULL) - to->active_force_len_func = NULL; - else - { - if ((to->active_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->active_force_len_func = *from->active_force_len_func; - } - } - - if (from->passive_force_len_func == deffrom->passive_force_len_func) - to->passive_force_len_func = defto->passive_force_len_func; - else - { - if (from->passive_force_len_func == NULL) - to->passive_force_len_func = NULL; - else - { - if ((to->passive_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->passive_force_len_func = *from->passive_force_len_func; - } - } - - if (from->tendon_force_len_func == deffrom->tendon_force_len_func) - to->tendon_force_len_func = defto->tendon_force_len_func; - else - { - if (from->tendon_force_len_func == NULL) - to->tendon_force_len_func = NULL; - else - { - if ((to->tendon_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->tendon_force_len_func = *from->tendon_force_len_func; - } - } - - if (from->force_vel_func == deffrom->force_vel_func) - to->force_vel_func = defto->force_vel_func; - else - { - if (from->force_vel_func == NULL) - to->force_vel_func = NULL; - else - { - if ((to->force_vel_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->force_vel_func = *from->force_vel_func; - } - } - - if (from->excitation_abscissa == deffrom->excitation_abscissa) - to->excitation_abscissa = defto->excitation_abscissa; - else - { - if (from->excitation_abscissa == NULL) - { - to->excitation_abscissa = NULL; - } - else - { - if ((to->excitation_abscissa = (void**)simm_malloc(sizeof(void*))) == NULL) - return code_bad; - if (*from->excitation_abscissa == TIME) - *to->excitation_abscissa = TIME; - else - { - *to->excitation_abscissa = (void*)get_q_from_gencoord_dp(modelTo, (GeneralizedCoord*)(*from->excitation_abscissa)); - // If you got a NULL in the destination, but the source has an abscissa, something went wron - if (*to->excitation_abscissa == NULL && *from->excitation_abscissa != NULL) - return code_bad; - } - } - } - - if (from->excitation_func == deffrom->excitation_func) - to->excitation_func = defto->excitation_func; - else - { - if (from->excitation_func == NULL) - to->excitation_func = NULL; - else - { - if ((to->excitation_func = (dpFunction**)simm_malloc(sizeof(dpFunction*))) == NULL) - return code_bad; - *to->excitation_func = *from->excitation_func; - } - } - - if (copy_ndint(from->muscle_model_index,&to->muscle_model_index, - deffrom->muscle_model_index,defto->muscle_model_index) == code_bad) - return code_bad; - - to->saved = NULL; - - // Not used in SIMM. - to->numStateParams = 0; - to->stateParams = NULL; - - return code_fine; -} - - -ReturnCode copy_nndouble(double* from, double** to) -{ - if (from == NULL) - *to = NULL; - else - { - if ((*to = (double*)simm_malloc(sizeof(double))) == NULL) - return code_bad; - **to = *from; - } - - return code_fine; -} - - -ReturnCode copy_nddouble(double* from, double** to, double* deffrom, double* defto) -{ - if (from == deffrom) - *to = defto; - else - { - if (from == NULL) - *to = NULL; - else - { - if ((*to = (double*)simm_malloc(sizeof(double))) == NULL) - return code_bad; - **to = *from; - } - } - - return code_fine; -} - - -ReturnCode copy_nnint(int* from, int** to) -{ - if (from == NULL) - *to = NULL; - else - { - if ((*to = (int*)simm_malloc(sizeof(int))) == NULL) - return code_bad; - **to = *from; - } - - return code_fine; -} - - -ReturnCode copy_ndint(int* from, int** to, int* deffrom, int* defto) -{ - if (from == deffrom) - *to = defto; - else - { - if (from == NULL) - *to = NULL; - else - { - if ((*to = (int*)simm_malloc(sizeof(int))) == NULL) - return code_bad; - **to = *from; - } - } - - return code_fine; -} - -WorldObject* copy_world_objects(WorldObject from[], int num) -{ - int i; - WorldObject* to = NULL; - - if (num <= 0) - return to; - - to = (WorldObject*)simm_malloc(sizeof(WorldObject)*num); - memcpy(to, from, sizeof(WorldObject)*num); - - for (i=0; iusernum); - if (from[i].dofs[j].gencoord) - to[i].dofs[j].gencoord = enter_gencoord(model, from[i].dofs[j].gencoord->name, no); - } - if (model->numsegments > 0) - { - to[i].in_seg_ground_path = (SBoolean*)simm_malloc(model->numsegments*sizeof(SBoolean)); - memcpy(to[i].in_seg_ground_path, from[i].in_seg_ground_path, model->numsegments*sizeof(SBoolean)); - } -#if INCLUDE_MOCAP_MODULE - mstrcpy(&to[i].mocap_segment, from[i].mocap_segment); -#endif - } - - return to; -} - -SegmentStruct* copy_segments(SegmentStruct from[], int num) -{ - int i, j; - SegmentStruct* to = NULL; - - if (num <= 0) - return to; - - to = (SegmentStruct*)simm_malloc(sizeof(SegmentStruct)*num); - memcpy(to, from, sizeof(SegmentStruct)*num); - - for (i=0; i 0) - { - to[i].bone = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)*to[i].numBones); - for (j=0; j 0) - { - to[i].group = (int*)simm_malloc(sizeof(int)*to[i].numgroups); - memcpy(to[i].group, from[i].group, sizeof(int)*to[i].numgroups); - } - if (from[i].numSpringPoints > 0) - { - to[i].springPoint = (SpringPoint*)simm_malloc(sizeof(SpringPoint)*to[i].numSpringPoints); - memcpy(to[i].springPoint, from[i].springPoint, sizeof(SpringPoint)*to[i].numSpringPoints); - for (j=0; jname, from[i].springFloor->name); - mstrcpy(&to[i].springFloor->filename, from[i].springFloor->filename); - if (from[i].springFloor->poly != NULL) - { - to[i].springFloor->poly = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - copy_polyhedron(from[i].springFloor->poly, to[i].springFloor->poly); - } - to[i].springFloor->numPoints = 0; - to[i].springFloor->points = NULL; - } - if (from[i].numContactObjects > 0) - { - to[i].contactObject = (ContactObject*)simm_malloc(sizeof(ContactObject)*to[i].numContactObjects); - memcpy(to[i].contactObject, from[i].contactObject, sizeof(ContactObject)*to[i].numContactObjects); - for (j=0; jname, from[i].forceMatte->name); - mstrcpy(&to[i].forceMatte->filename, from[i].forceMatte->filename); - if (from[i].forceMatte->poly != NULL) - { - to[i].forceMatte->poly = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - copy_polyhedron(from[i].forceMatte->poly, to[i].forceMatte->poly); - } - } - if (from[i].numMarkers > 0) - { - to[i].marker = (Marker**)simm_malloc(sizeof(Marker*) * from[i].numMarkers); - for (j=0; jname, from[i].marker[j]->name); - } - } - else - { - // from[i].marker may be non-NULL even if from[i] has no markers. - // So set to[i].marker to NULL so it doesn't keep a copy of the pointer. - to[i].marker = NULL; - } - to[i].markerArraySize = from[i].numMarkers; -#if INCLUDE_BONE_EDITOR_EXTRAS - to[i].pts_file = NULL; - to[i].raw_vertices = NULL; -#endif - if (from[i].num_deforms > 0) - { - to[i].deform = (DeformObject*)simm_malloc(sizeof(DeformObject)*from[i].num_deforms); - memcpy(to[i].deform, from[i].deform, sizeof(DeformObject)*from[i].num_deforms); - for (j=0; jtitle, from->title); - if (from->numoptions > 0) - { - to->option = (MenuItem*)simm_malloc(sizeof(MenuItem)*from->numoptions); - memcpy(to->option, from->option, sizeof(MenuItem)*from->numoptions); - for (i=0; inumoptions; i++) - mstrcpy(&to->option[i].name, from->option[i].name); - } -} - -MuscleGroup* copy_muscle_groups(MuscleGroup from[], int num) -{ - int i; - MuscleGroup* to = NULL; - - if (num <= 0) - return to; - - to = (MuscleGroup*)simm_malloc(sizeof(MuscleGroup)*num); - memcpy(to, from, sizeof(MuscleGroup)*num); - - for (i=0; i 0) - { - to[i].muscle_index = (int*)simm_malloc(sizeof(int)*from[i].num_muscles); - memcpy(to[i].muscle_index, from[i].muscle_index, sizeof(int)*from[i].num_muscles); - } - to[i].muscindex_array_size = from[i].num_muscles; - copy_menu(&to[i].menu, &from[i].menu); - } - - return to; -} - -SegmentGroup* copy_segment_groups(SegmentGroup from[], int num) -{ - int i; - SegmentGroup* to = NULL; - - if (num <= 0) - return to; - - to = (SegmentGroup*)simm_malloc(sizeof(SegmentGroup)*num); - memcpy(to, from, sizeof(SegmentGroup)*num); - - for (i=0; i 0) - { - to[i].segment = (int*)simm_malloc(sizeof(int)*from[i].num_segments); - memcpy(to[i].segment, from[i].segment, sizeof(int)*from[i].num_segments); - } - to[i].seg_array_size = from[i].num_segments; - } - - return to; -} - -GencoordGroup* copy_gencoord_groups(GencoordGroup from[], int num) -{ - int i; - GencoordGroup* to = NULL; - - if (num <= 0) - return to; - - to = (GencoordGroup*)simm_malloc(sizeof(GencoordGroup)*num); - memcpy(to, from, sizeof(GencoordGroup)*num); - - for (i=0; i 0) - { - to[i].gencoord = (GeneralizedCoord**)simm_malloc(sizeof(GeneralizedCoord*) * from[i].num_gencoords); - memcpy(to[i].gencoord, from[i].gencoord, sizeof(GeneralizedCoord*) * from[i].num_gencoords); - } - to[i].genc_array_size = from[i].num_gencoords; - } - - return to; -} - -GeneralizedCoord** copy_gencoords(ModelStruct* model, GeneralizedCoord* from[], int num) -{ - int i; - GeneralizedCoord** to = NULL; - - if (num <= 0) - return to; - - to = (GeneralizedCoord**)simm_malloc(sizeof(GeneralizedCoord*)*num); - - for (i=0; iname, from[i]->name); - if (from[i]->numjoints > 0) - { - to[i]->jointnum = (int*)simm_malloc(sizeof(int)*from[i]->numjoints); - memcpy(to[i]->jointnum, from[i]->jointnum, sizeof(int)*from[i]->numjoints); - } - if (from[i]->numgroups > 0) - { - to[i]->group = (int*)simm_malloc(sizeof(int)*to[i]->numgroups); - memcpy(to[i]->group, from[i]->group, sizeof(int)*to[i]->numgroups); - } - if (from[i]->restraint_function) - to[i]->restraint_function = getFunctionByUserNumber(model, from[i]->restraint_function->usernum); - if (from[i]->min_restraint_function) - to[i]->min_restraint_function = getFunctionByUserNumber(model, from[i]->min_restraint_function->usernum); - if (from[i]->max_restraint_function) - to[i]->max_restraint_function = getFunctionByUserNumber(model, from[i]->max_restraint_function->usernum); - -#if INCLUDE_MOCAP_MODULE - mstrcpy(&to[i]->mocap_segment, from[i]->mocap_segment); -#endif - } - - return to; -} - -dpFunction** copy_functions(dpFunction* from[], int fromArraySize, int* toArraySize) -{ - int i, index = 0; - dpFunction** to = NULL; - - *toArraySize = 0; - for (i = 0; i < fromArraySize; i++) - if (from[i] && from[i]->used == dpYes) - (*toArraySize)++; - - if (*toArraySize <= 0) - return to; - - to = (dpFunction**)simm_malloc(sizeof(dpFunction*)*(*toArraySize)); - - for (i=0; iused == dpYes) - { - to[index] = (dpFunction*)simm_calloc(1, sizeof(dpFunction)); - malloc_function(to[index], from[i]->numpoints); - copy_function(from[i], to[index]); - index++; - } - } - - return to; -} - -/* COPY_MUSCLES: copy all the muscles from one model to another */ -ReturnCode copy_muscles(ModelStruct* from, ModelStruct* to) -{ - int i; - - if (from->default_muscle == NULL) - { - to->default_muscle = NULL; - } - else - { - to->default_muscle = (dpMuscleStruct*)simm_calloc(1, sizeof(dpMuscleStruct)); - if (copy_default_muscle(from->default_muscle, to->default_muscle, to) == code_bad) - return code_bad; - } - - to->nummuscles = from->nummuscles; - - if (to->nummuscles == 0) - { - to->muscle = NULL; - to->muscle_array_size = 0; - return code_fine; - } - - // Malloc memory for the array of muscle pointers. - to->muscle = (dpMuscleStruct**)simm_malloc(from->nummuscles*sizeof(dpMuscleStruct*)); - to->muscle_array_size = to->nummuscles; - - // Copy the muscle structures. - for (i=0; inummuscles; i++) - { - to->muscle[i] = (dpMuscleStruct*)simm_calloc(1, sizeof(dpMuscleStruct)); - if (copy_muscle(from->muscle[i], to->muscle[i], from->default_muscle, to->default_muscle, to) == code_bad) - return code_bad; - } - return code_fine; -} - -/* COPY_LIGAMENTS: copy the ligament structure */ -LigamentStruct* copy_ligaments(LigamentStruct from[], int num) -{ - int i, j; - LigamentStruct* to = NULL; - - if (num <= 0) - return to; - - to = (LigamentStruct*)simm_malloc(sizeof(LigamentStruct) * num); - memcpy(to, from, sizeof(LigamentStruct) * num); - - for (i=0; iview_name[i]) { - mstrcpy(&to->view_name[i], from->view_name[i]); - } else { - to->view_name[i] = NULL; - } - } - to->applied_motion = NULL; - to->current_motion = NULL; - if (from->nummuscleson > 0 && from->muscleson != NULL) - { - to->muscleson = (int*)simm_malloc(sizeof(int)*from->nummuscleson); - memcpy(to->muscleson, from->muscleson, sizeof(int)*from->nummuscleson); - } - to->devs = NULL; - to->dev_values = NULL; - - if (from->mat.num_materials > 0 && from->mat.materials != NULL) - { - to->mat.materials = (MaterialStruct*)simm_calloc(from->mat.num_materials,sizeof(MaterialStruct)); - for (i=0; imat.num_materials; i++) - copy_material(&from->mat.materials[i], &to->mat.materials[i]); - to->mat.num_materials = from->mat.num_materials; - to->mat.material_array_size = to->mat.num_materials; - } -} - -ContactPair* copy_contact_pairs(ContactPair from[], int num) -{ - int i; - ContactPair* to = NULL; - - if (num <= 0) - return to; - - to = (ContactPair*)simm_malloc(sizeof(ContactPair)*num); - memcpy(to, from, sizeof(ContactPair)*num); - - for (i=0; iname, from[i]->name); - } - - return to; -} - -Deformity* copy_deformities(Deformity from[], int num, ModelStruct* ms) -{ - int i, j; - Deformity* to = NULL; - - if (num <= 0) - return to; - - to = (Deformity*)simm_malloc(sizeof(Deformity)*num); - memcpy(to, from, sizeof(Deformity)*num); - - for (i=0; iname, from->name); - if (from->deriv_names) - { - to->deriv_names = (char**)simm_malloc(sizeof(char*)*from->number_of_datacolumns); - for (j=0; jnumber_of_datacolumns; j++) - mstrcpy(&to->deriv_names[j], from->deriv_names[j]); - } - if (from->deriv_data) - { - to->deriv_data = (double**)simm_malloc(sizeof(double*)*from->number_of_datacolumns); - for (j=0; jnumber_of_datacolumns; j++) - { - to->deriv_data[j] = (double*)simm_malloc(sizeof(double)*from->number_of_datarows); - memcpy(to->deriv_data[j], from->deriv_data[j], sizeof(double)*from->number_of_datarows); - } - } - mstrcpy(&to->units, from->units); - if (from->columnname) - { - to->columnname = (char**)simm_malloc(sizeof(char*)*from->number_of_datacolumns); - for (j=0; jnumber_of_datacolumns; j++) - mstrcpy(&to->columnname[j], from->columnname[j]); - } - if (from->motiondata) - { - to->motiondata = (double**)simm_malloc(sizeof(double*)*from->number_of_datacolumns); - for (j=0; jnumber_of_datacolumns; j++) - { - to->motiondata[j] = (double*)simm_malloc(sizeof(double)*from->number_of_datarows); - memcpy(to->motiondata[j], from->motiondata[j], sizeof(double)*from->number_of_datarows); - } - } - if (from->data_std_dev) - { - to->data_std_dev = (double**)simm_malloc(sizeof(double*)*from->number_of_datacolumns); - for (j=0; jnumber_of_datacolumns; j++) - { - if (from->data_std_dev[j]) - { - to->data_std_dev[j] = (double*)simm_malloc(sizeof(double)*from->number_of_datarows); - memcpy(to->data_std_dev[j], from->data_std_dev[j], sizeof(double)*from->number_of_datarows); - } - else - { - to->data_std_dev[j] = NULL; - } - } - } - if (from->num_events > 0) - { - to->event = (smMotionEvent*)simm_malloc(sizeof(smMotionEvent)*from->num_events); - memcpy(to->event, from->event, sizeof(smMotionEvent)*from->num_events); - for (j=0; jnum_events; j++) - mstrcpy(&to->event[j].name, from->event[j].name); - } - - return to; -} - -ModelStruct* copy_model(ModelStruct* ms) -{ - int i; - - ModelStruct* copy = (ModelStruct*)simm_malloc(sizeof(ModelStruct)); - memcpy(copy, ms, sizeof(ModelStruct)); - memset(©->save, 0, sizeof(ModelSave)); // saved version is not copied - - mstrcpy(©->name, ms->name); - mstrcpy(©->forceUnits, ms->forceUnits); - mstrcpy(©->lengthUnits, ms->lengthUnits); - mstrcpy(©->HTRtranslationUnits, ms->HTRtranslationUnits); - - for (i=0; i<2*GENBUFFER; i++) - if (ms->genc_help[i].text) - mstrcpy(©->genc_help[i].text, ms->genc_help[i].text); - - // copy pathptrs - copy->pathptrs = NULL; - // copy segment_drawing_order - copy->segment_drawing_order = NULL; - // copy gencslider - copy->gencslider.sl = NULL; - copy->gencslider.numsliders = 0; - // copy gencform - copy->gencform.title = NULL; - copy->gencform.option = NULL; - copy->gencform.numoptions = 0; - // copy gc_chpanel - copy->gc_chpanel.title = NULL; - copy->gc_chpanel.checkbox = NULL; - copy->gc_chpanel.numoptions = 0; - // copy gc_lockPanel - copy->gc_lockPanel.title = NULL; - copy->gc_lockPanel.checkbox = NULL; - copy->gc_lockPanel.numoptions = 0; - // copy dynparamsform - copy->dynparamsform.title = NULL; - copy->dynparamsform.option = NULL; - copy->dynparamsform.numoptions = 0; - - mstrcpy(©->jointfilename, ms->jointfilename); - mstrcpy(©->bonepathname, ms->bonepathname); - mstrcpy(©->musclefilename, ms->musclefilename); - for (i=0; inum_motion_files; i++) - mstrcpy(©->motionfilename[i], ms->motionfilename[i]); - mstrcpy(©->mocap_dir, ms->mocap_dir); - - // Functions must be copied first so that all components that use them can - // get the appropriate pointer from the copied function array. - copy->function = copy_functions(ms->function, ms->func_array_size, ©->func_array_size); - - // Gencoords must be copied before joints because DOFs contain pointers - // to gencoords. - copy->gencgroup = copy_gencoord_groups(ms->gencgroup, ms->numgencgroups); - copy->gencgroup_array_size = ms->numgencgroups; - copy->gencoord = copy_gencoords(copy, ms->gencoord, ms->numgencoords); - copy->genc_array_size = ms->numgencoords; - - copy->worldobj = copy_world_objects(ms->worldobj, ms->numworldobjects); - copy->world_array_size = ms->numworldobjects; - - // Segments have to be copied before joints, because of joint->in_seg_ground_path. - copy->seggroup = copy_segment_groups(ms->seggroup, ms->numseggroups); - copy->seggroup_array_size = ms->numseggroups; - copy->segment = copy_segments(ms->segment, ms->numsegments); - copy->segment_array_size = ms->numsegments; - - copy->joint = copy_joints(copy, ms->joint, ms->numjoints); - copy->joint_array_size = ms->numjoints; - - copy->muscgroup = copy_muscle_groups(ms->muscgroup, ms->numgroups); - copy->muscgroup_array_size = ms->numgroups; - copy_muscles(ms, copy); - - copy->ligament = copy_ligaments(ms->ligament, ms->numligaments); - copy->ligament_array_size = ms->numligaments; - - copy_display_struct(&ms->dis, ©->dis); - - copy->contactPair = copy_contact_pairs(ms->contactPair, ms->numContactPairs); - copy->contactPairArraySize = ms->numContactPairs; - - copy->contactGroup = copy_contact_groups(ms->contactGroup, ms->numContactGroups); - copy->contactGroupArraySize = ms->numContactGroups; - - copy->motion_objects = copy_motion_objects(ms->motion_objects, ms->num_motion_objects); - - copy->wrapobj = copy_wrap_objects(ms->wrapobj, ms->num_wrap_objects); - copy->wrap_object_array_size = ms->num_wrap_objects; - - copy->deformity = copy_deformities(ms->deformity, ms->num_deformities, ms); - copy->deformity_array_size = ms->num_deformities; - - // copy loop - - copy->constraintobj = copy_constraint_objects(ms->constraintobj, ms->num_constraint_objects); - copy->constraint_object_array_size = ms->num_constraint_objects; - - copy->motion = copy_motions(ms->motion, ms->num_motions); - copy->motion_array_size = ms->num_motions; - - check_definitions(copy); - - return copy; -} - -static ReturnCode copy_nondefault_dyn_params(dpMuscleStruct* from, dpMuscleStruct* to, - dpMuscleStruct* deffrom, dpMuscleStruct* defto) -{ - int i; - - to->num_dynamic_params = from->num_dynamic_params; - - if (to->num_dynamic_params > 0) - { - // A muscle's param name array is always the same as its default muscle's. - to->dynamic_param_names = defto->dynamic_param_names; - to->dynamic_params = (double**)simm_malloc(to->num_dynamic_params * sizeof(double*)); - - if (to->dynamic_params == NULL) - return code_bad; - - for (i = 0; i < to->num_dynamic_params; i++) - { - if (copy_nddouble(from->dynamic_params[i], &to->dynamic_params[i], - deffrom->dynamic_params[i], defto->dynamic_params[i]) == code_bad) - return code_bad; - } - } - else - { - to->dynamic_param_names = NULL; - to->dynamic_params = NULL; - } - - return code_fine; -} - - -/* This function copies the parameter names and all of the non-null values. - */ -static ReturnCode copy_nonnull_dyn_params(dpMuscleStruct* from, dpMuscleStruct* to) -{ - to->num_dynamic_params = from->num_dynamic_params; - - if (to->num_dynamic_params > 0) - { - int i; - - to->dynamic_param_names = (char**)simm_malloc(to->num_dynamic_params * sizeof(char*)); - if (to->dynamic_param_names == NULL) - return code_bad; - - for (i=0; inum_dynamic_params; i++) - mstrcpy(&to->dynamic_param_names[i], from->dynamic_param_names[i]); - - to->dynamic_params = (double**)simm_malloc(to->num_dynamic_params * sizeof(double*)); - if (to->dynamic_params == NULL) - return code_bad; - - for (i = 0; i < to->num_dynamic_params; i++) - { - if (copy_nndouble(from->dynamic_params[i], &to->dynamic_params[i]) == code_bad) - return code_bad; - } - } - else - { - to->dynamic_param_names = NULL; - to->dynamic_params = NULL; - } - - return code_fine; -} - - -ReturnCode alloc_func(dpFunction** func, int size) -{ - - if ((*func = (dpFunction*)simm_malloc(sizeof(dpFunction))) == NULL) - return code_bad; - if (((*func)->x = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if (((*func)->y = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if (((*func)->b = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if (((*func)->c = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if (((*func)->d = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - - return code_fine; - -} - -/* Given a gencoord struct, return the QStruct that has the same name */ -static dpQStruct* get_q_from_gencoord_dp(dpModelStruct* model, GeneralizedCoord *genc) -{ - int i; - - if (model->q == NULL) - return NULL; - - for (i=0; inq; i++) - { - if (&model->q[i] == NULL) - return NULL; - if (STRINGS_ARE_EQUAL(genc->name, model->q[i].name)) - return &model->q[i]; - } - - return NULL; -} diff --git a/OpenSim/Utilities/simmToOpenSim/deformeditor.h b/OpenSim/Utilities/simmToOpenSim/deformeditor.h deleted file mode 100644 index db3f2c4c00..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/deformeditor.h +++ /dev/null @@ -1,131 +0,0 @@ -/******************************************************************************* - - DEFORMEDITOR.H - - Author: Kenny Smith - - Date: 19-FEB-99 - - Copyright (c) 1992-8 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef DEFORMEDITOR_H -#define DEFORMEDITOR_H - -enum { - DE_CHOOSE_SEGMENT = 0, - DE_DEFORM_OBJECT, - DE_SAVE_ALL, - DE_RESTORE_ALL, - DE_DELETE_OBJECT, - DE_APPLY_POS_XFORM, - DE_APPLY_NEG_XFORM, - DE_CLEAR_XFORM, - DE_IDENTITY_XFORM, - - DE_OBJECT_NAME = 0, - DE_INNER_MIN_X, - DE_INNER_MIN_Y, - DE_INNER_MIN_Z, - DE_INNER_MAX_X, - DE_INNER_MAX_Y, - DE_INNER_MAX_Z, - DE_OUTER_MIN_X, - DE_OUTER_MIN_Y, - DE_OUTER_MIN_Z, - DE_OUTER_MAX_X, - DE_OUTER_MAX_Y, - DE_OUTER_MAX_Z, - DE_TRANSLATE_X, - DE_TRANSLATE_Y, - DE_TRANSLATE_Z, - DE_ROTATE_X, - DE_ROTATE_Y, - DE_ROTATE_Z, - - DE_POSITION_RBTN = 0, - DE_DEFORM_START_RBTN, - DE_DEFORM_END_RBTN, - - DE_ACTIVE_CHBOX = 0, - DE_VISIBLE_CHBOX, - DE_AUTORESET_CHBOX, - DE_TRACKBALL_CHBOX, - - DE_LOCAL_FRAME_RBTN = 0, - DE_PARENT_FRAME_RBTN, - - DE_SLIDER_WIDTH = 16, - - DEFORM_ARRAY_INCREMENT = 2, - - DE_PICK_DEFORM_KEY = 0, - DE_MOVE_DEFORM_KEY, - DE_PAN_DEFORM_KEY, - DE_ZOOM_DEFORM_KEY, - DE_TRACKBALL_KEY, - DE_ROTATE_X_KEY, - DE_ROTATE_Y_KEY, - DE_ROTATE_Z_KEY, - DE_MAX_DEVS -}; - -enum { DE_POSITION_MODE, DE_DEFORM_START_MODE, DE_DEFORM_END_MODE, DE_SLIDER_MODE }; - -enum { DE_LOCAL_FRAME, DE_PARENT_FRAME }; - -STRUCT { - int segment; /* current segment */ - int deform; /* index of current wrapping object */ - int deformMode; /* mode = position or deform */ - dpCoord3D translate; - dpCoord3D rotate; - int xform_frame; - SBoolean trackball_rotation; - Form deformity_form; - SliderArray deformity_sliders; -} DEModelOptions; /* user-selectable plotting options */ - -ENUM { - DE_TOP_LEVEL, /* */ - DE_ENTER_VALUE, /* deform object form field mode */ - DE_DEFORMITY_ENTER_VALUE /* deformity form field mode */ -} DEMODE; /* */ - -STRUCT { - ModelStruct* model; /* */ - DEMODE current_mode; /* */ - int selected_item; /* */ - Menu pushbtns; /* */ - Form textfields; /* */ - CheckBoxPanel deform_radiopanel; - CheckBoxPanel active_visible_checkpanel; - SliderArray deform_slider; - CheckBoxPanel transform_radiopanel; - DEModelOptions deop[MODELBUFFER]; /* */ - HelpStruct help; /* */ - int reference_number; /* */ - Slider win_slider; /* */ - int canvas_height; /* */ - int deform_object_menu; - int numdevs; /* number of device numbers in devs[] */ - int devs[DE_MAX_DEVS]; /* button numbers to move bone vertices */ -} DeformEditorStruct; /* */ - -STRUCT { - Scene* scene; - ModelStruct* model; - double wx_old, wy_old, wz_old; - int mx_old, my_old; - double bpan_wx_old, bpan_wy_old, bpan_wz_old; - int bpan_mx_old, bpan_my_old; - double span_wx_old, span_wy_old, span_wz_old; - int span_mx_old, span_my_old; - int zoom_mx_old, zoom_my_old; - dpCoord3D zoom_vec; -} DeformEditorTracker; - -#endif /* DEFORMEDITOR_H */ diff --git a/OpenSim/Utilities/simmToOpenSim/defunctions.h b/OpenSim/Utilities/simmToOpenSim/defunctions.h deleted file mode 100644 index b837457ac7..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/defunctions.h +++ /dev/null @@ -1,84 +0,0 @@ -/******************************************************************************* - - DEFUNCTIONS.H - - Author: Kenny Smith - - Date: 22-OCT-98 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef DEFUNCTIONS_H -#define DEFUNCTIONS_H - -void make_deformeditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void de_simm_event_handler(SimmEvent); -SBoolean de_query_handler(QueryType, void* data); -void display_deformeditor(WindowParams*, WinUnion*); -void update_deformeditor(WindowParams*, WinUnion*); -void deformeditor(WindowParams*, WinUnion*, SimmEvent); -void slide_de(int arg1, double value, double delta); - -void init_deform(DeformObject*); -void init_deformity(Deformity*); - -void makedeformeditormenus(); -void de_entervalue(SimmEvent); -void de_enter_deformity_value(SimmEvent); -void update_de_forms(); - -void de_track_cb(void* data, SimmEvent se); -void deformity_slider_cb(int sliderIndex, double value, double delta); - -void deform_segment(ModelStruct*, int segment); -static SBoolean deform_joint_pt (ModelStruct* ms, JointStruct* jnt, double* pt); -void deform_vert (DeformObject* dfm, const double undeformed[3], double pt_to_deform[3], SBoolean* didDeform); - -void calc_joint_pretransform(ModelStruct*, JointStruct*); - -SBoolean is_current_deform(DeformObject*); -void select_deform(int deformobj); - -void reset_deform_xform(); -void recalc_deform_xforms(SegmentStruct*, DeformObject*); - -void apply_xform_to_deform(double factor); -void clear_de_xform_form(); - -XForm* get_deform_xform(DeformObject*, int deformMode); -XForm* get_deform_xform2(DeformObject*, int deformMode, double factor); -XForm* get_deform_xform3(DeformObject*, int deformMode, double factor); - -void save_all_deform_objects(ModelStruct* ms); -void restore_all_deform_objects(ModelStruct* ms); -void delete_deform_object(int deformobj); - -void do_de_help(); -void draw_de_help_window(WindowParams*, WinUnion*); -void de_help_input(WindowParams*, WinUnion*, SimmEvent); -void move_de_help_text(int dummy_int, double slider_value, double delta); - -void draw_deform_objects(ModelStruct* model, SegmentStruct* seg, int segment_index, ModelDrawOptions* mdo); - -DeformObject* lookup_deform(ModelStruct*, const char* deformName); - - -void init_deform_box_verts(DeformObject*); - -#define _SET_VERT(XYZ, _X, _Y, _Z) (XYZ[0] = _X, XYZ[1] = _Y, XYZ[2] = _Z) -#define _COPY_VERT(DST, SRC) ((DST)[0] = (SRC)[0], (DST)[1] = (SRC)[1], (DST)[2] = (SRC)[2]) -#define _MAKE_VEC(C, B, A) ((C)[0] = (B)[0] - (A)[0], (C)[1] = (B)[1] - (A)[1], (C)[2] = (B)[2] - (A)[2]) -#define _SCALE_VEC(V, T) ((V)[0] *= (T), (V)[1] *= (T), (V)[2] *= (T)) -#define _ADD_VEC(PT, V) ((PT)[0] += (V)[0], (PT)[1] += (V)[1], (PT)[2] += (V)[2]) - - -#define SEGMENTS_PER_BOX_EDGE 3 /* number of segments per box edge (was 15) */ - -#define VERTS_PER_BOX_EDGE (SEGMENTS_PER_BOX_EDGE + 1) -#define VERTS_PER_BOX (VERTS_PER_BOX_EDGE * 12) - -#endif /*DEFUNCTIONS_H*/ diff --git a/OpenSim/Utilities/simmToOpenSim/detools.c b/OpenSim/Utilities/simmToOpenSim/detools.c deleted file mode 100644 index 58b9c75746..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/detools.c +++ /dev/null @@ -1,1716 +0,0 @@ -/******************************************************************************* - - DETOOLS.C - - Author: Kenny Smith - - Date: 22-OCT-98 - - Copyright (c) 1992-8 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ -#include -#include -#include - -#include "universal.h" -#include "deformeditor.h" - -#include "globals.h" -#include "functions.h" -#include "wefunctions.h" -#include "defunctions.h" - -/*************** DEFINES (for this file only) *********************************/ -#define CHECK_MIN_VALUE 0 -#define CHECK_MAX_VALUE 1 - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static float* sBoneScale = NULL; - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ -#if ! SIMM_VIEWER && ! OPENSMAC -extern DeformEditorStruct* de; -extern WindowParams* de_win_params; -extern WinUnion* de_win_union; -#endif - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ - -/* ------------------------------------------------------------------------- - init_deform - ----------------------------------------------------------------------------- */ -void init_deform (DeformObject* dfm) -{ - dfm->name = NULL; - dfm->segment = -1; - dfm->active = yes; - dfm->visible = yes; - dfm->autoReset = no; - dfm->translationOnly = no; - dfm->innerMin.xyz[0] = dfm->innerMin.xyz[1] = dfm->innerMin.xyz[2] = -0.03; - dfm->innerMax.xyz[0] = dfm->innerMax.xyz[1] = dfm->innerMax.xyz[2] = 0.03; - dfm->outerMin.xyz[0] = dfm->outerMin.xyz[1] = dfm->outerMin.xyz[2] = -0.04; - dfm->outerMax.xyz[0] = dfm->outerMax.xyz[1] = dfm->outerMax.xyz[2] = 0.04; - - dfm->position.rotation_axis.xyz[0] = 1.0; - dfm->position.rotation_axis.xyz[1] = 0.0; - dfm->position.rotation_axis.xyz[2] = 0.0; - dfm->position.rotation_angle = 0.0; - dfm->position.translation.xyz[0] = 0.0; - dfm->position.translation.xyz[1] = 0.0; - dfm->position.translation.xyz[2] = 0.0; - dfm->position.xforms_valid = no; - identity_matrix(dfm->position.from_local_xform); - identity_matrix(dfm->position.to_local_xform); - - dfm->deform_start = dfm->deform_end = dfm->position; - - dfm->deform_factor = 0.0; - - identity_matrix(dfm->delta_xform); - identity_matrix(dfm->delta_xform2); - identity_matrix(dfm->delta_xform3); - - dfm->innerBox = dfm->innerBoxUndeformed = NULL; - dfm->outerBox = dfm->outerBoxUndeformed = NULL; - -#if ! ENGINE - init_deform_box_verts(dfm); -#endif -} - -/* ------------------------------------------------------------------------- - init_deformity - ----------------------------------------------------------------------------- */ -void init_deformity (Deformity* dty) -{ - dty->name = NULL; - dty->value = dty->default_value = MINMDOUBLE; - dty->range.start = 0.0; - dty->range.end = 100.0; - dty->keys[0] = null_key; - dty->keys[1] = null_key; - dty->num_deforms = 0; - dty->deform = NULL; -} - - -/* ------------------------------------------------------------------------- - pt_in_box - return true if the specified 3d point is inside the specified - 3d axis-aligned box. ----------------------------------------------------------------------------- */ -static SBoolean pt_in_box (const double pt[3], const dpCoord3D* min, const dpCoord3D* max) -{ - return (SBoolean) (pt[0] > min->xyz[0] && pt[0] < max->xyz[0] && - pt[1] > min->xyz[1] && pt[1] < max->xyz[1] && - pt[2] > min->xyz[2] && pt[2] < max->xyz[2]); -} - -/* ------------------------------------------------------------------------- - calc_transition_factor - return a "transition factor" value that smoothly - ranges from 1.0 to 0.0 as the specified 3d point moves from inside - the deform's inner box to outside the deform's outer box. ----------------------------------------------------------------------------- */ -static double calc_transition_factor (DeformObject* dfm, const double pt[3]) -{ - double factors[3], factor = 0.0; - int i, n = 0; - - if (pt[0] >= dfm->outerMin.xyz[0] && pt[0] <= dfm->innerMin.xyz[0]) - factors[n++] = (pt[0] - dfm->innerMin.xyz[0]) / (dfm->outerMin.xyz[0] - dfm->innerMin.xyz[0]); - else if (pt[0] <= dfm->outerMax.xyz[0] && pt[0] >= dfm->innerMax.xyz[0]) - factors[n++] = (pt[0] - dfm->innerMax.xyz[0]) / (dfm->outerMax.xyz[0] - dfm->innerMax.xyz[0]); - - if (pt[1] >= dfm->outerMin.xyz[1] && pt[1] <= dfm->innerMin.xyz[1]) - factors[n++] = (pt[1] - dfm->innerMin.xyz[1]) / (dfm->outerMin.xyz[1] - dfm->innerMin.xyz[1]); - else if (pt[1] <= dfm->outerMax.xyz[1] && pt[1] >= dfm->innerMax.xyz[1]) - factors[n++] = (pt[1] - dfm->innerMax.xyz[1]) / (dfm->outerMax.xyz[1] - dfm->innerMax.xyz[1]); - - if (pt[2] >= dfm->outerMin.xyz[2] && pt[2] <= dfm->innerMin.xyz[2]) - factors[n++] = (pt[2] - dfm->innerMin.xyz[2]) / (dfm->outerMin.xyz[2] - dfm->innerMin.xyz[2]); - else if (pt[2] <= dfm->outerMax.xyz[2] && pt[2] >= dfm->innerMax.xyz[2]) - factors[n++] = (pt[2] - dfm->innerMax.xyz[2]) / (dfm->outerMax.xyz[2] - dfm->innerMax.xyz[2]); - - for (i = 0; i < n; i++) - if (factor < factors[i]) - factor = factors[i]; - - return 1.0 - factor; -} - -#if ! ENGINE -#if ! OPENSMAC - -/* ------------------------------------------------------------------------- - deform_mesh - apply the specified deformation to the specified polygon mesh. ----------------------------------------------------------------------------- */ -static void deform_mesh (DeformObject* dfm, PolyhedronStruct* mesh) -{ - int i; - - if ( ! dfm->active) - return; - - for (i = 0; i < mesh->num_vertices; i++) - deform_vert(dfm, mesh->undeformed[i].coord, mesh->vertex[i].coord, NULL); -} - - -/* ------------------------------------------------------------------------- - deform_deform_boxes - ----------------------------------------------------------------------------- */ -static void deform_deform_boxes (ModelStruct* ms, DeformObject* dfm, DeformObject* deformer) -{ - - int i, n = 3 * VERTS_PER_BOX; - - if ( ! deformer->active) - return; - - for (i = 0; i < n; i += 3) - { - double pt[3]; -#if 0 - _COPY_VERT(pt, &dfm->innerBox[i]); - transform_pt(dfm->deform_start.from_local_xform, pt); - transform_pt(deformer->delta_xform, pt); - transform_pt(dfm->deform_start.to_local_xform, pt); - _COPY_VERT(&dfm->innerBox[i], pt); -#else - double undeformedPt[3]; - SBoolean inInnerBox, inOuterBox; - XForm* xform = &dfm->deform_start; - - _COPY_VERT(undeformedPt, &dfm->innerBoxUndeformed[i]); - transform_pt(xform->from_local_xform, undeformedPt); - transform_pt(deformer->position.to_local_xform, undeformedPt); - - inInnerBox = pt_in_box(undeformedPt, &deformer->innerMin, &deformer->innerMax); - inOuterBox = pt_in_box(undeformedPt, &deformer->outerMin, &deformer->outerMax); - - if (inInnerBox || inOuterBox) - { - _COPY_VERT(pt, &dfm->innerBox[i]); - transform_pt(xform->from_local_xform, pt); - - if (inInnerBox) - transform_pt(deformer->delta_xform, pt); - else - { - double pt2[3], delta[3], t = calc_transition_factor(deformer, undeformedPt); - - _COPY_VERT(pt2, pt); - transform_pt(deformer->delta_xform, pt2); - - _MAKE_VEC(delta, pt2, pt); - _SCALE_VEC(delta, t); - _ADD_VEC(pt, delta); - } - transform_pt(xform->to_local_xform, pt); - _COPY_VERT(&dfm->innerBox[i], pt); - } -#endif - } -} - -#if ! SIMM_DEMO_VERSION && ! SIMM_VIEWER - -/* ------------------------------------------------------------------------- - de_track_cb - ----------------------------------------------------------------------------- */ -void de_track_cb (void* data, SimmEvent se) -{ - int i, count, de_vals[DE_MAX_DEVS]; - DeformEditorTracker* tracker = (DeformEditorTracker*) data; - DEModelOptions* deop = NULL; - Scene* scene = NULL; - ModelStruct* ms = NULL; - SegmentStruct* seg; - DeformObject* dfm; - XForm* xform; - SBoolean redraw = no; - double z_dist, tmp_matrix[4][4], inv_view_matrix[4][4]; - double wpt[4], wpt2[4], owpt[4], owpt2[4]; - int bpan_mx_new, bpan_my_new; - double bpan_wx_new, bpan_wy_new, bpan_wz_new; - int mx_new, my_new; - double wx_new, wy_new, wz_new; - double angle, origin[4], new_origin[4], naxis[4], axis[4], x_percent, cursor_movement; - - getDev(de->numdevs, de->devs, de_vals); - - for (count = 0, i = 0; i < de->numdevs; i++) - if (de_vals[i] == 1) - count++; - - if (count == 0) - { - REMOVE_TRACKER(); - return; - } - - scene = tracker->scene; - ms = tracker->model; - deop = &de->deop[ms->modelnum]; - seg = &ms->segment[deop->segment]; - dfm = &seg->deform[deop->deform]; - - if (!dfm) - return; - xform = get_deform_xform(dfm, deop->deformMode); - - recalc_deform_xforms(seg, dfm); - - if (de_vals[DE_MOVE_DEFORM_KEY] && deop->deformMode <= DE_DEFORM_END_MODE) - { - mx_new = se.mouse_x; - my_new = se.mouse_y; - - if (deop->trackball_rotation) - { - if (de_vals[DE_TRACKBALL_KEY]) - { - z_dist = scene->tz; - - find_world_coords(scene, mx_new, my_new, - z_dist, &wx_new, &wy_new, &wz_new); - - if (tracker->mx_old == -1 || tracker->my_old == -1) - { - tracker->mx_old = mx_new; - tracker->my_old = my_new; - tracker->wx_old = wx_new; - tracker->wy_old = wy_new; - tracker->wz_old = wz_new; - } - else if (tracker->mx_old != mx_new || tracker->my_old != my_new) - { - origin[0] = origin[1] = origin[2] = 0.0; - origin[3] = 1.0; - - axis[0] = tracker->wy_old - wy_new; - axis[1] = wx_new - tracker->wx_old; - axis[2] = 0.0; - - normalize_vector(axis, naxis); - naxis[3] = 1.0; - - invert_4x4transform(scene->transform_matrix,tmp_matrix); - mult_4x4matrix_by_vector(tmp_matrix,naxis,axis); - mult_4x4matrix_by_vector(tmp_matrix,origin,new_origin); - - axis[0] -= new_origin[0]; - axis[1] -= new_origin[1]; - axis[2] -= new_origin[2]; - - normalize_vector(axis,naxis); - - naxis[0] -= origin[0]; - naxis[1] -= origin[1]; - naxis[2] -= origin[2]; - - normalize_vector(naxis, naxis); - convert_vector(ms, naxis, ms->ground_segment, dfm->segment); - normalize_vector(naxis, naxis); - - /* if cursor moves a full screen width, rotate by 90 degrees */ - cursor_movement = sqrt((mx_new-tracker->mx_old)*(mx_new-tracker->mx_old) + - (my_new-tracker->my_old)*(my_new-tracker->my_old)); - - x_percent = cursor_movement / (double)(scene->viewport[2]); - - angle = x_percent * 90.0; - - if (angle >= 0.0 && angle <= 180.0) - { - DMatrix m; - - identity_matrix(m); - rotate_matrix_axis_angle(m, xform->rotation_axis.xyz, - xform->rotation_angle); - rotate_matrix_axis_angle(m, naxis, angle * DTOR); - - extract_rotation(m, &xform->rotation_axis, &xform->rotation_angle); - - redraw = yes; - - tracker->mx_old = mx_new; - tracker->my_old = my_new; - tracker->wx_old = wx_new; - tracker->wy_old = wy_new; - tracker->wz_old = wz_new; - } - } - } - else - { - tracker->wx_old = tracker->wy_old = tracker->wz_old = -1.0; - tracker->mx_old = tracker->my_old = -1; - } - - if (de_vals[DE_PAN_DEFORM_KEY]) - { - wpt[0] = wpt[1] = wpt[2] = 0.0; - transform_pt(xform->from_local_xform, wpt); - wpt[3] = 1.0; - - convert(ms, wpt, dfm->segment, ms->ground_segment); - mult_4x4matrix_by_vector(scene->transform_matrix, wpt, wpt2); - - z_dist = wpt2[2]; - bpan_mx_new = se.mouse_x; - bpan_my_new = se.mouse_y; - - find_world_coords(scene, bpan_mx_new, bpan_my_new, - z_dist, &bpan_wx_new, &bpan_wy_new, &bpan_wz_new); - - if (tracker->bpan_mx_old == -1 || tracker->bpan_my_old == -1) - { - tracker->bpan_mx_old = bpan_mx_new; - tracker->bpan_my_old = bpan_my_new; - tracker->bpan_wx_old = bpan_wx_new; - tracker->bpan_wy_old = bpan_wy_new; - tracker->bpan_wz_old = bpan_wz_new; - } - else if (tracker->bpan_mx_old != bpan_mx_new || - tracker->bpan_my_old != bpan_my_new) - { - wpt[0] = bpan_wx_new; - wpt[1] = bpan_wy_new; - wpt[2] = bpan_wz_new; - wpt[3] = 1.0; - - invert_4x4transform(scene->transform_matrix,inv_view_matrix); - -#if !STEADYCAM - if (scene->camera_segment >= 0) - copy_4x4matrix(*get_ground_conversion(ms->modelnum, - scene->camera_segment, to_ground), tmp_matrix); - else - reset_4x4matrix(tmp_matrix); - append_4x4matrix(inv_view_matrix,tmp_matrix); -#endif - mult_4x4matrix_by_vector(inv_view_matrix,wpt,wpt2); - convert(ms, wpt2, ms->ground_segment, dfm->segment); - - owpt[0] = tracker->bpan_wx_old; - owpt[1] = tracker->bpan_wy_old; - owpt[2] = tracker->bpan_wz_old; - owpt[3] = 1.0; - - mult_4x4matrix_by_vector(inv_view_matrix,owpt,owpt2); - convert(ms, owpt2, ms->ground_segment, dfm->segment); - - xform->translation.xyz[0] += (wpt2[XX] - owpt2[XX]); - xform->translation.xyz[1] += (wpt2[YY] - owpt2[YY]); - xform->translation.xyz[2] += (wpt2[ZZ] - owpt2[ZZ]); - - tracker->bpan_mx_old = bpan_mx_new; - tracker->bpan_my_old = bpan_my_new; - tracker->bpan_wx_old = bpan_wx_new; - tracker->bpan_wy_old = bpan_wy_new; - tracker->bpan_wz_old = bpan_wz_new; - redraw = yes; - } - } - else - { - tracker->bpan_wx_old = tracker->bpan_wy_old = tracker->bpan_wz_old = -1.0; - tracker->bpan_mx_old = tracker->bpan_my_old = -1; - } - - if (de_vals[DE_ZOOM_DEFORM_KEY]) - { - if (tracker->zoom_mx_old == -1 || tracker->zoom_my_old == -1) - { - tracker->zoom_mx_old = mx_new; - tracker->zoom_my_old = my_new; - - wpt[0] = wpt[1] = 0.0; - wpt[2] = wpt[3] = 1.0; - - invert_4x4transform(scene->transform_matrix,tmp_matrix); - - mult_4x4matrix_by_vector(tmp_matrix, wpt, wpt2); - convert(ms, wpt2, ms->ground_segment, dfm->segment); - - tracker->zoom_vec.xyz[0] = wpt2[0]; - tracker->zoom_vec.xyz[1] = wpt2[1]; - tracker->zoom_vec.xyz[2] = wpt2[2]; - - wpt[0] = wpt[1] = wpt[2] = 0.0; - wpt[3] = 1.0; - - mult_4x4matrix_by_vector(tmp_matrix, wpt, wpt2); - convert(ms, wpt2, ms->ground_segment, dfm->segment); - - tracker->zoom_vec.xyz[0] -= wpt2[0]; - tracker->zoom_vec.xyz[1] -= wpt2[1]; - tracker->zoom_vec.xyz[2] -= wpt2[2]; - } - else if (tracker->zoom_mx_old != mx_new || tracker->zoom_my_old != my_new) - { - double netmove = (mx_new - tracker->zoom_mx_old) * 0.002; - - xform->translation.xyz[0] += netmove * tracker->zoom_vec.xyz[0]; - xform->translation.xyz[1] += netmove * tracker->zoom_vec.xyz[1]; - xform->translation.xyz[2] += netmove * tracker->zoom_vec.xyz[2]; - - redraw = yes; - - tracker->zoom_mx_old = mx_new; - tracker->zoom_my_old = my_new; - } - } - else - tracker->zoom_mx_old = tracker->zoom_my_old = -1; - } - else /* trackball turned off, dfm x,y,z rotation */ - { - double new_rot_angle; - DMatrix m; - - identity_matrix(m); - rotate_matrix_axis_angle(m, xform->rotation_axis.xyz, xform->rotation_angle); - - if (de_vals[DE_ROTATE_X_KEY]) - { - if (CURSOR_IN_VIEWPORT(mx_new, my_new, scene->viewport)) - { - new_rot_angle = DISTANCE_FROM_MIDPOINT(mx_new, scene->viewport) * 0.1; - - if (deop->xform_frame == DE_LOCAL_FRAME) - x_rotate_matrix_bodyfixed(m, new_rot_angle * DTOR); - else - x_rotate_matrix_spacefixed(m, new_rot_angle * DTOR); - - redraw = yes; - } - } - if (de_vals[DE_ROTATE_Y_KEY]) - { - if (CURSOR_IN_VIEWPORT(mx_new, my_new, scene->viewport)) - { - new_rot_angle = DISTANCE_FROM_MIDPOINT(mx_new, scene->viewport) * 0.1; - - if (deop->xform_frame == DE_LOCAL_FRAME) - y_rotate_matrix_bodyfixed(m, new_rot_angle * DTOR); - else - y_rotate_matrix_spacefixed(m, new_rot_angle * DTOR); - - redraw = yes; - } - } - if (de_vals[DE_ROTATE_Z_KEY]) - { - if (CURSOR_IN_VIEWPORT(mx_new, my_new, scene->viewport)) - { - new_rot_angle = DISTANCE_FROM_MIDPOINT(mx_new, scene->viewport) * 0.1; - - if (deop->xform_frame == DE_LOCAL_FRAME) - z_rotate_matrix_bodyfixed(m, new_rot_angle * DTOR); - else - z_rotate_matrix_spacefixed(m, new_rot_angle * DTOR); - - redraw = yes; - } - } - if (redraw) - extract_rotation(m, &xform->rotation_axis, &xform->rotation_angle); - } - } - - if (redraw == yes) - { - if (deop->deformMode == DE_POSITION_MODE) - { - dfm->deform_start = dfm->deform_end = dfm->position; - - de->deform_slider.sl[0].value = dfm->deform_factor = 0.0; - } - xform->xforms_valid = no; - recalc_deform_xforms(seg, dfm); - deform_segment(NULL, -1); - queue_model_redraw(ms); - if (ms == de->model) - display_deformeditor(de_win_params, de_win_union); - } -} /* de_track_cb */ - -#endif /* SIMM_DEMO_VERSION && ! SIMM_VIEWER */ - -#define FOG_DEFORM_BOXES 0 - -/* ------------------------------------------------------------------------- - set_fog_near_and_far - ----------------------------------------------------------------------------- */ -static void set_fog_near_and_far(const float* box) -{ - double fogNear = FLT_MAX, fogFar = 0.0; - int j,k,l; - DMatrix m, n; - - glGetDoublev(GL_MODELVIEW_MATRIX, (double*) m); - invert_4x4transform(m, n); - - for (j = 0, l = 0; j < 12; j++) - { - for (k = 0; k < VERTS_PER_BOX_EDGE; k++) - { - double pt[3]; - - pt[0] = box[l++]; - pt[1] = box[l++]; - pt[2] = box[l++]; - - transform_pt(n, pt); - - if (pt[2] > 0.0 && fogNear > pt[2]) - fogNear = pt[2]; - - if (fogFar < pt[2]) - fogFar = pt[2]; - } - } - - if (fogNear == FLT_MAX) - fogNear = 0.0; - - glFogf(GL_FOG_START, fogNear); - glFogf(GL_FOG_END, fogFar + 0.5 * (fogFar - fogNear)); - -#if 1 - fprintf(stderr, "%f %f\n", fogNear, fogFar); -#endif -} - -/* ------------------------------------------------------------------------- - draw_deform_objects - ----------------------------------------------------------------------------- */ -void draw_deform_objects(ModelStruct* model, SegmentStruct* seg, int segment_index, ModelDrawOptions* mdo) -{ -#if ! SIMM_VIEWER - -#define IS_CUR_DEFORM(DFM) (model == de->model && DFM == deop->deform) - - int i, j, k, l; - static const float active_dfm_color[3] = { 1.0, 0.5, 1.0 }; - static const float inactive_dfm_color[3] = { 0.0, 0.0, 1.0 }; - DEModelOptions* deop; - - if (de == NULL) - return; - - deop = &de->deop[model->modelnum]; - - if (seg != &model->segment[deop->segment]) - return; - - if (mdo->mode == GL_RENDER) - { - glPushAttrib(GL_ENABLE_BIT); - glDisable(GL_LIGHTING); - -#if FOG_DEFORM_BOXES - glEnable(GL_FOG); - glFogi(GL_FOG_MODE, GL_LINEAR); - glFogfv(GL_FOG_COLOR, root.color.cmap[MISC_MODEL_WINDOW_BACKGROUND].rgb); - glHint(GL_FOG_HINT, GL_FASTEST); /* or try GL_NICEST */ -#endif - } - - for (i = 0; i < seg->num_deforms; i++) - { - DeformObject* dfm = &seg->deform[i]; - - if ( ! dfm->visible) - continue; - - recalc_deform_xforms(seg, dfm); - - // Draw the deform's outer box. - glPushMatrix(); - glMultMatrixd((double*) dfm->position.from_local_xform); - - if (mdo->mode == GL_RENDER) - { - glColor3fv(IS_CUR_DEFORM(i) ? active_dfm_color : inactive_dfm_color); -#if FOG_DEFORM_BOXES - set_fog_near_and_far(dfm->outerBox); -#endif - } - else - { - PickIndex color_value; - GLubyte color[3]; - - glShadeModel(GL_FLAT); - color_value = (DEFORM_OBJECT << OBJECT_BITS) + ((PickIndex)segment_index << SUBELEMENT_BITS) + ((PickIndex)i << 3) + (PickIndex)0; // 0 is for outer box - pack_int_into_color(color_value, color); - glColor3ubv(color); - { - PickIndex obj_num, object_type; - int seg_num, deform_index, component; - get_object_type_and_number(color_value, &object_type, &obj_num); - seg_num = obj_num >> SUBELEMENT_BITS; - deform_index = obj_num & SUBELEMENT_MASK; - component = obj_num & 0x7; - printf("outer box: %d %d, %d %d %d %d\n", (int)obj_num, (int)object_type, seg_num, deform_index, component); - } - } - - for (j = 0, l = 0; j < 12; j++) - { - glBegin(GL_LINE_STRIP); - - for (k = 0; k < VERTS_PER_BOX_EDGE; k++) - { - glVertex3fv(&dfm->outerBox[l]); - - l += 3; - } - glEnd(); - } - glPopMatrix(); - - // Draw the deform's inner box. - glPushMatrix(); - glMultMatrixd((double*) get_deform_xform3(dfm, deop->deformMode, dfm->deform_factor)->from_local_xform); - - if (IS_CUR_DEFORM(i)) - { - if (deop->deformMode == DE_POSITION_MODE || dfm->deform_factor == 0.0 || dfm->deform_factor == 1.0) - { - glLineWidth(2.0); - } - } - - if (mdo->mode == GL_RENDER) - { - glColor3fv(IS_CUR_DEFORM(i) ? active_dfm_color : inactive_dfm_color); -#if FOG_DEFORM_BOXES - set_fog_near_and_far(dfm->innerBox); -#endif - } - else - { - PickIndex color_value; - GLubyte color[3]; - - glShadeModel(GL_FLAT); - color_value = (DEFORM_OBJECT << OBJECT_BITS) + ((PickIndex)segment_index << SUBELEMENT_BITS) + ((PickIndex)i << 3) + (PickIndex)1; // 1 is for inner box - pack_int_into_color(color_value, color); - glColor3ubv(color); - { - PickIndex obj_num, object_type; - int seg_num, deform_index, component; - get_object_type_and_number(color_value, &object_type, &obj_num); - seg_num = obj_num >> SUBELEMENT_BITS; - deform_index = obj_num & SUBELEMENT_MASK; - component = obj_num & 0x7; - printf("inner box: %d %d, %d %d %d %d\n", (int)obj_num, (int)object_type, seg_num, deform_index, component); - } - } - - for (j = 0, l = 0; j < 12; j++) - { - glBegin(GL_LINE_STRIP); - - for (k = 0; k < VERTS_PER_BOX_EDGE; k++) - { - glVertex3fv(&dfm->innerBox[l]); - - l += 3; - } - glEnd(); - } - - // Draw the deform's axes (current one only). - if (IS_CUR_DEFORM(i)) - { - glBegin(GL_LINES); - if (mdo->mode == GL_RENDER) - { - simm_color(RED); - } - else - { - PickIndex color_value; - GLubyte color[3]; - - glShadeModel(GL_FLAT); - color_value = (DEFORM_OBJECT << OBJECT_BITS) + ((PickIndex)segment_index << SUBELEMENT_BITS) + ((PickIndex)i << 3) + (PickIndex)2; // 2 is for X axis - pack_int_into_color(color_value, color); - glColor3ubv(color); - } - glVertex3d(0, 0, 0); - glVertex3d(1.5 * fabs(dfm->innerMax.xyz[0]), 0, 0); - if (mdo->mode == GL_RENDER) - { - simm_color(GREEN); - } - else - { - PickIndex color_value; - GLubyte color[3]; - - glShadeModel(GL_FLAT); - color_value = (DEFORM_OBJECT << OBJECT_BITS) + ((PickIndex)segment_index << SUBELEMENT_BITS) + ((PickIndex)i << 3) + (PickIndex)3; // 3 is for Y axis - pack_int_into_color(color_value, color); - glColor3ubv(color); - } - glVertex3d(0, 0, 0); - glVertex3d(0, 1.5 * fabs(dfm->innerMax.xyz[1]), 0); - if (mdo->mode == GL_RENDER) - { - simm_color(BLUE); - } - else - { - PickIndex color_value; - GLubyte color[3]; - - glShadeModel(GL_FLAT); - color_value = (DEFORM_OBJECT << OBJECT_BITS) + ((PickIndex)segment_index << SUBELEMENT_BITS) + ((PickIndex)i << 3) + (PickIndex)4; // 4 is for Z axis - pack_int_into_color(color_value, color); - glColor3ubv(color); - } - glVertex3d(0, 0, 0); - glVertex3d(0, 0, 1.5 * fabs(dfm->innerMax.xyz[2])); - glEnd(); - } - - glLineWidth(1.0); - glPopMatrix(); - } - - if (mdo->mode == GL_RENDER) - glPopAttrib(); -#endif /* ! SIMM_VIEWER */ -} - - -/* ------------------------------------------------------------------------- - get_deform_xform - ----------------------------------------------------------------------------- */ -XForm* get_deform_xform (DeformObject* dfm, int deformMode) -{ - switch (deformMode) - { - case DE_POSITION_MODE: - return &dfm->position; - case DE_DEFORM_START_MODE: - return &dfm->deform_start; - case DE_DEFORM_END_MODE: - return &dfm->deform_end; - } - - return NULL; -} - -/* ------------------------------------------------------------------------- - get_deform_xform2 - ----------------------------------------------------------------------------- */ -XForm* get_deform_xform2 (DeformObject* dfm, int deformMode, double factor) -{ - switch (deformMode) - { - case DE_POSITION_MODE: - return &dfm->position; - - default: - if (EQUAL_WITHIN_ERROR(factor,0.0)) - return &dfm->deform_start; - else if (EQUAL_WITHIN_ERROR(factor,1.0)) - return &dfm->deform_end; - else if (deformMode == DE_DEFORM_START_MODE) - return &dfm->deform_start; - else if (deformMode == DE_DEFORM_END_MODE) - return &dfm->deform_end; - } - - return NULL; -} - -/* ------------------------------------------------------------------------- - get_deform_xform3 - ----------------------------------------------------------------------------- */ -XForm* get_deform_xform3 (DeformObject* dfm, int deformMode, double factor) -{ - switch (deformMode) - { - //dkb - removed this because it messed up display of object using slider - // case DE_POSITION_MODE: - // return &dfm->position; - - default: -#if 0 - if (factor == 0.0) - return &dfm->deform_start; - else if (factor == 1.0) - return &dfm->deform_end; - else -#endif - { - static XForm xform; - - copy_4x4matrix(dfm->delta_xform3, xform.from_local_xform); - - return &xform; - } - } -} - -/* ------------------------------------------------------------------------- - deform_segment - ----------------------------------------------------------------------------- */ -void deform_segment (ModelStruct* ms, int segment) -{ - SegmentStruct* seg; - int i,j,k; - - /* if 'ms' or 'segment' are unspecified, then use the deform editor's - * current model and/or segment - */ -#if ! SIMM_VIEWER - if (ms == NULL) - ms = de->model; -#endif - - if (ms == NULL) - return; - -#if ! SIMM_VIEWER - if (segment < 0) - segment = de->deop[ms->modelnum].segment; -#endif - - if (segment < 0) - return; - - seg = &ms->segment[segment]; - - if (seg->num_deforms <= 0) - return; - -#if 0 - /* prepare any auto-reset transforms */ - calc_auto_reset_deform(seg, NULL); -#endif - - /* deform all bones in the segment */ - for (i = 0; i < seg->numBones; i++) - { - PolyhedronStruct* bone = &seg->bone[i]; - - int nBytes = bone->num_vertices * sizeof(VertexStruct); - - if (bone->undeformed == NULL) - { - bone->undeformed = (VertexStruct*) simm_malloc(nBytes); - - if (bone->undeformed) - memcpy(bone->undeformed, bone->vertex, nBytes); - } - if (bone->undeformed) - { - memcpy(bone->vertex, bone->undeformed, nBytes); -#if NOSCALE - if (NOT_EQUAL_WITHIN_ERROR(seg->bone_scale[0], 1.0) || - NOT_EQUAL_WITHIN_ERROR(seg->bone_scale[1], 1.0) || - NOT_EQUAL_WITHIN_ERROR(seg->bone_scale[2], 1.0)) - { - sBoneScale = seg->bone_scale; - } -#else - sBoneScale = NULL; -#endif - /*if (deop->deformMode != DE_POSITION_MODE)*/ - for (j = 0; j < seg->num_deforms; j++) - deform_mesh(&seg->deform[j], bone); - - sBoneScale = NULL; - } - } - - /* deform the muscle points attached to the segment */ - for (i = 0; i < ms->nummuscles; i++) - { - dpMuscleStruct* msc = ms->muscle[i]; - - for (j = 0; j < msc->path->num_orig_points; j++) - { - dpMusclePoint* mp = &msc->path->mp_orig[j]; - - if (mp->segment == segment) - { - _COPY_VERT(mp->point, mp->undeformed_point); - - /*if (deop->deformMode != DE_POSITION_MODE)*/ - for (k = 0; k < seg->num_deforms; k++) - if (seg->deform[k].active) - deform_vert(&seg->deform[k], - mp->undeformed_point, - mp->point, NULL); - } - } - } - - /* deform any muscle wrapping objects attached to the segment */ - for (i = 0; i < ms->num_wrap_objects; i++) - { - if (ms->wrapobj[i]->segment == segment) - { - dpWrapObject* wo = ms->wrapobj[i]; - - _COPY_VERT(wo->translation.xyz, wo->undeformed_translation.xyz); - - /*if (deop->deformMode != DE_POSITION_MODE)*/ - for (k = 0; k < seg->num_deforms; k++) - if (seg->deform[k].active) - deform_vert(&seg->deform[k], - wo->undeformed_translation.xyz, - wo->translation.xyz, NULL); - - wo->xforms_valid = no; - recalc_xforms(wo); - inval_model_wrapping(ms, ms->wrapobj[i]); - } - } - - /* deform any constraint objects attached to the segment */ - for (i = 0; i < ms->num_constraint_objects; i++) - { - if (ms->constraintobj[i].segment == segment) - { - ConstraintObject* co = &ms->constraintobj[i]; - - _COPY_VERT(co->translation.xyz, co->undeformed_translation.xyz); - - /*if (deop->deformMode != DE_POSITION_MODE)*/ - for (k = 0; k < seg->num_deforms; k++) - if (seg->deform[k].active) - deform_vert(&seg->deform[k], - co->undeformed_translation.xyz, - co->translation.xyz, NULL); - - co->xforms_valid = no; - recalc_constraint_xforms(co); - ce_recalc_loops_and_constraints(ms, i, no); - } - } - - /* deform (invalidate actually) any joints emanating from the segment */ - for (i = 0; i < ms->numjoints; i++) - { - if (ms->joint[i].from == segment) - { - double pt[3], jointOrigin[3] = { 0,0,0 }; - SBoolean didDeform = no; - - /* determine the joint's undeformed origin in the segment's frame */ - ms->joint[i].pretransform_active = no; - ms->joint[i].pretransform_condition = invalid; - invalidate_joint_matrix(ms, &ms->joint[i]); -#if 0 - if (deop->deformMode == DE_POSITION_MODE) - continue; -#endif - convert(ms, jointOrigin, ms->joint[i].to, ms->joint[i].from); - _COPY_VERT(pt, jointOrigin); - - /* determine the joint's deformed origin */ - for (k = 0; k < seg->num_deforms; k++) - if (seg->deform[k].active) - deform_vert(&seg->deform[k], jointOrigin, pt, &didDeform); - - /* if the joint is subject to deformation, then activate its pretransform */ - if (didDeform) - { - ms->joint[i].pretransform_active = yes; - ms->joint[i].pretransform_condition = invalid; - invalidate_joint_matrix(ms, &ms->joint[i]); - } - } - } -#if 0 - /* deform the deform boxes in the segment */ - for (i = 0; i < seg->num_deforms; i++) - { - DeformObject* dfm = &seg->deform[i]; - int nBytes = VERTS_PER_BOX * 3 * sizeof(float); - - memcpy(dfm->innerBox, dfm->innerBoxUndeformed, nBytes); - memcpy(dfm->outerBox, dfm->outerBoxUndeformed, nBytes); - -#if 0 - deform_deform_boxes(ms, dfm, dfm); -#elif 0 - /*if (deop->deformMode != DE_POSITION_MODE)*/ - for (j = 0; j < seg->num_deforms; j++) - deform_deform_boxes(ms, dfm, &seg->deform[j]); -#endif - } -#endif - - delete_segment_display_lists(seg, ms); - queue_model_redraw(ms); -} -#endif /* ! OPENSMAC */ -#endif /* ! ENGINE */ - - -/* ------------------------------------------------------------------------ - reorthogonalize_matrix - given a 4x4 matrix before and after deformation, - remove any shearing added to the upper 3x3 submatrix during deformation. - - The rows of the before and after matrices are treated as three mutually - orthogonal vectors. The dot-product between each before and after vector - is computed to determine which vector has changed the most under deformation. - - The vector that has changed the *most* under deformation remains unchanged. - The other two vectors are rebuilt via cross-products to create a pure - rotation matrix that tracks the strongest deformation gradient. ---------------------------------------------------------------------------- */ -static void reorthogonalize_matrix (double before[][4], double after[][4]) -{ -#define _SWAP(I, J) ( \ - map[3].dot = map[I].dot, map[I].dot = map[J].dot, map[J].dot = map[3].dot, \ - map[3].idx = map[I].idx, map[I].idx = map[J].idx, map[J].idx = map[3].idx) - -#define _LEAST_DEFORMED (map[0].idx) -#define _MIDDLE_DEFORMED (map[1].idx) -#define _MOST_DEFORMED (map[2].idx) - - struct { - double after[3]; - double dot; - int idx; - } map[4]; /* NOTE: the 4th slot is used as a temporary by _SWAP() */ - int i; - - /* compute and save the dot products of the before and after matrices */ - for (i = 0; i < 3; i++) - { - map[i].after[XX] = after[i][XX]; - map[i].after[YY] = after[i][YY]; - map[i].after[ZZ] = after[i][ZZ]; - - normalize_vector(map[i].after, map[i].after); - normalize_vector(before[i], before[i]); - - map[i].dot = 1.0 - DOT_VECTORS(before[i], map[i].after); - map[i].idx = i; - } - /* sort the dot products (and their associated indexes) into ascending order */ - if (map[0].dot > map[1].dot) _SWAP(0, 1); - if (map[1].dot > map[2].dot) _SWAP(1, 2); - if (map[0].dot > map[1].dot) _SWAP(0, 1); - - /* rebuild the least-deformed vector by crossing the two most-deformed vectors */ - cross_vectors(after[_MOST_DEFORMED], after[_MIDDLE_DEFORMED], after[_LEAST_DEFORMED]); - - /* rebuild the 2nd-least-deformed vector */ - cross_vectors(after[_LEAST_DEFORMED], after[_MOST_DEFORMED], after[_MIDDLE_DEFORMED]); - - /* get the direction right on the rebuilt least-deformed vector */ - if (DOT_VECTORS(after[_LEAST_DEFORMED], before[_LEAST_DEFORMED]) < 0.0) - { - after[_LEAST_DEFORMED][XX] = - after[_LEAST_DEFORMED][XX]; - after[_LEAST_DEFORMED][YY] = - after[_LEAST_DEFORMED][YY]; - after[_LEAST_DEFORMED][ZZ] = - after[_LEAST_DEFORMED][ZZ]; - } - - /* normalize the reorthogonalized vectors */ - for (i = 0; i < 3; i++) - normalize_vector(after[i], after[i]); - -#undef _MOST_DEFORMED -#undef _MIDDLE_DEFORMED -#undef _LEAST_DEFORMED -#undef _SWAP -} /* reorthogonalize_matrix */ - -/* ------------------------------------------------------------------------- - calc_joint_pretransform - compute a "pretransform" matrix for the specified - joint that contains the current deformation at the joint origin. - - NOTE: We create the joint pretransform matrix by deforming a "mini reference - frame" whose major axis are each 0.1 mm long. ----------------------------------------------------------------------------- */ -void calc_joint_pretransform (ModelStruct* ms, JointStruct* jnt) -{ - DMatrix n = { - { 1.0, 0.0, 0.0, 0.0 }, /* undeformed reference frame */ - { 0.0, 1.0, 0.0, 0.0 }, - { 0.0, 0.0, 1.0, 0.0 }, - { 0.0, 0.0, 0.0, 1.0 }}; - - DMatrix m = { - { 0.0001, 0.0000, 0.0000, 0.0 }, /* mini reference frame */ - { 0.0000, 0.0001, 0.0000, 0.0 }, - { 0.0000, 0.0000, 0.0001, 0.0 }, - { 0.0000, 0.0000, 0.0000, 1.0 }}; - - jnt->pretransform_active = no; - jnt->pretransform_condition = invalid; - - if (deform_joint_pt(ms, jnt, m[3])) - { - int i; - - for (i = 0; i < 3; i++) - { - deform_joint_pt(ms, jnt, m[i]); - _MAKE_VEC(m[i], m[i], m[3]); - normalize_vector(m[i], m[i]); - } - - /* NOTE: the deformed reference frame may have lost its orthogonality - * at this point. We straighten it out by calling reorthogonalize_matrix(): - */ - reorthogonalize_matrix(n, m); - - copy_4x4matrix(m, jnt->pretransform); - invert_4x4transform(jnt->pretransform, jnt->pretransform_inverse); - } - else - { - identity_matrix(jnt->pretransform); - identity_matrix(jnt->pretransform_inverse); - } - jnt->pretransform_active = yes; -} - - -/* ------------------------------------------------------------------------- - deform_vert - apply the specified deformation to the specified 3d point. - The point must be specified in the reference frame of the deform's - parent segment. - - Parameters: - dfm The deform to be applied. - - undeformed This is the point in its completely undeformed - position. This value is necessary to test for inclusion - in the specified deform's inner & outer boxes. - - pt_to_deform This is the point in its current (possibly already - deformed) position. This point may get (further) - deformed by the specified deform. - - didDeform A boolean flag that is set to TRUE if the point lies - inside the deform's inner or outer boxes. NOTE: this - flag is only set to TRUE. If the point is not effected - by this deform, then the flag is not modified. ----------------------------------------------------------------------------- */ -void deform_vert (DeformObject* dfm, const double undeformed[3], double pt_to_deform[3], SBoolean* didDeform) -{ - double undeformedPt[3]; - SBoolean inInnerBox, inOuterBox; - - _COPY_VERT(undeformedPt, undeformed); - - if (sBoneScale) - { - undeformedPt[0] *= sBoneScale[0]; - undeformedPt[1] *= sBoneScale[1]; - undeformedPt[2] *= sBoneScale[2]; - } - transform_pt(dfm->position.to_local_xform, undeformedPt); - - inInnerBox = pt_in_box(undeformedPt, &dfm->innerMin, &dfm->innerMax); - inOuterBox = pt_in_box(undeformedPt, &dfm->outerMin, &dfm->outerMax); - - if (inInnerBox || inOuterBox) - { - if (sBoneScale) - { - pt_to_deform[0] *= sBoneScale[0]; - pt_to_deform[1] *= sBoneScale[1]; - pt_to_deform[2] *= sBoneScale[2]; - } - - if (inInnerBox) - transform_pt(dfm->delta_xform, pt_to_deform); - else - { - double pt[3], delta[3], t = calc_transition_factor(dfm, undeformedPt); - - _COPY_VERT(pt, pt_to_deform); - transform_pt(dfm->delta_xform, pt); - - _MAKE_VEC(delta, pt, pt_to_deform); - _SCALE_VEC(delta, t); - _ADD_VEC(pt_to_deform, delta); - } - if (didDeform) - *didDeform = yes; - - if (sBoneScale) - { - pt_to_deform[0] /= sBoneScale[0]; - pt_to_deform[1] /= sBoneScale[1]; - pt_to_deform[2] /= sBoneScale[2]; - } - } -} - - -/* ------------------------------------------------------------------------- - deform_frame_pt - ----------------------------------------------------------------------------- */ -static SBoolean deform_frame_pt (SegmentStruct* seg, double* pt, DeformObject* ignoreDfm) -{ - double undeformedPt[3]; - int i; - SBoolean didDeform = no; - - _COPY_VERT(undeformedPt, pt); - - for (i = 0; i < seg->num_deforms; i++) - if (seg->deform[i].active && &seg->deform[i] != ignoreDfm) - deform_vert(&seg->deform[i], undeformedPt, pt, &didDeform); - - return didDeform; -} - - -/* ------------------------------------------------------------------------- - deform_joint_pt - ----------------------------------------------------------------------------- */ -static SBoolean deform_joint_pt (ModelStruct* ms, JointStruct* jnt, double* pt) -{ - SegmentStruct* seg = &ms->segment[jnt->from]; - double undeformedPt[3]; - int i; - SBoolean didDeform = no; - - convert(ms, pt, jnt->to, jnt->from); - - _COPY_VERT(undeformedPt, pt); - - for (i = 0; i < seg->num_deforms; i++) - if (seg->deform[i].active) - deform_vert(&seg->deform[i], undeformedPt, pt, &didDeform); - - convert(ms, pt, jnt->from, jnt->to); - - return didDeform; -} -/* ------------------------------------------------------------------------- - deform_frame_pt2 - ----------------------------------------------------------------------------- */ -static SBoolean deform_frame_pt2 (DeformObject* dfm, const double* undeformed, double* pt) -{ - SBoolean didDeform = no; - - if (dfm->active) - deform_vert(dfm, undeformed, pt, &didDeform); - - return didDeform; -} - -/* ------------------------------------------------------------------------- - deform_deform_reference_frame - this routine applies all deformations - preceeding 'dfm' to the reference frame of the 'dfm' deform. ----------------------------------------------------------------------------- */ -static void deform_deform_reference_frame (SegmentStruct* seg, DeformObject* dfm, double result[][4], SBoolean includeAutoResetDfm) -{ - DMatrix m1 = { - { 0.0005, 0.0000, 0.0000, 0.0 }, /* mini reference frame - UNDEFORMED */ - { 0.0000, 0.0005, 0.0000, 0.0 }, - { 0.0000, 0.0000, 0.0005, 0.0 }, - { 0.0000, 0.0000, 0.0000, 1.0 }}; - - DMatrix m2 = { - { 0.0005, 0.0000, 0.0000, 0.0 }, /* mini reference frame */ - { 0.0000, 0.0005, 0.0000, 0.0 }, - { 0.0000, 0.0000, 0.0005, 0.0 }, - { 0.0000, 0.0000, 0.0000, 1.0 }}; - - int i,j; - - for (i = 0; i < seg->num_deforms; i++) - { - DeformObject* deformer = &seg->deform[i]; - - if (deformer == dfm) - break; - - if (deform_frame_pt2(deformer, m1[3], m2[3])) - for (j = 0; j < 3; j++) - deform_frame_pt2(deformer, m1[j], m2[j]); - } - - if (includeAutoResetDfm) - { - DeformObject* autoDfm = NULL; - - for (i = 0; i < seg->num_deforms; i++) - if (seg->deform[i].autoReset) - break; - - if (i < seg->num_deforms) - autoDfm = &seg->deform[i]; - - if (autoDfm) - if (deform_frame_pt2(autoDfm, m1[3], m2[3])) - for (j = 0; j < 3; j++) - deform_frame_pt2(autoDfm, m1[j], m2[j]); - } - - for (j = 0; j < 3; j++) - { - _MAKE_VEC(m2[j], m2[j], m2[3]); - normalize_vector(m2[j], m2[j]); - } - - copy_4x4matrix(m2, result); -} - -/* ------------------------------------------------------------------------- - calc_auto_reset_deform - check for an auto-reset deform in the - specified segment. If one is found, then compute its deforming - transform here. ----------------------------------------------------------------------------- */ -static void calc_auto_reset_deform (SegmentStruct* seg, DeformObject* autoDfm) -{ - int i; - - if (autoDfm == NULL) - { - for (i = 0; i < seg->num_deforms; i++) - if (seg->deform[i].autoReset) - break; - - if (i < seg->num_deforms) - autoDfm = &seg->deform[i]; - } - - if (autoDfm) - { - DMatrix m = { - { 0.0001, 0.0000, 0.0000, 0.0 }, /* mini reference frame */ - { 0.0000, 0.0001, 0.0000, 0.0 }, - { 0.0000, 0.0000, 0.0001, 0.0 }, - { 0.0000, 0.0000, 0.0000, 1.0 }}; - - if (deform_frame_pt(seg, m[3], autoDfm)) - { - for (i = 0; i < 3; i++) - { - if ( ! autoDfm->translationOnly) - { - deform_frame_pt(seg, m[i], autoDfm); - _MAKE_VEC(m[i], m[i], m[3]); - } - - normalize_vector(m[i], m[i]); - } - - /* NOTE: the deformed reference frame may have lost its orthogonality - * at this point. We should write a routine to straighten it out: - * - * reorthogonalize_matrix(m); - */ - - invert_4x4transform(m, autoDfm->delta_xform); - } - else - identity_matrix(autoDfm->delta_xform); - } -} - -/* ------------------------------------------------------------------------- - lookup_deform - ----------------------------------------------------------------------------- */ -DeformObject* lookup_deform (ModelStruct* ms, const char* deformName) -{ - int i,j; - - for (i = 0; i < ms->numsegments; i++) - - for (j = 0; j < ms->segment[i].num_deforms; j++) - - if (ms->segment[i].deform[j].name && STRINGS_ARE_EQUAL(deformName, ms->segment[i].deform[j].name)) - - return &ms->segment[i].deform[j]; - - return NULL; -} - - -/* ------------------------------------------------------------------------- - recalc_deform_xforms - recalculate the specified deform's various matrices. ----------------------------------------------------------------------------- */ -void recalc_deform_xforms (SegmentStruct* seg, DeformObject* dfm) -{ - if ( ! dfm->position.xforms_valid) - { - /* calculate the deform placement transform */ - identity_matrix(dfm->position.from_local_xform); - - if (dfm->position.rotation_angle != 0.0) - rotate_matrix_axis_angle(dfm->position.from_local_xform, dfm->position.rotation_axis.xyz, dfm->position.rotation_angle); - - translate_matrix(dfm->position.from_local_xform, dfm->position.translation.xyz); - - invert_4x4transform(dfm->position.from_local_xform, dfm->position.to_local_xform); - - dfm->position.xforms_valid = yes; - } - -#if 0 - /* SPECIAL-CASE: auto-reset deforms can be used to automatically deform - * a segment's origin back to its original (undeformed) position. There - * should only be one auto-reset deform per segment, and it should be - * the last deform for the segment. We don't calculate the auto-deform's - * deforming transforms in this routine. This calculation is deferred - * until vertices are actually being deformed (see calc_auto_reset_deform()). - */ - if (dfm->autoReset) - { - return; - } -#endif - - if ( ! dfm->deform_start.xforms_valid && ! dfm->autoReset) - { - /* calculate the deform start transform */ - identity_matrix(dfm->deform_start.from_local_xform); - - if (dfm->deform_start.rotation_angle != 0.0) - rotate_matrix_axis_angle(dfm->deform_start.from_local_xform, dfm->deform_start.rotation_axis.xyz, dfm->deform_start.rotation_angle); - - translate_matrix(dfm->deform_start.from_local_xform, dfm->deform_start.translation.xyz); - - invert_4x4transform(dfm->deform_start.from_local_xform, dfm->deform_start.to_local_xform); - - dfm->deform_start.xforms_valid = yes; - } - - if ( ! dfm->deform_end.xforms_valid && ! dfm->autoReset) - { - /* calculate the deform end transform */ - identity_matrix(dfm->deform_end.from_local_xform); - - if (dfm->deform_end.rotation_angle != 0.0) - rotate_matrix_axis_angle(dfm->deform_end.from_local_xform, dfm->deform_end.rotation_axis.xyz, dfm->deform_end.rotation_angle); - - translate_matrix(dfm->deform_end.from_local_xform, dfm->deform_end.translation.xyz); - - invert_4x4transform(dfm->deform_end.from_local_xform, dfm->deform_end.to_local_xform); - - dfm->deform_end.xforms_valid = yes; - } - - /* calculate the current deforming transform */ - { - dpCoord3D axis, translation; - double angle; - DMatrix ref_frame_xform, ref_frame_xform_inv; - - if (dfm->deform_factor == 0.0) - { - axis = dfm->deform_start.rotation_axis; - angle = dfm->deform_start.rotation_angle; - translation = dfm->deform_start.translation; - } - else if (dfm->deform_factor == 1.0) - { - axis = dfm->deform_end.rotation_axis; - angle = dfm->deform_end.rotation_angle; - translation = dfm->deform_end.translation; - } - //dkb - else if (dfm->deform_factor == -1.0) - { - axis = dfm->position.rotation_axis; - angle = dfm->position.rotation_angle; - translation = dfm->position.translation; - } - else - { - lerp_pt(dfm->deform_start.translation.xyz, - dfm->deform_end.translation.xyz, - dfm->deform_factor, translation.xyz); - - slerp(&dfm->deform_start.rotation_axis, dfm->deform_start.rotation_angle, - &dfm->deform_end.rotation_axis, dfm->deform_end.rotation_angle, - dfm->deform_factor, &axis, &angle); - } - - /* NOTE: the code below to build 'delta_xform' and 'delta_xform2' is - * not that clear, and unfortunately not that well understood by its - * author (me, Kenny). I arrived at it by partial understanding and - * a fair amount of trial and error. - */ - - /* compute ref_frame_xform & ref_frame_xform_inv */ - deform_deform_reference_frame(seg, dfm, ref_frame_xform, no); - - invert_4x4transform(ref_frame_xform, ref_frame_xform_inv); - - /* compute delta_xform */ - if (dfm->autoReset) - calc_auto_reset_deform(seg, dfm); - else - { - identity_matrix(dfm->delta_xform); - - append_matrix(dfm->delta_xform, ref_frame_xform_inv); - - append_matrix(dfm->delta_xform, dfm->position.to_local_xform); - - if (angle != 0.0) - rotate_matrix_axis_angle(dfm->delta_xform, axis.xyz, angle); - - translate_matrix(dfm->delta_xform, translation.xyz); - - append_matrix(dfm->delta_xform, ref_frame_xform); - } - - /* NOTE: since a change to a deform can affect all deforms that follow, - * we must recursively call this routine again for every deform in the - * segment's list that follows this one. - */ - { - static SBoolean sRecursionBlock = no; - - if ( ! sRecursionBlock) - { - int i; - - sRecursionBlock = yes; - - for (i = 0; i < seg->num_deforms; i++) - if (&seg->deform[i] == dfm) - break; - - for (i++; i < seg->num_deforms; i++) - recalc_deform_xforms(seg, &seg->deform[i]); - - /* compute delta_xform2 and delta_xform3 */ - identity_matrix(dfm->delta_xform2); - - if (angle != 0.0) - rotate_matrix_axis_angle(dfm->delta_xform2, axis.xyz, angle); - - translate_matrix(dfm->delta_xform2, translation.xyz); - - copy_4x4matrix(dfm->delta_xform2, dfm->delta_xform3); - - deform_deform_reference_frame(seg, dfm, ref_frame_xform, yes); - - append_matrix(dfm->delta_xform3, ref_frame_xform); - - sRecursionBlock = no; - } - } - } -} - -/* ============================================================================== - * ==== DEFORM INNER/OUTER BOX REPRESENTATION - */ - -/* ------------------------------------------------------------------------- - build_deform_box_edge - ----------------------------------------------------------------------------- */ -static void build_deform_box_edge (const double* c0, const double* c1, - float** vert, int** edge, int* nVerts) -{ - double delta[3]; - int i,j; - - delta[0] = (c1[0] - c0[0]) / SEGMENTS_PER_BOX_EDGE; - delta[1] = (c1[1] - c0[1]) / SEGMENTS_PER_BOX_EDGE; - delta[2] = (c1[2] - c0[2]) / SEGMENTS_PER_BOX_EDGE; - - for (i = 0; i <= SEGMENTS_PER_BOX_EDGE; i++) - { - for (j = 0; j < 3; j++) - { - *(*vert)++ = c0[j] + i * delta[j]; - - if (edge) - *(*edge)++ = (*nVerts)++; - } - } -} - -/* ------------------------------------------------------------------------- - init_deform_box_verts - ----------------------------------------------------------------------------- */ -void init_deform_box_verts (DeformObject* dfm) -{ - enum { MINX, MINY, MINZ, MAXX, MAXY, MAXZ }; - - static const char m[12][6] = { { MINX, MINY, MINZ, MAXX, MINY, MINZ }, - { MINX, MAXY, MINZ, MAXX, MAXY, MINZ }, - { MINX, MAXY, MAXZ, MAXX, MAXY, MAXZ }, - { MINX, MINY, MAXZ, MAXX, MINY, MAXZ }, - { MAXX, MINY, MINZ, MAXX, MAXY, MINZ }, - { MAXX, MINY, MAXZ, MAXX, MAXY, MAXZ }, - { MINX, MINY, MAXZ, MINX, MAXY, MAXZ }, - { MINX, MINY, MINZ, MINX, MAXY, MINZ }, - { MAXX, MINY, MINZ, MAXX, MINY, MAXZ }, - { MAXX, MAXY, MAXZ, MAXX, MAXY, MINZ }, - { MINX, MAXY, MAXZ, MINX, MAXY, MINZ }, - { MINX, MINY, MINZ, MINX, MINY, MAXZ } }; - - double *corner, start[3], end[3]; - float *vert; - int i, nBytes = VERTS_PER_BOX * 3 * sizeof(float); - - /* alloc (if necessary) and init inner box vertices */ - if (dfm->innerBox == NULL) - dfm->innerBox = (float*) simm_malloc(nBytes); - - if (dfm->innerBoxUndeformed == NULL) - dfm->innerBoxUndeformed = (float*) simm_malloc(nBytes); - - if (dfm->innerBox && dfm->innerBoxUndeformed) - { - corner = &dfm->innerMin.xyz[0]; - vert = dfm->innerBoxUndeformed; - - for (i = 0; i < 12; i++) - { - _SET_VERT(start, corner[m[i][0]], corner[m[i][1]], corner[m[i][2]]); - _SET_VERT(end, corner[m[i][3]], corner[m[i][4]], corner[m[i][5]]); - - build_deform_box_edge(start, end, &vert, NULL, NULL); - } - memcpy(dfm->innerBox, dfm->innerBoxUndeformed, nBytes); - } - - /* alloc (if necessary) and init outer box vertices */ - if (dfm->outerBox == NULL) - dfm->outerBox = (float*) simm_malloc(nBytes); - - if (dfm->outerBoxUndeformed == NULL) - dfm->outerBoxUndeformed = (float*) simm_malloc(nBytes); - - if (dfm->outerBox && dfm->outerBoxUndeformed) - { - corner = &dfm->outerMin.xyz[0]; - vert = dfm->outerBoxUndeformed; - - for (i = 0; i < 12; i++) - { - _SET_VERT(start, corner[m[i][0]], corner[m[i][1]], corner[m[i][2]]); - _SET_VERT(end, corner[m[i][3]], corner[m[i][4]], corner[m[i][5]]); - - build_deform_box_edge(start, end, &vert, NULL, NULL); - } - memcpy(dfm->outerBox, dfm->outerBoxUndeformed, nBytes); - } -} - diff --git a/OpenSim/Utilities/simmToOpenSim/dp.h b/OpenSim/Utilities/simmToOpenSim/dp.h deleted file mode 100644 index 94b2ee2dae..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/dp.h +++ /dev/null @@ -1,654 +0,0 @@ -/******************************************************************************* - - DP.H - - Author: Peter Loan - - Copyright (c) 2004-5 MusculoGraphics, Inc, a division of Motion Analysis Corp. - All rights reserved. - - This header file defines the data structures used to hold a musculoskeletal - model, as well as the interface between SIMM the DLLs made by the Dynamics - Pipeline. If you are using simulation DLLs, do not modify this file, - as it is also compiled into SIMM. If you are using stand-alone simulation - executables, you can modify this file as needed. - -*******************************************************************************/ - -#ifndef DP_H -#define DP_H - -#ifdef _WINDOWS - #define DLL __declspec(dllexport) -#else - #define DLL -#endif - - -typedef void* dpSimmModelID; - -typedef enum -{ - dpNo, - dpYes -} dpBoolean; - -typedef enum -{ - dpSetup, - dpFrame, - dpMessage -} dpDataType; - -typedef enum -{ - dpModelWarning, - dpModelError, - dpSimWarning, - dpSimError, - dpInitialized, - dpFinished, - dpOverwritable, - dpNormal, - dpDllLoaded -} dpMessageID; - -typedef enum -{ - dpNoNormalization = 0, - dpNormalizeToOne, - dpNormalizeToOneHundred -} dpNormalizeOptions; - -typedef struct -{ - double xyz[3]; /* coordinates of a point in 3space */ -} dpCoord3D; - -typedef struct -{ - char* name; - double xCoord; -} dpMotionEvent; - -typedef struct -{ - char* motionName; - int numElements; - char** elementNames; - double startTime; - double endTime; - int numFrames; - int numEvents; - dpMotionEvent* event; - float eventColor[3]; -} dpDataSetup; - -typedef struct -{ - int numElements; - double* elements; -} dpDataFrame; - -typedef struct -{ - dpMessageID id; - char text[500]; - void* data; -} dpMessageStruct; - -typedef struct -{ - dpBoolean muscleActivations; - dpBoolean muscleLengths; - dpBoolean muscleFiberLengths; - dpBoolean muscleFiberVelocities; - dpBoolean muscleForces; - dpBoolean contactForces; - dpBoolean massCenterPositions; - dpBoolean massCenterVelocities; - dpBoolean systemEnergy; - dpBoolean jointReactionForces; - dpBoolean jointReactionTorques; - dpBoolean jointTorques; - dpBoolean springForces; - int numSpringForces; - dpBoolean muscleMomentArms; - dpBoolean muscleJointTorques; - dpBoolean totalMuscleJointTorques; - dpBoolean optimizedMuscleActivations; - dpNormalizeOptions normalization; - dpBoolean gencoordValues; - dpBoolean traditionalInverse; -} dpOutputOptions; - -typedef struct -{ - double kinetic_cutoff; - dpBoolean kinetic_cutoff_defined; - double force_cutoff; - dpBoolean force_cutoff_defined; - double emg_cutoff; - dpBoolean emg_cutoff_defined; -} dpInputOptions; - -typedef struct -{ - double startTime; - double endTime; - double stepSize; - char* workingDir; - char* inputMotionFile; - char* outputMotionFile; - char* boneDir; - dpOutputOptions outputOptions; - dpSimmModelID model; - dpInputOptions inputOptions; -} dpSimulationParameters; - -typedef enum -{ - dpPin, - dpReversePin, - dpUniversal, - dpReverseUniversal, - dpGimbal, - dpReverseGimbal, - dpBall, - dpReverseBall, - dpSlider, - dpReverseSlider, - dpCylindrical, - dpReverseCylindrical, - dpPlanar, - dpReversePlanar, - dpBushing, - dpReverseBushing, - dpBearing, - dpReverseBearing, - dpFree, - dpReverseFree, - dpWeld, - dpSkippable, - dpUnknownJoint -} dpJointType; - -typedef enum -{ - dpTranslational, - dpRotational, - dpNoDof -} dpDOFType; - -/* dpStepFunction through dpLinearFunction must remain - * 0 through 3 so that they match the enums in dpSplineType - * from SIMM 4.2. - */ -typedef enum -{ - dpFunctionTypeUndefined = -1, - dpStepFunction = 0, - dpNaturalCubicSpline, - dpGCVSpline, - dpLinearFunction -} dpFunctionType; - -typedef enum -{ - dpJointFile = 0, - dpMuscleFile -} dpFunctionSource; - -typedef enum -{ - dpFunctionUndefined = 0, - dpFunctionUserDefined, - dpFunctionSimmDefined -} dpFunctionStatus; - -typedef enum -{ - dpUnknownContact, - dpImpact, - dpRestingContact, - dpSeparatingContact -} dpCollisionTypes; - -typedef struct -{ - int body1; /* SDFast index of body1 */ - int body2; /* SDFast index of body2 */ - double pt1[3]; /* contact point location in body1 frame from b1 com */ - double pt2[3]; /* contact point location in body2 frame from b2 com */ - double norm1[3]; /* contact normal on body1 in body1 frame */ - double norm2[3]; /* contact normal on body2 in body2 frame */ - double outward_norm[3]; /* from body1 to body2 in ground frame */ - double vel1[3]; /* velocity of contact point on body1 wrt ground */ - double vel2[3]; /* velocity of contact point on body2 wrt ground */ - double rel_vel[3]; /* relative velocity v2 - v1 */ - double cont_frc[3]; /* contact force or impulse */ - double acc; /* magnitude of normal relative acceleration */ - double coef_rest; /* coefficient of restitution */ - double mu_dynamic; /* dynamic friction coefficient */ - double mu_static; /* static friction coefficient */ - dpCollisionTypes contact_type; /* IMPACT, CONTACT, or SEPARATING */ - char type1[30]; /* FACE, VERTEX, or EDGE */ - char type2[30]; /* FACE, VERTEX, or EDGE */ - double dist; /* for ideal contacts along outward normal */ -} dpContactInfo; /* contact node */ - -typedef struct -{ - double a, b, c, d; -} dpPlaneStruct; - -typedef struct -{ - dpFunctionType type; /* dpStepFunction, dpLinearFunction, dpNaturalCubicSpline, dpGCVSpline */ - int numpoints; /* number of points in the function */ - int coefficient_array_size; /* current size of x,y,b,c,d arrays */ - int usernum; /* user-supplied number for this function*/ - double* x; /* list of x coordinates */ - double* y; /* list of y coordinates */ - double* b; /* list of b coefficients for interpolation */ - double* c; /* list of c coefficients for interpolation */ - double* d; /* list of d coefficients for interpolation */ - dpFunctionStatus status; /* undefined, defined in file, created in SIMM GUI */ - dpBoolean used; /* is this structure currently being used? */ - double cutoff_frequency; /* for smoothing of GCV splines */ - dpFunctionSource source; /* is function from joint file or muscle file? */ -} dpFunction; /* a function with XY control points */ - -typedef struct -{ - void* gencoord; /* gencoord number */ - double start; /* starting value of range-of-motion */ - double end; /* ending value of range-of-motion */ -} dpPointRange; /* muscle point range (wrapping pts) */ - -typedef struct -{ - int segment; /* body segment this point is fixed to */ - dpBoolean selected; /* whether or not point has been selected */ - double point[3]; /* xyz coordinates of the point */ - double ground_pt[3]; /* coords of point in ground frame */ - dpFunction* function[3]; /* function for moving point (3 coordinates - 3 possible functions) */ - void* gencoord[3]; /* gencoord for moving point (3 coordinates - 3 possible gencoords) */ - dpBoolean isMovingPoint; /* whether the point has non-constant coordinates */ - dpBoolean isVia; /* is the point a via point */ - dpPointRange viaRange; /* array of ranges */ - dpBoolean is_auto_wrap_point; /* was this point calc-ed by auto wrapper? */ - double wrap_distance; /* stores distance of wrap over object surface */ - double* wrap_pts; /* points wrapping over surface */ - int num_wrap_pts; /* number of points wrapping over surface */ - double undeformed_point[3]; /* 'point' prior to deformation */ - float normal[3]; /* point normal used for muscle surfaces */ -} dpMusclePoint; /* properties of a muscle point */ - -typedef struct -{ - int num_orig_points; /* number of user-defined muscle points */ - int num_points; /* total number of muscle points */ - dpMusclePoint* mp_orig; /* list of user-defined muscle points */ - dpMusclePoint** mp; /* list of muscle points after auto wrapping */ - int mp_orig_array_size; /* current size of mp_orig[] */ - int mp_array_size; /* current size of mp[] */ -} dpMusclePathStruct; - -typedef enum -{ - dpConstraintSphere = 0, - dpConstraintCylinder, - dpConstraintEllipsoid, - dpConstraintPlane, - dpConstraintNone -} dpConstraintObjectType; - -typedef struct -{ - char *name; - int segment; - double offset[3]; - double weight; - int constraint_num; -} dpConstraintPoint; - -typedef struct -{ - char *name; /* name of constraint object */ - dpConstraintObjectType constraint_type; /* type of constraint object */ - dpBoolean active; /* is this constraint active? */ - int segment; /* body segment object is fixed to */ - double co_radius[3]; /* constraint object radius */ - double height; /* constraint object height (cylinder only) */ - int constraint_axis; /* which axis to constrain over */ - int constraint_sign; /* which side of constraint object to use */ - int numPoints; /* number of constraint points */ - int ptIndex; /* index of first pt linked to this constraint */ - double from_local_xform[4][4]; /* parent-to-wrapobj transform matrix */ - dpPlaneStruct plane; /* plane parameters (plane only) */ - double to_local_xform[4][4]; /* wrapobj-to-parent transform matrix */ - int cp_array_size; /* size of array of constraint points */ - dpConstraintPoint *points; /* constraint points */ -} dpConstraintObject; - -typedef struct -{ - int start; /* input: wrap starting muscle point index */ - int end; /* input: wrap ending muscle point index */ - double* wrap_pts; /* output: array of wrapping path points */ - int num_wrap_pts; /* output: size of 'wrap_pts' array */ - double wrap_path_length; /* output: distance along curved r1->r2 path */ - double r1[4]; /* output: wrap tangent point nearest to p1 */ - double r2[4]; /* output: wrap tangent point nearest to p2 */ -} dpWrapParams; - -typedef enum -{ - dpWrapSphere, - dpWrapCylinder, - dpWrapEllipsoid, - dpWrapTorus, - dpWrapNone -} dpWrapObjectType; - -typedef struct -{ - char* name; /* name of wrap object */ - dpWrapObjectType wrap_type; /* type of wrap object */ - dpBoolean active; /* is this wrap object active? */ - int wrap_algorithm; /* default wrap algorithm for wrap object */ - int segment; /* body segment object is fixed to */ - double radius[3]; /* wrap object radius */ - double height; /* wrap object height (cylinder only) */ - int wrap_axis; /* which axis to wrap over X=0, Y=1, Z=2 */ - int wrap_sign; /* which side of wrap axis to use (0 for both) */ - double from_local_xform[4][4]; /* parent-to-wrapobj transform matrix */ - double to_local_xform[4][4]; /* wrapobj-to-parent transform matrix */ - long display_list; /* only used for cylinder */ - dpBoolean display_list_is_stale; /* does display list need updating? */ - dpBoolean visible; /* is the wrap object visible? */ - dpBoolean show_wrap_pts; /* draw wrap point xyz coordinates? */ - dpCoord3D rotation_axis; /* local-to-parent transform parameter */ - double rotation_angle; /* local-to-parent transform parameter */ - dpCoord3D translation; /* local-to-parent transform parameter */ - dpBoolean xforms_valid; /* do xform matrices need recalculation? */ - dpCoord3D undeformed_translation; /* translation before applying deform objects */ -} dpWrapObject; - -typedef struct -{ - dpWrapObject* wrap_object; /* muscle is auto-wrapped over this object */ - int wrap_algorithm; /* wrap algorithm for this muscle */ - int startPoint; /* wrap only those muscle segments between startPoint */ - int endPoint; /* and endPoint */ - double c[3]; /* previous c point */ - double r1[3]; /* previous r1 (tangent) point */ - double r2[3]; /* previous r2 (tangent) point */ - dpMusclePoint mp_wrap[2]; /* the two muscle points created when the muscle wraps */ -} dpMuscleWrapStruct; - -typedef struct mss -{ - char* name; /* name of muscle */ - int index; /* index of this muscle in model's array of muscles */ - dpBoolean display; /* whether or not to display this muscle */ - dpBoolean output; /* write muscle values to output file? */ - dpBoolean selected; /* whether or not this muscle is selected */ - dpMusclePathStruct *path; /* all the muscle path information */ - double* max_isometric_force; /* maximum isometric force */ - double* pennation_angle; /* pennation angle of muscle fibers */ - double* optimal_fiber_length; /* muscle fiber length */ - double* resting_tendon_length; /* resting length of tendon */ - double* max_contraction_vel; /* maximum contraction velocity */ - double muscle_tendon_length; /* musculotendon length */ - double fiber_length; /* muscle fiber length */ - double tendon_length; /* tendon length */ - double muscle_tendon_vel; /* musculotendon velocity */ - double fiber_velocity; /* muscle velocity */ - double tendon_velocity; /* tendon velocity */ - double force; /* force in musculotendon unit */ - double applied_power; /* power this muscle applies to body segments */ - double heat_energy; /* heat energy generated by this muscle */ - double mechanical_energy; /* energy this muscle gives to body segments */ - double energy; /* energy this muscle gives to body segments */ - int nummomentarms; /* number of moment arms (= # of gencoords) */ - double* momentarms; /* list of moment arm values */ - dpFunction** tendon_force_len_func; /* tendon force-length function */ - dpFunction** active_force_len_func; /* muscle active force-length function */ - dpFunction** passive_force_len_func; /* muscle passive force-length function */ - dpFunction** force_vel_func; /* muscle force-velocity function */ - int num_dynamic_params; /* size of dynamic_params array */ - char** dynamic_param_names; /* list of dynamic parameter names */ - double** dynamic_params; /* array of dynamic (muscle model) parameters */ - double dynamic_activation; /* dynamic value of muscle activation */ - dpFunction** excitation_func; /* excitation function */ - void** excitation_abscissa; /* excitation is function of this gencoord (or 'time') */ - double excitation_level; /* current level of excitation */ - int* muscle_model_index; /* index for deriv, init, & assign func arrays */ - dpBoolean wrap_calced; /* has wrapping been calculated/updated? */ - int numWrapStructs; /* number of wrap objects used for this muscle */ - dpMuscleWrapStruct** wrapStruct; /* holds information about the wrap objects */ - int numStateParams; /* number of initial state values stored in dllparams.txt */ - double* stateParams; /* array of initial state values in dllparams.txt */ - int numgroups; /* number of groups to which musc belongs*/ - int* group; /* list of muscle group numbers */ - double* min_thickness; /* minimum thickness of muscle line */ - double* max_thickness; /* maximum thickness of muscle line */ - int* min_material; /* material to draw with for activation = 0.0 */ - int* max_material; /* material to draw with for activation = 1.0 */ - struct mss* saved; /* pointer to the saved version of the muscle */ -} dpMuscleStruct; /* properties of a musculotendon unit */ - -typedef struct -{ - double x1; - double x2; - double y1; - double y2; - double z1; - double z2; -} dpBoundingCube; - -typedef struct -{ - int num_vertices; - int *vertex_index; - double normal[3]; - double d; -} dpPolygonStruct; - -typedef struct -{ - double coord[3]; - double normal[3]; - int polygon_count; - int* polygons; -} dpVertexStruct; - -typedef struct -{ - char* name; - int body_segment; - dpBoundingCube bc; - dpVertexStruct* vertex; - int num_vertices; - dpPolygonStruct* polygon; - int num_polygons; -} dpPolyhedronStruct; - -typedef enum -{ - dpUnconstrainedQ, /* free gencoord */ - dpConstrainedQ, /* constrained as a function of another gencoord */ - dpPrescribedQ, /* gencoord with prescribed pos, vel, and acc */ - dpFixedQ /* fixed gencoord, vel and acc = 0.0 */ -} dpQType; - -typedef struct -{ - char* name; /* gencoord name */ - dpQType type; /* gencoord type - free, fixed, prescribed, constrained */ - int joint; /* joint gencoord belongs to */ - int axis; - int q_ind; /* independent q a constrained/prescribed q is dependent upon */ - int constraint_num; - double initial_value; - double initial_velocity; - double conversion; - double range_start; - double range_end; - dpFunction* restraint_function; - dpFunction* min_restraint_function; - dpFunction* max_restraint_function; - dpFunction* constraint_function; - dpBoolean function_active; - dpBoolean output; - double torque; - double pd_stiffness; -} dpQStruct; - -typedef struct -{ - int segment; /* SD/FAST segment floor associated with */ - char* name; /* name of spring_floor */ - dpPlaneStruct plane; /* parameters of floor plane */ - dpPolyhedronStruct* ph; /* polyhedron from bone file */ -} dpSpringFloor; - -typedef struct -{ - int segment; /* SD/FAST segment force matte associated with */ - char* name; /* name of force_matte */ - dpPolyhedronStruct* ph; /* polyhedron from bone file */ -} dpForceMatte; /* used for applying ground-based forces to other segments */ - -typedef struct -{ - int segment; /* SD/FAST segment spring is attached to */ - int floor; /* index of corresponding floor object */ - char* floor_name; /* name of corresponding floor object */ - double point[3]; /* xyz coordinates of the spring point */ - double force[3]; /* spring force vector */ - double friction; /* coefficient of friction with floor */ - double param_a; /* spring parameters */ - double param_b; - double param_c; - double param_d; - double param_e; - double param_f; -} dpSpringStruct; - -typedef struct -{ - double point[3]; - double vector[3]; -} dpForceStruct; - -typedef struct -{ - char* name; - double mass; - double inertia[3][3]; - double mass_center[3]; - double body_to_joint[3]; - double inboard_to_joint[3]; - dpBoolean output; - dpBoolean contactable; - int num_objects; - dpPolyhedronStruct** object; - double contact_force[3]; - double impact_force[3]; - double impact_point[3]; - dpBoolean* contact_joints; -} dpBodyStruct; - -typedef struct -{ - int inboard_body; - int outboard_body; - int dof; - int first_q; - int quat; - double axes[6][3]; - double force[3]; - double torque[3]; - dpDOFType dof_type[6]; - dpJointType jnt_type; - dpBoolean loop_joint; -} dpJointStruct; - -typedef struct -{ - char* name; - int nq; - int nu; - int neq; - int num_muscles; - int num_body_segments; - int num_gencoords; - int num_joints; - int num_closed_loops; - int num_constraints; - int num_user_constraints; - int num_contacts; - int num_bilat_contacts; - int contacts_size; - int bilat_contacts_size; - dpContactInfo* contacts; - dpContactInfo* bilat_contacts; - int num_springs; /* number of spring elements for spring-based contact */ - int num_spring_floors; /* number of floor elements for spring-based contact */ - int spring_array_size; - int spring_floor_array_size; - dpSpringStruct* spring; /* spring points for use with spring-based contact */ - dpSpringFloor* spring_floor; /* spring floors for use with spring-based contact */ - int num_force_mattes; /* number of force mattes for applying ground-based forces */ - int force_matte_array_size; - dpForceMatte* force_matte; /* force mattes used for applying ground-based forces */ - dpQStruct* q; - dpBodyStruct* body_segment; - dpMuscleStruct* muscles; - dpMuscleStruct default_muscle; - dpJointStruct* joint; - int num_wrap_objects; - dpWrapObject* wrap_object; - int num_constraint_objects; - dpConstraintObject* constraint_object; - int function_array_size; - int num_functions; /* number of functions (constraint and moving muscle point) in the model */ - dpFunction** function; /* array of functions used by the model (for constraints and moving muscle points) */ - double gravity[3]; - dpSimmModelID simmModel; - int enforce_constraints; /* enforce constraint objects while integrating? */ - int newInverseSimulation; /* new-style inverse with corrective torques? */ -} dpModelStruct; - -DLL int dpInitSimulation(void* params); -DLL int dpSetModel(void* model); -DLL int dpRunSimulation(int); -DLL int dpPauseSimulation(int); -DLL int dpResetSimulation(int); -DLL int dpRegisterCallback(int (*dataCallback)(dpSimmModelID, dpDataType, void*)); -DLL int dpGetVersion(void); - -typedef int (*dpInitSimulationFunc)(void* params); //dpSimulationParameters -typedef int (*dpSetModelFunc)(void* model); // dpModelStruct -typedef int (*dpRunSimulationFunc)(int); -typedef int (*dpPauseSimulationFunc)(int); -typedef int (*dpResetSimulationFunc)(int); -typedef int (*dpRegisterCallbackFunc)(int (*dataCallback)(dpSimmModelID, dpDataType, void*)); -typedef int (*dpGetVersionFunc)(void); - -typedef struct -{ - dpInitSimulationFunc dpInitSimulation; - dpSetModelFunc dpSetModel; - dpRunSimulationFunc dpRunSimulation; - dpPauseSimulationFunc dpPauseSimulation; - dpResetSimulationFunc dpResetSimulation; - dpRegisterCallbackFunc dpRegisterCallback; - dpGetVersionFunc dpGetVersion; -} dpSimFuncs; - -#endif /*DP_H*/ diff --git a/OpenSim/Utilities/simmToOpenSim/dp420.c b/OpenSim/Utilities/simmToOpenSim/dp420.c deleted file mode 100644 index b322ab3faa..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/dp420.c +++ /dev/null @@ -1,1430 +0,0 @@ -/******************************************************************************* - - DP420.C - - Author: Peter Loan - - Date: 29-NOV-2009 - - Copyright (c) 2009 MusculoGraphics, Inc. - All rights reserved. - - Description: This file contains functions that convert a SIMM model into - a Dynamics Pipeline model for use with dynamics DLLs created by SIMM 4.2 and 4.2.1. - - Routines: - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "sdfunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ -extern SDSegment* SDseg; -extern int num_SD_segs; -extern int* joint_order; - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void freeDP420DefaultMuscle(dp420MuscleStruct* dm); -static void freeDP420Muscle(dp420MuscleStruct* ms, dp420MuscleStruct* dm); -static void freeDP420Function(dp420SplineFunction* sf); -static dp420WrapObject* getDP420WrapObject(dp420ModelStruct* dp, char name[]); -static void copyConstraintsToDP420Constraints(ModelStruct* ms, dp420ModelStruct* dp); -static void copyMuscPointToDP420MuscPoint(dpMusclePoint* from, dp420MusclePoint* to, ModelStruct* ms); -static void copyWrapObjectsToDP420(ModelStruct* ms, dp420ModelStruct* dp); -static void copyQsToDP420(ModelStruct* ms, dp420ModelStruct* dp); -static void copyFunctionToDP420Function(dpFunction* from, dp420SplineFunction** to); -static void copySegmentsToDP420(ModelStruct* ms, dp420ModelStruct* dp); -static void copyMusclesToDP420(ModelStruct* ms, dp420ModelStruct* dp, int muscleList[]); -static ReturnCode copy_nondefault_dyn_params420(dpMuscleStruct* from, dp420MuscleStruct* to, - dpMuscleStruct* deffrom, dp420MuscleStruct* defto); -static ReturnCode copy_nonnull_dyn_params420(dpMuscleStruct* from, dp420MuscleStruct* to); -static void copyDP420DefaultMuscle(dpMuscleStruct* from, dp420MuscleStruct* to, ModelStruct* ms, dp420ModelStruct* dp); -static void copyMuscleToDP420Muscle(dpMuscleStruct* from, dpMuscleStruct* defFrom, - dp420MuscleStruct* to, dp420MuscleStruct* defTo, - ModelStruct* ms, dp420ModelStruct* dp); -static int get_sd_floor_num420(dp420ModelStruct* dp, char* name); - - -static void freeDP420DefaultMuscle(dp420MuscleStruct* dm) -{ - if (dm) - { - int i; - - FREE_IFNOTNULL(dm->name); - if (dm->num_orig_points && dm->mp_orig) - { - for (i = 0; i < *dm->num_orig_points; i++) - { - FREE_IFNOTNULL(dm->mp_orig[i].ranges); - FREE_IFNOTNULL(dm->mp_orig[i].wrap_pts); - } - FREE_IFNOTNULL(dm->mp_orig); - FREE_IFNOTNULL(dm->num_orig_points); - } - FREE_IFNOTNULL(dm->mp); - FREE_IFNOTNULL(dm->max_isometric_force); - FREE_IFNOTNULL(dm->pennation_angle); - FREE_IFNOTNULL(dm->optimal_fiber_length); - FREE_IFNOTNULL(dm->resting_tendon_length); - FREE_IFNOTNULL(dm->max_contraction_vel); - FREE_IFNOTNULL(dm->momentarms); - if (dm->tendon_force_len_curve) - { - freeDP420Function(dm->tendon_force_len_curve); - FREE_IFNOTNULL(dm->tendon_force_len_curve); - } - if (dm->active_force_len_curve) - { - freeDP420Function(dm->active_force_len_curve); - FREE_IFNOTNULL(dm->active_force_len_curve); - } - if (dm->passive_force_len_curve) - { - freeDP420Function(dm->passive_force_len_curve); - FREE_IFNOTNULL(dm->passive_force_len_curve); - } - if (dm->force_vel_curve) - { - freeDP420Function(dm->force_vel_curve); - FREE_IFNOTNULL(dm->force_vel_curve); - } - - if (dm->dynamic_params) - { - for (i = 0; i < dm->num_dynamic_params; i++) - FREE_IFNOTNULL(dm->dynamic_params[i]); - FREE_IFNOTNULL(dm->dynamic_params); - } - FREE_IFNOTNULL(dm->excitation_format); - if (dm->excitation) - { - freeDP420Function(dm->excitation); - FREE_IFNOTNULL(dm->excitation); - } - FREE_IFNOTNULL(dm->muscle_model_index); - if (dm->wrapStruct) - { - for (i = 0; i < dm->numWrapStructs; i++) - { - FREE_IFNOTNULL(dm->wrapStruct[i]->wrap_object); - FREE_IFNOTNULL(dm->wrapStruct[i]); - } - FREE_IFNOTNULL(dm->wrapStruct); - } - } -} - - -static void freeDP420Muscle(dp420MuscleStruct* ms, dp420MuscleStruct* dm) -{ - if (ms && dm) - { - int i; - - if (ms->name != dm->name) - FREE_IFNOTNULL(ms->name); - if (ms->mp_orig && ms->num_orig_points && ms->mp_orig != dm->mp_orig) - { - for (i = 0; i < *ms->num_orig_points; i++) - { - FREE_IFNOTNULL(ms->mp_orig[i].ranges); - FREE_IFNOTNULL(ms->mp_orig[i].wrap_pts); - } - FREE_IFNOTNULL(ms->mp_orig); - } - if (ms->num_orig_points != dm->num_orig_points) - FREE_IFNOTNULL(ms->num_orig_points); - if (ms->mp != dm->mp) - FREE_IFNOTNULL(ms->mp); - if (ms->max_isometric_force != dm->max_isometric_force) - FREE_IFNOTNULL(ms->max_isometric_force); - if (ms->pennation_angle != dm->pennation_angle) - FREE_IFNOTNULL(ms->pennation_angle); - if (ms->optimal_fiber_length != dm->optimal_fiber_length) - FREE_IFNOTNULL(ms->optimal_fiber_length); - if (ms->resting_tendon_length != dm->resting_tendon_length) - FREE_IFNOTNULL(ms->resting_tendon_length); - if (ms->max_contraction_vel != dm->max_contraction_vel) - FREE_IFNOTNULL(ms->max_contraction_vel); - FREE_IFNOTNULL(ms->momentarms); - if (ms->tendon_force_len_curve && ms->tendon_force_len_curve != dm->tendon_force_len_curve) - { - freeDP420Function(ms->tendon_force_len_curve); - FREE_IFNOTNULL(ms->tendon_force_len_curve); - } - if (ms->active_force_len_curve && ms->active_force_len_curve != dm->active_force_len_curve) - { - freeDP420Function(ms->active_force_len_curve); - FREE_IFNOTNULL(ms->active_force_len_curve); - } - if (ms->passive_force_len_curve && ms->passive_force_len_curve != dm->passive_force_len_curve) - { - freeDP420Function(ms->passive_force_len_curve); - FREE_IFNOTNULL(ms->passive_force_len_curve); - } - if (ms->force_vel_curve && ms->force_vel_curve != dm->force_vel_curve) - { - freeDP420Function(ms->force_vel_curve); - FREE_IFNOTNULL(ms->force_vel_curve); - } - - /* The dynamic_params array (of pointers) is always unique to each muscle, - * but the doubles that they point to could be in the default muscle. - */ - if (ms->dynamic_params) - { - for (i = 0; i < ms->num_dynamic_params; i++) - { - if (ms->dynamic_params[i] != dm->dynamic_params[i]) - FREE_IFNOTNULL(ms->dynamic_params[i]); - } - FREE_IFNOTNULL(ms->dynamic_params); - } - if (ms->excitation_format != dm->excitation_format) - FREE_IFNOTNULL(ms->excitation_format); - if (ms->excitation && ms->excitation != dm->excitation) - { - freeDP420Function(ms->excitation); - FREE_IFNOTNULL(ms->excitation); - } - if (ms->muscle_model_index != dm->muscle_model_index) - FREE_IFNOTNULL(ms->muscle_model_index); - if (ms->wrapStruct && ms->wrapStruct != dm->wrapStruct) - { - for (i = 0; i < ms->numWrapStructs; i++) - { - //FREE_IFNOTNULL(ms->wrapStruct[i]->wrap_object); the wrap object is freed later - FREE_IFNOTNULL(ms->wrapStruct[i]); - } - FREE_IFNOTNULL(ms->wrapStruct); - } - } -} - - -static void freeDP420Function(dp420SplineFunction* sf) -{ - if (sf) - { - FREE_IFNOTNULL(sf->x); - FREE_IFNOTNULL(sf->y); - FREE_IFNOTNULL(sf->b); - FREE_IFNOTNULL(sf->c); - FREE_IFNOTNULL(sf->d); - } -} - -void freeDP420ModelStruct(dp420ModelStruct* dp) -{ - int i, j; - - FREE_IFNOTNULL(dp->name); - - if (dp->dynamic_param_names) - { - for (i = 0; i < dp->num_dynamic_params; i++) - FREE_IFNOTNULL(dp->dynamic_param_names[i]); - FREE_IFNOTNULL(dp->dynamic_param_names); - } - - FREE_IFNOTNULL(dp->contacts); - FREE_IFNOTNULL(dp->bilat_contacts); - - if (dp->spring) - { - for (i = 0; i < dp->num_springs; i++) - FREE_IFNOTNULL(dp->spring[i].floor_name); - FREE_IFNOTNULL(dp->spring); - } - - if (dp->spring_floor) - { - for (i = 0; i < dp->num_spring_floors; i++) - { - FREE_IFNOTNULL(dp->spring_floor[i].name); - if (dp->spring_floor[i].ph) - { - freeDPPolyhedron(dp->spring_floor[i].ph); - FREE_IFNOTNULL(dp->spring_floor[i].ph); - } - } - FREE_IFNOTNULL(dp->spring_floor); - } - - if (dp->force_matte) - { - for (i = 0; i < dp->num_force_mattes; i++) - { - FREE_IFNOTNULL(dp->force_matte[i].name); - if (dp->force_matte[i].ph) - { - freeDPPolyhedron(dp->force_matte[i].ph); - FREE_IFNOTNULL(dp->force_matte[i].ph); - } - } - FREE_IFNOTNULL(dp->force_matte); - } - - if (dp->q) - { - for (i = 0; i < dp->nq; i++) - { - FREE_IFNOTNULL(dp->q[i].name); - freeDP420Function(dp->q[i].restraint_func); - freeDP420Function(dp->q[i].min_restraint_func); - freeDP420Function(dp->q[i].max_restraint_func); - freeDP420Function(dp->q[i].constraint_func); - } - FREE_IFNOTNULL(dp->q); - } - - if (dp->body_segment) - { - for (i = 0; i < dp->num_body_segments; i++) - { - FREE_IFNOTNULL(dp->body_segment[i].name); - for (j = 0; j < dp->body_segment[i].num_objects; j++) - { - freeDPPolyhedron(dp->body_segment[i].object[j]); - FREE_IFNOTNULL(dp->body_segment[i].object[j]); - } - FREE_IFNOTNULL(dp->body_segment[i].object); - } - FREE_IFNOTNULL(dp->body_segment); - } - - if (dp->muscles) - { - for (i = 0; i < dp->num_muscles; i++) - freeDP420Muscle(&dp->muscles[i], &dp->default_muscle); - FREE_IFNOTNULL(dp->muscles); - } - - freeDP420DefaultMuscle(&dp->default_muscle); - - FREE_IFNOTNULL(dp->joint); - - if (dp->wrap_object) - { - for (i = 0; i < dp->num_wrap_objects; i++) - FREE_IFNOTNULL(dp->wrap_object[i].name); - FREE_IFNOTNULL(dp->wrap_object); - } - - if (dp->constraint_object) - { - for (i = 0; i < dp->num_constraint_objects; i++) - { - FREE_IFNOTNULL(dp->constraint_object[i].name); - for (j = 0; j < dp->constraint_object[i].numPoints; j++) - FREE_IFNOTNULL(dp->constraint_object[i].points[j].name); - FREE_IFNOTNULL(dp->constraint_object[i].points); - } - FREE_IFNOTNULL(dp->constraint_object); - } - - FREE_IFNOTNULL(dp); -} - -static dp420WrapObject* getDP420WrapObject(dp420ModelStruct* dp, char name[]) -{ - - int i; - - if (name == NULL) - return NULL; - - for (i = 0; i < dp->num_wrap_objects; i++) - if (STRINGS_ARE_EQUAL(name, dp->wrap_object[i].name)) - return &dp->wrap_object[i]; - - return NULL; -} - -static void copyConstraintsToDP420Constraints(ModelStruct* ms, dp420ModelStruct* dp) -{ - int i, j, k, constraintNum; - double tmp_mat[4][4], tmp_inv[4][4]; - - /* Always turn on enforce_constraints for the dpModel (whether or not there - * are any active constraint objects). This parameter is only turned off by - * the simulation code if instructed to do so by a motion. - */ - dp->enforce_constraints = 1; - - /* The SD/FAST constraint numbers for the constraint points start after - * the last Q constraint, and are numbered consecutively. - */ - constraintNum = dp->nq - (ms->numgencoords - ms->numunusedgencoords); - - dp->num_constraint_objects = ms->num_constraint_objects; - if (dp->num_constraint_objects > 0) - { - dp->constraint_object = (dpConstraintObject*)simm_malloc(dp->num_constraint_objects * sizeof(dpConstraintObject)); - for (i = 0; i < dp->num_constraint_objects; i++) - { - ConstraintObject* from = &ms->constraintobj[i]; - dpConstraintObject* to = &dp->constraint_object[i]; - SegmentStruct* seg = &ms->segment[from->segment]; - mstrcpy(&to->name, from->name); - to->constraint_type = from->constraintType; - to->active = from->active; - to->segment = get_sd_seg_num(seg->name); - for (j = 0; j < 3; j++) - to->co_radius[j] = from->radius.xyz[j]; - to->height = from->height; - to->constraint_axis = from->constraintAxis; - to->constraint_sign = from->constraintSign; - to->numPoints = to->cp_array_size = from->numPoints; - - /* Form new transform matrices from the mass center of the segment to the - * constraint object, rather than from the origin of the segment. SD/FAST expects - * all segment-specific points (e.g., muscle points) to be specified w.r.t. - * the segment's mass center. - */ - copy_4x4matrix(from->from_local_xform, tmp_mat); - for (j = 0; j < 3; j++) - tmp_mat[3][j] -= seg->masscenter[j]; - invert_4x4transform(tmp_mat, tmp_inv); - copy_4x4matrix(tmp_mat, to->from_local_xform); - copy_4x4matrix(tmp_inv, to->to_local_xform); - to->plane.a = from->plane.a; - to->plane.b = from->plane.b; - to->plane.c = from->plane.c; - to->plane.d = from->plane.d; - - to->points = (dpConstraintPoint*)simm_malloc(to->numPoints * sizeof(dpConstraintPoint)); - for (j = 0; j < to->numPoints; j++) - { - seg = &ms->segment[from->points[j].segment]; - mstrcpy(&to->points[j].name, from->points[j].name); - to->points[j].segment = get_sd_seg_num(seg->name); - to->points[j].offset[XX] = from->points[j].offset[XX] - seg->masscenter[XX]; - to->points[j].offset[YY] = from->points[j].offset[YY] - seg->masscenter[YY]; - to->points[j].offset[ZZ] = from->points[j].offset[ZZ] - seg->masscenter[ZZ]; - to->points[j].weight = from->points[j].weight; - to->points[j].constraint_num = constraintNum++; - } - } - } - else - { - dp->constraint_object = NULL; - } -} - -static void copyMuscPointToDP420MuscPoint(dpMusclePoint* from, dp420MusclePoint* to, ModelStruct* ms) -{ - int i; - DofStruct* dof; - - to->segment = get_sd_seg_num(ms->segment[from->segment].name); - to->refpt = -1; - to->selected = from->selected; - to->point[XX] = from->point[XX] - ms->segment[from->segment].masscenter[XX]; - to->point[YY] = from->point[YY] - ms->segment[from->segment].masscenter[YY]; - to->point[ZZ] = from->point[ZZ] - ms->segment[from->segment].masscenter[ZZ]; - if (from->isVia == dpYes) - { - to->numranges = 1; - to->ranges = (dp420PointRange*)simm_malloc(sizeof(dp420PointRange)); - /* Find the SIMM DOF which uses the gencoord as an unconstrained Q. */ - dof = find_unconstrained_sd_dof(ms, (GeneralizedCoord*)from->viaRange.gencoord); - if (dof) - to->ranges[0].genc = dof->sd.state_number; - else - to->ranges[0].genc = 0; // TODO: serious error is index of gencoord in DP cannot be found! - to->ranges[0].start = from->viaRange.start; - to->ranges[0].end = from->viaRange.end; - } - else - { - to->numranges = 0; - to->ranges = NULL; - } - - to->is_auto_wrap_point = from->is_auto_wrap_point; - to->wrap_distance = from->wrap_distance; - to->num_wrap_pts = from->num_wrap_pts; - if (to->num_wrap_pts > 0) - { - to->wrap_pts = (double*)simm_malloc(to->num_wrap_pts * 3 * sizeof(double)); - for (i = 0; i < to->num_wrap_pts * 3; i++) - to->wrap_pts[i] = from->wrap_pts[i]; - } - else - to->wrap_pts = NULL; -} - - -static void copyWrapObjectsToDP420(ModelStruct* ms, dp420ModelStruct* dp) -{ - int i; - - dp->num_wrap_objects = ms->num_wrap_objects; - - if (dp->num_wrap_objects == 0) - { - dp->wrap_object = NULL; - return; - } - - dp->wrap_object = (dp420WrapObject*)simm_malloc(dp->num_wrap_objects * sizeof(dp420WrapObject)); - - for (i = 0; i < dp->num_wrap_objects; i++) - { - dpWrapObject* from = ms->wrapobj[i]; - dp420WrapObject* to = &dp->wrap_object[i]; - - mstrcpy(&to->name, from->name); - to->wrap_type = from->wrap_type; - to->active = from->active; - to->wrap_algorithm = from->wrap_algorithm; - to->segment = get_sd_seg_num(ms->segment[from->segment].name); - to->radius[0] = from->radius[0]; - to->radius[1] = from->radius[1]; - to->radius[2] = from->radius[2]; - to->height = from->height; - to->wrap_axis = from->wrap_axis; - to->wrap_sign = from->wrap_sign; - copy_4x4matrix(from->from_local_xform, to->from_local_xform); - to->from_local_xform[3][XX] -= ms->segment[from->segment].masscenter[XX]; - to->from_local_xform[3][YY] -= ms->segment[from->segment].masscenter[YY]; - to->from_local_xform[3][ZZ] -= ms->segment[from->segment].masscenter[ZZ]; - invert_4x4transform(to->from_local_xform, to->to_local_xform); - } -} - - -static void copyQsToDP420(ModelStruct* ms, dp420ModelStruct* dp) -{ - int i, j; - DofStruct* dof; - JointStruct* jnt; - dp420QStruct* q; - GeneralizedCoord* gc; - - dp->nu = dp->nq; - dp->num_gencoords = 0; - - if (dp->nq == 0) - { - dp->q = NULL; - return; - } - - dp->q = (dp420QStruct*)simm_malloc(dp->nq * sizeof(dp420QStruct)); - - for (i = 0; i < dp->nq; i++) - { - dof = find_nth_q_dof(ms, i); - jnt = find_nth_q_joint(ms, i); - if (dof->gencoord) //dkb - gc = dof->gencoord; - else - gc = NULL;//dkb - q = &dp->q[i]; - - mstrcpy(&q->name, dof->sd.name); - - if (dof->sd.fixed == yes) - { - q->type = dpFixedQ; - } - else if (dof->sd.constrained == no) - { - /* Locked gencoords are modeled as fixed Qs (as of version 4.1.1). */ - if (dof->gencoord && gc->locked == yes) - q->type = dpFixedQ; - else - q->type = dpUnconstrainedQ; - } - else - { - q->type = dpConstrainedQ; - } - q->joint = jnt->sd_num; - q->axis = dof->sd.axis; - q->conversion = dof->sd.conversion; - q->initial_value = dof->sd.initial_value; - q->initial_velocity = 0.0; - - if (dof->sd.constrained == no && dof->sd.fixed == no) - { - q->range_start = gc->range.start; - q->range_end = gc->range.end; - q->pd_stiffness = gc->pd_stiffness; - } - else - { - q->range_start = -99999.9; - q->range_end = 99999.9; - q->pd_stiffness = 0.0; - } - if (dof->sd.fixed == yes || dof->sd.constrained == yes) - { - q->restraint_func = NULL; - q->min_restraint_func = NULL; - q->max_restraint_func = NULL; - q->function_active = dpNo; - } - else - { - if (gc->restraint_function) - { - /* The index of the function in the ModelStruct and the dpModelStruct - * should be the same. - */ - q->restraint_func = &dp->constraint_function[getFunctionIndex(ms, gc->restraint_function)]; - q->min_restraint_func = NULL; - q->max_restraint_func = NULL; - if (gc->restraintFuncActive == yes) - q->function_active = dpYes; - else - q->function_active = dpNo; - } - else - { - q->restraint_func = NULL; - q->function_active = dpNo; - - /* The index of the functions in the ModelStruct and the dpModelStruct - * should be the same. - */ - if (gc->min_restraint_function) - q->min_restraint_func = &dp->constraint_function[getFunctionIndex(ms, gc->min_restraint_function)]; - else - q->min_restraint_func = NULL; - - if (gc->max_restraint_function) - q->max_restraint_func = &dp->constraint_function[getFunctionIndex(ms, gc->max_restraint_function)]; - else - q->max_restraint_func = NULL; - } - } - if (dof->sd.constrained == yes) - { - DofStruct* ind_dof = find_unconstrained_sd_dof(ms, dof->gencoord); - /* The index of the functions in the ModelStruct and the dpModelStruct should be the same. */ - q->constraint_func = &dp->constraint_function[getFunctionIndex(ms, dof->function)]; - q->constraint_num = dof->sd.error_number; - q->q_ind = ind_dof->sd.state_number; - } - else - { - q->constraint_func = NULL; - q->constraint_num = -1; - q->q_ind = -1; - } - - if (dof->sd.fixed == yes || dof->sd.constrained == yes) - q->output = dpNo; - else - q->output = dpYes; - - q->torque = 0.0; - - if (q->type == dpUnconstrainedQ) - dp->num_gencoords++; - } -} - -static void copyFunctionToDP420Function(dpFunction* from, dp420SplineFunction** to) -{ - dp420SplineFunction* f; - - f = *to = (dp420SplineFunction*)simm_calloc(1, sizeof(dp420SplineFunction)); - - f->type = from->type; - - f->cutoff_frequency = 0; - f->usernum = 0; - f->defined = dpYes; - - f->numpoints = f->coefficient_array_size = from->numpoints; - f->x = (double*)simm_calloc(f->numpoints, sizeof(double)); - f->y = (double*)simm_calloc(f->numpoints, sizeof(double)); - f->b = (double*)simm_calloc(f->numpoints, sizeof(double)); - f->c = (double*)simm_calloc(f->numpoints, sizeof(double)); - f->d = (double*)simm_calloc(f->numpoints, sizeof(double)); - - /* b, c, and d are not copied because they are recalculated - * by the simulation. - */ - memcpy(f->x, from->x, f->numpoints * sizeof(double)); - memcpy(f->y, from->y, f->numpoints * sizeof(double)); -} - - -static void copySegmentsToDP420(ModelStruct* ms, dp420ModelStruct* dp) -{ - int i, j, k; - - dp->num_body_segments = num_SD_segs; - - dp->body_segment = (dpBodyStruct*)simm_malloc(dp->num_body_segments * sizeof(dpBodyStruct)); - - for (i = 0; i < num_SD_segs; i++) - { - mstrcpy(&dp->body_segment[i].name, SDseg[i].name); - dp->body_segment[i].output = (i > 0) ? dpYes : dpNo; - dp->body_segment[i].mass = SDseg[i].mass; - for (j = 0; j < 3; j++) - { - dp->body_segment[i].mass_center[j] = SDseg[i].mass_center[j]; - dp->body_segment[i].body_to_joint[j] = SDseg[i].body_to_joint[j]; - dp->body_segment[i].inboard_to_joint[j] = SDseg[i].inboard_to_joint[j]; - } - for (j = 0; j < 3; j ++) - for (k = 0; k < 3; k++) - dp->body_segment[i].inertia[j][k] = SDseg[i].inertia[j][k]; - - /* These fields are filled in later by the simulation code. */ - dp->body_segment[i].contactable = dpNo; - dp->body_segment[i].contact_joints = NULL; - for (j = 0; j < 3; j++) - { - dp->body_segment[i].contact_force[j] = 0.0; - dp->body_segment[i].impact_force[j] = 0.0; - dp->body_segment[i].impact_point[j] = 0.0; - } - - /* If a SIMM segment with contactable objects gets split into more - * than one SD segment, you want to copy the objects only to the - * [one] SD segment with a valid simm_segment value. - */ - if (SDseg[i].simm_segment >= 0) - { - SegmentStruct* seg = &ms->segment[SDseg[i].simm_segment]; - dp->body_segment[i].object = (dpPolyhedronStruct**)simm_malloc(seg->numContactObjects * sizeof(dpPolyhedronStruct*)); - dp->body_segment[i].num_objects = seg->numContactObjects; - for (j = 0; j < seg->numContactObjects; j++) - copyPolyhedronToDPPolyhedron(seg->contactObject[j].poly, &dp->body_segment[i].object[j], - SDseg[i].simm_segment, i - 1, ms); - } - else - { - dp->body_segment[i].num_objects = 0; - dp->body_segment[i].object = NULL; - } - } -} - - -static void copyMusclesToDP420(ModelStruct* ms, dp420ModelStruct* dp, int muscleList[]) -{ - int i, index; - - dp->num_muscles = 0; - dp->muscles = NULL; - - if (muscleList == NULL) - return; - - for (i = 0; i < ms->nummuscles; i++) - if (muscleList[i]) - dp->num_muscles++; - - /* If there are no muscles to copy, don't copy the default muscle; just return. */ - if (dp->num_muscles == 0) - return; - - dp->muscles = (dp420MuscleStruct*)simm_malloc(dp->num_muscles * sizeof(dp420MuscleStruct)); - - copyDP420DefaultMuscle(ms->default_muscle, &dp->default_muscle, ms, dp); - - for (i = 0, index = 0; i < ms->nummuscles; i++) - { - if (muscleList[i]) - copyMuscleToDP420Muscle(ms->muscle[i], ms->default_muscle, &dp->muscles[index++], &dp->default_muscle, ms, dp); - } -} - - -static ReturnCode copy_nondefault_dyn_params420(dpMuscleStruct* from, dp420MuscleStruct* to, - dpMuscleStruct* deffrom, dp420MuscleStruct* defto) -{ - int i; - - to->num_dynamic_params = from->num_dynamic_params; - - if (to->num_dynamic_params > 0) - { - // A muscle's param name array is always the same as its default muscle's. - to->dynamic_param_names = defto->dynamic_param_names; - to->dynamic_params = (double**)simm_malloc(to->num_dynamic_params * sizeof(double*)); - - if (to->dynamic_params == NULL) - return code_bad; - - for (i = 0; i < to->num_dynamic_params; i++) - { - if (copy_nddouble(from->dynamic_params[i], &to->dynamic_params[i], - deffrom->dynamic_params[i], defto->dynamic_params[i]) == code_bad) - return code_bad; - } - } - else - { - to->dynamic_param_names = NULL; - to->dynamic_params = NULL; - } - - return code_fine; -} - - -/* This function copies the parameter names and all of the non-null values. - */ -static ReturnCode copy_nonnull_dyn_params420(dpMuscleStruct* from, dp420MuscleStruct* to) -{ - to->num_dynamic_params = from->num_dynamic_params; - - if (to->num_dynamic_params > 0) - { - int i; - - to->dynamic_params = (double**)simm_malloc(to->num_dynamic_params * sizeof(double*)); - if (to->dynamic_params == NULL) - return code_bad; - - for (i = 0; i < to->num_dynamic_params; i++) - { - if (copy_nndouble(from->dynamic_params[i], &to->dynamic_params[i]) == code_bad) - return code_bad; - } - } - else - { - to->dynamic_params = NULL; - } - - return code_fine; -} - - -static void copyDP420DefaultMuscle(dpMuscleStruct* from, dp420MuscleStruct* to, ModelStruct* ms, dp420ModelStruct* dp) -{ - int i; - - if (from->name == NULL) - to->name = NULL; - else - mstrcpy(&to->name, from->name); - - to->has_wrapping_points = dpNo; // deprecated in SIMM 5.0 and not used by Pipeline 4.2 - to->has_force_points = dpNo; // deprecated in SIMM 5.0 and not used by Pipeline 4.2 - - if (from->path == NULL || from->path->num_orig_points == 0) - { - to->num_orig_points = NULL; - to->mp_orig = NULL; - } - else - { - to->num_orig_points = (int*)simm_malloc(sizeof(int)); - *to->num_orig_points = from->path->num_orig_points; - to->mp_orig = (dp420MusclePoint*)simm_malloc((*to->num_orig_points) * sizeof(dp420MusclePoint)); - for (i = 0; i < *to->num_orig_points; i++) - copyMuscPointToDP420MuscPoint(&from->path->mp_orig[i], &to->mp_orig[i], ms); - } - - to->nummomentarms = 0; - to->momentarms = NULL; - - copy_nndouble(from->max_isometric_force, &to->max_isometric_force); - copy_nndouble(from->pennation_angle, &to->pennation_angle); - copy_nndouble(from->max_contraction_vel, &to->max_contraction_vel); - copy_nndouble(from->optimal_fiber_length,&to->optimal_fiber_length); - copy_nndouble(from->resting_tendon_length,&to->resting_tendon_length); - - // For the names of the dynamic parameters, use the array in the model struct. - to->dynamic_param_names = dp->dynamic_param_names; - - copy_nonnull_dyn_params420(from, to); - - if (from->active_force_len_func == NULL) - to->active_force_len_curve = NULL; - else - copyFunctionToDP420Function(*from->active_force_len_func, &to->active_force_len_curve); - - if (from->passive_force_len_func == NULL) - to->passive_force_len_curve = NULL; - else - copyFunctionToDP420Function(*from->passive_force_len_func, &to->passive_force_len_curve); - - if (from->tendon_force_len_func == NULL) - to->tendon_force_len_curve = NULL; - else - copyFunctionToDP420Function(*from->tendon_force_len_func, &to->tendon_force_len_curve); - - if (from->force_vel_func == NULL) - to->force_vel_curve = NULL; - else - copyFunctionToDP420Function(*from->force_vel_func, &to->force_vel_curve); - - if (from->excitation_abscissa) - { - if (*from->excitation_abscissa == TIME) - to->excitation_abscissa = -2; - else - { - DofStruct* dof = find_unconstrained_sd_dof(ms, (GeneralizedCoord*)(*from->excitation_abscissa)); - if (dof) - to->excitation_abscissa = dof->sd.state_number; - else - to->excitation_abscissa = 0; - } - } - else - { - to->excitation_abscissa = -2; - } - - if (from->excitation_func == NULL) - { - to->excitation = NULL; - to->excitation_format = NULL; - } - else - { - copyFunctionToDP420Function(*from->excitation_func, &to->excitation); - to->excitation_format = (dpFunctionType*)simm_malloc(sizeof(dpFunctionType)); - *to->excitation_format = (*from->excitation_func)->type; - } - - copy_nnint(from->muscle_model_index, &to->muscle_model_index); - /* In the Pipeline, muscle_model_index is zero-based. */ - if (to->muscle_model_index) - (*to->muscle_model_index)--; - - /* These are set by when simulation when reading dllparams.txt, but - * for the default muscle they are never used. - */ - to->numStateParams = 0; - to->stateParams = NULL; -} - - -static void copyMuscleToDP420Muscle(dpMuscleStruct* from, dpMuscleStruct* defFrom, - dp420MuscleStruct* to, dp420MuscleStruct* defTo, - ModelStruct* ms, dp420ModelStruct* dp) -{ - int i; - - /* Start by zero-ing out the entire muscle structure. */ - memset(to, 0, sizeof(dp420MuscleStruct)); - - if (from->name == defFrom->name) - to->name = defTo->name; - else - mstrcpy(&to->name, from->name); - - to->display = from->display; - to->output = from->output; - to->selected = from->selected; - to->has_wrapping_points = dpNo; // deprecated in SIMM 5.0 and not used by Pipeline 4.2 - to->has_force_points = dpNo; // deprecated in SIMM 5.0 and not used by Pipeline 4.2 - - if (from->path == NULL || from->path->num_orig_points == 0) - { - to->num_orig_points = NULL; - to->mp_orig = NULL; - } - else - { - to->num_orig_points = (int*)simm_malloc(sizeof(int)); - *to->num_orig_points = from->path->num_orig_points; - to->mp_orig = (dp420MusclePoint*)simm_malloc((*to->num_orig_points) * sizeof(dp420MusclePoint)); - for (i = 0; i < *to->num_orig_points; i++) - copyMuscPointToDP420MuscPoint(&from->path->mp_orig[i], &to->mp_orig[i], ms); - } - - if (from->max_isometric_force == defFrom->max_isometric_force) - to->max_isometric_force = defTo->max_isometric_force; - else - { - to->max_isometric_force = (double*)simm_malloc(sizeof(double)); - *to->max_isometric_force = *from->max_isometric_force; - } - - if (from->pennation_angle == defFrom->pennation_angle) - to->pennation_angle = defTo->pennation_angle; - else - { - to->pennation_angle = (double*)simm_malloc(sizeof(double)); - *to->pennation_angle = *from->pennation_angle; - } - - if (from->optimal_fiber_length == defFrom->optimal_fiber_length) - to->optimal_fiber_length = defTo->optimal_fiber_length; - else - { - to->optimal_fiber_length = (double*)simm_malloc(sizeof(double)); - *to->optimal_fiber_length = *from->optimal_fiber_length; - } - - if (from->resting_tendon_length == defFrom->resting_tendon_length) - to->resting_tendon_length = defTo->resting_tendon_length; - else - { - to->resting_tendon_length = (double*)simm_malloc(sizeof(double)); - *to->resting_tendon_length = *from->resting_tendon_length; - } - - if (from->tendon_force_len_func == defFrom->tendon_force_len_func) - to->tendon_force_len_curve = defTo->tendon_force_len_curve; - else - copyFunctionToDP420Function(*from->tendon_force_len_func, &to->tendon_force_len_curve); - - if (from->active_force_len_func == defFrom->active_force_len_func) - to->active_force_len_curve = defTo->active_force_len_curve; - else - copyFunctionToDP420Function(*from->active_force_len_func, &to->active_force_len_curve); - - if (from->passive_force_len_func == defFrom->passive_force_len_func) - to->passive_force_len_curve = defTo->passive_force_len_curve; - else - copyFunctionToDP420Function(*from->passive_force_len_func, &to->passive_force_len_curve); - - if (from->force_vel_func == defFrom->force_vel_func) - to->force_vel_curve = defTo->force_vel_curve; - else - copyFunctionToDP420Function(*from->force_vel_func, &to->force_vel_curve); - - if (from->max_contraction_vel == defFrom->max_contraction_vel) - to->max_contraction_vel = defTo->max_contraction_vel; - else - { - to->max_contraction_vel = (double*)simm_malloc(sizeof(double)); - *to->max_contraction_vel = *from->max_contraction_vel; - } - - copy_nondefault_dyn_params420(from, to, defFrom, defTo); - - if (from->excitation_abscissa == defFrom->excitation_abscissa) - { - to->excitation_abscissa = defTo->excitation_abscissa; - } - else if (from->excitation_abscissa) - { - DofStruct* dof = find_unconstrained_sd_dof(ms, (GeneralizedCoord*)(*from->excitation_abscissa)); - if (dof) - to->excitation_abscissa = dof->sd.state_number; - else - to->excitation_abscissa = 0; - } - else - { - to->excitation_abscissa = -2; - } - - if (from->excitation_func == defFrom->excitation_func) - { - to->excitation = defTo->excitation; - to->excitation_format = defTo->excitation_format; - } - else - { - copyFunctionToDP420Function(*from->excitation_func, &to->excitation); - to->excitation_format = (dpFunctionType*)simm_malloc(sizeof(dpFunctionType)); - *to->excitation_format = (*from->excitation_func)->type; - } - - if (from->muscle_model_index == defFrom->muscle_model_index) - to->muscle_model_index = defTo->muscle_model_index; - else - { - to->muscle_model_index = (int*)simm_malloc(sizeof(int)); - *to->muscle_model_index = *from->muscle_model_index - 1; - } - - to->numWrapStructs = from->numWrapStructs; - if (from->wrapStruct == defFrom->wrapStruct) - to->wrapStruct = defTo->wrapStruct; - else - { - to->wrapStruct = (dp420MuscleWrapStruct**)simm_malloc(to->numWrapStructs * sizeof(dp420MuscleWrapStruct*)); - for (i = 0; i < to->numWrapStructs; i++) - { - to->wrapStruct[i] = (dp420MuscleWrapStruct*)simm_calloc(1, sizeof(dp420MuscleWrapStruct)); - to->wrapStruct[i]->wrap_algorithm = from->wrapStruct[i]->wrap_algorithm; - to->wrapStruct[i]->startPoint = from->wrapStruct[i]->startPoint; - to->wrapStruct[i]->endPoint = from->wrapStruct[i]->endPoint; - to->wrapStruct[i]->wrap_object = getDP420WrapObject(dp, from->wrapStruct[i]->wrap_object->name); - } - } - - to->mp_array_size = *(to->num_orig_points) + (to->numWrapStructs * 2); - to->mp = (dp420MusclePoint**)simm_malloc(sizeof(dp420MusclePoint*) * to->mp_array_size); - - /* When wrapping is calculated by the simulation, the mp[] array will be filled in - * and num_points set appropriately. - */ - to->num_points = 0; - - /* Copy the muscle's excitation format into the excitation spline structure, - * whether or not it comes from the default muscle. - */ - if (to->excitation && to->excitation_format) - to->excitation->type = *to->excitation_format; - - /* These are set by when simulation when reading dllparams.txt. */ - to->numStateParams = 0; - to->stateParams = NULL; -} - - -dp420ModelStruct* copyModelToDP420Model(ModelStruct* ms, int muscleList[]) -{ - int i, j, k, segNum, count; - ReturnCode rc; - dp420QStruct* gencoord; - GeneralizedCoord* temp_gc; - dp420QStruct* temp_q; - dp420ModelStruct* dp; - - dp = (dp420ModelStruct*)simm_calloc(1, sizeof(dp420ModelStruct)); - - dp->simmModel = (dpSimmModelID)ms; - - /* This is only set to yes by set_up_kinetics_input() in the - * simulation code. - */ - dp->newInverseSimulation = dpNo; - - /* To fill in the dpModelStruct, you need to know how - * the SIMM segments/joints are mapped to the SD/FAST segments/joints. - * If there are loops in the model, this is not a one-to-one mapping. - * So you need to call make_sdfast_model() to create the mapping. - */ - - check_dynamic_params(ms); - if (ms->dynamics_ready == no) - { - sprintf(errorbuffer, "Unable to create dynamic simulation for %s", ms->name); - error(none, errorbuffer); - return NULL; - } - - for (i = 0; i < ms->numjoints; i++) - ms->joint[i].type = identify_joint_type(ms, i); - - if (valid_sdfast_model(ms) == no) - { - sprintf(buffer, "Unable to create dynamic simulation for %s", ms->name); - error(none, buffer); - error(none, errorbuffer); /* errorbuffer was filled in by valid_sdfast_model() */ - error(none, "Consult the SD/FAST manual or email technical support for more details."); - return NULL; - } - - if (make_sdfast_model(ms, NULL, no, 0) != code_fine) - { - sprintf(errorbuffer, "Unable to create dynamic simulation for %s", ms->name); - error(none, errorbuffer); - return NULL; - } - - /* If you made it to here, then SDseg[] was filled in with the segment info, - * and the Q info was stored in the model's joint array. So now you can copy - * the info to the dpModelStruct. - */ - - mstrcpy(&dp->name, ms->name); - - /* Set the gravity vector appropriately. */ - switch (ms->gravity) - { - case smNoAlign: - dp->gravity[0] = dp->gravity[1] = dp->gravity[2] = 0.0; - break; - case smX: - dp->gravity[1] = dp->gravity[2] = 0.0; - dp->gravity[0] = 9.80665; - break; - case smNegX: - dp->gravity[1] = dp->gravity[2] = 0.0; - dp->gravity[0] = -9.80665; - break; - case smY: - dp->gravity[0] = dp->gravity[2] = 0.0; - dp->gravity[1] = 9.80665; - break; - case smNegY: - dp->gravity[0] = dp->gravity[2] = 0.0; - dp->gravity[1] = -9.80665; - break; - case smZ: - dp->gravity[0] = dp->gravity[1] = 0.0; - dp->gravity[2] = 9.80665; - break; - case smNegZ: - default: - dp->gravity[0] = dp->gravity[1] = 0.0; - dp->gravity[2] = -9.80665; - break; - } - - dp->num_closed_loops = ms->numclosedloops; - - /* Most of the Q struct has to be copied after the constraint functions, because it contains - * pointers to functions. However, the number of Qs is needed for copying constraint - * objects, so it is calculated here. - */ - for (i = 0, dp->nq = 0; i < ms->numjoints; i++) - { - for (j = 0; j < 6; j++) - if (ms->joint[i].dofs[j].type == function_dof || ms->joint[i].dofs[j].sd.fixed == yes) - dp->nq++; - } - - /* Copy the segment info. */ - copySegmentsToDP420(ms, dp); - - /* The entire array of joint structs is created and filled in by - * init_joints() in the simulation code, using the information - * returned by sdjnt(). However, some of the joint information is - * filled in here so that the simulation can make sure that the - * passed-in model matches the one built from model.sd. - */ - dp->num_joints = 0; - if (ms->numjoints > 0 && joint_order) - { - dp->joint = (dpJointStruct*)simm_calloc(ms->numjoints, sizeof(dpJointStruct)); - for (i = 0; i < ms->numjoints; i++) - { - if (joint_order[i] >= 0 && ms->joint[joint_order[i]].type != dpSkippable && - ms->joint[joint_order[i]].type != dpUnknownJoint) - dp->joint[dp->num_joints++].jnt_type = ms->joint[joint_order[i]].type; - } - dp->joint = (dpJointStruct*)simm_realloc(dp->joint, dp->num_joints * sizeof(dpJointStruct), &rc); - } - else - { - dp->joint = NULL; - } - - /* Copy the wrap objects. */ - copyWrapObjectsToDP420(ms, dp); - - // SIMM 5.0 models do not hold dynamic_param_names in the model struct, - // and they are not needed in the 4.2 model struct (they are also stored - // in the default muscle). - dp->num_dynamic_params = 0; - dp->dynamic_param_names = NULL; - - /* Copy the spring floors. */ - /* First count how many floors there are, since they are not stored in one array. - * Each body segment has either 0 or 1 floors. Make sure the floor has a valid - * polyhedron, or else skip over it. - */ - for (i = 0, count = 0; i < ms->numsegments; i++) - if (ms->segment[i].springFloor && ms->segment[i].springFloor->poly) - count++; - - dp->num_spring_floors = dp->spring_floor_array_size = count; - if (dp->num_spring_floors > 0) - { - dp->spring_floor = (dpSpringFloor*)simm_malloc(dp->num_spring_floors * sizeof(dpSpringFloor)); - for (i = 0, count = 0; i < ms->numsegments; i++) - { - SpringFloor* floor = ms->segment[i].springFloor; - if (floor && floor->poly) - { - dp->spring_floor[count].segment = get_sd_seg_num(ms->segment[i].name); - mstrcpy(&dp->spring_floor[count].name, floor->name); - /* The plane information is taken from the polyhedron's first polygon. */ - copyPolyhedronToDPPolyhedron(floor->poly, &dp->spring_floor[count].ph, i, dp->spring_floor[count].segment, ms); - dp->spring_floor[count].plane.a = dp->spring_floor[count].ph->polygon[0].normal[0]; - dp->spring_floor[count].plane.b = dp->spring_floor[count].ph->polygon[0].normal[1]; - dp->spring_floor[count].plane.c = dp->spring_floor[count].ph->polygon[0].normal[2]; - dp->spring_floor[count].plane.d = dp->spring_floor[count].ph->polygon[0].d; - count++; - } - } - } - else - { - dp->spring_floor = NULL; - } - - /* Copy the spring points. - * First count how many points there are, since they are not stored in one array. - * But only count the spring points that are linked to a spring floor that has - * a valid polyhedron. - */ - for (i = 0, count = 0; i < ms->numsegments; i++) - { - for (j = 0; j < ms->segment[i].numSpringPoints; j++) - { - segNum = ms->segment[i].springPoint[j].floorSegment; - if (ms->segment[segNum].springFloor && ms->segment[segNum].springFloor->poly) - count++; - } - } - - dp->num_springs = dp->spring_array_size = count; - if (dp->num_springs > 0) - { - dp->spring = (dpSpringStruct*)simm_malloc(dp->num_springs * sizeof(dpSpringStruct)); - for (i = 0, count = 0; i < ms->numsegments; i++) - { - SegmentStruct* seg = &ms->segment[i]; - for (j = 0; j < seg->numSpringPoints; j++) - { - segNum = seg->springPoint[j].floorSegment; - if (ms->segment[segNum].springFloor && ms->segment[segNum].springFloor->poly) - { - dp->spring[count].segment = get_sd_seg_num(seg->name); - dp->spring[count].floor = get_sd_floor_num420(dp, ms->segment[segNum].springFloor->name); - mstrcpy(&dp->spring[count].floor_name, ms->segment[segNum].springFloor->name); - for (k = 0; k < 3; k++) - { - dp->spring[count].point[k] = seg->springPoint[j].point[k] - seg->masscenter[k]; - dp->spring[count].force[k] = 0.0; - } - dp->spring[count].friction = seg->springPoint[j].friction; - dp->spring[count].param_a = seg->springPoint[j].param_a; - dp->spring[count].param_b = seg->springPoint[j].param_b; - dp->spring[count].param_c = seg->springPoint[j].param_c; - dp->spring[count].param_d = seg->springPoint[j].param_d; - dp->spring[count].param_e = seg->springPoint[j].param_e; - dp->spring[count].param_f = seg->springPoint[j].param_f; - count++; - } - } - } - } - else - { - dp->spring = NULL; - } - - /* Copy the force mattes. */ - /* First count how many mattes there are, since they are not stored in one array. - * Each body segment has either 0 or 1 mattes. If the matte's polyhedron is missing - * (e.g., because the bone file could not be found), skip the matte. - */ - for (i = 0, count = 0; i < ms->numsegments; i++) - if (ms->segment[i].forceMatte && ms->segment[i].forceMatte->poly) - count++; - - dp->num_force_mattes = dp->force_matte_array_size = count; - if (dp->num_force_mattes > 0) - { - dp->force_matte = (dpForceMatte*)simm_malloc(dp->num_force_mattes * sizeof(dpForceMatte)); - for (i = 0, count = 0; i < ms->numsegments; i++) - { - ContactObject* matte = ms->segment[i].forceMatte; - if (matte && matte->poly) - { - dp->force_matte[count].segment = get_sd_seg_num(ms->segment[i].name); - mstrcpy(&dp->force_matte[count].name, matte->name); - copyPolyhedronToDPPolyhedron(matte->poly, &dp->force_matte[count].ph, i, dp->force_matte[count].segment, ms); - count++; - } - } - } - else - { - dp->force_matte = NULL; - } - - /* Copy the constraint objects. */ - copyConstraintsToDP420Constraints(ms, dp); - - /* Copy the functions, which are used for kinematic constraints and gencoord restraints. */ - dp->num_constraint_functions = countUsedFunctions(ms); - if (dp->num_constraint_functions > 0) - { - int index = 0; - dp->constraint_function = (dp420SplineFunction*)simm_calloc(dp->num_constraint_functions, sizeof(dp420SplineFunction)); - for (i = 0; i < ms->func_array_size; i++) - { - if (ms->function[i] && ms->function[i]->used == yes) - { - dpFunction* from = ms->function[i]; - dp420SplineFunction* to = &dp->constraint_function[index++]; - - to->type = from->type; - to->cutoff_frequency = 0.0; - to->usernum = from->usernum; - to->defined = dpYes; - - to->numpoints = to->coefficient_array_size = from->numpoints; - to->x = (double*)simm_calloc(to->numpoints, sizeof(double)); - to->y = (double*)simm_calloc(to->numpoints, sizeof(double)); - to->b = (double*)simm_calloc(to->numpoints, sizeof(double)); - to->c = (double*)simm_calloc(to->numpoints, sizeof(double)); - to->d = (double*)simm_calloc(to->numpoints, sizeof(double)); - - /* b, c, and d are not copied because they are recalculated - * by the simulation. - */ - memcpy(to->x, from->x, to->numpoints * sizeof(double)); - memcpy(to->y, from->y, to->numpoints * sizeof(double)); - } - } - } - else - { - dp->constraint_function = NULL; - } - - /* Copy the Q data. This has to be done after the constraint functions are copied - * because the Qs contain pointers to the functions. However, the number of Qs - * is calculated earlier, outside this function. - */ - copyQsToDP420(ms, dp); - - // SIMM 5.0 models do not contain dynamic_param_names at the model level (only in the muscles), - // but in SIMM 4.2 models they do. So copy them from the 5.0 default muscle to the 4.2 model. - // The same array of param names will be stored in the 4.2 model, the default muscle, and - // all the muscles. - dp->num_dynamic_params = ms->default_muscle->num_dynamic_params; - if (dp->num_dynamic_params > 0) - { - dp->dynamic_param_names = (char**)simm_malloc(dp->num_dynamic_params * sizeof(char*)); - for (i=0; inum_dynamic_params; i++) - mstrcpy(&dp->dynamic_param_names[i], ms->default_muscle->dynamic_param_names[i]); - } - else - { - dp->dynamic_param_names = NULL; - } - - /* Copy the muscles. */ - copyMusclesToDP420(ms, dp, muscleList); - - return dp; -} - - -static int get_sd_floor_num420(dp420ModelStruct* dp, char* name) -{ - int i; - - for (i = 0; i < dp->num_spring_floors; i++) - { - if (STRINGS_ARE_EQUAL(name, dp->spring_floor[i].name)) - return i; - } - - return -1; -} diff --git a/OpenSim/Utilities/simmToOpenSim/dp420.h b/OpenSim/Utilities/simmToOpenSim/dp420.h deleted file mode 100644 index 84924cc1d9..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/dp420.h +++ /dev/null @@ -1,225 +0,0 @@ -/******************************************************************************* - - DP420.H - - Author: Peter Loan - - Copyright (c) 2004-5 MusculoGraphics, Inc, a division of Motion Analysis Corp. - All rights reserved. - - This header file defines the data structures used to hold a musculoskeletal - model, as well as the interface between SIMM the DLLs made by the Dynamics - Pipeline. It is the version of the header file from SIMM 4.2 (and 4.2.1, since - there were no changes made in 4.2.1). It is included in later versions of SIMM - to support the loading of DLLs made by SIMM 4.2 and 4.2.1 into SIMM 5.0 and later. - This copy of the header has been modified as follows: - 1. data structures that did not change between 4.2 and 5.0 have been removed - (so that they do not need to be renamed) - 2. data structures that did change have been renamed - -*******************************************************************************/ - -#ifndef DP420_H -#define DP420_H - -#ifdef _WINDOWS - #define DLL __declspec(dllexport) -#else - #define DLL -#endif - -#include "dp.h" - - -typedef struct -{ - dpFunctionType type; /* step, linear, gcv, natural cubic */ - double cutoff_frequency; /* used only for gcv_spline */ - int numpoints; /* number of points in the function */ - int coefficient_array_size; /* current size of x,y,b,c,d arrays */ - int usernum; /* user-supplied number for this function */ - double* x; /* list of x coordinates */ - double* y; /* list of y coordinates */ - double* b; /* list of b coefficients for spline fit */ - double* c; /* list of c coefficients for spline fit */ - double* d; /* list of d coefficients for spline fit */ - dpBoolean defined; /* is function defined in joint file? */ -} dp420SplineFunction; /* description of a cubic-spline function*/ - - -typedef struct -{ - int genc; /* gencoord number */ - double start; /* starting value of range-of-motion */ - double end; /* ending value of range-of-motion */ -} dp420PointRange; /* muscle point range (wrapping pts) */ - - -typedef struct -{ - int segment; /* body segment this point is fixed to */ - int refpt; /* corresponding point in saved muscle */ - dpBoolean selected; /* whether or not point has been selected */ - double point[3]; /* xyz coordinates of the point */ - int numranges; /* number of ranges where point is active */ - dp420PointRange* ranges; /* array of ranges */ - dpBoolean is_auto_wrap_point; /* was this point calc-ed by auto wrapper? */ - double wrap_distance; /* stores distance of wrap over object surface */ - double* wrap_pts; /* points wrapping over surface */ - int num_wrap_pts; /* number of points wrapping over surface */ -} dp420MusclePoint; /* properties of a muscle point */ - - -typedef struct -{ - char* name; /* name of wrap object */ - dpWrapObjectType wrap_type; /* type of wrap object */ - dpBoolean active; /* is this wrap object active? */ - int wrap_algorithm; /* default wrap algorithm for wrap object */ - int segment; /* body segment object is fixed to */ - double radius[3]; /* wrap object radius */ - double height; /* wrap object height (cylinder only) */ - int wrap_axis; /* which axis to wrap over X=0, Y=1, Z=2 */ - int wrap_sign; /* which side of wrap axis to use (0 for both) */ - double from_local_xform[4][4]; /* parent-to-wrapobj transform matrix */ - double to_local_xform[4][4]; /* wrapobj-to-parent transform matrix */ -} dp420WrapObject; - - -typedef struct -{ - dp420WrapObject* wrap_object; /* muscle is auto-wrapped over this object */ - int wrap_algorithm; /* wrap algorithm for this muscle */ - int startPoint; /* wrap only those muscle segments between startPoint */ - int endPoint; /* and endPoint */ - double c[3]; /* previous c point */ - double r1[3]; /* previous r1 (tangent) point */ - double r2[3]; /* previous r2 (tangent) point */ - dp420MusclePoint mp_wrap[2]; /* the two muscle points created when the muscle wraps */ -} dp420MuscleWrapStruct; - - -typedef struct -{ - char* name; /* name of muscle */ - dpBoolean display; /* whether or not to display this muscle */ - dpBoolean output; /* write muscle values to output file? */ - dpBoolean selected; /* whether or not this muscle is selected */ - dpBoolean has_wrapping_points; /* does this muscle have wrapping pts? */ - dpBoolean has_force_points; /* does this muscle have force-dependent pts? */ - int* num_orig_points; /* number of muscle points */ - int num_points; /* number of muscle points */ - dp420MusclePoint* mp_orig; /* list of muscle points from muscle file */ - dp420MusclePoint** mp; /* list of muscle points after auto wrapping */ - int mp_array_size; /* current size of muscle point array */ - double* max_isometric_force; /* maximum isometric force */ - double* pennation_angle; /* pennation angle of muscle fibers */ - double* optimal_fiber_length; /* muscle fiber length */ - double* resting_tendon_length; /* resting length of tendon */ - double* max_contraction_vel; /* maximum contraction velocity */ - double muscle_tendon_length; /* musculotendon length */ - double fiber_length; /* muscle fiber length */ - double tendon_length; /* tendon length */ - double muscle_tendon_vel; /* musculotendon velocity */ - double fiber_velocity; /* muscle velocity */ - double tendon_velocity; /* tendon velocity */ - double force; /* force in musculotendon unit */ - double applied_power; /* power this muscle applies to body segments */ - double heat_energy; /* heat energy generated by this muscle */ - double mechanical_energy; /* energy this muscle gives to body segments */ - double energy; /* energy this muscle gives to body segments */ - int nummomentarms; /* number of moment arms (= # of gencoords) */ - double* momentarms; /* list of moment arm values */ - dp420SplineFunction* tendon_force_len_curve; /* tendon force-length curve */ - dp420SplineFunction* active_force_len_curve; /* muscle active force-length curve */ - dp420SplineFunction* passive_force_len_curve; /* muscle passive force-length curve */ - dp420SplineFunction* force_vel_curve;/* muscle force-velocity curve */ - int num_dynamic_params; /* size of dynamic_params array */ - char** dynamic_param_names; /* list of dynamic parameter names */ - double** dynamic_params; /* array of dynamic (muscle model) parameters */ - double dynamic_activation; /* dynamic value of muscle activation */ - dpFunctionType* excitation_format; /* format for interpolating excitation data. */ - int excitation_abscissa; /* excit. is func of this gencoord (-1 = time) */ - double excitation_level; /* current level of excitation */ - dp420SplineFunction* excitation; /* excitation (activation) sequence */ - int* muscle_model_index; /* index for deriv, init, & assign func arrays */ - dpBoolean wrap_calced; /* has wrapping been calculated/updated? */ - int numWrapStructs; /* number of wrap objects used for this muscle */ - dp420MuscleWrapStruct** wrapStruct; /* holds information about the wrap objects */ - int numStateParams; /* number of initial state values stored in dllparams.txt */ - double* stateParams; /* array of initial state values in dllparams.txt */ -} dp420MuscleStruct; /* properties of a musculotendon unit */ - - -typedef struct -{ - char* name; - dpQType type; - int joint; - int axis; - int q_ind; - int constraint_num; - double initial_value; - double initial_velocity; - double conversion; - double range_start; - double range_end; - dp420SplineFunction* restraint_func; - dp420SplineFunction* min_restraint_func; - dp420SplineFunction* max_restraint_func; - dp420SplineFunction* constraint_func; - dpBoolean function_active; - dpBoolean output; - double torque; - double pd_stiffness; -} dp420QStruct; - - -typedef struct -{ - char* name; - int nq; - int nu; - int neq; - int num_muscles; - int num_body_segments; - int num_gencoords; - int num_joints; - int num_closed_loops; - int num_constraints; - int num_user_constraints; - int num_dynamic_params; /* size of dynamic_param_names array */ - char** dynamic_param_names; /* names of dynamic muscle parameters */ - int num_contacts; - int num_bilat_contacts; - int contacts_size; - int bilat_contacts_size; - dpContactInfo* contacts; - dpContactInfo* bilat_contacts; - int num_springs; /* number of spring elements for spring-based contact */ - int num_spring_floors; /* number of floor elements for spring-based contact */ - int spring_array_size; - int spring_floor_array_size; - dpSpringStruct* spring; /* spring points for use with spring-based contact */ - dpSpringFloor* spring_floor; /* spring floors for use with spring-based contact */ - int num_force_mattes; /* number of force mattes for applying ground-based forces */ - int force_matte_array_size; - dpForceMatte* force_matte; /* force mattes used for applying ground-based forces */ - dp420QStruct* q; - dpBodyStruct* body_segment; - dp420MuscleStruct* muscles; - dp420MuscleStruct default_muscle; - dpJointStruct* joint; - int num_wrap_objects; - dp420WrapObject* wrap_object; - int num_constraint_objects; - dpConstraintObject* constraint_object; - int num_constraint_functions; - dp420SplineFunction* constraint_function; - double gravity[3]; - dpSimmModelID simmModel; - int enforce_constraints; /* enforce constraint objects while integrating? */ - int newInverseSimulation; /* new-style inverse with corrective torques? */ -} dp420ModelStruct; - -#endif /*DP420_H*/ diff --git a/OpenSim/Utilities/simmToOpenSim/dpfunctions.h b/OpenSim/Utilities/simmToOpenSim/dpfunctions.h deleted file mode 100644 index 55a5e6eeb4..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/dpfunctions.h +++ /dev/null @@ -1,55 +0,0 @@ -/******************************************************************************* - - DPFUNCTIONS.H - this header declares the DPTool Editor's public functions - - Author: Krystyne Blaikie (based on cefunctions.h by Kenny Smith) - - Date: 25-NOV-03 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef DPFUNCTIONS_H -#define DPFUNCTIONS_H - -/* defined in dptooltools.c */ -void makedptoolmenus(void); -void make_dp_motion_menu(void); - -void dp_entervalue(SimmEvent se); -void update_dp_forms(void); -void update_dp_radiopanel(void); -void update_dp_analogradiopanel(void); -void update_dp_globalcheckboxpanel(void); -void update_dp_sliderpanel(void); -void update_dp_win_status(void); -void do_dp_chpanel(WindowParams* win_parameters, int num); -void do_dp_radiopanel(WindowParams* win_parameters, int num); -void do_dp_analogradiopanel(WindowParams* win_parameters, int num); - -void dp_shorten_filename(char *in, char *out, int length); -void move_dptool_help_text(int dummy_int, double slider_value, double delta); - -void dp_choose_working_dir(void); -void dp_choose_dll(void); -void dp_choose_input_file(void); -void dp_choose_output_file(void); - -void dp_update_column_names(MotionSequence *motion); -void dp_init_motion_sequence(MotionSequence *motion); - -/* defined in dpsimulation.c */ -void sendSimMessage(SimStruct* ss, int mess, void* data); -SimMessageType getSimMessage(SimStruct* ss, void** data); -SBoolean loadSimulationDll(ModelStruct* model); -SBoolean setupSimulation(ModelStruct* model); -void runSimulation(ModelStruct* model); -void pauseSimulation(ModelStruct* model); -void resumeSimulation(ModelStruct* model); -void resetSimulation(ModelStruct* model); -DWORD WINAPI simThreadMain(LPVOID lpParam); - -#endif /* DPFUNCTIONS_H */ diff --git a/OpenSim/Utilities/simmToOpenSim/ellipsoid_wrap.c b/OpenSim/Utilities/simmToOpenSim/ellipsoid_wrap.c deleted file mode 100644 index 3e419e7eea..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/ellipsoid_wrap.c +++ /dev/null @@ -1,1684 +0,0 @@ -/******************************************************************************* - - ELLIPSOID_WRAP.C - - Author: Ken Smith, Craig Robinson, Frans van der Helm, Peter Loan - - Copyright (c) 1994-2002 MusculoGraphics, Inc. - All rights reserved. - - Description: - - The algorithm for wrapping muscles around an ellipsoid was originally - developed in Fortran by Frans van der Helm, translated to C by Craig, - modified by Pete, and modified again by Kenny. Pete and Kenny spent - quite a bit of time in early '99 trying to make the algorithm stable - (i.e. eliminate all muscle skips & jumps during wrapping). - - The routines contributed by Frans and still in use are: - - calc_line_intersect_ellipsoid - the main routine, wraps a muscle - around an ellipsoid - - calc_r - find a tangent point on the ellipsoid surface - - dell - find wrapping line segments along ellipsoid surface - - Another routine, afstll(), was conttributed by Frans but replaced by - pt_to_ellipsoid() which was written by Dave Eberly and located by - searching usenet for "ellipsoid ellipse plane". - - ALGORITHM OVERVIEW - - These are the basic steps taken in computing a wrap path around an - ellipsoid: - - 1. Find the intersections of the line segment connecting the two - muscle points (p1 & p2) with the ellipsoid. Call the intersection - points r1 & r2. We sometimes refer to the line segment r1->r2 that - passes through the ellipsoid as the "muscle line". - - 2. Find a third point, c1, to use with r1 and r2 to define a "wrapping - plane". Finding a good c1 such that it creates a desirable (i.e. - short) wrapping path turns out to be one of the most difficult steps. - This is described in more detail below. - - 3. Find two "tangent points" on the ellipsoid surface. These points - must lie in the wrapping plane found in step 2, and create line - segments that are tangent to the ellipsoid when connected to their - respective muscle end-points. The routine calc_r() is called to find - each tangent point. calc_r() takes a muscle end-point and a point - on the ellipsoid surface as input. It adjusts the surface point to - become a tangent point. Since there are two possible tangent points - in the wrapping plane, calc_r() uses the initial position of the - surface point to determine which direction around the ellipsoid - to search for a tangent point. Therefore it is important to pass - a good surface point into calc_r() (i.e. one that directs it to - search in the direction of the shortest wrapping path). - - 4. Connect the two tangent points with a series of line segments. The - routine dell() does this. - - CHOOSING A WRAPPING PLANE - - Choosing the plane that the wrapping path lies in ended up taking more - effort than we expected. Also, it is likely that there are better - solutions than the one we ended up with. This section describes our - current solution, as well as some that didn't work out so well. - - (1) The Frans Solution: The method used in Frans' contributed code was - to pick the coordinate axis that is most parallel to the muscle line, - then determine the point along the muscle line where that coordinate axis' - value is zero (i.e. if the muscle line is most parallel to the x-axis, - then determine the point where the muscle line intersects the yz-plane). - This point is called "sv". Pass sv to the pt_to_ellipsoid() routine - to find the closest point on the ellipsoid surface, c1. - - This solution works well in most cases because it takes advantage of the - special-case handling in pt_to_ellipsoid() that it triggered because - sv is always on one of the coordinate planes. In this situation - pt_to_ellipsoid() discards one dimension and solves the 2d pt-to-ellipse - problem which always returns a c1 on one of the ellipsoid's axis- - aligned "equator" ellipses. When the muscle line is mostly parallel - to a major axis, this choice of c1 works well. - - The main problem with the Frans solution occurs when the muscle line - approaches a 45-degree angle with its most-parallel axis, and then - switches axes that it is most parallel to. This can cause large jumps - in muscle wrapping path. - - (2) The Fan Solution: We looked closely at the behavior of pt_to_ellipsoid(). - If you call pt_to_ellipsoid() repeatedly, passing input points all along - the length of the muscle line, you get an interesting "fan" of vectors - from the muscle line to the ellipsoid surface. Depending on the - orientation of the muscle line, this fan can be smoothly continuous (much - like an oriental fan) or sharply discontinuous (i.e. half the fan blades - pointing forward along the muscle line, the other half pointing 180-degrees - in the opposite direction). - - We tried using the fan in various ways to deliver a good c1 point. - Some of the things that we tried were: - - a. binary searching the fan for the most perpendicular vector, - b. choosing the longest vector, - c. averaging all the vectors together. - - All of these solutions have problems when the discontinuity of the - fan becomes very sharp, however we noticed that the Frans solution - performs well for muscle line orintations that generate sharp - discontinuities in the fan. Therefore, our saving grace was that the - fan and Frans solutions appear to compliment each other. Each one - works well where the other one fails. - - (3) Our Solution -- Frans + Fan Hybrid: What we ended up with is - a solution that first computes the Frans result, then evauates its - "strength". A Frans result is considered strong if the muscle line is - very parallel to one of the major axes. As the muscle line moves toward - 45-degrees from its most parallel axis, the strength of the Frans result - is gradually reduced. Any major axis with an angle of 45 degrees or more - from the muscle line has a Frans result strength of zero. - - When the strength of the Frans result dips below a certain threshold, we - begin using fan solution 'c' (averaging all fan vectors) to influence the - result. We gradually fade from Frans to Fan solutions. At 45 degrees the - Fan solution momentarily takes over completely just as the Frans solution - is switching its most-parallel major axes. - - Another issue that we had to address was the situation where a Frans - solution generated an sv point that was outside of the ellipsoid. This - created several problems. Therefore we additionally reduce the strength - of a Frans result as its sv point approaches the ends of the r1->r2 - muscle line. - - (4) The Midpoint Solution: For a while we used a solution that simply picked - the midpoint of the muscle line as the sv point. This worked well enough - in most cases, but suffered when the fan became sharply discontinuous. - In this case the resulting c1 point could end up being nearly coincident - with either r1 or r2, making it impossible to create a wrapping plane - from the three points. Although RIC used several versions of SIMM with - the midpoint solutions, we ultimately replaced it with the hybrid solution - described above. - - CONSTRAINING WRAPPING TO ONE HALF OF THE ELLIPSOID - - FINDING THE CLOSEST POINT ON AN ELLIPSOID -- SPECIAL CASES - -*******************************************************************************/ - -#ifdef WRAP_LIB -#include -#else -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "wefunctions.h" -#endif - - -/*************** DEFINES (for this file only) *********************************/ -#define E_INSIDE_RADIUS -1 -#define E_NO_WRAP 0 -#define E_WRAPPED 1 -#define E_MANDATORY_WRAP 2 - -#define ELLIPSOID_TOLERANCE_1 1e-4 /* tolerance for pt_to_ellipsoid() special case detection */ -#define ELLIPSOID_TINY 0.00000001 -#define MU_BLEND_MIN 0.7073 /* 100% fan (must be greater than cos(45)!) */ -#define MU_BLEND_MAX 0.9 /* 100% Frans */ -#define NUM_FAN_SAMPLES 300 /* IMPORTANT: larger numbers produce less jitter */ -#define NUM_DISPLAY_SAMPLES 30 -#define N_STEPS 16 -#define SV_BOUNDARY_BLEND 0.3 - -#define PREVENT_PT_TO_ELLIPSOID_SPECIAL_CASE_HANDLING 0 - -#define _UNFACTOR \ - for (i = 0; i < 3; i++) \ - { \ - p1[i] /= factor; \ - p2[i] /= factor; \ - m[i] /= factor; \ - a[i] /= factor; \ - r1[i] /= factor; \ - r2[i] /= factor; \ - } - -#define PT_TO_ELLIPSOID(_PT, _M, _A, _P1A) \ - pt_to_ellipsoid(_A[0],_A[1],_A[2], \ - _PT[0],_PT[1],_PT[2], \ - &_P1A[0],&_P1A[1],&_P1A[2], -1) - -#define PT_TO_ELLIPSOID_2(_PT, _M, _A, _P1A, _SPECIAL_CASE_AXIS) \ - pt_to_ellipsoid(_A[0],_A[1],_A[2], \ - _PT[0],_PT[1],_PT[2], \ - &_P1A[0],&_P1A[1],&_P1A[2], _SPECIAL_CASE_AXIS) - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static const float red[] = { 1, 0, 0 }, green[] = { 0, 1, 0 }, blue[] = { 0, 0, 1 }; -static const float cyan[] = { 0, 1, 1 }, magenta[] = { 1, 0, 1 }, yellow[] = { 1, 1, 0 }; -static const float white[] = { 1, 1, 1 }, pink[] = { 1, 0.5, 0.5 }; -static const float* fan_color = white; -static double factor, sv[3], c1[3]; - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void ellipsoid_normal_at_pt(const double m[], const double a[], - const double pt[], double n[], double nLength); - -static double pt_to_ellipse(double a, double b, double u, double v, double* x, double* y); - -static double pt_to_ellipsoid(double a, double b, double c, - double u, double v, double w, - double* x, double* y, double* z, int specialCaseAxis); - -static int calc_r(double p1e, double r1[], double p1[], double m[], double a[], - double vs[], double vs4); - -static void dell(double r1[], double r2[], double m[], double a[], double vs[], - double vs4, SBoolean far_side_wrap, double *afst, double** wrap_pts, int* num_wrap_pts); - -/**** for constraints ****/ -static double pt_to_ellipse2(double a, double b, double u, double v, double* x, double* y); - -static double pt_to_ellipsoid2(double a, double b, double c, - double u, double v, double w, - double* x, double* y, double* z, int specialCaseAxis); -//void cons_ellipsoid_normal_at_pt(const double m[], const double a[], -// const double pt[], double n[], double nLength); - - -/* use pt_to_ellipsoid2: which has a much smaller tolerance when checking - * if values are equal to zero. For some reason this helps, especially with - * points near axes. - for constraint calculations */ -double calc_distance_to_ellipsoid(double p1[], double radii[], double projpt[]) -{ - double distance; - distance = pt_to_ellipsoid2(radii[0], radii[1], radii[2], p1[0], p1[1], p1[2], - &projpt[0], &projpt[1], &projpt[2], -1); - return distance; -} - -void cons_ellipsoid_normal_at_pt(const double m[], const double a[], - const double pt[], double n[], double desired_length) -{ -// ellipsoid_normal_at_pt(m, a, pt, n, nLength); -/* ------------------------------------------------------------------------- - ellipsoid_normal_at_pt - return the normal vector to the ellipsoid at the - specified point. - m - ellipsoid origin - a - ellipsoid radii ----------------------------------------------------------------------------- */ - double x = pt[0] - m[0]; - double y = pt[1] - m[1]; - double z = pt[2] - m[2], length, ratio; - - n[0] = 2 * x / SQR(a[0]); - n[1] = 2 * y / SQR(a[1]); - n[2] = 2 * z / SQR(a[2]); - - length = VECTOR_MAGNITUDE(n); - - ratio = desired_length / length; - - n[0] *= ratio; - n[1] *= ratio; - n[2] *= ratio; -} - -/**** ****/ - -/* ------------------------------------------------------------------------- - calc_line_intersect_ellipsoid - calculate an optimal wrapping path about - the specified ellipsoid. ----------------------------------------------------------------------------- */ -int calc_line_intersect_ellipsoid ( - double p1[3], /* input: muscle point 1 */ - double p2[3], /* input: muscle point 2 */ - double m[3], /* input: ellipsoid origin (0,0,0) */ - double a[3], /* input: ellipsoid xyz radii */ - double* rlen, /* output: length of surface wrap */ - double r1[], /* output: wrap tangent point 1 */ - double r2[], /* output: wrap tangent point 2 */ - double** wrap_pts, /* output: intermediate surface wrap pts*/ - int* num_wrap_pts, /* output: number of intermediate pts */ - int wrap_axis, /* input: constraint axis */ - int wrap_sign, /* input: constraint direction */ - dpMuscleWrapStruct* mwrap, /* in/out: muscle wrap structure */ - int* p_flag, /* output: did wrapping occur? */ - dpWrapObject* wo) -{ - int i,j, bestMu; - - double p1p2[3], p1m[3], p2m[3], ppm, f1[3], f2[3], aa, bb, cc, disc, l1, l2, - vs[3], p1e, p2e, p1c1[3], vs4, dist, t[3], fanWeight = DBL_MIN; - - double r1r2[3], t_sv[3][3], t_c1[3][3]; - double mu[3]; - - SBoolean far_side_wrap = no; - - *p_flag = TRUE; - *num_wrap_pts = 0; - *wrap_pts = NULL; - -#if VISUAL_WRAPPING_DEBUG - enable_debug_shapes(yes); - - wo->num_debug_glyphs = 0; -#endif - - /* This algorithm works best if the coordinates (p1, p2, m, a) are all somewhat - * close to 1.0. So use the ellipsoid dimensions to calculate a multiplication - * factor that will be applied to all of the coordinates. You want to use just - * the ellipsoid dimensions because they do not change from one call to the - * next. You don't want the factor to change because the algorithm uses - * some vectors (r1, r2, c1) from the previous call. - */ - factor = 3.0 / (a[0] + a[1] + a[2]); - - for (i = 0; i < 3; i++) - { - p1[i] *= factor; - p2[i] *= factor; - m[i] *= factor; - a[i] *= factor; - } - - p1e = -1.0; - p2e = -1.0; - - for (i = 0; i < 3;i++) - { - p1e += SQR((p1[i] - m[i]) / a[i]); - p2e += SQR((p2[i] - m[i]) / a[i]); - } - - /* check if p1 and p2 are inside the ellipsoid */ - if (p1e < -0.0001 || p2e < -0.0001) - { - /* p1 or p2 is inside the ellipsoid */ - *p_flag = FALSE; - *rlen = 0.0; - - _UNFACTOR; /* transform back to starting coordinate system */ - - return E_INSIDE_RADIUS; - } - - MAKE_3DVECTOR21(p1, p2, p1p2); - MAKE_3DVECTOR21(p1, m, p1m); - normalize_vector(p1m, p1m); - MAKE_3DVECTOR21(p2, m, p2m); - normalize_vector(p2m, p2m); - - ppm = DOT_VECTORS(p1m, p2m) - 1.0; /* angle between p1->m and p2->m: -2.0 to 0.0 */ - - if (fabs(ppm) < 0.0001) - { - /* vector p1m and p2m are colinear */ - *p_flag = FALSE; - *rlen = 0.0; - - _UNFACTOR; /* transform back to starting coordinate system */ - - return E_NO_WRAP; - } - - /* check if the line through p1 and p2 intersects the ellipsoid */ - for (i = 0; i < 3;i++) - { - f1[i] = p1p2[i] / a[i]; - f2[i] = (p2[i] - m[i]) / a[i]; - } - aa = DOT_VECTORS(f1, f1); - bb = 2.0 * DOT_VECTORS(f1, f2); - cc = DOT_VECTORS(f2, f2) - 1.0; - disc = SQR(bb) - 4.0 * aa * cc; - - if (disc < 0.0) - { - /* no intersection */ - *p_flag = FALSE; - *rlen = 0.0; - - _UNFACTOR; /* transform back to starting coordinate system */ - - return E_NO_WRAP; - } - - l1 = (-bb + sqrt(disc)) / (2.0 * aa); - l2 = (-bb - sqrt(disc)) / (2.0 * aa); - - if ( ! (0.0 < l1 && l1 < 1.0) || ! (0.0 < l2 && l2 < 1.0) ) - { - /* no intersection */ - *p_flag = FALSE; - *rlen = 0.0; - - _UNFACTOR; /* transform back to starting coordinate system */ - - return E_NO_WRAP; - } - - /* r1 & r2: intersection points of p1->p2 with the ellipsoid */ - for (i = 0; i < 3; i++) - { - r1[i] = p2[i] + l1 * p1p2[i]; - r2[i] = p2[i] + l2 * p1p2[i]; - } - -#if VISUAL_WRAPPING_DEBUG - add_debug_line(wo, factor, r1, r2, 2.0, "", "", yellow); /* the "muscle line" */ -#endif - - /* ==== COMPUTE WRAPPING PLANE (begin) ==== */ - - MAKE_3DVECTOR21(r2, r1, r1r2); - - /* (1) Frans technique: choose the most parallel coordinate axis, then set - * 'sv' to the point along the muscle line that crosses the plane where - * that major axis equals zero. This takes advantage of the special-case - * handling in pt_to_ellipsoid() that reduces the 3d point-to-ellipsoid - * problem to a 2d point-to-ellipse problem. The 2d case returns a nice - * c1 in situations where the "fan" has a sharp discontinuity. - */ - normalize_vector(p1p2, mu); - - for (i = 0; i < 3; i++) - { - mu[i] = fabs(mu[i]); - - t[i] = (m[i] - r1[i]) / r1r2[i]; - - for (j = 0; j < 3; j++) - t_sv[i][j] = r1[j] + t[i] * r1r2[j]; - - PT_TO_ELLIPSOID_2(t_sv[i], m, a, t_c1[i], i); - } - - /* pick most parallel major axis */ - for (bestMu = 0, i = 1; i < 3; i++) - if (mu[i] > mu[bestMu]) - bestMu = i; - - if (mwrap->wrap_algorithm == WE_HYBRID_ALGORITHM || - mwrap->wrap_algorithm == WE_AXIAL_ALGORITHM || - mwrap->wrap_algorithm == WE_FAN_ALGORITHM) - { - if (mwrap->wrap_algorithm == WE_HYBRID_ALGORITHM && mu[bestMu] > MU_BLEND_MIN) - { - /* If Frans' technique produces an sv that is not within the r1->r2 - * line segment, then that means that sv will be outside the ellipsoid. - * This can create an sv->c1 vector that points roughly 180-degrees - * opposite to the fan solution's sv->c1 vector. This creates problems - * when interpolating between the Frans and fan solutions because the - * interpolated c1 can become colinear to the muscle line during - * interpolation. Therefore we detect Frans-solution sv points near - * the ends of r1->r2 here, and fade out the Frans result for them. - */ - - double s = 1.0; - - if (t[bestMu] < 0.0 || t[bestMu] > 1.0) - s = 0.0; - - else if (t[bestMu] < SV_BOUNDARY_BLEND) - s = t[bestMu] / SV_BOUNDARY_BLEND; - - else if (t[bestMu] > (1.0 - SV_BOUNDARY_BLEND)) - s = (1.0 - t[bestMu]) / SV_BOUNDARY_BLEND; - - if (s < 1.0) - mu[bestMu] = MU_BLEND_MIN + s * (mu[bestMu] - MU_BLEND_MIN); -#if 0 - fprintf(stderr, mu[bestMu] < MU_BLEND_MAX ? "mu{%.3f} " : "mu[%.3f] ", mu[bestMu]); - fprintf(stderr, s < 1.0 ? "t{%.2f}\n" : "t %.2f\n", s); -#endif - } - - if (mwrap->wrap_algorithm == WE_AXIAL_ALGORITHM || mu[bestMu] > MU_BLEND_MIN) - { - /* if the Frans solution produced a strong result, copy it into - * sv and c1. - */ - for (i = 0; i < 3; i++) - { - c1[i] = t_c1[bestMu][i]; - sv[i] = t_sv[bestMu][i]; - } -#if VISUAL_WRAPPING_DEBUG && 0 - if (mu[bestMu] < MU_BLEND_MAX && mwrap->wrap_algorithm == WE_HYBRID_ALGORITHM) - add_debug_line(wo, factor, t_sv[bestMu], t_c1[bestMu], 1.0, "", "Frans", blue); -#endif - } -#if 0 - fprintf(stderr, bestMu == 0 ? "[%.3f] " : "%.3f ", mu[0]); - fprintf(stderr, bestMu == 1 ? "[%.3f] " : "%.3f ", mu[1]); - fprintf(stderr, bestMu == 2 ? "[%.3f]\n" : "%.3f\n", mu[2]); -#endif - - if (mwrap->wrap_algorithm == WE_FAN_ALGORITHM || - mwrap->wrap_algorithm == WE_HYBRID_ALGORITHM && mu[bestMu] < MU_BLEND_MAX) - { - /* (2) Fan technique: sample the fan at fixed intervals and average the - * fan "blade" vectors together to determine c1. This only works when - * the fan is smoothly continuous. The sharper the discontinuity, the - * more jumpy c1 becomes. - */ - double v_sum[3] = {0,0,0}; - - for (i = 0; i < 3; i++) - t_sv[2][i] = r1[i] + 0.5 * r1r2[i]; - - for (i = 1; i < NUM_FAN_SAMPLES - 1; i++) - { - double v[3], tt = (double) i / NUM_FAN_SAMPLES; - - for (j = 0; j < 3; j++) - t_sv[0][j] = r1[j] + tt * r1r2[j]; - - PT_TO_ELLIPSOID(t_sv[0], m, a, t_c1[0]); - - MAKE_3DVECTOR21(t_c1[0], t_sv[0], v); - - normalize_vector(v, v); - - /* add sv->c1 "fan blade" vector to the running total */ - for (j = 0; j < 3; j++) - v_sum[j] += v[j]; - -#if VISUAL_WRAPPING_DEBUG && 0 - if (i % (NUM_FAN_SAMPLES / NUM_DISPLAY_SAMPLES) == 0) - add_debug_line(wo, factor, t_sv[0], t_c1[0], 1.0, NULL, NULL, fan_color); -#endif - } - /* use vector sum to determine c1 */ - normalize_vector(v_sum, v_sum); - - for (i = 0; i < 3; i++) - t_c1[0][i] = t_sv[2][i] + v_sum[i]; - - if (mwrap->wrap_algorithm == WE_FAN_ALGORITHM || mu[bestMu] <= MU_BLEND_MIN) - { - PT_TO_ELLIPSOID(t_c1[0], m, a, c1); - - for (i = 0; i < 3; i++) - sv[i] = t_sv[2][i]; - - fanWeight = 1.0; - } - else - { - double tt = (mu[bestMu] - MU_BLEND_MIN) / (MU_BLEND_MAX - MU_BLEND_MIN); - - double oneMinusT = 1.0 - tt; - - PT_TO_ELLIPSOID(t_c1[0], m, a, t_c1[1]); - - for (i = 0; i < 3; i++) - { - t_c1[2][i] = tt * c1[i] + oneMinusT * t_c1[1][i]; - - sv[i] = tt * sv[i] + oneMinusT * t_sv[2][i]; - } - PT_TO_ELLIPSOID(t_c1[2], m, a, c1); - -#if VISUAL_WRAPPING_DEBUG && 0 - add_debug_line(wo, factor, t_sv[2], t_c1[1], 1.0, "", "fan", blue); -#endif - fanWeight = oneMinusT; - } - } - } - else /* mwrap->wrap_algorithm == WE_MIDPOINT_ALGORITHM */ - { - for (i = 0; i < 3; i++) - sv[i] = r1[i] + 0.5 * (r2[i] - r1[i]); - - PT_TO_ELLIPSOID(sv, m, a, c1); - } - -#if VISUAL_WRAPPING_DEBUG - add_debug_line(wo, factor, sv, c1, 2.0, "", "c1", green); -#endif - /* ==== COMPUTE WRAPPING PLANE (end) ==== */ - - /* The old way of initializing r1 used the intersection point - * of p1p2 and the ellipsoid. This caused the muscle path to - * "jump" to the other side of the ellipsoid as sv[] came near - * a plane of the ellipsoid. It jumped to the other side while - * c1[] was still on the first side. The new way of initializing - * r1 sets it to c1 so that it will stay on c1's side of the - * ellipsoid. - */ - { - SBoolean use_c1_to_find_tangent_pts = yes; - - if (mwrap->wrap_algorithm == WE_AXIAL_ALGORITHM) - use_c1_to_find_tangent_pts = (SBoolean) (t[bestMu] > 0.0 && t[bestMu] < 1.0); - - if (use_c1_to_find_tangent_pts) - for (i = 0; i < 3; i++) - r1[i] = r2[i] = c1[i]; - } - - /* if wrapping is constrained to one half of the ellipsoid, - * check to see if we need to flip c1 to the active side of - * the ellipsoid. - */ - if (wrap_sign != 0) - { - dist = c1[wrap_axis] - m[wrap_axis]; - - if (DSIGN(dist) != wrap_sign) - { - double orig_c1[3]; - - for (i = 0; i < 3; i++) - orig_c1[i] = c1[i]; - - c1[wrap_axis] = - c1[wrap_axis]; - - for (i = 0; i < 3; i++) - r1[i] = r2[i] = c1[i]; - -#if VISUAL_WRAPPING_DEBUG - add_debug_line(wo, factor, sv, c1, 2.0, "", "c1\'", green); -#endif - - if (fanWeight == DBL_MIN) - fanWeight = 1.0 - (mu[bestMu] - MU_BLEND_MIN) / (MU_BLEND_MAX - MU_BLEND_MIN); - - if (fanWeight > 1.0) - fanWeight = 1.0; - - if (fanWeight > 0.0) - { - double tc1[3], bisection = (orig_c1[wrap_axis] + c1[wrap_axis]) / 2.0; - - c1[wrap_axis] = c1[wrap_axis] + fanWeight * (bisection - c1[wrap_axis]); - - for (i = 0; i < 3; i++) - tc1[i] = c1[i]; - - PT_TO_ELLIPSOID(tc1, m, a, c1); - -#if VISUAL_WRAPPING_DEBUG - add_debug_line(wo, factor, sv, c1, 2.0, "", "c1\'\'", green); -#endif - } - } - } - - /* use p1, p2, and c1 to create parameters for the wrapping - * plane. - */ - MAKE_3DVECTOR21(p1, c1, p1c1); - cross_vectors(p1p2, p1c1, vs); - normalize_vector(vs, vs); - - vs4 = - DOT_VECTORS(vs, c1); - - /* find r1 & r2 by starting at c1 moving toward p1 & p2 */ -#if VISUAL_WRAPPING_DEBUG && 1 - add_debug_line(wo, factor, sv, r1, 3.0, "", "r1\'", white); - add_debug_line(wo, factor, sv, r2, 3.0, "", "r2\'", white); -#endif - - calc_r(p1e, r1, p1, m, a, vs, vs4); - calc_r(p2e, r2, p2, m, a, vs, vs4); - -#if VISUAL_WRAPPING_DEBUG && 1 - add_debug_line(wo, factor, sv, r1, 3.0, "", "", pink); - add_debug_line(wo, factor, sv, r2, 3.0, "", "", pink); -#endif - - /* create a series of line segments connecting r1 & r2 along the - * surface of the ellipsoid. - */ - calc_wrap_path: - dell(r1, r2, m, a, vs, vs4, far_side_wrap, rlen, wrap_pts, num_wrap_pts); - - if (wrap_sign != 0 && *num_wrap_pts > 2 && ! far_side_wrap) - { - double r1p1[3], r2p2[3], r1w1[3], r2w2[3]; - - double *w1 = &(*wrap_pts)[3]; - double *w2 = &(*wrap_pts)[(*num_wrap_pts - 2) * 3]; - - /* check for wrong-way wrap by testing angle of first and last - * wrap path segments: - */ - MAKE_3DVECTOR(r1, p1, r1p1); - MAKE_3DVECTOR(r1, w1, r1w1); - MAKE_3DVECTOR(r2, p2, r2p2); - MAKE_3DVECTOR(r2, w2, r2w2); - - normalize_vector(r1p1, r1p1); - normalize_vector(r1w1, r1w1); - normalize_vector(r2p2, r2p2); - normalize_vector(r2w2, r2w2); - - if (DOT_VECTORS(r1p1, r1w1) > 0.0 || DOT_VECTORS(r2p2, r2w2) > 0.0) - { - /* NOTE: I added the ability to call dell() a 2nd time in this - * situation to force a far-side wrap instead of aborting the - * wrap. -- KMS 9/3/99 - */ - far_side_wrap = yes; - - goto calc_wrap_path; - } - } - - /* store the center point and tangent points for next time */ - for (i = 0; i < 3; i++) - { - mwrap->c[i] = c1[i]; - mwrap->r1[i] = r1[i]; - mwrap->r2[i] = r2[i]; - } - - /* unfactor the output coordinates */ - *rlen /= factor; - - for (i = 0; i < *num_wrap_pts * 3; i++) - (*wrap_pts)[i] /= factor; - - _UNFACTOR; /* transform back to starting coordinate system */ - - return E_MANDATORY_WRAP; - -} /* calc_line_intersect_ellipsoid */ - -/* ------------------------------------------------------------------------- - ellipsoid_normal_at_pt - return the normal vector to the ellipsoid at the - specified point. ----------------------------------------------------------------------------- */ -static void ellipsoid_normal_at_pt (const double m[], const double a[], - const double pt[], double n[], double desired_length) -{ - double x = pt[0] - m[0]; - double y = pt[1] - m[1]; - double z = pt[2] - m[2], length, ratio; - - n[0] = 2 * x / SQR(a[0]); - n[1] = 2 * y / SQR(a[1]); - n[2] = 2 * z / SQR(a[2]); - - length = VECTOR_MAGNITUDE(n); - - ratio = desired_length / length; - - n[0] *= ratio; - n[1] *= ratio; - n[2] *= ratio; -} - -/* ------------------------------------------------------------------------- - pt_to_ellipse - this routine is courtesy of Dave Eberly - www.magic-software.com - - Input: Ellipse (x/a)^2+(y/b)^2 = 1, point (u,v). - - Output: Closest point (x,y) on ellipse to (u,v), function returns - the distance sqrt((x-u)^2+(y-v)^2). ----------------------------------------------------------------------------- */ -static double pt_to_ellipse (double a, double b, double u, double v, double* x, double* y) -{ - /* Graphics Gems IV algorithm for computing distance from point to - * ellipse (x/a)^2 + (y/b)^2 = 1. The algorithm as stated is not stable - * for points near the coordinate axes. The first part of this code - * handles those points separately. - */ - double a2 = a*a, b2 = b*b; - double u2 = u*u, v2 = v*v; - double a2u2 = a2*u2, b2v2 = b2*v2; - double dx, dy, xda, ydb; - int i, which; - double t, P, Q, P2, Q2, f, fp; - - SBoolean nearXOrigin = (SBoolean) EQUAL_WITHIN_ERROR(0.0,u); - SBoolean nearYOrigin = (SBoolean) EQUAL_WITHIN_ERROR(0.0,v); - - fan_color = pink; - - /* handle points near the coordinate axes */ - if (nearXOrigin && nearYOrigin) - { - fan_color = yellow; - - if (a < b) - { - *x = (u < 0.0 ? -a : a); - *y = v; - return a; - } else { - *x = u; - *y = (v < 0.0 ? -b : b); - return b; - } - } - - if (nearXOrigin) /* u == 0 */ - { - fan_color = red; - - if ( a >= b || fabs(v) >= b - a2/b ) - { - *x = u; - *y = ( v >= 0 ? b : -b ); - dy = *y - v; - return fabs(dy); - } - else - { - *y = b2 * v / (b2-a2); - dy = *y - v; - ydb = *y / b; - *x = a * sqrt(fabs(1 - ydb*ydb)); - return sqrt(*x * *x + dy*dy); - } - } - - if (nearYOrigin) /* v == 0 */ - { - fan_color = red; - - if ( b >= a || fabs(u) >= a - b2/a ) - { - *x = ( u >= 0 ? a : -a ); - dx = *x - u; - *y = v; - return fabs(*x - u); - } - else - { - *x = a2 * u / (a2-b2); - dx = *x - u; - xda = *x / a; - *y = b * sqrt(fabs(1 - xda*xda)); - return sqrt(dx*dx + *y * *y); - } - } - - /* initial guess */ - if ( (u/a)*(u/a) + (v/b)*(v/b) < 1.0 ) - { - which = 0; - t = 0.0; - } - else - { - double max = a; - - which = 1; - - if ( b > max ) - max = b; - - t = max * sqrt(u*u + v*v); - } - - for (i = 0; i < 64; i++) - { - P = t+a2; - P2 = P*P; - Q = t+b2; - Q2 = Q*Q; - f = P2*Q2 - a2u2*Q2 - b2v2*P2; - - if ( fabs(f) < 1e-09 ) - break; - - fp = 2.0 * (P*Q*(P+Q) - a2u2*Q - b2v2*P); - t -= f / fp; - } -#if 0 - if (i >= 64) - fprintf(stderr, "2D: i: %d, f: %.0e\n", i, fabs(f)); -#endif - - *x = a2 * u / P; - *y = b2 * v / Q; - dx = *x - u; - dy = *y - v; - - return sqrt(dx*dx + dy*dy); - -} /* pt_to_ellipse */ - -/* ------------------------------------------------------------------------- - pt_to_ellipsoid - this routine is courtesy of Dave Eberly - www.magic-software.com - - Input: Ellipsoid (x/a)^2+(y/b)^2+(z/c)^2 = 1, point (u,v,w). - - Output: Closest point (x,y,z) on ellipsoid to (u,v,w), function returns - the distance sqrt((x-u)^2+(y-v)^2+(z-w)^2). ----------------------------------------------------------------------------- */ -static double pt_to_ellipsoid (double a, double b, double c, - double u, double v, double w, - double* x, double* y, double* z, - int specialCaseAxis) -{ - /* Graphics Gems IV algorithm for computing distance from point to - * ellipsoid (x/a)^2 + (y/b)^2 +(z/c)^2 = 1. The algorithm as stated - * is not stable for points near the coordinate planes. The first part - * of this code handles those points separately. - */ - int i,j; - -#if PREVENT_PT_TO_ELLIPSOID_SPECIAL_CASE_HANDLING - /* handle points near the coordinate planes by preventing them from - * getting too close to the coordinate planes. - */ - if (EQUAL_WITHIN_ERROR(u,0.0)) - u = (u < 0.0 ? - TINY_NUMBER : TINY_NUMBER); - - if (EQUAL_WITHIN_ERROR(v,0.0)) - v = (v < 0.0 ? - TINY_NUMBER : TINY_NUMBER); - - if (EQUAL_WITHIN_ERROR(w,0.0)) - w = (w < 0.0 ? - TINY_NUMBER : TINY_NUMBER); - -#else - /* handle points near the coordinate planes by reducing the problem - * to a 2-dimensional pt-to-ellipse. - * - * if uvw is close to more than one coordinate plane, pick the 2d - * elliptical cross-section with the narrowest radius. - */ - if (specialCaseAxis < 0) - { - double abc[3], uvw[3], minEllipseRadiiSum = FLT_MAX; - - abc[0] = a; abc[1] = b; abc[2] = c; - uvw[0] = u; uvw[1] = v; uvw[2] = w; - - for (i = 0; i < 3; i++) - { - if (EQUAL_WITHIN_ERROR(0.0,uvw[i])) - { - double ellipseRadiiSum = 0; - - for (j = 0; j < 3; j++) - if (j != i) - ellipseRadiiSum += abc[j]; - - if (minEllipseRadiiSum > ellipseRadiiSum) - { - specialCaseAxis = i; - minEllipseRadiiSum = ellipseRadiiSum; - } - } - } - } - if (specialCaseAxis == 0) /* use elliptical cross-section in yz plane */ - { - *x = u; - //printf(" yz plane "); - return pt_to_ellipse(b, c, v, w, y, z); - } - if (specialCaseAxis == 1) /* use elliptical cross-section in xz plane */ - { - *y = v; - //printf(" xz plane "); - return pt_to_ellipse(c, a, w, u, z, x); - } - if (specialCaseAxis == 2) /* use elliptical cross-section in xy plane */ - { - *z = w; - //printf(" xy plane "); - return pt_to_ellipse(a, b, u, v, x, y); - } -#endif - - fan_color = white; - - /* if we get to here, then the point is not close to any of the major planes, - * so we can solve the general case. - */ - { - double a2 = a*a, b2 = b*b, c2 = c*c; - double u2 = u*u, v2 = v*v, w2 = w*w; - double a2u2 = a2*u2, b2v2 = b2*v2, c2w2 = c2*w2; - double dx, dy, dz, t, f; - - /* initial guess */ - if ( (u/a)*(u/a) + (v/b)*(v/b) + (w/c)*(w/c) < 1.0 ) - { - t = 0.0; - } - else - { - double max = a; - - if ( b > max ) - max = b; - if ( c > max ) - max = c; - - t = max*sqrt(u*u+v*v+w*w); - } - - for (i = 0; i < 64; i++) - { - double P = t+a2, P2 = P*P; - double Q = t+b2, Q2 = Q*Q; - double R = t+c2, _R2 = R*R; - double PQ, PR, QR, PQR, fp; - - f = P2*Q2*_R2 - a2u2*Q2*_R2 - b2v2*P2*_R2 - c2w2*P2*Q2; - - if ( fabs(f) < 1e-09 ) - { - *x = a2 * u / P; - *y = b2 * v / Q; - *z = c2 * w / R; - - dx = *x - u; - dy = *y - v; - dz = *z - w; - - return sqrt(dx*dx+dy*dy+dz*dz); - } - - PQ = P*Q; - PR = P*R; - QR = Q*R; - PQR = P*Q*R; - fp = 2.0*(PQR*(QR+PR+PQ)-a2u2*QR*(Q+R)-b2v2*PR*(P+R)-c2w2*PQ*(P+Q)); - - t -= f/fp; - } -#if 0 - if (i >= 64) - fprintf(stderr, "3D: i: %d, f: %.0e\n", i, fabs(f)); -#endif - } - return -1.0; - -} /* pt_to_ellipsoid */ - -/* ------------------------------------------------------------------------- - calc_r - this routine adjusts a point (r1) such that the point remains in - a specified plane (vs, vs4), and creates a TANGENT line segment connecting - it to a specified point (p1) outside of the ellipsoid. - - Input: - p1e ellipsoid parameter for 'p1'? - r1 point to be adjusted to satisfy tangency-within-a-plane constraint - p1 point outside of ellipsoid - m ellipsoid origin - a ellipsoid axis - vs plane vector - vs4 plane coefficient - - Output: - r1 tangent point on ellipsoid surface and in vs plane ----------------------------------------------------------------------------- */ -static int calc_r (double p1e, double r1[], double p1[], double m[], double a[], - double vs[], double vs4) -{ - int i, j, k, nit, nit2, maxit=50, maxit2=1000; - double nr1[3], d1, v[4], ee[4], ssqo, ssq, pcos, p1r1[3], p1m[3], dedth[4][4]; - double fakt, alpha=0.01, dedth2[4][4], diag[4], ddinv2[4][4], vt[4], dd; - - if (fabs(p1e) < 0.0001) - { - for (i = 0; i < 3; i++) - r1[i] = p1[i]; - } - else - { - for (i = 0; i < 3; i++) - nr1[i] = 2.0 * (r1[i] - m[i])/(SQR(a[i])); - - d1 = -DOT_VECTORS(nr1, r1); - ee[0] = DOT_VECTORS(vs, r1) + vs4; - ee[1] = -1.0; - - for (i = 0; i < 3; i++) - ee[1] += SQR((r1[i] - m[i]) / a[i]); - - ee[2] = DOT_VECTORS(nr1, r1) + d1; - ee[3] = DOT_VECTORS(nr1, p1) + d1; - - ssqo = SQR(ee[0]) + SQR(ee[1]) + SQR(ee[2]) + SQR(ee[3]); - ssq = ssqo; - - nit = 0; - - while ((ssq > ELLIPSOID_TINY) && (nit < maxit)) - { - nit++; - - for (i = 0; i < 3; i++) - { - dedth[i][0] = vs[i]; - dedth[i][1] = 2.0 * (r1[i] - m[i]) / SQR(a[i]); - dedth[i][2] = 2.0 * (2.0 * r1[i] - m[i]) / SQR(a[i]); - dedth[i][3] = 2.0 * p1[i] / SQR(a[i]); - } - - dedth[3][0] = 0.0; - dedth[3][1] = 0.0; - dedth[3][2] = 1.0; - dedth[3][3] = 1.0; -#if 1 - MAKE_3DVECTOR21(p1, r1, p1r1); - normalize_vector(p1r1, p1r1); - - MAKE_3DVECTOR21(p1, m, p1m); - normalize_vector(p1m, p1m); - - pcos = DOT_VECTORS(p1r1, p1m); - - if (pcos > 0.1) - dd = 1.0 - pow(pcos, 100); - else -#endif - dd = 1.0; - - for (i = 0; i < 4; i++) - { - v[i] = 0.0; - - for (j = 0; j < 4; j++) - v[i] -= dedth[i][j] * ee[j]; - } - - for (i = 0; i < 4; i++) - { - for (j = 0; j < 4; j++) - { - dedth2[i][j] = 0.0; - - for (k = 0; k < 4; k++) - dedth2[i][j] += dedth[i][k] * dedth[j][k]; - } - } - - for (i = 0; i < 4; i++) - diag[i] = dedth2[i][i]; - - nit2 = 0; - - while ((ssq >= ssqo) && (nit2 < maxit2)) - { - for (i = 0; i < 4; i++) - dedth2[i][i] = diag[i] * (1.0 + alpha); - - invert_4x4matrix(dedth2, ddinv2); - - for (i = 0; i < 4; i++) - { - vt[i] = 0.0; - - for (j = 0; j < 4; j++) - vt[i] += dd * ddinv2[i][j] * v[j] / 16.0; - } - - for (i = 0; i < 3; i++) - r1[i] += vt[i]; - - d1 += vt[3]; - - for (i = 0; i < 3; i++) - nr1[i] = 2.0 * (r1[i] - m[i])/SQR(a[i]); - - ee[0] = DOT_VECTORS(vs, r1) + vs4; - ee[1] = -1.0; - - for (i = 0; i < 3; i++) - ee[1] += SQR((r1[i] - m[i])/a[i]); - - ee[2] = DOT_VECTORS(nr1, r1) + d1; - ee[3] = DOT_VECTORS(nr1, p1) + d1; - - ssqo = ssq; - - ssq = SQR(ee[0]) + SQR(ee[1]) + SQR(ee[2]) + SQR(ee[3]); - - alpha *= 4.0; - nit2++; - } - - alpha /= 8.0; - - fakt = 0.5; - - nit2 = 0; - - while ((ssq <= ssqo) && (nit2 < maxit2)) - { - fakt *= 2.0; - - for (i = 0; i < 3; i++) - r1[i] += vt[i] * fakt; - - d1 += vt[3] * fakt; - - for (i = 0; i < 3; i++) - nr1[i] = 2.0 * (r1[i] - m[i]) / SQR(a[i]); - - ee[0] = DOT_VECTORS(vs, r1) + vs4; - ee[1] = -1.0; - - for (i=0; i<3; i++) - ee[1] += SQR((r1[i] - m[i]) / a[i]); - - ee[2] = DOT_VECTORS(nr1, r1) + d1; - ee[3] = DOT_VECTORS(nr1, p1) + d1; - - ssqo = ssq; - - ssq = SQR(ee[0]) + SQR(ee[1]) + SQR(ee[2]) + SQR(ee[3]); - - nit2++; - } - - for (i = 0; i < 3; i++) - r1[i] -= vt[i] * fakt; - - d1 -= vt[3] * fakt; - - for (i=0; i<3; i++) - nr1[i] = 2.0 * (r1[i] - m[i]) / SQR(a[i]); - - ee[0] = DOT_VECTORS(vs, r1) + vs4; - ee[1] = -1.0; - - for (i = 0; i < 3; i++) - ee[1] += SQR((r1[i] - m[i]) / a[i]); - - ee[2] = DOT_VECTORS(nr1, r1) + d1; - ee[3] = DOT_VECTORS(nr1, p1) + d1; - - ssq = SQR(ee[0]) + SQR(ee[1]) + SQR(ee[2]) + SQR(ee[3]); - ssqo = ssq; - } - } - return 1; - -} /* calc_r */ - -/* ------------------------------------------------------------------------- - dell - calculate the distance between two points on the surface of an - ellipsoid over the surface of the ellipsoid. - - Input: - r1, r2: the points - m: center of the ellipsoid - a: axes of the ellipsoid - vs: orientation plane - - Output: - afst: the distance over the surface of the ellipsoid - wrap_pts: line segments connecting r1 to r2 along the ellipsoid surface ----------------------------------------------------------------------------- */ -static void dell (double r1[], double r2[], double m[], double a[], - double vs[], double vs4, SBoolean far_side_wrap, - double *afst, - double** wrap_pts, int* num_wrap_pts) -{ - int i, j, k, l, imax, index, numPts = 101; - int numSegs = numPts - 1; - int numInteriorPts = numPts - 2; - - double u[3], ux[3], mu, a0[3], ar1[3], ar2[3], phi, dphi, phi0, len, - r0[3][3], vsy[3], vsz[3], rphi[3][3], t[3], r[3], f1[3], f2[3], dr[3], - aa, bb, cc, mu3, s[100][3], dv[3], q[3], p[3]; - - MAKE_3DVECTOR21(r1, r2, dr); - len = VECTOR_MAGNITUDE(dr); - - /* If the distance between r1 and r2 is very small, then don't bother - * calculating 100 wrap points along the surface of the ellipsoid. - * Just use r1 and r2 as the surface points and return the distance - * between them as the distance along the ellipsoid. - */ - if (len < 0.0001) - { - *num_wrap_pts = 2; - *wrap_pts = (double*) simm_malloc(6 * sizeof(double)); - - (*wrap_pts)[0] = r1[0]; - (*wrap_pts)[1] = r1[1]; - (*wrap_pts)[2] = r1[2]; - - (*wrap_pts)[3] = r2[0]; - (*wrap_pts)[4] = r2[1]; - (*wrap_pts)[5] = r2[2]; - - *afst = len; - return; - } - - ux[0] = 1.0; - ux[1] = 0.0; - ux[2] = 0.0; - - imax = 0; - - for (i = 1; i < 3; i++) - if (fabs(vs[i]) > fabs(vs[imax])) - imax = i; - - clear_vector(u, 3); - - u[imax] = 1.0; - - mu = (-DOT_VECTORS(vs, m) - vs4) / DOT_VECTORS(vs, u); - - for (i=0;i<3;i++) - a0[i] = m[i] + mu * u[i]; - - MAKE_3DVECTOR21(r1, a0, ar1); - normalize_vector(ar1, ar1); - MAKE_3DVECTOR21(r2, a0, ar2); - normalize_vector(ar2, ar2); - - phi0 = acos(DOT_VECTORS(ar1, ar2)); - - if (far_side_wrap) - dphi = - (2 * M_PI - phi0) / (double)numSegs; - else - dphi = phi0 / (double)numSegs; - - cross_vectors(ar1, ar2, vsz); - normalize_vector(vsz, vsz); - cross_vectors(vsz, ar1, vsy); - - for (i = 0; i < 3; i++) - { - for (j = 0; j < 3; j++) - { - rphi[i][j] = 0.0; - r0[i][j] = 0.0; - } - } - - for (i = 0; i < 3; i++) - { - r0[i][0] = ar1[i]; - r0[i][1] = vsy[i]; - r0[i][2] = vsz[i]; - } - - rphi[2][2] = 1; - - for (i = 0; i < numInteriorPts; i++) - { - phi = (i + 1) * dphi; - rphi[0][0] = cos(phi); - rphi[0][1] = -sin(phi); - rphi[1][0] = sin(phi); - rphi[1][1] = cos(phi); - - for (j = 0; j < 3; j++) - { - r[j] = 0.0; - - for (k = 0; k < 3; k++) - { - t[k] = 0.0; - - for (l = 0; l < 3; l++) - t[k] += rphi[k][l] * ux[l]; - - r[j] += r0[j][k] * t[k]; - } - } - - for (j = 0; j < 3; j++) - { - f1[j] = r[j]/a[j]; - f2[j] = (a0[j] - m[j])/a[j]; - } - - aa = DOT_VECTORS(f1, f1); - bb = 2.0 * (DOT_VECTORS(f1, f2)); - cc = DOT_VECTORS(f2, f2) - 1.0; - mu3 = (-bb + sqrt(SQR(bb) - 4.0 * aa * cc)) / (2.0 * aa); - - for (j = 0; j < 3; j++) - s[i][j] = a0[j] + mu3 * r[j]; - } - - *num_wrap_pts = numPts; - - if (*wrap_pts == NULL) - *wrap_pts = (double*) simm_malloc((*num_wrap_pts) * 3 * sizeof(double)); - - (*wrap_pts)[0] = r1[0]; - (*wrap_pts)[1] = r1[1]; - (*wrap_pts)[2] = r1[2]; - - (*wrap_pts)[(numPts-1)*3] = r2[0]; - (*wrap_pts)[(numPts-1)*3+1] = r2[1]; - (*wrap_pts)[(numPts-1)*3+2] = r2[2]; - - for (i = 0; i < numInteriorPts; i++) - { - index = (i+1) * 3; - - (*wrap_pts)[index] = s[i][0]; - (*wrap_pts)[index+1] = s[i][1]; - (*wrap_pts)[index+2] = s[i][2]; - } - - *afst = 0.0; - - for (i = 0; i < numSegs; i++) - { - p[0] = (*wrap_pts)[i*3]; - p[1] = (*wrap_pts)[i*3+1]; - p[2] = (*wrap_pts)[i*3+2]; - - q[0] = (*wrap_pts)[(i+1)*3]; - q[1] = (*wrap_pts)[(i+1)*3+1]; - q[2] = (*wrap_pts)[(i+1)*3+2]; - - MAKE_3DVECTOR21(q, p, dv); - - *afst += VECTOR_MAGNITUDE(dv); - } -} /* dell */ - - - - -/* ------------------------------------------------------------------------- - pt_to_ellipse - this routine is courtesy of Dave Eberly - www.magic-software.com - - Input: Ellipse (x/a)^2+(y/b)^2 = 1, point (u,v). - - Output: Closest point (x,y) on ellipse to (u,v), function returns - the distance sqrt((x-u)^2+(y-v)^2). ----------------------------------------------------------------------------- */ -static double pt_to_ellipse2 (double a, double b, double u, double v, double* x, double* y) -{ - /* Graphics Gems IV algorithm for computing distance from point to - * ellipse (x/a)^2 + (y/b)^2 = 1. The algorithm as stated is not stable - * for points near the coordinate axes. The first part of this code - * handles those points separately. - */ - double a2 = a*a, b2 = b*b; - double u2 = u*u, v2 = v*v; - double a2u2 = a2*u2, b2v2 = b2*v2; - double dx, dy, xda, ydb; - int i, which; - double t, P, Q, P2, Q2, f, fp; - - SBoolean nearXOrigin = (SBoolean) EQUAL_WITHIN_ERROR(0.0,u); - SBoolean nearYOrigin = (SBoolean) EQUAL_WITHIN_ERROR(0.0,v); - - /* handle points near the coordinate axes */ - if (nearXOrigin && nearYOrigin) - { - if (a < b) - { - *x = (u < 0.0 ? -a : a); - *y = v; - return a; - } else { - *x = u; - *y = (v < 0.0 ? -b : b); - return b; - } - } - - if (nearXOrigin) /* u == 0 */ - { - if ( a >= b || fabs(v) >= b - a2/b ) - { - *x = u; - *y = ( v >= 0 ? b : -b ); - dy = *y - v; - return fabs(dy); - } - else - { - *y = b2 * v / (b2-a2); - dy = *y - v; - ydb = *y / b; - *x = a * sqrt(fabs(1 - ydb*ydb)); - return sqrt(*x * *x + dy*dy); - } - } - - if (nearYOrigin) /* v == 0 */ - { - if ( b >= a || fabs(u) >= a - b2/a ) - { - *x = ( u >= 0 ? a : -a ); - dx = *x - u; - *y = v; - return fabs(*x - u); - } - else - { - *x = a2 * u / (a2-b2); - dx = *x - u; - xda = *x / a; - *y = b * sqrt(fabs(1 - xda*xda)); - return sqrt(dx*dx + *y * *y); - } - } - - /* initial guess */ - if ( (u/a)*(u/a) + (v/b)*(v/b) < 1.0 ) - { - which = 0; - t = 0.0; - } - else - { - double max = a; - - which = 1; - - if ( b > max ) - max = b; - - t = max * sqrt(u*u + v*v); - } - - for (i = 0; i < 64; i++) - { - P = t+a2; - P2 = P*P; - Q = t+b2; - Q2 = Q*Q; - f = P2*Q2 - a2u2*Q2 - b2v2*P2; - - //dkb - in original (above) was: if ( fabs(f) < 1e-09 ) - if (fabs(f) < 1e-20) - break; - - fp = 2.0 * (P*Q*(P+Q) - a2u2*Q - b2v2*P); - t -= f / fp; - } - - *x = a2 * u / P; - *y = b2 * v / Q; - dx = *x - u; - dy = *y - v; - - return sqrt(dx*dx + dy*dy); - -} /* pt_to_ellipse */ - -/* ------------------------------------------------------------------------- - pt_to_ellipsoid - this routine is courtesy of Dave Eberly - www.magic-software.com - - Input: Ellipsoid (x/a)^2+(y/b)^2+(z/c)^2 = 1, point (u,v,w). - - Output: Closest point (x,y,z) on ellipsoid to (u,v,w), function returns - the distance sqrt((x-u)^2+(y-v)^2+(z-w)^2). ----------------------------------------------------------------------------- */ -static double pt_to_ellipsoid2 (double a, double b, double c, - double u, double v, double w, - double* x, double* y, double* z, - int specialCaseAxis) -{ - /* Graphics Gems IV algorithm for computing distance from point to - * ellipsoid (x/a)^2 + (y/b)^2 +(z/c)^2 = 1. The algorithm as stated - * is not stable for points near the coordinate planes. The first part - * of this code handles those points separately. - */ - int i,j; - - /* handle points near the coordinate planes by reducing the problem - * to a 2-dimensional pt-to-ellipse. - * - * if uvw is close to more than one coordinate plane, pick the 2d - * elliptical cross-section with the narrowest radius. - */ - if (specialCaseAxis < 0) - { - double abc[3], uvw[3], minEllipseRadiiSum = FLT_MAX; - - abc[0] = a; abc[1] = b; abc[2] = c; - uvw[0] = u; uvw[1] = v; uvw[2] = w; - - for (i = 0; i < 3; i++) - { - if (EQUAL_WITHIN_ERROR(0.0,uvw[i])) - { - double ellipseRadiiSum = 0; - - for (j = 0; j < 3; j++) - if (j != i) - ellipseRadiiSum += abc[j]; - - if (minEllipseRadiiSum > ellipseRadiiSum) - { - specialCaseAxis = i; - minEllipseRadiiSum = ellipseRadiiSum; - } - } - } - } - if (specialCaseAxis == 0) /* use elliptical cross-section in yz plane */ - { - *x = u; - return pt_to_ellipse2(b, c, v, w, y, z); - } - if (specialCaseAxis == 1) /* use elliptical cross-section in xz plane */ - { - *y = v; - return pt_to_ellipse2(c, a, w, u, z, x); - } - if (specialCaseAxis == 2) /* use elliptical cross-section in xy plane */ - { - *z = w; - return pt_to_ellipse2(a, b, u, v, x, y); - } - - /* if we get to here, then the point is not close to any of the major planes, - * so we can solve the general case. - */ - { - double a2 = a*a, b2 = b*b, c2 = c*c; - double u2 = u*u, v2 = v*v, w2 = w*w; - double a2u2 = a2*u2, b2v2 = b2*v2, c2w2 = c2*w2; - double dx, dy, dz, t, f; - - /* initial guess */ - if ( (u/a)*(u/a) + (v/b)*(v/b) + (w/c)*(w/c) < 1.0 ) - { - t = 0.0; - } - else - { - double max = a; - - if ( b > max ) - max = b; - if ( c > max ) - max = c; - - t = max*sqrt(u*u+v*v+w*w); - } - - for (i = 0; i < 64; i++) - { - double P = t+a2, P2 = P*P; - double Q = t+b2, Q2 = Q*Q; - double R = t+c2, _R2 = R*R; - double PQ, PR, QR, PQR, fp; - - f = P2*Q2*_R2 - a2u2*Q2*_R2 - b2v2*P2*_R2 - c2w2*P2*Q2; - - // dkb - in original (above) was: if ( fabs(f) < 1e-09 ) - if ( fabs(f) < 1e-20) - { - *x = a2 * u / P; - *y = b2 * v / Q; - *z = c2 * w / R; - - dx = *x - u; - dy = *y - v; - dz = *z - w; - return sqrt(dx*dx+dy*dy+dz*dz); - } - - PQ = P*Q; - PR = P*R; - QR = Q*R; - PQR = P*Q*R; - fp = 2.0*(PQR*(QR+PR+PQ)-a2u2*QR*(Q+R)-b2v2*PR*(P+R)-c2w2*PQ*(P+Q)); - - t -= f/fp; - } - } - return -1.0; - -} /* pt_to_ellipsoid2 */ diff --git a/OpenSim/Utilities/simmToOpenSim/enter.c b/OpenSim/Utilities/simmToOpenSim/enter.c deleted file mode 100644 index 72bda1d453..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/enter.c +++ /dev/null @@ -1,508 +0,0 @@ -/******************************************************************************* - - ENTER.C - - Author: Peter Loan - - Date: 8-DEC-88 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines that keep track of various - model parameters, such as gencoords and segments. They store the user - names and numbers for these parameters, but also generate internal - numbers to reference the structures which hold them. For example, - if in a model file, function #17 is defined first, this function - will internally be referred to as function #0, and all references - to it (by the number 17) will be replaced by references to 0. - - Routines: - enter_gencoord : stores a gencoord reference in a list and returns id - enter_function : stores a function reference in a list and returns id - get_wrap_object : scans array of wrap objects and returns id - enter_segment : stores a segment number in a list and returns id - entergroup : stores a muscle group name in a list and returns id - enter_joint : stores a joint number in a list and returns id - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ - - - -/* ENTER_GENCOORD: whenever a gencoord is read from a file, the user-specified - * number and name are compared to the existing array of gencoords to see - * if that one has already been defined. If it has, the routine just returns - * the internal number of that gencoord. If it has not yet been defined, it - * adds the name and number to a new element in the gencoords array and - * increments model.numgencoords, unless there is no permission to add it. - * You don't want to add a gencoord to the list when you find a new one in a - * muscle file. In this case, you want to print an error because this new - * gencoord was not defined in the joints file. - */ -GeneralizedCoord* enter_gencoord(ModelStruct* model, const char username[], SBoolean permission_to_add) -{ - int i, new_gc; - ReturnCode rc = code_fine; - - if (username == NULL) - return NULL; - - for (i = 0; i < model->numgencoords; i++) - if (STRINGS_ARE_EQUAL(username, model->gencoord[i]->name)) - return model->gencoord[i]; - - if (permission_to_add == no) - return NULL; - - new_gc = model->numgencoords; - - if (model->numgencoords == model->genc_array_size) - { - model->genc_array_size += GENC_ARRAY_INCREMENT; - model->gencoord = (GeneralizedCoord**)simm_realloc(model->gencoord, model->genc_array_size*sizeof(GeneralizedCoord*), &rc); - if (rc == code_bad) - { - model->genc_array_size -= GENC_ARRAY_INCREMENT; - return NULL; - } - } - - model->gencoord[new_gc] = (GeneralizedCoord*)simm_malloc(sizeof(GeneralizedCoord)); - init_gencoord(model->gencoord[new_gc]); - mstrcpy(&model->gencoord[new_gc]->name, username); - - model->numgencoords++; - - return model->gencoord[new_gc]; -} - - -dpWrapObject* get_wrap_object(ModelStruct* model, char username[]) -{ - int i; - - if (username == NULL) - return NULL; - - for (i=0; inum_wrap_objects; i++) - if (STRINGS_ARE_EQUAL(username, model->wrapobj[i]->name)) - return model->wrapobj[i]; - - return NULL; -} - - -/* ENTER_SEGMENT: whenever a segment name is read from a file, the user-specified - * name is compared to the existing array of segments to see if that one has - * already been defined. If it has, the routine just returns the internal - * number of that segment. If it has not yet been defined, it adds the name - * to a new element in the segmentlist and increments model.numsegments, unless - * there is no permission to add the new name. You don't really want to add a - * new segment name when you find segments in a muscle file. In this case, - * if the segment is not already known, you want to print an error message, because - * the muscle is referencing a segment that was not defined in the joints file. - */ -int enter_segment(ModelStruct* model, const char username[], SBoolean permission_to_add) -{ - int i, new_seg; - ReturnCode rc = code_fine; - - if (username == NULL) - return -1; - - for (i = 0; i < model->numsegments; i++) - if (STRINGS_ARE_EQUAL(username, model->segment[i].name)) - return i; - - if (permission_to_add == no) - return -1; - - new_seg = model->numsegments; - - if (model->numsegments == model->segment_array_size) - { - model->segment_array_size += SEGMENT_ARRAY_INCREMENT; - model->segment = (SegmentStruct*)simm_realloc(model->segment, model->segment_array_size*sizeof(SegmentStruct), &rc); - if (rc == code_bad) - { - model->segment_array_size -= SEGMENT_ARRAY_INCREMENT; - return -1; - } - } - - init_segment(model, &model->segment[new_seg]); - mstrcpy(&model->segment[new_seg].name, username); - - model->numsegments++; - - return new_seg; -} - - -int enter_muscle_group(ModelStruct* model, const char username[], int muscleIndex) -{ - int i, j, new_group; - MuscleGroup* mg; - ReturnCode rc = code_fine; - - if (model == NULL || muscleIndex < 0) - return -1; - - for (i=0; inumgroups; i++) - { - if (STRINGS_ARE_EQUAL(username, model->muscgroup[i].name)) - { - mg = &model->muscgroup[i]; - - // If the muscle is already in the group, just return the index of the group. - for (j=0; jnum_muscles; j++) - { - if (muscleIndex == mg->muscle_index[j]) - return i; - } - - // Make room for more groups. - if (mg->num_muscles == mg->muscindex_array_size) - { - mg->muscindex_array_size += MUSCINDEX_ARRAY_INCREMENT; - mg->muscle_index = (int*)simm_realloc(mg->muscle_index, mg->muscindex_array_size*sizeof(int), &rc); - if (rc == code_bad) - { - mg->muscindex_array_size -= MUSCINDEX_ARRAY_INCREMENT; - return -1; - } - } - - // Add the muscle to the group. - mg->muscle_index[mg->num_muscles++] = muscleIndex; - return i; - } - } - - // If you get to here, then the group name has not yet been recorded, meaning - // that this is a new group. The variable "i" now equals the number of groups - // already recorded, so it becomes the index of the group array where you - // want to record this new group. First make sure the muscle group array is - // big enough to hold the new group. - - new_group = i; - - if (model->numgroups == model->muscgroup_array_size) - { - model->muscgroup_array_size += MUSCGROUP_ARRAY_INCREMENT; - model->muscgroup = (MuscleGroup*)simm_realloc(model->muscgroup, model->muscgroup_array_size*sizeof(MuscleGroup), &rc); - if (rc == code_bad) - { - model->muscgroup_array_size -= MUSCGROUP_ARRAY_INCREMENT; - return -1; - } - } - - mg = &model->muscgroup[new_group]; - - mstrcpy(&mg->name,username); - - mg->muscindex_array_size = MUSCINDEX_ARRAY_INCREMENT; - mg->muscle_index = (int*)simm_malloc(mg->muscindex_array_size*sizeof(int)); - if (mg->muscle_index == NULL) - return -1; - - mg->muscle_index[0] = muscleIndex; - mg->num_muscles = 1; - mg->menu.title = NULL; - mg->menu.option = NULL; - mg->menu.numoptions = 0; - - model->numgroups++; - - return new_group; -} - - -int enter_segment_group(ModelStruct* model, const char username[], int segmentIndex) -{ - int i, j, new_group; - SegmentGroup* sg; - ReturnCode rc = code_fine; - - if (segmentIndex < 0) - return -1; - - for (i = 0; i < model->numseggroups; i++) - { - if (STRINGS_ARE_EQUAL(username, model->seggroup[i].name)) - { - sg = &model->seggroup[i]; - - // If the segment is already in the group, just return the index of the group. - for (j=0; jnum_segments; j++) - { - if (segmentIndex == sg->segment[j]) - return i; - } - - // Make room for more groups. - if (sg->num_segments == sg->seg_array_size) - { - sg->seg_array_size += MUSCINDEX_ARRAY_INCREMENT; - - sg->segment = (int*) simm_realloc(sg->segment, sg->seg_array_size * sizeof(int), &rc); - if (rc == code_bad) - { - sg->seg_array_size -= MUSCINDEX_ARRAY_INCREMENT; - return -1; - } - } - - // Add the segment to the group. - sg->segment[sg->num_segments++] = segmentIndex; - - return i; - } - } - - // If you get to here, then the group name has not yet been recorded, meaning - // that this is a new group. The variable "i" now equals the number of groups - // already recorded, so it becomes the index of the group array where you - // want to record this new group. First make sure the group array is - // big enough to hold the new group. - - new_group = i; - - if (model->numseggroups == model->seggroup_array_size) - { - model->seggroup_array_size += MUSCGROUP_ARRAY_INCREMENT; - - if (model->seggroup == NULL) - { - model->seggroup = (SegmentGroup*) simm_malloc(model->seggroup_array_size * sizeof(SegmentGroup)); - rc = (model->seggroup ? code_fine : code_bad); - } - else - model->seggroup = (SegmentGroup*) simm_realloc(model->seggroup, model->seggroup_array_size * sizeof(SegmentGroup), &rc); - if (rc == code_bad) - { - model->seggroup_array_size -= MUSCGROUP_ARRAY_INCREMENT; - return -1; - } - } - - sg = &model->seggroup[new_group]; - - mstrcpy(&sg->name, username); - - sg->seg_array_size = MUSCINDEX_ARRAY_INCREMENT; - - sg->segment = (int*)simm_malloc(sg->seg_array_size * sizeof(int)); - - if (sg->segment == NULL) - return -1; - - sg->segment[0] = segmentIndex; - sg->num_segments = 1; - - model->numseggroups++; - - return new_group; -} - - -int enter_gencoord_group(ModelStruct* model, const char username[], GeneralizedCoord* gencoord) -{ - int i, j, new_group; - GencoordGroup* gg; - ReturnCode rc = code_fine; - - if (gencoord == NULL) - return -1; - - for (i = 0; i < model->numgencgroups; i++) - { - if (STRINGS_ARE_EQUAL(username, model->gencgroup[i].name)) - { - gg = &model->gencgroup[i]; - - // If the gencoord is already in the group, just return the index of the group. - for (j=0; jnum_gencoords; j++) - { - if (gencoord == gg->gencoord[j]) - return i; - } - - // Make room for more groups. - if (gg->num_gencoords == gg->genc_array_size) - { - gg->genc_array_size += MUSCINDEX_ARRAY_INCREMENT; - - gg->gencoord = (GeneralizedCoord**)simm_realloc(gg->gencoord, gg->genc_array_size*sizeof(GeneralizedCoord*), &rc); - if (rc == code_bad) - { - gg->genc_array_size -= MUSCINDEX_ARRAY_INCREMENT; - return -1; - } - } - - // Add the gencoord to the group. - gg->gencoord[gg->num_gencoords++] = gencoord; - - return i; - } - } - - // If you get to here, then the group name has not yet been recorded, meaning - // that this is a new group. The variable "i" now equals the number of groups - // already recorded, so it becomes the index of the group array where you - // want to record this new group. First make sure the group array is - // big enough to hold the new group. - - new_group = i; - - if (model->numgencgroups == model->gencgroup_array_size) - { - model->gencgroup_array_size += MUSCGROUP_ARRAY_INCREMENT; - - if (model->gencgroup == NULL) - { - model->gencgroup = (GencoordGroup*)simm_malloc(model->gencgroup_array_size*sizeof(GencoordGroup)); - rc = (model->gencgroup ? code_fine : code_bad); - } - else - model->gencgroup = (GencoordGroup*)simm_realloc(model->gencgroup, model->gencgroup_array_size*sizeof(GencoordGroup), &rc); - if (rc == code_bad) - { - model->gencgroup_array_size -= MUSCGROUP_ARRAY_INCREMENT; - return -1; - } - } - - gg = &model->gencgroup[new_group]; - - mstrcpy(&gg->name, username); - - gg->genc_array_size = MUSCINDEX_ARRAY_INCREMENT; - - gg->gencoord = (GeneralizedCoord**)simm_malloc(gg->genc_array_size * sizeof(GeneralizedCoord*)); - - if (gg->gencoord == NULL) - return -1; - - gg->gencoord[0] = gencoord; - gg->num_gencoords = 1; - - model->numgencgroups++; - - return new_group; -} - - -/* ENTER_JOINT: whenever a joint name is read from a file, the user-specified - * name is compared to the existing array of joints to see if that one has - * already been defined. If it has, the routine just returns the internal - * number of that joint. If it has not yet been defined, it adds the name - * to a new element in the jointlist and increments model.numjoints, unless - * there is no permission to add the new name. - */ -int enter_joint(ModelStruct* model, const char username[], SBoolean permission_to_add) -{ - int i, new_jnt; - ReturnCode rc = code_fine; - - if (username == NULL) - return -1; - - for (i = 0; i < model->numjoints; i++) - if (STRINGS_ARE_EQUAL(username, model->joint[i].name)) - return i; - - if (permission_to_add == no) - return -1; - - new_jnt = model->numjoints; - - if (model->numjoints == model->joint_array_size) - { - model->joint_array_size += JOINT_ARRAY_INCREMENT; - model->joint = (JointStruct*)simm_realloc(model->joint, model->joint_array_size*sizeof(JointStruct),&rc); - if (rc == code_bad) - { - model->joint_array_size -= JOINT_ARRAY_INCREMENT; - return -1; - } - } - - init_joint(model, &model->joint[new_jnt]); - mstrcpy(&model->joint[new_jnt].name, username); - - model->numjoints++; - - return new_jnt; -} - -#if ! OPENSIM_BUILD - -int enter_preference(const char name[], const char value[]) -{ - int i; - - // If a preference with the same name is already in the list, - // replace its value with the new one. - for (i=0; i -#include - -#include "universal.h" -#include "functions.h" -#include "wefunctions.h" -#include "sdfunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ -#define GROUND_NAME "ground" - -typedef struct { - int jointnum; - int dofnum; - SBoolean constrained; -} GencoordInfo; - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static double gWorld_x[] = {1.0, 0.0, 0.0, 0.0}; -static double gWorld_y[] = {0.0, 1.0, 0.0, 0.0}; -static double gWorld_z[] = {0.0, 0.0, 1.0, 0.0}; - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ -extern SDSegment* SDseg; -extern int num_SD_segs; -extern int* joint_order; -extern int gAngleUnits; -extern char buffer[]; - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void make_string_xml_safe(char str[]); -static void make_names_xml_safe(ModelStruct* ms); -static void write_xml_gravity(FILE* fp, ModelStruct* ms); -static void write_xml_muscle_groups(FILE* fp, ModelStruct* ms); -static void write_xml_muscle(FILE* fp, ModelStruct* ms, dpMuscleStruct* m, const char* muscleClassName, int writingDefault, int angleUnits); -static void write_xml_muscles(FILE* fp, ModelStruct* ms, int angleUnits); -static void write_xml_markers(FILE* fp, ModelStruct* ms, const char* markerSetOut); -static void write_vtk_bone(PolyhedronStruct* bone, char geometryDirectory[], char filename[]); -static int segment_has_wrap_objects(ModelStruct* ms, SegmentStruct* seg); -static void write_xml_wrap_object(FILE* fp, dpWrapObject* wo, int angleUnits); -static void write_xml_units(FILE* fp, ModelStruct* ms); -static void write_xml_defaults(FILE* fp, ModelStruct* ms, int angleUnits); -static void extract_joint_locations_and_orientations(ModelStruct* ms, - JointStruct* joint, int dof_order[], double locationInParent[], - double orientationInParent[], double locationInChild[], double orientationInChild[]); -static ReturnCode write_simbody_engine(FILE* fp, ModelStruct* ms, char geometryDirectory[], const char* markerSetOut, int angleUnits); -static void write_opensim_function(FILE* fp, dpFunction* function, double ind_conv, double dep_conv, int tab_level); -static void write_opensim_constant_function(FILE* fp, double value, int tab_level); -static SBoolean function_is_simple_linear(dpFunction* function); -static ReturnCode make_simbody_model(FILE* fp, ModelStruct* ms, char geometryDirectory[], int angleUnits); -static void make_simbody_joint(ModelStruct* ms, FILE* fp, JointSDF* jntsdf, int jointnum, int sdnum, - SegmentSDF segs[], GencoordInfo gcInfo[], int* dofcount, int* constrainedcount, - char geometryDirectory[], int angleUnits); -static void write_opensim_ground_body(FILE* fp, ModelStruct* ms, SegmentStruct* seg, char geometryDirectory[], int angleUnits); -static void write_opensim_coordinate(FILE* fp, ModelStruct* ms, JointStruct* joint, int dofIndex, int angleUnits); -static void write_opensim_transformAxis(FILE* fp, ModelStruct* ms, JointStruct* joint, int dofIndex, - Direction dir, SBoolean writeFunction, int current_dof, int angleUnits); -static void write_opensim_constraint(FILE* fp, ModelStruct* ms, DofStruct* dof, int dofIndex, int angleUnits); -static void convert_fixed_joints(ModelStruct* ms, GencoordInfo gcInfo[]); -static dpFunction* add_constant_function_to_model(ModelStruct* ms, double y_value); -void convert_dof_to_function(ModelStruct* ms, DofStruct* dof, GeneralizedCoord* gencoord); -static SBoolean joint_is_fixed(JointStruct* joint); - - -ReturnCode write_opensim_model(ModelStruct* ms, char filename[], char geometryDirectory[], const char* markerSetOut, int angleUnits) -{ - int i; - FILE* fp; - ReturnCode rc = code_fine; - - // First see if all of the joints can be converted to OpenSim joints. - // If any joint has a [function] translation after a rotation, it cannot be converted. - // TODO2.0: it's OK to have a translation at the end of the order if the - // joint will be processed in reverse. - for (i=0; inumjoints; i++) - { - if (ms->joint[i].order[0] > 0) - { - if (ms->joint[i].dofs[TX].type == function_dof || - ms->joint[i].dofs[TY].type == function_dof || - ms->joint[i].dofs[TZ].type == function_dof) - { - sprintf(buffer, "Joint %s cannot be converted to OpenSim because it contains a [function] translation after a rotation.\n", ms->joint[i].name); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); - return code_bad; - } - } - } - - // Make sure the name of the ground segment is "ground". - if (STRINGS_ARE_NOT_EQUAL(ms->segment[ms->ground_segment].name, GROUND_NAME)) - { - free(ms->segment[ms->ground_segment].name); - mstrcpy(&ms->segment[ms->ground_segment].name, GROUND_NAME); - } - - // Make sure names are legal: meeting the following criteria Alphanumeric with _ or - or . allowed - make_names_xml_safe(ms); - - fp = fopen(filename, "w"); - fprintf(fp, "\n"); - fprintf(fp, "\n"); - fprintf(fp, "\n", ms->name); - write_xml_defaults(fp, ms, angleUnits); - //if (angleUnits == DEGREES) - // fprintf(fp, "\t degrees \n"); - //else - // fprintf(fp, "\t radians \n"); - write_xml_units(fp, ms); - write_xml_muscles(fp, ms, angleUnits); - rc = write_simbody_engine(fp, ms, geometryDirectory, markerSetOut, angleUnits); - fprintf(fp, "\n"); - fprintf(fp, "\n"); - fclose(fp); - - if (rc == code_bad) - remove(filename); - - return rc; -} - -#if SIMMTOOPENSIM || OPENSMAC - -void convert_string(char str[], SBoolean prependUnderscore) -{ - int i, len; - - len = strlen(str); - - for (i = 0; i < len; i++) - { - if (str[i]=='>' || str[i]=='<' || str[i]=='&') - str[i]='_'; - } -} - -#endif - -// Fix names in the model to take out XML meta characters that cause parsing problems (bug 505). -Ayman -static void make_names_xml_safe(ModelStruct* ms) -{ - int i, j; - - for (i = 0; i < ms->numsegments; i++) - { - SegmentStruct* seg = &ms->segment[i]; - char* name = ms->segment[i].name; - convert_string(name, yes); - for (j=0; jsegment[i].numMarkers; j++) - { - char* markerName = ms->segment[i].marker[j]->name; - convert_string(markerName, yes); - } - } - - for (i = 0; i < ms->numgencoords; i++) - { - char* name = ms->gencoord[i]->name; - convert_string(name, yes); - } - - for (i = 0; i < ms->numjoints; i++) - { - char* name = ms->joint[i].name; - convert_string(name, yes); - } - - for (i = 0; i < ms->nummuscles; i++) - { - char* name = ms->muscle[i]->name; - convert_string(name, yes); - } - - for (i = 0; i < ms->numgroups; i++) - { - char* name = ms->muscgroup[i].name; - convert_string(name, yes); - } - - for (i = 0; i < ms->num_wrap_objects; i++) - { - char* name = ms->wrapobj[i]->name; - convert_string(name, yes); - } -} - - -static void write_xml_gravity(FILE* fp, ModelStruct* ms) -{ - if (ms->gravity == smX) - fprintf(fp, "\t\t\t9.80665 0.0 0.0\n"); - else if (ms->gravity == smNegX) - fprintf(fp, "\t\t\t-9.80665 0.0 0.0\n"); - else if (ms->gravity == smY) - fprintf(fp, "\t\t\t0.0 9.80665 0.0\n"); - else if (ms->gravity == smNegY) - fprintf(fp, "\t\t\t0.0 -9.80665 0.0\n"); - else if (ms->gravity == smZ) - fprintf(fp, "\t\t\t0.0 0.0 9.80665\n"); - else if (ms->gravity == smNegZ) - fprintf(fp, "\t\t\t0.0 0.0 -9.80665\n"); - else if (ms->gravity == smNoAlign) - fprintf(fp, "\t\t\t0.0 0.0 0.0\n"); - else // -Y is the default - fprintf(fp, "\t\t\t0.0 -9.80665 0.0\n"); -} - - -static void write_xml_muscle_groups(FILE* fp, ModelStruct* ms) -{ - int i, j; - - fprintf(fp, "\t\n"); - for (i = 0; i < ms->numgroups; i++) - { - fprintf(fp, "\t\t\n", ms->muscgroup[i].name); - fprintf(fp, "\t\t\t"); - for (j = 0; j < ms->muscgroup[i].num_muscles; j++) - fprintf(fp, "%s ", ms->muscle[ms->muscgroup[i].muscle_index[j]]->name); - fprintf(fp, "\n"); - fprintf(fp, "\t\t\n"); - } - fprintf(fp, "\t\n"); -} - - -#define NEED_TO_WRITE_MUSCLE_VALUE(field) \ - m->field && (writingDefault || m->field != ms->default_muscle->field) - -static void write_xml_muscle(FILE* fp, ModelStruct* ms, dpMuscleStruct* m, const char* muscleClassName, int writingDefault, int angleUnits) -{ - int j; - double ind_conv = 1.0; - - fprintf(fp, "\t\t<%s name=\"%s\">\n", muscleClassName, m->name); - - /* Attachment points. */ - if (m->path && m->path->num_orig_points > 0) { - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - for (j = 0; j < m->path->num_orig_points; j++) - { - dpMusclePoint* mp = &m->path->mp_orig[j]; - if (mp->isMovingPoint) - { - fprintf(fp, "\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t %s \n", ms->segment[getMusclePointSegment(m,j)].name); - if (mp->function[0]) - { - GeneralizedCoord* gencoord = (GeneralizedCoord*)mp->gencoord[0]; - if (angleUnits == RADIANS) - { - // check if gencoord (independent coordinate) is rotational - if (gencoord->type == rotation_gencoord) - ind_conv = DEG_TO_RAD; - } - fprintf(fp, "\t\t\t\t\t %s \n", gencoord->name); - fprintf(fp, "\t\t\t\t\t\n"); - write_opensim_function(fp, mp->function[0], ind_conv, 1.0, 6); - fprintf(fp, "\t\t\t\t\t\n"); - } - else - { - fprintf(fp, "\t\t\t\t\t\n"); - write_opensim_constant_function(fp, mp->point[0], 6); - fprintf(fp, "\t\t\t\t\t\n"); - } - if (mp->function[1]) - { - GeneralizedCoord* gencoord = (GeneralizedCoord*)mp->gencoord[1]; - if (angleUnits == RADIANS) - { - // check if gencoord (independent coordinate) is rotational - if (gencoord->type == rotation_gencoord) - ind_conv = DEG_TO_RAD; - } - fprintf(fp, "\t\t\t\t\t %s \n", gencoord->name); - fprintf(fp, "\t\t\t\t\t\n"); - write_opensim_function(fp, mp->function[1], ind_conv, 1.0, 6); - fprintf(fp, "\t\t\t\t\t\n"); - } - else - { - fprintf(fp, "\t\t\t\t\t\n"); - write_opensim_constant_function(fp, mp->point[1], 6); - fprintf(fp, "\t\t\t\t\t\n"); - } - if (mp->function[2]) - { - GeneralizedCoord* gencoord = (GeneralizedCoord*)mp->gencoord[2]; - if (angleUnits == RADIANS) - { - // check if gencoord (independent coordinate) is rotational - if (gencoord->type == rotation_gencoord) - ind_conv = DEG_TO_RAD; - } - fprintf(fp, "\t\t\t\t\t %s \n", gencoord->name); - fprintf(fp, "\t\t\t\t\t\n"); - write_opensim_function(fp, mp->function[2], ind_conv, 1.0, 6); - fprintf(fp, "\t\t\t\t\t\n"); - } - else - { - fprintf(fp, "\t\t\t\t\t\n"); - write_opensim_constant_function(fp, mp->point[2], 6); - fprintf(fp, "\t\t\t\t\t\n"); - } - // Check if OpenSim can handle one of the attachment coordinates being a constant. - // If it can't then you have to make up a gencoord name and use it in the natural - // cubic spline (2 points, constant value). - fprintf(fp, "\t\t\t\t\n"); - } - else if (mp->isVia) - { - // If the gencoord used for this via point range is [primarily] translational, then do - // not convert the range values. If it's rotational, then use DEG_TO_RAD (if angleUnits - // is RADIANS, which means the user wants radians in the output file). - double conversion; - GeneralizedCoord* gencoord = (GeneralizedCoord*)mp->viaRange.gencoord; - if (gencoord->type == rotation_gencoord && angleUnits == RADIANS) - conversion = DEG_TO_RAD; - else - conversion = 1.0; - - fprintf(fp, "\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", mp->point[0], - mp->point[1], mp->point[2]); - fprintf(fp, "\t\t\t\t\t %s \n", ms->segment[getMusclePointSegment(m,j)].name); - fprintf(fp, "\t\t\t\t\t %s \n", gencoord->name); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf \n", mp->viaRange.start * conversion, - mp->viaRange.end * conversion); - fprintf(fp, "\t\t\t\t\n"); - } - else // regular muscle point - { - fprintf(fp, "\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", mp->point[0], - mp->point[1], mp->point[2]); - fprintf(fp, "\t\t\t\t\t %s \n", ms->segment[getMusclePointSegment(m,j)].name); - fprintf(fp, "\t\t\t\t\n"); - } - } - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - - if (m->numWrapStructs > 0) - { - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - for (j = 0; j < m->numWrapStructs; j++) - { - dpWrapObject* wo = m->wrapStruct[j]->wrap_object; - fprintf(fp, "\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t %s \n", wo->name); - if (wo->wrap_type == dpWrapEllipsoid) - fprintf(fp, "\t\t\t\t\t %s \n", get_wrap_algorithm_name(m->wrapStruct[j]->wrap_algorithm)); - if (m->wrapStruct[j]->startPoint > 0 || m->wrapStruct[j]->endPoint > 0) - fprintf(fp,"\t\t\t\t\t %d %d \n", m->wrapStruct[j]->startPoint, m->wrapStruct[j]->endPoint); - fprintf(fp, "\t\t\t\t\n"); - } - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - } - - fprintf(fp, "\t\t\t\n"); - } - - /* Simple (double) parameters. */ - if (NEED_TO_WRITE_MUSCLE_VALUE(max_isometric_force)) - fprintf(fp, "\t\t\t %.12lf \n", *m->max_isometric_force); - if (NEED_TO_WRITE_MUSCLE_VALUE(optimal_fiber_length)) - fprintf(fp, "\t\t\t %.12lf \n", *m->optimal_fiber_length); - if (NEED_TO_WRITE_MUSCLE_VALUE(resting_tendon_length)) - fprintf(fp, "\t\t\t %.12lf \n", *m->resting_tendon_length); - if (NEED_TO_WRITE_MUSCLE_VALUE(pennation_angle)) - { - if (angleUnits == RADIANS) - fprintf(fp, "\t\t\t %.12lf \n", *m->pennation_angle * DEG_TO_RAD); - else - fprintf(fp, "\t\t\t %.12lf \n", *m->pennation_angle); - } - if (NEED_TO_WRITE_MUSCLE_VALUE(max_contraction_vel)) - fprintf(fp, "\t\t\t %.12lf \n", *m->max_contraction_vel); - - /* muscle model. */ - //if (m->muscle_model_index) // Conservative fix in case muscle model is not specified. Ayman 1/07 - // fprintf(fp, "\t\t\t %d \n", *m->muscle_model_index); - - /* Dynamic parameters. */ - for (j = 0; j < m->num_dynamic_params; j++) - { - // map dynamic parameter "timescale" to "time_scale" property name - if (NEED_TO_WRITE_MUSCLE_VALUE(dynamic_params[j])) - { - if(!strcmp(m->dynamic_param_names[j],"timescale")) - fprintf(fp, "\t\t\t %.12lf \n", *m->dynamic_params[j]); - else - fprintf(fp, "\t\t\t<%s> %.12lf \n", m->dynamic_param_names[j], *m->dynamic_params[j], m->dynamic_param_names[j]); - } - } - - /* Tendon force-length curve -- only write if non-default value. */ - if (NEED_TO_WRITE_MUSCLE_VALUE(tendon_force_len_func)) - { - fprintf(fp, "\t\t\t\n"); - write_opensim_function(fp, *m->tendon_force_len_func, 1.0, 1.0, 5); - fprintf(fp, "\t\t\t\n"); - } - - /* Active force-length curve -- only write if non-default value. */ - if (NEED_TO_WRITE_MUSCLE_VALUE(active_force_len_func)) - { - fprintf(fp, "\t\t\t\n"); - write_opensim_function(fp, *m->active_force_len_func, 1.0, 1.0, 5); - fprintf(fp, "\t\t\t\n"); - } - - /* Passive force-length curve -- only write if non-default. */ - if (NEED_TO_WRITE_MUSCLE_VALUE(passive_force_len_func)) - { - fprintf(fp, "\t\t\t\n"); - write_opensim_function(fp, *m->passive_force_len_func, 1.0, 1.0, 5); - fprintf(fp, "\t\t\t\n"); - } - - /* Force-velocity curve -- only write if non-default. */ - if (NEED_TO_WRITE_MUSCLE_VALUE(force_vel_func)) - { - fprintf(fp, "\t\t\t\n"); - write_opensim_function(fp, *m->force_vel_func, 1.0, 1.0, 5); - fprintf(fp, "\t\t\t\n"); - } - - fprintf(fp, "\t\t\n", muscleClassName); - -} - -static void write_xml_muscles(FILE* fp, ModelStruct* ms, int angleUnits) -{ - int i; - char muscleClassName[64]; - - fprintf(fp, "\t\n"); - write_xml_muscle_groups(fp, ms); - fprintf(fp, "\t\n"); - for (i = 0; i < ms->nummuscles; i++) - { - dpMuscleStruct* m = ms->muscle[i]; - - // For now, model = 9 and map to Thelen2003Muscle. - // All other values map to Schutte1993Muscle. Eventually, each model - // index should map to a distinct muscle class in OpenSim, - // especially model = 7, which is a ligament model. - if (m->muscle_model_index == NULL || *m->muscle_model_index == 4) - strcpy(muscleClassName, "Schutte1993Muscle"); - else if (*m->muscle_model_index == 9) - strcpy(muscleClassName, "Thelen2003Muscle"); - else if (*m->muscle_model_index == 1 || *m->muscle_model_index == 2) - strcpy(muscleClassName, "Delp1990Muscle"); - else{ // Warn about unsupported but use Schutte1993Muscle - strcpy(muscleClassName, "Schutte1993Muscle"); - printf("Warning: muscle %s has unsupported muscle model %d.\n", m->name, *m->muscle_model_index); - } - write_xml_muscle(fp, ms, m, muscleClassName, 0, angleUnits); - } - fprintf(fp, "\t\n"); - fprintf(fp, "\t\n"); -} - - -static void write_xml_markers(FILE* fp, ModelStruct* ms, const char* markerSetOut) -{ - int i, j; - char tabs[5]; - - if (markerSetOut) { - fp = fopen(markerSetOut, "w"); - fprintf(fp, "\n"); - strcpy(tabs, ""); - } else { - strcpy(tabs, "\t\t\t"); - } - - fprintf(fp, "%s\n", tabs); - fprintf(fp, "%s\t\n", tabs); - - for (i = 0; i < ms->numsegments; i++) - { - SegmentStruct* ss = &ms->segment[i]; - for (j = 0; j < ms->segment[i].numMarkers; j++) - { - fprintf(fp, "%s\t\t\n", tabs, ss->marker[j]->name); - fprintf(fp, "%s\t\t\t %s \n", tabs, ss->name); - fprintf(fp, "%s\t\t\t %.12lf %.12lf %.12lf \n", tabs, ss->marker[j]->offset[0], ss->marker[j]->offset[1], ss->marker[j]->offset[2]); - //fprintf(fp, "%s\t\t\t %.12lf \n", tabs, ss->marker[j]->weight); - fprintf(fp, "%s\t\t\t %s \n", tabs, (ss->marker[j]->fixed == yes) ? ("true") : ("false")); - fprintf(fp, "%s\t\t\n", tabs); - } - } - - fprintf(fp, "%s\t\n", tabs); - fprintf(fp, "%s\n", tabs); - - if(markerSetOut) { - fclose(fp); - printf("Wrote MarkerSet file %s\n", markerSetOut); - } -} - - -static void write_vtk_bone(PolyhedronStruct* bone, char geometryDirectory[], char filename[]) -{ - int i, j, count; - char path[4096]; - FILE* fp; - - build_full_path(geometryDirectory, filename, path); - fp = fopen(path, "w"); - if (fp == NULL) - return; - - fprintf(fp, "\n"); - fprintf(fp, "\n"); - fprintf(fp, "\t\n"); - fprintf(fp, "\t\t\n", - bone->num_vertices, bone->num_polygons); - fprintf(fp, "\t\t\n"); - fprintf(fp, "\t\t\n"); - for (i = 0; i < bone->num_vertices; i++) - fprintf(fp, "\t\t\t%9.6lf %9.6lf %9.6lf\n", bone->vertex[i].normal[0], bone->vertex[i].normal[1], bone->vertex[i].normal[2]); - fprintf(fp, "\t\t\n"); - fprintf(fp, "\t\n"); - fprintf(fp, "\t\n"); - fprintf(fp, "\t\t\n"); - for (i = 0; i < bone->num_vertices; i++) - fprintf(fp, "\t\t\t%9.6lf %9.6lf %9.6lf\n", bone->vertex[i].coord[0], bone->vertex[i].coord[1], bone->vertex[i].coord[2]); - fprintf(fp, "\t\t\n"); - fprintf(fp, "\t\n"); - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\t\n"); - for (i = 0; i < bone->num_polygons; i++) - { - fprintf(fp, "\t\t\t\t\t"); - for (j = 0; j < bone->polygon[i].num_vertices; j++) - fprintf(fp, "%d ", bone->polygon[i].vertex_index[j]); - fprintf(fp, "\n"); - } - fprintf(fp, "\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t"); - for (i = 0, count = 0; i < bone->num_polygons; i++) - { - count += bone->polygon[i].num_vertices; - fprintf(fp, "%d ", count); - } - fprintf(fp, "\n"); - fprintf(fp, "\t\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\n"); - fprintf(fp, "\t\n"); - fprintf(fp, "\n"); - - fclose(fp); -} - -static int segment_has_wrap_objects(ModelStruct* ms, SegmentStruct* seg) -{ - int i; - - for (i = 0; i < ms->num_wrap_objects; i++) - if (&ms->segment[ms->wrapobj[i]->segment] == seg) - return 1; - - return 0; -} - -static void write_xml_wrap_object(FILE* fp, dpWrapObject* wo, int angleUnits) -{ - double xyz[3]; - const char *wrap_quadrant; - double conversion; - - // The angles output by extract_xyz_rot_bodyfixed are in radians, - // so this conversion factor is different than in the other functions. - if (angleUnits == DEGREES) - conversion = RAD_TO_DEG; - else - conversion = 1.0; - - switch (wo->wrap_type) - { - case dpWrapSphere: - fprintf(fp, "\t\t\t\t\t\t\n", wo->name); - fprintf(fp, "\t\t\t\t\t\t\t %.8lf \n", wo->radius[0]); - break; - case dpWrapCylinder: - fprintf(fp, "\t\t\t\t\t\t\n", wo->name); - fprintf(fp, "\t\t\t\t\t\t\t %.8lf \n", wo->radius[0]); - fprintf(fp, "\t\t\t\t\t\t\t %.8lf \n", wo->height); - break; - case dpWrapEllipsoid: - fprintf(fp, "\t\t\t\t\t\t\n", wo->name); - fprintf(fp, "\t\t\t\t\t\t\t %.8lf %.8lf %.8lf \n", - wo->radius[0], wo->radius[1], wo->radius[2]); - break; - case dpWrapTorus: - fprintf(fp, "\t\t\t\t\t\t\n", wo->name); - fprintf(fp, "\t\t\t\t\t\t\t %.8lf \n", wo->radius[0]); - fprintf(fp, "\t\t\t\t\t\t\t %.8lf \n", wo->radius[1]); - break; - } - - fprintf(fp,"\t\t\t\t\t\t\t %s \n", wo->active ? "true" : "false"); - - recalc_xforms(wo); - extract_xyz_rot_bodyfixed(wo->from_local_xform, xyz); - - fprintf(fp, "\t\t\t\t\t\t\t %.8lf %.8lf %.8lf \n", - xyz[0] * conversion, xyz[1] * conversion, xyz[2] * conversion); - - fprintf(fp,"\t\t\t\t\t\t\t %.8lf %.8lf %.8lf \n", - wo->translation.xyz[0], wo->translation.xyz[1], wo->translation.xyz[2]); - - if (wo->wrap_sign != 0) - { - switch ((wo->wrap_axis + 1) * wo->wrap_sign) - { - default: - break; - case 1: wrap_quadrant = "x"; - break; - case -1: wrap_quadrant = "-x"; - break; - case 2: wrap_quadrant = "y"; - break; - case -2: wrap_quadrant = "-y"; - break; - case 3: wrap_quadrant = "z"; - break; - case -3: wrap_quadrant = "-z"; - break; - } - fprintf(fp,"\t\t\t\t\t\t\t %s \n", wrap_quadrant); - } - - switch (wo->wrap_type) - { - case dpWrapSphere: - fprintf(fp, "\t\t\t\t\t\t\n"); - break; - case dpWrapCylinder: - fprintf(fp, "\t\t\t\t\t\t\n"); - break; - case dpWrapEllipsoid: - fprintf(fp, "\t\t\t\t\t\t\n"); - break; - case dpWrapTorus: - fprintf(fp, "\t\t\t\t\t\t\n"); - break; - } -} - -static void write_xml_units(FILE* fp, ModelStruct* ms) -{ - if (ms->forceUnits != NULL) - fprintf(fp, "\t %s \n", ms->forceUnits); - - if (ms->lengthUnits != NULL) - fprintf(fp, "\t %s \n", ms->lengthUnits); -} - -static void write_xml_defaults(FILE* fp, ModelStruct* ms, int angleUnits) -{ - fprintf(fp, "\t\n"); - if (ms && ms->default_muscle) - { - // We need to write a default muscle for all supported types of muscles (since - // the default mechanism works by matching class names) - write_xml_muscle(fp, ms, ms->default_muscle, "Schutte1993Muscle", 1, angleUnits); - write_xml_muscle(fp, ms, ms->default_muscle, "Thelen2003Muscle", 1, angleUnits); - write_xml_muscle(fp, ms, ms->default_muscle, "Delp1990Muscle", 1, angleUnits); - } - fprintf(fp, "\t\n"); -} - -// This function deals with the constant translations and rotations that are in a joint. -// Simbody does not allow them in DOFs, so they are handled as follows: -// 1. The string of constants at the beginning of the DOF list are converted into -// locationInParent and orientationInParent. -// 2. The string of constants at the end of the DOF list are converted into -// locationInChild and orientationInChild. -// 3. The constants in between functions are turned into functions that are -// constrained to remain at the proper value. -static void extract_joint_locations_and_orientations(ModelStruct* ms, - JointStruct* joint, int dof_order[], double locationInParent[], - double orientationInParent[], double locationInChild[], double orientationInChild[]) -{ - int i, first_function=6, last_function=6, order[4]; - double parentTransform[4][4], childTransform[4][4], childInverse[4][4]; - double dof_value[6], x[4], y[4], z[4], ra1[4], ra2[4], ra3[4]; - - for (i=0; i<6; i++) - { - if (joint->dofs[dof_order[i]].type == function_dof) - { - first_function = i; - break; - } - } - - for (i=5; i>=0; i--) - { - if (joint->dofs[dof_order[i]].type == function_dof) - { - last_function = i; - break; - } - } - - // Constants that are in between functions are converted to functions. - // The gencoord used for these converted constants is the - // 'first_function' gencoord. - for (i=first_function+1; idofs[dof_order[i]]; - if (dof->type == constant_dof && NOT_EQUAL_WITHIN_ERROR(dof->value, 0.0)) - convert_dof_to_function(ms, dof, joint->dofs[dof_order[first_function]].gencoord); - } - - // Clear the dof_value array so you can fill in only - // the values you care about. - for (i=0; i<6; i++) - dof_value[i] = 0.0; - - // Constants at the beginning go into the parent matrix. - for (i=0; idofs[dof_order[i]].value; - - //TODO2.0: reverse axes for INVERSE, or maybe just negate dof value? - - /* initialize the [parent] x, y, and z axes */ - COPY_1X4VECTOR(gWorld_x,joint->parentframe[XX]); - COPY_1X4VECTOR(gWorld_y,joint->parentframe[YY]); - COPY_1X4VECTOR(gWorld_z,joint->parentframe[ZZ]); - order[TRANS] = joint->order[TRANS]+1; - order[ROT1] = joint->order[ROT1]+1; - order[ROT2] = joint->order[ROT2]+1; - order[ROT3] = joint->order[ROT3]+1; - COPY_1X4VECTOR(joint->parentframe[XX],x); - COPY_1X4VECTOR(joint->parentframe[YY],y); - COPY_1X4VECTOR(joint->parentframe[ZZ],z); - COPY_1X4VECTOR(joint->parentrotaxes[R1],ra1); - normalize_vector(ra1, ra1); - COPY_1X4VECTOR(joint->parentrotaxes[R2],ra2); - normalize_vector(ra2, ra2); - COPY_1X4VECTOR(joint->parentrotaxes[R3],ra3); - normalize_vector(ra3, ra3); - - calc_joint_transform(order, dof_value, parentTransform, - x, y, z, ra1, ra2, ra3, BF, FORWARD, joint); - - // Clear the dof_value array so you can fill in only - // the values you care about. - for (i=0; i<6; i++) - dof_value[i] = 0.0; - - // Constants at the end go into the child matrix. - for (i=last_function+1; i<6; i++) - dof_value[dof_order[i]] = joint->dofs[dof_order[i]].value; - - // Reset the axes - COPY_1X4VECTOR(joint->parentframe[XX],x); - COPY_1X4VECTOR(joint->parentframe[YY],y); - COPY_1X4VECTOR(joint->parentframe[ZZ],z); - COPY_1X4VECTOR(joint->parentrotaxes[R1],ra1); - normalize_vector(ra1, ra1); - COPY_1X4VECTOR(joint->parentrotaxes[R2],ra2); - normalize_vector(ra2, ra2); - COPY_1X4VECTOR(joint->parentrotaxes[R3],ra3); - normalize_vector(ra3, ra3); - - // Calculate the forward transform for the DOFs after the - // last function, and then invert it. - calc_joint_transform(order, dof_value, childTransform, - x, y, z, ra1, ra2, ra3, BF, FORWARD, joint); - invert_4x4matrix(childTransform, childInverse); - - // Extract the translations from the matrices. - for (i=0; i<3; i++) - { - locationInParent[i] = parentTransform[3][i]; - locationInChild[i] = childTransform[3][i]; - } - - // Extract the rotations from the matrices. - extract_xyz_rot_bodyfixed(parentTransform, orientationInParent); - extract_xyz_rot_bodyfixed(childInverse, orientationInChild); -} - -static ReturnCode write_simbody_engine(FILE* fp, ModelStruct* ms, char geometryDirectory[], const char* markerSetOut, int angleUnits) -{ - // Gravity - write_xml_gravity(fp, ms); - - // Bodies (with joints, wrap objects), constraints - if (make_simbody_model(fp, ms, geometryDirectory, angleUnits) == code_bad) - return code_bad; - - // Markers - write_xml_markers(fp, ms, markerSetOut); - - return code_fine; -} - -static void write_opensim_function(FILE* fp, dpFunction* function, double ind_conv, double dep_conv, int tab_level) -{ - int i; - char tabs[CHARBUFFER]; - - for (i=0; iy[1] - function->y[0]) / (function->x[1] - function->x[0]); - double b = function->y[0] - function->x[0] * m; - fprintf(fp, "%s\n", tabs); - fprintf(fp, "%s\t %lf %lf \n", tabs, m, b); - fprintf(fp, "%s\n", tabs); - return; - } - - if (function->type == dpNaturalCubicSpline || function->type == dpFunctionTypeUndefined) - fprintf(fp, "%s\n", tabs); - else if (function->type == dpStepFunction) - fprintf(fp, "%s\n", tabs); - else if (function->type == dpLinearFunction) - fprintf(fp, "%s\n", tabs); - else if (function->type == dpGCVSpline) - { - fprintf(fp, "%s\n", tabs); - fprintf(fp, "%s\t 3 \n", tabs); - } - - fprintf(fp, "%s\t ", tabs); - for (i=0; inumpoints; i++) - fprintf(fp, "%.12lf ", function->x[i] * ind_conv); - fprintf(fp, "\n"); - - fprintf(fp, "%s\t ", tabs); - for (i=0; inumpoints; i++) - fprintf(fp, "%.12lf ", function->y[i] * dep_conv); - fprintf(fp, "\n"); - - if (function->type == dpNaturalCubicSpline || function->type == dpFunctionTypeUndefined) - fprintf(fp, "%s\n", tabs); - else if (function->type == dpStepFunction) - fprintf(fp, "%s\n", tabs); - else if (function->type == dpLinearFunction) - fprintf(fp, "%s\n", tabs); - else if (function->type == dpGCVSpline) - fprintf(fp, "%s\n", tabs); -} - -static void write_opensim_constant_function(FILE* fp, double value, int tab_level) -{ - int i; - char tabs[CHARBUFFER]; - - for (i=0; i\n", tabs); - fprintf(fp, "%s\t %lf \n", tabs, value); - fprintf(fp, "%s\n", tabs); -} - -static SBoolean function_is_simple_linear(dpFunction* function) -{ - if (function->numpoints == 2 && EQUAL_WITHIN_ERROR(function->x[0], function->y[0])) - { - double slope = (function->y[1] - function->y[0]) / (function->x[1] - function->x[0]); - if (EQUAL_WITHIN_ERROR(slope, 1.0)) - return yes; - } - - return no; -} - -static ReturnCode make_simbody_model(FILE* fp, ModelStruct* ms, char geometryDirectory[], int angleUnits) -{ - int i, j, dofcount, constrainedcount, *joint_order; - JointSDF* jnts; - SegmentSDF* segs; - GencoordInfo* gcInfo; - - joint_order = (int*)malloc(ms->numjoints*sizeof(int)); - for (i = 0; i < ms->numjoints; i++) - joint_order[i] = -1; - jnts = (JointSDF*)simm_calloc(ms->numjoints, sizeof(JointSDF)); - segs = (SegmentSDF*)simm_calloc(ms->numsegments, sizeof(SegmentSDF)); - // Make extra room for gcInfo because when fixed joints are converted, a new - // gencoord is added to the model. - gcInfo = (GencoordInfo*)simm_calloc(ms->numgencoords + ms->numjoints, sizeof(GencoordInfo)); - - // Make the joint transformation matrices once you have changed the gencoords. - for (i=0; inumjoints; i++) - make_conversion(ms, i); - - // Re-initialize the SD/FAST variables. Mark all dofs as - // constrained (not true degrees of freedom). - // Then for each gencoord, find the ONE dof which best - // corresponds to the gencoord and mark it unconstrained. - // That is, of all the dofs that are functions of a particular - // gencoord, one should be a direct mapping (between dof and - // gencoord) and the others should be non-trivial functions. - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - ms->joint[i].dofs[j].sd.name = NULL; - ms->joint[i].dofs[j].sd.con_name = NULL; - ms->joint[i].dofs[j].sd.initial_value = 0.0; - ms->joint[i].dofs[j].sd.constrained = yes; - ms->joint[i].dofs[j].sd.fixed = no; - ms->joint[i].dofs[j].sd.state_number = -1; - ms->joint[i].dofs[j].sd.error_number = -1; - ms->joint[i].dofs[j].sd.joint = -1; - ms->joint[i].dofs[j].sd.axis = -1; - ms->joint[i].dofs[j].sd.conversion = 0.0; - ms->joint[i].dofs[j].sd.conversion_sign = 1.0; - } - } - - // Give the joints one-token names. In most cases, this is just - // the user-given name of the joint. However, if that name has special - // characters in it (e.g. -), those characters must be removed. Also, - // if the name starts with a number, then an underscore is prepended. - // Set the type to dpUnknownJoint; the type is not needed by the Simbody - // exporter, except that it can't be dpSkippable. - for (i = 0; i < ms->numjoints; i++) - { - JointStruct* jnt = &ms->joint[i]; - FREE_IFNOTNULL(jnt->sd_name); - jnt->sd_name = (char*)simm_malloc(strlen(jnt->name) + 2); - strcpy(jnt->sd_name, jnt->name); - convert_string(jnt->sd_name, yes); - ms->joint[i].type = dpUnknownJoint; - } - - for (i=0; inumgencoords; i++) - { - if (ms->gencoord[i]->used_in_model == yes) - { - if (!mark_unconstrained_dof(ms, ms->gencoord[i], &gcInfo[i].jointnum, &gcInfo[i].dofnum, &gcInfo[i].constrained)) - { -#if 0 - sprintf(errorbuffer, "At least one DOF must be a \"simple\" function of gencoord %s (2 points, slope=1, passes thru zero).", - ms->gencoord[i]->name); - error(none,errorbuffer); - return code_bad; -#endif - } - } - } - - // Now give the dofs names for use in the OpenSim model file. - // Names of unconstrained dofs will simply be the names of - // the gencoords to which they correspond. Names of - // constrained dofs will be formed from the joint name - // and the dof keyword (e.g. "hip_tx" and "knee_r1"). - name_dofs(ms); - - find_sdfast_joint_order(ms, jnts, segs, joint_order, SIMBODY_GROUND); - - // Malloc space for the array of segment names. These names include $ground - // and the names of the "split" body segments. There can be at most - // (numjoints + 1) segment names. - // Free the SDseg array, if it was used previously. - if (SDseg) - { - for (i = 0; i < num_SD_segs; i++) - FREE_IFNOTNULL(SDseg[i].name); - FREE_IFNOTNULL(SDseg); - } - - SDseg = (SDSegment*)simm_calloc(ms->numjoints + 1, sizeof(SDSegment)); - SDseg[0].name = (char*)simm_malloc(strlen(ms->segment[ms->ground_segment].name) + 2); - strcpy(SDseg[0].name, ms->segment[ms->ground_segment].name); - convert_string(SDseg[0].name, yes); - SDseg[0].simm_segment = ms->ground_segment; - SDseg[0].mass_center[0] = ms->segment[ms->ground_segment].masscenter[0]; - SDseg[0].mass_center[1] = ms->segment[ms->ground_segment].masscenter[1]; - SDseg[0].mass_center[2] = ms->segment[ms->ground_segment].masscenter[2]; - SDseg[0].mass = 0.0; - for (i = 0; i < 3; i++) - for (j = 0; j < 3; j++) - SDseg[0].inertia[i][j] = 0.0; - - num_SD_segs = 1; - - //convert_fixed_joints(ms, gcInfo); - - // Bodies (with joints and wrap objects) - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - write_opensim_ground_body(fp, ms, &ms->segment[ms->ground_segment], geometryDirectory, angleUnits); - for (i=0, dofcount=0, constrainedcount=0; inumjoints; i++) - { - make_simbody_joint(ms, fp, &jnts[joint_order[i]], joint_order[i], ms->joint[i].sd_num, segs, gcInfo, - &dofcount, &constrainedcount, geometryDirectory, angleUnits); - } - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - - // Constraints are used when the DOF is constrained to a coordinate - // that is used in another joint. - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - for (i = 0; i < ms->numjoints; i++) - { - JointStruct* joint = &ms->joint[joint_order[i]]; - for (j = 0; j < 6; j++) - { - if (joint->dofs[j].sd.constrained == yes && joint->dofs[j].gencoord && - gcInfo[getGencoordIndex(ms, joint->dofs[j].gencoord)].jointnum != joint_order[i]) - write_opensim_constraint(fp, ms, &joint->dofs[j], j, angleUnits); - } - } - for (i=0; inumsegments; i++) - { - for (j=0; jsegment[i].name, j+1); - fprintf(fp, "\t\t\t\t\n", ms->segment[i].name, new_name); - fprintf(fp, "\t\t\t\t\t false \n"); - fprintf(fp, "\t\t\t\t\t %s \n", ms->segment[i].name); - fprintf(fp, "\t\t\t\t\t %s \n", new_name); - fprintf(fp, "\t\t\t\t\t 0.0 0.0 0.0 \n"); - fprintf(fp, "\t\t\t\t\t 0.0 0.0 0.0 \n"); - fprintf(fp, "\t\t\t\t\t 0.0 0.0 0.0 \n"); - fprintf(fp, "\t\t\t\t\t 0.0 0.0 0.0 \n"); - fprintf(fp, "\t\t\t\t\n"); - FREE_IFNOTNULL(new_name); - } - } - fprintf(fp, "\t\t\t\n"); - fprintf(fp, "\t\t\t\n"); - - free(joint_order); - for (i=0; inumjoints; i++) - { - FREE_IFNOTNULL(jnts[i].inbname); - FREE_IFNOTNULL(jnts[i].outbname); - } - free(jnts); - free(segs); - free(gcInfo); - - return code_fine; -} - - -static void make_simbody_joint(ModelStruct* ms, FILE* fp, JointSDF* jntsdf, int jointnum, int sdnum, - SegmentSDF segs[], GencoordInfo gcInfo[], int* dofcount, int* constrainedcount, - char geometryDirectory[], int angleUnits) -{ - int i, j, dof_order[6], current_dof = 0; - SegmentStruct* seg1; - SegmentStruct* seg2; - SegmentSDF* segsdf; - JointStruct* joint; - double locationInParent[3], locationInChild[3], orientationInParent[3], orientationInChild[3]; - - joint = &ms->joint[jointnum]; - - if (jntsdf->dir == FORWARD) - { - seg1 = &ms->segment[joint->from]; - seg2 = &ms->segment[joint->to]; - segsdf = &segs[joint->to]; - } - else - { - seg1 = &ms->segment[joint->to]; - seg2 = &ms->segment[joint->from]; - segsdf = &segs[joint->from]; - } - - // Add the body name to the list of segment names. This list contains 'real' - // segment names as well as the names of 'split' body segments. - mstrcpy(&(SDseg[num_SD_segs].name),jntsdf->outbname); - - // If there are loops in the model, then SIMM segments get split and there - // will be more SD segments than SIMM segments. So that unsplittable segment - // parameters (like contact objects) can be assigned to the SD segments, - // each SD segment has an index of its corresponding SIMM segment. But for - // segments that were split, only one piece will have a valid index. - if (jntsdf->closes_loop == no) - { - if (jntsdf->dir == FORWARD) - SDseg[num_SD_segs].simm_segment = joint->to; - else - SDseg[num_SD_segs].simm_segment = joint->from; - } - else - { - SDseg[num_SD_segs].simm_segment = -1; - } - - SDseg[num_SD_segs].mass = seg2->mass / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[0][0] = seg2->inertia[0][0] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[0][1] = seg2->inertia[0][1] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[0][2] = seg2->inertia[0][2] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[1][0] = seg2->inertia[1][0] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[1][1] = seg2->inertia[1][1] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[1][2] = seg2->inertia[1][2] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[2][0] = seg2->inertia[2][0] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[2][1] = seg2->inertia[2][1] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[2][2] = seg2->inertia[2][2] / segsdf->mass_factor; - SDseg[num_SD_segs].mass_center[0] = seg2->masscenter[0]; - SDseg[num_SD_segs].mass_center[1] = seg2->masscenter[1]; - SDseg[num_SD_segs].mass_center[2] = seg2->masscenter[2]; - - fprintf(fp, "\t\t\t\t\n", jntsdf->outbname); - fprintf(fp, "\t\t\t\t\t %.12lf \n", SDseg[num_SD_segs].mass); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", SDseg[num_SD_segs].mass_center[0], SDseg[num_SD_segs].mass_center[1], SDseg[num_SD_segs].mass_center[2]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", SDseg[num_SD_segs].inertia[0][0]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", SDseg[num_SD_segs].inertia[1][1]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", SDseg[num_SD_segs].inertia[2][2]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", SDseg[num_SD_segs].inertia[0][1]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", SDseg[num_SD_segs].inertia[0][2]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", SDseg[num_SD_segs].inertia[1][2]); - - // Figure out in what order the 6 DOFs should be processed. - dof_order[joint->order[TRANS]] = TX; - dof_order[joint->order[TRANS]+1] = TY; - dof_order[joint->order[TRANS]+2] = TZ; - if (joint->order[ROT1] < joint->order[TRANS]) - dof_order[joint->order[ROT1]] = R1; - else - dof_order[joint->order[ROT1]+2] = R1; - if (joint->order[ROT2] < joint->order[TRANS]) - dof_order[joint->order[ROT2]] = R2; - else - dof_order[joint->order[ROT2]+2] = R2; - if (joint->order[ROT3] < joint->order[TRANS]) - dof_order[joint->order[ROT3]] = R3; - else - dof_order[joint->order[ROT3]+2] = R3; - - if (jntsdf->dir == INVERSE) - { - int rev_dof_order[6]; - - for (i=0; i<6; i++) - rev_dof_order[i] = dof_order[5-i]; - extract_joint_locations_and_orientations(ms, joint, rev_dof_order, locationInParent, orientationInParent, - locationInChild, orientationInChild); - } - else - { - extract_joint_locations_and_orientations(ms, joint, dof_order, locationInParent, orientationInParent, - locationInChild, orientationInChild); - } - - fprintf(fp, "\t\t\t\t\t\n"); - - if (joint_is_fixed(joint)) - { - fprintf(fp, "\t\t\t\t\t\t\n", joint->name); - fprintf(fp, "\t\t\t\t\t\t\t %s \n", jntsdf->inbname); - fprintf(fp, "\t\t\t\t\t\t\t %.12lf %.12lf %.12lf \n", - locationInParent[0], locationInParent[1], locationInParent[2]); - fprintf(fp, "\t\t\t\t\t\t\t %.12lf %.12lf %.12lf \n", - orientationInParent[0], orientationInParent[1], orientationInParent[2]); - fprintf(fp, "\t\t\t\t\t\t\t %.12lf %.12lf %.12lf \n", - locationInChild[0], locationInChild[1], locationInChild[2]); - fprintf(fp, "\t\t\t\t\t\t\t %.12lf %.12lf %.12lf \n", - orientationInChild[0], orientationInChild[1], orientationInChild[2]); - fprintf(fp, "\t\t\t\t\t\t\n"); - } else { - fprintf(fp, "\t\t\t\t\n", joint->name); - fprintf(fp, "\t\t\t\t\t %s \n", jntsdf->dir == FORWARD ? "false" : "true"); - fprintf(fp, "\t\t\t\t\t %s \n", jntsdf->inbname); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", - locationInParent[0], locationInParent[1], locationInParent[2]); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", - orientationInParent[0], orientationInParent[1], orientationInParent[2]); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", - locationInChild[0], locationInChild[1], locationInChild[2]); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", - orientationInChild[0], orientationInChild[1], orientationInChild[2]); - - fprintf(fp, "\t\t\t\t\t\n"); - - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\n"); - // The coordinates must be listed in the same order that they are referenced in the transform axis list. - // First do the rotations. - for (i=0; i<6; i++) - { - if (dof_order[i] < 3) - { - DofStruct* dof = &joint->dofs[dof_order[i]]; - // This DOF shows up in the coordinate list if it represents an unconstrained coordinate, - // or if it's constrained to a coordinate that is used in another joint. Or if it is one - // of the newly created gencoords. - if (dof->type == function_dof) - { - int index = getGencoordIndex(ms, dof->gencoord); - if ((gcInfo[index].jointnum == jointnum && gcInfo[index].dofnum == dof_order[i]) || - gcInfo[index].jointnum != jointnum || ms->gencoord[index]->defined == no) - write_opensim_coordinate(fp, ms, joint, dof_order[i], angleUnits); - } - } - } - // Now do the translations. - for (i=0; i<6; i++) - { - if (dof_order[i] >= 3) - { - DofStruct* dof = &joint->dofs[dof_order[i]]; - // This DOF shows up in the coordinate list if it represents an unconstrained coordinate, - // or if it's constrained to a coordinate that is used in another joint. Or if it is one - // of the newly created gencoords. - if (dof->type == function_dof) - { - int index = getGencoordIndex(ms, dof->gencoord); - if ((gcInfo[index].jointnum == jointnum && gcInfo[index].dofnum == dof_order[i]) || - gcInfo[index].jointnum != jointnum || ms->gencoord[index]->defined == no) - write_opensim_coordinate(fp, ms, joint, dof_order[i], angleUnits); - } - } - } - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\n"); - - // In an OpenSim CustomJoint, the SpatialTransform always - // has 6 transform axes (3 rotation, 3 translation). The - // 3 rotations are always specified first, and the 3 translations - // are all w.r.t. the parent reference frame. - fprintf(fp, "\t\t\t\t\t\t\t\n"); - // First output the rotations. - for (i=0; i<6; i++) - { - if (dof_order[i] < 3) - { - DofStruct* dof = &joint->dofs[dof_order[i]]; - // DOFs that are functions always show up in the transform axis list. If the DOF represents - // an unconstrained coordinate or if it's constrained to a coordinate in another joint, - // then it shows up without a function. If the DOF is constrained to a coordinate in this - // joint, it shows up with a function (i.e., it's a function-based mobilizer). - if (dof->type == function_dof) - { - int index = getGencoordIndex(ms, dof->gencoord); - if (/*(gcInfo[index].jointnum == jointnum && gcInfo[index].dofnum == dof_order[i] && gcInfo[index].constrained == no) || */ - gcInfo[index].jointnum != jointnum) - write_opensim_transformAxis(fp, ms, joint, dof_order[i], FORWARD, no, current_dof++, angleUnits); - else - write_opensim_transformAxis(fp, ms, joint, dof_order[i], FORWARD, yes, current_dof++, angleUnits); - } - else - { - // Constant rotational DOFs still need to be added to the SpatialTransform, but with values of - // zero because they have already been added to either orientation_in_parent or orientation. - double saved_value = dof->value; - dof->value = 0.0; - write_opensim_transformAxis(fp, ms, joint, dof_order[i], FORWARD, yes, current_dof++, angleUnits); - dof->value = saved_value; - } - } - } - // Now output the translations. - for (i=0; i<6; i++) - { - if (dof_order[i] >= 3) - { - DofStruct* dof = &joint->dofs[dof_order[i]]; - // DOFs that are functions always show up in the transform axis list. If the DOF represents - // an unconstrained coordinate or if it's constrained to a coordinate in another joint, - // then it shows up without a function. If the DOF is constrained to a coordinate in this - // joint, it shows up with a function (i.e., it's a function-based mobilizer). - if (dof->type == function_dof) - { - int index = getGencoordIndex(ms, dof->gencoord); - if (/*(gcInfo[index].jointnum == jointnum && gcInfo[index].dofnum == dof_order[i] && gcInfo[index].constrained == no) || */ - gcInfo[index].jointnum != jointnum) - write_opensim_transformAxis(fp, ms, joint, dof_order[i], FORWARD, no, current_dof++, angleUnits); - else - write_opensim_transformAxis(fp, ms, joint, dof_order[i], FORWARD, yes, current_dof++, angleUnits); - } - else - { - // Constant translational DOFs still need to be added to the SpatialTransform, but with values of - // zero because they have already been added to either location_in_parent or location. - double saved_value = dof->value; - dof->value = 0.0; - write_opensim_transformAxis(fp, ms, joint, dof_order[i], FORWARD, yes, current_dof++, angleUnits); - dof->value = saved_value; - } - } - } - fprintf(fp, "\t\t\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\t\n"); - } - fprintf(fp, "\t\t\t\t\t\n"); - - num_SD_segs++; - - // Don't output bones, wrap object, etc., for the "split" segments. - if (jntsdf->closes_loop == no) - { - if (seg2->numBones > 0) - { - char vtpFileName[CHARBUFFER]; - SBoolean madeDir = no; - - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\t "); - for (j = 0; j < seg2->numBones; j++) - { - change_filename_suffix(seg2->bone[j].name, vtpFileName, "vtp", CHARBUFFER); - fprintf(fp, "%s ", vtpFileName); - // Only write the bone file if the PolyhedronStruct is not empty - if (seg2->bone[j].num_vertices > 0 && seg2->bone[j].num_polygons > 0) - { - // The first time you write a bone file, check to see if the - // output directory needs to be created. - if (madeDir == no) - { - if (geometryDirectory) - { - if (makeDir(geometryDirectory) != 0 && errno != EEXIST) - { - printf("Warning: Unable to create geometry directory %s.\n", geometryDirectory); - printf(" VTK geometry files will not be output for this model.\n"); - } - } - // Whether or not you successfully created the geometry directory, - // you don't want to try to create it again. - madeDir = yes; - } - write_vtk_bone(&seg2->bone[j], geometryDirectory, vtpFileName); - } - } - fprintf(fp, "\n"); - fprintf(fp, "\t\t\t\t\t\t %.12f %.12f %.12f \n", seg2->bone_scale[0], seg2->bone_scale[1], seg2->bone_scale[2]); - fprintf(fp, "\t\t\t\t\t\n"); - } - if (segment_has_wrap_objects(ms, seg2)) - { - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\n"); - for (j = 0; j < ms->num_wrap_objects; j++) - { - if (&ms->segment[ms->wrapobj[j]->segment] == seg2) - write_xml_wrap_object(fp, ms->wrapobj[j], angleUnits); - } - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\n"); - } - } - - fprintf(fp, "\t\t\t\t\n"); -} - -static void write_opensim_ground_body(FILE* fp, ModelStruct* ms, SegmentStruct* seg, char geometryDirectory[], int angleUnits) -{ - int j; - SBoolean madeDir = no; - - fprintf(fp, "\t\t\t\t\n", seg->name); - fprintf(fp, "\t\t\t\t\t %.12lf \n", seg->mass); - fprintf(fp, "\t\t\t\t\t %.12lf %.12lf %.12lf \n", seg->masscenter[0], seg->masscenter[1], seg->masscenter[2]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", seg->inertia[0][0]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", seg->inertia[1][1]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", seg->inertia[2][2]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", seg->inertia[0][1]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", seg->inertia[0][2]); - fprintf(fp, "\t\t\t\t\t %.12lf \n", seg->inertia[1][2]); - fprintf(fp, "\t\t\t\t\t\n"); - - // The ground body does not have a joint, but it can have bones and wrap objects. - if (seg->numBones > 0) - { - char vtpFileName[CHARBUFFER]; - - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\t "); - for (j = 0; j < seg->numBones; j++) - { - change_filename_suffix(seg->bone[j].name, vtpFileName, "vtp", CHARBUFFER); - fprintf(fp, "%s ", vtpFileName); - // Only write the bone file if the PolyhedronStruct is not empty - if (seg->bone[j].num_vertices > 0 && seg->bone[j].num_polygons > 0) - { - // The first time you write a bone file, check to see if the - // output directory needs to be created. - if (madeDir == no) - { - if (geometryDirectory) - { - if (makeDir(geometryDirectory) != 0 && errno != EEXIST) - { - printf("Warning: Unable to create geometry directory %s.\n", geometryDirectory); - printf(" VTK geometry files will not be output for this model.\n"); - } - } - // Whether or not you successfully created the geometry directory, - // you don't want to try to create it again. - madeDir = yes; - } - write_vtk_bone(&seg->bone[j], geometryDirectory, vtpFileName); - } - } - fprintf(fp, "\n"); - fprintf(fp, "\t\t\t\t\t\t %.12f %.12f %.12f \n", seg->bone_scale[0], seg->bone_scale[1], seg->bone_scale[2]); - fprintf(fp, "\t\t\t\t\t\n"); - } - if (segment_has_wrap_objects(ms, seg)) - { - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\n"); - for (j = 0; j < ms->num_wrap_objects; j++) - { - if (&ms->segment[ms->wrapobj[j]->segment] == seg) - write_xml_wrap_object(fp, ms->wrapobj[j], angleUnits); - } - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\t\n"); - } - fprintf(fp, "\t\t\t\t\n"); -} - -static void write_opensim_coordinate(FILE* fp, ModelStruct* ms, JointStruct* joint, int dofIndex, int angleUnits) -{ - DofStruct* dof = &joint->dofs[dofIndex]; - GeneralizedCoord* gc = dof->gencoord; - double conversion = 1.0; - - fprintf(fp, "\t\t\t\t\t\t\t\t\n", dof->sd.name); - if (dof->sd.constrained == no) - { - // If the gencoord is [primarily] translational, then do not convert its - // range, default_value, etc. If it's rotational, then use DEG_TO_RAD. - if (gc->type == rotation_gencoord && angleUnits == RADIANS) - conversion = DEG_TO_RAD; - - fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf \n", gc->default_value * conversion); - fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf \n", gc->default_value * conversion); - //if (NOT_EQUAL_WITHIN_ERROR(gc->tolerance, 0.0)) - // fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf \n", gc->tolerance * conversion); - //if (NOT_EQUAL_WITHIN_ERROR(gc->pd_stiffness, 0.0)) - // fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf \n", gc->pd_stiffness); - fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf %.12lf \n", gc->range.start * conversion, gc->range.end * conversion); - if (gc->keys[0] != null_key) - { - fprintf(fp, "\t\t\t\t\t\t\t\t\t %s ", get_simmkey_name((int)gc->keys[0])); - if (gc->keys[1] != gc->keys[0]) - fprintf(fp, "%s ", get_simmkey_name((int)gc->keys[1])); - fprintf(fp,"\n"); - } - fprintf(fp, "\t\t\t\t\t\t\t\t\t %s \n", (gc->clamped == yes) ? ("true") : ("false")); - fprintf(fp, "\t\t\t\t\t\t\t\t\t %s \n", (gc->locked == yes) ? ("true") : ("false")); - } else { // dof->sd.constrained = yes - if (angleUnits == RADIANS) - { - // check if dof (dependent coordinate) is r1, r2, or r3 - if (dofIndex < 3) - conversion = DEG_TO_RAD; - } - fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf \n", dof->value * conversion); - fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf \n", dof->value * conversion); - fprintf(fp, "\t\t\t\t\t\t\t\t\t -99999.9 99999.9 \n"); //TODO20: maybe not specify, so default is used? - fprintf(fp, "\t\t\t\t\t\t\t\t\t false \n"); - fprintf(fp, "\t\t\t\t\t\t\t\t\t false \n"); - } - fprintf(fp, "\t\t\t\t\t\t\t\t\n"); -} - -static void write_opensim_transformAxis(FILE* fp, ModelStruct* ms, JointStruct* joint, int dofIndex, - Direction dir, SBoolean writeFunction, int current_dof, int angleUnits) -{ - DofStruct* dof = &joint->dofs[dofIndex]; - GeneralizedCoord* gc = dof->gencoord; - - if (current_dof >= 0 && current_dof <= 2) - fprintf(fp, "\t\t\t\t\t\t\t\t\n", current_dof+1); - else - fprintf(fp, "\t\t\t\t\t\t\t\t\n", current_dof-2); - //fprintf(fp, "\t\t\t\t\t\t\t\t\t %s \n", (dofIndex > 2) ? ("false") : ("true")); - if (dir == FORWARD) - { - if (dofIndex == TX) - fprintf(fp, "\t\t\t\t\t\t\t\t\t 1.0 0.0 0.0 \n"); - else if (dofIndex == TY) - fprintf(fp, "\t\t\t\t\t\t\t\t\t 0.0 1.0 0.0 \n"); - else if (dofIndex == TZ) - fprintf(fp, "\t\t\t\t\t\t\t\t\t 0.0 0.0 1.0 \n"); - else - { - double axis[4]; - COPY_1X4VECTOR(joint->parentrotaxes[dofIndex], axis); - normalize_vector(axis, axis); - fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf %.12lf %.12lf \n", axis[0], axis[1], axis[2]); - } - } - else - { - if (dofIndex == TX) - fprintf(fp, "\t\t\t\t\t\t\t\t\t -1.0 0.0 0.0 \n"); - else if (dofIndex == TY) - fprintf(fp, "\t\t\t\t\t\t\t\t\t 0.0 -1.0 0.0 \n"); - else if (dofIndex == TZ) - fprintf(fp, "\t\t\t\t\t\t\t\t\t 0.0 0.0 -1.0 \n"); - else - { - double axis[4]; - COPY_1X4VECTOR(joint->parentrotaxes[dofIndex], axis); - normalize_vector(axis, axis); - fprintf(fp, "\t\t\t\t\t\t\t\t\t %.12lf %.12lf %.12lf \n", -axis[0], -axis[1], -axis[2]); - } - } - if (writeFunction == yes) - { - dpFunction* sf = dof->function; - double ind_conv = 1.0, dep_conv = 1.0; - - if (angleUnits == RADIANS) - { - // check if dof (dependent coordinate) is r1, r2, or r3 - if (dofIndex < 3) - dep_conv = DEG_TO_RAD; - } - if (sf) - { - // check if gencoord (independent coordinate) is rotational - if (dof->gencoord->type == rotation_gencoord) - ind_conv = DEG_TO_RAD; - fprintf(fp, "\t\t\t\t\t\t\t\t\t\n", sf->usernum); - write_opensim_function(fp, sf, ind_conv, dep_conv, 10); - fprintf(fp, "\t\t\t\t\t\t\t\t\t\n"); - // If you are writing the function, the 'coordinate' is the independent coordinte in - // the function. - fprintf(fp, "\t\t\t\t\t\t\t\t\t %s \n", dof->gencoord->name); - } - else - { - fprintf(fp, "\t\t\t\t\t\t\t\t\t\n"); - write_opensim_constant_function(fp, dof->value * dep_conv, 10); - fprintf(fp, "\t\t\t\t\t\t\t\t\t\n"); - } - } - else - { - // If you're not writing the function, then this transform axis is its own - // coordinate (either unconstrained, or constrained to a coordinate in - // another joint with a CoordinateCoupler. - fprintf(fp, "\t\t\t\t\t\t\t\t\t %s \n", dof->sd.name); - } - fprintf(fp, "\t\t\t\t\t\t\t\t\n"); -} - -static void write_opensim_constraint(FILE* fp, ModelStruct* ms, DofStruct* dof, int dofIndex, int angleUnits) -{ - if (dof->gencoord >= 0 && dof->function != NULL) - { - dpFunction* func = dof->function; - double ind_conv = 1.0, dep_conv = 1.0; - - if (angleUnits == RADIANS) - { - // check if gencoord (independent coordinate) is rotational - if (dof->gencoord->type == rotation_gencoord) - ind_conv = DEG_TO_RAD; - // check if dof (dependent coordinate) is r1, r2, or r3 - if (dofIndex < 3) - dep_conv = DEG_TO_RAD; - } - - fprintf(fp, "\t\t\t\t\n", dof->sd.con_name); - fprintf(fp, "\t\t\t\t\t false \n"); - fprintf(fp, "\t\t\t\t\t %s \n", dof->sd.name); - fprintf(fp, "\t\t\t\t\t %s \n", dof->gencoord->name); - fprintf(fp, "\t\t\t\t\t\n", func->usernum); - write_opensim_function(fp, func, ind_conv, dep_conv, 6); - fprintf(fp, "\t\t\t\t\t\n"); - fprintf(fp, "\t\t\t\t\n"); - } -} - -// The current version of Simbody cannot handle joints with no degrees of freedom. -// For now, deal with them by making one DOF a constrained function of a gencoord. -// The DOF chosen is the last one in the joint, so that in case the others are -// non-zero they can be more easily converted into the and -// parameters. A new gencoord is created for this constraint, -// so that it can be implemented as a function-based mobilizer in Simbody. -static void convert_fixed_joints(ModelStruct* ms, GencoordInfo gcInfo[]) -{ - int i; - - for (i=0; inumjoints; i++) - { - JointStruct* joint = &ms->joint[i]; - if (joint_is_fixed(joint)) - { - int dofnum; - int index = getGencoordIndex(ms, joint->dofs[dofnum].gencoord); - if (joint->order[TRANS] == 3) - dofnum = TX; - else if (joint->order[ROT1] == 3) - dofnum = R1; - else if (joint->order[ROT2] == 3) - dofnum = R2; - else - dofnum = R3; - convert_dof_to_function(ms, &joint->dofs[dofnum], NULL); - // Fill in gcInfo with values that will cause this constraint - // to be implemented as a function-based mobilizer. That is, - // specify that the gencoord used for this new function is - // "based" in this joint, but in another DOF. - gcInfo[index].jointnum = i; - gcInfo[index].dofnum = 5 - dofnum; - } - } -} - -static dpFunction* add_constant_function_to_model(ModelStruct* ms, double y_value) -{ - dpFunction f; - - malloc_function(&f, 2); - - f.type = dpLinearFunction; - f.cutoff_frequency = -1.0; - f.numpoints = 2; - f.x[0] = -999999.9999999; - f.x[1] = 999999.9999999; - f.y[0] = f.y[1] = y_value; - - calc_function_coefficients(&f); - - return load_simm_function(ms, &f, no); -} - -void convert_dof_to_function(ModelStruct* ms, DofStruct* dof, GeneralizedCoord* gencoord) -{ - dof->type = function_dof; - dof->function = add_constant_function_to_model(ms, dof->value); - if (gencoord == NULL) - { - // Create a new gencoord for use in this function. The range and - // other parameters of this gencoord are not specified, but they - // are not needed for writing out an OpenSim model because the - // OpenSim coordinate is constrained anyway. - dof->gencoord = enter_gencoord(ms, dof->sd.name, yes); - } - else - { - dof->gencoord = gencoord; - } - dof->sd.initial_value = dof->value; - dof->sd.constrained = yes; - dof->sd.fixed = no; -} - -static SBoolean joint_is_fixed(JointStruct* joint) -{ - int i; - for (i=0; i<6; i++) - if (joint->dofs[i].type == function_dof) - return no; - return yes; -} diff --git a/OpenSim/Utilities/simmToOpenSim/extern.h b/OpenSim/Utilities/simmToOpenSim/extern.h deleted file mode 100644 index 3e32298e91..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/extern.h +++ /dev/null @@ -1,15 +0,0 @@ -extern JointName joint_name[]; -extern SegmentName segment_name[]; -extern ComponentName component_name[]; -extern SideName side_name[]; -extern GaitEventName event_name[]; -extern GenCoordDefinition gen_coord_def[]; -extern MuscleGroupName muscle_group_name[]; -extern char *drawing_mode_name[]; -extern char *dof_name[]; -extern double default_tendon_frc_len[NUM_TENDON_PTS][2]; -extern double default_msl_active_frc_len[NUM_MSL_ACT_PTS][2]; -extern double default_msl_passive_frc_len[NUM_MSL_PAS_PTS][2]; -extern MuscleDefinition muscledef[]; -extern MuscleDefinition normmuscledef[]; - diff --git a/OpenSim/Utilities/simmToOpenSim/f2c.h b/OpenSim/Utilities/simmToOpenSim/f2c.h deleted file mode 100644 index a1acfaf531..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/f2c.h +++ /dev/null @@ -1,227 +0,0 @@ -/* f2c.h -- Standard Fortran to C header file */ - -/** barf [ba:rf] 2. "He suggested using FORTRAN, and everybody barfed." - - - From The Shogakukan DICTIONARY OF NEW ENGLISH (Second edition) */ - -#ifndef F2C_INCLUDE -#define F2C_INCLUDE - -typedef long int integer; -typedef unsigned long int uinteger; -typedef char *address; -typedef short int shortint; -typedef float real; -typedef double doublereal; -typedef struct { real r, i; } complex; -typedef struct { doublereal r, i; } doublecomplex; -typedef long int logical; -typedef short int shortlogical; -typedef char logical1; -typedef char integer1; -#ifdef INTEGER_STAR_8 /* Adjust for integer*8. */ -typedef long long longint; /* system-dependent */ -typedef unsigned long long ulongint; /* system-dependent */ -#define qbit_clear(a,b) ((a) & ~((ulongint)1 << (b))) -#define qbit_set(a,b) ((a) | ((ulongint)1 << (b))) -#endif - -#define TRUE_ (1) -#define FALSE_ (0) - -/* Extern is for use with -E */ -#ifndef Extern -#define Extern extern -#endif - -/* I/O stuff */ - -#ifdef f2c_i2 -/* for -i2 */ -typedef short flag; -typedef short ftnlen; -typedef short ftnint; -#else -typedef long int flag; -typedef long int ftnlen; -typedef long int ftnint; -#endif - -/*external read, write*/ -typedef struct -{ flag cierr; - ftnint ciunit; - flag ciend; - char *cifmt; - ftnint cirec; -} cilist; - -/*internal read, write*/ -typedef struct -{ flag icierr; - char *iciunit; - flag iciend; - char *icifmt; - ftnint icirlen; - ftnint icirnum; -} icilist; - -/*open*/ -typedef struct -{ flag oerr; - ftnint ounit; - char *ofnm; - ftnlen ofnmlen; - char *osta; - char *oacc; - char *ofm; - ftnint orl; - char *oblnk; -} olist; - -/*close*/ -typedef struct -{ flag cerr; - ftnint cunit; - char *csta; -} cllist; - -/*rewind, backspace, endfile*/ -typedef struct -{ flag aerr; - ftnint aunit; -} alist; - -/* inquire */ -typedef struct -{ flag inerr; - ftnint inunit; - char *infile; - ftnlen infilen; - ftnint *inex; /*parameters in standard's order*/ - ftnint *inopen; - ftnint *innum; - ftnint *innamed; - char *inname; - ftnlen innamlen; - char *inacc; - ftnlen inacclen; - char *inseq; - ftnlen inseqlen; - char *indir; - ftnlen indirlen; - char *infmt; - ftnlen infmtlen; - char *inform; - ftnint informlen; - char *inunf; - ftnlen inunflen; - ftnint *inrecl; - ftnint *innrec; - char *inblank; - ftnlen inblanklen; -} inlist; - -#define VOID void - -union Multitype { /* for multiple entry points */ - integer1 g; - shortint h; - integer i; - /* longint j; */ - real r; - doublereal d; - complex c; - doublecomplex z; - }; - -typedef union Multitype Multitype; - -/*typedef long int Long;*/ /* No longer used; formerly in Namelist */ - -struct Vardesc { /* for Namelist */ - char *name; - char *addr; - ftnlen *dims; - int type; - }; -typedef struct Vardesc Vardesc; - -struct Namelist { - char *name; - Vardesc **vars; - int nvars; - }; -typedef struct Namelist Namelist; - -#define abs(x) ((x) >= 0 ? (x) : -(x)) -#define dabs(x) (doublereal)abs(x) -#ifndef min -#define min(a,b) ((a) <= (b) ? (a) : (b)) -#endif -#ifndef max -#define max(a,b) ((a) >= (b) ? (a) : (b)) -#endif -#define dmin(a,b) (doublereal)min(a,b) -#define dmax(a,b) (doublereal)max(a,b) -#define bit_test(a,b) ((a) >> (b) & 1) -#define bit_clear(a,b) ((a) & ~((uinteger)1 << (b))) -#define bit_set(a,b) ((a) | ((uinteger)1 << (b))) - -/* procedure parameter types for -A and -C++ */ - -#define F2C_proc_par_types 1 -#ifdef __cplusplus -typedef int /* Unknown procedure type */ (*U_fp)(...); -typedef shortint (*J_fp)(...); -typedef integer (*I_fp)(...); -typedef real (*R_fp)(...); -typedef doublereal (*D_fp)(...), (*E_fp)(...); -typedef /* Complex */ VOID (*C_fp)(...); -typedef /* Double Complex */ VOID (*Z_fp)(...); -typedef logical (*L_fp)(...); -typedef shortlogical (*K_fp)(...); -typedef /* Character */ VOID (*H_fp)(...); -typedef /* Subroutine */ int (*S_fp)(...); -#else -typedef int /* Unknown procedure type */ (*U_fp)(); -typedef shortint (*J_fp)(); -typedef integer (*I_fp)(); -typedef real (*R_fp)(); -typedef doublereal (*D_fp)(), (*E_fp)(); -typedef /* Complex */ VOID (*C_fp)(); -typedef /* Double Complex */ VOID (*Z_fp)(); -typedef logical (*L_fp)(); -typedef shortlogical (*K_fp)(); -typedef /* Character */ VOID (*H_fp)(); -typedef /* Subroutine */ int (*S_fp)(); -#endif -/* E_fp is for real functions when -R is not specified */ -typedef VOID C_f; /* complex function */ -typedef VOID H_f; /* character function */ -typedef VOID Z_f; /* double complex function */ -typedef doublereal E_f; /* real function with -R not specified */ - -/* undef any lower-case symbols that your C compiler predefines, e.g.: */ - -#ifndef Skip_f2c_Undefs -#undef cray -#undef gcos -#undef mc68010 -#undef mc68020 -#undef mips -#undef pdp11 -#undef sgi -#undef sparc -#undef sun -#undef sun2 -#undef sun3 -#undef sun4 -#undef u370 -#undef u3b -#undef u3b2 -#undef u3b5 -#undef unix -#undef vax -#endif -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/free.c b/OpenSim/Utilities/simmToOpenSim/free.c deleted file mode 100644 index bd51da74f8..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/free.c +++ /dev/null @@ -1,724 +0,0 @@ -/******************************************************************************* - - FREE.C - - Author: Peter Loan - - Date: 07-MAY-90 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: - - Routines: - free_model : frees a model structure - free_plot : frees a plot structure - free_muscles : frees memory malloced for muscle structure elements - free_default_muscle : frees memory malloced for defaultmuscle elements - free_menu : frees a menu structure - free_form : frees a form structure - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "normtools.h" - - -/*************** DEFINES (for this file only) *********************************/ - - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ -#if ! ENGINE -extern ModelStruct* sMotionModel; -#endif - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void free_segment(SegmentStruct* seg, ModelStruct* ms); -static void free_saved_segment(SaveSegments* seg, ModelStruct* ms); - - -void free_model(int mod) -{ - if (gModel[mod] != NULL) - { -#if ! ENGINE - if (sMotionModel == gModel[mod]) - sMotionModel = NULL; -#endif - freeModelStruct(gModel[mod]); - gModel[mod] = NULL; - } -} - -void freeModelStruct(ModelStruct* ms) -{ - int i, j, k; - - FREE_IFNOTNULL(ms->name); - if (ms->pathptrs != NULL) - { - for (i=0; inumsegments*ms->numsegments; i++) - FREE_IFNOTNULL(ms->pathptrs[i]); - free(ms->pathptrs); - } - FREE_IFNOTNULL(ms->jointfilename); - FREE_IFNOTNULL(ms->musclefilename); - FREE_IFNOTNULL(ms->bonepathname); - FREE_IFNOTNULL(ms->mocap_dir); - for (i=0; inum_motion_files; i++) - FREE_IFNOTNULL(ms->motionfilename[i]); - - for (i=0; inumgroups; i++) - { - FREE_IFNOTNULL(ms->muscgroup[i].name); - FREE_IFNOTNULL(ms->muscgroup[i].muscle_index); - free_menu(&ms->muscgroup[i].menu); - } - FREE_IFNOTNULL(ms->muscgroup); - for (i=0; isave.numsavedmuscgroups; i++) - { - FREE_IFNOTNULL(ms->save.muscgroup[i].name); - FREE_IFNOTNULL(ms->save.muscgroup[i].muscle_index); - } - FREE_IFNOTNULL(ms->save.muscgroup); - - free_form(&ms->gencform); - free_form(&ms->dynparamsform); - - for (i = 0; i < ms->gc_chpanel.numoptions; i++) - FREE_IFNOTNULL(ms->gc_chpanel.checkbox[i].name); - FREE_IFNOTNULL(ms->gc_chpanel.checkbox); - //FREE_IFNOTNULL(ms->gc_chpanel.title); title uses static char - - for (i = 0; i < ms->gc_lockPanel.numoptions; i++) - FREE_IFNOTNULL(ms->gc_lockPanel.checkbox[i].name); - FREE_IFNOTNULL(ms->gc_lockPanel.checkbox); - //FREE_IFNOTNULL(ms->gc_lockPanel.title); title uses static char - - for (i = 0; i < ms->numseggroups; i++) - { - FREE_IFNOTNULL(ms->seggroup[i].name); - FREE_IFNOTNULL(ms->seggroup[i].segment); - } - FREE_IFNOTNULL(ms->seggroup); - - for (i=0; inumjoints; i++) - { - FREE_IFNOTNULL(ms->joint[i].name); - FREE_IFNOTNULL(ms->joint[i].solverType); - FREE_IFNOTNULL(ms->joint[i].in_seg_ground_path); -#if INCLUDE_MOCAP_MODULE - FREE_IFNOTNULL(ms->joint[i].mocap_segment); -#endif - } - FREE_IFNOTNULL(ms->joint); - - for (i=0; isave.numsavedjnts; i++) - FREE_IFNOTNULL(ms->save.joint[i].name); - FREE_IFNOTNULL(ms->save.joint); - - for (i=0; inumsegments; i++) - free_segment(&ms->segment[i], ms); - FREE_IFNOTNULL(ms->segment); - for (i=0; isave.numsavedsegments; i++) - free_saved_segment(&ms->save.segment[i], ms); - FREE_IFNOTNULL(ms->save.segment); - - for (i=0; isave.num_markers; i++) - FREE_IFNOTNULL(ms->save.marker[i].name); - FREE_IFNOTNULL(ms->save.marker); - - for (i=0; inum_wrap_objects; i++) - { - FREE_IFNOTNULL(ms->wrapobj[i]->name); - FREE_IFNOTNULL(ms->wrapobj[i]); - } - FREE_IFNOTNULL(ms->wrapobj); - for (i=0; isave.num_wrap_objects; i++) - FREE_IFNOTNULL(ms->save.wrap_object[i].name); - FREE_IFNOTNULL(ms->save.wrap_object); - - for (i=0; inumgencoords; i++) - { - if (ms->gencoord[i]->defined == yes) - { - FREE_IFNOTNULL(ms->gencoord[i]->name); - FREE_IFNOTNULL(ms->gencoord[i]->jointnum); -#if INCLUDE_MOCAP_MODULE - FREE_IFNOTNULL(ms->gencoord[i]->mocap_segment); -#endif - FREE_IFNOTNULL(ms->gencoord[i]->group); - } - } - FREE_IFNOTNULL(ms->gencoord); - FREE_IFNOTNULL(ms->save.gencoord); - - for (i = 0; i < 2*GENBUFFER; i++) - FREE_IFNOTNULL(ms->genc_help[i].text); - - for (i = 0; i < ms->num_deformities; i++) - { - if (ms->deformity[i].deform_name) - { - for (j = 0; j < ms->deformity[i].num_deforms; j++) - FREE_IFNOTNULL(ms->deformity[i].deform_name[j]); - - FREE_IFNOTNULL(ms->deformity[i].deform_name); - } - - FREE_IFNOTNULL(ms->deformity[i].deform); - } - FREE_IFNOTNULL(ms->deformity); - - for (i = 0; i < ms->numligaments; i++) - { - FREE_IFNOTNULL(ms->ligament[i].name); - for (j = 0; j < ms->ligament[i].numlines; j++) - { - FREE_IFNOTNULL(ms->ligament[i].line[j].mp_orig); - FREE_IFNOTNULL(ms->ligament[i].line[j].mp); - } - FREE_IFNOTNULL(ms->ligament[i].line); - } - FREE_IFNOTNULL(ms->ligament); - - free_muscles(ms); - free_default_muscle(ms->default_muscle); - - for (i=0; ifunc_array_size; i++) - free_function(ms->function[i], yes); - FREE_IFNOTNULL(ms->function); - - if (ms->save.function) - { - for (i=0; ifunc_array_size; i++) - free_function(ms->save.function[i], yes); - FREE_IFNOTNULL(ms->save.function); - } - -#if ! ENGINE - for (i = 0; i < ms->num_motion_objects; i++) - free_motion_object(&ms->motion_objects[i], ms); - - FREE_IFNOTNULL(ms->motion_objects); -#endif - - FREE_IFNOTNULL(ms->save.muscwrap_associations); - - for (i = 0; i < ms->numworldobjects; i++) - { - FREE_IFNOTNULL(ms->worldobj[i].name); - FREE_IFNOTNULL(ms->worldobj[i].filename); - if (ms->worldobj[i].wobj) - free_polyhedron(ms->worldobj[i].wobj, yes, ms); - } - FREE_IFNOTNULL(ms->worldobj); - - for (i = 0; i < ms->save.num_deforms; i++) - { - FREE_IFNOTNULL(ms->save.deform[i].name); - FREE_IFNOTNULL(ms->save.deform[i].innerBox); - FREE_IFNOTNULL(ms->save.deform[i].innerBoxUndeformed); - FREE_IFNOTNULL(ms->save.deform[i].outerBox); - FREE_IFNOTNULL(ms->save.deform[i].outerBoxUndeformed); - } - FREE_IFNOTNULL(ms->save.deform); - - for (i = 0; i < ms->num_constraint_objects; i++) - { - FREE_IFNOTNULL(ms->constraintobj[i].name); - FREE_IFNOTNULL(ms->constraintobj[i].joints); - FREE_IFNOTNULL(ms->constraintobj[i].qs); - for (j = 0; j < ms->constraintobj[i].numPoints; j++) - FREE_IFNOTNULL(ms->constraintobj[i].points[j].name); - FREE_IFNOTNULL(ms->constraintobj[i].points); - } - FREE_IFNOTNULL(ms->constraintobj); - - for (i = 0; i < ms->save.num_constraint_objects; i++) - { - FREE_IFNOTNULL(ms->save.constraintobj[i].name); - FREE_IFNOTNULL(ms->save.constraintobj[i].joints); - FREE_IFNOTNULL(ms->save.constraintobj[i].qs); - for (j = 0; j < ms->save.constraintobj[i].numPoints; j++) - FREE_IFNOTNULL(ms->save.constraintobj[i].points[j].name); - FREE_IFNOTNULL(ms->save.constraintobj[i].points); - } - FREE_IFNOTNULL(ms->save.constraintobj); - - for (i = 0; i < ms->save.num_conspt_associations; i++) - { - for (j = 0; j < ms->save.conspt_associations[i].numPoints; j++) - FREE_IFNOTNULL(ms->save.conspt_associations[i].savedPoints[j].name); - FREE_IFNOTNULL(ms->save.conspt_associations[i].savedPoints); - } - FREE_IFNOTNULL(ms->save.conspt_associations); - - FREE_IFNOTNULL(ms->segment_drawing_order); - - for (i = 0; i < MAXSAVEDVIEWS; i++) - FREE_IFNOTNULL(ms->dis.view_name[i]); - - for (i = 0; i < ms->numgencgroups; i++) - { - FREE_IFNOTNULL(ms->gencgroup[i].name); - FREE_IFNOTNULL(ms->gencgroup[i].gencoord); - } - FREE_IFNOTNULL(ms->gencgroup); - - FREE_IFNOTNULL(ms->gencslider.sl); - FREE_IFNOTNULL(ms->dis.devs); - FREE_IFNOTNULL(ms->dis.dev_values); - FREE_IFNOTNULL(ms->dis.muscleson); - FREE_IFNOTNULL(ms->forceUnits); - FREE_IFNOTNULL(ms->lengthUnits); - - // The motions are deleted by delete_model() so that the appropriate - // events can be generated, so all that remains here is the array of - // motion structure pointers. - FREE_IFNOTNULL(ms->motion); - - FREE_IFNOTNULL(ms); -} - -static void free_segment(SegmentStruct* seg, ModelStruct* ms) -{ - int j; - - if (seg->defined == no) - return; - - FREE_IFNOTNULL(seg->name); - for (j=0; jnumBones; j++) - free_polyhedron(&seg->bone[j], no, ms); - FREE_IFNOTNULL(seg->bone); - for (j=0; jnumSpringPoints; j++) - { - FREE_IFNOTNULL(seg->springPoint[j].name); - } - FREE_IFNOTNULL(seg->springPoint); - FREE_IFNOTNULL(seg->group); - - if (seg->springFloor) - { - FREE_IFNOTNULL(seg->springFloor->name); - FREE_IFNOTNULL(seg->springFloor->filename); - free_polyhedron(seg->springFloor->poly, yes, ms); - FREE_IFNOTNULL(seg->springFloor->points); - FREE_IFNOTNULL(seg->springFloor); - } - - for (j=0; jnumContactObjects; j++) - { - FREE_IFNOTNULL(seg->contactObject[j].name); - FREE_IFNOTNULL(seg->contactObject[j].filename); - free_polyhedron(seg->contactObject[j].poly, yes, ms); - } - FREE_IFNOTNULL(seg->contactObject); - - if (seg->forceMatte) - { - FREE_IFNOTNULL(seg->forceMatte->name); - FREE_IFNOTNULL(seg->forceMatte->filename); - free_polyhedron(seg->forceMatte->poly, yes, ms); - FREE_IFNOTNULL(seg->forceMatte); - } - - for (j=0; jnumMarkers; j++) - { - FREE_IFNOTNULL(seg->marker[j]->name); - FREE_IFNOTNULL(seg->marker[j]); - } - FREE_IFNOTNULL(seg->marker); - - for (j=0; jnum_deforms; j++) - { - FREE_IFNOTNULL(seg->deform[j].name); - FREE_IFNOTNULL(seg->deform[j].innerBox); - FREE_IFNOTNULL(seg->deform[j].innerBoxUndeformed); - FREE_IFNOTNULL(seg->deform[j].outerBox); - FREE_IFNOTNULL(seg->deform[j].outerBoxUndeformed); - } - FREE_IFNOTNULL(seg->deform); - -#if INCLUDE_MOCAP_MODULE - FREE_IFNOTNULL(seg->gait_scale_segment); - FREE_IFNOTNULL(seg->mocap_segment); - FREE_IFNOTNULL(seg->mocap_scale_chain_end1); - FREE_IFNOTNULL(seg->mocap_scale_chain_end2); -#endif -} - -static void free_saved_segment(SaveSegments* seg, ModelStruct* ms) -{ - int j; - - FREE_IFNOTNULL(seg->name); - for (j=0; jnumSpringPoints; j++) - { - FREE_IFNOTNULL(seg->springPoint[j].name); - } - FREE_IFNOTNULL(seg->springPoint); - - if (seg->springFloor) - { - FREE_IFNOTNULL(seg->springFloor->name); - FREE_IFNOTNULL(seg->springFloor->filename); - free_polyhedron(seg->springFloor->poly, yes, ms); - FREE_IFNOTNULL(seg->springFloor->points); - FREE_IFNOTNULL(seg->springFloor); - } - - for (j=0; jnumContactObjects; j++) - { - FREE_IFNOTNULL(seg->contactObject[j].name); - FREE_IFNOTNULL(seg->contactObject[j].filename); - free_polyhedron(seg->contactObject[j].poly, yes, ms); - } - FREE_IFNOTNULL(seg->contactObject); - - if (seg->forceMatte) - { - FREE_IFNOTNULL(seg->forceMatte->name); - FREE_IFNOTNULL(seg->forceMatte->filename); - free_polyhedron(seg->forceMatte->poly, yes, ms); - FREE_IFNOTNULL(seg->forceMatte); - } -} - -#if ! ENGINE -void free_plot(int plotnum) -{ - int i, j; - - FREE_IFNOTNULL(gPlot[plotnum]->title); - FREE_IFNOTNULL(gPlot[plotnum]->xname); - /* JPL 11/2/00 TODO: for some reason, freeing the yname is causing - * a crash, so remove it for now. - */ -/* FREE_IFNOTNULL(gPlot[plotnum]->yname);*/ - - for (i=0; inumcurves; i++) - { - FREE_IFNOTNULL(gPlot[plotnum]->curve[i]->xvalues); - FREE_IFNOTNULL(gPlot[plotnum]->curve[i]->yvalues); - FREE_IFNOTNULL(gPlot[plotnum]->curve[i]->name); - if (gPlot[plotnum]->curve[i]->num_events > 0) - { - for (j=0; jcurve[i]->num_events; j++) - FREE_IFNOTNULL(gPlot[plotnum]->curve[i]->event[j].name); - FREE_IFNOTNULL(gPlot[plotnum]->curve[i]->event); - } - FREE_IFNOTNULL(gPlot[plotnum]->curve[i]); - } - - if (gPlot[plotnum]->num_file_events > 0) - { - for (j=0; jnum_file_events; j++) - FREE_IFNOTNULL(gPlot[plotnum]->file_event[j].name); - FREE_IFNOTNULL(gPlot[plotnum]->file_event); - } - - FREE_IFNOTNULL(gPlot[plotnum]); - - gPlot[plotnum] = NULL; -} -#endif - - -void free_muscle(dpMuscleStruct *muscle, dpMuscleStruct* dm) -{ - int i; - - if (muscle == NULL) - return; - - if (muscle->name != dm->name) - FREE_IFNOTNULL(muscle->name); - - if (muscle->path) - { - FREE_IFNOTNULL(muscle->path->mp_orig); - FREE_IFNOTNULL(muscle->path->mp); - FREE_IFNOTNULL(muscle->path); - } - - if (muscle->group != dm->group) - FREE_IFNOTNULL(muscle->group); - if (muscle->max_isometric_force != dm->max_isometric_force) - FREE_IFNOTNULL(muscle->max_isometric_force); - if (muscle->pennation_angle != dm->pennation_angle) - FREE_IFNOTNULL(muscle->pennation_angle); - if (muscle->min_thickness != dm->min_thickness) - FREE_IFNOTNULL(muscle->min_thickness); - if (muscle->max_thickness != dm->max_thickness) - FREE_IFNOTNULL(muscle->max_thickness); - if (muscle->min_material != dm->min_material) - FREE_IFNOTNULL(muscle->min_material); - if (muscle->max_material != dm->max_material) - FREE_IFNOTNULL(muscle->max_material); - if (muscle->max_contraction_vel != dm->max_contraction_vel) - FREE_IFNOTNULL(muscle->max_contraction_vel); - if (muscle->optimal_fiber_length != dm->optimal_fiber_length) - FREE_IFNOTNULL(muscle->optimal_fiber_length); - if (muscle->resting_tendon_length != dm->resting_tendon_length) - FREE_IFNOTNULL(muscle->resting_tendon_length); - if (muscle->momentarms != dm->momentarms) - FREE_IFNOTNULL(muscle->momentarms); - if (muscle->active_force_len_func != dm->active_force_len_func) - FREE_IFNOTNULL(muscle->active_force_len_func); - if (muscle->passive_force_len_func != dm->passive_force_len_func) - FREE_IFNOTNULL(muscle->passive_force_len_func); - if (muscle->tendon_force_len_func != dm->tendon_force_len_func) - FREE_IFNOTNULL(muscle->tendon_force_len_func); - if (muscle->force_vel_func != dm->force_vel_func) - FREE_IFNOTNULL(muscle->force_vel_func); - if (muscle->excitation_func != dm->excitation_func) - FREE_IFNOTNULL(muscle->excitation_func); - - if (muscle->wrapStruct) - { - for (i = 0; i < muscle->numWrapStructs; i++) - { - FREE_IFNOTNULL(muscle->wrapStruct[i]->mp_wrap[0].wrap_pts); - FREE_IFNOTNULL(muscle->wrapStruct[i]->mp_wrap[1].wrap_pts); - FREE_IFNOTNULL(muscle->wrapStruct[i]); - } - FREE_IFNOTNULL(muscle->wrapStruct); - } - if (muscle->muscle_model_index != dm->muscle_model_index) - FREE_IFNOTNULL(muscle->muscle_model_index); - - if (muscle->dynamic_params) - { - for (i = 0; i < muscle->num_dynamic_params; i++) - { - if (muscle->dynamic_params[i] != dm->dynamic_params[i]) - FREE_IFNOTNULL(muscle->dynamic_params[i]); - } - FREE_IFNOTNULL(muscle->dynamic_params); - } -} - -void free_muscles(ModelStruct* model) -{ - int i; - - if (model == NULL) - return; - - for (i=0; inummuscles; i++) - { - free_muscle(model->muscle[i], model->default_muscle); - FREE_IFNOTNULL(model->muscle[i]); - } - - FREE_IFNOTNULL(model->muscle); -} - -/* FREE_DEFMUSC: */ -void free_default_muscle(dpMuscleStruct* dm) -{ - int i; - - if (dm == NULL) - return; - - FREE_IFNOTNULL(dm->name); - FREE_IFNOTNULL(dm->group); - FREE_IFNOTNULL(dm->max_isometric_force); - FREE_IFNOTNULL(dm->pennation_angle); - FREE_IFNOTNULL(dm->min_thickness); - FREE_IFNOTNULL(dm->max_thickness); - FREE_IFNOTNULL(dm->min_material); - FREE_IFNOTNULL(dm->max_material); - FREE_IFNOTNULL(dm->muscle_model_index); - FREE_IFNOTNULL(dm->max_contraction_vel); - - FREE_IFNOTNULL(dm->optimal_fiber_length); - FREE_IFNOTNULL(dm->resting_tendon_length); - FREE_IFNOTNULL(dm->momentarms); - - FREE_IFNOTNULL(dm->tendon_force_len_func); - FREE_IFNOTNULL(dm->active_force_len_func); - FREE_IFNOTNULL(dm->passive_force_len_func); - FREE_IFNOTNULL(dm->force_vel_func); - FREE_IFNOTNULL(dm->excitation_func); - - for (i = 0; i < dm->num_dynamic_params; i++) - FREE_IFNOTNULL(dm->dynamic_params[i]); - FREE_IFNOTNULL(dm->dynamic_params); - - for (i = 0; i < dm->num_dynamic_params; i++) - FREE_IFNOTNULL(dm->dynamic_param_names[i]); - FREE_IFNOTNULL(dm->dynamic_param_names); -} - -void free_menu(Menu* mn) -{ - int i; - - for (i=0; inumoptions; i++) - FREE_IFNOTNULL(mn->option[i].name); - - FREE_IFNOTNULL(mn->title); - FREE_IFNOTNULL(mn->option); -} - - -void free_form(Form* frm) -{ - int i; - - for (i=0; inumoptions; i++) - FREE_IFNOTNULL(frm->option[i].name); - - FREE_IFNOTNULL(frm->title); - FREE_IFNOTNULL(frm->option); -} - -void free_checkbox_panel(CheckBoxPanel* panel) -{ - int i; - - for (i=0; inumoptions; i++) - FREE_IFNOTNULL(panel->checkbox[i].name); - - FREE_IFNOTNULL(panel->title); - FREE_IFNOTNULL(panel->checkbox); -} - -/* ------------------------------------------------------------------------- - free_motion_object - ----------------------------------------------------------------------------- */ -public void free_motion_object(MotionObject* mo, ModelStruct* ms) -{ - if (mo) - { - FREE_IFNOTNULL(mo->name); - FREE_IFNOTNULL(mo->filename); - FREE_IFNOTNULL(mo->materialname); - free_polyhedron(&mo->shape, no, ms); - } -} - -/* ------------------------------------------------------------------------- - free_motion_object_instance - ----------------------------------------------------------------------------- */ -public void free_motion_object_instance(MotionObjectInstance* mi, ModelStruct* model) -{ - if (mi) - { - FREE_IFNOTNULL(mi->name); - mi->num_channels = 0; - -#if ! ENGINE - if (mi->currentMaterial.normal_list) - glDeleteLists(mi->currentMaterial.normal_list, 1); - - if (mi->currentMaterial.highlighted_list) - glDeleteLists(mi->currentMaterial.highlighted_list, 1); - - delete_display_list(mi->aux_display_obj, model); -#endif - - FREE_IFNOTNULL(mi->channels); - } -} - -#if ! ENGINE - -void delete_display_list(GLuint display_list, ModelStruct* model) -{ - if (display_list) - { - if (model) - { - // TODO_SCENE: the model for this display list may be in more than one scene - // (window). To delete the display list, you have to glutSetWindow to the one - // that was current when the display list was created. For now, assume that - // this is the first scene that contains the model. - int savedWindow = glutGetWindow(); - Scene* scene = get_first_scene_containing_model(model); - - if (scene) - { - glutSetWindow(scene->window_glut_id); - glDeleteLists(display_list, 1); - } - glutSetWindow(savedWindow); - } - else - { - glDeleteLists(display_list, 1); - } - } -} - - -void delete_polyhedron_display_list(PolyhedronStruct* ph, ModelStruct* model) -{ - if (ph && ph->gl_display) - { - if (model) - { - // TODO_SCENE: the polyhedron has only one display list, but the model - // may be in more than one scene (window). To delete the display list, - // you have to glutSetWindow to the one that was current when the display - // list was created. For now, assume that this is the first scene that - // contains the model. - int savedWindow = glutGetWindow(); - Scene* scene = get_first_scene_containing_model(model); - - if (scene) - { - glutSetWindow(scene->window_glut_id); - glDeleteLists(ph->gl_display, 1); - ph->gl_display = 0; - } - glutSetWindow(savedWindow); - } - else - { - glDeleteLists(ph->gl_display, 1); - ph->gl_display = 0; - } - } -} - - -void delete_segment_display_lists(SegmentStruct* seg, ModelStruct* model) -{ - if (seg && model) - { - // TODO_SCENE: the segment's polyhedra have only one display list each, - // but the model may be in more than one scene (window). To delete the display - // lists, you have to glutSetWindow to the one that was current when the display - // lists were created. For now, assume that this is the first scene that - // contains the model. - int i, savedWindow = glutGetWindow(); - Scene* scene = get_first_scene_containing_model(model); - - if (scene) - { - glutSetWindow(scene->window_glut_id); - for (i=0; inumBones; i++) - { - glDeleteLists(seg->bone[i].gl_display, 1); - seg->bone[i].gl_display = 0; - } - } - glutSetWindow(savedWindow); - } -} - -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/function.c b/OpenSim/Utilities/simmToOpenSim/function.c deleted file mode 100644 index a88c76f149..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/function.c +++ /dev/null @@ -1,892 +0,0 @@ -/******************************************************************************* - - FUNCTION.C - - Author: Peter Loan - - Date: 5-NOV-2009 - - Copyright (c) 2009 MusculoGraphics, Inc. - All rights reserved. - - Description: - - Routines: - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "gcvspl.h" - - -/*************** DEFINES (for this file only) *********************************/ - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void calc_natural_cubic_coefficients(dpFunction* f); - - -ReturnCode read_function(ModelStruct* ms, FILE* fp, SBoolean muscle_function, dpFunctionType funcType, const char endTag[]) -{ - dpFunction func; - char fname[CHARBUFFER]; - - func.type = funcType; - func.cutoff_frequency = 0.0; - func.source = (muscle_function == yes) ? dpMuscleFile : dpJointFile; - - // Malloc space for the points and coefficients of this function. - (void)malloc_function(&func, FUNCTION_ARRAY_INCREMENT); - - if (read_string(fp, buffer) == EOF) - { - (void)sprintf(errorbuffer, "Unexpected EOF found reading a function"); - error(abort_action,errorbuffer); - return code_bad; - } - - // Read the function name, which is usually a number (e.g. "f19"). - if (sscanf(buffer, "f%d", &func.usernum) != 1) - { - error(abort_action, "Error reading a function name"); - return code_bad; - } - - if (func.usernum < 0) - { - (void)sprintf(errorbuffer, "Function numbers must be non-negative (%d not allowed)", func.usernum); - error(abort_action,errorbuffer); - return code_bad; - } - - if (muscle_function) - (void)sprintf(fname, "muscle function f%d", func.usernum); - else - (void)sprintf(fname, "f%d", func.usernum); - - // Get the array of x-y pairs which comprises the function. - if (read_double_array(fp, endTag, fname, &func) == code_bad) - { - free(func.x); - free(func.y); - free(func.b); - free(func.c); - free(func.d); - return code_bad; - } - - // If you made it to here, a valid function with two or more points has been - // read-in. You want to leave the arrays large so that the user can add more - // points to the function. So just calculate the coefficients, and then load - // it into the model's function list. - calc_function_coefficients(&func); - - if (load_user_function(ms, &func) == NULL) - return code_bad; - - return code_fine; -} - - -/* ENTER_FUNCTION: whenever a function number is read from a file, the - * user-specified number is compared to the existing array of functions to - * see if that one has already been defined. If it has, the routine just - * returns the internal number of that function. If it has not yet been - * defined, it adds the number to a new element in the function list. - */ -int enter_function(ModelStruct* model, int usernum, SBoolean permission_to_add) -{ - int i, new_func, old_count; - ReturnCode rc1 = code_fine, rc2 = code_fine; - - if (usernum != UNDEFINED_USERFUNCNUM) - { - for (i=0; ifunc_array_size; i++) - { - if (model->function[i] && usernum == model->function[i]->usernum) - { - if (model->function[i]->status == dpFunctionSimmDefined) - { - // If the usernum is already being used by a SIMM-defined function, - // change the usernum of the SIMM-defined function. - if (permission_to_add == yes) - { - model->function[i]->usernum = findHighestUserFuncNum(model) + 1; - break; - } - else // You can't match a user-defined function with a SIMM-defined one. - { - return INVALID_FUNCTION; - } - } - else - { - return i; - } - } - } - } - - if (permission_to_add == no) - return INVALID_FUNCTION; - - for (i=0; ifunc_array_size; i++) - if (model->function[i] == NULL) - break; - new_func = i; - - if (new_func == model->func_array_size) - { - old_count = model->func_array_size; - model->func_array_size += FUNC_ARRAY_INCREMENT; - model->function = (dpFunction**)simm_realloc(model->function, model->func_array_size*sizeof(dpFunction*), &rc1); - model->save.function = (dpFunction**)simm_realloc(model->save.function, model->func_array_size*sizeof(dpFunction*), &rc2); - if (rc1 == code_bad || rc2 == code_bad) - { - model->func_array_size = old_count; - return INVALID_FUNCTION; - } - - for (i=old_count; ifunc_array_size; i++) - { - model->function[i] = NULL; - model->save.function[i] = NULL; - } - } - - model->function[new_func] = (dpFunction*)simm_calloc(1, sizeof(dpFunction)); - model->function[new_func]->used = dpYes; - model->function[new_func]->status = dpFunctionUndefined; - model->function[new_func]->usernum = usernum; - model->function[new_func]->numpoints = 0; - model->function[new_func]->coefficient_array_size = 0; - model->function[new_func]->cutoff_frequency = 0.0; - - return new_func; -} - -/* This function passes ownership of the x, y, b, c, d arrays in 'func' to the model. */ -dpFunction* load_user_function(ModelStruct* model, dpFunction* func) -{ - // Get a slot to put the function in. enter_function() will allocate space for - // the dpFunction structure, but not for the x, y, b, c, d arrays, which are - // taken from 'func.' - int funcIndex = enter_function(model, func->usernum, yes); - - // This should happen only when the function array can't be expanded - // to hold the new function. - if (funcIndex == INVALID_FUNCTION) - return NULL; - - if (model->function[funcIndex]->status != dpFunctionUndefined) - { - (void)sprintf(errorbuffer, "Warning: redefinition of function f%d. Replacing earlier definition.", func->usernum); - error(none, errorbuffer); - } - - model->function[funcIndex]->type = func->type; - model->function[funcIndex]->numpoints = func->numpoints; - model->function[funcIndex]->coefficient_array_size = func->coefficient_array_size; - model->function[funcIndex]->cutoff_frequency = func->cutoff_frequency; - model->function[funcIndex]->x = func->x; - model->function[funcIndex]->y = func->y; - model->function[funcIndex]->b = func->b; - model->function[funcIndex]->c = func->c; - model->function[funcIndex]->d = func->d; - model->function[funcIndex]->status = dpFunctionUserDefined; - model->function[funcIndex]->usernum = func->usernum; - model->function[funcIndex]->used = dpYes; - model->function[funcIndex]->source = func->source; - - //Put a copy of the function in model->save.function. - free_function(model->save.function[funcIndex], no); - if (model->save.function[funcIndex] == NULL) - model->save.function[funcIndex] = (dpFunction*)simm_calloc(1, sizeof(dpFunction)); - malloc_function(model->save.function[funcIndex], func->coefficient_array_size); - copy_function(model->function[funcIndex], model->save.function[funcIndex]); - - return model->function[funcIndex]; -} - - -//TODO5.0: backup the function in model.save.function? -/* This function passes ownership of the x, y, b, c, d arrays in 'func' to the model. */ -dpFunction* load_simm_function(ModelStruct* model, dpFunction* func, SBoolean isMuscleFunc) -{ - int i, funcIndex; - - // Find an unused slot in the function array. - for (i=0; ifunc_array_size; i++) - if (model->function[i] == NULL) - break; - funcIndex = i; - - if (funcIndex == model->func_array_size) - { - ReturnCode rc1, rc2; - int old_count = model->func_array_size; - model->func_array_size += FUNC_ARRAY_INCREMENT; - model->function = (dpFunction**)simm_realloc(model->function, model->func_array_size*sizeof(dpFunction*), &rc1); - model->save.function = (dpFunction**)simm_realloc(model->save.function, model->func_array_size*sizeof(dpFunction*), &rc2); - if (rc1 == code_bad || rc2 == code_bad) - { - model->func_array_size = old_count; - return NULL; - } - - for (i=old_count; ifunc_array_size; i++) - { - model->function[i] = NULL; - model->save.function[i] = NULL; - } - } - - model->function[funcIndex] = (dpFunction*)simm_calloc(1, sizeof(dpFunction)); - model->function[funcIndex]->usernum = findHighestUserFuncNum(model) + 1; - model->function[funcIndex]->source = (isMuscleFunc) ? dpMuscleFile : dpJointFile; - model->function[funcIndex]->type = func->type; - model->function[funcIndex]->numpoints = func->numpoints; - model->function[funcIndex]->coefficient_array_size = func->coefficient_array_size; - model->function[funcIndex]->cutoff_frequency = func->cutoff_frequency; - model->function[funcIndex]->x = func->x; - model->function[funcIndex]->y = func->y; - model->function[funcIndex]->b = func->b; - model->function[funcIndex]->c = func->c; - model->function[funcIndex]->d = func->d; - model->function[funcIndex]->status = dpFunctionSimmDefined; - model->function[funcIndex]->used = dpYes; - - //Put a copy of the function in model->save.function. - free_function(model->save.function[funcIndex], no); - if (model->save.function[funcIndex] == NULL) - model->save.function[funcIndex] = (dpFunction*)simm_calloc(1, sizeof(dpFunction)); - malloc_function(model->save.function[funcIndex], func->coefficient_array_size); - copy_function(model->function[funcIndex], model->save.function[funcIndex]); - - return model->function[funcIndex]; -} - - -ReturnCode malloc_function(dpFunction* func, int size) -{ - func->numpoints = 0; - - if ((func->x = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if ((func->y = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if ((func->b = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if ((func->c = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - if ((func->d = (double*)simm_malloc(size*sizeof(double))) == NULL) - return code_bad; - - func->coefficient_array_size = size; - - return code_fine; -} - - -ReturnCode realloc_function(dpFunction* func, int size) -{ - ReturnCode rc1 = code_fine, rc2 = code_fine, rc3 = code_fine, rc4 = code_fine, rc5 = code_fine; - - func->x = (double*)simm_realloc(func->x,size*sizeof(double),&rc1); - func->y = (double*)simm_realloc(func->y,size*sizeof(double),&rc2); - func->b = (double*)simm_realloc(func->b,size*sizeof(double),&rc3); - func->c = (double*)simm_realloc(func->c,size*sizeof(double),&rc4); - func->d = (double*)simm_realloc(func->d,size*sizeof(double),&rc5); - - if (rc1 == code_bad || rc2 == code_bad || rc3 == code_bad || rc4 == code_bad || - rc5 == code_bad) - return code_bad; - - func->coefficient_array_size = size; - - return code_fine; -} - - -/* CALC_NATURAL_CUBIC_COEFFICIENTS: this function computes the coefficients - * of a natural cubic spline defined by the XY control points. The code - * is translated from a Fortran version printed in "Computer Methods - * for Mathematical Computations" by Forsythe, Malcolm, and Moler, - * pp 77-8. To better handle splines which have two or more control points - * with the same X values, checks were added to make sure that the code never - * divides by zero. - */ -static void calc_natural_cubic_coefficients(dpFunction* f) -{ - int nm1, nm2, i, j; - double t; - - if (f->numpoints < 2 || f->used == dpNo || f->type != dpNaturalCubicSpline) - return; - - if (f->numpoints == 2) - { - t = _MAX(TINY_NUMBER,f->x[1]-f->x[0]); - f->b[0] = f->b[1] = (f->y[1]-f->y[0])/t; - f->c[0] = f->c[1] = 0.0; - f->d[0] = f->d[1] = 0.0; - return; - } - - nm1 = f->numpoints - 1; - nm2 = f->numpoints - 2; - - /* Set up tridiagonal system: - * b = diagonal, d = offdiagonal, c = right-hand side - */ - f->d[0] = _MAX(TINY_NUMBER,f->x[1] - f->x[0]); - f->c[1] = (f->y[1]-f->y[0])/f->d[0]; - for (i=1; id[i] = _MAX(TINY_NUMBER,f->x[i+1] - f->x[i]); - f->b[i] = 2.0*(f->d[i-1]+f->d[i]); - f->c[i+1] = (f->y[i+1]-f->y[i])/f->d[i]; - f->c[i] = f->c[i+1] - f->c[i]; - } - - /* End conditions. Third derivatives at x[0] and x[n-1] - * are obtained from divided differences. - */ - f->b[0] = -f->d[0]; - f->b[nm1] = -f->d[nm2]; - f->c[0] = 0.0; - f->c[nm1] = 0.0; - - if (f->numpoints > 3) - { - double d1, d2, d3, d20, d30, d31; - - d31 = _MAX(TINY_NUMBER,f->x[3] - f->x[1]); - d20 = _MAX(TINY_NUMBER,f->x[2] - f->x[0]); - d1 = _MAX(TINY_NUMBER,f->x[nm1]-f->x[f->numpoints-3]); - d2 = _MAX(TINY_NUMBER,f->x[nm2]-f->x[f->numpoints-4]); - d30 = _MAX(TINY_NUMBER,f->x[3] - f->x[0]); - d3 = _MAX(TINY_NUMBER,f->x[nm1]-f->x[f->numpoints-4]); - f->c[0] = f->c[2]/d31 - f->c[1]/d20; - f->c[nm1] = f->c[nm2]/d1 - f->c[f->numpoints-3]/d2; - f->c[0] = f->c[0]*f->d[0]*f->d[0]/d30; - f->c[nm1] = -f->c[nm1]*f->d[nm2]*f->d[nm2]/d3; - } - - /* Forward elimination */ - for (i=1; inumpoints; i++) - { - t = f->d[i-1]/f->b[i-1]; - f->b[i] -= t*f->d[i-1]; - f->c[i] -= t*f->c[i-1]; - } - - /* Back substitution */ - f->c[nm1] /= f->b[nm1]; - for (j=0; jc[i] = (f->c[i]-f->d[i]*f->c[i+1])/f->b[i]; - } - - /* compute polynomial coefficients */ - f->b[nm1] = (f->y[nm1]-f->y[nm2])/f->d[nm2] + - f->d[nm2]*(f->c[nm2]+2.0*f->c[nm1]); - for (i=0; ib[i] = (f->y[i+1]-f->y[i])/f->d[i] - f->d[i]*(f->c[i+1]+2.0*f->c[i]); - f->d[i] = (f->c[i+1]-f->c[i])/f->d[i]; - f->c[i] *= 3.0; - } - f->c[nm1] *= 3.0; - f->d[nm1] = f->d[nm2]; -} - - -/* CALC_FUNCTION_COEFFICIENTS: this function takes an array of x and y values, - * and computes the coefficients to interpolate the data. It handles linear, - * natural cubic, and GCV spline functions (step functions do not need - * coefficients). - */ -void calc_function_coefficients(dpFunction* sf) -{ - if (sf == NULL) - return; - - if (sf->status == dpFunctionUndefined) - return; - - memset(sf->b, 0.0, sf->coefficient_array_size * sizeof(double)); - memset(sf->c, 0.0, sf->coefficient_array_size * sizeof(double)); - memset(sf->d, 0.0, sf->coefficient_array_size * sizeof(double)); - - if (sf->type == dpLinearFunction) - { - int i; - - if (sf->numpoints < 2) - { - error(abort_action, "Error: linear function must have at least 2 points."); - return; - } - - /* For a linear function, the slope of each segment is stored in the b array. */ - for (i = 0; i < sf->numpoints - 1; i++) - sf->b[i] = (sf->y[i+1] - sf->y[i]) / (sf->x[i+1] - sf->x[i]); - sf->b[sf->numpoints - 1] = sf->b[sf->numpoints - 2]; - } - else if (sf->type == dpNaturalCubicSpline) - { - calc_natural_cubic_coefficients(sf); - } - else if (sf->type == dpGCVSpline) - { - int i, ier = 0; - double val, sampling_rate, *weight_x = NULL, *weight_y = NULL, *work = NULL; - int mode = 1; // Mode for GCVSPL function chosen to specify smoothing parameter directly - int order = 5; //5th order for quintic spline - int half_order = (order + 1) / 2; // GCVSPL works with the half order - int k = 1; // Only one colum of y data is splined at a time - - sampling_rate = 1.0 / (sf->x[1] - sf->x[0]); - - // Allocate memory as needed - weight_x = (double*)simm_malloc(sf->numpoints*sizeof(double)); - weight_y = (double*)simm_malloc(1*sizeof(double)); - work = (double*)simm_malloc((6*(sf->numpoints*half_order+1)+sf->numpoints)*sizeof(double)); - - // Assign weights - for (i = 0; i < sf->numpoints; i++) - weight_x[i] = 1.0; - weight_y[0] = 1.0; - - // Calculate GCVSPL version of cut-off frequency. If cut-off frequency is <= 0.0, - // set val = 0.0 so that no smoothing is performed. - if (sf->cutoff_frequency <= 0.0) - val = 0.0; - else - val = sampling_rate / pow(2.0*M_PI*sf->cutoff_frequency/pow(sqrt(2.0)-1.0,0.5/half_order), 2.0*half_order); - - gcvspl(sf->x, sf->y, &sf->numpoints, weight_x, weight_y, &half_order, - &sf->numpoints, &k, &mode, &val, sf->c, &sf->numpoints, work, &ier); - - free(weight_x); - free(weight_y); - free(work); - - if (ier > 0) - { - char buf[CHARBUFFER]; - if (ier == 1) - { - sprintf(buf, "GCV spline error: Only %d coordinates specified (%d or more are required).", - sf->numpoints, order + 1); - error(abort_action, buf); - } - else if (ier == 2) - { - error(abort_action, "GCV spline error: X coordinates do not consistently increase in value."); - } - else - { - sprintf(buf, "Error code returned by gcvspl() = %d.", ier); - error(abort_action, buf); - } - return; - } - } -} - - -/* INTERPOLATE_FUNCTION: given a function and an x-value, this routine - * finds the corresponding y-value by interpolating the spline. It - * can return the zeroth, first, or second derivative of the function - * at that x-value. - */ -double interpolate_function(double abscissa, dpFunction* func, Derivative deriv, double velocity, double acceleration) -{ - int i, j, k, n; - double dx; - - if (func->status == dpFunctionUndefined) - return ERROR_DOUBLE; - - if (func->type == dpStepFunction) - { - if (deriv != zeroth) - return 0.0; - - for (i = func->numpoints - 1; i >= 0; i--) - { - if (abscissa >= func->x[i] - ROUNDOFF_ERROR) - return func->y[i]; - } - - /* If the abscissa is less than x[0], return y[0]. */ - return func->y[0]; - } - else if (func->type == dpLinearFunction) - { - if (abscissa < func->x[0]) - { - if (deriv == zeroth) - return func->y[0] - (func->x[0] - abscissa) * func->b[0]; - else if (deriv == first) - return func->b[0] * velocity; - else - return 0.0; - } - else - { - for (i = func->numpoints - 1; i >= 0; i--) - { - if (abscissa >= func->x[i] - ROUNDOFF_ERROR) - { - if (deriv == zeroth) - return func->y[i] + (abscissa - func->x[i]) * func->b[i]; - else if (deriv == first) - return func->b[i] * velocity; - else - return 0.0; - } - } - } - } - else if (func->type == dpGCVSpline) - { - int l = 1; - int order = 5; //5th order for quintic spline - int half_order = (order + 1) / 2; // GCVSPL works with the half order - double work[20]; // size of work array must be >= order - - // TODO5.0: derivatives > 0 seem to be wrong (and they don't take into - // account velocity or acceleration). - return splder((int*)&deriv, &half_order, &func->numpoints, &abscissa, func->x, func->c, &l, work); - } - else // dpNaturalCubicSpline - { - n = func->numpoints; - - // Check if the abscissa is out of range of the function. If it is, - // then use the slope of the function at the appropriate end point to - // extrapolate. You do this rather than printing an error because the - // assumption is that this will only occur in relatively harmless - // situations (like a motion file that contains an out-of-range gencoord - // value). The rest of the SIMM code has many checks to clamp a gencoord - // value within its range of motion, so if you make it to this function - // and the gencoord is still out of range, deal with it quietly. - if (abscissa < func->x[0] - ROUNDOFF_ERROR) - { - if (deriv == zeroth) - return func->y[0] + (abscissa - func->x[0])*func->b[0]; - if (deriv == first) - return func->b[0]*velocity; - if (deriv == second) - return func->b[0]*acceleration; - } - else if (abscissa > func->x[n-1] + ROUNDOFF_ERROR) - { - if (deriv == zeroth) - return func->y[n-1] + (abscissa - func->x[n-1])*func->b[n-1]; - if (deriv == first) - return func->b[n-1]*velocity; - if (deriv == second) - return func->b[n-2]*acceleration; - } - - // Check to see if the abscissa is close to one of the end points - // (the binary search method doesn't work well if you are at one of the - // end points. - if (EQUAL_WITHIN_ERROR(abscissa, func->x[0])) - { - if (deriv == zeroth) - return func->y[0]; - if (deriv == first) - return func->b[0]*velocity; - if (deriv == second) - return func->b[0]*acceleration + 2.0*func->c[0]*velocity*velocity; - } - else if (EQUAL_WITHIN_ERROR(abscissa, func->x[n-1])) - { - if (deriv == zeroth) - return func->y[n-1]; - if (deriv == first) - return func->b[n-1]*velocity; - if (deriv == second) - return func->b[n-1]*acceleration + 2.0*func->c[n-1]*velocity*velocity; - } - - if (n < 3) - { - // If there are only 2 function points, then set k to zero - // (you've already checked to see if the abscissa is out of - // range or equal to one of the endpoints). - k = 0; - } - else - { - // Do a binary search to find which two points the abscissa is between. - i = 0; - j = n; - while (1) - { - k = (i+j)/2; - if (abscissa < func->x[k]) - j = k; - else if (abscissa > func->x[k+1]) - i = k; - else - break; - } - } - - dx = abscissa - func->x[k]; - - if (deriv == zeroth) - return func->y[k] + dx*(func->b[k] + dx*(func->c[k] + dx*func->d[k])); - - if (deriv == first) - return (func->b[k] + dx*(2.0*func->c[k] + 3.0*dx*func->d[k]))*velocity; - - if (deriv == second) - return (func->b[k] + dx*(2.0*func->c[k] + 3.0*dx*func->d[k]))*acceleration + - (2.0*func->c[k] + 6.0*dx*func->d[k])*velocity*velocity; - } - - return ERROR_DOUBLE; -} - - -const char* get_function_type_name(dpFunctionType funcType) -{ - switch (funcType) - { - case dpStepFunction: - return "step_function"; - case dpLinearFunction: - return "linear_function"; - case dpNaturalCubicSpline: - return "natural_cubic"; - case dpGCVSpline: - return "gcv_spline"; - case dpFunctionTypeUndefined: - default: - return "function_type_undefined"; // should probably be an error - } -} - - -const char* get_function_tag(dpFunctionType funcType, int beginEnd) -{ - // beginEnd = 0 for "begin", beginEnd = 1 for "end" - switch (funcType) - { - case dpStepFunction: - return (beginEnd) ? "endstepfunction" : "beginstepfunction"; - case dpLinearFunction: - return (beginEnd) ? "endlinearfunction" : "beginlinearfunction"; - case dpNaturalCubicSpline: - return (beginEnd) ? "endnaturalcubicspline" : "beginnaturalcubicspline"; - case dpGCVSpline: - return (beginEnd) ? "endgcvspline" : "begingcvspline"; - case dpFunctionTypeUndefined: - default: - return (beginEnd) ? "endfunction" : "beginfunction"; // old-style function definitions - } -} - -dpFunction* clone_function(ModelStruct* model, dpFunction* oldFunction, SBoolean isMuscleFunc) -{ - dpFunction f; - - malloc_function(&f, oldFunction->coefficient_array_size); - copy_function(oldFunction, &f); - - return load_simm_function(model, &f, isMuscleFunc); -} - -#if ! ENGINE -void draw_function(dpFunction* func, float lineWidth, int color, double x1, double x2, int extrapolated_color) -{ - int i, j, numSteps; - float currentLineWidth; - double stepSize, pnt[2]; - - glGetFloatv(GL_LINE_WIDTH, ¤tLineWidth); - - if (func->type == dpNaturalCubicSpline || func->type == dpGCVSpline) - numSteps = 48; - else - numSteps = 1; - - simm_color(color); - - for (i=0; inumpoints-1; i++) - { - stepSize = (func->x[i+1] - func->x[i]) / numSteps; - - glLineWidth(lineWidth); - glBegin(GL_LINE_STRIP); - - if (func->type == dpStepFunction) - { - pnt[0] = func->x[i]; - pnt[1] = func->y[i]; - glVertex2dv(pnt); - pnt[0] = func->x[i+1]; - pnt[1] = func->y[i]; - glVertex2dv(pnt); - pnt[0] = func->x[i+1]; - pnt[1] = func->y[i+1]; - glVertex2dv(pnt); - } - else - { - for (j=0; j<=numSteps; j++) - { - pnt[0] = func->x[i] + j * stepSize; - pnt[1] = interpolate_function(pnt[0], func, zeroth, 0.0, 0.0); - glVertex2dv(pnt); - } - } - - glEnd(); - } - - simm_color(extrapolated_color); - glBegin(GL_LINES); - pnt[0] = x1; - pnt[1] = interpolate_function(pnt[0], func, zeroth, 0.0, 0.0); - glVertex2dv(pnt); - pnt[0] = func->x[0]; - pnt[1] = func->y[0]; - glVertex2dv(pnt); - pnt[0] = func->x[func->numpoints-1]; - pnt[1] = func->y[func->numpoints-1]; - glVertex2dv(pnt); - pnt[0] = x2; - pnt[1] = interpolate_function(pnt[0], func, zeroth, 0.0, 0.0); - glVertex2dv(pnt); - glEnd(); - - glLineWidth(currentLineWidth); -} -#endif - - -/* return the highest user function number */ -int findHighestUserFuncNum(ModelStruct* ms) -{ - int i, highest_user_num = -1; - - for (i=0; ifunc_array_size; i++) - { - if (ms->function[i] && ms->function[i]->used == dpYes && ms->function[i]->usernum > highest_user_num) - highest_user_num = ms->function[i]->usernum; - } - - return highest_user_num; -} - - -int countUsedFunctions(ModelStruct* ms) -{ - int i, count=0; - - for (i=0; ifunc_array_size; i++) - if (ms->function[i] && ms->function[i]->used == dpYes) - count++; - - return count; -} - - -int getFunctionIndex(ModelStruct* model, dpFunction* function) -{ - int i; - - if (function) - { - for (i=0; ifunc_array_size; i++) - if (model->function[i] == function) - return i; - } - - return INVALID_FUNCTION; -} - - -dpFunction* getFunctionByUserNumber(ModelStruct* model, int userNumber) -{ - int i; - - for (i=0; ifunc_array_size; i++) - if (model->function[i] && model->function[i]->used == yes && model->function[i]->usernum == userNumber) - return model->function[i]; - - return NULL; -} - - -void save_function(ModelStruct* model, dpFunction* function) -{ - if (model && function) - { - int funcIndex = getFunctionIndex(model, function); - if (funcIndex != INVALID_FUNCTION) - { - free_function(model->save.function[funcIndex], no); - malloc_function(model->save.function[funcIndex], model->function[funcIndex]->coefficient_array_size); - copy_function(model->function[funcIndex], model->save.function[funcIndex]); - } - } -} - - -void restore_function(ModelStruct* model, dpFunction* function) -{ - if (model && function) - { - int funcIndex = getFunctionIndex(model, function); - if (funcIndex != INVALID_FUNCTION) - { - free_function(model->function[funcIndex], no); - malloc_function(model->function[funcIndex], model->save.function[funcIndex]->coefficient_array_size); - copy_function(model->save.function[funcIndex], model->function[funcIndex]); - } - } -} - - -void free_function(dpFunction* func, SBoolean freeTheFuncToo) -{ - if (func == NULL) - return; - - FREE_IFNOTNULL(func->x); - FREE_IFNOTNULL(func->y); - FREE_IFNOTNULL(func->b); - FREE_IFNOTNULL(func->c); - FREE_IFNOTNULL(func->d); - - if (freeTheFuncToo) - { - free(func); - } - else - { - func->status = dpFunctionUndefined; - func->used = dpNo; - } -} diff --git a/OpenSim/Utilities/simmToOpenSim/functions.h b/OpenSim/Utilities/simmToOpenSim/functions.h deleted file mode 100644 index 306135f411..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/functions.h +++ /dev/null @@ -1,988 +0,0 @@ -/******************************************************************************* - - FUNCTIONS.H - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef FUNCTIONS_H -#define FUNCTIONS_H - -#include "basic.h" -#include "f2c.h" - -SBoolean _confirm_overwrite (const char* filename); -void _extract_rotations_for_joint(const DMatrix m, const JointStruct*, double* rots); -void _open_files (int argc, char* argv[], SBoolean showDialogBox); -SBoolean _read_til (FILE* file, int c); -int _read_til_tokens (FILE* file, char* buf, const char* delimiters); -void _strip_outer_whitespace (char* str); -void add_default_motion_objects(ModelStruct*); -MotionObjectInstance* add_foot_print(ModelStruct* model, MotionSequence* motion, const char name[], double scale, - double translation[], double eulerRotation[]); -MotionObjectInstance* add_force_plate(ModelStruct* model, MotionSequence* motion, const char name[], double scale[], - double translation[], double eulerRotation[]); -Marker* add_marker(ModelStruct* model, SegmentStruct* segment, Marker* marker, SBoolean notify); -ReturnCode add_model(char jointfilename[], char musclefilename[], int suggested_win_width, - int* mod, SBoolean showTopLevelMessages); -void add_motion_frame(ModelStruct* ms, const char* fullpath); -SBoolean add_motion_to_model(MotionSequence* motion, ModelStruct* ms, SBoolean showTopLevelMessages); -void add_muscle_attachment_point(ModelStruct* model, int muscleIndex, int pointIndex); -PlotStruct* add_plot(void); -void add_preprocessor_option(SBoolean isDefaultOption, const char* format, ...); -void add_text_to_help_struct(char line[], HelpStruct* hp, SBoolean new_line, - int text_format, int text_xoffset); -int add_window(WindowParams* win_parameters, WinUnion* win_struct, - WindowType type, int ref, SBoolean iconified, - void (*display_function)(WindowParams*, WinUnion*), - void (*update_function)(WindowParams*, WinUnion*), - void (*input_handler)(WindowParams*, WinUnion*, SimmEvent)); -void addNameToString(char name[], char string[], int maxStringSize); -void adjust_main_menu(); -ReturnCode alloc_func(dpFunction** func, int pts); -int allocate_colormap_entry(GLfloat red, GLfloat green, GLfloat blue); -void annotate_scene(Scene* scene); -void append_if_necessary (char* str, char c); -#if OPENSMAC -MotionSequence* applyForceMattesToMotion(ModelStruct* ms, MotionSequence* motionKin, MotionSequence* motionAnalog, SBoolean addToModel); -#endif -void apply_bone_scales_to_vertices(ModelStruct*); -void apply_material(ModelStruct* ms, int mat, SBoolean highlight); -int apply_motion_to_model(ModelStruct* ms, MotionSequence* motion, double value, SBoolean update_modelview, SBoolean draw_plot); -SBoolean avoid_gl_clipping_planes(); -void background(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -int best_fit_color(GLfloat red, GLfloat green, GLfloat blue); -void boundplot(PlotStruct* p); -void build_file_filters(char fileFilter[]); -void build_file_list_from_pattern(const char* pattern, char*** list, int* numFiles); -void build_full_path(const char* prefPath, const char* filePath, char* fullPath); -void calc_camera_vector(Scene* scene, GLfloat vector[]); -void calc_foot_prints(ModelStruct* model, MotionSequence* motion, smC3DStruct* c3d); -void calc_function_coefficients(dpFunction* sf); -void calc_marker_events(MotionSequence* motion, smC3DStruct *c3d); -void calc_joint_transform(int order[], double dofs[], double rmat[][4], double xaxis[], - double yaxis[], double zaxis[], double axis1[], double axis2[], - double axis3[], int mode, Direction dir, JointStruct* joint); -void calc_motion_derivatives(MotionSequence* motion); -void calc_muscle_moment_arms(ModelStruct* ms, int musc, GeneralizedCoord* gencoord); -ReturnCode calc_muscle_tendon_force(ModelStruct* ms, int musc, SBoolean compute_active, - SBoolean compute_passive, double activation, - SBoolean isometric); -double calc_muscle_tendon_length(ModelStruct *ms, dpMuscleStruct *muscl); -double calc_muscle_tendon_velocity(ModelStruct* ms, int musc); -void calc_near_and_far_clip_planes(Scene* scene, double viewVecLength); -void calc_numerical_moment_arms(ModelStruct* ms, int musc, GeneralizedCoord* gencoord); -double calc_vector_length(ModelStruct* ms, double p1[], int frame1, double p2[], int frame2); -SBoolean can_create_file(const char filename[]); -void change_filename_suffix(const char input[], char output[], const char suffix[], int outputSize); -int check_checkboxpanel(CheckBoxPanel* check, int mx, int my); -void check_combobox(ComboBoxPanel* cbpanel, int mx, int my); -int check_combopanel(ComboBoxPanel* cbpanel, int mx, int my); -ReturnCode check_definitions(ModelStruct* ms); -int check_form(Form* form, int mx, int my); -void check_gencoord_usage(ModelStruct* ms, SBoolean change_visibility); -double check_gencoord_wrapping(GeneralizedCoord* gc, double change); -int check_menu(Menu* menu, int mx, int my); -double check_motion_wrapping(ModelStruct* ms, MotionSequence* motion, double change); -void calc_segment_arms(ModelStruct* ms, int musc, dpMusclePoint *mp1, dpMusclePoint *mp2); -SBoolean check_slider(Slider* sl, SimmEvent se, void (*callback)(int, double, double), int arg1); -SBoolean check_slider_array(SliderArray* sa, SimmEvent se, void (*callback)(int, double, double)); -int check_title_area(int title_mask, int mx, int my, WindowParams* win_params, - void** struct_ptr, ModelStruct* ms, PlotStruct* ps, - title_area_cb); -void check_wrapping_points(ModelStruct *ms, dpMuscleStruct *muscl); -void clear_preprocessor_options(); -dpFunction* clone_function(ModelStruct* model, dpFunction* oldFunction, SBoolean isMuscleFunc); -void collect_system_info(const char folder[]); -void confirm_action(WindowParams* win_parameters, char mess[], - void (*confirm_callback)(SBoolean)); -void confirm_input_handler(SimmEvent se); -int convert(ModelStruct* ms, double p[], int n1, int n2); -void convertSpacesInString(char str[]); -void convert_string(char str[], SBoolean prependUnderscore); -int convert_vector(ModelStruct* ms, double p[], int n1, int n2); -ReturnCode copy_default_muscle(dpMuscleStruct* from, dpMuscleStruct* to, ModelStruct* modelTo); -ReturnCode copy_default_muscle_dp(dpMuscleStruct* from, dpMuscleStruct* to, dpModelStruct* modelTo); -void copy_dof(DofStruct* from, DofStruct* to); -void copy_function(dpFunction* from, dpFunction* to); -void copy_gencoord_info(GeneralizedCoord *from, SaveGencoords *to); -void copy_material(MaterialStruct* src, MaterialStruct* dst); -ModelStruct* copy_model(ModelStruct* ms); -ReturnCode copy_muscle_path(dpMusclePathStruct *from, dpMusclePathStruct *to); -ReturnCode copy_muscle_point(dpMusclePoint* from, dpMusclePoint* to); -ReturnCode copy_muscle(dpMuscleStruct* from, dpMuscleStruct* to, dpMuscleStruct* deffrom, dpMuscleStruct *defto, ModelStruct* modelTo); -ReturnCode copy_muscle_dp(dpMuscleStruct* from, dpMuscleStruct* to, dpMuscleStruct* deffrom, dpMuscleStruct *defto, dpModelStruct* modelTo); -ReturnCode copy_nddouble(double* from, double** to, double* deffrom, double* defto); -ReturnCode copy_ndint(int* from, int** to, int* deffrom, int* defto); -ReturnCode copy_nndouble(double* from, double** to); -ReturnCode copy_nnint(int* from, int** to); -void copy_4x4matrix(double from[][4], double to[][4]); -void copy_1x4vector(double from[], double to[]); -void copy_point(dpCoord3D *from, dpCoord3D *to); -int countTokens(char str[]); -int count_remaining_lines(FILE*, SBoolean countEmptyLines); -void coverpage(void); -ReturnCode create_default_muscle(ModelStruct* model); -dpMuscleStruct* create_muscle(ModelStruct* model); -#if ! OPENSIM_BUILD -MotionSequence* createMotionFromTRC(ModelStruct* ms, int nq, smGenCoord gcList[], smTRCStruct *trc, - int firstFrame, int lastFrame, int numRows, char motionName[], - glutTRCOptions *options, glutRealtimeOptions *realtimeOptions, - const smC3DStruct *c3d); -#endif -MotionSequence* createMotionStruct(ModelStruct* ms); -void cropFormDisplay(FormItem* item, int rule, int cursorPosition); -int define_material(ModelStruct* ms, MaterialStruct* mat); -void deiconify_message_window(); -void delete_last_char_from_help_struct(HelpStruct* hp); -void delete_last_line_from_help_struct(HelpStruct* hp); -void delete_curves(PlotStruct* ps); -void delete_display_list(GLuint display_list, ModelStruct* model); -void delete_marker(ModelStruct* model, SegmentStruct* segment, Marker* marker, SBoolean notify); -void delete_model(ModelStruct* ms); -void delete_motion(ModelStruct* ms, MotionSequence* motion, SBoolean printMessage); -void delete_muscle(ModelStruct* model, dpMuscleStruct* muscle); -void delete_muscle_attachment_point(ModelStruct* model, int muscleIndex, int pointIndex); -void delete_plot(int plotnum); -void delete_polyhedron_display_list(PolyhedronStruct* ph, ModelStruct* model); -void delete_segment_display_lists(SegmentStruct* seg, ModelStruct* model); -void delete_window(int id); -void destroy_main_menu(); -void determine_gencoord_type(ModelStruct* model, GeneralizedCoord* gencoord); -void display_background(WindowParams* win_parameters, WinUnion* win_struct); -int divide_string(char string[], char* word_array[], int max_words); -void do_combo_box(ComboBox* cb, XYIntCoord origin, int mx, int my); -void do_be_help(); -void do_ce_help(); -void do_ge_help(); -void do_je_help(); -void do_ke_help(); -void do_mv_help(); -void do_pm_help(); -void do_se_help(); -void do_we_help(); -void do_help_window(HelpStruct* hp, char* windowName, char* iconName, - void (*displayFunction)(WindowParams*, WinUnion*), - void (*updateFunction)(WindowParams*, WinUnion*), - void (*inputHandler)(WindowParams*, WinUnion*, SimmEvent)); -int domusclemenus(ModelStruct* ms, MuscleMenu mgroup[], int mlist[], - WindowParams* win_parameters, int mx, int my, int columns[], int *menu); -void do_tool_window_shut(const char* toolname, int id); -double dot_vectors(double vector1[], double vector2[]); -void draw_all_muscle_points(ModelStruct* ms, int mx, int my); -void draw_arrow(ArrowButton* ab); -void draw_bone(Scene* scene, ModelStruct* ms, int seg_num, int bone_num, ModelDrawOptions* mdo); -void draw_bones(Scene* scene, ModelStruct* ms, ModelDrawOptions* mdo); -void draw_bone_polygon(Scene* scene, ModelStruct* ms, int seg_num, int bone_num, int polygon_num); -void draw_bounding_box_bone(PolyhedronStruct* ph, int seg_num, int bone_num, ModelDrawOptions* mdo, ModelDisplayStruct* dis); -//void draw_combo_box(ComboBox* cb); -void draw_combo(ComboBoxPanel *cb); -void draw_gouraud_bone(ModelStruct* model, PolyhedronStruct* ph, ModelDisplayStruct* dis); -void draw_flat_bone(ModelStruct* model, PolyhedronStruct* ph, ModelDisplayStruct* dis); -void draw_marker(ModelStruct* model, SegmentStruct* seg, int segment_index, int marker_index, ModelDrawOptions* mdo); -void draw_normals(ModelStruct* model, PolyhedronStruct* ph); -void draw_outlined_bone(Scene* scene, ModelStruct* model, PolyhedronStruct* ph, SegmentStruct* seg, ModelDisplayStruct* dis); -void draw_solid_fill_bone(ModelStruct* model, PolyhedronStruct* ph, int seg_num, int bone_num, ModelDrawOptions* mdo); -void draw_wireframe_bone(ModelStruct* model, PolyhedronStruct* ph, int seg_num, int bone_num, ModelDrawOptions* mdo); -void draw_bordered_rect(int x1, int y1, int x2, int y2, int fill_color, int border_color); -void draw_box(GUIObjectMode mode, int x1, int y1, int x2, int y2, int col); -void draw_checkboxpanel(CheckBoxPanel* chpanel); -void draw_confirm_window(WindowParams* win_parameters, WinUnion* win_struct); -void draw_diamond_box(GUIObjectMode mode, int x1, int y1, int x2, int y2, int col); -void draw_form(Form* form); -void draw_form_item_cursor(Form* form); -void draw_function(dpFunction* func, float lineWidth, int color, double x1, double x2, int extrapolated_color); -void draw_help_text(HelpStruct* hp); -void draw_help_window(HelpStruct* hp); -void draw_highlighted_object(Scene* scene, ModelStruct* model, PickIndex object, SBoolean highlight); -void draw_highlighted_polygon(Scene* scene, ModelStruct* ms, int seg_num, int bone_num, - int polygon_num, SBoolean highlight); -void draw_ligament(ModelStruct* ms, int lignum, ModelDrawOptions* mdo); -void draw_me_polygon(Scene* scene, ModelStruct* ms, SelectedPolygon* hp, GLenum buffer); -void draw_menu(Menu* ms); -void draw_model(Scene* scene, ModelStruct* ms, ModelDrawOptions* mdo); -void draw_motion_objects(Scene* scene, ModelStruct*, ModelDrawOptions*); -void draw_muscle_menu(Menu* ms); -void draw_muscle_point(Scene* scene, ModelStruct* ms, int musc, int pt, ModelDrawOptions* mdo, SBoolean highlight); -void draw_muscle_points(Scene* scene, ModelStruct* ms, int musc, ModelDrawOptions* mdo); -void draw_muscles(Scene* scene, ModelStruct* ms, ModelDrawOptions* mdo); -void draw_object_selection_box(Scene* scene, char text[], SBoolean for_display); -void draw_poly_muscle(Scene* scene, ModelStruct* ms, int musc, ModelDrawOptions* mdo); -void draw_plot(WindowParams* win_parameters, WinUnion* win_struct); -void draw_plotkey(WindowParams* win_parameters, WinUnion* win_struct); -void drawscene(WindowParams* win_parameters, WinUnion* win_struct); -void draw_scene(Scene* scene, ModelStruct* model, ModelDrawOptions* mdo); -void draw_slider(Slider* sl); -void draw_slider_array(SliderArray* sa); -void draw_stiffness_ellipsoids(ModelStruct* ms); -void draw_title_area(WindowParams* win_params, ModelStruct* ms, PlotStruct* ps, int title_mask); -void draw_world_object(Scene* scene, ModelStruct* model, int obj_num, ModelDrawOptions* mdo); -void drawwindows(void); -int enter_function(ModelStruct* model, int usernum, SBoolean permission_to_add); -GeneralizedCoord* enter_gencoord(ModelStruct* model, const char username[], SBoolean permission_to_add); -int enter_gencoord_group(ModelStruct* model, const char username[], GeneralizedCoord* gencoord); -int enter_muscle_group(ModelStruct* model, const char username[], int muscleIndex); -int enter_joint(ModelStruct* model, const char username[], SBoolean permission_to_add); -int enter_material(ModelStruct* ms, const char name[], EnterMode emode); -int enter_preference(const char name[], const char value[]); -int enter_segment(ModelStruct* model, const char username[], SBoolean permission_to_add); -int enter_segment_group(ModelStruct* model, const char username[], int segmentIndex); -void error(ErrorAction action, char str_buffer[]); -double evaluate_dof(ModelStruct* ms, DofStruct* var); -void evaluate_active_movingpoints(ModelStruct* ms, dpMusclePathStruct* path); -void evaluate_orig_movingpoints(ModelStruct* ms, dpMusclePathStruct* path); -void exit_simm_confirm(SBoolean answer); -SBoolean file_exists(const char filename[]); -void fill_rect(int x1, int y1, int x2, int y2); -int find_critical_marker(smMotionStruct* motionData, smOrthoTrakMarkerSet marker); -int find_curve_color(PlotStruct* ps, int num_curves); -void find_ground_joint(ModelStruct* ms); -int find_joint_between_frames(ModelStruct* ms, int from_frame, int to_frame, Direction* dir); -int find_model_ordinal(int modelnum); -int find_next_active_field(Form* form, int current_field, TextFieldAction tfa); -ModelStruct* find_nth_model(int modcount); -MotionSequence* find_nth_motion(ModelStruct* model, int motcount); -PlotStruct* find_nth_plot(int plotcount); -int find_plot_ordinal(int plotnum); -smAxes find_primary_direction(double vec[]); -ReturnCode find_segment_drawing_order(ModelStruct* ms); -int find_string_in_list(const char name[], const char* string_list[], int n); -void find_world_coords(Scene* scene, int mx, int my, double z_value, double* wx, double* wy, double* wz); -int findMarkerInModel(ModelStruct* ms, char name[], int *seg); -void frame_rect(int x1, int y1, int x2, int y2); -void freeModelStruct(ModelStruct* ms); -void free_checkbox_panel(CheckBoxPanel* panel); -void free_combobox_panel(ComboBoxPanel* combo); -void free_default_muscle(dpMuscleStruct* defmusc); -void free_form(Form* frm); -void free_function(dpFunction* func, SBoolean freeTheFuncToo); -void free_menu(Menu* mn); -void free_model(int mod); -void free_motion(MotionSequence* motion, ModelStruct* model); -void free_motion_object(MotionObject* mo, ModelStruct* ms); -void free_motion_object_instance(MotionObjectInstance* mi, ModelStruct* model); -void free_muscle(dpMuscleStruct *muscle, dpMuscleStruct* dm); -void free_muscles(ModelStruct* model); -void free_plot(int plotnum); -SBoolean gencoord_in_path(ModelStruct* ms, int n1, int n2, GeneralizedCoord* gencoord); -dpFunction* getFunctionByUserNumber(ModelStruct* model, int userNumber); -int getFunctionIndex(ModelStruct* model, dpFunction* function); -int getGencoordIndex(ModelStruct* model, GeneralizedCoord* gencoord); -int getJointIndex(ModelStruct* model, JointStruct* joint); -int getLigamentIndex(ModelStruct* model, char lig_name[]); -dpMuscleStruct* getMuscle(ModelStruct* model, int muscleIndex); -dpFunction* getMuscleFunction(ModelStruct* model, dpMuscleStruct* muscle, int functionParam); -int getMuscleIndex(ModelStruct* model, char muscleName[]); -smUnit getsmUnits(char *str); -const char* getsmUnitsString(smUnit units); -int getWindowsDialogTextWidth(char text[]); -ModelStruct* get_associated_model(int window); -PlotStruct* get_associated_plot(int window); -const char* get_bones_folder(void); -const char* get_clipboard_text(void); -const char* get_color_folder(void); -int get_color_index(char name[]); -int get_cursor_position(FormItem* item, int xpos, int currentPosition); -double get_double(Form* form, SimmEvent se, TextFieldAction* tfa); -char* get_drawmode_name(DrawingMode); -const char* get_filename_from_path(const char* pathname); -ModelStruct* get_first_model_in_scene(Scene* scene); -Scene* get_first_scene_containing_model(ModelStruct* model); -#if ! ENGINE -GLfloat* get_float_conversion(ModelStruct* ms, int joint, Direction dir); -int get_form_item_xpos(FormItem* item); -GLfloat* get_float_ground_conversion(ModelStruct* ms, int seg, GroundDirection gd); -#endif -const char* get_function_tag(dpFunctionType funcType, int beginEnd); -const char* get_function_type_name(dpFunctionType funcType); -DMatrix* get_ground_conversion(ModelStruct* ms, int seg, GroundDirection gd); -const char* get_help_folder(void); -const char* get_mocap_folder(void); -const char* get_mocap_misc_folder(void); -char* get_simmkey_name(int keynum); -char* get_mocap_model(void); -ModelStruct* get_model_by_name(const char name[]); -ModelStruct* get_modelstruct(void); -MotionSequence* get_motion_by_name(ModelStruct* model, const char name[]); -int get_motion_frame_number(MotionSequence* motion, double value); -int get_motion_number(ModelStruct* ms, MotionSequence* motion); -dpWrapObject* get_muscle_wrap_object(ModelStruct* model, dpMuscleStruct* muscle, dpMusclePoint* mpt); -int get_name_and_value_from_string(char* string, double* value); -void get_object_type_and_number(PickIndex object, PickIndex* type, PickIndex* num); -int* get_path_between_frames(ModelStruct* ms, int frame1, int frame2); -#if ! ENGINE -PlotStruct* get_plot_by_name(const char name[]); -#endif -const char* get_preference(const char name[]); -void get_pure_path_from_path (const char* fullPath, char** purePath); -Scene* get_scene(void); -int get_simm_event(SimmEvent* se); -int get_string(Form* form, SimmEvent se, TextFieldAction* tfa, SBoolean spacesAllowed); -ReturnCode get_string_pair(char str_buffer[], char str1[], char str2[]); -char* get_suffix(char str[]); -char* get_tradeshow_end(void); -void get_transform_between_segs(ModelStruct* ms, double tmat[][4], int n1, int n2); -int get_window_index(WindowType type, int ref); -dpWrapObject* get_wrap_object(ModelStruct* model, char username[]); -DMatrix* get_conversion(ModelStruct* ms, int joint, Direction dir); -char* getjointvarname(int num); -int getjointvarnum(char string[]); -int getMusclePointSegment(dpMuscleStruct *muscle, int pointIndex); -int getGlutWindowIndexByName(const char windowName[]); -int getSimmWindowIndexByNameAndType(const char windowName[], WindowType type); -void hack_tool_updates(ModelStruct* model, int model_index); -void handle_combo_selection(WindowParams* win_parameters, ComboBoxPanel *panel, int item); -void highlight_form_item(Form* form, int selected_item, SBoolean draw_cursor, SBoolean draw_in_front); -void highlight_menu_item(Menu* menu, int num, OnOffSwitch state, SBoolean draw_in_front); -void highlight_muscmenu_item(Menu* menu, int num, OnOffSwitch state, SBoolean draw_in_front); -void import_model_archive(); -void init_color_database(void); -ReturnCode init_default_muscle(ModelStruct* ms); -ReturnCode init_dynamic_param_array(dpMuscleStruct* muscle, dpMuscleStruct* default_muscle); -void init_gencoord(GeneralizedCoord* gencoord); -ReturnCode init_gencoords(ModelStruct* ms); -void init_global_lighting(void); -void init_joint(ModelStruct* ms, JointStruct* jointstr); -void init_main_menu(void); -void init_materials(ModelStruct* ms); -ReturnCode init_model(ModelStruct* ms); -void init_model_lighting(void); -void init_pick_map(void); -void init_preferences(void); -ReturnCode init_scene(Scene* sc); -void init_segment(ModelStruct* ms, SegmentStruct* seg); -void initialize(void); -void initializeHTRDataStruct(smHTRData *htrData, smModel* model, smTRCStruct *trc); -void initializeHTRFrame(smHTRFrame *htrFrame, int numSegments); -void initializeTRC(smTRCStruct *predicted, smTRCStruct *actual); -void initwrapobject(dpWrapObject* wo); -void initconstraintobject(ConstraintObject *co); -void initconstraintpoint(ConstraintPoint *pt); -ReturnCode init_model_display(ModelStruct* ms); -ReturnCode init_muscle(ModelStruct* model, dpMuscleStruct* muscle, dpMuscleStruct* default_muscle); -ReturnCode initMusclePath(dpMusclePathStruct *musclepoints); -ReturnCode init_musclepoint(dpMusclePoint *mp); -ReturnCode initplot(int plotnum); -void install_move_scene_tracker(SimmEvent); -double interpolate_function(double abscissa, dpFunction* func, Derivative deriv, double velocity, double acceleration); -void invalidate_joint_matrix(ModelStruct* model, JointStruct* joint); -void invalidate_joints_using_func(ModelStruct* ms, dpFunction* function); -void invert_3x3matrix(double matrix[][3], double inverse[][3]); -void invert_4x4matrix(double matrix[][4], double inverse[][4]); -void invert_4x4transform(double matrix[][4], double inverse[][4]); -void invert_matrix(double* matrix[], double* inverse[], int size); -SBoolean is_absolute_path(const char* pathname); -SBoolean is_in_demo_mode(); -SBoolean is_demo_model_open(int* model_index); -SBoolean is_marker_visible(double pt[]); -SBoolean is_marker_visible_meters(double pt[]); -SBoolean is_marker_data_visible(MotionSequence* motion, int motionObject, int row); -SBoolean is_model_in_scene(Scene* scene, ModelStruct* model); -RTConnection is_model_realtime(ModelStruct* ms); -SBoolean is_muscle_function_inherited(ModelStruct* model, dpMuscleStruct* muscle, int funcNum); -SBoolean is_object_selected(ModelStruct* ms, PickIndex object); -SBoolean is_preference_on(const char value[]); -SBoolean isVisible(double pt[]); -SBoolean isC3DOtherData(int index, const smC3DStruct* c3d); -void link_derivs_to_model(ModelStruct* ms, MotionSequence* motion); -void load_camera_transform(Scene* scene); -void load_double_matrix(double mat[][4]); -MotionSequence* load_motion(char filename[], int mod, SBoolean showTopLevelMessages); -dpFunction* load_simm_function(ModelStruct* model, dpFunction* func, SBoolean isMuscleFunc); -dpFunction* load_user_function(ModelStruct* model, dpFunction* func); -ReturnCode load_plot(const char* filename); -void load_preferences_file(SBoolean verbose); -#if ! OPENSIM_BUILD -ReturnCode loadTrackedFile(ModelStruct *ms, glutTRCOptions *options, smC3DStruct *c3d, SBoolean freeData); -#endif -void lock_model(ModelStruct *ms); -ReturnCode lookup_polyhedron(PolyhedronStruct*, char filename[], ModelStruct*); -FileReturnCode lookup_texture_file(PolyhedronStruct* ph, char filename[], ModelStruct* ms); -FileReturnCode lookup_texture_coord_file(PolyhedronStruct* ph, char filename[], ModelStruct* ms); -int lookup_simm_key(const char* keyname); -void lowerstr(char*); -#if ! ENGINE -void simm_event_handler(void); -void make_ambient_color(GLfloat old_color[], GLfloat new_color[]); -void make_and_queue_simm_event(unsigned int event_code, int model_index, void* struct_ptr1, void* struct_ptr2, int field1, int field2); -ComboBox* make_combo_box(char* defaultName, int x1, int y1, int x2, int y2, long menu, - void (*menuCallback)(int menuValue, void* userData), void* userData); -void make_function_menu(ModelStruct* ms); -void make_gencoord_help_text(ModelStruct* ms); -void make_force_trails(ModelStruct* model, MotionSequence* motion); -void make_marker_trails(ModelStruct* model, MotionSequence* motion); -#endif -void make_gencoord_popup_menus(ModelStruct* model); -void make_ground_conversion(ModelStruct* ms, int seg); -void make_muscle_curve_unique(ModelStruct* model, dpMuscleStruct *muscle, int funcNum); -void make_muscle_curve_inherit_default(ModelStruct* model, dpMuscleStruct *muscle, int funcNum); -#if ! ENGINE -void make_help_window(char help_file[], HelpStruct* hp, int num_lines); -void make_highlight_color(GLfloat old_color[], GLfloat new_color[]); -void make_mat_display_list(MaterialStruct*); -void make_highlight_mat_display_list(MaterialStruct*); -void make_message_port(void); -void make_motion_curve_menu(ModelStruct *model, MotionSequence* motion, int mnum); -ReturnCode make_muscle_menus(ModelStruct* ms); -ReturnCode make_selectors(void); -void make_slider(Slider* sl, SliderType type, IntBox bbox, int thumb_thickness, - double value, double min_value, double max_value, - double arrow_step, char label[], void* data); -void make_cropslider(CropSlider* sl, int originX, int originY, IntBox bbox, int thumb_thickness, double start_value, - double end_value, double min_value, double max_value, double arrow_step, - char start_label[], char end_label[], void* data); -#endif -void make_string_lower_case(char str_buffer[]); -void make_time_string(char** time_string); -void make_tools(void); -void make_wrap_cylinder(dpWrapObject* wo); -void make_wrap_torus(dpWrapObject* wo); -void make_conversion(ModelStruct* ms, int joint); -int makeDir(const char aDirName[]); -void makeMarkerMenu(ModelStruct* model); -#if ! ENGINE -void make_specular_color(GLfloat old_color[], GLfloat new_color[]); -ReturnCode makegencform(ModelStruct* ms); -ReturnCode makedynparamsform(ModelStruct* ms); -void makelabels(PlotStruct* ps); -#endif -ReturnCode makepaths(ModelStruct* ms); -ReturnCode malloc_function(dpFunction* func, int size); -int mark_unconstrained_dof(ModelStruct* ms, GeneralizedCoord* gencoord, int* jnt, int* dof, SBoolean* constrained); -void* memalloc(MEMORYBLOCK* block, int mem_size, SBoolean* moved_block); -void message(char message_str[], int format, int xoffset); -SBoolean modelHasMuscles(ModelStruct* ms); -void model_deletion_confirm(SBoolean answer); -void scene_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void movetoframe(ModelStruct* ms, int from, int to); -ReturnCode mstrcat(char* old_str[], const char append_str[]); -#if ! MEMORY_LEAK -ReturnCode mstrcpy(char* dest_str[], const char original_str[]); -#endif -SBoolean muscle_has_force_params(dpMuscleStruct* ms); -void musclemenu(ModelStruct* ms); -int name_is_body_segment(ModelStruct* ms, char name[], int* motion_object, int* component, - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd); -int name_is_forceplate(ModelStruct* ms, char name[], int* motion_object, int* component, - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd); -GeneralizedCoord* name_is_gencoord(char name[], ModelStruct* model, char suffix[], - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd); -int name_is_marker(ModelStruct* ms, char name[], int* motion_object, int* component, - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd); -int name_is_muscle(ModelStruct* ms, char name[], char suffix[], - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd); -void new_motion_file(ModelStruct* ms, const char* fullpath); -ReturnCode newplotwindow(PlotStruct* ps, char plot_name[]); -void normalize_motion(MotionSequence* motion, ModelStruct* model); -SBoolean onSameWrapObject(ModelStruct* ms, dpMuscleStruct* muscl, int pt1, int pt2); -ReturnCode open_c3d_file(const char c3dFile[], int modelIndex, SBoolean showDialogBox); -FILE* open_forceplate_calibration_file(const char dataFile[]); -FILE* open_import_variables_file(const char dataFile[]); -#if ! ENGINE -ReturnCode open_demo_arm_model(); -ReturnCode open_demo_leg_model(); -ReturnCode open_demo_neck_model(); -void open_demo_mac(); -void open_main_window(); -ReturnCode open_model_archive(char archiveFilename[], int* modelIndex); -ReturnCode open_motion_analysis_file(const char gaitFile[], int modelIndex, int numAnalogFiles, const char* analogFiles[]); -ReturnCode open_opensim_mocap_model(glutOpenSimConverterOptions* options); -ReturnCode open_opensim_trb_file(glutOpenSimConverterOptions* options); -ReturnCode open_mocap_model(SBoolean mac_demo, const char staticFile[], SBoolean showDialogBox); -ReturnCode open_tracked_file(const char gaitFile[], int modelIndex, int numAnalogFiles, const char* analogFiles[], SBoolean showDialogBox); -PickIndex pack_bone_value(int seg, int bone); -PickIndex pack_muscle_value(int musc); -PickIndex pack_point_value(int musc, int pt); -PickIndex pack_polygon_value(int bone, int polygon); -void pack_int_into_color(PickIndex value, GLubyte color_array[]); -PickIndex pack_world_value(int world_obj); -#endif -void partialvelocity(ModelStruct* ms, double p[], int from, int to, GeneralizedCoord* gencoord, dpMusclePoint *mpt, double result[]); -#if ! ENGINE -SBoolean peek_simm_event(SimmEvent* se); -int place_musclemenu(int menunum, int xoffset, int yoffset, int columns[], - int columns_needed, int column_width, MuscleMenu mgroup[]); -void place_slider(Slider*, IntBox bbox); -void plot_depthchange(int action, int win); -void plotinc(double start, double end, int* numticks, double* starttick, - double* step, char format[]); -void plotinput(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void plotkeyinput(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void plotlabels(WindowParams* win_parameters, PlotStruct* p); -void plotvport(WindowParams* win_parameters, PlotStruct* p); -void plotxadj(WindowParams* win_parameters, PlotStruct* p); -#endif -void popframe(ModelStruct* ms); -ReturnCode post_init_muscle_path(dpMusclePathStruct* path, int numWrapStructs); -void post_motion_event(ModelStruct* ms, MotionSequence* motion, int eventCode); -void post_process_bones(ModelStruct* ms); -void calc_transformation(ModelStruct* ms, int from, int to, DMatrix mat); -FILE* preprocess_file(char infile[], const char outfile[]); -void print_duration(char text_string[]); -void print_4x4matrix(double matrix[][4]); -void print_simm_event(SimmEvent se); -void print_simm_event_queue(); -void print_time(void); -void printmuscle(dpMuscleStruct* musc); -void printpaths(ModelStruct* ms); -int purge_simm_events_for_model(int model_index, unsigned int event_code); -int purge_simm_events_for_struct(void* struct_ptr, unsigned int event_code); -void pushframe(ModelStruct* ms); -void queue_model_redraw(ModelStruct* model); -void queue_window_redraw(WindowType type, int ref); -void queue_simm_event(SimmEvent se); -ReturnCode read_color_file(SBoolean refreshWindows, SBoolean verbose); -ReturnCode read_double(FILE*, double*); -ReturnCode read_double_array(FILE* fp, const char ending[], const char name[], dpFunction* func); -ReturnCode read_double_tab(FILE*, double*); -ReturnCode read_drawmode(FILE*, DrawingMode*); -int read_line(FILE* fp, char str_buffer[]); -ReturnCode read_material(ModelStruct *ms, FILE* fp); -ReturnCode read_model_file(Scene* scene, ModelStruct* ms, char filename[], SBoolean showTopLevelMessages); -ReturnCode read_muscle_attachment_points(ModelStruct* ms, FILE* fp, dpMuscleStruct *muscle); -ReturnCode read_muscle_file(ModelStruct* ms, char filename[], SBoolean* file_exists, SBoolean showTopLevelMessages); -int* read_muscle_groups(ModelStruct* ms, FILE* fp, int* num_groups, int muscle_number); -int read_nonempty_line(FILE* fp, char str_buffer[]); -CurveStruct* readplotfile(FILE* fp); -int read_string(FILE* fp, char str_buffer[]); -ReturnCode read_world_object(ModelStruct* ms, FILE* fp); -ReturnCode read_function(ModelStruct* ms, FILE* fp, SBoolean muscleFunc, dpFunctionType funcType, const char endTag[]); -FileReturnCode read_tiff_image(const char* filename, int* width, int* height, unsigned** image); -ReturnCode realloc_function(dpFunction* func, int size); -void recalc_constraint_xforms(ConstraintObject *co); -void remove_preference(const char name[]); -ReturnCode rename_muscle(ModelStruct* model, dpMuscleStruct* muscle, char newName[]); -#if ! ENGINE -ToolStruct* register_tool(int struct_size, unsigned int event_mask, - void (*event_handler)(SimmEvent), - void (*command_handler)(char*), - SBoolean (*query_handler)(QueryType, void*), - char name[], int* ref_number); -void replace_help_text(char new_help_file[], HelpStruct* hp); -void reshapewindow(void); -void resize_model_display(Scene* scene, ModelStruct*); -#endif -ReturnCode restore_all_muscles(ModelStruct* ms); -ReturnCode restore_default_muscle(ModelStruct* ms); -void restore_dofs(void); -void restore_function(ModelStruct* model, dpFunction* function); -ReturnCode restore_muscle(ModelStruct* ms, dpMuscleStruct *muscle); -ReturnCode restore_muscle_groups(ModelStruct* ms); -ReturnCode save_all_muscles(ModelStruct* ms, SBoolean verbose); -ReturnCode save_default_muscle(ModelStruct* ms); -void save_function(ModelStruct* model, dpFunction* function); -ReturnCode save_muscle(ModelStruct* ms, dpMuscleStruct *muscle, SBoolean verbose); -void save_muscle_groups(ModelStruct* ms); -void save_preferences_file(SBoolean verbose); -void scale_model(ModelStruct*, const dpCoord3D* seg_scales, ScaleModelOptions* options); -void select_form_region(Form* form, int mx, SimmEvent se); -void select_muscle_attachment(ModelStruct* model, dpMuscleStruct* muscle, int pointIndex, SBoolean state); -void set_clipboard_text(const char text[]); -void set_combobox_item(ComboBox* cb, int menuValue); -void set_gencoord_info(ModelStruct* ms); -int set_gencoord_value(ModelStruct* ms, GeneralizedCoord* gencoord, double value, SBoolean solveLoops); -void set_gencoord_velocity(ModelStruct* ms, GeneralizedCoord* gencoord, double value); -void set_hourglass_cursor(double percent); -void set_interpolated_color(int col1, int col2, double factor); -void set_ortho2(double x1, double x2, double y1, double y2); -void set_ortho2i(GLint ortho[]); -void set_ortho2o(Ortho box); -void set_plot_cursors(ModelStruct *model, MotionSequence* motion); -void set_plot_ortho(PlotStruct* p); -void set_prefform(void); -void set_qs(ModelStruct* ms); -void set_us(ModelStruct* ms); -void set_viewport(int x1, int y1, int xsize, int ysize); -void setMuscleFunction(ModelStruct* model, dpMuscleStruct* muscle, int functionParam, dpFunction* function); -void setMusclePointSegment(dpMuscleStruct *muscle, int pointIndex, int newSeg); -void setmvmenus(WindowParams* win_parameters, WinUnion* win_struct); -ReturnCode setup_motion_derivatives(MotionSequence* motion); -//dkb void setup_muscle_wrapping(dpMuscleStruct* muscl); -int shape_window(WindowParams* win, SBoolean setortho); -void show_musclemenus(ModelStruct* ms, MuscleMenu mgroup[], int mlist[]); -void show_opening_and_saving_help(); -void show_tutorial(int whichTutorial); -SBoolean show_window(const char* windowName, WindowType type); -#if ! MEMORY_LEAK -void* simm_calloc(unsigned num_elements, unsigned elem_size); -#endif -void simm_color(int index); -void simm_exit(void); -FILE* simm_fopen(const char* name, const char* mode); -FILE* simm_lookup_file(char* pathList, const char* fileName, const char* mode); -#if ! MEMORY_LEAK -void* simm_malloc(unsigned mem_size); -#endif -int simm_open(const char *name, int oflag, ...); -void simm_pre_exit(void); -int simm_printf(SBoolean hilite_text, const char* format, ...); -#if ! MEMORY_LEAK -void* simm_realloc(void* ptr, unsigned mem_size, ReturnCode* rc); -#endif -void size_plotkey(PlotStruct* ps); -void size_model(Scene* scene, ModelStruct*, BoundingCube* model_bounds); -void size_model_display(Scene* scene, ModelStruct* ms, int* suggested_width, int* suggested_height); -void simmPrintMultiLines(char string[], SBoolean hilite, int lineSize, int pixelIndent); -void smooth_motion(MotionSequence* motion, ModelStruct* model, double cutoff_frequency); -void sort_events(MotionSequence* motion, int* index); -void start_simm(); -void start_timer(void); -void stop_timer(void); -void storeDoubleInForm(FormItem* item, double value, int decimal_places); -void storeIntInForm(FormItem* item, int value); -void storeStringInForm(FormItem* item, const char str[]); -void strcat3(char dest[], const char str1[], const char str2[], const char str3[], int destSize); -int strings_equal_case_insensitive(const char str1[], const char str2[]); -int strings_equal_n_case_insensitive(const char str1[], const char str2[], int n); -void strip_brackets_from_string(char name[]); -void strip_trailing_white_space(char string[]); -int strrcspn(const char* string, const char* strCharSet); -void toggle_continuous_motion(ModelStruct* model, OnOffSwitch state); -void update_background(WindowParams* win_parameters, WinUnion* win_struct); -void update_scene(WindowParams* win_parameters, WinUnion* win_struct); -void update_drawmode_menus(ModelStruct* ms); -void update_drawplot(WindowParams* win_parameters, WinUnion* win_struct); -void update_drawplotkey(WindowParams* win_parameters, WinUnion* win_struct); -void update_ligament_path(ModelStruct* model, LigamentStruct* ligament); -void updatemodelmenu(void); -void updateplotmenu(void); -void unlock_model(ModelStruct *ms); -void unpack_bone_value(PickIndex value, int* seg, int* bone); -void unpack_muscle_value(PickIndex value, int* musc); -void unpack_point_value(PickIndex value, int* musc, int* pt); -void unpack_polygon_value(PickIndex value, int* bone, int* polygon); -void unpack_int_from_color(PickIndex *value, GLubyte color_array[]); -void unpack_world_value(PickIndex value, int* world_obj); -void update_muscle_path(ModelStruct* model, dpMuscleStruct* muscle); -void upperstr(char*); -void verify_key(void); -int which_window(int gid); -ReturnCode write_motion(MotionSequence *motion, const char filename[]); -ReturnCode write_opensim_model(ModelStruct* ms, char filename[], char geometryDirectory[], - const char* markerSetOut, int angleUnits); -#if EXPERIMENTAL -ReturnCode write_motion_with_deriv(MotionSequence *motion, const char filename[]); -#endif -void write_motion_object(MotionObject*, FILE*); -ReturnCode write_trc_file(smMotionStruct* data, const char filename[]); - -char* get_snapshot_filename(Scene* scene, char* buf); -void write_tif_frame(Scene* scene); - -/******* Function Prototypes for About Box window *******/ -void show_about_box(); - -/******* Function Prototypes for JointEditor *******/ -void make_jointeditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void jointeditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_jointeditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_jointeditor(WindowParams* win_parameters, WinUnion* win_struct); -void je_simm_event_handler(SimmEvent se); -SBoolean je_query_handler(QueryType, void* data); -void draw_je_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void je_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_je_help_text(int dummy_int, double slider_value, double delta); - -/******* Function Prototypes for GencoordEditor *******/ -void make_gencoordeditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void gencoordeditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_gencoordeditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_gencoordeditor(WindowParams* win_parameters, WinUnion* win_struct); -void ge_simm_event_handler(SimmEvent se); -SBoolean ge_query_handler(QueryType, void* data); -void draw_ge_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void ge_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_ge_help_text(int dummy_int, double slider_value, double delta); -void ge_set_default_value(GeneralizedCoord* gencoord, double value); - -/******* Function Prototypes for MusclePointEditor *******/ -void make_muscleeditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void muscleeditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_muscleeditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_muscleeditor(WindowParams* win_parameters, WinUnion* win_struct); -void me_simm_event_handler(SimmEvent se); -SBoolean me_query_handler(QueryType, void* data); -void draw_me_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void me_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_me_help_text(int dummy_int, double slider_value, double delta); -public void slide_me(int arg1, double value, double delta); - -/******* Function Prototypes for MarkerEditor *******/ -void make_markereditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void markereditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_markereditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_markereditor(WindowParams* win_parameters, WinUnion* win_struct); -void ke_simm_event_handler(SimmEvent se); -SBoolean ke_query_handler(QueryType query, void* data); -void draw_ke_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void ke_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_ke_help_text(int dummy_int, double slider_value, double delta); - - -/******* Function Prototypes for PlotMaker *******/ -void make_plotmaker(int rootWindowX, int rootWindowY, SBoolean iconified); -void plotmaker(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_plotmaker(WindowParams* win_parameters, WinUnion* win_struct); -void update_plotmaker(WindowParams* win_parameters, WinUnion* win_struct); -void pm_simm_event_handler(SimmEvent se); -SBoolean pm_query_handler(QueryType, void* data); -void draw_pm_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void pm_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_pm_help_text(int dummy_int, double slider_value, double delta); -public void slide_pm(int arg1, double value, double delta); -public void pm_command_handler(char command[]); - -/* ------------------------------------------------------------------------- - CURVE_VALUE - this macro is the only way to access a curve's values. - It allows curve value arrays to be circularized so that realtime data - can be continuously streamed into them. -- added KMS 2/25/00 ----------------------------------------------------------------------------- */ -int circularize_curve_index(CurveStruct*, void* array, int val); - -#define CIRCULARIZE_CURVE_INDEX(CURVE, ARRAY, INDEX) \ - (((CURVE)->realtime_circular_index == 0 /*|| (CURVE)->ARRAY == (CURVE)->xvalues*/) ? \ - (INDEX) : circularize_curve_index(CURVE, (CURVE)->ARRAY, INDEX)) - -#define CURVE_VALUE(CURVE, ARRAY, INDEX) \ - (((double*) (CURVE)->ARRAY)[CIRCULARIZE_CURVE_INDEX(CURVE, ARRAY, INDEX)]) - - -/******* Function Prototypes for Model Viewer *******/ -void make_modelviewer(int rootWindowX, int rootWindowY, SBoolean iconified); -void modelviewer(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_modelviewer(WindowParams* win_parameters, WinUnion* win_struct); -void update_modelviewer(WindowParams* win_parameters, WinUnion* win_struct); -void mv_simm_event_handler(SimmEvent se); -SBoolean mv_query_handler(QueryType, void* data); -void draw_mv_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void mv_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_mv_help_text(int dummy_int, double slider_value, double delta); -void slide_gencoord(int genc, double value, double delta); -void slide_mv(int arg1, double value, double delta); -void reposition_gencoord_sliders(ModelStruct*); -void mv_startstop_printf(ModelStruct*, SBoolean hiliteButton, const char* format, ...); - -/******* Function Prototypes for Muscle Editor *******/ -void makeUniqueMuscleName(ModelStruct* model, char name[]); -SBoolean isValidMuscleName(ModelStruct* model, char name[]); -public void me_command_handler(char command[]); - -/******* Function Prototypes for file writing *******/ -void write_plot_file(PlotStruct*, PLOTFILEFORMAT, const char* fullpath); -void write_model_joints(ModelStruct*, const char* fullpath, SBoolean showMessage); -void write_model_muscles(ModelStruct*, const char* fullpath, SBoolean showMessage); - - -/******* Function Prototypes for Preferences *******/ -void make_preferences(int rootWindowX, int rootWindowY, SBoolean iconified); -void preferences(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_preferences(WindowParams* win_parameters, WinUnion* win_struct); -void update_preferences(WindowParams* win_parameters, WinUnion* win_struct); -void pr_simm_event_handler(SimmEvent se); -SBoolean pr_query_handler(QueryType, void* data); -void draw_pr_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void pr_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_pr_help_text(int dummy_int, double slider_value, double delta); - - -/******* Function Prototypes for GaitLoader *******/ -void make_gaitloader(int rootWindowX, int rootWindowY, SBoolean iconified); -void init_gaitloader_struct(void* buffer, int ref); -void gaitloader(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_gaitloader(WindowParams* win_parameters, WinUnion* win_struct); -void update_gaitloader(WindowParams* win_parameters, WinUnion* win_struct); -void gl_simm_event_handler(SimmEvent se); -SBoolean gl_query_handler(QueryType, void* data); -void draw_gl_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void gl_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_gl_help_text(int dummy_int, double slider_value, double delta); - - -/******* Function Prototypes for the Bone Editor *******/ -public void make_boneeditor(int rootWindowX, int rootWindowY, SBoolean iconified); -public void boneeditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -public void display_boneeditor(WindowParams* win_parameters, WinUnion* win_struct); -public void update_boneeditor(WindowParams* win_parameters, WinUnion* win_struct); -public void be_simm_event_handler(SimmEvent se); -SBoolean be_query_handler(QueryType, void* data); -public void slide_boneed(int arg1, double value, double delta); -public void draw_be_help_window(WindowParams* win_parameters, WinUnion* win_struct); -public void move_be_help_text(int dummy_int, double slider_value, double delta); -public void be_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -public void draw_bone_editor_stuff(Scene* scene, ModelStruct* ms, ModelDrawOptions* mdo); -public void be_command_handler(char command[]); - - -/******* Function Prototypes for Motion Module help *******/ -SBoolean is_mac_demo_open(void); - -/******* Function Prototypes for WrapEditor *******/ -void make_wrapeditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void wrapeditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_wrapeditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_wrapeditor(WindowParams* win_parameters, WinUnion* win_struct); -void we_simm_event_handler(SimmEvent se); -SBoolean we_query_handler(QueryType, void* data); -void draw_we_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void we_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_we_help_text(int dummy_int, double slider_value, double delta); -public void slide_we(int arg1, double value, double delta); - -/******* Function Prototypes for ConstraintEditor *******/ -void make_constrainteditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void constrainteditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_constrainteditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_constrainteditor(WindowParams* win_parameters, WinUnion* win_struct); -void ce_simm_event_handler(SimmEvent se); -SBoolean ce_query_handler(QueryType, void* data); -void draw_ce_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void ce_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_ce_help_text(int dummy_int, double slider_value, double delta); -public void slide_ce(int arg1, double value, double delta); - - -/******* Function Prototypes for Segment Editor *********/ -public void se_simm_event_handler(SimmEvent sevent); -SBoolean se_query_handler(QueryType, void* data); -void display_segmenteditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_segmenteditor(WindowParams* win_parameters, WinUnion* win_struct); -void make_segmenteditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void segmenteditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void draw_se_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void se_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_se_help_text(int dummy_int, double slider_value, double delta); -//public void slide_se(int arg1, double value, double delta); - -/******* Function Prototypes for DP Tool *********/ -public void dp_simm_event_handler(SimmEvent se); -SBoolean dp_query_handler(QueryType, void* data); -void display_dptool(WindowParams* win_parameters, WinUnion* win_struct); -void update_dptool(WindowParams* win_parameters, WinUnion* win_struct); -void make_dptool(int rootWindowX, int rootWindowY, SBoolean iconified); -void dptool(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void draw_dptool_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void dptool_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_dptool_help_text(int dummy_int, double slider_value, double delta); -void do_dptool_help(void); -//public void slide_dp(int arg1, double value, double delta); - - -/******* Function Prototypes for DeformEditor *******/ -void write_deformities(FILE*, ModelStruct*); -void write_deform(FILE*, SegmentStruct*, DeformObject*); -ReturnCode read_deform(FILE*, SegmentStruct*, int segmentnum); -ReturnCode read_deformity(ModelStruct*, FILE*); - -/******* Function Prototypes for MotionEditor *******/ -void make_motioneditor(int rootWindowX, int rootWindowY, SBoolean iconified); -void motioneditor(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void display_motioneditor(WindowParams* win_parameters, WinUnion* win_struct); -void update_motioneditor(WindowParams* win_parameters, WinUnion* win_struct); -void mt_simm_event_handler(SimmEvent se); -SBoolean mt_query_handler(QueryType, void* data); -void draw_mt_help_window(WindowParams* win_parameters, WinUnion* win_struct); -void mt_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -void move_mt_help_text(int dummy_int, double slider_value, double delta); -public void slide_mt(int arg1, double value, double delta); -public void mt_command_handler(char command[]); - - -#if INCLUDE_COMMAND_PARSER -/******* Function Prototypes for the Command Parser *******/ -public void make_commandparser(int rootWindowX, int rootWindowY, SBoolean iconified); -public void cp_simm_event_handler(SimmEvent se); -public SBoolean cp_query_handler(QueryType query, void* data); -public void commandparser(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -public void display_commandparser(WindowParams* win_parameters, WinUnion* win_struct); -public void update_commandparser(WindowParams* win_parameters, WinUnion* win_struct); -public void move_command_text(int dummy_int, double slider_value, double delta); -public void draw_cp_help_window(WindowParams* win_parameters, WinUnion* win_struct); -public void cp_help_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -public void move_cp_help_text(int dummy_int, double slider_value, double delta); -public int execute_simm_command(void); -#endif - -/******* Function Prototypes for the Message Window *******/ -public void draw_message_window(WindowParams* win_parameters, WinUnion* win_struct); -public void update_message_window(WindowParams* win_parameters, WinUnion* win_struct); -public void messages_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se); -public void move_message_text(int dummy_int, double slider_value, double delta); - -/******* Function Prototypes for the persistent SIMM state mechanism *******/ -void load_simm_state(); -void save_simm_state(); - -const char* get_simm_state_value(const char* name); -void set_simm_state_value(const char* name, const char* value); - - -int convertNEW(ModelStruct* ms, double p[], int path[], int len); -int findHighestUserFuncNum(ModelStruct* ms); -int countUsedFunctions(ModelStruct* ms); - -/********************** Function Prototypes in IK_solver *******************/ -int lmdif_(int (*fcn) (void *data, int *m, int *n, double x[], double fvec[], - int *iflag), integer *m, integer *n, doublereal *x, - doublereal *fvec, - doublereal *ftol, doublereal *xtol, doublereal *gtol, - integer *maxfev, - doublereal *epsfcn, doublereal *diag, integer *mode, doublereal *factor, - integer *nprint, integer *info, - integer *nfev, doublereal *fjac, integer *ldfjac, integer *ipvt, - doublereal *qtf, doublereal *wa1, doublereal *wa2, doublereal *wa3, - doublereal *wa4, logical *firstframe, doublereal *qrjac, - doublereal *rdiag, void *data); -int hessian_(integer *nres, int *nq); -int hessdecomp_(integer *ierror, int *nq); -int dpodi_(doublereal *a, integer *lda, integer *n, doublereal *det, - integer *job); -int dpofa_(doublereal *a, integer *lda, integer *n, integer *info); - - -/**************** Function Prototypes for loop and constraint solving *******/ -SBoolean loopsToSolve(ModelStruct *model); -SBoolean constraintsToSolve(ModelStruct *model); - -/************** Function Prototypes in IK.c **********************/ -SBoolean resolveClosedLoops(ModelStruct *ms, int q_num, double *q_value); -void calculateResiduals(void *data, int *nres, int *ndof, double q[], - double resid[], int *iflag); - -/**************** Function Prototypes in Loop.c ************************/ -int getLoopPath(int seg, int *path, int dir, LoopStruct *loop); -int setGencoordValue2(ModelStruct *ms, GeneralizedCoord* gencoord, double value); -void checkGencoordRange(GeneralizedCoord* gencoord, double *value); -SBoolean makeLoops(ModelStruct *ms); -void markLoopJoints(ModelStruct *ms); - -/**************** Function Prototypes in constraint.c ************************/ -void calculateConstraintResids(void *data, int numQ, double q[], int numResid, - double resid[], int startIndex, int endIndex, - int *iflag); -void updateConstraintInfo(ModelStruct *ms); -void markAffectedGencoords(ModelStruct *ms); - - -/**************** Function Prototypes in LCSolver.c ************************/ -void solve_initial_loops_and_constraints(ModelStruct *model); -void recalc_default_loops_and_constraints(ModelStruct *ms); -void solveAllLoopsAndConstraints(ModelStruct *ms, LoopStatus *loopStatus, - ConstraintStatus *constraintStatus, - SBoolean enforce_constraints); -SBoolean solveLCAffectedByJNT(ModelStruct *ms, int joint, LoopStatus *loopStatus, ConstraintStatus *constraintStatus); -SBoolean solveLCAffectedByGC(ModelStruct *ms, GeneralizedCoord* controlled_gc, double *gc_value); -void approveNewDefaultGCs(ModelStruct *ms); -void evaluateLoopsAndConstraintsInCurrentConfiguration(ModelStruct *ms, LoopStatus *loopStatus, - ConstraintStatus *constraintStatus, - SBoolean enforce_constraints); -int convert2(ModelStruct *ms, double p[], int n1, int n2); - -#endif /*FUNCTIONS_H*/ - diff --git a/OpenSim/Utilities/simmToOpenSim/gcvspl.c b/OpenSim/Utilities/simmToOpenSim/gcvspl.c deleted file mode 100644 index 775a4a8e3d..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/gcvspl.c +++ /dev/null @@ -1,1164 +0,0 @@ -/********************************************************************** - - GCVSPL - - Purpose: - ******* - - Natural B-spline data smoothing subroutine, using the Generali- - zed Cross-Validation and Mean-Squared Prediction Error Criteria - of Craven & Wahba (1979). - - Acknowledgements: - ********** - - This code was originally implemented in Fortran by Herman Woltring, - and then translated to C by Dwight Meglan, using the f2c program. - It was then integrated into the Dynamics Pipeline and tested by - Kelly Rooney and B.J. Fregly, at the University of Florida. - - Remarks: - ******* - - (1) GVCSPL calculates a natural spline of order 2*M (degree - 2*M-1) which smoothes or interpolates a given set of data - points, using statistical considerations to determine the - amount of smoothing required (Craven & Wahba, 1979). If the - error variance is a priori known, it should be supplied to - the routine in VAR. The degree of smoothing is then deter- - mined to minimize an unbiased estimate of the true mean - squared error. On the other hand, if the error variance is - not known, VAR should be set to a negative number. The - routine then determines the degree of smoothing to minimize - the generalized cross validation function. This is asympto- - tically the same as minimizing the true mean squared error - (Craven & Wahba, 1979). In this case, an estimate of the - error variance is returned in VAR which may be compared - with any a priori, approximate estimates. In either case, - an estimate of the true mean square error is returned in - WK(4). - - (2) The number of arithmetic operations and the amount of - storage required are both proportional to n, so very large - datasets may be accomodated. The data points do not have - to be equidistant in the independant variable X or uniformly - weighted in the dependant variable Y. - - (3) When VAR is a priori known, any value of N.ge.2*M is - acceptable. It is advisable, however, for N to be rather - large (if M.eq.2, about 20) when VAR is unknown. If the - degree of smoothing done by GCVSPL when VAR is unknown - is not satisfactory, the user should try specifying the - degree of smoothing by setting VAR to a reasonable value. - - (4) GCVSPL calculates the spline coefficient array C(N); - this array can be used to calculate the spline function - value and any of its derivatives up to the degree 2*M-1 - at any argument T within the knot range, using subroutines - SPLDER and SEARCH, and the knot array X(N). Since the - spline is constrained at its Mth derivative, only the - lower spline derivatives will tend to be reliable estim- - ates of the underlying signal's true derivatives. - - (5) GCVSPL combines elements of subroutine CRVO5 by Utreras - (1980), subroutine SMOOTH by Lyche et al. (1983), and - subroutine CUBGCV by Hutchinson (1985). The trace of the - influence matrix is assessed in a similar way as described - by Hutchinson & de Hoog (1985). The major difference is - that the present approach utilizes non-symmetrical B-spline - design matrices as described by Lyche et al. (1983); there- - fore, the original algorithm by Erisman & Tinney (1975) has - been used, rather than the symmetrical version adopted by - Hutchinson & de Hoog. - - References: - ********** - - P. Craven & G. Wahba (1979), Smoothing noisy data with - spline functions. Numerische Mathematik 31, 377-403. - - A.M. Erisman & W.F. Tinney (1975), On computing certain - elements of the inverse of a sparse matrix. Communications - of the ACM 18(3), 177-179. - - M.F. Hutchinson & F.R. de Hoog (1985), Smoothing noisy data - with spline functions. Numerische Mathematik 47(1) [in print]. - - M.F. Hutchinson (1985), Subroutine CUBGCV. CSIRO Division of - Mathematics and Statistics, P.O. Box 1965, Canberra, ACT 2601, - Australia. - - T. Lyche, L.L. Schumaker, & K. Sepehrnoori (1983), Fortran - subroutines for computing smoothing and interpolating natural - splines. Advances in Engineering Software 5(1), 2-5. - - F. Utreras (1980), Un paquete de programas para ajustar curvas - mediante funciones spline. Informe Tecnico MA-80-B-209, Depar- - tamento de Matematicas, Faculdad de Ciencias Fisicas y Matema- - ticas, Universidad de Chile, Santiago. - -**********************************************************************/ - -#include "f2c.h" - -typedef double dbl; -typedef long int inl; - -int gcvspl(dbl *, dbl *, inl *, dbl *, dbl *, inl *, inl *, - inl *, inl *, dbl *, dbl *, inl *, dbl *, inl *); - -dbl splder(inl *, inl *, inl *, dbl *, dbl *, dbl *, inl *, dbl *); - - -/* Table of constant values */ - -static doublereal c_b6 = 1e-15; - -int gcvspl(doublereal *x, doublereal *y, integer *ny, - doublereal *wx, doublereal *wy, integer *m, integer *n, integer *k, - integer *md, doublereal *val, doublereal *c, integer *nc, doublereal * - wk, integer *ier) -{ - static integer m2 = 0; - static integer nm1 = 0; - static doublereal el = 0.; - - /* System generated locals */ - integer y_dim1, y_offset, c_dim1, c_offset, i__1; - - /* Local variables */ - static integer nm2m1, nm2p1; - extern doublereal splc(integer *, integer *, integer *, doublereal *, - integer *, doublereal *, doublereal *, integer *, doublereal *, - doublereal *, doublereal *, doublereal *, integer *, doublereal *, - doublereal *, doublereal *, doublereal *, doublereal *); - extern /* Subroutine */ int prep(integer *, integer *, doublereal *, - doublereal *, doublereal *, doublereal *); - static integer i, j; - static doublereal alpha; - extern /* Subroutine */ int basis(integer *, integer *, doublereal *, - doublereal *, doublereal *, doublereal *); - static doublereal r1, r2, r3, r4; - static integer ib; - static doublereal gf2, gf1, gf3, gf4; - static integer iwe; - static doublereal err; - - /* Parameter adjustments */ - --wk; - - c_dim1 = *nc; - c_offset = c_dim1 + 1; - c -= c_offset; - - --wy; - --wx; - - y_dim1 = *ny; - y_offset = y_dim1 + 1; - y -= y_offset; - - --x; - - /* Function Body */ - - *ier = 0; - if (abs(*md) > 4 || *md == 0 || abs(*md) == 1 && *val < 0. || abs(*md) == 3 && - *val < 0. || abs(*md) == 4 && (*val < 0. || *val > (doublereal) (*n - *m))) - { - *ier = 3; - return 0; - } - if (*md > 0) { - m2 = *m << 1; - nm1 = *n - 1; - } else { - if (m2 != *m << 1 || nm1 != *n - 1) { - *ier = 3; - return 0; - } - } - if (*m <= 0 || *n < m2) { - *ier = 1; - return 0; - } - if (wx[1] <= 0.) { - *ier = 2; - } - i__1 = *n; - for (i = 2; i <= i__1; ++i) { - if (wx[i] <= 0. || x[i - 1] >= x[i]) { - *ier = 2; - } - if (*ier != 0) { - return 0; - } - } - i__1 = *k; - for (j = 1; j <= i__1; ++j) { - if (wy[j] <= 0.) { - *ier = 2; - } - if (*ier != 0) { - return 0; - } - } - - - nm2p1 = *n * (m2 + 1); - nm2m1 = *n * (m2 - 1); - ib = nm2p1 + 7; - iwe = ib + nm2m1; - - if (*md > 0) { - basis(m, n, &x[1], &wk[ib], &r1, &wk[7]); - prep(m, n, &x[1], &wx[1], &wk[iwe], &el); - el /= r1; - } - if (abs(*md) != 1) { - goto L20; - } - r1 = *val; - goto L100; - - -L20: - if (*md < -1) { - r1 = wk[4]; - } else { - r1 = 1. / el; - } - r2 = r1 * 2.; - gf2 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r2, & - c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]); -L40: - gf1 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r1, & - c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]); - if (gf1 > gf2) { - goto L50; - } - if (wk[4] <= 0.) { - goto L100; - } - r2 = r1; - gf2 = gf1; - r1 /= 2.; - goto L40; -L50: - r3 = r2 * 2.; -L60: - gf3 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, & - c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]); - if (gf3 > gf2) { - goto L70; - } - if (wk[4] >= 999999999999999.88) { - goto L100; - } - r2 = r3; - gf2 = gf3; - r3 *= 2.; - goto L60; -L70: - r2 = r3; - gf2 = gf3; - alpha = (r2 - r1) / 1.618033983; - r4 = r1 + alpha; - r3 = r2 - alpha; - gf3 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, & - c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]); - gf4 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r4, & - c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]); -L80: - if (gf3 <= gf4) { - r2 = r4; - gf2 = gf4; - err = (r2 - r1) / (r1 + r2); - if (err * err + 1. == 1. || err <= 1e-6) { - goto L90; - } - r4 = r3; - gf4 = gf3; - alpha /= 1.618033983; - r3 = r2 - alpha; - gf3 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, & - c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7] - ); - } else { - r1 = r3; - gf1 = gf3; - err = (r2 - r1) / (r1 + r2); - if (err * err + 1. == 1. || err <= 1e-6) { - goto L90; - } - r3 = r4; - gf3 = gf4; - alpha /= 1.618033983; - r4 = r1 + alpha; - gf4 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r4, & - c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7] - ); - } - goto L80; -L90: - r1 = (r1 + r2) * .5; - - -L100: - gf1 = splc(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r1, &c_b6, - &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]); - - return 0; - -} - - - -/* BASIS.FOR, 1985-06-03 */ - -int basis(integer *m, integer *n, doublereal *x, doublereal *b, doublereal *bl, - doublereal *q) -{ - /* System generated locals */ - integer b_dim1, b_offset, q_offset, i__1, i__2, i__3, i__4; - doublereal d__1; - - /* Local variables */ - static integer nmip1, i, j, k, l; - static doublereal u, v, y; - static integer j1, j2, m2, ir, mm1, mp1; - static doublereal arg; - - /* Parameter adjustments */ - q_offset = 1 - *m; - q -= q_offset; - - b_dim1 = *m - 1 - (1 - *m) + 1; - b_offset = 1 - *m + b_dim1; - b -= b_offset; - - --x; - - if (*m == 1) { - i__1 = *n; - for (i = 1; i <= i__1; ++i) { - b[i * b_dim1] = 1.; - } - *bl = 1.; - return 0; - } - - mm1 = *m - 1; - mp1 = *m + 1; - m2 = *m << 1; - i__1 = *n; - for (l = 1; l <= i__1; ++l) { - i__2 = *m; - for (j = -mm1; j <= i__2; ++j) { - q[j] = 0.; - } - q[mm1] = 1.; - if (l != 1 && l != *n) { - q[mm1] = 1. / (x[l + 1] - x[l - 1]); - } - arg = x[l]; - i__2 = m2; - for (i = 3; i <= i__2; ++i) { - ir = mp1 - i; - v = q[ir]; - if (l < i) { - i__3 = i; - for (j = l + 1; j <= i__3; ++j) { - u = v; - v = q[ir + 1]; - q[ir] = u + (x[j] - arg) * v; - ++ir; - } - } - i__3 = l - i + 1; - j1 = max(i__3,1); - i__3 = l - 1, i__4 = *n - i; - j2 = min(i__3,i__4); - if (j1 <= j2) { - if (i < m2) { - i__3 = j2; - for (j = j1; j <= i__3; ++j) { - y = x[i + j]; - u = v; - v = q[ir + 1]; - q[ir] = u + (v - u) * (y - arg) / (y - x[j]); - ++ir; - } - } else { - i__3 = j2; - for (j = j1; j <= i__3; ++j) { - u = v; - v = q[ir + 1]; - q[ir] = (arg - x[j]) * u + (x[i + j] - arg) * v; - ++ir; - } - } - } - nmip1 = *n - i + 1; - if (nmip1 < l) { - i__3 = l - 1; - for(j = nmip1; j <= i__3; ++j) { - u = v; - v = q[ir + 1]; - q[ir] = (arg - x[j]) * u + v; - ++ir; - } - } - } - i__2 = mm1; - for (j = -mm1; j <= i__2; ++j) { - b[j + l * b_dim1] = q[j]; - } - } - - i__1 = mm1; - for (i = 1; i <= i__1; ++i) { - i__2 = mm1; - for (k = i; k <= i__2; ++k) { - b[-k + i * b_dim1] = 0.; - b[k + (*n + 1 - i) * b_dim1] = 0.; - } - } - - *bl = 0.; - i__1 = *n; - for (i = 1; i <= i__1; ++i) { - i__2 = mm1; - for (k = -mm1; k <= i__2; ++k) { - *bl += (d__1 = b[k + i * b_dim1], abs(d__1)); - } - } - *bl /= *n; - - return 0; -} - - - -/* PREP.FOR, 1985-07-04 */ - -int prep(integer *m, integer *n, doublereal *x, doublereal *w, doublereal *we, - doublereal *el) -{ - /* System generated locals */ - integer i__1, i__2, i__3; - doublereal d__1; - - /* Local variables */ - static doublereal f; - static integer i, j, k, l; - static doublereal y, f1; - static integer i1, i2, m2; - static doublereal ff; - static integer jj, jm, kl, nm, ku; - static doublereal wi; - static integer n2m, mp1, i2m1, inc, i1p1, m2m1, m2p1; - - /* Parameter adjustments */ - --we; - --w; - --x; - - /* Function Body */ - m2 = *m << 1; - mp1 = *m + 1; - m2m1 = m2 - 1; - m2p1 = m2 + 1; - nm = *n - *m; - f1 = -1.; - if (*m != 1) { - i__1 = *m; - for (i = 2; i <= i__1; ++i) { - f1 = -f1 * i; - } - i__1 = m2m1; - for (i = mp1; i <= i__1; ++i) { - f1 *= i; - } - } - - i1 = 1; - i2 = *m; - jm = mp1; - i__1 = *n; - for (j = 1; j <= i__1; ++j) { - inc = m2p1; - if (j > nm) { - f1 = -f1; - f = f1; - } else { - if (j < mp1) { - inc = 1; - f = f1; - } else { - f = f1 * (x[j + *m] - x[j - *m]); - } - } - if (j > mp1) { - ++i1; - } - if (i2 < *n) { - ++i2; - } - jj = jm; - ff = f; - y = x[i1]; - i1p1 = i1 + 1; - i__2 = i2; - for (i = i1p1; i <= i__2; ++i) { - ff /= y - x[i]; - } - we[jj] = ff; - jj += m2; - i2m1 = i2 - 1; - if (i1p1 <= i2m1) { - i__2 = i2m1; - for (l = i1p1; l <= i__2; ++l) { - ff = f; - y = x[l]; - i__3 = l - 1; - for (i = i1; i <= i__3; ++i) { - ff /= y - x[i]; - } - i__3 = i2; - for (i = l + 1; i <= i__3; ++i) { - ff /= y - x[i]; - } - we[jj] = ff; - jj += m2; - } - } - ff = f; - y = x[i2]; - i__2 = i2m1; - for (i = i1; i <= i__2; ++i) { - ff /= y - x[i]; - } - we[jj] = ff; - jj += m2; - jm += inc; - } - - kl = 1; - n2m = m2p1 * *n + 1; - i__1 = *m; - for (i = 1; i <= i__1; ++i) { - ku = kl + *m - i; - i__2 = ku; - for (k = kl; k <= i__2; ++k) { - we[k] = 0.; - we[n2m - k] = 0.; - } - kl += m2p1; - } - - jj = 0; - *el = 0.; - i__1 = *n; - for (i = 1; i <= i__1; ++i) { - wi = w[i]; - i__2 = m2p1; - for (j = 1; j <= i__2; ++j) { - ++jj; - we[jj] /= wi; - *el += (d__1 = we[jj], abs(d__1)); - } - } - *el /= *n; - - return 0; -} - - - -/* SPLC.FOR, 1985-12-12 */ - -doublereal splc(integer *m, integer *n, integer *k, doublereal *y, integer * - ny, doublereal *wx, doublereal *wy, integer *mode, doublereal *val, - doublereal *p, doublereal *eps, doublereal *c, integer *nc, - doublereal *stat, doublereal *b, doublereal *we, doublereal *el, - doublereal *bwe) -{ - /* System generated locals */ - integer y_dim1, y_offset, c_dim1, c_offset, b_dim1, b_offset, we_dim1, - we_offset, bwe_dim1, bwe_offset, i__1, i__2, i__3, i__4; - doublereal ret_val, d__1; - - /* Local variables */ - static integer i, j, l; - extern doublereal trinv(doublereal *, doublereal *, integer *, integer *) - ; - static doublereal dp; - static integer km; - static doublereal dt; - static integer kp; - extern /* Subroutine */ int bandet(doublereal *, integer *, integer *), - bansol(doublereal *, doublereal *, integer *, doublereal *, - integer *, integer *, integer *, integer *); - static doublereal pel, esn, trn; - -/* *** Check on p-value */ - - /* Parameter adjustments */ - bwe_dim1 = *m - (-(*m)) + 1; - bwe_offset = -(*m) + bwe_dim1; - bwe -= bwe_offset; - - we_dim1 = *m - (-(*m)) + 1; - we_offset = -(*m) + we_dim1; - we -= we_offset; - - b_dim1 = *m - 1 - (1 - *m) + 1; - b_offset = 1 - *m + b_dim1; - b -= b_offset; - - --stat; - - c_dim1 = *nc; - c_offset = c_dim1 + 1; - c -= c_offset; - - --wy; - --wx; - - y_dim1 = *ny; - y_offset = y_dim1 + 1; - y -= y_offset; - - /* Function Body */ - dp = *p; - stat[4] = *p; - pel = *p * *el; - if (pel < *eps) { - dp = *eps / *el; - stat[4] = 0.; - } - if (pel * *eps > 1.) { - dp = 1. / (*el * *eps); - stat[4] = dp; - } - - i__1 = *n; - for (i = 1; i <= i__1; ++i) { - i__2 = *m, i__3 = i - 1; - km = -min(i__2,i__3); - i__2 = *m, i__3 = *n - i; - kp = min(i__2,i__3); - i__2 = kp; - for (l = km; l <= i__2; ++l) { - if (abs(l) == *m) { - bwe[l + i * bwe_dim1] = dp * we[l + i * we_dim1]; - } else { - bwe[l + i * bwe_dim1] = b[l + i * b_dim1] + dp * we[l + i * - we_dim1]; - } - } - } - - bandet(&bwe[bwe_offset], m, n); - bansol(&bwe[bwe_offset], &y[y_offset], ny, &c[c_offset], nc, m, n, k); - stat[3] = trinv(&we[we_offset], &bwe[bwe_offset], m, n) * dp; - trn = stat[3] / *n; - - esn = 0.; - i__1 = *k; - for (j = 1; j <= i__1; ++j) { - i__2 = *n; - for (i = 1; i <= i__2; ++i) { - dt = -y[i + j * y_dim1]; - i__3 = *m - 1, i__4 = i - 1; - km = -min(i__3,i__4); - i__3 = *m - 1, i__4 = *n - i; - kp = min(i__3,i__4); - i__3 = kp; - for (l = km; l <= i__3; ++l) { - dt += b[l + i * b_dim1] * c[i + l + j * c_dim1]; - } - esn += dt * dt * wx[i] * wy[j]; - } - } - esn /= *n * *k; - - stat[6] = esn / trn; - stat[1] = stat[6] / trn; - stat[2] = esn; - if (abs(*mode) != 3) { - stat[5] = stat[6] - esn; - if (abs(*mode) == 1) { - ret_val = 0.; - } - if (abs(*mode) == 2) { - ret_val = stat[1]; - } - if (abs(*mode) == 4) { - ret_val = (d__1 = stat[3] - *val, abs(d__1)); - } - } else { - stat[5] = esn - *val * (trn * 2. - 1.); - ret_val = stat[5]; - } - - return ret_val; -} - - - -/* BANDET.FOR, 1985-06-03 */ - -int bandet(doublereal *e, integer *m, integer *n) -{ - /* System generated locals */ - integer e_dim1, e_offset, i__1, i__2, i__3, i__4; - - /* Local variables */ - static integer i, k, l; - static doublereal di, dl; - static integer mi, km, lm; - static doublereal du; - - /* Parameter adjustments */ - - e_dim1 = *m - (-(*m)) + 1; - e_offset = -(*m) + e_dim1; - e -= e_offset; - - /* Function Body */ - if (*m <= 0) { - return 0; - } - i__1 = *n; - for (i = 1; i <= i__1; ++i) { - di = e[i * e_dim1]; - i__2 = *m, i__3 = i - 1; - mi = min(i__2,i__3); - if (mi >= 1) { - i__2 = mi; - for (k = 1; k <= i__2; ++k) { - di -= e[-k + i * e_dim1] * e[k + (i - k) * e_dim1]; - } - e[i * e_dim1] = di; - } - i__2 = *m, i__3 = *n - i; - lm = min(i__2,i__3); - if (lm >= 1) { - i__2 = lm; - for (l = 1; l <= i__2; ++l) { - dl = e[-l + (i + l) * e_dim1]; - i__3 = *m - l, i__4 = i - 1; - km = min(i__3,i__4); - if (km >= 1) { - du = e[l + i * e_dim1]; - i__3 = km; - for (k = 1; k <= i__3; ++k) { - du -= e[-k + i * e_dim1] * e[l + k + (i - k) * e_dim1] - ; - dl -= e[-l - k + (l + i) * e_dim1] * e[k + (i - k) * - e_dim1]; - } - e[l + i * e_dim1] = du; - } - e[-l + (i + l) * e_dim1] = dl / di; - } - } - } - - return 0; -} - - - -/* BANSOL.FOR, 1985-12-12 */ - -int bansol(doublereal *e, doublereal *y, integer *ny, - doublereal *c, integer *nc, integer *m, integer *n, integer *k) -{ - /* System generated locals */ - integer e_dim1, e_offset, y_dim1, y_offset, c_dim1, c_offset, i__1, i__2, i__3, i__4; - - /* Local variables */ - static doublereal d; - static integer i, j, l, mi, nm1; - -/* *** Check on special cases: M=0, M=1, M>1 */ - - /* Parameter adjustments */ - - c_dim1 = *nc; - c_offset = c_dim1 + 1; - c -= c_offset; - - y_dim1 = *ny; - y_offset = y_dim1 + 1; - y -= y_offset; - - e_dim1 = *m - (-(*m)) + 1; - e_offset = -(*m) + e_dim1; - e -= e_offset; - - /* Function Body */ - nm1 = *n - 1; - if ((i__1 = *m - 1) < 0) { - goto L10; - } else if (i__1 == 0) { - goto L40; - } else { - goto L80; - } - -L10: - i__1 = *n; - for (i = 1; i <= i__1; ++i) { - i__2 = *k; - for (j = 1; j <= i__2; ++j) { - c[i + j * c_dim1] = y[i + j * y_dim1] / e[i * e_dim1]; - } - } - return 0; - -L40: - i__1 = *k; - for (j = 1; j <= i__1; ++j) { - c[j * c_dim1 + 1] = y[j * y_dim1 + 1]; - i__2 = *n; - for (i = 2; i <= i__2; ++i) { - c[i + j * c_dim1] = y[i + j * y_dim1] - e[i * e_dim1 - 1] * c[i - - 1 + j * c_dim1]; - } - c[*n + j * c_dim1] /= e[*n * e_dim1]; - for (i = nm1; i >= 1; --i) { - c[i + j * c_dim1] = (c[i + j * c_dim1] - e[i * e_dim1 + 1] * c[i - + 1 + j * c_dim1]) / e[i * e_dim1]; - } - } - return 0; - - -L80: - i__1 = *k; - for (j = 1; j <= i__1; ++j) { - c[j * c_dim1 + 1] = y[j * y_dim1 + 1]; - i__2 = *n; - for (i = 2; i <= i__2; ++i) { - i__3 = *m, i__4 = i - 1; - mi = min(i__3,i__4); - d = y[i + j * y_dim1]; - i__3 = mi; - for (l = 1; l <= i__3; ++l) { - d -= e[-l + i * e_dim1] * c[i - l + j * c_dim1]; - } - c[i + j * c_dim1] = d; - } - c[*n + j * c_dim1] /= e[*n * e_dim1]; - for (i = nm1; i >= 1; --i) { - i__2 = *m, i__3 = *n - i; - mi = min(i__2,i__3); - d = c[i + j * c_dim1]; - i__2 = mi; - for (l = 1; l <= i__2; ++l) { - d -= e[l + i * e_dim1] * c[i + l + j * c_dim1]; - } - c[i + j * c_dim1] = d / e[i * e_dim1]; - } - } - return 0; -} - - - -/* TRINV.FOR, 1985-06-03 */ - -doublereal trinv(doublereal *b, doublereal *e, integer *m, integer *n) -{ - /* System generated locals */ - integer b_dim1, b_offset, e_dim1, e_offset, i__1, i__2, i__3; - doublereal ret_val; - - /* Local variables */ - static integer i, j, k; - static doublereal dd, dl; - static integer mi; - static doublereal du; - static integer mn, mp; - -/* *** Assess central 2*M+1 bands of E**-1 and store in array E */ - - /* Parameter adjustments */ - - e_dim1 = *m - (-(*m)) + 1; - e_offset = -(*m) + e_dim1; - e -= e_offset; - - b_dim1 = *m - (-(*m)) + 1; - b_offset = -(*m) + b_dim1; - b -= b_offset; - - /* Function Body */ - e[*n * e_dim1] = 1. / e[*n * e_dim1]; - for (i = *n - 1; i >= 1; --i) { - i__1 = *m, i__2 = *n - i; - mi = min(i__1,i__2); - dd = 1. / e[i * e_dim1]; - i__1 = mi; - for (k = 1; k <= i__1; ++k) { - e[k + *n * e_dim1] = e[k + i * e_dim1] * dd; - e[-k + e_dim1] = e[-k + (k + i) * e_dim1]; - } - dd += dd; - for (j = mi; j >= 1; --j) { - du = 0.; - dl = 0.; - i__1 = mi; - for (k = 1; k <= i__1; ++k) { - du -= e[k + *n * e_dim1] * e[j - k + (i + k) * e_dim1]; - dl -= e[-k + e_dim1] * e[k - j + (i + j) * e_dim1]; - } - e[j + i * e_dim1] = du; - e[-j + (j + i) * e_dim1] = dl; - dd -= e[j + *n * e_dim1] * dl + e[-j + e_dim1] * du; - } - e[i * e_dim1] = dd * .5; - } - - dd = 0.; - i__1 = *n; - for (i = 1; i <= i__1; ++i) { - i__2 = *m, i__3 = i - 1; - mn = -min(i__2,i__3); - i__2 = *m, i__3 = *n - i; - mp = min(i__2,i__3); - i__2 = mp; - for (k = mn; k <= i__2; ++k) { - dd += b[k + i * b_dim1] * e[-k + (k + i) * e_dim1]; - } - } - ret_val = dd; - i__1 = *m; - for (k = 1; k <= i__1; ++k) { - e[k + *n * e_dim1] = 0.; - e[-k + e_dim1] = 0.; - } - return ret_val; -} - - - -/* SPLDER.FOR, 1985-06-11 */ - -doublereal splder(integer *ider, integer *m, integer *n, doublereal *t, - doublereal *x, doublereal *c, integer *l, doublereal *q) -{ - /* System generated locals */ - integer i__1, i__2; - doublereal ret_val; - - /* Local variables */ - static integer lk1i1; - static doublereal xjki; - static integer i, j, k; - static doublereal z; - static integer i1, j1, k1, j2, m2, ii, jj, ki, jl, lk, mi, nk, lm, ml, jm, ir, ju; - extern /* Subroutine */ int search(integer *, doublereal *, doublereal *, integer *); - static doublereal tt; - static integer lk1, mp1, m2m1, jin, nki, npm, lk1i, nki1; - -/* *** Derivatives of IDER.ge.2*M are always zero */ - - /* Parameter adjustments */ - - --q; - --c; - --x; - - /* Function Body */ - m2 = *m << 1; - k = m2 - *ider; - if (k < 1) { - ret_val = 0.; - return ret_val; - } - - search(n, &x[1], t, l); - - tt = *t; - mp1 = *m + 1; - npm = *n + *m; - m2m1 = m2 - 1; - k1 = k - 1; - nk = *n - k; - lk = *l - k; - lk1 = lk + 1; - lm = *l - *m; - jl = *l + 1; - ju = *l + m2; - ii = *n - m2; - ml = -(*l); - i__1 = ju; - for (j = jl; j <= i__1; ++j) { - if (j >= mp1 && j <= npm) { - q[j + ml] = c[j - *m]; - } else { - q[j + ml] = 0.; - } - } - - if (*ider > 0) { - jl -= m2; - ml += m2; - i__1 = *ider; - for (i = 1; i <= i__1; ++i) { - ++jl; - ++ii; - j1 = max(1,jl); - j2 = min(*l,ii); - mi = m2 - i; - j = j2 + 1; - if (j1 <= j2) { - i__2 = j2; - for (jin = j1; jin <= i__2; ++jin) { - --j; - jm = ml + j; - q[jm] = (q[jm] - q[jm - 1]) / (x[j + mi] - x[j]); - } - } - if (jl >= 1) { - goto L6; - } - i1 = i + 1; - j = ml + 1; - if (i1 <= ml) { - i__2 = ml; - for (jin = i1; jin <= i__2; ++jin) { - --j; - q[j] = -q[j - 1]; - } - } -L6: - ; - } - i__1 = k; - for (j = 1; j <= i__1; ++j) { - q[j] = q[j + *ider]; - } - } - - if (k1 >= 1) { - i__1 = k1; - for (i = 1; i <= i__1; ++i) { - nki = nk + i; - ir = k; - jj = *l; - ki = k - i; - nki1 = nki + 1; - if (*l >= nki1) { - i__2 = *l; - for (j = nki1; j <= i__2; ++j) { - q[ir] = q[ir - 1] + (tt - x[jj]) * q[ir]; - --jj; - --ir; - } - } - lk1i = lk1 + i; - j1 = max(1,lk1i); - j2 = min(*l,nki); - if (j1 <= j2) { - i__2 = j2; - for (j = j1; j <= i__2; ++j) { - xjki = x[jj + ki]; - z = q[ir]; - q[ir] = z + (xjki - tt) * (q[ir - 1] - z) / (xjki - x[jj]) - ; - --ir; - --jj; - } - } - if (lk1i <= 0) { - jj = ki; - lk1i1 = 1 - lk1i; - i__2 = lk1i1; - for (j = 1; j <= i__2; ++j) { - q[ir] += (x[jj] - tt) * q[ir - 1]; - --jj; - --ir; - } - } - } - } - - z = q[k]; - if (*ider > 0) { - i__1 = m2m1; - for (j = k; j <= i__1; ++j) { - z *= j; - } - } - ret_val = z; - return ret_val; -} - - - -/* SEARCH.FOR, 1985-06-03 */ - -int search(integer *n, doublereal *x, doublereal *t, integer *l) -{ - static integer il, iu; - - /* Parameter adjustments */ - - --x; - - /* Function Body */ - - if (*t < x[1]) { - *l = 0; - return 0; - } - if (*t >= x[*n]) { - *l = *n; - return 0; - } - *l = max(*l,1); - if (*l >= *n) { - *l = *n - 1; - } - - if (*t >= x[*l]) { - goto L5; - } - --(*l); - if (*t >= x[*l]) { - return 0; - } - - il = 1; -L3: - iu = *l; -L4: - *l = (il + iu) / 2; - if (iu - il <= 1) { - return 0; - } - if (*t < x[*l]) { - goto L3; - } - il = *l; - goto L4; -L5: - if (*t < x[*l + 1]) { - return 0; - } - ++(*l); - if (*t < x[*l + 1]) { - return 0; - } - il = *l + 1; - iu = *n; - goto L4; - -} - diff --git a/OpenSim/Utilities/simmToOpenSim/gcvspl.h b/OpenSim/Utilities/simmToOpenSim/gcvspl.h deleted file mode 100644 index 2ce95a4332..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/gcvspl.h +++ /dev/null @@ -1,7 +0,0 @@ -/* - function prototypes -*/ -int gcvspl(double *, double *, int *, double *, double *, int *, int *, - int *, int *, double *, double *, int *, double *, int *); - -double splder(int *, int *, int *, double *, double *, double *, int *, double *); diff --git a/OpenSim/Utilities/simmToOpenSim/globals.h b/OpenSim/Utilities/simmToOpenSim/globals.h deleted file mode 100644 index 535197af04..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/globals.h +++ /dev/null @@ -1,48 +0,0 @@ -/******************************************************************************* - - GLOBALS.H - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992, 1993 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef GLOBALS_H -#define GLOBALS_H - -extern Scene* gScene[]; -extern ModelStruct* gModel[]; -extern PlotStruct* gPlot[]; -extern ToolStruct tool[]; -#if ! OPENSIM_BUILD -extern RootStruct root; -#endif - -extern char msg[]; -extern char buffer[]; -extern char errorbuffer[]; -extern double origin[]; -extern double x_axis[]; -extern double y_axis[]; -extern double z_axis[]; - -extern char program_full_name[]; -extern char program_name[]; -extern char program_version[]; -extern char program_with_version[]; -extern char program_date[]; -extern SBoolean pipe_included; -extern SBoolean motion_post_included; -extern SBoolean motion_real_included; -extern char copyright_notice[]; -extern char memory_message[]; -extern char tool_message[]; -extern char new_ascii_label[]; - -#endif /*GLOBALS_H*/ - diff --git a/OpenSim/Utilities/simmToOpenSim/init.c b/OpenSim/Utilities/simmToOpenSim/init.c deleted file mode 100644 index f753bb0224..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/init.c +++ /dev/null @@ -1,1497 +0,0 @@ -/******************************************************************************* - - INIT.C - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines that initialize - structures, set initial program parameter values, or - reset values of a structure. - - Routines: - initialize : called at startup, does basic initialization - init_model : sets initial values of some model structures - init_segment : initializes a segment structure - init_model_display : sets initial values of a model display structure - initplot : sets initial values of a plot display structure - init_muscle : sets initial values of muscle structure elements - init_gencoords : sets initial gencoord structure values - set_gencoord_info : records some info about each gencoord - init_joint : sets initial values in the joint structure - make_confirm_menu : makes the menu used in the Confirm window - -*******************************************************************************/ - -#include - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "wefunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ -#define EVENT_QUEUE_SIZE 50 -#define RAMPSIZE 32 -#define INIT_WORLD_ARRAY_SIZE 10 - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static void make_confirm_menu(void); -static char* confirmstr[] = { -"yes","cancel" -}; - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - -/*************** GLOBAL VARIABLES (for use in a few files) ****************/ - -char* expirationMessage = NULL; - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ - - -#if ! ENGINE - -/* INITIALIZE: the main initializing routine that is called once at startup. - * It makes the program window, queues the keyboard and mouse buttons, and - * calls routines to make the color map. - */ - -void initialize(void) -{ - int i; - const char* p; - - add_preprocessor_option(yes, "-I%s", get_preference("RESOURCES_FOLDER")); - - root.numwindows = 0; - root.messages.line = NULL; - - root.gldesc.max_screen_x = glutGet(GLUT_SCREEN_WIDTH); - if (root.gldesc.max_screen_x <= 0) - root.gldesc.max_screen_x = 1024; - root.gldesc.max_screen_y = glutGet(GLUT_SCREEN_HEIGHT); - if (root.gldesc.max_screen_y <= 0) - root.gldesc.max_screen_y = 768; - - root.gldesc.max_screen_x -= 10; - root.gldesc.max_screen_y -= 30; - - if (is_preference_on(get_preference("MULTIPLE_SCREENS")) == no) - glueSetConstrainedWindows(yes); - else - glueSetConstrainedWindows(no); - - if (is_preference_on(get_preference("MULTIPLE_SCREENS")) == no) - open_main_window(); - - init_global_lighting(); - - if (make_selectors() == code_bad) - error(exit_program,tool_message); - - /* Initialize some root structure variables */ - root.nummodels = 0; - root.numplots = 0; - root.numtools = 0; - root.currentwindow = NO_WINDOW; - root.modelcount = 0; - root.confirm_window_open = no; - - root.gfont.defaultfont = SIMM_DEFAULT_FONT; - root.gfont.largefont = SIMM_LARGE_FONT; - root.gfont.smallfont = SIMM_SMALL_FONT; - root.gfont.italics = SIMM_ITALIC_FONT; - root.gfont.bold = SIMM_BOLD_FONT; - - /* Set all the plot pointers to NULL */ - for (i=0; iscene_num is preset; store it here so you can restore it after memset(). - int scene_number = sc->scene_num; - const char* p = NULL; - - memset(sc, 0, sizeof(Scene)); - - sc->scene_num = scene_number; - sc->trackball = yes; - sc->camera_segment_model = NULL; - sc->camera_segment = -1; - - sc->windowX1 = -1; - sc->windowY1 = -1; - sc->windowHeight = 400; - sc->windowWidth = 400; - - reset_4x4matrix(sc->transform_matrix); - - for (i=0; i<3; i++) - { - sc->background_color[i] = 0.2f; - sc->crosshairs_color[i] = 1.0f; - } - - sc->fov_angle = 40.0; - sc->y_zoom_constant = tan((double)sc->fov_angle*0.5*DTOR); - sc->near_clip_plane = 0.0002; /* reset in size_model */ - sc->far_clip_plane = 12.3; /* reset in size_model */ - - sc->snapshot_mode = SNAPSHOT_INACTIVE; - sc->snapshot_counter = 1; - sc->snapshot_file_suffix = NULL; - sc->snapshot_include_depth = no; - - sc->movie_mode = MAKEMOVIE_INACTIVE; - sc->movie_file = NULL; - - sc->trackball = yes; - sc->camera_segment = -1; - - sc->windowX1 = -1; - sc->windowY1 = -1; - sc->windowHeight = 400; - sc->windowWidth = 400; - - return code_fine; -} - - -/* INITMODEL: this guy initializes much of the model structure. - */ -ReturnCode init_model(ModelStruct* ms) -{ - int i; - const char *p; - // ms->modelnum is preset; store it here so you can restore it after memset(). - int model_number = ms->modelnum; - - memset(ms, 0, sizeof(ModelStruct)); - - ms->modelnum = model_number; - ms->is_demo_model = no; - ms->useIK = yes; - ms->defaultGCApproved = yes; - ms->defaultLoopsOK = yes; - ms->defaultConstraintsOK = yes; - ms->constraintsOK = yes; - ms->loopsOK = yes; - ms->muscle_array_size = MUSCLE_ARRAY_INCREMENT; - ms->ligament_array_size = MUSCLE_ARRAY_INCREMENT; - ms->muscgroup_array_size = MUSCGROUP_ARRAY_INCREMENT; - ms->world_array_size = INIT_WORLD_ARRAY_SIZE; - ms->genc_array_size = GENC_ARRAY_INCREMENT; - ms->segment_array_size = SEGMENT_ARRAY_INCREMENT; - ms->joint_array_size = JOINT_ARRAY_INCREMENT; - ms->func_array_size = FUNC_ARRAY_INCREMENT; - ms->specified_min_thickness = yes; - ms->specified_max_thickness = yes; - ms->dynamics_ready = no; - ms->max_diagonal_needs_recalc = yes; - ms->GEFuncOK = no; - ms->marker_visibility = yes; - ms->marker_radius = DEFAULT_MARKER_RADIUS; - ms->loop_tolerance = DEFAULT_LOOP_TOLERANCE; - ms->loop_weight = DEFAULT_LOOP_WEIGHT; - ms->solver.accuracy = DEFAULT_SOLVER_ACCURACY; - ms->solver.method = smLevenbergMarquart; - ms->solver.max_iterations = 100; - ms->solver.joint_limits = smYes; - ms->solver.orient_body = smNo; - ms->solver.fg_contact = smNo; - ms->global_show_masscenter = no; - ms->global_show_inertia = no; - - ms->gravity = smNegY; - - ms->functionMenu = 0; - - ms->motion_array_size = MOTION_ARRAY_INCREMENT; - ms->motion = (MotionSequence**)simm_malloc(ms->motion_array_size * sizeof(MotionSequence*)); - if (ms->motion == NULL) - { - error(none,"Not enough memory to add another model."); - return code_bad; - } - else - { - for (i = 0; i < ms->motion_array_size; i++) - ms->motion[i] = NULL; - } - - - for (i=0; i<100; i++) - ms->motionfilename[i] = NULL; - ms->num_motion_files = 0; - - ms->joint = (JointStruct*)simm_malloc(ms->joint_array_size*sizeof(JointStruct)); - ms->segment = (SegmentStruct*)simm_malloc(ms->segment_array_size*sizeof(SegmentStruct)); - ms->muscgroup = (MuscleGroup*)simm_malloc(ms->muscgroup_array_size*sizeof(MuscleGroup)); - ms->seggroup = NULL; - ms->gencgroup = NULL; - ms->gencoord = (GeneralizedCoord**)simm_calloc(ms->genc_array_size, sizeof(GeneralizedCoord*)); - - ms->muscle = (dpMuscleStruct**)simm_calloc(ms->muscle_array_size, sizeof(dpMuscleStruct*)); - - ms->ligament = (LigamentStruct*)simm_malloc(ms->ligament_array_size*sizeof(LigamentStruct)); - - ms->function = (dpFunction**)simm_calloc(ms->func_array_size, sizeof(dpFunction*)); - - ms->save.function = (dpFunction**)simm_calloc(ms->func_array_size, sizeof(dpFunction*)); - - ms->worldobj = (WorldObject*)simm_malloc(ms->world_array_size*sizeof(WorldObject)); - - if (ms->joint == NULL || ms->segment == NULL || ms->muscgroup == NULL || - ms->gencoord == NULL || ms->function == NULL || - ms->save.function == NULL || ms->worldobj == NULL) - { - error(none,"Not enough memory to add another model."); - return code_bad; - } - - ms->num_wrap_objects = 0; - ms->wrap_object_array_size = WRAP_OBJECT_ARRAY_INCREMENT; - ms->wrapobj = (dpWrapObject**)simm_malloc(ms->wrap_object_array_size*sizeof(dpWrapObject*)); - if (ms->wrapobj == NULL) - { - error(none,"Not enough memory to add another wrap object."); - return code_bad; - } - -// ms->constraint_tolerance = DEFAULT_CONSTRAINT_TOLERANCE; - ms->num_constraint_objects = 0; - ms->constraint_object_array_size = CONSTRAINT_OBJECT_ARRAY_INCREMENT; - ms->constraintobj = (ConstraintObject*)simm_malloc(ms->constraint_object_array_size*sizeof(ConstraintObject)); - if (ms->constraintobj == NULL) - { - error(none,"Not enough memory to add another constraint."); - return code_bad; - } - - ms->num_deformities = 0; - ms->deformity_array_size = DEFORMITY_ARRAY_INCREMENT; - ms->deformity = (Deformity*) simm_malloc(ms->deformity_array_size * sizeof(Deformity)); - if (ms->deformity == NULL) - { - error(none,"Not enough memory to add another deformity object."); - return code_bad; - } - - init_materials(ms); - - ms->num_motion_objects = 0; - ms->motion_objects = NULL; - -#if ! ENGINE || OPENSMAC - add_default_motion_objects(ms); -#endif - - /* This part of the display structure must be initialized here because - * it can be changed when reading in the joints file, which happens before - * the display structure is initialized. - */ - ms->dis.num_file_views = 0; -// ms->dis.muscle_array_size = 0;//dkb = MUSCLE_ARRAY_INCREMENT; - ms->dis.muscle_array_size = MUSCLE_ARRAY_INCREMENT; - - ms->dis.fast_muscle_drawing = no; - -#if ! ENGINE - ms->dis.fast_muscle_drawing = is_preference_on(get_preference("FASTER_MUSCLE_DRAWING")); - -#if INCLUDE_MSL_LENGTH_COLOR - ms->dis.muscle_color_factor = 0.0; -#endif - - for (i=0; idis.view_used[i] = no; - ms->dis.view_name[i] = NULL; - } - - for (i=0; i<3; i++) - { - ms->dis.background_color[i] = 0.2f; - ms->dis.vertex_label_color[i] = 0.0f; - ms->dis.rotation_axes_color[i] = 1.0f; - ms->dis.crosshairs_color[i] = 1.0f; - } - ms->dis.rotation_axes_color[2] = 0.0f; - ms->dis.vertex_label_color[0] = 1.0f; - - ms->dis.background_color_spec = no; - ms->dis.vertex_label_color_spec = no; - ms->dis.rotation_axes_color_spec = no; - ms->dis.crosshairs_color_spec = no; - - ms->modelLock = glutNewMutex(); - ms->realtimeState = rtNotConnected; - -#endif /* ENGINE */ - - ms->gencoordmenu = -1; - ms->gencoordmenu2 = -1; - ms->gencoord_group_menu = -1; - ms->xvarmenu = -1; - ms->momentgencmenu = -1; - ms->momentarmgencmenu = -1; - ms->momentarmnumgencmenu = -1; - ms->maxmomentgencmenu = -1; - - return code_fine; -} - - -void init_segment(ModelStruct* ms, SegmentStruct* seg) -{ - int i, j; - - seg->defined = no; - seg->numBones = 0; - seg->boneArraySize = 0; - seg->shadow = no; - seg->shadow_scale[0] = seg->shadow_scale[1] = seg->shadow_scale[2] = 1.0; - seg->shadow_trans[0] = seg->shadow_trans[1] = seg->shadow_trans[2] = 0.0; - seg->shadow_color.rgb[RD] = 0.1f; - seg->shadow_color.rgb[GR] = 0.1f; - seg->shadow_color.rgb[BL] = 0.1f; - seg->shadow_color_spec = no; - seg->drawmode = gouraud_shading; - seg->material = ms->dis.mat.default_bone_material; - seg->mass = 0.0; - seg->bone = NULL; - seg->draw_axes = no; - seg->axis_length = 0.0; - - seg->ground_condition = invalid; - - seg->numMarkers = 0; - seg->markerArraySize = 0; - seg->marker = NULL; - -#if INCLUDE_BONE_EDITOR_EXTRAS - seg->pts_file = NULL; - seg->num_raw = 0; - seg->raw_vertices = NULL; -#endif - - seg->bone_scale[0] = 1.0; - seg->bone_scale[1] = 1.0; - seg->bone_scale[2] = 1.0; - - seg->htr_o[0] = seg->htr_o[1] = seg->htr_o[2] = UNDEFINED_DOUBLE; - seg->htr_x[0] = seg->htr_x[1] = seg->htr_x[2] = UNDEFINED_DOUBLE; - seg->htr_y[0] = seg->htr_y[1] = seg->htr_y[2] = UNDEFINED_DOUBLE; - - for (i=0; i<3; i++) - { - for (j=0; j<3; j++) - seg->inertia[i][j] = 0.0; - seg->masscenter[i] = 0.0; - } - - seg->mass_specified = no; - seg->inertia_specified = no; - seg->masscenter_specified = no; -#if INCLUDE_MOCAP_MODULE - seg->lengthstartend_specified = no; -#endif - seg->show_masscenter = ms->global_show_masscenter;//no; - seg->show_inertia = ms->global_show_inertia;//no; - - seg->numgroups = 0; - seg->group = NULL; - - seg->springFloor = NULL; - seg->numSpringPoints = 0; - seg->springPointArraySize = 0; - seg->springPoint = NULL; - - seg->numContactObjects = 0; - seg->contactObjectArraySize = 0; - seg->contactObject = NULL; - - seg->forceMatte = NULL; - - seg->num_deforms = 0; - seg->deform_obj_array_size = 0; - seg->deform = NULL; - -#if INCLUDE_MOCAP_MODULE - for (i = 0; i < 3; i++) - { - seg->lengthstart[i] = 0.0; - seg->lengthend[i] = 0.0; - } - seg->gait_scale_segment = NULL; - seg->gait_scale_factor[0] = 1.0; - seg->gait_scale_factor[1] = 1.0; - seg->gait_scale_factor[2] = 1.0; - seg->mocap_segment = NULL; - seg->mocap_scaling_method = INHERIT_SCALE; - seg->mocap_scale_chain_end1 = NULL; - seg->mocap_scale_chain_end2 = NULL; - identity_matrix(seg->mocap_adjustment_xform); -#endif -} - - -/* INIT_MODEL_DISPLAY: this routine initializes the model display structure, which - * holds the viewport and ortho information as well as the current viewing - * transformations. - */ - -ReturnCode init_model_display(ModelStruct* ms) -{ - int i; - - ms->dis.motion_speed = DEFAULT_MOTION_SPEED; - ms->dis.default_view = 0; // the default camera will be cam0 unless overridden by user - ms->dis.show_highlighted_polygon = no; - ms->dis.show_selected_coords = no; - ms->dis.show_all_muscpts = no; - ms->dis.show_shadow = no; - ms->dis.applied_motion = NULL; - ms->dis.current_motion = NULL; - ms->dis.hpoly.segmentnum = -1; - ms->dis.hpoly.bonenum = -1; - ms->dis.hpoly.polynum = -1; - ms->dis.continuous_motion = no; - ms->dis.display_motion_info = yes; - - ms->dis.devs = (int*)simm_malloc(ms->numgencoords*2*sizeof(int)); - ms->dis.dev_values = (int*)simm_malloc(ms->numgencoords*2*sizeof(int)); - if (ms->dis.devs == NULL || ms->dis.dev_values == NULL) - return code_bad; - - for (i=0; inumgencoords; i++) - { - ms->dis.devs[i*2] = ms->gencoord[i]->keys[0]; - ms->dis.devs[i*2+1] = ms->gencoord[i]->keys[1]; - } - ms->dis.numdevs = ms->numgencoords*2; - - ms->dis.nummuscleson = ms->nummuscles; - ms->dis.muscle_array_size = ms->muscle_array_size; - if (ms->dis.muscle_array_size > 0) - { - ms->dis.muscleson = (int*)simm_malloc(ms->dis.muscle_array_size*sizeof(int)); - if (ms->dis.muscleson == NULL) - return code_bad; - } - - ms->dis.muscle_cylinder_id = -1; - - for (i=0; idis.muscle_array_size; i++) - { - if (i < ms->nummuscles) - ms->dis.muscleson[i] = ms->muscle[i]->display; - else - ms->dis.muscleson[i] = 0; - } - - for (i=0; idis.menucolumns[i] = EMPTY; - - for (i=0; inumgroups; i++) - { - ms->dis.mgroup[i].state = off; - ms->dis.mgroup[i].xo = -1; - ms->dis.mgroup[i].yo = -1; - } - - ms->dis.view_menu = 0; - ms->dis.maindrawmodemenu = 0; - ms->dis.allsegsdrawmodemenu = 0; - ms->dis.allligsdrawmodemenu = 0; - ms->dis.allworlddrawmodemenu = 0; - ms->dis.alldrawmodemenu = 0; - ms->dis.eachsegdrawmodemenu = 0; - -#if ! ENGINE - ms->dis.display_motion_info = is_preference_on(get_preference("DISPLAY_MOTION_INFO")); -#endif - - return code_fine; -} - -#if ! ENGINE - -/* INITPLOT: this guy initializes the plot structure, which contains viewport - * and ortho values as well as the set of curve structures for that plot. - * First it calls a routine to set the viewport and window-resize box - * variables. - */ - -ReturnCode initplot(int plotnum) -{ - gPlot[plotnum]->plotnum = plotnum; - gPlot[plotnum]->numcurves = 0; - gPlot[plotnum]->zoomed_yet = no; - gPlot[plotnum]->numpoints = 0; - gPlot[plotnum]->xname_len = 0; - gPlot[plotnum]->yname_len = 0; - gPlot[plotnum]->title_len = 256; - gPlot[plotnum]->title = (char*)simm_malloc(gPlot[plotnum]->title_len); - gPlot[plotnum]->xname = NULL; - gPlot[plotnum]->yname = NULL; - gPlot[plotnum]->cursor_modelPtr = NULL; - gPlot[plotnum]->cursor_motion = NULL; - gPlot[plotnum]->show_cursor = yes; - gPlot[plotnum]->show_events = yes; - gPlot[plotnum]->needs_bounding = no; - gPlot[plotnum]->num_file_events = 0; - gPlot[plotnum]->file_event = NULL; - - if (gPlot[plotnum]->title == NULL) - return code_bad; - - NULLIFY_STRING(gPlot[plotnum]->title); - - return code_fine; -} - -#endif /* ! ENGINE */ - -/* INIT_MUSCLE: this routine initializes much of the muscle structure. Before - * a given muscle is read-in from an input file, each of the pointers to its - * muscle-specific parameters is set to point into the default-muscle structure. - * Thus if a certain parameter is not specified for a muscle (e.g. force-length - * curve), the default-muscle's parameter is used. This cuts down significantly - * on the amount of information that must be supplied in a muscle file since - * most muscles share the same active_force_len_func, tendon_force_len_func, - * passive_force_len_func, and force_vel_func. - */ -ReturnCode init_muscle(ModelStruct* model, dpMuscleStruct* muscle, dpMuscleStruct* default_muscle) -{ - int i; - - memset(muscle, 0, sizeof(dpMuscleStruct)); - - muscle->display = default_muscle->display; - muscle->output = yes; - muscle->selected = no; - muscle->numgroups = default_muscle->numgroups; - muscle->group = default_muscle->group; - muscle->max_isometric_force = default_muscle->max_isometric_force; - muscle->pennation_angle = default_muscle->pennation_angle; - muscle->optimal_fiber_length = default_muscle->optimal_fiber_length; - muscle->resting_tendon_length = default_muscle->resting_tendon_length; - muscle->min_thickness = default_muscle->min_thickness; - muscle->max_thickness = default_muscle->max_thickness; - muscle->min_material = default_muscle->min_material; - muscle->max_material = default_muscle->max_material; - muscle->max_contraction_vel = default_muscle->max_contraction_vel; - muscle->force_vel_func = default_muscle->force_vel_func; - - if (init_dynamic_param_array(muscle, default_muscle) == code_bad) - return code_bad; - - muscle->muscle_model_index = default_muscle->muscle_model_index; - muscle->excitation_func = default_muscle->excitation_func; - muscle->excitation_abscissa = default_muscle->excitation_abscissa; - - muscle->nummomentarms = model->numgencoords; - muscle->momentarms = (double *)simm_malloc(muscle->nummomentarms*sizeof(double)); - if (muscle->momentarms == NULL) - return code_bad; - - for (i=0; inummomentarms; i++) - muscle->momentarms[i] = 0.0; - muscle->dynamic_activation = 1.0; - muscle->tendon_force_len_func = default_muscle->tendon_force_len_func; - muscle->active_force_len_func = default_muscle->active_force_len_func; - muscle->passive_force_len_func = default_muscle->passive_force_len_func; - - muscle->wrap_calced = no; - - return code_fine; -} - -ReturnCode initMusclePath(dpMusclePathStruct *musclepoints) -{ - musclepoints->num_orig_points = 0; - musclepoints->mp_orig_array_size = MUSCLEPOINT_ARRAY_INCREMENT; - musclepoints->mp_orig = (dpMusclePoint *)simm_malloc(musclepoints->mp_orig_array_size * sizeof(dpMusclePoint)); - if (musclepoints->mp_orig == NULL) - return code_bad; - - musclepoints->num_points = 0; - musclepoints->mp_array_size = 0; - musclepoints->mp = NULL; - - return code_fine; -} - - -ReturnCode init_musclepoint(dpMusclePoint *mp) -{ - int i; - - mp->segment = -1; - mp->selected = dpNo; - mp->isMovingPoint = dpNo; - mp->isVia = dpNo; - mp->is_auto_wrap_point = dpNo; - mp->viaRange.gencoord = NULL; - mp->viaRange.start = UNDEFINED_DOUBLE; - mp->viaRange.end = UNDEFINED_DOUBLE; - mp->num_wrap_pts = 0; - mp->wrap_pts = NULL; - mp->wrap_distance = 0; - for (i = 0; i < 3; i++) - { - mp->point[i] = ERROR_DOUBLE; - mp->ground_pt[i] = ERROR_DOUBLE; - mp->undeformed_point[i] = ERROR_DOUBLE; - mp->function[i] = NULL; - mp->gencoord[i] = NULL; - mp->normal[i] = 0.0; - } - - return code_fine; -} - - -/* INIT_GENCOORD: this routine initializes a gencoord structure before a gencoord is - * read from an input file. - */ -void init_gencoord(GeneralizedCoord* gencoord) -{ - memset(gencoord, 0, sizeof(GeneralizedCoord)); - - gencoord->type = rotation_gencoord; - gencoord->defined = no; - gencoord->wrap = no; - - gencoord->keys[0] = gencoord->keys[1] = null_key; - - gencoord->default_value_specified = no; - gencoord->clamped = yes; - gencoord->clamped_save = gencoord->clamped; - gencoord->locked = no; - gencoord->locked_save = gencoord->locked; - gencoord->range.end = 90.0; - gencoord->restraintFuncActive = yes; - gencoord->used_in_model = no; - gencoord->used_in_loop = no; - gencoord->used_in_constraint = no; - gencoord->slider_visible = yes; - - gencoord->restraint_sdcode_num = -1; - gencoord->min_restraint_sdcode_num = -1; - gencoord->max_restraint_sdcode_num = -1; - -#if INCLUDE_MOCAP_MODULE - gencoord->mocap_seg_index = -1; - gencoord->mocap_column = -1; -#endif -} - - -/* INIT_GENCOORDS: this function sets some of the initial values in the gencoord - * structs. It must be called after all the joints have been read in because it - * mallocs an array of size numjoints for each gencoord. - */ -ReturnCode init_gencoords(ModelStruct* ms) -{ - int i; - - for (i=0; inumgencoords; i++) - { - if (ms->gencoord[i]->default_value >= ms->gencoord[i]->range.start && - ms->gencoord[i]->default_value <= ms->gencoord[i]->range.end) - ms->gencoord[i]->value = ms->gencoord[i]->default_value; - else if (ms->gencoord[i]->range.start > 0.0 || - ms->gencoord[i]->range.end < 0.0) - ms->gencoord[i]->value = ms->gencoord[i]->range.start; - else - ms->gencoord[i]->value = 0.0; - - ms->gencoord[i]->velocity = 0.0; - -#if ! ENGINE - storeDoubleInForm(&ms->gencform.option[i], ms->gencoord[i]->value, 3); -#endif - - ms->gencoord[i]->numjoints = 0; - ms->gencoord[i]->jointnum = (int*)simm_malloc(ms->numjoints*sizeof(int)); - if (ms->gencoord[i]->jointnum == NULL) - return code_bad; - } - - return code_fine; -} - - -/* SET_GENCOORD_INFO: This routine records some information about the - * gencoords. It first makes a list of the joints that each gencoord - * appears in, then sets the type (rotation, translation) of the gencoord. - */ -void set_gencoord_info(ModelStruct* ms) -{ - int i, j, k, en; - DofStruct* adof; - - /* For each joint, go through the six dofs (translations and rotations) - * and list the gencoords that appear in the functions that define these - * dofs. Then add this joint number to the joint list for each of these - * gencoords. What you have at the end is a joint-dependency list for - * each gencoord. That is, with each gencoord is a list of the joints - * which have that gencoord in one or more of its dof functions. Thus - * when you change the value of one gencoord, you know which joint - * transformations you have to recalculate. - * The loop over k checks to make sure you don't enter a joint twice in - * a gencoord's list of joints. If a joint has two or more dofs which use - * the same gencoord, you need to watch out for this. So as you scan - * the dofs for each joint and find a gencoord, scan the previous dofs - * again to see if one of them uses the same gencoord, which means you - * don't need to [re-]tell the gencoord about the joint. - */ - for (i=0; inumgencoords; i++) - ms->gencoord[i]->numjoints = 0; - - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - adof = &ms->joint[i].dofs[j]; - if (adof->type == function_dof) - { - for (k=0; kgencoord == ms->joint[i].dofs[k].gencoord) - break; - if (k == j) - { - en = adof->gencoord->numjoints++; - adof->gencoord->jointnum[en] = i; - } - } - } - } - - for (i = 0; i < ms->numgencoords; i++) - determine_gencoord_type(ms, ms->gencoord[i]); -} - - -/* This function tries to determine whether each gencoord is primarily rotational - * or translational. This information is needed for two reasons: - * 1. to know how big to set the step size for the arrow buttons on the slider for - * that gencoord in the Model Viewer (0.001 for translations, 1.0 for rotations). - * 2. to know how to interpret the moment arm values w.r.t. that gencoord. If the - * gencoord maps directly to a rotational dof, then the gencoord's units need to - * be treated as degrees, and thus converted to radians when calculating moment arms. - * If the gencoord maps directly to a translational dof, then its units are meters - * and no conversion is needed. If the gencoord maps directly to a dof of each type, - * then it really is an abstract quantity, and interpreting the moment arm in - * real-world units is problematic. So just find the first dof that maps directly - * to it and go with that one. - * To determine whether a gencoord is rotational or translational, find the first - * dof that maps directly to the gencoord (has a function with two points and slope - * equal to 1.0 or -1.0). If there is no such function, then just look at the dofs - * that the gencoord is used in. If they are all translational, then the gencoord is - * translational. If one or more is rotational, then the gencoord is rotational. - */ -void determine_gencoord_type(ModelStruct* model, GeneralizedCoord* gencoord) -{ - int j, k, dofnum, jointnum; - GencoordType gencoord_type; - - if (mark_unconstrained_dof(model, gencoord, &jointnum, &dofnum, NULL)) - { - if (dofnum == R1 || dofnum == R2 || dofnum == R3) - gencoord->type = rotation_gencoord; - else if (dofnum == TX || dofnum == TY || dofnum == TZ) - gencoord->type = translation_gencoord; - } - else - { - gencoord_type = translation_gencoord; - for (j = 0; j < gencoord->numjoints; j++) - { - jointnum = gencoord->jointnum[j]; - for (k = R1; k <= R3; k++) - { - if (model->joint[jointnum].dofs[k].type == function_dof && - model->joint[jointnum].dofs[k].gencoord == gencoord) - { - gencoord_type = rotation_gencoord; - j = gencoord->numjoints; // to break out of outer loop - break; - } - } - } - gencoord->type = gencoord_type; - } -} - - -int mark_unconstrained_dof(ModelStruct* ms, GeneralizedCoord* gencoord, int* jnt, int* dof, SBoolean* constrained) -{ - int i, j; - double slope; - dpFunction* func; - - *jnt = *dof = -1; - - /* This function looks through all the dofs in all the joints which are a - * function of the specified gencoord, and tries to find one which should - * be treated as the unconstrained dof. If the dof has a function with two - * points, and the slope of the function is 1.0 or -1.0, then it is a good - * match. The dof is marked as unconstrained, and the function returns. - * If no such dof is found, the function returns an error. If there are - * multiple dofs which meet these criteria, the first one is treated as the - * unconstrained one, and the others will end up constrained (which will - * create a correct model). - * JPL 8/2/04: this function has been updated to also check to make sure - * that the 2-point, slope=1 function also passes through zero. - * JPL 3/6/06: this function appeared to have a bug which prevented - * the negative-slope case from getting used, but it turns out that - * the code for that case is incorrect. So negative-slope functions - * are not allowed, and will return an error when attempting to save - * dynamics. - */ - for (i = 0; i < ms->numjoints; i++) - { - for (j = 0; j < 6; j++) - { - if (ms->joint[i].dofs[j].type == function_dof && - ms->joint[i].dofs[j].gencoord == gencoord) - { - func = ms->joint[i].dofs[j].function; - if (func->numpoints == 2 && EQUAL_WITHIN_ERROR(func->x[0], func->y[0])) - { - slope = (func->y[1] - func->y[0]) / (func->x[1] - func->x[0]); - if (EQUAL_WITHIN_ERROR(slope, 1.0)) - { - ms->joint[i].dofs[j].sd.constrained = no; - ms->joint[i].dofs[j].sd.conversion_sign = 1.0; - *jnt = i; - *dof = j; - if (constrained) - *constrained = no; - return 1; - } - else if (EQUAL_WITHIN_ERROR(slope,-1.0)) - { - ms->joint[i].dofs[j].sd.constrained = no; - /* If slope is negative, set the conversion_sign to -1 - * so the conversion factor (which is set later) will - * be negative. - */ - ms->joint[i].dofs[j].sd.conversion_sign = -1.0; - *jnt = i; - *dof = j; - if (constrained) - *constrained = no; - return 1; - } - } - } - } - } - - // This code is specifically for conversion to Simbody-based models, which do - // not have the restriction that every gencoord must map to a DOF with a "simple" - // function. If there is such a gencoord, its "primary" DOF is set to the first - // one that uses it, but it's marked as constrained so that the OpenSim export - // code knows to add the coordinate to the joint and the kinematic function to - // the corresponding transform axis. - if (constrained) - { - for (i = 0; i < ms->numjoints; i++) - { - for (j = 0; j < 6; j++) - { - if (ms->joint[i].dofs[j].type == function_dof && - ms->joint[i].dofs[j].gencoord == gencoord) - { - ms->joint[i].dofs[j].sd.constrained = no; // so other code will not create a new coordinate - ms->joint[i].dofs[j].sd.conversion_sign = 1.0; // and constrain it to this gencoord - *jnt = i; - *dof = j; - *constrained = yes; - return 1; - } - } - } - } - - return 0; -} - - -/* INIT_JOINT: this routine initializes a joint structure before a joint is read - * from an input file. - */ -void init_joint(ModelStruct* ms, JointStruct* jointstr) -{ - int i; - - jointstr->to = -1; - jointstr->from = -1; - jointstr->name = NULL; - jointstr->type = dpUnknownJoint; - jointstr->defined = no; - jointstr->solverType = NULL; - jointstr->loop_joint = no; - jointstr->user_loop = no; - jointstr->conversion.condition = invalid; - jointstr->sd_name = NULL; - - /* In case the transformation order is not specified in the file, assume - * a default ordering of t, r1, r2, r3. - */ - for (i=0; i<4; i++) - jointstr->order[i] = i; - - /* If any dofs are not specified in the file, the error checking will throw out - * the bad joint (and model), but make all the dofs constants of 0.0 anyway. - */ - for (i=0; i<6; i++) - { - jointstr->dofs[i].type = constant_dof; - jointstr->dofs[i].value = 0.0; - jointstr->dofs[i].function = NULL; - jointstr->dofs[i].gencoord = NULL; - jointstr->dofs[i].sd.name = NULL; - jointstr->dofs[i].sd.con_name = NULL; - jointstr->dofs[i].sd.initial_value = 0.0; - jointstr->dofs[i].sd.constrained = no; - jointstr->dofs[i].sd.fixed = no; - jointstr->dofs[i].sd.state_number = -1; - jointstr->dofs[i].sd.error_number = -1; - jointstr->dofs[i].sd.joint = -1; - jointstr->dofs[i].sd.axis = -1; - jointstr->dofs[i].sd.conversion = 0.0; - jointstr->dofs[i].sd.conversion_sign = 1.0; - } - - jointstr->dofs[0].key = r1; - jointstr->dofs[1].key = r2; - jointstr->dofs[2].key = r3; - jointstr->dofs[3].key = tx; - jointstr->dofs[4].key = ty; - jointstr->dofs[5].key = tz; - - jointstr->show_axes[0] = no; - jointstr->show_axes[1] = no; - jointstr->show_axes[2] = no; - - jointstr->axis_length[0] = 0.0; - jointstr->axis_length[1] = 0.0; - jointstr->axis_length[2] = 0.0; - - jointstr->in_seg_ground_path = NULL; - - jointstr->pretransform_active = no; - jointstr->pretransform_condition = valid; - - identity_matrix(jointstr->pretransform); - identity_matrix(jointstr->pretransform_inverse); - -#if INCLUDE_MOCAP_MODULE - jointstr->mocap_segment = NULL; - jointstr->mocap_seg_index = -1; -#endif -} - - -void initwrapobject(dpWrapObject* wo) -{ - wo->name = NULL; - wo->wrap_type = dpWrapSphere; - wo->wrap_algorithm = WE_HYBRID_ALGORITHM; - wo->segment = 0; - wo->display_list = 0; - wo->display_list_is_stale = no; - wo->active = yes; /* set wrap object fields to default values */ - wo->visible = yes; - wo->show_wrap_pts = no; - wo->radius[0] = 0.05; - wo->radius[1] = 0.1; - wo->radius[2] = 0.05; - wo->height = 0.1; - wo->wrap_axis = 0; - wo->wrap_sign = 0; - - wo->rotation_axis.xyz[0] = 1.0; - wo->rotation_axis.xyz[1] = 0.0; - wo->rotation_axis.xyz[2] = 0.0; - wo->rotation_angle = 0.0; - wo->translation.xyz[0] = 0.0; - wo->translation.xyz[1] = 0.0; - wo->translation.xyz[2] = 0.0; - - wo->undeformed_translation.xyz[0] = 0.0; - wo->undeformed_translation.xyz[1] = 0.0; - wo->undeformed_translation.xyz[2] = 0.0; - - identity_matrix(wo->from_local_xform); - identity_matrix(wo->to_local_xform); - - wo->xforms_valid = yes; - -#if VISUAL_WRAPPING_DEBUG - wo->num_debug_glyphs = 0; -#endif -} - -void initconstraintobject(ConstraintObject* co) -{ - co->name = NULL; - co->numPoints = 0; - co->points = NULL; - co->cp_array_size = CP_ARRAY_INCREMENT; - co->constraintType = constraint_sphere; - co->segment = 0; - co->display_list = 0; - co->display_list_is_stale = no; - co->active = yes; - co->visible = yes; - co->radius.xyz[0] = 0.05; - co->radius.xyz[1] = 0.1; - co->radius.xyz[2] = 0.05; - co->height = 0.1; - co->constraintAxis = 0; - co->constraintSign = 0; - - co->plane.a = 0.0; - co->plane.b = 0.0; - co->plane.c = 1.0; - co->plane.d = 0.0; - - co->rotationAxis.xyz[0] = 1.0; - co->rotationAxis.xyz[1] = 0.0; - co->rotationAxis.xyz[2] = 0.0; - co->rotationAngle = 0.0; - co->translation.xyz[0] = 0.0; - co->translation.xyz[1] = 0.0; - co->translation.xyz[2] = 0.0; - - co->undeformed_translation.xyz[0] = 0.0; - co->undeformed_translation.xyz[1] = 0.0; - co->undeformed_translation.xyz[2] = 0.0; - - identity_matrix(co->from_local_xform); - identity_matrix(co->to_local_xform); - - co->xforms_valid = yes; - - co->num_qs = 0; - co->num_jnts = 0; - co->qs = NULL; - co->joints = NULL; -} - -void initconstraintpoint(ConstraintPoint *pt) -{ - pt->name = NULL; - pt->weight = 1.0; - pt->offset[0] = 0.01; - pt->offset[1] = 0.01; - pt->offset[2] = 0.01; - pt->segment = 0; - pt->tolerance = DEFAULT_CONSTRAINT_TOLERANCE; -// pt->visible = yes; -// pt->active = yes; -} - -#if ! ENGINE - -static void make_confirm_menu(void) -{ - Menu* ms; - - ms = &root.confirm_menu; - - ms->title = NULL; - ms->numoptions = 2; - ms->origin.x = 0; - ms->origin.y = 0; - ms->type = normal_menu; - ms->option = (MenuItem*)simm_malloc(ms->numoptions*sizeof(MenuItem)); - if (ms->option == NULL) - error(exit_program,tool_message); - - ms->option[0].active = yes; - ms->option[1].active = yes; - ms->option[0].visible = yes; - ms->option[1].visible = yes; - mstrcpy(&ms->option[0].name,confirmstr[0]); - mstrcpy(&ms->option[1].name,confirmstr[1]); - - SET_BOX(ms->option[0].box,30,100,30,ms->option[0].box.y1+MENU_ITEM_HEIGHT); - SET_BOX(ms->option[1].box,160,230,30,ms->option[1].box.y1+MENU_ITEM_HEIGHT); -} - - -void make_message_port(void) -{ - IntBox bbox; - HelpStruct* hp; - int windex; - WindowParams mwin; - WinUnion wun; - SBoolean iconified; - - hp = &root.messages; - - hp->lines_per_page = 10; - hp->window_width = 750; - hp->window_height = 10*HELP_WINDOW_TEXT_Y_SPACING + 4; - - hp->line = (TextLine*)simm_malloc(100*sizeof(TextLine)); - if (hp->line == NULL) - error(exit_program,tool_message); - - hp->num_lines_malloced = 100; - hp->num_lines = 0; - hp->starting_line = 0; - hp->background_color = HELP_WINDOW_BACKGROUND; - - bbox.x2 = hp->window_width; - bbox.x1 = bbox.x2 - 20; - bbox.y2 = hp->window_height; - bbox.y1 = 0; - - make_slider(&hp->sl,vertical_slider,bbox,0,(double)(hp->num_lines)*20.0, - (double)(hp->lines_per_page)*20.0,(double)(hp->num_lines)*20.0,4.0,NULL,NULL); - glueSetWindowPlacement(GLUT_NORMAL_WINDOW, GLUE_SOUTHWEST, - hp->window_width, hp->window_height, - 0, 0, NULL); - -#if defined WIN32 && SIMM_DEMO_VERSION - iconified = is_in_demo_mode(); -#else - iconified = no; -#endif - mwin.id = hp->window_id = glueOpenWindow("simmmess",iconified,GLUE_TOOL_WINDOW); - - glueSetWindowMinSize(500,100); - mstrcpy(&mwin.name,"SIMM Messages"); - glutSetIconTitle("Messages"); - glutSetWindowTitle(mwin.name); - - wun.tool = NULL; - - windex = add_window(&mwin,&wun,NOTYPE,ZERO,no,draw_message_window, - update_message_window,messages_input); - if (windex == -1) - error(exit_program,tool_message); - - glutSetWindowData(mwin.id, windex); - - if (expirationMessage) - { - error(none, expirationMessage); - - FREE_IFNOTNULL(expirationMessage); - } -} - - - -public void update_message_window(WindowParams* win_parameters, WinUnion* win_struct) -{ - IntBox bbox; - HelpStruct* hp; - - if (shape_window(win_parameters,no) < CHANGED_SIZE) - return; - - hp = &root.messages; - - hp->window_width = win_parameters->xsize; - hp->window_height = win_parameters->ysize; - hp->lines_per_page = (hp->window_height-4)/20; - - bbox.x2 = hp->window_width; - bbox.x1 = bbox.x2 - 20; - bbox.y2 = hp->window_height; - bbox.y1 = 0; - - make_slider(&hp->sl,vertical_slider,bbox,0,hp->sl.value, - (double)(hp->lines_per_page)*20.0,(double)(hp->num_lines)*20.0,4.0,NULL,NULL); - - if (hp->num_lines <= hp->lines_per_page) - { - hp->starting_line = 0; - hp->sl.value = hp->sl.max_value; - hp->sl.thumb_thickness = -1; - } - else - { - if (hp->starting_line + hp->lines_per_page*20 > hp->num_lines*20) - { - hp->starting_line = (hp->num_lines - hp->lines_per_page)*20; - hp->sl.value = hp->sl.max_value - hp->starting_line; - } - hp->sl.thumb_thickness = hp->lines_per_page* - (hp->sl.shaft.y2-hp->sl.shaft.y1)/hp->num_lines; - } -} - -public void draw_message_window(WindowParams* win_parameters, WinUnion* win_struct) -{ - draw_help_window(&root.messages); -} - - -public void messages_input(WindowParams* win_parameters, WinUnion* win_struct, - SimmEvent se) -{ - if (se.field1 == window_shut) - { - do_tool_window_shut("Messages", win_parameters->id); - return; - } - - if (se.field1 != select_button || se.field2 != key_pressed) - return; - - if (check_slider(&root.messages.sl,se,move_message_text,DUMMY_INT) == yes) - draw_help_window(&root.messages); -} - - - -public void move_message_text(int dummy_int, double slider_value, double delta) -{ - root.messages.starting_line = root.messages.sl.max_value - slider_value; - - draw_help_window(&root.messages); -} - -#endif /* ! ENGINE */ - - -#if ! OPENSIM_BUILD && ! CORTEX_PLUGIN - -char* get_mocap_model(void) -{ - char* m; - const char* p = get_preference("MOCAP_MODEL"); - - if (p) - { - if (is_absolute_path(p)) - { - strcpy(buffer, p); - } - else - { - strcpy(buffer, get_preference("SIMM_FOLDER")); - append_if_necessary(buffer, DIR_SEP_CHAR); - strcat(buffer, p); - } - } - else - { - strcpy(buffer, get_preference("RESOURCES_FOLDER")); - append_if_necessary(buffer, DIR_SEP_CHAR); - strcat(buffer, "mocap\\mocap.jnt"); - } - - mstrcpy(&m, buffer); - - return m; -} -#endif - -#if INCLUDE_PERSISTENT_SIMM_STATE - -/* =========================================================================== - PERSISTENT SIMM STATE ------------------------------------------------------------------------------- */ -#if 0 - #pragma mark - - #pragma mark PERSISTENT SIMM STATE (unfinished) -#endif - -typedef struct { - char* name; - char* value; -} SimmStateVar; - -static int sSimmStateNumVars = 0; -static SimmStateVar* sSimmStateVars = NULL; - -/* --------------------------------------------------------------------------- - _free_simm_state - ------------------------------------------------------------------------------- */ -static void _free_simm_state () -{ - if (sSimmStateVars) - { - int i; - - for (i = 0; i < sSimmStateNumVars; i++) - { - FREE_IFNOTNULL(sSimmStateVars[i].name); - FREE_IFNOTNULL(sSimmStateVars[i].value); - } - FREE_IFNOTNULL(sSimmStateVars); - - sSimmStateNumVars = 0; - } -} - -/* --------------------------------------------------------------------------- - load_simm_state - ------------------------------------------------------------------------------- */ -public void load_simm_state () -{ - FILE* file = NULL; - - strcpy(buffer, get_preference("RESOURCES_FOLDER")); - append_if_necessary(buffer, DIR_SEP_CHAR); - strcat(buffer, "SimmState.txt"); - - file = simm_fopen(buffer, "r"); - - if (file) - { - int numLines = count_remaining_lines(file, no); - - _free_simm_state(); - fclose(file); - } -} - -/* --------------------------------------------------------------------------- - save_simm_state - ------------------------------------------------------------------------------- */ -public void save_simm_state () -{ - -} - -/* --------------------------------------------------------------------------- - get_simm_state_value - ------------------------------------------------------------------------------- */ -public const char* get_simm_state_value (const char* name) -{ - return NULL; -} - -/* --------------------------------------------------------------------------- - set_simm_state_value - ------------------------------------------------------------------------------- */ -public void set_simm_state_value (const char* name, const char* value) -{ - -} - -#endif /* INCLUDE_PERSISTENT_SIMM_STATE */ diff --git a/OpenSim/Utilities/simmToOpenSim/invert.c b/OpenSim/Utilities/simmToOpenSim/invert.c deleted file mode 100644 index e69d73652f..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/invert.c +++ /dev/null @@ -1,260 +0,0 @@ -/******************************************************************************* - - INVERT.C - - Author: Peter Loan - - Date: 13-APR-90 - - Copyright (c) 1992-3 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file ontains routines that invert a matrix. All three - routines were taken from "Numerical Recipes in C." - - Routines: - invert_matrix : inverts a square matrix of any size - ludcmp : used by invert() - lubksb : used by invert() - -*******************************************************************************/ - -#include "universal.h" - -#include "globals.h" -#include "functions.h" - - -/*************** DEFINES (for this file only) *********************************/ -#define TINY 1.0e-20 - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void ludcmp(double* a[], int n, int indx[], double* d); -static void lubksb(double* a[], int n, int indx[], double b[]); - - - -void invert_matrix(double* matrix[], double* inverse[], int size) -{ - - double d, col[MAX_MATRIX_SIZE]; - int i, j, indx[MAX_MATRIX_SIZE]; - - ludcmp(matrix,size,indx,&d); - for (j=0; j big) - big = temp; - if (big == 0.0) - { - error(recover,"Warning: singular matrix in routine LUDCMP"); - return; - } - vv[i] = 1.0/big; - } - - for (j=0; j= big) - { - big = dum; - imax = i; - } - } - if (j != imax) - { - for (k=0; k= 0) - for (j=k; j<=i-1; j++) - sum -= a[i][j]*b[j]; - else if (sum) - k = i; - b[i] = sum; - } - for (i=n-1; i>=0; i--) - { - sum = b[i]; - for (j=i+1; j -#include - -void pmat(int m, int n, double y[]); -int mod(int k, int m); -int min0(int a, int b); -double dmin1(double a, double b); -double dmax1(double a, double b); - -//#define BUG 0 -int JACOBIAN; - -void fdjac2( - void (*fcn)(int, int, double[], double[], int *, void *), - int m, - int n, - double x[], - double fvec[], - double fjac[], - int ldfjac, - int *iflag, - double epsfcn, - double wa[], - void *data); - -double enorm(int n, double x[]); - -void qrsolv( - int n, - double r[], - int ldr, - int ipvt[], - double diag[], - double qtb[], - double x[], - double sdiag[], - double wa[]); - -void lmdif_C( - void (*fcn)(int, int, double[], double[], int *, void *), - int m, - int n, - double x[], - double fvec[], - double ftol, - double xtol, - double gtol, - int maxfev, - double epsfcn, - double diag[], - int mode, - double factor, - int nprint, - int *info, - int *nfev, - double fjac[], - int ldfjac, - int ipvt[], - double qtf[], - double wa1[], - double wa2[], - double wa3[], - double wa4[], - void *data); - -void qrfac( - int m, - int n, - double a[], - int lda, - int pivot, - int ipvt[], - int lipvt, - double rdiag[], - double acnorm[], - double wa[]); - -void lmpar( - int n, - double r[], - int ldr, - int ipvt[], - double diag[], - double qtb[], - double delta, - double *par, - double x[], - double sdiag[], - double wa1[], - double wa2[]); - - - -#define MACHEP 1.2e-16 -#define DWARF 1.0e-38 - - - -/* -* ********** -* -* subroutine lmdif -* -* the purpose of lmdif is to minimize the sum of the squares of -* m nonlinear functions in n variables by a modification of -* the levenberg-marquardt algorithm. the user must provide a -* subroutine which calculates the functions. the jacobian is -* then calculated by a forward-difference approximation. -* -* the subroutine statement is -* -* subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, -* diag,mode,factor,nprint,info,nfev,fjac, -* ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) -* -* where -* -* fcn is the name of the user-supplied subroutine which -* calculates the functions. fcn must be declared -* in an external statement in the user calling -* program, and should be written as follows. -* -* subroutine fcn(m,n,x,fvec,iflag) -* integer m,n,iflag -* double precision x(n),fvec(m) -* ---------- -* calculate the functions at x and -* return this vector in fvec. -* ---------- -* return -* end -* -* the value of iflag should not be changed by fcn unless -* the user wants to terminate execution of lmdif. -* in this case set iflag to a negative integer. -* -* m is a positive integer input variable set to the number -* of functions. -* -* n is a positive integer input variable set to the number -* of variables. n must not exceed m. -* -* x is an array of length n. on input x must contain -* an initial estimate of the solution vector. on output x -* contains the final estimate of the solution vector. -* -* fvec is an output array of length m which contains -* the functions evaluated at the output x. -* -* ftol is a nonnegative input variable. termination -* occurs when both the actual and predicted relative -* reductions in the sum of squares are at most ftol. -* therefore, ftol measures the relative error desired -* in the sum of squares. -* -* xtol is a nonnegative input variable. termination -* occurs when the relative error between two consecutive -* iterates is at most xtol. therefore, xtol measures the -* relative error desired in the approximate solution. -* -* gtol is a nonnegative input variable. termination -* occurs when the cosine of the angle between fvec and -* any column of the jacobian is at most gtol in absolute -* value. therefore, gtol measures the orthogonality -* desired between the function vector and the columns -* of the jacobian. -* -* maxfev is a positive integer input variable. termination -* occurs when the number of calls to fcn is at least -* maxfev by the end of an iteration. -* -* epsfcn is an input variable used in determining a suitable -* step length for the forward-difference approximation. this -* approximation assumes that the relative errors in the -* functions are of the order of epsfcn. if epsfcn is less -* than the machine precision, it is assumed that the relative -* errors in the functions are of the order of the machine -* precision. -* -* diag is an array of length n. if mode = 1 (see -* below), diag is internally set. if mode = 2, diag -* must contain positive entries that serve as -* multiplicative scale factors for the variables. -* -* mode is an integer input variable. if mode = 1, the -* variables will be scaled internally. if mode = 2, -* the scaling is specified by the input diag. other -* values of mode are equivalent to mode = 1. -* -* factor is a positive input variable used in determining the -* initial step bound. this bound is set to the product of -* factor and the euclidean norm of diag*x if nonzero, or else -* to factor itself. in most cases factor should lie in the -* interval (.1,100.). 100. is a generally recommended value. -* -* nprint is an integer input variable that enables controlled -* printing of iterates if it is positive. in this case, -* fcn is called with iflag = 0 at the beginning of the first -* iteration and every nprint iterations thereafter and -* immediately prior to return, with x and fvec available -* for printing. if nprint is not positive, no special calls -* of fcn with iflag = 0 are made. -* -* info is an integer output variable. if the user has -* terminated execution, info is set to the (negative) -* value of iflag. see description of fcn. otherwise, -* info is set as follows. -* -* info = 0 improper input parameters. -* -* info = 1 both actual and predicted relative reductions -* in the sum of squares are at most ftol. -* -* info = 2 relative error between two consecutive iterates -* is at most xtol. -* -* info = 3 conditions for info = 1 and info = 2 both hold. -* -* info = 4 the cosine of the angle between fvec and any -* column of the jacobian is at most gtol in -* absolute value. -* -* info = 5 number of calls to fcn has reached or -* exceeded maxfev. -* -* info = 6 ftol is too small. no further reduction in -* the sum of squares is possible. -* -* info = 7 xtol is too small. no further improvement in -* the approximate solution x is possible. -* -* info = 8 gtol is too small. fvec is orthogonal to the -* columns of the jacobian to machine precision. -* -* nfev is an integer output variable set to the number of -* calls to fcn. -* -* fjac is an output m by n array. the upper n by n submatrix -* of fjac contains an upper triangular matrix r with -* diagonal elements of nonincreasing magnitude such that -* -* t t t -* p *(jac *jac)*p = r *r, -* -* where p is a permutation matrix and jac is the final -* calculated jacobian. column j of p is column ipvt(j) -* (see below) of the identity matrix. the lower trapezoidal -* part of fjac contains information generated during -* the computation of r. -* -* ldfjac is a positive integer input variable not less than m -* which specifies the leading dimension of the array fjac. -* -* ipvt is an integer output array of length n. ipvt -* defines a permutation matrix p such that jac*p = q*r, -* where jac is the final calculated jacobian, q is -* orthogonal (not stored), and r is upper triangular -* with diagonal elements of nonincreasing magnitude. -* column j of p is column ipvt(j) of the identity matrix. -* -* qtf is an output array of length n which contains -* the first n elements of the vector (q transpose)*fvec. -* -* wa1, wa2, and wa3 are work arrays of length n. -* -* wa4 is a work array of length m. -* -* subprograms called -* -* user-supplied ...... fcn -* -* minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac -* -* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod -* -* argonne national laboratory. minpack project. march 1980. -* burton s. garbow, kenneth e. hillstrom, jorge j. more -* -* ********** -*/ -void lmdif_C( - void (*fcn)(int, int, double[], double[], int *, void *), - int m, - int n, - double x[], - double fvec[], - double ftol, - double xtol, - double gtol, - int maxfev, - double epsfcn, - double diag[], - int mode, - double factor, - int nprint, - int *info, - int *nfev, - double fjac[], - int ldfjac, - int ipvt[], - double qtf[], - double wa1[], - double wa2[], - double wa3[], - double wa4[], - void *data) -{ - int i; - int iflag; - int ij; - int jj; - int iter; - int j; - int l; - double actred; - double delta; - double dirder; - double fnorm; - double fnorm1; - double gnorm; - double par; - double pnorm; - double prered; - double ratio; - double sum; - double temp; - double temp1; - double temp2; - double temp3; - double xnorm; - static double one = 1.0; - static double p1 = 0.1; - static double p5 = 0.5; - static double p25 = 0.25; - static double p75 = 0.75; - static double p0001 = 1.0e-4; - static double zero = 0.0; - static double p05 = 0.05; - - *info = 0; - iflag = 0; - *nfev = 0; - - /* - * check the input parameters for errors. - */ - if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < zero) - || (xtol < zero) || (gtol < zero) || (maxfev <= 0) - || (factor <= zero)) - goto L300; - - if (mode == 2) - { - /* scaling by diag[] */ - for (j=0; j 0) - { - iflag = 0; - if (mod(iter-1,nprint) == 0) - { - fcn(m,n,x,fvec,&iflag, data); - if (iflag < 0) - goto L300; - // printf( "fnorm %.15e\n", enorm(m,fvec)); - } - } - /* compute the qr factorization of the jacobian. */ - qrfac(m,n,fjac,ldfjac,1,ipvt,n,wa1,wa2,wa3); -// for (j = 0; j < n; j++) -// { -// printf("wa1[%d] = %e\n", j, wa1[j]); -// printf("wa2[%d] = %e\n", j, wa2[j]); -// printf("wa3[%d] = %e\n", j, wa3[j]); -// } - /* on the first iteration and if mode is 1, scale according - * to the norms of the columns of the initial jacobian. - */ - if (iter == 1) - { - // printf("iter = 1, mode = %d\n", mode); - if (mode != 2) - { - for (j=0; j= zero) - temp = p5; - else - temp = p5*dirder/(dirder + p5*actred); - if (((p1*fnorm1) >= fnorm) || (temp < p1)) - temp = p1; - - delta = temp*dmin1(delta,pnorm/p1); - par = par/temp; - } - else - { - if ((par == zero) || (ratio >= p75)) - { - delta = pnorm/p5; - par = p5*par; - } - } - /* test for successful iteration. */ - if (ratio >= p0001) - { - /* successful iteration. update x, fvec, and their norms. */ - for (j=0; j= maxfev) - *info = 5; - - if ((fabs(actred) <= MACHEP) && (prered <= MACHEP) && (p5*ratio <= one)) - { - *info = 6; - } - - if (delta <= MACHEP*xnorm) - *info = 7; - - if (gnorm <= MACHEP) - *info = 8; - - if (*info != 0) - goto L300; - - /* end of the inner loop. repeat if iteration unsuccessful. */ - if (ratio < p0001) - goto L200; - - /* end of the outer loop. */ - goto L30; - -L300: - - /* termination, either normal or user imposed. */ - if (iflag < 0) - *info = iflag; - - iflag = 0; - if (nprint > 0) - fcn(m,n,x,fvec,&iflag, data); -} - - -/* ********** -* -* subroutine lmpar -* -* given an m by n matrix a, an n by n nonsingular diagonal -* matrix d, an m-vector b, and a positive number delta, -* the problem is to determine a value for the parameter -* par such that if x solves the system -* -* a*x = b , sqrt(par)*d*x = 0 , -* -* in the least squares sense, and dxnorm is the euclidean -* norm of d*x, then either par is zero and -* -* (dxnorm-delta) .le. 0.1*delta , -* -* or par is positive and -* -* abs(dxnorm-delta) .le. 0.1*delta . -* -* this subroutine completes the solution of the problem -* if it is provided with the necessary information from the -* qr factorization, with column pivoting, of a. that is, if -* a*p = q*r, where p is a permutation matrix, q has orthogonal -* columns, and r is an upper triangular matrix with diagonal -* elements of nonincreasing magnitude, then lmpar expects -* the full upper triangle of r, the permutation matrix p, -* and the first n components of (q transpose)*b. on output -* lmpar also provides an upper triangular matrix s such that -* -* t t t -* p *(a *a + par*d*d)*p = s *s . -* -* s is employed within lmpar and may be of separate interest. -* -* only a few iterations are generally needed for convergence -* of the algorithm. if, however, the limit of 10 iterations -* is reached, then the output par will contain the best -* value obtained so far. -* -* the subroutine statement is -* -* subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, -* wa1,wa2) -* -* where -* -* n is a positive integer input variable set to the order of r. -* -* r is an n by n array. on input the full upper triangle -* must contain the full upper triangle of the matrix r. -* on output the full upper triangle is unaltered, and the -* strict lower triangle contains the strict upper triangle -* (transposed) of the upper triangular matrix s. -* -* ldr is a positive integer input variable not less than n -* which specifies the leading dimension of the array r. -* -* ipvt is an integer input array of length n which defines the -* permutation matrix p such that a*p = q*r. column j of p -* is column ipvt(j) of the identity matrix. -* -* diag is an input array of length n which must contain the -* diagonal elements of the matrix d. -* -* qtb is an input array of length n which must contain the first -* n elements of the vector (q transpose)*b. -* -* delta is a positive input variable which specifies an upper -* bound on the euclidean norm of d*x. -* -* par is a nonnegative variable. on input par contains an -* initial estimate of the levenberg-marquardt parameter. -* on output par contains the final estimate. -* -* x is an output array of length n which contains the least -* squares solution of the system a*x = b, sqrt(par)*d*x = 0, -* for the output par. -* -* sdiag is an output array of length n which contains the -* diagonal elements of the upper triangular matrix s. -* -* wa1 and wa2 are work arrays of length n. -* -* subprograms called -* -* minpack-supplied ... dpmpar,enorm,qrsolv -* -* fortran-supplied ... dabs,dmax1,dmin1,dsqrt -* -* argonne national laboratory. minpack project. march 1980. -* burton s. garbow, kenneth e. hillstrom, jorge j. more -* -* ********** -*/ -void lmpar( - int n, - double r[], - int ldr, - int ipvt[], - double diag[], - double qtb[], - double delta, - double *par, - double x[], - double sdiag[], - double wa1[], - double wa2[]) -{ - int i; - int iter; - int ij; - int jj; - int j; - int jm1; - int jp1; - int k; - int l; - int nsing; - double dxnorm; - double fp; - double gnorm; - double parc; - double parl; - double paru; - double sum; - double temp; - static double zero = 0.0; - static double one = 1.0; - static double p1 = 0.1; - static double p001 = 0.001; - -#ifdef BUG - printf( "lmpar\n" ); -#endif - - /* compute and store in x the gauss-newton direction. if the - * jacobian is rank-deficient, obtain a least squares solution. - */ - nsing = n; - jj = 0; - for (j=0; j= 1) - { - for (k=0; k= 0) - { - ij = ldr * j; - for (i=0; i<=jm1; i++) - { - wa1[i] -= r[ij]*temp; - ij += 1; - } - } - } - } - - for (j=0; j= n) - { - for (j=0; j= 0) - { - ij = jj; - for (i=0; i<=jm1; i++) - { - sum += r[ij]*wa1[i]; - ij += 1; - } - } - wa1[j] = (wa1[j] - sum)/r[j+ldr*j]; - jj += ldr; /* [i+ldr*j] */ - } - temp = enorm(n,wa1); - parl = ((fp/delta)/temp)/temp; - } - /* calculate an upper bound, paru, for the zero of the function. */ - jj = 0; - for (j=0; j zero) - parl = dmax1(parl, *par); - - if (fp < zero) - paru = dmin1(paru, *par); - - /* compute an improved estimate for par. */ - *par = dmax1(parl, *par + parc); - - /* end of an iteration. */ - - goto L150; - -L220: - -/* termination. */ - - if (iter == 0) - *par = zero; -} - - -/* -* ********** -* -* subroutine qrfac -* -* this subroutine uses householder transformations with column -* pivoting (optional) to compute a qr factorization of the -* m by n matrix a. that is, qrfac determines an orthogonal -* matrix q, a permutation matrix p, and an upper trapezoidal -* matrix r with diagonal elements of nonincreasing magnitude, -* such that a*p = q*r. the householder transformation for -* column k, k = 1,2,...,min(m,n), is of the form -* -* t -* i - (1/u(k))*u*u -* -* where u has zeros in the first k-1 positions. the form of -* this transformation and the method of pivoting first -* appeared in the corresponding linpack subroutine. -* -* the subroutine statement is -* -* subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) -* -* where -* -* m is a positive integer input variable set to the number -* of rows of a. -* -* n is a positive integer input variable set to the number -* of columns of a. -* -* a is an m by n array. on input a contains the matrix for -* which the qr factorization is to be computed. on output -* the strict upper trapezoidal part of a contains the strict -* upper trapezoidal part of r, and the lower trapezoidal -* part of a contains a factored form of q (the non-trivial -* elements of the u vectors described above). -* -* lda is a positive integer input variable not less than m -* which specifies the leading dimension of the array a. -* -* pivot is a logical input variable. if pivot is set true, -* then column pivoting is enforced. if pivot is set false, -* then no column pivoting is done. -* -* ipvt is an integer output array of length lipvt. ipvt -* defines the permutation matrix p such that a*p = q*r. -* column j of p is column ipvt(j) of the identity matrix. -* if pivot is false, ipvt is not referenced. -* -* lipvt is a positive integer input variable. if pivot is false, -* then lipvt may be as small as 1. if pivot is true, then -* lipvt must be at least n. -* -* rdiag is an output array of length n which contains the -* diagonal elements of r. -* -* acnorm is an output array of length n which contains the -* norms of the corresponding columns of the input matrix a. -* if this information is not needed, then acnorm can coincide -* with rdiag. -* -* wa is a work array of length n. if pivot is false, then wa -* can coincide with rdiag. -* -* subprograms called -* -* minpack-supplied ... dpmpar,enorm -* -* fortran-supplied ... dmax1,dsqrt,min0 -* -* argonne national laboratory. minpack project. march 1980. -* burton s. garbow, kenneth e. hillstrom, jorge j. more -* -* ********** -*/ -void qrfac(int m, - int n, - double a[], - int lda, - int pivot, - int ipvt[], - int lipvt, - double rdiag[], - double acnorm[], - double wa[]) -{ - int i; - int ij; - int jj; - int j; - int jp1; - int k; - int kmax; - int minmn; - double ajnorm; - double sum; - double temp; - static double zero = 0.0; - static double one = 1.0; - static double p05 = 0.05; - - /* compute the initial column norms and initialize several arrays. */ - //printf("\nqrfac\n"); - ij = 0; - for (j=0; j rdiag[kmax]) - kmax = k; - } - if (kmax == j) - goto L40; - - ij = m * j; - jj = m * kmax; - for (i=0; i kp1) - { - ik = kk + 1; - for (i=kp1; i jp1) - { - ij = jp1 + ldr * j; - for (i=jp1; i rdwarf) && (xabs < agiant)) - { - /* sum for intermediate components. */ - s2 += xabs*xabs; - continue; - } - - if (xabs > rdwarf) - { - /* sum for large components. */ - if (xabs > x1max) - { - temp = x1max/xabs; - s1 = one + s1*temp*temp; - x1max = xabs; - } - else - { - temp = xabs/x1max; - s1 += temp*temp; - } - continue; - } - /* sum for small components. */ - if (xabs > x3max) - { - temp = x3max/xabs; - s3 = one + s3*temp*temp; - x3max = xabs; - } - else - { - if (xabs != zero) - { - temp = xabs/x3max; - s3 += temp*temp; - } - } - } - /* calculation of norm. */ - if(s1 != zero) - { - temp = s1 + (s2/x1max)/x1max; - ans = x1max*sqrt(temp); - return ans; - } - if (s2 != zero) - { - if (s2 >= x3max) - temp = s2*(one+(x3max/s2)*(x3max*s3)); - else - temp = x3max*((s2/x3max)+(x3max*s3)); - ans = sqrt(temp); - } - else - { - ans = x3max*sqrt(s3); - } - return ans; -} - - -/* -* ********** -* -* subroutine fdjac2 -* -* this subroutine computes a forward-difference approximation -* to the m by n jacobian matrix associated with a specified -* problem of m functions in n variables. -* -* the subroutine statement is -* -* subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) -* -* where -* -* fcn is the name of the user-supplied subroutine which -* calculates the functions. fcn must be declared -* in an external statement in the user calling -* program, and should be written as follows. -* -* subroutine fcn(m,n,x,fvec,iflag) -* integer m,n,iflag -* double precision x(n),fvec(m) -* ---------- -* calculate the functions at x and -* return this vector in fvec. -* ---------- -* return -* end -* -* the value of iflag should not be changed by fcn unless -* the user wants to terminate execution of fdjac2. -* in this case set iflag to a negative integer. -* -* m is a positive integer input variable set to the number -* of functions. -* -* n is a positive integer input variable set to the number -* of variables. n must not exceed m. -* -* x is an input array of length n. -* -* fvec is an input array of length m which must contain the -* functions evaluated at x. -* -* fjac is an output m by n array which contains the -* approximation to the jacobian matrix evaluated at x. -* -* ldfjac is a positive integer input variable not less than m -* which specifies the leading dimension of the array fjac. -* -* iflag is an integer variable which can be used to terminate -* the execution of fdjac2. see description of fcn. -* -* epsfcn is an input variable used in determining a suitable -* step length for the forward-difference approximation. this -* approximation assumes that the relative errors in the -* functions are of the order of epsfcn. if epsfcn is less -* than the machine precision, it is assumed that the relative -* errors in the functions are of the order of the machine -* precision. -* -* wa is a work array of length m. -* -* subprograms called -* -* user-supplied ...... fcn -* -* minpack-supplied ... dpmpar -* -* fortran-supplied ... dabs,dmax1,dsqrt -* -* argonne national laboratory. minpack project. march 1980. -* burton s. garbow, kenneth e. hillstrom, jorge j. more -* -********** -*/ -//#define BUG 0 -void fdjac2(void (*fcn)(int, int, double[], double[], int *, void *), - int m, - int n, - double x[], - double fvec[], - double fjac[], - int ldfjac, - int *iflag, - double epsfcn, - double wa[], - void *data) -{ - int i; - int j; - int ij; - double eps; - double h; - double temp; - static double zero = 0.0; - - //dkb - changed to equal TvdB solver code, use constant delta = 1e-5 - // temp = dmax1(epsfcn,MACHEP); - // eps = sqrt(temp); - - JACOBIAN = 1; - eps = 1e-5; - -#ifdef BUG - printf( "fdjac2\n" ); -#endif - - ij = 0; - for (j=0; j= b) - return a; - else - return b; -} - -double dmin1(double a, double b) -{ - if (a <= b) - return a; - else - return b; -} - -int min0(int a, int b) -{ - if (a <= b) - return a; - else - return b; -} - -int mod(int k, int m) -{ - return k % m; -} - - -void pmat(int m, int n, double y[]) -{ - int i; - int j; - int k; - - k = 0; - for (i=0; i= 0; j--) - { - path[i++] = -1 * loop->joints[j]; - } - path[i] = 0; - } - else - { - /* make a list of joints from the given segment to the end of the - * joint array. Joints used in forward direction */ - i = 0; - for (j = seg; j < loop->num_jnts; j++) - { - path[i++] = loop->joints[j]; - } - path[i] = 0; - } - return (i); -} - -/* Set the gencoord values without affecting the display. - * If the given gencoord value is different than the value in the model, - * reset the value and invalidate all transformation matrices that use - * that gencoord. If the gencoord value has not changed, return 0. If the - * gencoord is clamped and the value is out of range, return 2. Else, - * return 1. - */ -int setGencoordValue2(ModelStruct *ms, GeneralizedCoord* gencoord, double value) -{ - int i; - - /* if the new value is the same as the old value, check if the value - * is out of range and return. - */ - if (DABS(value - gencoord->value) <= gencoord->tolerance) - { - if (gencoord->clamped == yes) - { - if (value < gencoord->range.start) - return 2; - if (value > gencoord->range.end) - return 3; - } - return 0; - } - - if (gencoord->type == rotation_gencoord) - checkGencoordRange(gencoord, &value); - gencoord->value = value; - - /* invalidate all joint matrices that use the gencoord */ - for (i=0; inumjoints; i++) - invalidate_joint_matrix(ms, &ms->joint[gencoord->jointnum[i]]); - - if (gencoord->clamped == yes) - { - if (value < gencoord->range.start) - return 2; - if (value > gencoord->range.end) - return 3; - } - - return 1; -} - -/* convert a point from one frame to another across a loop joint. - * Don't use the paths between the bodies since these don't include - * the loop joint. If one of the bodies is ground, still use the - * the actual joint given so the loop joint is used to calculate - * the transform */ -int convert2(ModelStruct *ms, double p[], int n1, int n2) -{ - - int i; - double result[3]; - DMatrix* mat; - - if (n1 == n2) - return (0); - - /* If you want to convert a point in A to B, and the joint is defined A -> B, - * then you need to use the inverse transformation matrix. - */ - for (i = 0; i < ms->numjoints; i++) - { - if ((ms->joint[i].from == n1) && (ms->joint[i].to == n2)) - { - mat = get_conversion(ms, i, INVERSE); - break; - } - else if ((ms->joint[i].from == n2) && (ms->joint[i].to == n1)) - { - mat = get_conversion(ms, i, FORWARD); - break; - } - } - if (i == ms->numjoints) - { - printf("No joint from %d to %d\n", n1, n2); - } - - result[0] = p[0]*(*mat)[0][0] + p[1]*(*mat)[1][0] + - p[2]*(*mat)[2][0] + (*mat)[3][0]; - result[1] = p[0]*(*mat)[0][1] + p[1]*(*mat)[1][1] + - p[2]*(*mat)[2][1] + (*mat)[3][1]; - result[2] = p[0]*(*mat)[0][2] + p[1]*(*mat)[1][2] + - p[2]*(*mat)[2][2] + (*mat)[3][2]; - - p[0] = result[0]; - p[1] = result[1]; - p[2] = result[2]; - - return 1; - -} - - -/* create structures to hold info about each closed loop. Store joints - * and segments in the loops. */ -SBoolean makeLoops(ModelStruct *ms) -{ -int i, j, m, n, index, segments; -int from, to, jnt, dir; -int num_loops, num_segs, num_jnts; -char buffer[255]; -int loop_joint, user_loop, num_user_loop_joints; -SBoolean changed; - - if (ms->numclosedloops == 0) - return no; - - segments = ms->numsegments; - ms->loop = (LoopStruct *)simm_malloc(ms->numclosedloops * sizeof(LoopStruct)); - - for (i = 0; i < ms->numclosedloops; i++) - { - ms->loop[i].model_num = ms->modelnum; - ms->loop[i].ms = ms; - ms->loop[i].first_iter = yes; - } - - num_loops = 0; - for (i = 0; i < ms->numjoints; i++) - { - if (ms->joint[i].loop_joint == yes) - { - num_jnts = 0; - num_segs = 0; - index = ms->joint[i].from * segments + ms->joint[i].to; - for (j = 0; ms->pathptrs[index][j] != (ms->numjoints + 1); j++) - num_jnts++; - - num_jnts += 1; - num_segs = num_jnts + 1; - ms->loop[num_loops].segs = (int *)simm_malloc(num_segs * sizeof(int)); - ms->loop[num_loops].joints = (int *)simm_malloc(num_jnts * sizeof(int)); - ms->loop[num_loops].num_segs = num_segs; - ms->loop[num_loops].num_jnts = num_jnts; - num_loops++; - } - } - - /* if there are any closed loops in the model, store which - * segments are used in the closed loop. Look at the bodies in - * the loop joint, and copy the path connecting the two bodies (m->n) (which - * goes around the loop without going through loop joint), then add the - * beginning segment (m) to the end of the list. - */ - num_loops = 0; - for (i = 0; i < ms->numjoints; i++) - { - if (ms->joint[i].loop_joint == yes) - { - m = ms->joint[i].from; - n = ms->joint[i].to; - - index = m * segments + n; - if (ms->pathptrs[index] == NULL) - { - (void)sprintf(buffer, - "No path between %s and %s in closed loop", ms->segment[m].name, - ms->segment[n].name); - error(none, buffer); - } - /* copy existing path between m and n */ - for (j = 0; ms->pathptrs[index][j] != (ms->numjoints + 1); j++) - ms->loop[num_loops].joints[j] = ms->pathptrs[index][j]; - - /* have path from m->n now add path from n->m (loop joint), reverse dir */ - ms->loop[num_loops++].joints[j] = -(i + 1); - } - } - - for (i = 0; i < num_loops; i++) - { - for (j = 0; j < ms->loop[i].num_jnts; j++) - { - jnt = ms->loop[i].joints[j]; - if (jnt > 0) - { - dir = FORWARD; - jnt = jnt - 1; - from = ms->joint[jnt].from; - to = ms->joint[jnt].to; - } - else - { - dir = INVERSE; - jnt = abs(jnt) - 1; - to = ms->joint[jnt].from; - from = ms->joint[jnt].to; - } - if (j == 0) - ms->loop[i].segs[0] = from; - ms->loop[i].segs[j+1] = to; - } - } - - changed = no; - for (i = 0; i < ms->numclosedloops; i++) - { - loop_joint = -1; - user_loop = -1; - num_user_loop_joints = 0; - for (j = 0; j < ms->loop[i].num_jnts; j++) - { - jnt = abs(ms->loop[i].joints[j]) - 1; - if (ms->joint[jnt].user_loop == yes) - { - user_loop = jnt; - ms->joint[jnt].user_loop = no; - num_user_loop_joints++; - } - if (ms->joint[jnt].loop_joint == yes) - loop_joint = jnt; - } - if ((user_loop != -1) && (user_loop != loop_joint)) - { - ms->joint[loop_joint].loop_joint = no; - ms->joint[user_loop].loop_joint = yes; - changed = yes; - } - if (num_user_loop_joints > 1) - { - sprintf(errorbuffer, "Too many loopjoints defined in model for loop %d.\n", i); - error(none, errorbuffer); - } - } - -////////////// display - /* - for (i = 0; i < ms->numclosedloops; i++) - { - printf("loopjoint%d\n", i); - for (j = 0; j < ms->loop[i].num_jnts; j++) - { - jnt = ms->loop[i].joints[j]; - if (jnt > 0) - jnt = jnt - 1; - else - jnt = abs(jnt) - 1; - printf("%s ", ms->joint[jnt].name); - } - printf("\n"); - printf("segs = \n"); - for (j = 0; j < ms->loop[i].num_segs; j++) - printf("%s ", ms->segment[ms->loop[i].segs[j]].name); - printf("\n"); - } -*/ - //////////////// - - return changed; -} - - -/* store the number of qs and residuals for each each loop. Store the - * gencoord indices for each q */ -void updateLoopInfo(ModelStruct *ms) -{ - int i, j, k, nq, jnt; - GeneralizedCoord* gc; - - for (i = 0; i < ms->numclosedloops; i++) - { - nq = 0; - ms->loop[i].qs = (GeneralizedCoord**)simm_malloc(6 * ms->loop[i].num_jnts * sizeof(GeneralizedCoord*)); - for (j = 0; j < ms->loop[i].num_jnts; j++) - { - jnt = abs(ms->loop[i].joints[j]) - 1; - for (k = 0; k < 6; k++) - { - gc = ms->joint[jnt].dofs[k].gencoord; - if (gc && gc->used_in_model == yes) - { - gc->used_in_loop = yes; - ms->loop[i].qs[nq] = gc; - nq++; - } - } - } - ms->loop[i].num_qs = nq; - ms->loop[i].num_resids = 3 * ms->loop[i].num_jnts; - } - -#if EXPERIMENTAL - if (DEBUG_LEVEL > 0) - { - printf("\nStore loop info\n"); - for (i = 0; i < ms->numclosedloops; i++) - { - printf("loop%d:\n", i); - printf(" num_jnts = %d\n num_segs = %d\n", ms->loop[i].num_jnts, - ms->loop[i].num_segs); - printf(" num_qs = %d\n num_resids = %d\n", ms->loop[i].num_qs, - ms->loop[i].num_resids); - printf(" jnts: "); - for (j = 0; j < ms->loop[i].num_jnts; j++) - printf(" %d", abs(ms->loop[i].joints[j]) -1); - printf("\n segs: ", i); - for (j = 0; j < ms->loop[i].num_segs; j++) - printf("%d ", ms->loop[i].segs[j]); - printf("\n"); - printf("\n segs: ", i); - for (j = 0; j < ms->loop[i].num_segs; j++) - printf("%s ", ms->segment[ms->loop[i].segs[j]].name); - printf("\n"); -/* printf(" qs: "); - for (j = 0; j < ms->loop[i].num_qs; j++) - printf("%d ", ms->loop[i].qs[j]); - printf("\n"); -*/ - printf(" qs: "); - for (j = 0; j < ms->loop[i].num_qs; j++) - printf("%s ", ms->loop[i].qs[j]->name); - printf("\n"); - } - } -#endif -} - -/* Determine whether any closed loops exist in the model. If so, mark - * one of the joints in the closed loop as the loop joint. To determine - * whether closed loops exist, go through the joint list and mark each segment - * used in the joint. If both segments in a joint have already been used, - * mark that joint as the loop joint. - */ -void markLoopJoints(ModelStruct *ms) -{ - int i, from, to, joints_handled, joints_handled_last_time, loop_jnt = -1; - SBoolean *segment_used, *joint_used; - - /* initialize segment_used array */ - segment_used = (SBoolean *)simm_malloc(ms->numsegments * sizeof(SBoolean)); - for (i = 0; i < ms->numsegments; i++) - segment_used[i] = no; - - /* initialize joint_used array */ - joint_used = (SBoolean *)simm_malloc(ms->numjoints * sizeof(SBoolean)); - for (i = 0; i < ms->numjoints; i++) - joint_used[i] = no; - - /* find closed loops. Look at each joint and see if the segments defining - * it have been used before: - * if both have been used, the joint closes a loop - * if one has been used, mark the other as used - * if neither has been used, you can't process the joint yet. - * Keep going until all joints have been processed. - */ - ms->numclosedloops = 0; - segment_used[0] = yes; - joints_handled = 0; - - while (joints_handled < ms->numjoints) - { - joints_handled_last_time = joints_handled; - for (i=0; inumjoints; i++) - { - if (joint_used[i] == no) - { - from = ms->joint[i].from; - to = ms->joint[i].to; - if ((segment_used[from] == yes) && (segment_used[to] == yes)) - { - ms->joint[i].loop_joint = yes; - joint_used[i] = yes; - ms->numclosedloops++; - joints_handled++; - } - else if (segment_used[from] == yes) - { - segment_used[to] = yes; - joint_used[i] = yes; - joints_handled++; - } - else if (segment_used[to] == yes) - { - segment_used[from] = yes; - joint_used[i] = yes; - joints_handled++; - } - } - } - /* If you did not process any joints this time around, - * then there's a problem (e.g., a missing joint). - * So break out and let makepaths() report the error later. - */ - if (joints_handled_last_time == joints_handled) - break; - } - -#if EXPERIMENTAL - if (DEBUG_LEVEL > 0) - { - printf("Model %s: %d closed loops\n", ms->name, ms->numclosedloops); - for (i = 0; i < ms->numjoints; i++) - { - printf("joint %d (%s->%s)", i, ms->segment[ms->joint[i].from].name, - ms->segment[ms->joint[i].to].name); - if (ms->joint[i].loop_joint == yes) - printf(" - loop joint"); - printf("\n"); - } - for (i = 0; i < ms->numgencoords; i++) - { - printf("gencoord%d = %-20s", i, ms->gencoord[i]->name); - printf(": range % 10.3f -> % 10.3f\n", ms->gencoord[i]->range.start, - ms->gencoord[i]->range.end); - } - } -#endif - - free(segment_used); - free(joint_used); -} - - -void checkGencoordRange(GeneralizedCoord* gencoord, double *value) -{ - long count; - double min1, min2, min, max1, max2, max, mid; - - if (gencoord->clamped == yes) - return; - - mid = (gencoord->range.start + gencoord->range.end)/2; - max1 = mid + 180; - min1 = mid - 180; - - min2 = gencoord->range.start; - max2 = gencoord->range.end; - - max = _MAX(max1, max2) + 180; - min = _MIN(min1, min2) - 180; - - if (*value > max) - { - count = (*value - max + 359.99999) / 360.0; - *value -= 360.0 * count; - } - else if (*value < min) - { - count = (min - *value + 359.99999) / 360.0; - *value += 360.0 * count; - } -} diff --git a/OpenSim/Utilities/simmToOpenSim/macros.h b/OpenSim/Utilities/simmToOpenSim/macros.h deleted file mode 100644 index 72699ff497..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/macros.h +++ /dev/null @@ -1,118 +0,0 @@ -/******************************************************************************* - - MACROS.H - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef MACROS_H -#define MACROS_H - -#define ABS(a) ((a)>0?(a):(-(a))) -#define DABS(a) ((a)>(double)0.0?(a):(-(a))) -#define EQUAL_WITHIN_ERROR(a,b) (DABS(((a)-(b))) <= ROUNDOFF_ERROR) -#define NOT_EQUAL_WITHIN_ERROR(a,b) (DABS(((a)-(b))) > ROUNDOFF_ERROR) -#define EQUAL_WITHIN_TOLERANCE(a,b,c) (DABS(((a)-(b))) <= (c)) -#define NOT_EQUAL_WITHIN_TOLERANCE(a,b,c) (DABS(((a)-(b))) > (c)) -#define NEAR_LT_OR_EQ(a, b) ((a) <= (b) + ROUNDOFF_ERROR) -#define NEAR_GT_OR_EQ(a, b) ((a) >= (b) - ROUNDOFF_ERROR) -#define SIGN(a) ((a)>=0?(1):(-1)) -#define _MAX(a,b) ((a)>=(b)?(a):(b)) -#define _MIN(a,b) ((a)<=(b)?(a):(b)) -#define SQR(x) ((x) * (x)) -#define CUBE(x) ((x) * (x) * (x)) -#define DSIGN(a) ((a)>=0.0?(1):(-1)) -#define CEILING(a,b) (((a)+(b)-1)/(b)) -#define SETCOLOR(i,r,g,b) {root.color.cmap[i].red=r; root.color.cmap[i].green=g; \ -root.color.cmap[i].blue=b;} -#define POP3FROMQ qread(&val);qread(&val);qread(&val) -#define IS_ODD(a) ((a) & 0x01) -#define IS_EVEN(a) ( ! IS_ODD(a)) -#define FREE_IFNOTNULL(p) {if (p != NULL) {free(p); p = NULL;}} -#define STRLEN(p) (strlen(p)+1) -#define CURSOR_IN_BOX(mx,my,vp) ((SBoolean) ((mx) >= (vp).x1 && (mx) <= (vp).x2 && \ -(my) >= (vp).y1 && (my) <= (vp).y2)) -#define CURSOR_IN_VIEWPORT(mx,my,vp) ((SBoolean) ((mx) >= (vp)[0] && (mx) <= ((vp)[0] + (vp)[2]) && \ -(my) >= (vp)[1] && (my) <= ((vp)[1] + (vp)[3]))) -#define DISTANCE_FROM_MIDPOINT(mx,vp) ((mx) - (vp)[0] - ((vp)[2] * 0.5)) -#define PERCENT_FROM_MIDPOINT(mx,vp) ((double)(((mx) - (vp)[0] - ((vp)[2] * 0.5)) / ((vp)[2] * 0.5))) -#define VECTOR_MAGNITUDE(vec) (sqrt((vec[0]*vec[0])+(vec[1]*vec[1])+(vec[2]*vec[2]))) -#define CHAR_IS_WHITE_SPACE(ch) ((ch) == ' ' || (ch) == '\t' || (ch) == '\n' || (ch) == '\r') -#define CHAR_IS_NOT_WHITE_SPACE(ch) ((ch) != ' ' && (ch) != '\t' && (ch) != '\n' && (ch) != '\r') -#define NULLIFY_STRING(str) ((str)[0] = '\0') -#define STRING_IS_NULL(str) (str[0] == '\0') -#define STRING_IS_NOT_NULL(str) (str[0] != '\0') -#define STRINGS_ARE_EQUAL(ptr1,ptr2) (!strcmp(ptr1,ptr2)) -#define STRINGS_ARE_NOT_EQUAL(ptr1,ptr2) (strcmp(ptr1,ptr2)) -#define STRINGS_ARE_EQUAL_CI(ptr1,ptr2) (!stricmp(ptr1,ptr2)) -#define STRINGS_ARE_NOT_EQUAL_CI(ptr1,ptr2) (stricmp(ptr1,ptr2)) -#define FORCE_VALUE_INTO_RANGE(a,b,c) if ((a)<(b)) (a)=(b); else if ((a)>(c)) (a)=(c); -#define MODULATE_VALUE_INTO_RANGE(a,b,c) while ((a)<(b)) (a)+=((c)-(b)); \ -while ((a)>(c)) (a)-=((c)-(b)); -#define BOX_BIG_ENOUGH(box) (((box.x2 - box.x1 < 10) || (box.y2 - box.y1 < 10)) ? 0 : 1) -#define CHECK_DIVIDE(t,b) ((ABS(b))pathptrs[ms->numsegments*f1+f2]) -#define MAKE_3DVECTOR(pt1,pt2,pt3) {pt3[XX]=pt2[XX]-pt1[XX];pt3[YY]=pt2[YY]-pt1[YY];pt3[ZZ]=pt2[ZZ]-pt1[ZZ];} -#define MAKE_3DVECTOR21(pt1,pt2,pt3) {pt3[XX]=pt1[XX]-pt2[XX];pt3[YY]=pt1[YY]-pt2[YY];pt3[ZZ]=pt1[ZZ]-pt2[ZZ];} -#define DOT_VECTORS(u,v) ( (u[0])*(v[0]) + (u[1])*(v[1]) + (u[2])*(v[2]) ) -#define CALC_DETERMINANT(m) (((m[0][0]) * ((m[1][1])*(m[2][2]) - (m[1][2])*(m[2][1]))) - \ -((m[0][1]) * ((m[1][0])*(m[2][2]) - (m[1][2])*(m[2][0]))) + \ -((m[0][2]) * ((m[1][0])*(m[2][1]) - (m[1][1])*(m[2][0])))) -#define ANGLES_APX_EQ(x, y) (DABS((x) - (y)) <= ANGLE_EPSILON) -#define PTS_ARE_EQUAL(Pt1, Pt2) (BOOL_APX_EQ(Pt1[0], Pt2[0]) && \ - BOOL_APX_EQ(Pt1[1], Pt2[1]) && \ - BOOL_APX_EQ(Pt1[2], Pt2[2])) - -#define PTS_ARE_EQUAL3(Pt1, Pt2) (BOOL_APX_EQ3(Pt1[0], Pt2[0]) && \ - BOOL_APX_EQ3(Pt1[1], Pt2[1]) && \ - BOOL_APX_EQ3(Pt1[2], Pt2[2])) - -#define READ4(ptr,fp) {\ -*(((char*)(ptr)) )=getc(fp);\ -*(((char*)(ptr))+1)=getc(fp);\ -*(((char*)(ptr))+2)=getc(fp);\ -*(((char*)(ptr))+3)=getc(fp);\ -} - -#define COPY_1X4VECTOR(from,to) {\ -to[0] = from[0];\ -to[1] = from[1];\ -to[2] = from[2];\ -to[3] = from[3];\ -} -#define COPY_1X3VECTOR(from,to) {\ -to[0] = from[0];\ -to[1] = from[1];\ -to[2] = from[2];\ -} - -#define MAKE_IDENTITY_MATRIX(mat) {\ -mat[0][1] = mat[0][2] = mat[0][3] = 0.0;\ -mat[1][0] = mat[1][2] = mat[1][3] = 0.0;\ -mat[2][0] = mat[2][1] = mat[2][3] = 0.0;\ -mat[3][0] = mat[3][1] = mat[3][2] = 0.0;\ -mat[0][0] = mat[1][1] = mat[2][2] = mat[3][3] = 1.0;\ -} - -#define ADD_VECTORS(v1, v2, out) {\ -out[0] = v1[0] + v2[0];\ -out[1] = v1[1] + v2[1];\ -out[2] = v1[2] + v2[2];\ -} -#define SUB_VECTORS(v1, v2, out) {\ -out[0] = v1[0] - v2[0];\ -out[1] = v1[1] - v2[1];\ -out[2] = v1[2] - v2[2];\ -} - -#endif /*MACROS_H*/ - diff --git a/OpenSim/Utilities/simmToOpenSim/main.h b/OpenSim/Utilities/simmToOpenSim/main.h deleted file mode 100644 index 53eaac84d4..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/main.h +++ /dev/null @@ -1,62 +0,0 @@ -/******************************************************************************* - - MAIN.H - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992-2005 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef MAIN_H -#define MAIN_H - -#include "functions.h" - -ModelStruct* gModel[MODELBUFFER]; -Scene* gScene[SCENEBUFFER]; - -#if ! ENGINE -PlotStruct* gPlot[PLOTBUFFER]; -ToolStruct tool[TOOLBUFFER]; -#endif - -#if ! OPENSIM_BUILD -RootStruct root; -#endif - -char msg[CHARBUFFER]; -char buffer[CHARBUFFER*10]; -char errorbuffer[CHARBUFFER]; -double origin[] = {0.0, 0.0, 0.0}; -double x_axis[] = {1.0, 0.0, 0.0}; -double y_axis[] = {0.0, 1.0, 0.0}; -double z_axis[] = {0.0, 0.0, 1.0}; - -char program_full_name[] = "Software for Interactive Musculoskeletal Modeling"; - -#if SIMM_VIEWER -char program_name[] = "SIMM Viewer"; -#else -char program_name[] = "SIMM"; -#endif -char program_version[] = "5.0a3"; -char program_with_version[20]; - -SBoolean pipe_included = no; -SBoolean motion_post_included = no; -SBoolean motion_real_included = no; - -char program_date[] = __DATE__; -char copyright_notice[] = "Copyright (c) 1992-2009 MusculoGraphics (a division of Motion Analysis Corp.)"; -char memory_message[] = "Ran out of memory."; -char tool_message[] = "Could not make SIMM tools."; - -/* char new_ascii_label[] = "NORM_ASCII"; multiply defined in normio.h */ - -#endif /*MAIN_H*/ - diff --git a/OpenSim/Utilities/simmToOpenSim/makemodel.c b/OpenSim/Utilities/simmToOpenSim/makemodel.c deleted file mode 100644 index f05f90320c..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/makemodel.c +++ /dev/null @@ -1,2728 +0,0 @@ -/******************************************************************************* - - MAKEMODEL.C - - Author: Peter Loan - - Date: 26-APR-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines that read in bone - descriptions and make bone objects. - - Routines: - add_model : main routine to make a model and its bones - make_gencoord_sliders : makes the gencoord slider bars - get_modelstruct : mallocs a model structure for new model - add_model_window : creates a model window - post_process_bones : sets some bone variables after files are read - make_modelpopups : makes the pop-up menus for a model - scene_input : input handler for model window - model_deletion_confirm : confirms the deletion of a model - check_definitions : make sure all joint file elements were defined - size_model : determines good size for the model window - -*******************************************************************************/ - -#include -#include -#include -#include - -#include "universal.h" - -#include "globals.h" -#include "functions.h" -#include "normio.h" -#if ! OPENSIM_BUILD -#include "modelviewer.h" -#include "password.h" -#endif - - -/*************** DEFINES (for this file only) *********************************/ -#define WINDOW_WIDTH 420 -#define WINDOW_MARGIN 160 - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static char* model_deletion_text = "Are you sure that you want to delete this model?"; -static Scene* scene_to_confirm; -static char* rot_label[] = {"X","Y","Z"}; - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ -extern char badLoopErrorMsg[]; -extern char gencoordResidualErrorMsg[]; -extern char badConstraintErrorMsg[]; -extern char badGencoordErrorMsg2[]; - -/*************** GLOBAL VARIABLES (used in only a few files) *****************/ -char archive_password[] = "P.i3h78sw,l1"; - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static SBoolean joint_uses_segment(ModelStruct* ms, int jointnum, int segmentnum, - int* other_segment); -static ReturnCode make_gencoord_sliders(ModelStruct* ms); -static ReturnCode add_model_window(ModelStruct* ms, int suggested_width, - int suggested_height); -static void make_modelpopups(ModelStruct* ms); -#if ! ENGINE -static void init_tracked_file_options(ModelStruct* model, glutTRCOptions* options, const char inputFileName[]); -static void handle_right_click(Scene* scene, int mouse_x, int mouse_y); -static void call_simm_event_handler(const char tool_id[], SimmEvent se); -#endif -static double initializeEmgWindowSize(); -static int initializeEmgSmoothingPasses(); -static ReturnCode add_model_to_scene(Scene* scene, ModelStruct* model); - - -Scene* get_scene(void) -{ - int i; - Scene* sc; - - for (i=0; iscene_num = i; - - return sc; -} - - -ModelStruct* get_modelstruct(void) -{ - int i; - ModelStruct* ms; - - for (i=0; imodelnum = i; - - return ms; -} - - -ModelStruct* get_model_by_name(const char name[]) -{ - int i; - - for (i=0; iname, name)) - return gModel[i]; - - return NULL; -} - -#if ! ENGINE -#if ! OPENSMAC -#if ! CORTEX_PLUGIN - -/* ADD_MODEL: this routine controls the making of a model. It gets a model-file - * name and a muscle-file name from the user and reads-in the model definition. - * After some processing and arranging of the definition, it sets up a model - * window to display it. - */ -ReturnCode add_model(char jointfilename[], char musclefilename[], - int suggested_win_width, int* modelIndex, SBoolean showTopLevelMessages) -{ - int i, j, window_width, window_height; - ModelStruct* ms; - Scene* scene; - MotionSequence* motion = NULL; - SBoolean muscle_file_exists = no; - char fullpath[CHARBUFFER]; - ReturnCode rc; - - /* For now, make a new scene for each model. */ - scene = get_scene(); - ms = get_modelstruct(); - - if (scene == NULL || ms == NULL) - { - if (showTopLevelMessages == yes) - error(none,"Unable to make new model (perhaps too many models already)"); - return code_bad; - } - - init_scene(scene); - - if (init_model(ms) == code_bad) - { - if (showTopLevelMessages == yes) - error(none,"Unable to add another model."); - return code_bad; - } - - strcpy(fullpath, jointfilename); - - if (read_model_file(scene, ms, fullpath, showTopLevelMessages) == code_bad) - { - free_model(ms->modelnum); - if (showTopLevelMessages == yes) - error(none,"Unable to load model."); - return code_bad; - } - - if (check_definitions(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - find_ground_joint(ms); /* must be called before makepaths() ! */ - - /* determine whether the model has any closed loops and if so, set one - * of the joints as the loop joint. */ - markLoopJoints(ms); - if (ms->numclosedloops == 0) - ms->useIK = no; - - if (makepaths(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - /* create structure to hold loop information and store all info - * If the loop joint was changed (user had entered a different one), - * recalculate the paths and remake the loops */ - if (makeLoops(ms) == yes) - { - if (makepaths(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - makeLoops(ms); - } - updateLoopInfo(ms); - - /* determine which gencoords are affected by constraints in the model - * and create the structure to hold constraint information. */ - markAffectedGencoords(ms); - updateConstraintInfo(ms); - - if (find_segment_drawing_order(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - if ( ! is_in_demo_mode()) - { - if (showTopLevelMessages == yes) - { - (void)sprintf(buffer,"Read joint file %s", fullpath); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); - } - } - - /* try to load muscle file, first check for a muscle file specified - * within the joint file. - */ - if (ms->musclefilename && - STRINGS_ARE_NOT_EQUAL(ms->musclefilename, "NULL") && - STRINGS_ARE_NOT_EQUAL(ms->musclefilename, "null") && - STRINGS_ARE_NOT_EQUAL(ms->musclefilename, "no_muscles_for_this_model")) - { - if (musclefilename && strlen(musclefilename) > 0) - { - char* mname = strrchr(musclefilename, DIR_SEP_CHAR); - - if (mname) - mname++; - else - mname = musclefilename; - - if (showTopLevelMessages == yes) - { - sprintf(buffer, "Overriding muscle file specified in joint file (%s)", ms->musclefilename); - error(none, buffer); - sprintf(buffer, "with muscle file chosen in file browser (%s).", mname); - error(none, buffer); - } - - FREE_IFNOTNULL(ms->musclefilename); - mstrcpy(&ms->musclefilename, musclefilename); - strcpy(fullpath, musclefilename); - } - else - { - char* purePath = NULL; - get_pure_path_from_path(ms->jointfilename, &purePath); - build_full_path(purePath, ms->musclefilename, fullpath); - FREE_IFNOTNULL(purePath); - } - } - else if (musclefilename != NULL) /* try user-selected musclefile */ - { - strcpy(fullpath, musclefilename); - } - else - { - fullpath[0] = '\0'; - } - - /* if a muscle file was found, try to load it: - */ - if (fullpath[0] && read_muscle_file(ms, fullpath, &muscle_file_exists, showTopLevelMessages) == code_bad) - { - free_model(ms->modelnum); - if (showTopLevelMessages == yes) - error(none,"Unable to load muscle definitions"); - return code_bad; - } - - /* If you made it to here, then the joint and muscle files were both valid, - * and so another model will be added to the system. - */ - root.nummodels++; - root.modelcount++; - - /* If a model name was not specified in the joints file, make a default one. - * If a name was specified but it's the same as the name of an existing model, - * append a suffix so it's a little different. - */ - if (ms->name == NULL) - { - (void)sprintf(buffer,"model %d", root.modelcount); - ms->name = (char*)simm_malloc(250*sizeof(char)); - if (ms->name == NULL) - { - free_model(ms->modelnum); - return code_bad; - } - (void)strcpy(ms->name,buffer); - } - else - { - for (i=0; imodelnum) - continue; - if (STRINGS_ARE_EQUAL(ms->name, gModel[i]->name)) - { - (void)sprintf(buffer," (%d)", ms->modelnum + 1); - (void)strcat(ms->name, buffer); - break; - } - } - } - - if (muscle_file_exists == yes && ms->musclefilename == NULL) - mstrcpy(&ms->musclefilename, musclefilename); - - if (makegencform(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - if (init_gencoords(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - set_gencoord_info(ms); - - if (init_model_display(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - if (make_gencoord_sliders(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - if (make_dynparams_form(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - post_process_bones(ms); - - /* solve any loops or constraints in the system */ - solve_initial_loops_and_constraints(ms); - - if (make_muscle_menus(ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - - make_modelpopups(ms); - - /* Now that you know how many of each thing there are (segments, muscle groups, etc.), - * realloc the arrays to free unused space. Don't realloc the muscle lists, function lists, - * muscle groups, world objects, and gencoords since the user can add more of them interactively. - * rc should always be code_fine since you're reallocing to a smaller size. - */ - if (ms->numsegments > 0) - { - ms->segment = (SegmentStruct*)simm_realloc(ms->segment, ms->numsegments*sizeof(SegmentStruct),&rc); - } - else - { - FREE_IFNOTNULL(ms->segment); - } - if (ms->numjoints > 0) - { - ms->joint = (JointStruct*)simm_realloc(ms->joint, ms->numjoints*sizeof(JointStruct),&rc); - } - else - { - FREE_IFNOTNULL(ms->joint); - } - - ms->wrap_object_array_size = ms->num_wrap_objects; - if (ms->wrap_object_array_size == 0) - ms->wrap_object_array_size = 1; - ms->wrapobj = (dpWrapObject**)simm_realloc(ms->wrapobj, - ms->wrap_object_array_size*sizeof(dpWrapObject*),&rc); - - ms->constraint_object_array_size = ms->num_constraint_objects; - if (ms->constraint_object_array_size == 0) - ms->constraint_object_array_size = CONSTRAINT_OBJECT_ARRAY_INCREMENT; - ms->constraintobj = (ConstraintObject*)simm_realloc(ms->constraintobj, - ms->constraint_object_array_size*sizeof(ConstraintObject),&rc); - - /* make a model window structure and register it with the window manager */ - if (scene->windowWidth > 0) - window_width = scene->windowWidth; - else - window_width = suggested_win_width; - - window_height = scene->windowHeight; - - size_model_display(scene, ms, &window_width, &window_height); - - /* If the user did not define all possible model views, - * then fill-in the unused ones with a standard transform. - */ - for (i = ms->dis.num_file_views; i < MAXSAVEDVIEWS; i++) - { - reset_4x4matrix(ms->dis.saved_view[i]); - ms->dis.saved_view[i][3][0] = scene->tx; - ms->dis.saved_view[i][3][1] = scene->ty; - ms->dis.saved_view[i][3][2] = scene->tz; - } - - /* Now apply the default view to the model */ - if (ms->dis.default_view >= 0 && ms->dis.default_view < MAXSAVEDVIEWS) - { - copy_4x4matrix(ms->dis.saved_view[ms->dis.default_view], scene->transform_matrix); - scene->tx = ms->dis.saved_view[ms->dis.default_view][3][0]; - scene->ty = ms->dis.saved_view[ms->dis.default_view][3][1]; - scene->tz = ms->dis.saved_view[ms->dis.default_view][3][2]; - } - - if (make_scene_window(scene, window_width, window_height) == code_bad) - { - //TODO_SCENE free_scene(scene); - return code_bad; - } - - if (add_model_to_scene(scene, ms) == code_bad) - { - free_model(ms->modelnum); - return code_bad; - } - -#if ! ENGINE - load_model_textures(ms); -#endif - - make_gencoord_help_text(ms); - - for (i = 0; i < ms->numgencoords; i++) - { - if (ms->gencoord[i]->slider_visible == yes) - { - ms->gencslider.sl[i].visible = yes; - ms->gencform.option[i].active = yes; - ms->gencform.option[i].visible = yes; - ms->gc_chpanel.checkbox[i].active = yes; - ms->gc_chpanel.checkbox[i].visible = yes; - ms->gc_lockPanel.checkbox[i].active = yes; - ms->gc_lockPanel.checkbox[i].visible = yes; - } - else - { - ms->gencslider.sl[i].visible = no; - ms->gencform.option[i].active = no; - ms->gencform.option[i].visible = no; - ms->gc_chpanel.checkbox[i].active = no; - ms->gc_chpanel.checkbox[i].visible = no; - ms->gc_lockPanel.checkbox[i].active = no; - ms->gc_lockPanel.checkbox[i].visible = no; - } - } - -#if 0 - /* If there are more than 30 gencoords, then hide them all in - * the Model Viewer. If there are less than 30, turn on all - * 'used' gencoords, and turn off all 'unused' ones. - */ - if (ms->numgencoords > 30) - { - for (i = 0; i < ms->numgencoords; i++) - { - ms->gencslider.sl[i].visible = no; - ms->gencform.option[i].active = no; - ms->gc_chpanel.checkbox[i].active = no; - ms->gc_lockPanel.checkbox[i].active = no; - ms->gencform.option[i].visible = no; - ms->gc_chpanel.checkbox[i].visible = no; - ms->gc_lockPanel.checkbox[i].visible = no; - } - } - else - { - for (i = 0; i < ms->numgencoords; i++) - { - if (ms->gencoord[i]->used_in_model == yes) - { - ms->gencslider.sl[i].visible = yes; - ms->gencform.option[i].active = yes; - ms->gc_chpanel.checkbox[i].active = yes; - ms->gc_lockPanel.checkbox[i].active = yes; - ms->gencform.option[i].visible = yes; - ms->gc_chpanel.checkbox[i].visible = yes; - ms->gc_lockPanel.checkbox[i].visible = yes; - } - else - { - ms->gencslider.sl[i].visible = no; - ms->gencform.option[i].active = no; - ms->gc_chpanel.checkbox[i].active = no; - ms->gc_lockPanel.checkbox[i].active = no; - ms->gencform.option[i].visible = no; - ms->gc_chpanel.checkbox[i].visible = no; - ms->gc_lockPanel.checkbox[i].visible = no; - } - } - } -#endif - - reposition_gencoord_sliders(ms); - - /* Make display lists for all the materials. This must be done - * while the current window is set to the model window (some - * sort of weird OpenGL thing, at least on the SGI). - */ - if (scene->window_index != -1) - { - glutSetWindow(root.window[scene->window_index].win_parameters->id); - for (i=0; idis.mat.num_materials; i++) - { - make_mat_display_list(&ms->dis.mat.materials[i]); - make_highlight_mat_display_list(&ms->dis.mat.materials[i]); - } - } - - /* If the user specified motion files in the joints file, - * read them in now. - */ - for (i = 0; i < ms->num_motion_files; i++) - motion = load_motion(ms->motionfilename[i], ms->modelnum, showTopLevelMessages); - - if (motion && ms->num_motion_files == 1) - apply_motion_to_model(ms, ms->motion[0], ms->motion[0]->min_value, yes, yes); - - if (modelIndex) - *modelIndex = ms->modelnum; - - return code_fine; -} -#endif /* CORTEX_PLUGIN */ - - -void resize_model_display(Scene* scene, ModelStruct* ms) -{ - int i; - - size_model_display(scene, ms, &scene->windowWidth, &scene->windowHeight); - - // Copy the newly calculated standard transform to all of the - // model views that were not defined by the user. - for (i = ms->dis.num_file_views; i < MAXSAVEDVIEWS; i++) - { - reset_4x4matrix(ms->dis.saved_view[i]); - ms->dis.saved_view[i][3][0] = scene->tx; - ms->dis.saved_view[i][3][1] = scene->ty; - ms->dis.saved_view[i][3][2] = scene->tz; - } - - // Now apply the default view to the model. - if (ms->dis.default_view >= 0 && ms->dis.default_view < MAXSAVEDVIEWS) - copy_4x4matrix(ms->dis.saved_view[ms->dis.default_view], scene->transform_matrix); -} - -static ReturnCode make_gencoord_sliders(ModelStruct* ms) -{ - int i; - double stepsize; - IntBox bbox; - SliderArray* sa; - - sa = &ms->gencslider; - - sa->numsliders = ms->numgencoords; - - ms->gencslider.sl = (Slider*)simm_malloc(sa->numsliders * sizeof(Slider)); - if (ms->gencslider.sl == NULL) - return code_bad; - - for (i=0; inumsliders; i++) - { - SET_BOX1221(bbox,0,170+2*FORM_FIELD_HEIGHT,-FORM_FIELD_YSPACING*i, - bbox.y2-FORM_FIELD_HEIGHT); - if (ms->gencoord[i]->type == translation_gencoord) - stepsize = 0.001; - else - stepsize = 1.0; - make_slider(&sa->sl[i],horizontal_slider,bbox,FORM_FIELD_HEIGHT-2, - ms->gencoord[i]->value, - ms->gencoord[i]->range.start, - ms->gencoord[i]->range.end,stepsize,NULL,NULL); - } - - return code_fine; -} - - -unsigned short curs2[] = { -0x3455, 0x6452, 0x6321, 0x3563, -}; - -static ReturnCode make_scene_window(Scene* scene, int suggested_width, int suggested_height) -{ - WindowParams mwin; - WinUnion wun; - IntBox prefpos; - int i, offset = scene->scene_num % 6; - - mwin.minwidth = 30; - mwin.minheight = 50; - - if (scene->windowX1 > 0 && scene->windowY1 > 0) - glueSetWindowPlacement(GLUT_NORMAL_WINDOW, GLUE_NORTHWEST, - suggested_width, suggested_height, - scene->windowX1, scene->windowY1, &prefpos); - else - glueSetWindowPlacement(GLUT_NORMAL_WINDOW, GLUE_NORTHEAST, - suggested_width, suggested_height, - -offset * TITLE_BAR_HEIGHT, offset * TITLE_BAR_HEIGHT, &prefpos); - - scene->window_glut_id = mwin.id = glueOpenWindow("scene", no, GLUE_DEPTH_BUFFERED); - - mwin.name = (char*)simm_malloc(256*sizeof(char)); - if (mwin.name == NULL) - return code_bad; - - (void)sprintf(mwin.name, "Scene %d", scene->scene_num); - //(void)strcpy(mwin.name, ms->name); - glutSetIconTitle(mwin.name); - glutSetWindowTitle(mwin.name); - - init_model_lighting(); - - wun.scene = scene; - - scene->window_index = add_window(&mwin, &wun, SCENE, scene->scene_num, no, drawscene, update_scene, scene_input); - - if (scene->window_index == -1) - { - glutDestroyWindow(mwin.id); - return code_bad; - } - - glutSetWindowData(mwin.id, scene->window_index); - - FREE_IFNOTNULL(mwin.name); - - return code_fine; -} - - -static ReturnCode add_model_to_scene(Scene* scene, ModelStruct* model) -{ - glutSetWindow(scene->window_glut_id); - - //TODO_SCENE: for now, window takes name of this model - (void)strcpy(buffer, model->name); - glutSetIconTitle(buffer); - glutSetWindowTitle(buffer); - FREE_IFNOTNULL(root.window[scene->window_index].win_parameters->name); - mstrcpy(&root.window[scene->window_index].win_parameters->name, buffer); - - scene->num_models = 1; - scene->model = (ModelStruct**)simm_calloc(1, sizeof(ModelStruct*)); - scene->model[0] = model; - - updatemodelmenu(); - - //TODO5.0: should the scene have a title? - //what should the movie file name be? - sprintf(buffer, "%s.avi", model->name); - mstrcpy(&scene->movie_file, buffer); - - make_and_queue_simm_event(MODEL_ADDED, model->modelnum, NULL, NULL, ZERO, ZERO); - - return code_fine; -} - - -static void make_modelpopups(ModelStruct* ms) -{ - int i, j, item_count; - - /* Make the joint menu */ - ms->jointmenu = glueCreateMenu("Joints"); - for (i=0; inumjoints; i++) - glueAddMenuEntry(ms->jointmenu, ms->joint[i].name); - - make_gencoord_popup_menus(ms); - /* Make the muscle group menu */ - ms->musclegroupmenu = glueCreateMenu("Muscle Groups"); - for (i=0; inumgroups; i++) - glueAddMenuEntry(ms->musclegroupmenu,ms->muscgroup[i].name); - - glueAddMenuEntry(ms->musclegroupmenu, "---"); - glueAddMenuEntryWithValue(ms->musclegroupmenu, "all on", ALL_MUSCLES_ON); - glueAddMenuEntryWithValue(ms->musclegroupmenu, "all off", ALL_MUSCLES_OFF); - - /* If there are no muscles, disable all the menu items. They will be - * enabled if and when a new muscle is created. - */ - if (ms->nummuscles == 0) - { - int numItems = glueGetNumMenuItems(ms->musclegroupmenu); - for (i = 0; i < numItems; i++) - glueEnableMenuItem(ms->musclegroupmenu, i + 1, GLUE_DISABLE); - } - - ms->markerMenu = -1; // gets created in ke_add_model() - - /* Make the segment menu */ - ms->segmentmenu = glueCreateMenu("Body Segments"); - for (i=0; inumsegments; i++) - glueAddMenuEntry(ms->segmentmenu,ms->segment[i].name); - - /* Make the motion menus */ - ms->motionmenu = glueCreateMenu("Motions"); - glueAddMenuEntry(ms->motionmenu, "none loaded"); - glueEnableMenuItem(ms->motionmenu, 1, GLUE_DISABLE); - ms->motionwithrealtimemenu = glueCreateMenu("Motions"); - ms->motionplotmenu = glueCreateMenu("Motions"); - glueAddMenuEntry(ms->motionplotmenu, "none loaded"); - glueEnableMenuItem(ms->motionplotmenu, 1, GLUE_DISABLE); - - /* Add the motion curve menu to the bottom of the x-var menu. */ - glueAddSubmenuEntry(ms->xvarmenu, "motion curves", ms->motionplotmenu); - glueAddMenuEntryWithValue(ms->xvarmenu, "---", -1); - - if (is_module_present(MOTION_REAL)) - { - const char* evaHost = get_preference("EVART_MACHINE"); - - if (evaHost) - sprintf(buffer, "realtime connection to %s", evaHost); - else - sprintf(buffer, "realtime connection"); - - glueAddMenuEntryWithValue(ms->motionwithrealtimemenu, buffer, REALTIME_MENU_ITEM); - glueAddMenuEntryWithValue(ms->motionwithrealtimemenu, "---", REALTIME_MENU_ITEM + 1); - } - - ms->dis.view_menu = glueCreateMenu("Cameras"); - for (i=0; idis.num_file_views; i++) - glueAddMenuEntry(ms->dis.view_menu,ms->dis.view_name[i]); - - for (i=ms->dis.num_file_views; idis.view_menu,buffer); - } - - for (i=0; inumsegments; i++) - { - ms->segment[i].drawmodemenu = glueCreateMenu("Draw Mode"); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"gouraud shaded", 10*i); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"flat shaded", 10*i+1); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"solid fill", 10*i+2); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"wireframe", 10*i+3); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"outlined", 10*i+4); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"bounding box", 10*i+5); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"none", 10*i+6); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"---", 10*i+7); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"normal vectors", 10*i+8); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"---", 10*i+10); - glueAddMenuEntryWithValue(ms->segment[i].drawmodemenu,"segment axes", 10*i+9); - } - - ms->material_menu = glueCreateMenu("Materials"); - for (i=1; idis.mat.num_materials; i++) - glueAddMenuEntryWithValue(ms->material_menu, ms->dis.mat.materials[i].name, START_MATERIALS + i); - - update_drawmode_menus(ms); - - make_function_menu(ms); -} - -void make_gencoord_popup_menus(ModelStruct* model) -{ - int i, j; - - // Make the gencoord menu. - if (model->gencoordmenu != -1) - glutDestroyMenu(model->gencoordmenu); - model->gencoordmenu = glueCreateMenu("Gencoords"); - for (i=0; inumgencoords; i++) - glueAddMenuEntry(model->gencoordmenu, model->gencoord[i]->name); - - if (model->gencoordmenu2 != -1) - glutDestroyMenu(model->gencoordmenu2); - model->gencoordmenu2 = glueCreateMenu(NULL); - for (i = 0; i < model->numgencoords; i++) - glueAddMenuEntry(model->gencoordmenu2, model->gencoord[i]->name); - - // Make the gencoord groups menu. - if (model->numgencgroups > 0) - { - if (model->gencoord_group_menu != -1) - glutDestroyMenu(model->gencoord_group_menu); - model->gencoord_group_menu = glueCreateMenu("Gencoord Groups"); - - for (i = 0; i < model->numgencgroups; i++) - glueAddMenuEntryWithValue(model->gencoord_group_menu, model->gencgroup[i].name, i + 2 * MAX_GENCOORDS); - - glueAddMenuEntry(model->gencoord_group_menu, "---"); - glueAddSubmenuEntry(model->gencoord_group_menu, "individual gencoords", model->gencoordmenu); - glueAddMenuEntryWithValue(model->gencoord_group_menu, "all on", 4 * MAX_GENCOORDS); - glueAddMenuEntryWithValue(model->gencoord_group_menu, "all off", 4 * MAX_GENCOORDS + 1); - } - else - { - model->gencoord_group_menu = 0; - glueAddMenuEntry(model->gencoordmenu, "---"); - glueAddMenuEntryWithValue(model->gencoordmenu, "all on", 4 * MAX_GENCOORDS); - glueAddMenuEntryWithValue(model->gencoordmenu, "all off", 4 * MAX_GENCOORDS + 1); - } - - // Make the x-var menu (for plotting). - if (model->xvarmenu != -1) - glutDestroyMenu(model->xvarmenu); - model->xvarmenu = glueCreateMenu("X variables"); - if (model->numgencoords > 0) - { - for (i=0; inumgencoords; i++) - glueAddMenuEntryWithValue(model->xvarmenu,model->gencoord[i]->name,i); - - glueAddMenuEntryWithValue(model->xvarmenu,"---",-1); - } - - // Make the moment, moment arm, numerical moment arm, and moment@maxforce submenus. - // Each of these menus is a list of gencoords for which you can find a moment arm - // (y-var in the plotmaker). It is essentially the same menu as the model's - // "gencoordmenu" pop-up menu (without the "all on" and "all off" items), - // but the selections must return different numbers, so you need to make - // a whole new menu for each y-var - if (model->momentgencmenu != -1) - glutDestroyMenu(model->momentgencmenu); - model->momentgencmenu = glueCreateMenu("Gencoords"); - for (i=0; inumgencoords; i++) - glueAddMenuEntryWithValue(model->momentgencmenu, model->gencoord[i]->name, i); - - if (model->momentarmgencmenu != -1) - glutDestroyMenu(model->momentarmgencmenu); - model->momentarmgencmenu = glueCreateMenu("Gencoords"); - for (i=0; inumgencoords; i++) - glueAddMenuEntryWithValue(model->momentarmgencmenu, model->gencoord[i]->name, MAX_GENCOORDS + i); - - if (model->momentarmnumgencmenu != -1) - glutDestroyMenu(model->momentarmnumgencmenu); - model->momentarmnumgencmenu = glueCreateMenu("Gencoords"); - for (i=0; inumgencoords; i++) - glueAddMenuEntryWithValue(model->momentarmnumgencmenu, model->gencoord[i]->name, 2 * MAX_GENCOORDS + i); - - if (model->maxmomentgencmenu != -1) - glutDestroyMenu(model->maxmomentgencmenu); - model->maxmomentgencmenu = glueCreateMenu("Gencoords"); - for (i=0; inumgencoords; i++) - glueAddMenuEntryWithValue(model->maxmomentgencmenu, model->gencoord[i]->name, 3 * MAX_GENCOORDS + i); -} - - -void update_drawmode_menus(ModelStruct* ms) -{ - int i, item_count; - - /* TODO: this function was carved out of make_model_popups because when loading - * HTR files with analog data, you may load a model and then add forceplates to - * it (as worldobjects) after the model popups have already been created. In order - * to get the new forceplates into the drawmode menus, you have to call this - * function from within the analog processing code. Improvements that can be - * made: (1) there should be a glueDestroyMenu() so that the menus being updated - * are not left hanging around, (2) each world object's drawmode menu should be - * created as the object is created, not in this function (this is done properly - * for body segments). - */ - for (i=0; inumworldobjects; i++) - { - if (ms->worldobj[i].drawmodemenu != 0) - glutDestroyMenu(ms->worldobj[i].drawmodemenu); - ms->worldobj[i].drawmodemenu = glueCreateMenu("Draw Mode"); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"gouraud shaded", 10*(i+ms->numsegments)); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"flat shaded", 10*(i+ms->numsegments)+1); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"solid fill", 10*(i+ms->numsegments)+2); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"wireframe", 10*(i+ms->numsegments)+3); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"outlined", 10*(i+ms->numsegments)+4); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"bounding box", 10*(i+ms->numsegments)+5); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"none", 10*(i+ms->numsegments)+6); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"---", 10*(i+ms->numsegments)+7); - glueAddMenuEntryWithValue(ms->worldobj[i].drawmodemenu,"normal vectors", 10*(i+ms->numsegments)+8); - } - - item_count = ms->numsegments + ms->numworldobjects; - - if (ms->dis.allsegsdrawmodemenu != 0) - glutDestroyMenu(ms->dis.allsegsdrawmodemenu); - ms->dis.allsegsdrawmodemenu = glueCreateMenu("Draw Mode"); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"gouraud shaded", 10*item_count); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"flat shaded", 10*item_count+1); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"solid fill", 10*item_count+2); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"wireframe", 10*item_count+3); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"outlined", 10*item_count+4); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"bounding box", 10*item_count+5); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"none", 10*item_count+6); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"---", 10*item_count+7); - glueAddMenuEntryWithValue(ms->dis.allsegsdrawmodemenu,"normal vectors", 10*item_count+8); - - item_count++; - - if (ms->dis.allligsdrawmodemenu != 0) - glutDestroyMenu(ms->dis.allligsdrawmodemenu); - ms->dis.allligsdrawmodemenu = glueCreateMenu("Draw Mode"); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"gouraud shaded", 10*item_count); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"flat shaded", 10*item_count+1); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"solid fill", 10*item_count+2); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"wireframe", 10*item_count+3); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"outline", 10*item_count+4); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"bounding box", 10*item_count+5); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"none", 10*item_count+6); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"---", 10*item_count+7); - glueAddMenuEntryWithValue(ms->dis.allligsdrawmodemenu,"normal vectors", 10*item_count+8); - - item_count++; - - if (ms->dis.allworlddrawmodemenu != 0) - glutDestroyMenu(ms->dis.allworlddrawmodemenu); - ms->dis.allworlddrawmodemenu = glueCreateMenu("Draw Mode"); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"gouraud shaded", 10*item_count); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"flat shaded", 10*item_count+1); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"solid fill", 10*item_count+2); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"wireframe", 10*item_count+3); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"outlined", 10*item_count+4); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"bounding box", 10*item_count+5); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"none", 10*item_count+6); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"---", 10*item_count+7); - glueAddMenuEntryWithValue(ms->dis.allworlddrawmodemenu,"normal vectors", 10*item_count+8); - - item_count++; - - if (ms->dis.alldrawmodemenu != 0) - glutDestroyMenu(ms->dis.alldrawmodemenu); - ms->dis.alldrawmodemenu = glueCreateMenu("Draw Mode"); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"gouraud shaded", 10*item_count); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"flat shaded", 10*item_count+1); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"solid fill", 10*item_count+2); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"wireframe", 10*item_count+3); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"outlined", 10*item_count+4); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"bounding box", 10*item_count+5); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"none", 10*item_count+6); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"---", 10*item_count+7); - glueAddMenuEntryWithValue(ms->dis.alldrawmodemenu,"normal vectors", 10*item_count+8); - - if (ms->dis.maindrawmodemenu != 0) - glutDestroyMenu(ms->dis.maindrawmodemenu); - ms->dis.maindrawmodemenu = glueCreateMenu("Object"); - if (ms->numseggroups > 0) - { - for (i = 0; i < ms->numseggroups; i++) - { - ms->seggroup[i].displaymodemenu = glueCreateMenu(ms->seggroup[i].name); - - item_count++; - - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"gouraud shaded", 10*item_count); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"flat shaded", 10*item_count+1); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"solid fill", 10*item_count+2); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"wireframe", 10*item_count+3); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"outlined", 10*item_count+4); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"bounding box", 10*item_count+5); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"none", 10*item_count+6); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"---", 10*item_count+7); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"normal vectors", 10*item_count+8); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"---", 10*item_count+10); - glueAddMenuEntryWithValue(ms->seggroup[i].displaymodemenu,"segment axes GRP", 10*item_count+9); - - glueAddSubmenuEntry(ms->dis.maindrawmodemenu, ms->seggroup[i].name, ms->seggroup[i].displaymodemenu); - } - - // The "individual segments" menu is used only if there are segment groups. - if (ms->dis.eachsegdrawmodemenu != 0) - glutDestroyMenu(ms->dis.eachsegdrawmodemenu); - ms->dis.eachsegdrawmodemenu = glueCreateMenu("Segments"); - - for (i = 0; i < ms->numsegments; i++) - glueAddSubmenuEntry(ms->dis.eachsegdrawmodemenu, ms->segment[i].name, ms->segment[i].drawmodemenu); - - glueAddSubmenuEntry(ms->dis.maindrawmodemenu, "individual segments", ms->dis.eachsegdrawmodemenu); - } - else - { - for (i=0; inumsegments; i++) - { - glueAddSubmenuEntry(ms->dis.maindrawmodemenu, - ms->segment[i].name,ms->segment[i].drawmodemenu); - } - } - - for (i=0; inumworldobjects; i++) - { - glueAddSubmenuEntry(ms->dis.maindrawmodemenu, - ms->worldobj[i].name,ms->worldobj[i].drawmodemenu); - } - - glueAddSubmenuEntry(ms->dis.maindrawmodemenu,"all body segments", ms->dis.allsegsdrawmodemenu); - glueAddSubmenuEntry(ms->dis.maindrawmodemenu,"all muscle surfaces", ms->dis.allligsdrawmodemenu); - glueAddSubmenuEntry(ms->dis.maindrawmodemenu,"all world objects", ms->dis.allworlddrawmodemenu); - glueAddSubmenuEntry(ms->dis.maindrawmodemenu,"all objects", ms->dis.alldrawmodemenu); - -} - -void make_function_menu(ModelStruct* ms) -{ - int i; - - if (ms->functionMenu != 0) - glutDestroyMenu(ms->functionMenu); - - ms->functionMenu = glueCreateMenu("Functions"); - for (i=0; ifunc_array_size; i++) - { - if (ms->function[i] && ms->function[i]->used == dpYes) - { - if (ms->function[i]->source == dpMuscleFile) - sprintf(buffer, "m%d", ms->function[i]->usernum); - else - sprintf(buffer, "%d", ms->function[i]->usernum); - glueAddMenuEntryWithValue(ms->functionMenu, buffer, FUNCTION_MENU_START + i); - } - } -} - -void scene_input(WindowParams* win_parameters, WinUnion* win_struct, SimmEvent se) -{ - int i; - - glutSetWindow(win_parameters->id); - - if (se.field1 == window_quit) - { - delete_scene(win_struct->scene); - return; - } - -#if ! CORTEX_PLUGIN - if ((se.field1 == window_shut) || (((se.field1 == backspace_key) || (se.field1 == delete_key)) && se.field2 == key_pressed)) - { - confirm_action(win_parameters, model_deletion_text, model_deletion_confirm); - scene_to_confirm = win_struct->scene; - return; - } -#endif - - if (win_struct->scene == NULL) - return; - - /* Because many tools are interested in mouse buttons pressed in the model window - * (e.g. to change the model view or pick muscle points), no tool is allowed to - * attach a function to a solo mouse button press. For example, to pick muscle - * points in the MuscleEditor, you press the space bar and click the select button, - * not just click the select button. Thus when a mouse button is pressed, do not - * generate a SIMM event to notify the tools. It is the responsibility of each - * tool to wait until it gets a non-mouse-button event, and then enter a loop within - * its own code which uses getButton() to check if other buttons (like the mouse - * buttons) have been pressed. - * Consider the case of selecting muscle points. If the space bar is pressed first, - * the MuscleEditor tool will get the corresponding SIMM event, and it will loop - * as long as the space bar is pressed. If the select button (left mouse button) - * is then pressed, this MuscleEditor loop will use getButton() calls to find out - * that it is pressed, then let the user select muscle points. - * If the select button is pressed first, no SIMM event will be generated, so none - * of the tools will find out about the button press. Control flow will return to - * glutMainLoop() which waits for and distributes all events. If the space - * bar is then pressed, the MuscleEditor will pick up the corresponding SIMM event - * and loop until the button is released. This loop will, as before, use getButton() - * to see if the select button is pressed, but since the select button was pressed - * first (and presumably was kept pressed as the space bar was pressed), the - * MuscleEditor will immediately execute the code to let the user select muscle - * points. - */ - - /* Hack to support right-clicking on a bone to change its drawmode */ - //TODO5.0: figure out which object was clicked on, and send an event to - //the appropriate tool's simm event handler. - if (se.field1 == rightmouse_button && se.field2 == key_pressed) - { - handle_right_click(win_struct->scene, se.mouse_x, se.mouse_y); - } - else if (se.field1 != leftmouse_button && se.field1 != middlemouse_button && se.field1 != rightmouse_button) - { - forward_simm_event(se, SCENE_INPUT_EVENT, (void*)win_struct->scene); - } -} - -static void handle_right_click(Scene* scene, int mouse_x, int mouse_y) -{ - GLubyte pixel[3]; - PickIndex obj_num, object_type, object; - ModelDrawOptions mdo; - - mdo.mode = GL_SELECT; - mdo.draw_world_objects = yes; - mdo.draw_bones = yes; - mdo.draw_muscles = yes; - mdo.draw_selection_box = no; - mdo.skin_mode = draw_skin_and_bones; - mdo.field1 = mdo.field2 = DRAW_ALL; - - draw_model(scene, scene->model[0], &mdo); - - glutSetWindow(scene->window_glut_id); - glReadBuffer(GL_BACK); - glReadPixels(mouse_x, mouse_y, 1, 1, GL_RGB, GL_UNSIGNED_BYTE, pixel); - - unpack_int_from_color(&object, pixel); - - if (object != 0) - { - SimmEvent se; - - get_object_type_and_number(object, &object_type, &obj_num); - - memset(&se, 0, sizeof(SimmEvent)); - se.event_code = SCENE_RIGHT_CLICK_EVENT; - se.struct_ptr1 = (void*)scene; - se.struct_ptr2 = (void*)scene->model[0]; - se.model_index = scene->model[0]->modelnum; - se.mouse_x = mouse_x; - se.mouse_y = mouse_y; - se.window_id = scene->window_index; - se.object = object; - - if (object_type == BONE_OBJECT || object_type == WORLD_OBJECT) - { - call_simm_event_handler("mv", se); - } - else if (object_type == MUSCLE_OBJECT) - { - call_simm_event_handler("me", se); - } - else if (object_type == MARKER_OBJECT) - { - call_simm_event_handler("mk", se); - } - } -} - -static void call_simm_event_handler(const char tool_id[], SimmEvent se) -{ - int i; - - for (i=0; inumsegments*sizeof(SBoolean)); - ms->segment_drawing_order = (int*)simm_malloc(ms->numsegments*sizeof(int)); - - if (seg_used == NULL || ms->segment_drawing_order == NULL) - return code_bad; - - for (i=0; inumsegments; i++) - seg_used[i] = no; - - ms->segment_drawing_order[0] = ms->ground_segment; - seg_used[ms->ground_segment] = yes; - index = 1; - - /* The next 3 lines are a hack until the code at the bottom works better */ - for (i=0; inumsegments; i++) - if (seg_used[i] == no) - ms->segment_drawing_order[index++] = i; - -/* - for (i=ms->numjoints-1; i>=0; i--) - { - if (joint_uses_segment(ms,i,ms->ground_segment,&other_seg) == yes && - seg_used[other_seg] == no) - { - ms->segment_drawing_order[index++] = other_seg; - seg_used[other_seg] = yes; - } - } - - for (i=0; inumsegments; i++) - if (seg_used[i] == no) - ms->segment_drawing_order[index++] = i; -*/ - -/* - printf("drawing order:\n"); - for (i=0; inumsegments; i++) - printf("%s\n", ms->segment[ms->segment_drawing_order[i]].name); -*/ - - free(seg_used); - - return code_fine; - -} - - - -static SBoolean joint_uses_segment(ModelStruct* ms, int jointnum, int segmentnum, - int* other_segment) -{ - - if (ms->joint[jointnum].from == segmentnum) - { - *other_segment = ms->joint[jointnum].to; - return (yes); - } - else if (ms->joint[jointnum].to == segmentnum) - { - *other_segment = ms->joint[jointnum].from; - return (yes); - } - - return (no); - -} - -//TODO_SCENE: should this be independent of scene? -public void size_model(Scene* scene, ModelStruct* ms, BoundingCube* bc) -{ - int i, j, num_bones = 0; - double pnt[4]; - double xmin = MAXMDOUBLE, xmax = MINMDOUBLE; - double ymin = MAXMDOUBLE, ymax = MINMDOUBLE; - double zmin = MAXMDOUBLE, zmax = MINMDOUBLE; - - // Determine max_dimension of all model segments. - for (i=0; inumsegments; i++) - { - for (j=0; jsegment[i].numBones; j++, num_bones++) - { - if (ms->segment[i].bone[j].num_vertices == 0) - { - pnt[0] = pnt[1] = pnt[2] = 0.0;//dkb dec30, 2003 -1.0; - } - else - { -#if NOSCALE - pnt[0] = ms->segment[i].bone[j].bc.x1 * ms->segment[i].bone_scale[0]; - pnt[1] = ms->segment[i].bone[j].bc.y1 * ms->segment[i].bone_scale[1]; - pnt[2] = ms->segment[i].bone[j].bc.z1 * ms->segment[i].bone_scale[2]; -#else - pnt[0] = ms->segment[i].bone[j].bc.x1; - pnt[1] = ms->segment[i].bone[j].bc.y1; - pnt[2] = ms->segment[i].bone[j].bc.z1; -#endif - convert(ms, pnt, i, ms->ground_segment); - } - if (pnt[0] < xmin) - xmin = pnt[0]; - if (pnt[1] < ymin) - ymin = pnt[1]; - if (pnt[2] < zmin) - zmin = pnt[2]; - - if (ms->segment[i].bone[j].num_vertices == 0) - { - pnt[0] = pnt[1] = pnt[2] = 0.0;//dkb dec 30, 2003 1.0; - } - else - { -#if NOSCALE - pnt[0] = ms->segment[i].bone[j].bc.x2 * ms->segment[i].bone_scale[0]; - pnt[1] = ms->segment[i].bone[j].bc.y2 * ms->segment[i].bone_scale[1]; - pnt[2] = ms->segment[i].bone[j].bc.z2 * ms->segment[i].bone_scale[2]; -#else - pnt[0] = ms->segment[i].bone[j].bc.x2; - pnt[1] = ms->segment[i].bone[j].bc.y2; - pnt[2] = ms->segment[i].bone[j].bc.z2; -#endif - convert(ms, pnt, i, ms->ground_segment); - } - if (pnt[0] > xmax) - xmax = pnt[0]; - if (pnt[1] > ymax) - ymax = pnt[1]; - if (pnt[2] > zmax) - zmax = pnt[2]; - } - } - - // This code handles the case in which there are no bones in the - // model. Ideally, you should look at muscle attachment points or - // joint parameters to determine the best dimensions. - if (num_bones == 0) - { - xmin = ymin = zmin = -1.0; - xmax = ymax = zmax = 1.0; - } - - ms->max_dimension = xmax - xmin; - if (ymax-ymin > ms->max_dimension) - ms->max_dimension = ymax - ymin; - if (zmax-zmin > ms->max_dimension) - ms->max_dimension = zmax - zmin; - - // Determine max_dimension of all model segments and world objects. - for (i = 0; i < ms->numworldobjects; i++) - { - double npnt[4]; - WorldObject* obj = &ms->worldobj[i]; - - pnt[0] = obj->wobj->bc.x1; - pnt[1] = obj->wobj->bc.y1; - pnt[2] = obj->wobj->bc.z1; - pnt[3] = 1.0; - mult_4x4matrix_by_vector(obj->transform, pnt, npnt); - - if (npnt[0] < xmin) xmin = npnt[0]; - if (npnt[1] < ymin) ymin = npnt[1]; - if (npnt[2] < zmin) zmin = npnt[2]; - - pnt[0] = obj->wobj->bc.x2; - pnt[1] = obj->wobj->bc.y2; - pnt[2] = obj->wobj->bc.z2; - pnt[3] = 1.0; - mult_4x4matrix_by_vector(obj->transform, pnt, npnt); - - if (npnt[0] > xmax) xmax = npnt[0]; - if (npnt[1] > ymax) ymax = npnt[1]; - if (npnt[2] > zmax) zmax = npnt[2]; - } - ms->max_dimension2 = xmax - xmin; - if (ymax-ymin > ms->max_dimension2) - ms->max_dimension2 = ymax - ymin; - if (zmax-zmin > ms->max_dimension2) - ms->max_dimension2 = zmax - zmin; - - // Compute max_diagonal of all model segments and world objects. - pnt[0] = xmax - xmin; - pnt[1] = ymax - ymin; - pnt[2] = zmax - zmin; - - ms->max_diagonal = sqrt(SQR(pnt[0]) + SQR(pnt[1]) + SQR(pnt[2])); - scene->max_diagonal = ms->max_diagonal; //TODO_SCENE: scene should have its own diagonal calculation - - ms->max_diagonal_needs_recalc = no; - -#if 0 - fprintf(stderr, "%s\n", ms->name); - fprintf(stderr, " min: %6.2f, %6.2f, %6.2f\n", xmin, ymin, zmin); - fprintf(stderr, " max: %6.2f, %6.2f, %6.2f\n", xmax, ymax, zmax); - fprintf(stderr, " max_dimension: %.2f\n", ms->max_dimension); - fprintf(stderr, " max_dimension2: %.2f\n", ms->max_dimension2); - fprintf(stderr, " max_diagonal: %.2f\n", ms->max_diagonal); -#endif - - calc_near_and_far_clip_planes(scene, fabs(scene->tz)); - - if (bc) - { - bc->x1 = xmin; - bc->x2 = xmax; - bc->y1 = ymin; - bc->y2 = ymax; - bc->z1 = zmin; - bc->z2 = zmax; - } -} - -//TODO_SCENE: make this work for multiple models -void size_model_display(Scene* scene, ModelStruct* ms, int* suggested_width, int* suggested_height) -{ - int i, j, xpixwidth, sug_height, num_bones=0; - BoundingCube bc; - - size_model(scene, ms, &bc); - - scene->start_tx = scene->tx = -(bc.x2+bc.x1)/2.0; - scene->start_ty = scene->ty = -(bc.y2+bc.y1)/2.0; - scene->start_tz = scene->tz = ms->max_dimension * -1.7; - - if (ms->specified_min_thickness == no) - { - if (ms->specified_max_thickness == yes) - *ms->default_muscle->min_thickness = (*ms->default_muscle->max_thickness)*0.25; - else - *ms->default_muscle->min_thickness = ms->max_dimension*0.002; - } - if (ms->specified_max_thickness == no) - { - if (ms->specified_min_thickness == yes) - *ms->default_muscle->max_thickness = (*ms->default_muscle->min_thickness)*4.0; - else - *ms->default_muscle->max_thickness = ms->max_dimension*0.008; - } - - ms->dis.muscle_point_radius = ms->max_dimension * 0.0026; - ms->dis.muscle_point_id = -1; - - scene->model_move_increment = ms->max_dimension * 0.02; - - if (*suggested_width < 0) - *suggested_width = WINDOW_WIDTH; - - if (*suggested_height < 0) - { - xpixwidth = *suggested_width - 2*WINDOW_MARGIN; - sug_height = xpixwidth*(bc.y2-bc.y1)/(bc.x2-bc.x1) + 2*WINDOW_MARGIN; - *suggested_height = _MIN(900,sug_height); - } - - for (i = 0; i < ms->numsegments; i++) - { - ms->segment[i].mc_radius = ms->max_dimension * 0.007; - - for (j = 0; j < ms->segment[i].numSpringPoints; j++) - { - if (ms->default_muscle->min_thickness && ms->default_muscle->max_thickness) - ms->segment[i].springPoint[j].radius = *ms->default_muscle->min_thickness; - else - ms->segment[i].springPoint[j].radius = ms->max_dimension * 0.002; - } - } - - { - int screen_y = glutGet(GLUT_SCREEN_HEIGHT); - - if (*suggested_height > screen_y - 40) - *suggested_height = screen_y - 40; - } -} - - -public void calc_near_and_far_clip_planes(Scene* scene, double viewVecLen) -{ - if (viewVecLen > scene->max_diagonal * 2.0) - scene->near_clip_plane = viewVecLen - scene->max_diagonal * 2.0; - else - scene->near_clip_plane = viewVecLen / 1000.0; - - scene->far_clip_plane = viewVecLen + scene->max_diagonal * 2.0; - -#if 0 - fprintf(stderr, "n: %.2f (%s), v: %.2f, f: %.2f\n", - ms->dis.near_clip_plane, - viewVecLen > ms->max_diagonal ? " " : "*", - viewVecLen, - ms->dis.far_clip_plane); -#endif -} - - -#if INCLUDE_MOCAP_MODULE - -#if WIN32 - -/* ------------------------------------------------------------------------- - _read_token - ----------------------------------------------------------------------------- */ -static SBoolean _read_token (FILE* file, char* token) -{ - SBoolean didReadToken = yes; - - restart: - /* ignore any preceeding whitespace: - */ - token[0] = fgetc(file); - - while (isspace(token[0]) && token[0] != (char) EOF) - token[0] = fgetc(file); - - /* check for comments: - */ - if (token[0] == '#') - { - _read_til(file, '\n'); - - if (feof(file) || ferror(file)) - token[0] = EOF; - else - goto restart; - } - - /* read the rest of the token: - */ - if (token[0] == (char) EOF) - { - didReadToken = no; - - token[0] = '\0'; - } - else - { - int i = 0; - - while ( ! isspace(token[i]) && token[i] != (char) EOF) - token[++i] = fgetc(file); - - token[i] = '\0'; - } - - return didReadToken; -} - -#if ! SIMM_VIEWER - -STRUCT { - int numFrames; - int dataFrameRate; - int numSegments; - int gravityAxis; -} HtrHeaderInfo; - -/* ------------------------------------------------------------------------- - scan_htr_header - read the header section of the specified HTR file to - extract a few key bits of information that will help us choose good - defaults for the motion import options dialog box. ----------------------------------------------------------------------------- */ -static void scan_htr_header (const char* htrFile, HtrHeaderInfo* htrInfo) -{ - FILE* file = simm_fopen(htrFile, "r"); - - if (file) - { - while (_read_token(file, buffer)) - { - if (STRINGS_ARE_EQUAL(buffer, "[SegmentNames&Hierarchy]") || - STRINGS_ARE_EQUAL(buffer, "[BasePosition]")) - { - break; - } - else if (STRINGS_ARE_EQUAL(buffer, "NumFrames")) - { - fscanf(file, "%d", &htrInfo->numFrames); - } - else if (STRINGS_ARE_EQUAL(buffer, "DataFrameRate")) - { - fscanf(file, "%d", &htrInfo->dataFrameRate); - } - else if (STRINGS_ARE_EQUAL(buffer, "NumSegments")) - { - fscanf(file, "%d", &htrInfo->numSegments); - } - else if (STRINGS_ARE_EQUAL(buffer, "GlobalAxisofGravity")) - { - if (fscanf(file, "%s", buffer) == 1) - { - switch (tolower(buffer[0])) - { - case 'x': htrInfo->gravityAxis = POS_X; break; - case 'y': htrInfo->gravityAxis = POS_Y; break; - case 'z': htrInfo->gravityAxis = POS_Z; break; - } - } - } - } - fclose(file); - } -} - -#if ! OPENSMAC - -/* The mocap options are needed by the real-time analog import, - * so make them global. This assumes that: - * (1) A post-processed htr and/or analog file was pre-loaded - * (2) the real-time data matches the format of the *last* time a - * post-processed htr and/or analog file was loaded. - * Not the best way of handling this!!! JPL 10/31/00 - */ -glutMocapOptions options; - -/* ------------------------------------------------------------------------- - open_motion_analysis_file - Win32-specific routine to display mocap - options and then load mocap files. ----------------------------------------------------------------------------- */ -public ReturnCode open_motion_analysis_file( - const char htrFile[], - int modelIndex, - int numAnalogFiles, - const char* analogFiles[]) -{ - HtrHeaderInfo htrInfo; - char *p, htrPathBuf[1024], *baseName, *extension, *htrPath = htrPathBuf; - int i; - SBoolean isHtr2 = no; - ReturnCode rc = code_fine; - - memset(&options, 0, sizeof(options)); /* build_file_list_from_pattern() depends on this! */ - - /* split the input file name into path and base-name componants: - */ - strcpy(htrPathBuf, htrFile); - - extension = strrchr(htrPathBuf, '.'); - - if (extension) - { - lowerstr(extension); - - *extension++ = '\0'; - - isHtr2 = (SBoolean) STRINGS_ARE_EQUAL(extension, "htr2"); - } - p = strrchr(htrPathBuf, DIR_SEP_CHAR); - - if (p) - { - baseName = p + 1; - *p = '\0'; - } - else - baseName = htrPathBuf; - - /* extract useful information from htr file header: - */ - htrInfo.numFrames = 300; - htrInfo.dataFrameRate = 60; - htrInfo.numSegments = 0; - htrInfo.gravityAxis = POS_Y; - - scan_htr_header(htrFile, &htrInfo); - - /* scan the SIMM/Resources/mocap/mappings/ directory for mocap mapping files: - */ - sprintf(buffer, "%s%s*", get_mocap_folder(), "mappings" DIR_SEP_STRING); - - build_file_list_from_pattern(buffer, &options.mappings, &options.numMappings); - options.selectedMapping = 0; - - /* look for a mapping file name that contains the number of segments - * that are specified in the htr file's header: - */ - if (htrInfo.numSegments > 0 && options.numMappings > 0) - { - char numSegs[32]; - - sprintf(numSegs, "%d", htrInfo.numSegments); - - for (i = 0; i < options.numMappings; i++) - { - const char* p = strstr(options.mappings[i], numSegs); - - if (p && (p == options.mappings[i] || ! isdigit(*(p - 1)))) - { - options.selectedMapping = i; - break; - } - } - } - - /* scan the HTR file's directory and SIMM/Resources/mocap/basePositions/ - * for base position files: - */ - sprintf(buffer, "%s%s*.pose", htrPath, DIR_SEP_STRING); - - build_file_list_from_pattern(buffer, &options.basePositions, &options.numBasePositions); - - sprintf(buffer, "%s%s*", get_mocap_folder(), "basePositions" DIR_SEP_STRING); - - build_file_list_from_pattern(buffer, &options.basePositions, &options.numBasePositions); - - /* look for a base postion file that has the same base name as - * the motion file: - */ - options.selectedBasePosition = -1; - - for (i = 0; i < options.numBasePositions; i++) - { - /* check if the htr file's base name is part of a base position file: - */ - if (strstr(options.basePositions[i], baseName)) - { - options.selectedBasePosition = i; - break; - } - } - - if (options.selectedBasePosition == -1) - { - /* check if the a base position file's base name is part of the htr - * file's base name: - */ - for (i = 0; i < options.numBasePositions; i++) - { - p = strrchr(options.basePositions[i], DIR_SEP_CHAR); - - strcpy(buffer, p ? p+1 : options.basePositions[i]); - - if (strrchr(buffer, '.')) - *strrchr(buffer, '.') = '\0'; - - if (strstr(baseName, buffer)) - { - options.selectedBasePosition = i; - break; - } - } - } - - if (options.selectedBasePosition == -1) - { - /* otherwise select the appropriate base position as the default: - */ - for (i = 0; i < options.numBasePositions; i++) - { - if (strstr(options.basePositions[i], isHtr2 ? "global" : "init")) - { - options.selectedBasePosition = i; - break; - } - } - } - - if (options.selectedBasePosition == -1) - options.selectedBasePosition = 0; - - /* initialize the list of analog files to be imported with the motion: - */ - if (numAnalogFiles > 0 && analogFiles) - { - options.numAnalogFiles = numAnalogFiles; - options.analogFiles = (char**) analogFiles; - } - else - { - /* scan the HTR file's directory for analog & xls files with the same - * base name at the HTR file: - */ - if (options.numAnalogFiles == 0) - { - sprintf(buffer, "%s%s%s.anb", htrPath, DIR_SEP_STRING, baseName); - build_file_list_from_pattern(buffer, &options.analogFiles, &options.numAnalogFiles); - } - - if (options.numAnalogFiles == 0) - { - sprintf(buffer, "%s%s%s.anc", htrPath, DIR_SEP_STRING, baseName); - build_file_list_from_pattern(buffer, &options.analogFiles, &options.numAnalogFiles); - } - - sprintf(buffer, "%s%s%s.xls", htrPath, DIR_SEP_STRING, baseName); - build_file_list_from_pattern(buffer, &options.analogFiles, &options.numAnalogFiles); - } - - /* init the remainder of the mocap options record: - */ - options.inputFile = htrFile; - options.upDirection = htrInfo.gravityAxis; - - switch (options.upDirection) - { - case POS_X: options.faceDirection = POS_Z; break; - case POS_Y: options.faceDirection = POS_Z; break; - case POS_Z: options.faceDirection = POS_X; break; - } - options.rightLeg = 1; - options.leftLeg = 1; - options.upperBody = 1; - options.muscles = 1; - options.scaleSegments = 1; - options.emgSmoothingCycles = 1; - options.emgWindowSize = 0.032; - options.showBasePosition = 0; - options.showZeroFrame = 0; - options.autocalibrateForceplates = 1; - options.modelName = baseName; - options.saveIntermediateFiles = 0; - - sprintf(buffer, "%s.jnt", baseName); - mstrcpy(&options.jntFile, buffer); - - sprintf(buffer, "%s.msl", baseName); - mstrcpy(&options.mslFile, buffer); - - sprintf(buffer, "%s.mot", baseName); - mstrcpy(&options.motFile, buffer); - - /* initialize the realtime import options: - */ - strcpy(buffer, baseName); - lowerstr(buffer); - - if (is_module_present(MOTION_REAL) == no) - options.allowRealtimeImport = -1; - else - options.allowRealtimeImport = strstr(buffer, "realtime") ? 1 : 0; - options.realtimeDuration = (double) htrInfo.numFrames / htrInfo.dataFrameRate; - options.realtimeFrequency = htrInfo.dataFrameRate; - options.timeScaleMin = - options.realtimeDuration; - options.timeScaleMax = 0.0; - options.slidingTimeScale = 1; - - /* display the mocap options dialog to the user: - */ - if (glutMocapOptionsBox(&options) != GLUT_MSG_OK) - { - rc = code_bad; - goto cleanup; - } - - /* translate the specified Motion Analysis HTR file into SIMM joint, - * muscle, and motion files: - */ - rc = mocap_to_simm(&options, &modelIndex); - - if (rc != code_fine) - goto cleanup; - - //resize_model_display(gModel[modelIndex]); TODO_SCENE: mocap_to_simm should return scene index? - -#if 0 - fprintf(stderr, "@@@ %d segments, %d joints, %d gencoords, %d muscles\n", - model[modelIndex]->numsegments, - model[modelIndex]->numjoints, - model[modelIndex]->numgencoords, - model[modelIndex]->nummuscles); -#endif - - cleanup: - if (options.mappings) - { - for (i = 0; i < options.numMappings; i++) - FREE_IFNOTNULL(options.mappings[i]); - - free(options.mappings); - } - if (options.basePositions) - { - for (i = 0; i < options.numBasePositions; i++) - FREE_IFNOTNULL(options.basePositions[i]); - - free(options.basePositions); - } - if (options.analogFiles != analogFiles) - { - for (i = 0; i < options.numAnalogFiles; i++) - FREE_IFNOTNULL(options.analogFiles[i]); - - free(options.analogFiles); - } - FREE_IFNOTNULL(options.jntFile); - FREE_IFNOTNULL(options.mslFile); - FREE_IFNOTNULL(options.motFile); - - return rc; - -} /* open_motion_analysis_file */ - -#endif // ! OPENSMAC - - -/* ------------------------------------------------------------------------- - open_tracked_file - Win32-specific routine to display trc import options and - load a trc file. ----------------------------------------------------------------------------- */ -public ReturnCode open_tracked_file( - const char trcFile[], - int modelIndex, - int numAnalogFiles, - const char* analogFiles[], - SBoolean showDialogBox) -{ - FILE* fp; - glutTRCOptions options; - smTRCHeaderInfo trcInfo; - ReturnCode rc; - char *p, trcPathBuf[CHARBUFFER], buf[CHARBUFFER], buf2[CHARBUFFER], analogFileBuf[CHARBUFFER]; - char *baseName, *extension, *trcPath = trcPathBuf; - ModelStruct* ms = NULL; - smC3DStruct* c3d = NULL; - MocapInfo mi; - glutMocapOptions mo; - - if (modelIndex < 0) - return code_bad; - - ms = gModel[modelIndex]; - - init_tracked_file_options(ms, &options, trcFile); - options.isC3DFile = 0; - options.markerNameSource = -1; - - /* split the input file name into path and base-name components */ - strcpy(trcPathBuf, trcFile); - - extension = strrchr(trcPathBuf, '.'); - - if (extension) - { - lowerstr(extension); - *extension++ = '\0'; - } - p = strrchr(trcPathBuf, DIR_SEP_CHAR); - - if (p) - { - baseName = p + 1; - *p = '\0'; - } - else - baseName = trcPathBuf; - - if (smReadTrackedFileHeader(trcFile, &trcInfo) != smNoError) - { - strcpy(errorbuffer, smGetGlobalErrorBuffer()); - error(abort_action, errorbuffer); - return code_bad; - } - - if (trcInfo.numFrames <= 0) - { - sprintf(buffer, "There are no frames of data in %s", trcFile); - error(none, buffer); - return code_bad; - } - - options.firstFrame = trcInfo.firstFrameNum; - options.lastFrame = trcInfo.firstFrameNum + trcInfo.numFrames - 1; - - sprintf(buffer, "markers: %d frames: %d (%d to %d)", trcInfo.numMarkers, trcInfo.numFrames, - trcInfo.firstFrameNum, trcInfo.numFrames + trcInfo.firstFrameNum - 1); - mstrcpy(&options.infoText[0], buffer); - sprintf(buffer, "data rate: %.1lf Hz camera rate: %.1lf Hz", trcInfo.dataRate, trcInfo.cameraRate); - mstrcpy(&options.infoText[1], buffer); - - /* initialize the list of analog files to be imported with the motion */ - if (numAnalogFiles > 0 && analogFiles) - { - options.numAnalogFiles = numAnalogFiles; - options.analogFiles = (char**) analogFiles; - /* If the user selected one or more analog files, disable the analog auto-load function */ - options.loadAnalogData = 3; /* auto-load will be inactive, but other analog checkboxes will be active */ - } - else - { - /* Look for the corresponding analog data files in the chosen folder. - * If one or more exists, set the loadAnalogData option to yes; - * if none exists, set the option to be grayed out. - */ - options.loadAnalogData = 1; /* default is active and checked */ - strcpy(analogFileBuf, baseName); - strcat(analogFileBuf, ".anb"); - if ((fp = fopen(analogFileBuf, "r")) == NULL) - { - strcpy(analogFileBuf, baseName); - strcat(analogFileBuf, ".anc"); - if ((fp = fopen(analogFileBuf, "r")) == NULL) - { - strcpy(analogFileBuf, baseName); - strcat(analogFileBuf, ".xls"); - if ((fp = fopen(analogFileBuf, "r")) == NULL) - { - /* no corresponding analog files are in the folder */ - options.loadAnalogData = 2; /* all analog checkboxes will be inactive */ - } - } - } - if (fp != NULL) - fclose(fp); - - /* scan the HTR file's directory for analog & xls files with the same - * base name at the HTR file: - */ - if (options.loadAnalogData == 1) - { - if (options.numAnalogFiles == 0) - { - sprintf(buffer, "%s%s%s.anb", trcPath, DIR_SEP_STRING, baseName); - build_file_list_from_pattern(buffer, &options.analogFiles, &options.numAnalogFiles); - } - - if (options.numAnalogFiles == 0) - { - sprintf(buffer, "%s%s%s.anc", trcPath, DIR_SEP_STRING, baseName); - build_file_list_from_pattern(buffer, &options.analogFiles, &options.numAnalogFiles); - } - - sprintf(buffer, "%s%s%s.xls", trcPath, DIR_SEP_STRING, baseName); - build_file_list_from_pattern(buffer, &options.analogFiles, &options.numAnalogFiles); - } - } - - if (showDialogBox) - { - // Display the trc options dialog to the user. - if (glutTRCOptionsBox(&options) != GLUT_MSG_OK) - { - rc = code_bad; - goto cleanup; - } - else - { - // To the user, firstFrame and lastFrame are w.r.t. the user-defined - // frame numbers in the file, which may not start at 1. Once the user - // has selected the desired frame sequence, keep them defined this way, - // but check to make sure the user hasn't changed them to invalid - // numbers. - if (options.firstFrame < trcInfo.firstFrameNum) - options.firstFrame = trcInfo.firstFrameNum; - else if (options.firstFrame > (trcInfo.firstFrameNum + trcInfo.numFrames - 1)) - options.firstFrame = trcInfo.firstFrameNum + trcInfo.numFrames - 1; - - if (options.lastFrame < trcInfo.firstFrameNum) - options.lastFrame = trcInfo.firstFrameNum; - else if (options.lastFrame > (trcInfo.firstFrameNum + trcInfo.numFrames - 1)) - options.lastFrame = trcInfo.firstFrameNum + trcInfo.numFrames - 1; - - if (options.frameIncrement == 0) - options.frameIncrement = 1; - else if (options.firstFrame > options.lastFrame && options.frameIncrement > 0) - options.frameIncrement = -options.frameIncrement; - } - } - - // These are the only two fields in MocapInfo that are - // needed for importing analog data. - mi.model = ms; - mi.dataFrameRate = trcInfo.dataRate; - - switch (mi.model->gravity) - { - case smNegX: - mo.upDirection = POS_X; - break; - case smX: - mo.upDirection = NEG_X; - break; - case smNegY: - mo.upDirection = POS_Y; - break; - case smY: - mo.upDirection = NEG_Y; - break; - case smNegZ: - mo.upDirection = POS_Z; - break; - case smZ: - mo.upDirection = NEG_Z; - break; - default: - mo.upDirection = NEG_Z; - break; - } - - mo.emgSmoothingCycles = options.emgSmoothingPasses; - mo.emgWindowSize = options.emgWindowSize; - mo.muscles = 1; - mo.autocalibrateForceplates = options.calibrateForcePlates; - mo.calculateGaitEvents = options.calculateGaitEvents; - - // Load all of the analog data files into the smAnalogStruct struct, - // then pass the struct to loadTrackedFile() so it can put the data - // into the motion. - if (options.loadAnalogData == 1 && options.numAnalogFiles > 0) - c3d = read_analog_data_files(options.numAnalogFiles, options.analogFiles, &mi, &mo); - - // If there are no analog files or there was an error reading them, - // allocate space for the C3D struct. - if (!c3d) - c3d = (smC3DStruct*) simm_calloc(1, sizeof(smC3DStruct)); - - rc = loadTrackedFile(ms, &options, c3d, yes); - - free_analog_data(c3d->analogData); - free(c3d); - - if (rc != code_fine) - error(none, "Could not create model."); - -cleanup: - smFreeTRCHeader(&trcInfo); - FREE_IFNOTNULL(options.inputFile); - FREE_IFNOTNULL(options.htrFile); - FREE_IFNOTNULL(options.motFile); - - return rc; -} - -/* ------------------------------------------------------------------------- - open_c3d_file - Win32-specific routine to display c3d import options and - load a c3d file. ----------------------------------------------------------------------------- */ -public ReturnCode open_c3d_file(const char c3dFile[], int modelIndex, SBoolean showDialogBox) -{ - int numFrames; - glutTRCOptions options; - smC3DHeader head; - ReturnCode rc; - char *p, buf[CHARBUFFER], buf2[CHARBUFFER]; - ModelStruct* ms = NULL; - MocapInfo mi; - glutMocapOptions mo; - - if (modelIndex < 0) - return code_bad; - - ms = gModel[modelIndex]; - - init_tracked_file_options(ms, &options, c3dFile); - options.isC3DFile = 1; - - smScanC3DHeader(c3dFile, &head); - - numFrames = head.lastFrame - head.firstFrame + 1; - - if (numFrames <= 0) - { - sprintf(buffer, "There are no frames of data in %s", c3dFile); - error(none, buffer); - return code_bad; - } - - options.firstFrame = head.firstFrame; - options.lastFrame = head.lastFrame; - - sprintf(buffer, "markers: %d frames: %d", head.num3DPoints, numFrames); - mstrcpy(&options.infoText[0], buffer); - sprintf(buffer, "data rate: %.1lf Hz", head.pointFrameRate); - mstrcpy(&options.infoText[1], buffer); - - if (showDialogBox) - { - // Display the trc options dialog to the user. - if (glutTRCOptionsBox(&options) != GLUT_MSG_OK) - { - rc = code_bad; - goto cleanup; - } - else - { - // To the user, firstFrame and lastFrame are w.r.t. the user-defined - // frame numbers in the file, which may not start at 1. Once the user - // has selected the desired frame sequence, keep them defined this way, - // but check to make sure the user hasn't changed them to invalid - // numbers. - if (options.firstFrame < head.firstFrame) - options.firstFrame = head.firstFrame; - else if (options.firstFrame > head.lastFrame) - options.firstFrame = head.lastFrame; - - if (options.lastFrame < head.firstFrame) - options.lastFrame = head.firstFrame; - else if (options.lastFrame > head.lastFrame) - options.lastFrame = head.lastFrame; - - if (options.frameIncrement == 0) - options.frameIncrement = 1; - else if (options.firstFrame > options.lastFrame && options.frameIncrement > 0) - options.frameIncrement = -options.frameIncrement; - } - } - - // These are the only two fields in MocapInfo that are - // needed for importing analog data. - mi.model = ms; - mi.dataFrameRate = head.pointFrameRate; - - switch (mi.model->gravity) - { - case smNegX: - mo.upDirection = POS_X; - break; - case smX: - mo.upDirection = NEG_X; - break; - case smNegY: - mo.upDirection = POS_Y; - break; - case smY: - mo.upDirection = NEG_Y; - break; - case smNegZ: - mo.upDirection = POS_Z; - break; - case smZ: - mo.upDirection = NEG_Z; - break; - default: - mo.upDirection = NEG_Z; - break; - } - - mo.emgSmoothingCycles = options.emgSmoothingPasses; - mo.emgWindowSize = options.emgWindowSize; - mo.muscles = 1; - mo.autocalibrateForceplates = options.calibrateForcePlates; - - rc = loadC3DFile(ms, &options, &mi, &mo); - - if (rc != code_fine) - error(none, "Could not create model."); - -cleanup: - FREE_IFNOTNULL(options.inputFile); - FREE_IFNOTNULL(options.htrFile); - FREE_IFNOTNULL(options.motFile); - - return rc; -} - -static void init_tracked_file_options(ModelStruct* model, glutTRCOptions* options, const char inputFileName[]) -{ - int len; - const char* pref; - - memset(options, 0, sizeof(glutTRCOptions)); - - mstrcpy(&options->inputFile, inputFileName); - - // the maximum length of htrFile and motFile - len = strlen(inputFileName) + 4; - - options->htrFile = (char*)simm_malloc(len * sizeof(char)); - change_filename_suffix(inputFileName, options->htrFile, "htr", len); - options->motFile = (char*)simm_malloc(len * sizeof(char)); - change_filename_suffix(inputFileName, options->motFile, "mot", len); - - pref = get_preference("MOCAP_INCREMENT"); - if (pref) - options->frameIncrement = atoi(pref); - else - options->frameIncrement = 1; - - if (model->solver.method == smLevenbergMarquart) - options->quickSolve = 0; - else - options->quickSolve = 1; - - if (is_preference_on(get_preference("MOCAP_CROP_ENDS")) == no) - options->cropEnds = 0; - else - options->cropEnds = 1; - - if (is_preference_on(get_preference("MOCAP_CALC_DERIVATIVES")) == yes) - options->calculateDerivatives = 1; - else - options->calculateDerivatives = 0; - - if (is_preference_on(get_preference("MOCAP_SHOW_MARKERS")) == yes) - options->showMarkers = 1; - else - options->showMarkers = 0; - - if (is_preference_on(get_preference("MOCAP_CALC_GAIT_EVENTS")) == yes) - options->calculateGaitEvents = 1; - else - options->calculateGaitEvents = 0; - - options->xAxisUnits = X_AXIS_TIME; - pref = get_preference("MOCAP_X_AXIS"); - if (pref) - { - if (STRINGS_ARE_EQUAL(pref, "TIME")) - options->xAxisUnits = X_AXIS_TIME; - else if (STRINGS_ARE_EQUAL(pref, "FRAME_NUMBER")) - options->xAxisUnits = X_AXIS_FRAME_NUMBER; - } - - if (is_preference_on(get_preference("MOCAP_START_AT_ZERO")) == yes) - options->xAxisStartZero = 1; - else - options->xAxisStartZero = 0; - - if (is_preference_on(get_preference("MOCAP_LOAD_ANALOG")) == no) - options->loadAnalogData = 0; - else - options->loadAnalogData = 1; // active and checked - - if (is_preference_on(get_preference("MOCAP_CALIBRATE_FORCES")) == no) - options->calibrateForcePlates = 0; - else - options->calibrateForcePlates = 1; - - options->emgSmoothingPasses = initializeEmgSmoothingPasses(); - options->emgWindowSize = initializeEmgWindowSize(); - - options->markerNameSource = POINT_LABELS; - pref = get_preference("MOCAP_MARKER_NAME_SOURCE"); - if (pref) - { - if (STRINGS_ARE_EQUAL(pref, "POINT:LABELS")) - options->markerNameSource = POINT_LABELS; - else if (STRINGS_ARE_EQUAL(pref, "POINT:DESCRIPTIONS")) - options->markerNameSource = POINT_DESCRIPTIONS; - } - - if (is_preference_on(get_preference("MOCAP_SAVE_HTR_FILE")) == yes) - options->saveHTRFile = 1; - else - options->saveHTRFile = 0; - - if (is_preference_on(get_preference("MOCAP_SAVE_MOTION_FILE")) == yes) - options->saveMOTFile = 1; - else - options->saveMOTFile = 0; -} - -#endif /* ! SIMM_VIEWER */ - -#endif /* WIN32 */ - -#endif /* INCLUDE_MOCAP_MODULE */ - -#endif /* ! ENGINE */ - - -void post_process_bones(ModelStruct* ms) -{ - int i, j, k; - - for (i = 0; i < ms->numsegments; i++) - { - for (j = 0; j < ms->segment[i].numBones; j++) - { - /* If the material and/or drawmode were not specified for the - * bone, it inherits them from the segment. - */ - if (ms->segment[i].bone[j].drawmode == -1) - ms->segment[i].bone[j].drawmode = ms->segment[i].drawmode; - if (ms->segment[i].bone[j].material == -1) - ms->segment[i].bone[j].material = ms->segment[i].material; - - /* Multiply each bone vertex by the segment's scale factors. */ - for (k = 0; k < ms->segment[i].bone[j].num_vertices; k++) - { - ms->segment[i].bone[j].vertex[k].coord[XX] *= ms->segment[i].bone_scale[XX]; - ms->segment[i].bone[j].vertex[k].coord[YY] *= ms->segment[i].bone_scale[YY]; - ms->segment[i].bone[j].vertex[k].coord[ZZ] *= ms->segment[i].bone_scale[ZZ]; - } - - /* Multiply the bone's bounding box by the segment's scale factors. */ - ms->segment[i].bone[j].bc.x1 *= ms->segment[i].bone_scale[XX]; - ms->segment[i].bone[j].bc.x2 *= ms->segment[i].bone_scale[XX]; - ms->segment[i].bone[j].bc.y1 *= ms->segment[i].bone_scale[YY]; - ms->segment[i].bone[j].bc.y2 *= ms->segment[i].bone_scale[YY]; - ms->segment[i].bone[j].bc.z1 *= ms->segment[i].bone_scale[ZZ]; - ms->segment[i].bone[j].bc.z2 *= ms->segment[i].bone_scale[ZZ]; - } - } -} - - -/* CHECK_DEFINITIONS: Checks to make sure that no joints, segments, or - * functions were referenced but not defined in the joint file. - */ -ReturnCode check_definitions(ModelStruct* ms) -{ - int i, j, k; - SBoolean any_errors = no; - char buf[CHARBUFFER]; - - if (ms->numsegments == 0) - { - error(none,"There were no segments defined in the joint file."); - return code_bad; - } - - for (i=0; inumsegments; i++) - { - if (ms->segment[i].defined == no) - { - (void)sprintf(errorbuffer,"Segment %s referenced but not defined.", ms->segment[i].name); - error(none,errorbuffer); - any_errors = yes; - } - } - - for (i=0; inumgencoords; i++) - { - if (ms->gencoord[i]->defined == no) - { - (void)sprintf(errorbuffer,"Generalized coordinate %s referenced but not defined.", ms->gencoord[i]->name); - error(none,errorbuffer); - any_errors = yes; - } - } - - for (i=0; ifunc_array_size; i++) - { - if (ms->function[i] && ms->function[i]->used == dpYes && ms->function[i]->status == dpFunctionUndefined) - { - (void)sprintf(errorbuffer,"Function f%d referenced but not defined.", ms->function[i]->usernum); - error(none,errorbuffer); - any_errors = yes; - } - } - - if (ms->numsegments > 1) - { - for (i=0; inumsegments; i++) - { - for (j=0; jnumjoints; j++) - if (ms->joint[j].from == i || ms->joint[j].to == i) - break; - if (j == ms->numjoints) - { - (void)sprintf(errorbuffer,"Segment %s not used in any joint.", ms->segment[i].name); - error(none,errorbuffer); - any_errors = yes; - } - } - } - -#if NO_LONGER_NECESSARY - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - if (ms->joint[i].dofs[j].type == constant_dof) - continue; - gc = ms->joint[i].dofs[j].gencoord; - fn = ms->joint[i].dofs[j].funcnum; - if (ms->gencoord[gc]->clamped == no || ms->function[fn].defined == no) - continue; - if (ms->function[fn].x[0] > ms->gencoord[gc]->range.start || - ms->function[fn].x[ms->function[fn].numpoints-1] < - ms->gencoord[gc]->range.end) - { - (void)sprintf(errorbuffer,"Function f%d does not cover full range of gencoord %s", - ms->function[fn].usernum, ms->gencoord[gc]->name); - error(none,errorbuffer); - any_errors = yes; - } - } - } -#endif - - check_gencoord_usage(ms,no); - - for (i=0; idis.mat.num_materials; i++) - { - if (ms->dis.mat.materials[i].defined_yet == no) - { - (void)sprintf(errorbuffer,"Material %s referenced but not defined.", ms->dis.mat.materials[i].name); - error(none,errorbuffer); - any_errors = yes; - } - } - - /* check spring contact definitions, create list of points for each floor */ -#if 0 - /* check that floor for each spring exists */ - for (i = 0; i < ms->numsegments; i++) - { - for (j = 0; j < ms->segment[i].numSpringPoints; j++) - { - SBoolean floor_defined = no; - for (k = 0; k < ms->numsegments; k++) - { - if (ms->segment[k].springFloor) - { - if (STRINGS_ARE_EQUAL(ms->segment[k].springFloor->name, ms->segment[i].springPoint[j].floorName)) - { - floor_defined = yes; - } - } - } - if (!floor_defined) - { - (void)sprintf(errorbuffer,"Floor %s referenced but not defined.", ms->segment[i].springPoint[j].floorName); - error(none,errorbuffer); - any_errors = yes; - } - } - } -#endif - - if (any_errors == yes) - return code_bad; - - return code_fine; -} - - -void check_gencoord_usage(ModelStruct* ms, SBoolean change_visibility) -{ - int i, j, k; - SBoolean used_in_model; - - ms->numunusedgencoords = 0; - for (i=0; inumgencoords; i++) - { - used_in_model = no; - - for (j=0; jnumjoints; j++) - { - for (k=0; k<6; k++) - { - if (ms->joint[j].dofs[k].type == constant_dof) - continue; - if (ms->joint[j].dofs[k].gencoord == ms->gencoord[i]) - { - used_in_model = yes; - break; - } - } - if (used_in_model == yes) - { - /* If the gencoord was previously unused, turn its slider on */ - if (change_visibility == yes && ms->gencoord[i]->used_in_model == no) - { - ms->gencform.option[i].active = yes; - ms->gencslider.sl[i].visible = yes; - ms->gc_chpanel.checkbox[i].active = yes; - ms->gc_lockPanel.checkbox[i].active = yes; - ms->gencform.option[i].visible = yes; - ms->gencslider.sl[i].visible = yes; - ms->gc_chpanel.checkbox[i].visible = yes; - ms->gc_lockPanel.checkbox[i].visible = yes; -#if ! ENGINE -#if ! OPENSMAC - make_and_queue_simm_event(GENCOORD_CHANGED, ms->modelnum, &ms->gencoord[i], NULL, i, ZERO); -#endif -#endif - } - ms->gencoord[i]->used_in_model = yes; - break; - } - } - if (used_in_model == no) - { - ms->numunusedgencoords++; - if (change_visibility) - { - /* If the gencoord was previously used, turn its slider off */ - if (ms->gencoord[i]->used_in_model == yes) - { - ms->gencform.option[i].active = no; - ms->gencslider.sl[i].visible = no; - ms->gc_chpanel.checkbox[i].active = no; - ms->gc_lockPanel.checkbox[i].active = no; - ms->gencform.option[i].visible = no; - ms->gencslider.sl[i].visible = no; - ms->gc_chpanel.checkbox[i].visible = no; - ms->gc_lockPanel.checkbox[i].visible = no; -#if ! ENGINE -#if ! OPENSMAC - make_and_queue_simm_event(GENCOORD_CHANGED, ms->modelnum, &ms->gencoord[i], NULL, i, ZERO); -#endif -#endif - } - } - else - { - (void)sprintf(errorbuffer,"Gencoord %s not used in any joint.", ms->gencoord[i]->name); - error(none,errorbuffer); - } - ms->gencoord[i]->used_in_model = no; - } - } -} - - -void find_ground_joint(ModelStruct* ms) -{ - // Find the number of the ground segment. If there is no ground segment, - // make the zeroth segment ground. - ms->ground_segment = enter_segment(ms, "ground", no); - - if (ms->ground_segment == -1) - ms->ground_segment = 0; - - ms->initial_ground_segment = ms->ground_segment; - - /* Set the initial value of the current frame, which is the ground frame */ - ms->currentframe = ms->ground_segment; -} - -SBoolean modelHasMuscles(ModelStruct* ms) -{ - if (ms->nummuscles > 0 || ms->default_muscle->name != NULL) - return yes; - - return no; -} - -static double initializeEmgWindowSize() -{ - const char* value = get_preference("EMG_SMOOTHING_WINDOW"); - - if (value) - { - double v = atof(value); - if (v > 0.0001 && v < 10.0) - return v; - } - - return 0.032; -} - -static int initializeEmgSmoothingPasses() -{ - const char* value = get_preference("EMG_SMOOTHING_PASSES"); - - if (value) - { - int i = atoi(value); - if (i >= 0 && i < 50) - return i; - } - - return 1; -} - -#if ! ENGINE -ReturnCode open_model_archive(char archiveFilename[], int* modelIndex) -{ - char jointFilename[CHARBUFFER], folder[CHARBUFFER], bones_folder[CHARBUFFER], cwdSave[CHARBUFFER], resFolder[CHARBUFFER], *buf = NULL; - int result; - ReturnCode rc = code_fine; - - _getcwd(cwdSave, CHARBUFFER); - - mstrcpy(&buf, glutGetTempFileName("smm")); - strcpy(folder, buf); - result = makeDir(folder); - if (result != 0 && errno != EEXIST) - { - sprintf(errorbuffer, "Unable to create temporary folder %s\n", folder); - error(none, errorbuffer); - rc = code_bad; - goto cleanup; - } - - // Temporarily set the current working directory to the tmp - // folder, so SIMM will find the motion files. After loading - // the model, it should be set back to whatever it was (which - // should be the folder containing the archive file. - chdir(folder); - - sprintf(buffer, "Extracting model from archive %s...", archiveFilename); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); - - // Make the archive - strcpy(resFolder, get_preference("SIMM_FOLDER")); - append_if_necessary(resFolder, DIR_SEP_CHAR); - sprintf(buffer, "\"%s7za\" x -p%s -o\"%s\" -y \"%s\"", resFolder, archive_password, folder, archiveFilename); - result = glutSystem(buffer); - - // The .jnt file should always be named model.jnt. Try to open - // a file by that name and print an error about an invalid - // archive if it is not found. - sprintf(jointFilename, "%s\\model.jnt", folder); - if (file_exists(jointFilename) == no) - { - sprintf(buffer, "Error opening model archive %s. Archive is invalid or corrupted.", archiveFilename); - error(none, buffer); - rc = code_bad; - goto cleanup; - } - - // Load the model - rc = add_model(jointFilename, NULL, -1, modelIndex, no); - if (rc == code_bad) - { - sprintf(buffer, "Error reading model from archive %s.", archiveFilename); - error(none, buffer); - goto cleanup; - } - - if (STRINGS_ARE_EQUAL(gModel[*modelIndex]->bonepathname, ".")) - strcpy(bones_folder, folder); - else - sprintf(bones_folder, "%s\\%s", folder, gModel[*modelIndex]->bonepathname); - - sprintf(buffer, "Extracting model from archive %s... Done.", archiveFilename); - message(buffer, OVERWRITE_LAST_LINE, DEFAULT_MESSAGE_X_OFFSET); - -cleanup: - FREE_IFNOTNULL(buf); - - // Reset the current working directory. - chdir(cwdSave); - - // Delete the files in the temporary folder - if (1) - { - OSVERSIONINFO osvi; - osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO); - GetVersionEx(&osvi); - if (osvi.dwMajorVersion < 5) // Windows 95, 98, Me, NT - sprintf(buffer, "deltree /y \"%s\"", folder); - else // Windows 2000, XP, Vista, 7 - sprintf(buffer, "rmdir /s /q \"%s\"", folder); - result = glutSystem(buffer); - } - - return rc; -} -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/materials.c b/OpenSim/Utilities/simmToOpenSim/materials.c deleted file mode 100644 index a615f98070..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/materials.c +++ /dev/null @@ -1,1082 +0,0 @@ -/******************************************************************************* - - MATERIALS.C - - Author: Peter Loan - - Date: 30-DEC-91 - - Copyright (c) 1992-8 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: - - Routines: - read_material - define_material - enter_material - - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#if ! ENGINE -#include "tiffio.h" -#endif - - -/*************** DEFINES (for this file only) *********************************/ -#define ALL_MODELS -1 - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static GLfloat light0_diffuse[] = {1.0f, 0.96f, 0.87f, 1.0f}; -static GLfloat light0_position[] = {0.10f, 0.01f, 0.5f, 0.0f}; -static GLfloat light1_diffuse[] = {1.0f, 1.0f, 0.7f, 1.0f}; -static GLfloat light1_position[] = {-0.01f, 0.01f, 0.5f, 0.0f}; -static GLfloat light_model[] = {0.77f, 0.67f, 0.57f}; -static MaterialStruct null_mat = {"null_mat", yes, no, -yes, 0.0f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.0f, 0.0f, 0.0f, 1.0f, /* diffuse */ -yes, 0.0f, 0.0f, 0.0f, 1.0f, /* specular */ -yes, 0.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 1.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct arrow = {"def_motion", yes, no, -yes, 0.1f, 0.7f, 0.1f, 1.0f, /* ambient */ -yes, 0.2f, 0.3f, 0.2f, 1.0f, /* diffuse */ -yes, 0.2f, 0.0f, 0.2f, 1.0f, /* specular */ -yes, 1.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct joint_vector = {"def_joint_vector", yes, no, -yes, 0.01f, 0.9f, 0.01f, //0.9f, 0.3f, 0.2f, 1.0f, /* ambient */ -yes, 0.1f, 0.9f, 0.1f, //0.2f, 0.3f, 0.2f, 1.0f, /* diffuse */ -yes, 0.14f, 0.0f, 0.2f, 1.0f, /* specular */ -yes, 25.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct def_bone = {"def_bone", yes, no, -yes, 0.7f, 0.70f, 0.7f, 1.0f, /* ambient */ -yes, 0.6f, 0.45f, 0.4f, 1.0f, /* diffuse */ -yes, 0.7f, 0.55f, 0.4f, 1.0f, /* specular */ -yes, 10.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct highlighted1_polygon = {"def_sel_poly", yes, no, -yes, 0.2f, 0.70f, 0.2f, 1.0f, /* ambient */ -yes, 0.1f, 0.45f, 0.1f, 1.0f, /* diffuse */ -yes, 0.4f, 0.65f, 0.4f, 1.0f, /* specular */ -yes, 10.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct highlighted2_polygon = {"def_sel_poly2", yes, no, -yes, 0.2f, 0.2f, 0.7f, 1.0f, /* ambient */ -yes, 0.1f, 0.1f, 0.45f, 1.0f, /* diffuse */ -yes, 0.4f, 0.4f, 0.65f, 1.0f, /* specular */ -yes, 10.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct marker_mat = {"def_marker", yes, no, -yes, 0.8f, 0.2f, 0.8f, 1.0f, /* ambient */ -yes, 0.4f, 0.1f, 0.4f, 1.0f, /* diffuse */ -yes, 1.0f, 0.3f, 1.0f, 1.0f, /* specular */ -yes, 30.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct sel_marker_mat = {"sel_marker", yes, no, -yes, 0.9f, 0.9f, 0.0f, 1.0f, /* ambient */ -yes, 0.2f, 0.2f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 0.0f, 1.0f, /* specular */ -yes, 30.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct bone_outline = {"def_bone_outline", yes, no, -yes, 0.2f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.2f, 0.0f, 0.0f, 1.0f, /* diffuse */ -yes, 0.2f, 0.0f, 0.0f, 1.0f, /* specular */ -yes, 90.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct def_min_muscle = {"def_min_muscle", yes, no, -yes, 0.2f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.4f, 0.0f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 0.15f, 0.15f, 1.0f, /* specular */ -yes, 40.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct def_max_muscle = {"def_max_muscle", yes, no, -yes, 0.4f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.7f, 0.0f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 0.15f, 0.15f, 1.0f, /* specular */ -yes, 90.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct def_muscle_point = {"def_muscle_point", yes, no, -yes, 0.0f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.0f, 0.0f, 0.7f, 1.0f, /* diffuse */ -yes, 0.15f, 0.15f, 1.0f, 1.0f, /* specular */ -yes, 90.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct sel_muscle_point = {"def_sel_muscle_point", yes, no, -yes, 0.0f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.7f, 0.7f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 0.2f, 1.0f, /* specular */ -yes, 90.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct def_world_object = {"def_world_object", yes, no, -yes, 0.2f, 0.2f, 0.7f, 1.0f, /* ambient */ -yes, 0.1f, 0.1f, 0.45f, 1.0f, /* diffuse */ -yes, 0.4f, 0.4f, 0.65f, 1.0f, /* specular */ -yes, 10.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; - -/* constraint object materials */ -static MaterialStruct co_current_active = {"co_current_active", yes, no, -yes, 0.0f, 1.0f, 0.6f, 1.0f, /* ambient */ -yes, 0.0f, 0.5f, 0.6f, 1.0f, /* diffuse */ -yes, 0.0f, 0.7f, 0.7f, 1.0f, /* specular */ -yes, 10.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct co_current_inactive = {"co_current_inactive", yes, no, -yes, 0.6f, 0.05f, 0.0f, 1.0f, /* ambient */ -yes, 0.2f, 0.2f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 0.0f, 1.0f, /* specular */ -yes, 30.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct co_back_active = {"co_back_active", yes, no, -yes, 0.0f, 0.55f, 0.25f, 1.0f, /* ambient */ -yes, 0.0f, 0.05f, 0.06f, 1.0f, /* diffuse */ -yes, 0.0f, 0.07f, 0.07f, 1.0f, /* specular */ -yes, 10.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct co_back_inactive = {"co_back_inactive", yes, no, -yes, 0.4f, 0.02f, 0.0f, 1.0f, /* ambient */ -yes, 0.02f, 0.02f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 0.0f, 1.0f, /* specular */ -yes, 30.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; - -/* constraint point materials */ -static MaterialStruct cp_sel_ok = {"cp_sel_ok", yes, no, -yes, 0.9f, 0.9f, 0.0f, 1.0f, /* ambient */ -yes, 0.2f, 0.2f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 0.0f, 1.0f, /* specular */ -yes, 30.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct cp_sel_broken = {"cp_sel_broken", yes, no, -//yes, 0.0f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.2f, 0.0f, 0.0f, 1.0f, /* ambient */ -yes, 0.7f, 0.7f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 0.2f, 1.0f, /* specular */ -yes, 90.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct cp_current_ok = {"cp_current_ok", yes, no, -yes, 0.0f, 1.0f, 0.6f, 1.0f, /* ambient */ -yes, 0.0f, 0.5f, 0.6f, 1.0f, /* diffuse */ -yes, 0.0f, 0.7f, 0.7f, 1.0f, /* specular */ -yes, 10.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct cp_current_broken = {"cp_current_broken", yes, no, -yes, 1.0f, 0.3f, 0.01f, 1.0f, /* ambient */ -yes, 0.5f, 0.1f, 0.1f, 1.0f, /* diffuse */ -yes, 0.5f, 0.5f, 0.5f, 1.0f, /* specular */ -yes, 30.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct cp_back_broken = {"cp_back_broken", yes, no, -yes, 0.4f, 0.02f, 0.0f, 1.0f, /* ambient */ -yes, 0.02f, 0.02f, 0.0f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 0.0f, 1.0f, /* specular */ -yes, 30.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct masscenter_mat1 = {"masscenter_mat1", yes, no, -yes, 0.4f, 0.2f, 0.0f, 1.0f, /* ambient */ -yes, 0.22f, 0.18f, 0.05f, 1.0f, /* diffuse */ -yes, 0.8f, 0.8f, 0.4f, 1.0f, /* specular */ -yes, 93.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct masscenter_mat2 = {"masscenter_mat2", yes, no, -yes, 0.2f, 0.2f, 0.2f, 1.0f, /* ambient */ -yes, 0.05f, 0.05f, 0.05f, 1.0f, /* diffuse */ -yes, 1.0f, 1.0f, 1.0f, 1.0f, /* specular */ -yes, 95.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; -static MaterialStruct foot_print_mat = {"foot_print_mat", yes, no, -yes, 0.2f, 0.2f, 0.2f, 1.0f, /* ambient */ -yes, 0.05f, 0.05f, 0.05f, 1.0f, /* diffuse */ -yes, 0.8f, 0.8f, 0.4f, 1.0f, /* specular */ -yes, 95.0f, /* shininess */ -yes, 0.0f, 0.0f, 0.0f, 0.0f, /* emission */ -no, /* alpha defined */ --1, -1 /* GL list numbers */ -}; - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -#if ! ENGINE -static FileReturnCode load_texture_file(PolyhedronStruct* ph, char filename[]); -static FileReturnCode load_texture_coordinates(PolyhedronStruct* ph, char filename[]); -#endif - - -#if ! ENGINE -void init_global_lighting() -{ - /* Initialize the global lighting parameters. The specific - * values of the light source parameters need to be set in - * each model window, not here. - */ - glShadeModel(GL_FLAT); - glEnable(GL_FRONT); -} -#endif - -void init_materials(ModelStruct* ms) -{ - if (ms != NULL) - { - int i; - ModelMaterials* mat = &ms->dis.mat; - - mat->num_materials = 0; - mat->material_array_size = DEFAULT_ARRAY_INCREMENT; - mat->materials = (MaterialStruct*)simm_malloc(mat->material_array_size * sizeof(MaterialStruct)); - - for (i = 0; i < mat->material_array_size; i++) - { - mat->materials[i].name = NULL; - mat->materials[i].defined_yet = no; - mat->materials[i].defined_in_file = no; - } - - mat->null_material = define_material(ms, &null_mat); - mat->default_bone_material = define_material(ms, &def_bone); - mat->default_world_object_material = define_material(ms, &def_world_object); - mat->default_muscle_min_material = define_material(ms, &def_min_muscle); - mat->default_muscle_max_material = define_material(ms, &def_max_muscle); - mat->def_muscle_point_material = define_material(ms, &def_muscle_point); - mat->sel_muscle_point_material = define_material(ms, &sel_muscle_point); - mat->bone_outline_material = define_material(ms, &bone_outline); - mat->highlighted1_polygon_material = define_material(ms, &highlighted1_polygon); - mat->highlighted2_polygon_material = define_material(ms, &highlighted2_polygon); - mat->marker_material = define_material(ms, &marker_mat); - mat->sel_marker_material = define_material(ms, &sel_marker_mat); - - mat->co_current_active_material = define_material(ms, &co_current_active); - mat->co_current_inactive_material = define_material(ms, &co_current_inactive); - mat->co_back_active_material = define_material(ms, &co_back_active); - mat->co_back_inactive_material = define_material(ms, &co_back_inactive); - - mat->cp_current_sel_ok_material = define_material(ms, &cp_sel_ok); /* */ - mat->cp_current_sel_broken_material = define_material(ms, &cp_sel_broken); /* */ - mat->cp_current_ok_material = define_material(ms, &cp_current_ok); /* */ - mat->cp_current_broken_material = define_material(ms, &cp_current_broken); /* */ - mat->cp_current_inactive_material = ms->dis.mat.co_current_inactive_material; - mat->cp_back_ok_material = ms->dis.mat.co_back_active_material; - mat->cp_back_broken_material = define_material(ms, &cp_back_broken); - mat->cp_back_inactive_material = ms->dis.mat.co_back_inactive_material; - - mat->masscenter_material1 = define_material(ms, &masscenter_mat1); - mat->masscenter_material2 = define_material(ms, &masscenter_mat2); - - define_material(ms, &arrow); - define_material(ms, &joint_vector); - define_material(ms, &foot_print_mat); - } - -} - -#if ! ENGINE -void init_model_lighting() -{ - /* These parameters need to be set in each model window */ - glLightfv(GL_LIGHT0, GL_DIFFUSE, light0_diffuse); - glLightfv(GL_LIGHT0, GL_POSITION, light0_position); - -#if INCLUDE_2ND_LIGHT_SOURCE || 0 - glLightfv(GL_LIGHT1, GL_DIFFUSE, light1_diffuse); - glLightfv(GL_LIGHT1, GL_POSITION, light1_position); -#endif - glLightModelfv(GL_LIGHT_MODEL_AMBIENT, light_model); -} - - -void make_mat_display_list(MaterialStruct* mat) -{ - - float polygonBackColor[4]; - -#if 0 /* this color is now specified on a per model basis */ - polygonBackColor[0] = root.color.cmap[MISC_MODEL_WINDOW_BACKGROUND].rgb[0]; - polygonBackColor[1] = root.color.cmap[MISC_MODEL_WINDOW_BACKGROUND].rgb[1]; - polygonBackColor[2] = root.color.cmap[MISC_MODEL_WINDOW_BACKGROUND].rgb[2]; - polygonBackColor[3] = 1.0; -#else - polygonBackColor[0] = 0.0; - polygonBackColor[1] = 0.0; - polygonBackColor[2] = 0.0; - polygonBackColor[3] = 1.0; -#endif - - if (mat->normal_list) - glDeleteLists(mat->normal_list, 1); - - mat->normal_list = glGenLists(1); - - if (mat->normal_list == 0) - { - fprintf(stderr, "Unable to allocate GL display list.\n"); - mat->normal_list = -1; - return; - } - - glNewList(mat->normal_list, GL_COMPILE); - - if (mat->diffuse[3] < 1.0) - { - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - } - else - glDisable(GL_BLEND); - - glMaterialfv(GL_BACK, GL_DIFFUSE, polygonBackColor); - - if (mat->ambient_defined) - glMaterialfv(GL_FRONT, GL_AMBIENT, mat->ambient); - else - glMaterialfv(GL_FRONT, GL_AMBIENT, null_mat.ambient); - - if (mat->diffuse_defined) - glMaterialfv(GL_FRONT, GL_DIFFUSE, mat->diffuse); - else - glMaterialfv(GL_FRONT, GL_DIFFUSE, null_mat.diffuse); - - if (mat->specular_defined) - glMaterialfv(GL_FRONT, GL_SPECULAR, mat->specular); - else - glMaterialfv(GL_FRONT, GL_SPECULAR, null_mat.specular); - - if (mat->emission_defined) - glMaterialfv(GL_FRONT, GL_EMISSION, mat->emission); - else - glMaterialfv(GL_FRONT, GL_EMISSION, null_mat.emission); - - if (mat->shininess_defined) - glMaterialf(GL_FRONT, GL_SHININESS, mat->shininess); - else - glMaterialf(GL_FRONT, GL_SHININESS, null_mat.shininess); - - glEndList(); - -} - - -void make_highlight_mat_display_list(MaterialStruct* mat) -{ - - GLfloat new_col[3], new_col2[3]; - float polygonBackColor[4]; - -#if 0 /* this color is now specified on a per model basis */ - polygonBackColor[0] = root.color.cmap[MISC_MODEL_WINDOW_BACKGROUND].rgb[0]; - polygonBackColor[1] = root.color.cmap[MISC_MODEL_WINDOW_BACKGROUND].rgb[1]; - polygonBackColor[2] = root.color.cmap[MISC_MODEL_WINDOW_BACKGROUND].rgb[2]; - polygonBackColor[3] = 1.0; -#else - polygonBackColor[0] = 0.0; - polygonBackColor[1] = 0.0; - polygonBackColor[2] = 0.0; - polygonBackColor[3] = 1.0; -#endif - - if (mat->highlighted_list) - glDeleteLists(mat->highlighted_list, 1); - - mat->highlighted_list = glGenLists(1); - - if (mat->highlighted_list == 0) - { - fprintf(stderr, "Unable to allocate GL display list.\n"); - return; - } - - glNewList(mat->highlighted_list, GL_COMPILE); - - if (mat->diffuse[3] < 1.0) - { - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - } - else - glDisable(GL_BLEND); - - glMaterialfv(GL_BACK, GL_DIFFUSE, polygonBackColor); - - make_highlight_color(mat->diffuse, new_col); - - if (mat->diffuse_defined) - glMaterialfv(GL_FRONT, GL_DIFFUSE, new_col); - - if (mat->ambient_defined) - { - make_ambient_color(new_col, new_col2); - glMaterialfv(GL_FRONT, GL_AMBIENT, new_col2); - } - - if (mat->specular_defined) - { - make_specular_color(new_col,new_col2); - glMaterialfv(GL_FRONT, GL_SPECULAR, new_col2); - } - - if (mat->emission_defined) - glMaterialfv(GL_FRONT, GL_EMISSION, mat->emission); - - if (mat->shininess_defined) - glMaterialf(GL_FRONT, GL_SHININESS, mat->shininess); - - glEndList(); - -} -#endif /* ENGINE */ - - -ReturnCode read_material(ModelStruct* ms, FILE* fp) -{ - int i, ac = 0; - MaterialStruct mat; - char name[CHARBUFFER]; - - if (fscanf(fp, "%s", name) != 1) - { - error(abort_action, "Error reading name in material definition"); - return code_bad; - } - - memset(&mat, 0, sizeof(MaterialStruct)); - - mat.defined_in_file = yes; - mat.shininess = 10.0; - /* the alpha component is stored in diffuse[3] */ - mat.diffuse[3] = 1.0; - - while (1) - { - if (read_string(fp,buffer) == EOF) - { - error(abort_action, "Unexpected EOF found reading material definition"); - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "endmaterial")) - { - mstrcpy(&mat.name,name); - if (define_material(ms, &mat) == -1) - { - FREE_IFNOTNULL(mat.name); - return code_bad; - } - break; - } - else if (STRINGS_ARE_EQUAL(buffer, "ambient")) - { - if (mat.ambient_defined == yes) - error(none, "Ignoring redefinition of ambient in material definition"); - else - { - if (fscanf(fp, "%f %f %f", &mat.ambient[0], &mat.ambient[1], - &mat.ambient[2]) != 3) - { - error(abort_action, "Error reading ambient in material definition"); - return code_bad; - } - mat.ambient[3] = 1.0; - ac += 3; - mat.ambient_defined = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "diffuse")) - { - if (mat.diffuse_defined == yes) - error(none, "Ignoring redefinition of diffuse in material definition"); - else - { - if (fscanf(fp, "%f %f %f", &mat.diffuse[0], &mat.diffuse[1], - &mat.diffuse[2]) != 3) - { - error(abort_action, "Error reading diffuse in material definition"); - return code_bad; - } - ac += 3; - mat.diffuse_defined = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "emission")) - { - if (mat.emission_defined == yes) - error(none, "Ignoring redefinition of emission in material definition"); - else - { - if (fscanf(fp, "%f %f %f", &mat.emission[0], &mat.emission[1], - &mat.emission[2]) != 3) - { - error(abort_action, "Error reading emission in material definition"); - return code_bad; - } - mat.emission[3] = 1.0; - ac += 3; - mat.emission_defined = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "specular")) - { - if (mat.specular_defined == yes) - error(none, "Ignoring redefinition of specular in material definition"); - else - { - if (fscanf(fp, "%f %f %f", &mat.specular[0], &mat.specular[1], - &mat.specular[2]) != 3) - { - error(abort_action, "Error reading specular in material definition"); - return code_bad; - } - mat.specular[3] = 1.0; - ac += 3; - mat.specular_defined = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "shininess")) - { - if (mat.shininess_defined == yes) - error(none, "Ignoring redefinition of shininess in material definition"); - else - { - if (fscanf(fp, "%f", &mat.shininess) != 1) - { - error(abort_action, "Error reading shininess in material definition"); - return code_bad; - } - ac++; - mat.shininess_defined = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "alpha")) - { - if (mat.alpha_defined == yes) - error(none, "Ignoring redefinition of alpha in material definition"); - else - { - if (fscanf(fp, "%f", &mat.diffuse[3]) != 1) - { - error(abort_action, "Error reading alpha in material definition"); - return code_bad; - } - ac++; - mat.alpha_defined = yes; - } - } - else - { - (void)sprintf(errorbuffer, "Unknown string (%s) in definition of material", buffer); - error(recover, errorbuffer); - } - } - - FREE_IFNOTNULL(mat.name); - - return code_fine; -} - - -public void copy_material(MaterialStruct* src, MaterialStruct* dst) -{ - int i; - - FREE_IFNOTNULL(dst->name); - - memcpy(dst, src, sizeof(MaterialStruct)); - - mstrcpy(&dst->name, src->name); - - dst->normal_list = 0; - dst->highlighted_list = 0; - dst->defined_yet = yes; -} - - -int define_material(ModelStruct* ms, MaterialStruct* mat) -{ - int m; - - if (ms == NULL) - return -1; - - m = enter_material(ms, mat->name, defining_element); - - /* If there was an error entering the material in the array, return the - * null material. - */ - - if (m == -1) - return ms->dis.mat.null_material; - - if (ms->dis.mat.materials[m].defined_in_file == yes) - { - (void)sprintf(errorbuffer,"Error: redefinition of material %s", mat->name); - error(abort_action,errorbuffer); - return -1; - } - - copy_material(mat, &ms->dis.mat.materials[m]); - - return m; -} - - -int enter_material(ModelStruct* ms, const char name[], EnterMode emode) -{ - int i, m; - ReturnCode rc = code_fine; - - if (ms == NULL) - return -1; - - for (i=0; idis.mat.num_materials; i++) - if (STRINGS_ARE_EQUAL(name,ms->dis.mat.materials[i].name)) - return i; - - m = i; - - /* If you are just checking to see if the name has already been defined (or declared), - * and you reach the end of the array without finding it, return an error. - */ - - if (m == ms->dis.mat.num_materials && emode == just_checking_element) - return -1; - - if (ms->dis.mat.num_materials >= ms->dis.mat.material_array_size) - { - ms->dis.mat.material_array_size += DEFAULT_ARRAY_INCREMENT; - ms->dis.mat.materials = (MaterialStruct*)simm_realloc(ms->dis.mat.materials, - ms->dis.mat.material_array_size*sizeof(MaterialStruct),&rc); - if (rc == code_bad) - { - ms->dis.mat.material_array_size -= DEFAULT_ARRAY_INCREMENT; - return ZERO; - } - for (i=ms->dis.mat.num_materials; idis.mat.material_array_size; i++) - { - ms->dis.mat.materials[i].name = NULL; - ms->dis.mat.materials[i].defined_yet = no; - ms->dis.mat.materials[i].defined_in_file = no; - } - } - mstrcpy(&ms->dis.mat.materials[m].name,name); - ms->dis.mat.num_materials++; - - return m; -} - -#if ! ENGINE -void apply_material(ModelStruct* ms, int mat, SBoolean highlight) -{ - - if (highlight == yes) - glCallList(ms->dis.mat.materials[mat].highlighted_list); - else - glCallList(ms->dis.mat.materials[mat].normal_list); - -} - -static FileReturnCode load_texture_file(PolyhedronStruct* ph, char filename[]) -{ - int width, height; - unsigned int* data; - - if (read_tiff_image(filename, &width, &height, &data) == file_missing) - { - ph->texture = -1; - return file_missing; - } - - // allocate a texture name - glGenTextures(1, &ph->texture); - - // select our current texture - glBindTexture(GL_TEXTURE_2D, ph->texture); - - // select modulate to mix texture with color for shading - glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); - - // when texture area is small, bilinear filter the closest mipmap - //glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_NEAREST); - // when texture area is large, bilinear filter the first mipmap - //glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - - // if wrap is true, the texture wraps over at the edges (repeat) - // ... false, the texture ends at the edges (clamp) - //glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrap ? GL_REPEAT : GL_CLAMP); - //glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrap ? GL_REPEAT : GL_CLAMP); - - // build our texture mipmaps - //gluBuild2DMipmaps(GL_TEXTURE_2D, 3, width, height, GL_RGB, GL_UNSIGNED_BYTE, data); - - glTexImage2D(GL_TEXTURE_2D, 0, 4, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, data); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - - // free buffer - _TIFFfree(data); - - return file_good; -} - -//TODO5.0: right now textures are supported only in world objects. -// This function must be called when the model's window is the current -// GL window. -//TODO5.0: what if the model is in two windows? -void load_model_textures(ModelStruct* model) -{ - int i; - - for (i=0; inumworldobjects; i++) - { - if (model->worldobj[i].texture_filename && model->worldobj[i].wobj) - { - FileReturnCode frc = lookup_texture_file(model->worldobj[i].wobj, model->worldobj[i].texture_filename, model); - - if (frc == file_missing) - { - (void)sprintf(errorbuffer, "Unable to locate texture file %s", model->worldobj[i].texture_filename); - error(none, errorbuffer); - model->worldobj[i].wobj->texture = -1; - } - else if (frc == file_bad) - { - (void)sprintf(errorbuffer, "Unable to read texture from file %s", model->worldobj[i].texture_filename); - error(none, errorbuffer); - model->worldobj[i].wobj->texture = -1; - } - } - } -} -/* ------------------------------------------------------------------------- - lookup_texture_file - this routine tries to read the specified texture file - by first checking the current directory, and then checking the - standard SIMM directory for bones. - TODO5.0: If one of the paths to search starts with a drive letter that - does not exist, Vista will display an annoying error dialog (other versions - of Windows quietly move on). Find some way to check for a valid drive - letter before performing the action that causes the error. ----------------------------------------------------------------------------- */ -FileReturnCode lookup_texture_file(PolyhedronStruct* ph, char filename[], ModelStruct* ms) -{ - int i; - char* jointpath = NULL; - char fullpath[CHARBUFFER], tmppath[CHARBUFFER]; - FileReturnCode rc; - - /* (0) strip the joint file name from ms->jointfilename to get just - * the path to the joint file. - */ - if (ms && ms->jointfilename) - { - get_pure_path_from_path(ms->jointfilename, &jointpath); - } - - /* (1) First check the bone folder specified in the joint file. - * If this is an absolute path, use it as is. If it is a relative - * path, it is assumed to be relative to the joint file's folder. - */ - if (ms && ms->bonepathname) - { - if (ms->jointfilename) - build_full_path(jointpath, ms->bonepathname, tmppath); - else - build_full_path(NULL, ms->bonepathname, tmppath); - build_full_path(tmppath, filename, fullpath); - rc = load_texture_file(ph, fullpath); - if (rc == file_good || rc == file_bad) - { - FREE_IFNOTNULL(jointpath); - return rc; - } - } - - /* (2) Next check the folder "bones" under the joint file's - * folder (PC only). - */ - if (ms->jointfilename) - strcat3(tmppath, jointpath, DIR_SEP_STRING, "bones", CHARBUFFER); - else - strcpy(tmppath, "bones"); - strcat3(fullpath, tmppath, DIR_SEP_STRING, filename, CHARBUFFER); - rc = load_texture_file(ph, fullpath); - if (rc == file_good || rc == file_bad) - { - FREE_IFNOTNULL(jointpath); - return rc; - } - - /* (3) Next check the joint file's folder itself (PC only). */ - if (ms->jointfilename) - strcpy(tmppath, jointpath); - else - strcpy(tmppath, "."); - FREE_IFNOTNULL(jointpath); - strcat3(fullpath, tmppath, DIR_SEP_STRING, filename, CHARBUFFER); - rc = load_texture_file(ph, fullpath); - if (rc == file_good || rc == file_bad) - return rc; - - /* (4) check the global bones folder. */ - build_full_path(get_bones_folder(), filename, fullpath); - rc = load_texture_file(ph, fullpath); - if (rc == file_good || rc == file_bad) - return rc; - - /* (5) check the mocap bones folder. */ - sprintf(fullpath, "%s%s%s%s", get_mocap_folder(), "bones", DIR_SEP_STRING, filename); - rc = load_texture_file(ph, fullpath); - if (rc == file_good || rc == file_bad) - return rc; - - /* You only make it to here if the file was not found in - * any of the folders. - */ - return file_missing; -} - - -static FileReturnCode load_texture_coordinates(PolyhedronStruct* ph, char filename[]) -{ - FileReturnCode frc = file_good; - FILE* fp = fopen(filename, "r"); - - if (fp == NULL) - { - frc = file_missing; - } - else - { - int i; - - for (i=0; inum_vertices; i++) - { - if (fscanf(fp, "%f %f", &ph->vertex[i].texture[0], &ph->vertex[i].texture[1]) != 2) - { - frc = file_bad; - fclose(fp); - return frc; - } - } - - fclose(fp); - } - - return frc; -} - - -/* ------------------------------------------------------------------------- - lookup_texture_coord_file - this routine tries to read the specified file - of texture coordinates by first checking the current directory, and then - checking the standard SIMM directory for bones. - TODO5.0: If one of the paths to search starts with a drive letter that - does not exist, Vista will display an annoying error dialog (other versions - of Windows quietly move on). Find some way to check for a valid drive - letter before performing the action that causes the error. ----------------------------------------------------------------------------- */ -FileReturnCode lookup_texture_coord_file(PolyhedronStruct* ph, char filename[], ModelStruct* ms) -{ - int i; - char* jointpath = NULL; - char fullpath[CHARBUFFER], tmppath[CHARBUFFER]; - FileReturnCode rc; - - /* (0) strip the joint file name from ms->jointfilename to get just - * the path to the joint file. - */ - if (ms && ms->jointfilename) - { - get_pure_path_from_path(ms->jointfilename, &jointpath); - } - - /* (1) First check the bone folder specified in the joint file. - * If this is an absolute path, use it as is. If it is a relative - * path, it is assumed to be relative to the joint file's folder. - */ - if (ms && ms->bonepathname) - { - if (ms->jointfilename) - build_full_path(jointpath, ms->bonepathname, tmppath); - else - build_full_path(NULL, ms->bonepathname, tmppath); - build_full_path(tmppath, filename, fullpath); - rc = load_texture_coordinates(ph, fullpath); - if (rc == file_good || rc == file_bad) - { - FREE_IFNOTNULL(jointpath); - return rc; - } - } - - /* (2) Next check the folder "bones" under the joint file's - * folder (PC only). - */ - if (ms->jointfilename) - strcat3(tmppath, jointpath, DIR_SEP_STRING, "bones", CHARBUFFER); - else - strcpy(tmppath, "bones"); - strcat3(fullpath, tmppath, DIR_SEP_STRING, filename, CHARBUFFER); - rc = load_texture_coordinates(ph, fullpath); - if (rc == file_good || rc == file_bad) - { - FREE_IFNOTNULL(jointpath); - return rc; - } - - /* (3) Next check the joint file's folder itself (PC only). */ - if (ms->jointfilename) - strcpy(tmppath, jointpath); - else - strcpy(tmppath, "."); - FREE_IFNOTNULL(jointpath); - strcat3(fullpath, tmppath, DIR_SEP_STRING, filename, CHARBUFFER); - rc = load_texture_coordinates(ph, fullpath); - if (rc == file_good || rc == file_bad) - return rc; - - /* (4) check the global bones folder. */ - build_full_path(get_bones_folder(), filename, fullpath); - rc = load_texture_coordinates(ph, fullpath); - if (rc == file_good || rc == file_bad) - return rc; - - /* (5) check the mocap bones folder. */ - sprintf(fullpath, "%s%s%s%s", get_mocap_folder(), "bones", DIR_SEP_STRING, filename); - rc = load_texture_coordinates(ph, fullpath); - if (rc == file_good || rc == file_bad) - return rc; - - /* You only make it to here if the file was not found in - * any of the folders. - */ - return file_missing; -} - -/* ------------------------------------------------------------------------- - read_tiff_image - read the specified TIFF file into memory as a 32-bit - RGBA image. - - NOTE: if 'image' is NULL, then the image is not actually read, only the - width and height of the image are returned. - - NOTE: it is the caller's responsibility to deallocated the image when it - finished with it. ----------------------------------------------------------------------------- */ -FileReturnCode read_tiff_image(const char* filename, int* width, int* height, unsigned** image) -{ - TIFF* tif; - FILE* fp = simm_fopen(filename, "r"); - - if (fp) - { - fclose(fp); - } - else - { - return file_missing; - } - - tif = TIFFOpen(filename, "r"); - - if (tif) - { - TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, width); - TIFFGetField(tif, TIFFTAG_IMAGELENGTH, height); - - if (image) - { - *image = (unsigned*) _TIFFmalloc(*width * *height * sizeof(unsigned)); - - if (*image) - TIFFReadRGBAImage(tif, *width, *height, (unsigned long*) *image, 0); - } - TIFFClose(tif); - } - - return file_good; -} - -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/mathtools.c b/OpenSim/Utilities/simmToOpenSim/mathtools.c deleted file mode 100644 index d16337073e..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/mathtools.c +++ /dev/null @@ -1,1705 +0,0 @@ -/******************************************************************************* - - MATHTOOLS.C - - Author: Peter Loan - - Date: 8-DEC-88 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines that implement various - mathematical tools and functions. Every one operates exclusively - on the formal parameter list, and so does not change any external - variables or other general model or plot structures. - - Routines: - get_array_length : gets the length of an integer array - mult_4x4matrix_by_vector : multiplies a vector by a matrix - mult_4x4matrices : multiplies two 4x4 matrices - transpose_4x4matrix : transposes a 4x4 matrix - cross_vectors : finds cros product of two vectors - normalize_vector : normalizes a vector - calc_function_coefficients : finds coefficients of a function - interpolate_function : given an x, interpolates a cubic to get a y value - format_double : determines appropriate printf format for a number - reset_4x4matrix : sets a 4x4 matrix equal to the identity matrix - make_4x4dircos_matrix : makes direction cosine matrix given angle & axis - -*******************************************************************************/ - -#include "universal.h" -#include "functions.h" - - -/*************** DEFINES (for this file only) *********************************/ -#define LINE_EPSILON 0.00001 - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void quat_to_axis_angle_rot(const Quat, dpCoord3D* axis, double* angle); -static void matrix_to_quat(double m[][4], Quat); -static void make_quaternion(Quat, const double axis[3], double angle); -static void quat_to_matrix(const Quat q, double m[][4]); -static void rotate_matrix_by_quat(double m[][4], const Quat); - - -/* GET_ARRAY_LENGTH: get the length of an array of ints. The end is - * marked by the END_OF_ARRAY constant. - */ -int get_array_length(int list[]) -{ - int i = 0; - - while (list[i] != END_OF_ARRAY) - i++; - - return i; -} - - -/* MULT_4X4MATRIX_BY_VECTOR: this routine premultiplies a 4x4 matrix by - * a 1x4 vector. That is, it does vector*mat, not mat*vector. - */ - -void mult_4x4matrix_by_vector(double mat[][4], double vector[], double result[]) -{ - - result[0] = vector[0]*mat[0][0] + vector[1]*mat[1][0] + - vector[2]*mat[2][0] + vector[3]*mat[3][0]; - result[1] = vector[0]*mat[0][1] + vector[1]*mat[1][1] + - vector[2]*mat[2][1] + vector[3]*mat[3][1]; - result[2] = vector[0]*mat[0][2] + vector[1]*mat[1][2] + - vector[2]*mat[2][2] + vector[3]*mat[3][2]; - result[3] = vector[0]*mat[0][3] + vector[1]*mat[1][3] + - vector[2]*mat[2][3] + vector[3]*mat[3][3]; - -} - - - -/* MULT_4X4MATRICES: this routine multiplies the two specified 4x4 matrices - * in the order: mat1*mat2. - */ - -void mult_4x4matrices(double mat1[][4], double mat2[][4], double result[][4]) -{ - - result[0][0] = mat1[0][0]*mat2[0][0] + mat1[0][1]*mat2[1][0] + - mat1[0][2]*mat2[2][0] + mat1[0][3]*mat2[3][0]; - - result[0][1] = mat1[0][0]*mat2[0][1] + mat1[0][1]*mat2[1][1] + - mat1[0][2]*mat2[2][1] + mat1[0][3]*mat2[3][1]; - - result[0][2] = mat1[0][0]*mat2[0][2] + mat1[0][1]*mat2[1][2] + - mat1[0][2]*mat2[2][2] + mat1[0][3]*mat2[3][2]; - - result[0][3] = mat1[0][0]*mat2[0][3] + mat1[0][1]*mat2[1][3] + - mat1[0][2]*mat2[2][3] + mat1[0][3]*mat2[3][3]; - - - result[1][0] = mat1[1][0]*mat2[0][0] + mat1[1][1]*mat2[1][0] + - mat1[1][2]*mat2[2][0] + mat1[1][3]*mat2[3][0]; - - result[1][1] = mat1[1][0]*mat2[0][1] + mat1[1][1]*mat2[1][1] + - mat1[1][2]*mat2[2][1] + mat1[1][3]*mat2[3][1]; - - result[1][2] = mat1[1][0]*mat2[0][2] + mat1[1][1]*mat2[1][2] + - mat1[1][2]*mat2[2][2] + mat1[1][3]*mat2[3][2]; - - result[1][3] = mat1[1][0]*mat2[0][3] + mat1[1][1]*mat2[1][3] + - mat1[1][2]*mat2[2][3] + mat1[1][3]*mat2[3][3]; - - - result[2][0] = mat1[2][0]*mat2[0][0] + mat1[2][1]*mat2[1][0] + - mat1[2][2]*mat2[2][0] + mat1[2][3]*mat2[3][0]; - - result[2][1] = mat1[2][0]*mat2[0][1] + mat1[2][1]*mat2[1][1] + - mat1[2][2]*mat2[2][1] + mat1[2][3]*mat2[3][1]; - - result[2][2] = mat1[2][0]*mat2[0][2] + mat1[2][1]*mat2[1][2] + - mat1[2][2]*mat2[2][2] + mat1[2][3]*mat2[3][2]; - - result[2][3] = mat1[2][0]*mat2[0][3] + mat1[2][1]*mat2[1][3] + - mat1[2][2]*mat2[2][3] + mat1[2][3]*mat2[3][3]; - - - result[3][0] = mat1[3][0]*mat2[0][0] + mat1[3][1]*mat2[1][0] + - mat1[3][2]*mat2[2][0] + mat1[3][3]*mat2[3][0]; - - result[3][1] = mat1[3][0]*mat2[0][1] + mat1[3][1]*mat2[1][1] + - mat1[3][2]*mat2[2][1] + mat1[3][3]*mat2[3][1]; - - result[3][2] = mat1[3][0]*mat2[0][2] + mat1[3][1]*mat2[1][2] + - mat1[3][2]*mat2[2][2] + mat1[3][3]*mat2[3][2]; - - result[3][3] = mat1[3][0]*mat2[0][3] + mat1[3][1]*mat2[1][3] + - mat1[3][2]*mat2[2][3] + mat1[3][3]*mat2[3][3]; - -} - - -/* APPEND_4X4MATRIX: multiplies mat1 by mat2 and stores the - * result in mat1. - */ - -void append_4x4matrix(double mat1[][4], double mat2[][4]) -{ - - int i, j; - double mat[4][4]; - - mult_4x4matrices(mat1,mat2,mat); - - for (i=0; i<4; i++) - for (j=0; j<4; j++) - mat1[i][j] = mat[i][j]; - -} - - -/* TRANSPOSE_4X4MATRIX: this routine transposes a 4x4 matrix */ - -void transpose_4x4matrix(double mat[][4], double mat_transpose[][4]) -{ - - int i, j; - - for (i=0; i<4; i++) - for (j=0; j<4; j++) - mat_transpose[j][i] = mat[i][j]; - -} - - -/* MAKE_TRANSLATION_MATRIX: makes a 4x4 matrix with either an X, Y, or Z - * translation. - */ - -void make_translation_matrix(double mat[][4], int axis, double trans) -{ - - reset_4x4matrix(mat); - - mat[3][axis] = trans; - -} - - -/* MAKE_SCALE_MATRIX: makes a 4x4 matrix with either an X, Y, or Z - * scale. - */ - -void make_scale_matrix(double mat[][4], int axis, double scale) -{ - - reset_4x4matrix(mat); - - mat[axis][axis] = scale; - -} - - -/* CROSS_VECTORS: computes the cross product of two 1x3 vectors */ - -void cross_vectors(double vector1[], double vector2[], double result[]) -{ - - result[0] = vector1[1]*vector2[2] - vector1[2]*vector2[1]; - result[1] = vector1[2]*vector2[0] - vector1[0]*vector2[2]; - result[2] = vector1[0]*vector2[1] - vector1[1]*vector2[0]; - -} - - -/* NORMALIZE_VECTOR: normalizes a 1x3 vector. */ -double normalize_vector(double vector[], double norm_vector[]) -{ - double magnitude; - - magnitude = VECTOR_MAGNITUDE(vector); - - if (magnitude < TINY_NUMBER) - { - norm_vector[0] = vector[0]; - norm_vector[1] = vector[1]; - norm_vector[2] = vector[2]; - } - else - { - norm_vector[0] = vector[0]/magnitude; - norm_vector[1] = vector[1]/magnitude; - norm_vector[2] = vector[2]/magnitude; - } - - return magnitude; -} - - -/* NORMALIZE_VECTOR: normalizes a 1x3 vector. */ -double normalize_vector_f(float vector[], float norm_vector[]) -{ - float magnitude; - - magnitude = VECTOR_MAGNITUDE(vector); - - if (magnitude < TINY_NUMBER) - { - norm_vector[0] = vector[0]; - norm_vector[1] = vector[1]; - norm_vector[2] = vector[2]; - } - else - { - norm_vector[0] = vector[0]/magnitude; - norm_vector[1] = vector[1]/magnitude; - norm_vector[2] = vector[2]/magnitude; - } - - return magnitude; -} - - -/* FORMAT_DOUBLE: this routine finds a good format (printf style) for a given - * number by checking its magnitude. For example, if the number is greater than - * 50, you don't really need to show any decimal places, but if the number is - * less than 0.01, you should show several decimal places. - */ - -void format_double(double number, char format[]) -{ - - if (number > 50.0) - (void)strcpy(format,"%.3lf"); - else if (number > 10.0) - (void)strcpy(format,"%.4lf"); - else if (number > 1.0) - (void)strcpy(format,"%.5lf"); - else if (number > 0.1) - (void)strcpy(format,"%.6lf"); - else if (number > 0.01) - (void)strcpy(format,"%.7lf"); - else - (void)strcpy(format,"%.8lf"); - -} - - -/* RESET_4X4MATRIX: this routine sets a 4x4 matrix equal to the identity matrix */ - -void reset_4x4matrix(double mat[][4]) -{ - - mat[0][1] = mat[0][2] = mat[0][3] = 0.0; - mat[1][0] = mat[1][2] = mat[1][3] = 0.0; - mat[2][0] = mat[2][1] = mat[2][3] = 0.0; - mat[3][0] = mat[3][1] = mat[3][2] = 0.0; - - mat[0][0] = mat[1][1] = mat[2][2] = mat[3][3] = 1.0; - -} - - -/* MAKE_4X4DIRCOS_MATRIX: this routine creates a 4x4 direction cosine matrix - * given an arbitrarily oriented axis and a rotation angle in degrees. - */ - -void make_4x4dircos_matrix(double angle, double axis[], double mat[][4]) -{ - - double rad_angle, cl, sl, omc; - - reset_4x4matrix(mat); - normalize_vector(axis,axis); - - rad_angle = angle*DTOR; - - cl = cos(rad_angle); - sl = sin(rad_angle); - omc = 1.0 - cl; - - /* the following matrix is taken from Kane's 'Spacecraft Dynamics,' pp 6-7 */ - - mat[0][0] = cl + axis[XX]*axis[XX]*omc; - mat[1][0] = -axis[ZZ]*sl + axis[XX]*axis[YY]*omc; - mat[2][0] = axis[YY]*sl + axis[ZZ]*axis[XX]*omc; - mat[0][1] = axis[ZZ]*sl + axis[XX]*axis[YY]*omc; - mat[1][1] = cl + axis[YY]*axis[YY]*omc; - mat[2][1] = -axis[XX]*sl + axis[YY]*axis[ZZ]*omc; - mat[0][2] = -axis[YY]*sl + axis[ZZ]*axis[XX]*omc; - mat[1][2] = axis[XX]*sl + axis[YY]*axis[ZZ]*omc; - mat[2][2] = cl + axis[ZZ]*axis[ZZ]*omc; - -} - - -/* VECTOR_INTERSECTS_POLYHEDRON: This function checks for intersection of a vector - * and a polyhedron. - */ -SBoolean vector_intersects_polyhedron(double pt[], double vec[], PolyhedronStruct* ph, double inter[]) -{ - int i; - double pt2[3], t; - - pt2[0] = pt[0] + vec[0]; - pt2[1] = pt[1] + vec[1]; - pt2[2] = pt[2] + vec[2]; - - /* Check to see if the vector intersects any of the polygons in the polyhedron. - * intersect_line_plane() is used to get the point of intersection between the - * vector and the plane of the polygon, and then this point is checked to see if - * it is inside the boundaries of the polygon. Return the first intersection found. - */ - for (i = 0; i < ph->num_polygons; i++) - { - if (intersect_line_plane(pt, pt2, ph->polygon[i].normal, ph->polygon[i].d, inter, &t) == yes && - point_in_polygon3D(inter, ph, i) == yes) - { - return yes; - } - } - - return no; -} - - -/* intersection between a line (pt1,pt2) and plane (plane,d) - * will give the intersection point(inter) and where on the line - * it is (t). Ratio t should does not have to lie within 0 and 1 - */ -SBoolean intersect_line_plane(double pt1[], double pt2[], - double plane[], double d, double inter[], double* t) -{ - double dotprod; - double vec[3]; - - MAKE_3DVECTOR(pt1,pt2,vec); - dotprod = DOT_VECTORS(vec,plane); - - if (DABS(dotprod) < LINE_EPSILON) - return no; - - *t = (-d - plane[0]*pt1[0] - plane[1]*pt1[1] - plane[2]*pt1[2])/dotprod; - - inter[0] = pt1[0] + ((*t) * vec[0]); - inter[1] = pt1[1] + ((*t) * vec[1]); - inter[2] = pt1[2] + ((*t) * vec[2]); - - return yes; -} - -/* intersection between a line (pt1,pt2) and plane (plane,d) -** will give the intersection point (inter) and where on the line it is (t) -** ratio t should lie within 0 and 1 -*/ -SBoolean intersect_line_plane01(double pt1[], double pt2[], - double plane[], double d, double inter[], double* t) -{ - double dotprod; - double vec[3]; - - MAKE_3DVECTOR(pt1,pt2,vec); - dotprod = DOT_VECTORS(vec,plane); - - if (DABS(dotprod) < LINE_EPSILON) - return no; - - *t = (-d - plane[0]*pt1[0] - plane[1]*pt1[1] - plane[2]*pt1[2])/dotprod; - - if ((*t < -LINE_EPSILON) || (*t > 1.0 + LINE_EPSILON)) - return no; - - inter[0] = pt1[0] + ((*t) * vec[0]); - inter[1] = pt1[1] + ((*t) * vec[1]); - inter[2] = pt1[2] + ((*t) * vec[2]); - - return yes; -} - -/* POINT_IN_POLYGON3D: determines if a 3D point is inside a 3D polygon. The polygon - * is the first one in the PolyhedronStruct, and is assumed to be planar. - */ - -SBoolean point_in_polygon3D(double pt[], PolyhedronStruct* ph, int polygon_index) -{ - int i, j, index, skip_coord; - double max_value, polygon[50][2], pt2D[2]; - - /* if the polygon has more than 50 vertices, abort. */ - if (ph->polygon[polygon_index].num_vertices > 50) - return no; - - /* Project the polygon onto one of the major planes by ignoring one of the - * coordinates. Ignore the one most parallel to the plane's normal so that - * the area of the projected polygon will be as large as possible. - */ - skip_coord = XX; - max_value = DABS(ph->polygon[polygon_index].normal[XX]); - if (DABS(ph->polygon[polygon_index].normal[YY]) > max_value) - { - skip_coord = YY; - max_value = DABS(ph->polygon[polygon_index].normal[YY]); - } - if (DABS(ph->polygon[polygon_index].normal[ZZ]) > max_value) - skip_coord = ZZ; - - /* Copy the vertices into a 2D array, skipping the skip_coord */ - for (i = 0; i < ph->polygon[polygon_index].num_vertices; i++) - { - for (j = 0, index = 0; j < 3; j++) - { - if (j != skip_coord) - polygon[i][index++] = ph->vertex[ph->polygon[polygon_index].vertex_index[i]].coord[j]; - } - } - - /* Copy the 3D point into a 2D point, skipping the skip_coord */ - for (i = 0, index = 0; i < 3; i++) - { - if (i != skip_coord) - pt2D[index++] = pt[i]; - } - - if (point_in_polygon2D(pt2D, polygon, ph->polygon[polygon_index].num_vertices)) - return yes; - - return no; -} - - -/* POINT_IN_POLYGON2D: determines if a 2D point is inside a 2D polygon. Uses the - * "crossings test" taken from: - * Haines, Eric, "Point in Polygon Strategies," Graphics Gems IV, ed. Paul Heckbert, - * Academic Press, p. 24-46, 1994. - * Shoot a test ray along +X axis. The strategy, from MacMartin, is to - * compare vertex Y values to the testing point's Y and quickly discard - * edges which are entirely to one side of the test ray. - * - * Input 2D polygon _pgon_ with _numverts_ number of vertices and test point - * _point_, returns 1 if inside, 0 if outside. - */ - -int point_in_polygon2D(double point[], double pgon[][2], int numverts) -{ - register int j, yflag0, yflag1, inside_flag, xflag0 ; - register double ty, tx, *vtx0, *vtx1 ; - - tx = point[XX]; - ty = point[YY]; - - vtx0 = pgon[numverts-1]; - - /* get test bit for above/below X axis */ - yflag0 = ( vtx0[YY] >= ty ); - vtx1 = pgon[0]; - - inside_flag = 0; - - for (j = numverts+1; --j ;) - { - yflag1 = ( vtx1[YY] >= ty ); - /* check if endpoints straddle (are on opposite sides) of X axis - * (i.e. the Y's differ); if so, +X ray could intersect this edge. - */ - if (yflag0 != yflag1) - { - xflag0 = (vtx0[XX] >= tx); - /* check if endpoints are on same side of the Y axis (i.e. X's - * are the same); if so, it's easy to test if edge hits or misses. - */ - if ( xflag0 == ( vtx1[XX] >= tx ) ) - { - /* if edge's X values both right of the point, must hit */ - if ( xflag0 ) - inside_flag = !inside_flag; - } - else - { - /* compute intersection of pgon segment with +X ray, note - * if >= point's X; if so, the ray hits it. - */ - if ((vtx1[XX] - (vtx1[YY]-ty) * (vtx0[XX]-vtx1[XX])/(vtx0[YY]-vtx1[YY])) >= tx) - { - inside_flag = !inside_flag; - } - } - } - - /* move to next pair of vertices, retaining info as possible */ - yflag0 = yflag1; - vtx0 = vtx1; - vtx1 += 2; - } - - return inside_flag; -} - -int polygon_ray_inter3d(PolyhedronStruct* newph, int poly_index, double pt[3], int axes) -{ - - int i,j, num_inter, numpts; - double **poly_pts, temppt[3], rotpt[3]; - PolygonStruct* poly; - VertexStruct* v1; - double **mat; - double amat[3][3]; - - poly = &newph->polygon[poly_index]; - numpts = poly->num_vertices; - poly_pts = (double**)simm_malloc(numpts*sizeof(double*)); - for (i=0; ivertex[poly->vertex_index[i]]; - poly_pts[i][0] = v1->coord[0]; - poly_pts[i][1] = v1->coord[1]; - poly_pts[i][2] = v1->coord[2]; - } - - mat = (double**)simm_malloc(4*sizeof(double*)); - mat[0] = (double*)simm_malloc(3*sizeof(double)); - mat[1] = (double*)simm_malloc(3*sizeof(double)); - mat[2] = (double*)simm_malloc(3*sizeof(double)); - make_rotation_matrix(mat,poly->normal); - - for (i=0; i<3; i++) - for (j=0; j<3; j++) - amat[i][j] = mat[i][j]; - - for (i=0; i ptray[axes] && NOT_EQUAL_WITHIN_TOLERANCE(intermin,ptray[axes],LINE_EPSILON)) - numinter++; - } - algstate = newstate; - } - - return (numinter); - -} - -/* returns the relation between a ray and a given point -** returns 0,1,2 -** 0= on the ray; 1= above ray, 2= below ray -** the pt is either on, above or below the ray -*/ -int point_ray_relation(double* pt, double ptray[], int axes) -{ - - if (EQUAL_WITHIN_TOLERANCE(ptray[axes],pt[axes],LINE_EPSILON)) - return (ON_RAY); - else if (ptray[axes] < pt[axes]) - return (ABOVE_RAY); - else - return (BELOW_RAY); - -} - -/***************************************************************************** -** DESCRIPTION: -** -** prepare a transformation martix to rotate such that Dir is -** parallel to the Z axes. Used to rotate the -** polygons to be XY plane parallel. -** -** Algorithm: form a 4 by 4 matrix from Dir as follows: -** | tx ty tz 0 | A transformation which takes the coord -** | bx by bz 0 | system into t, b & n as required. -** [X Y Z 1] * | nx ny nz 0 | -** | 0 0 0 1 | -** N is exactly Dir, but we got freedom on T & B which must be on -** a plane perpendicular to N and perpendicular between them but thats all! -** T is therefore selected using this (heuristic ?) algorithm: -** Let P be the axis of which the absolute N coefficient is the smallest. -** Let B be (N cross P) and T be (B cross N). -** -** PARAMETERS: -** mat: To place the constructed homogeneous transformation. -** dir: To derive a transformation such that Dir goes to Z axis. -** -*/ -void make_rotation_matrix(double** mat, double normal[]) -{ - int i,j; - double r; - double dir_n[3], t[3],b[3],p[3]; - - COPY_1X3VECTOR(normal,dir_n); - normalize_vector(dir_n,dir_n); - p[0] = 0.0; - p[1] = 0.0; - p[2] = 0.0; - - j=0; - r = DABS(dir_n[0]); - for(i=1; i<3; i++) - { - if( r > DABS(dir_n[i])) - { - r = dir_n[i]; - j = i; - } - } - p[j] = 1.0; /* set p to the axis with the biggest angle from dirn */ - - cross_vectors(dir_n,p,b); /* calc the bi-normal*/ - normalize_vector(b,b); - cross_vectors(b,dir_n,t); /* calc the tangent */ - normalize_vector(t,t); - - for(i=0; i<3; i++) - { - mat[i][0] = t[i]; - mat[i][1] = b[i]; - mat[i][2] = dir_n[i]; - } - -} - -int intersect_lines(double p1[], double p2[], double p3[], double p4[], - double p_int1[], double* t, double p_int2[], double* s) -{ - - double mag1, mag2, cross_prod[3], denom, vec1[3], vec2[3], mat[3][3]; - - vec1[0] = p2[0] - p1[0]; - vec1[1] = p2[1] - p1[1]; - vec1[2] = p2[2] - p1[2]; - mag1 = normalize_vector(vec1,vec1); - - vec2[0] = p4[0] - p3[0]; - vec2[1] = p4[1] - p3[1]; - vec2[2] = p4[2] - p3[2]; - mag2 = normalize_vector(vec2,vec2); - - cross_vectors(vec1,vec2,cross_prod); - - denom = cross_prod[0]*cross_prod[0] + cross_prod[1]*cross_prod[1] - + cross_prod[2]*cross_prod[2]; - - if (EQUAL_WITHIN_ERROR(denom,0.0)) - { - *s = *t = MINMDOUBLE; - return (0); - } - - mat[0][0] = p3[0] - p1[0]; - mat[0][1] = p3[1] - p1[1]; - mat[0][2] = p3[2] - p1[2]; - mat[1][0] = vec1[0]; - mat[1][1] = vec1[1]; - mat[1][2] = vec1[2]; - mat[2][0] = cross_prod[0]; - mat[2][1] = cross_prod[1]; - mat[2][2] = cross_prod[2]; - - *s = CALC_DETERMINANT(mat) / denom; - - p_int2[0] = p3[0] + (*s) * (vec2[0]); - p_int2[1] = p3[1] + (*s) * (vec2[1]); - p_int2[2] = p3[2] + (*s) * (vec2[2]); - - mat[1][0] = vec2[0]; - mat[1][1] = vec2[1]; - mat[1][2] = vec2[2]; - - *t = CALC_DETERMINANT(mat) / denom; - - p_int1[0] = p1[0] + (*t) * (vec1[0]); - p_int1[1] = p1[1] + (*t) * (vec1[1]); - p_int1[2] = p1[2] + (*t) * (vec1[2]); - - (*s) /= mag2; - (*t) /= mag1; - - return (1); - -} - -/* Same as intersect_lines(), but this routine does not scale the S and T - * parameters to be between 0.0 and 1.0. S ranges from 0.0 to mag2, - * and T ranges from 0.0 to mag1. - */ - -int intersect_lines_scaled(double p1[], double p2[], double p3[], double p4[], - double p_int1[], double* t, double* mag1, - double p_int2[], double* s, double* mag2) -{ - - double cross_prod[3], denom, vec1[3], vec2[3], mat[3][3]; - - vec1[0] = p2[0] - p1[0]; - vec1[1] = p2[1] - p1[1]; - vec1[2] = p2[2] - p1[2]; - *mag1 = normalize_vector(vec1,vec1); - - vec2[0] = p4[0] - p3[0]; - vec2[1] = p4[1] - p3[1]; - vec2[2] = p4[2] - p3[2]; - *mag2 = normalize_vector(vec2,vec2); - - cross_vectors(vec1,vec2,cross_prod); - - denom = cross_prod[0]*cross_prod[0] + cross_prod[1]*cross_prod[1] - + cross_prod[2]*cross_prod[2]; - - if (EQUAL_WITHIN_ERROR(denom,0.0)) - { - *s = *t = MINMDOUBLE; - return (0); - } - - mat[0][0] = p3[0] - p1[0]; - mat[0][1] = p3[1] - p1[1]; - mat[0][2] = p3[2] - p1[2]; - mat[1][0] = vec1[0]; - mat[1][1] = vec1[1]; - mat[1][2] = vec1[2]; - mat[2][0] = cross_prod[0]; - mat[2][1] = cross_prod[1]; - mat[2][2] = cross_prod[2]; - - *s = CALC_DETERMINANT(mat) / denom; - - p_int2[0] = p3[0] + (*s) * (vec2[0]); - p_int2[1] = p3[1] + (*s) * (vec2[1]); - p_int2[2] = p3[2] + (*s) * (vec2[2]); - - mat[1][0] = vec2[0]; - mat[1][1] = vec2[1]; - mat[1][2] = vec2[2]; - - *t = CALC_DETERMINANT(mat) / denom; - - p_int1[0] = p1[0] + (*t) * (vec1[0]); - p_int1[1] = p1[1] + (*t) * (vec1[1]); - p_int1[2] = p1[2] + (*t) * (vec1[2]); - - return (1); - -} - - -/* MULT_3X3MATRIX_BY_VECTOR: this routine premultiplies a 3x3 matrix by -** a 1x3 vector. That is, it does vector*mat, not mat*vector. -*/ -void mult_3x3matrix_by_vector(double mat[][3], double vector[], double result[]) -{ - - result[0] = vector[0]*mat[0][0] + vector[1]*mat[1][0] + - vector[2]*mat[2][0]; - result[1] = vector[0]*mat[0][1] + vector[1]*mat[1][1] + - vector[2]*mat[2][1]; - result[2] = vector[0]*mat[0][2] + vector[1]*mat[1][2] + - vector[2]*mat[2][2]; - -} - - -void make_3x3dircos_matrix(double angle, double axis[], double mat[][3]) -{ - - double rad_angle, cl, sl, omc; - - reset_3x3matrix(mat); - - rad_angle = angle*DTOR; - - cl = cos(rad_angle); - sl = sin(rad_angle); - omc = 1.0 - cl; - - /* the following matrix is taken from Kane's 'Spacecraft Dynamics,' pp 6-7 */ - - mat[0][0] = cl + axis[0]*axis[0]*omc; - mat[1][0] = -axis[2]*sl + axis[0]*axis[1]*omc; - mat[2][0] = axis[1]*sl + axis[2]*axis[0]*omc; - mat[0][1] = axis[2]*sl + axis[0]*axis[1]*omc; - mat[1][1] = cl + axis[1]*axis[1]*omc; - mat[2][1] = -axis[0]*sl + axis[1]*axis[2]*omc; - mat[0][2] = -axis[1]*sl + axis[2]*axis[0]*omc; - mat[1][2] = axis[0]*sl + axis[1]*axis[2]*omc; - mat[2][2] = cl + axis[2]*axis[2]*omc; - -} - - -void reset_3x3matrix(double matrix[][3]) -{ - - matrix[0][1] = matrix[0][2] = matrix[1][2] = 0.0; - matrix[1][0] = matrix[2][0] = matrix[2][1] = 0.0; - matrix[0][0] = matrix[1][1] = matrix[2][2] = 1.0; - -} - - -void clear_vector(double a[], int n) -{ - - int i; - - for (i=0; ia = pt3[XX]; - plane->b = pt3[YY]; - plane->c = pt3[ZZ]; - - /* d is the distance along the plane's normal vector from the plane to the - * origin. Thus it is negative when the plane is above the origin, and - * positive when the plane is below the origin. - */ - - plane->d = -plane->a*pt1[XX] - plane->b*pt1[YY] - plane->c*pt1[ZZ]; - -} - - -double compute_angle_between_vectors(double vector1[], double vector2[]) -{ - - double normal_vector1[3], normal_vector2[3]; - - normalize_vector(vector1,normal_vector1); - normalize_vector(vector2,normal_vector2); - - return (acos(DOT_VECTORS(normal_vector1,normal_vector2))); - -} - - - -void project_point_onto_plane(double pt[], PlaneStruct* plane, double projpt[]) -{ - - double plane_normal[3]; - double distance_to_plane; - - plane_normal[XX] = plane->a; - plane_normal[YY] = plane->b; - plane_normal[ZZ] = plane->c; - - normalize_vector(plane_normal,plane_normal); - - /* distance_to_plane is the distance from pt to the plane along the - * plane's normal vector. Thus it is negative when pt is below - * the plane, and positive when pt is above the plane. - */ - - distance_to_plane = DOT_VECTORS(pt,plane_normal) + plane->d; - - projpt[XX] = pt[XX] - distance_to_plane*plane_normal[XX]; - projpt[YY] = pt[YY] - distance_to_plane*plane_normal[YY]; - projpt[ZZ] = pt[ZZ] - distance_to_plane*plane_normal[ZZ]; - -} - - - -double distancesqr_between_vertices(double vertex1[], double vertex2[]) -{ - - double vec[3]; - - vec[0] = vertex2[0] - vertex1[0]; - vec[1] = vertex2[1] - vertex1[1]; - vec[2] = vertex2[2] - vertex1[2]; - - return vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]; - -} - - -double distance_between_vertices(double vertex1[], double vertex2[]) -{ - - double vec[3]; - - vec[0] = vertex2[0] - vertex1[0]; - vec[1] = vertex2[1] - vertex1[1]; - vec[2] = vertex2[2] - vertex1[2]; - - return (sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2])); - -} - - -/* Calculates the square of the shortest distance from a point (point) - * to a line (vl, through pl). - */ -double get_distsqr_point_line(double point[], double pl[], double vl[]) -{ - - double ptemp[3]; - - /* find the closest point on line */ - get_point_from_point_line(point,pl,vl,ptemp); - - return distancesqr_between_vertices(point,ptemp); - -} - - -/* to calculate the closest 3d point to given 3d line. - * the line is defined as vector(vec) and a point(pt) - * the line has not been normalized to a unit vector - * the value hypo*(cosalpha) of a rt triangle is found out - * to get the closest point - */ -void get_point_from_point_line(double point[], double pt[], double vec[], - double closest_pt[]) -{ - - double v1[3], v2[3]; - double t, mag; - - v1[0] = point[0] - pt[0]; - v1[1] = point[1] - pt[1]; - v1[2] = point[2] - pt[2]; - - v2[0] = vec[0]; - v2[1] = vec[1]; - v2[2] = vec[2]; - - mag = normalize_vector(v1,v1); - normalize_vector(v2,v2); - t = DOT_VECTORS(v1,v2) * mag; - - closest_pt[0] = pt[0] + t * v2[0]; - closest_pt[1] = pt[1] + t * v2[1]; - closest_pt[2] = pt[2] + t * v2[2]; - -} - - -/* ------------------------------------------------------------------------ - atan3 - like the Standard C Library's atan2(), except returning a result - between 0 and 2pi instead of -pi and pi. ---------------------------------------------------------------------------- */ -double atan3 (double y, double x) -{ - double result = atan2(y, x); - - if (result < 0.0) - result += 2 * M_PI; - - return result; -} - -void identity_matrix(double m[][4]) -{ - /* set matrix 'm' to the identity matrix */ - - m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = 0.0; - m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; m[1][3] = 0.0; - m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; m[2][3] = 0.0; - m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0; -} - -void x_rotate_matrix_bodyfixed(double m[][4], double radians) -{ - /* append rotation about local x-axis to matrix 'm' */ - Quat q; - - make_quaternion(q, m[0], radians); - rotate_matrix_by_quat(m, q); -} - -void y_rotate_matrix_bodyfixed(double m[][4], double radians) -{ - /* append rotation about local y-axis to matrix 'm' */ - Quat q; - - make_quaternion(q, m[1], radians); - rotate_matrix_by_quat(m, q); -} - -void z_rotate_matrix_bodyfixed(double m[][4], double radians) -{ - /* append rotation about local z-axis to matrix 'm' */ - Quat q; - - make_quaternion(q, m[2], radians); - rotate_matrix_by_quat(m, q); -} - -void x_rotate_matrix_spacefixed(double m[][4], double radians) -{ - /* append rotation about global x-axis to matrix 'm' */ - double sinTheta = sin(radians); - double cosTheta = cos(radians); - - double t = m[0][1]; - m[0][1] = t * cosTheta - m[0][2] * sinTheta; - m[0][2] = t * sinTheta + m[0][2] * cosTheta; - - t = m[1][1]; - m[1][1] = t * cosTheta - m[1][2] * sinTheta; - m[1][2] = t * sinTheta + m[1][2] * cosTheta; - - t = m[2][1]; - m[2][1] = t * cosTheta - m[2][2] * sinTheta; - m[2][2] = t * sinTheta + m[2][2] * cosTheta; - - t = m[3][1]; - m[3][1] = t * cosTheta - m[3][2] * sinTheta; - m[3][2] = t * sinTheta + m[3][2] * cosTheta; -} - -void y_rotate_matrix_spacefixed(double m[][4], double radians) -{ - /* append rotation about global y-axis to matrix 'm' */ - double sinTheta = sin(radians); - double cosTheta = cos(radians); - - double t = m[0][0]; - m[0][0] = t * cosTheta + m[0][2] * sinTheta; - m[0][2] = m[0][2] * cosTheta - t * sinTheta; - - t = m[1][0]; - m[1][0] = t * cosTheta + m[1][2] * sinTheta; - m[1][2] = m[1][2] * cosTheta - t * sinTheta; - - t = m[2][0]; - m[2][0] = t * cosTheta + m[2][2] * sinTheta; - m[2][2] = m[2][2] * cosTheta - t * sinTheta; - - t = m[3][0]; - m[3][0] = t * cosTheta + m[3][2] * sinTheta; - m[3][2] = m[3][2] * cosTheta - t * sinTheta; -} - -void z_rotate_matrix_spacefixed(double m[][4], double radians) -{ - /* append rotation about global z-axis to matrix 'm' */ - double sinTheta = sin(radians); - double cosTheta = cos(radians); - - double t = m[0][0]; - m[0][0] = t * cosTheta - m[0][1] * sinTheta; - m[0][1] = t * sinTheta + m[0][1] * cosTheta; - - t = m[1][0]; - m[1][0] = t * cosTheta - m[1][1] * sinTheta; - m[1][1] = t * sinTheta + m[1][1] * cosTheta; - - t = m[2][0]; - m[2][0] = t * cosTheta - m[2][1] * sinTheta; - m[2][1] = t * sinTheta + m[2][1] * cosTheta; - - t = m[3][0]; - m[3][0] = t * cosTheta - m[3][1] * sinTheta; - m[3][1] = t * sinTheta + m[3][1] * cosTheta; -} - -void translate_matrix(double m[][4], const double* delta) -{ - /* append translation 'delta' to matrix 'm' */ - - m[0][0] += m[0][3] * delta[0]; m[0][1] += m[0][3] * delta[1]; m[0][2] += m[0][3] * delta[2]; - m[1][0] += m[1][3] * delta[0]; m[1][1] += m[1][3] * delta[1]; m[1][2] += m[1][3] * delta[2]; - m[2][0] += m[2][3] * delta[0]; m[2][1] += m[2][3] * delta[1]; m[2][2] += m[2][3] * delta[2]; - m[3][0] += m[3][3] * delta[0]; m[3][1] += m[3][3] * delta[1]; m[3][2] += m[3][3] * delta[2]; -} - -void scale_matrix (double m[][4], const double* scaleBy) -{ - m[0][0] *= scaleBy[XX]; m[0][1] *= scaleBy[YY]; m[0][2] *= scaleBy[ZZ]; - m[1][0] *= scaleBy[XX]; m[1][1] *= scaleBy[YY]; m[1][2] *= scaleBy[ZZ]; - m[2][0] *= scaleBy[XX]; m[2][1] *= scaleBy[YY]; m[2][2] *= scaleBy[ZZ]; - m[3][0] *= scaleBy[XX]; m[3][1] *= scaleBy[YY]; m[3][2] *= scaleBy[ZZ]; -} - -void append_matrix(DMatrix m, const DMatrix b) -{ - /* append matrix 'b' to matrix 'm' */ - - double ta[4][4]; - - memcpy(ta, m, 16 * sizeof(double)); - - m[0][0] = ta[0][0] * b[0][0] + ta[0][1] * b[1][0] + ta[0][2] * b[2][0] + ta[0][3] * b[3][0]; - m[0][1] = ta[0][0] * b[0][1] + ta[0][1] * b[1][1] + ta[0][2] * b[2][1] + ta[0][3] * b[3][1]; - m[0][2] = ta[0][0] * b[0][2] + ta[0][1] * b[1][2] + ta[0][2] * b[2][2] + ta[0][3] * b[3][2]; - m[0][3] = ta[0][0] * b[0][3] + ta[0][1] * b[1][3] + ta[0][2] * b[2][3] + ta[0][3] * b[3][3]; - - m[1][0] = ta[1][0] * b[0][0] + ta[1][1] * b[1][0] + ta[1][2] * b[2][0] + ta[1][3] * b[3][0]; - m[1][1] = ta[1][0] * b[0][1] + ta[1][1] * b[1][1] + ta[1][2] * b[2][1] + ta[1][3] * b[3][1]; - m[1][2] = ta[1][0] * b[0][2] + ta[1][1] * b[1][2] + ta[1][2] * b[2][2] + ta[1][3] * b[3][2]; - m[1][3] = ta[1][0] * b[0][3] + ta[1][1] * b[1][3] + ta[1][2] * b[2][3] + ta[1][3] * b[3][3]; - - m[2][0] = ta[2][0] * b[0][0] + ta[2][1] * b[1][0] + ta[2][2] * b[2][0] + ta[2][3] * b[3][0]; - m[2][1] = ta[2][0] * b[0][1] + ta[2][1] * b[1][1] + ta[2][2] * b[2][1] + ta[2][3] * b[3][1]; - m[2][2] = ta[2][0] * b[0][2] + ta[2][1] * b[1][2] + ta[2][2] * b[2][2] + ta[2][3] * b[3][2]; - m[2][3] = ta[2][0] * b[0][3] + ta[2][1] * b[1][3] + ta[2][2] * b[2][3] + ta[2][3] * b[3][3]; - - m[3][0] = ta[3][0] * b[0][0] + ta[3][1] * b[1][0] + ta[3][2] * b[2][0] + ta[3][3] * b[3][0]; - m[3][1] = ta[3][0] * b[0][1] + ta[3][1] * b[1][1] + ta[3][2] * b[2][1] + ta[3][3] * b[3][1]; - m[3][2] = ta[3][0] * b[0][2] + ta[3][1] * b[1][2] + ta[3][2] * b[2][2] + ta[3][3] * b[3][2]; - m[3][3] = ta[3][0] * b[0][3] + ta[3][1] * b[1][3] + ta[3][2] * b[2][3] + ta[3][3] * b[3][3]; -} - -void transform_pt(double m[][4], double* pt) -{ - /* apply a matrix transformation to a 3d point */ - - double tx = pt[XX] * m[0][0] + pt[YY] * m[1][0] + pt[ZZ] * m[2][0] + m[3][0]; - double ty = pt[XX] * m[0][1] + pt[YY] * m[1][1] + pt[ZZ] * m[2][1] + m[3][1]; - - pt[ZZ] = pt[XX] * m[0][2] + pt[YY] * m[1][2] + pt[ZZ] * m[2][2] + m[3][2]; - pt[XX] = tx; - pt[YY] = ty; -} - -void transform_vec(double m[][4], double* vec) -{ - /* apply a matrix transformation to a 3d vector */ - - double tx = vec[XX] * m[0][0] + vec[YY] * m[1][0] + vec[ZZ] * m[2][0]; - double ty = vec[XX] * m[0][1] + vec[YY] * m[1][1] + vec[ZZ] * m[2][1]; - - vec[ZZ] = vec[XX] * m[0][2] + vec[YY] * m[1][2] + vec[ZZ] * m[2][2]; - vec[XX] = tx; - vec[YY] = ty; -} - -static void quat_to_axis_angle_rot (const Quat q, dpCoord3D* axis, double* angle) -{ - double sin_a2; - - axis->xyz[XX] = q[XX]; - axis->xyz[YY] = q[YY]; - axis->xyz[ZZ] = q[ZZ]; - - /* |sin a/2|, w = cos a/2 */ - sin_a2 = sqrt(SQR(q[XX]) + SQR(q[YY]) + SQR(q[ZZ])); - - /* 0 <= angle <= PI , because 0 < sin_a2 */ - *angle = 2.0 * atan2(sin_a2, q[WW]); - - if (EQUAL_WITHIN_ERROR(0.0,*angle)) - { - axis->xyz[XX] = 1.0; - axis->xyz[YY] = 0.0; - axis->xyz[ZZ] = 0.0; - } -} - -static void matrix_to_quat (double m[][4], Quat q) -{ - double s, trace = m[XX][XX] + m[YY][YY] + m[ZZ][ZZ]; - - if (NEAR_GT_OR_EQ(trace, 0.0)) - { - s = sqrt(trace + m[WW][WW]); - q[WW] = s * 0.5; - s = 0.5 / s; - - q[XX] = (m[YY][ZZ] - m[ZZ][YY]) * s; - q[YY] = (m[ZZ][XX] - m[XX][ZZ]) * s; - q[ZZ] = (m[XX][YY] - m[YY][XX]) * s; - } - else - { - static const int next[] = { YY, ZZ, XX }; - - int i = XX, j, k; - - if (m[YY][YY] > m[XX][XX]) - i = YY; - if (m[ZZ][ZZ] > m[i][i]) - i = ZZ; - - j = next[i]; - k = next[j]; - - s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + m[WW][WW]); - - q[i] = s * 0.5; - s = 0.5 / s; - - q[j] = (m[i][j] + m[j][i]) * s; - q[k] = (m[i][k] + m[k][i]) * s; - q[WW] = (m[j][k] - m[k][j]) * s; - } - if (NOT_EQUAL_WITHIN_ERROR(m[WW][WW], 1.0)) - { - double scale = 1.0 / sqrt(m[WW][WW]); - - q[XX] *= scale; q[YY] *= scale; q[ZZ] *= scale; q[WW] *= scale; - } -} - -void extract_rotation(double m[][4], dpCoord3D* axis, double* angle) -{ - /* extract matrix rotation in axis-angle format */ - - Quat q; - - matrix_to_quat(m, q); - - quat_to_axis_angle_rot(q, axis, angle); -} - -void extract_xyz_rot_spacefixed(double m[][4], double xyz_rot[3]) -{ - /* NOTE: extracts SPACE-FIXED rotations in x,y,z order. - * The matrix may have scale factors in it, so it needs to - * be normalized first. - */ - double mat[4][4]; - - copy_4x4matrix(m, mat); - normalize_vector(mat[0], mat[0]); - normalize_vector(mat[1], mat[1]); - normalize_vector(mat[2], mat[2]); - - xyz_rot[YY] = asin(-mat[0][2]); - - if (NOT_EQUAL_WITHIN_ERROR(0.0,cos(xyz_rot[YY]))) - { - xyz_rot[XX] = atan2(mat[1][2], mat[2][2]); - xyz_rot[ZZ] = atan2(mat[0][1], mat[0][0]); - } - else - { - xyz_rot[XX] = atan2(mat[1][0], mat[1][1]); - xyz_rot[ZZ] = 0.0; - } -} - -void extract_xyz_rot_bodyfixed(double m[][4], double xyz_rot[3]) -{ - /* NOTE: extracts BODY-FIXED rotations in x,y,z order, which - * is the same as space-fixed rotations in z,y,x order. - * The matrix may have scale factors in it, so it needs to - * be normalized first. - */ - double mat1[4][4], mat2[4][4]; - -#if 0 - // Normalize the columns by transposing so that the - // vectors are contiguous. Then transpose back. - transpose_4x4matrix(m, mat1); - normalize_vector(mat1[0], mat1[0]); - normalize_vector(mat1[1], mat1[1]); - normalize_vector(mat1[2], mat1[2]); - transpose_4x4matrix(mat1, mat2); -#else - copy_4x4matrix(m, mat2); - normalize_vector(mat2[0], mat2[0]); - normalize_vector(mat2[1], mat2[1]); - normalize_vector(mat2[2], mat2[2]); -#endif - - xyz_rot[YY] = asin(mat2[2][0]); - - if (NOT_EQUAL_WITHIN_ERROR(0.0,cos(xyz_rot[YY]))) - { - xyz_rot[XX] = atan2(-mat2[2][1], mat2[2][2]); - xyz_rot[ZZ] = atan2(-mat2[1][0], mat2[0][0]); - } - else - { - xyz_rot[XX] = atan2(mat2[0][1], mat2[1][1]); - xyz_rot[ZZ] = 0.0; - } - /* NOTE: a body-fixed sequence of rotations is equivalent to - * the same sequence of space-fixed rotation in the *opposite* - * order!! (see: "Introduction to Robotics, 2nd Ed. by J. Craig, - * page 49) -- KMS 2/17/99 - */ -} - -static void make_quaternion(Quat q, const double axis[3], double angle) -{ - /* make a quaternion given an axis-angle rotation - * (from java-based vecmath package) - */ - double n, halfAngle; - - q[XX] = axis[XX]; - q[YY] = axis[YY]; - q[ZZ] = axis[ZZ]; - - n = sqrt(SQR(q[XX]) + SQR(q[YY]) + SQR(q[ZZ])); - - halfAngle = 0.5 * angle; - - if (NOT_EQUAL_WITHIN_ERROR(n,0.0)) - { - double s = sin(halfAngle) / n; - - q[XX] *= s; q[YY] *= s; q[ZZ] *= s; - - q[WW] = cos(halfAngle); - } -} - -static void quat_to_matrix (const Quat q, double m[][4]) -{ - /* make a rotation matrix from a quaternion */ - - double Nq = SQR(q[XX]) + SQR(q[YY]) + SQR(q[ZZ]) + SQR(q[WW]); - double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0; - - double xs = q[XX] * s, ys = q[YY] * s, zs = q[ZZ] * s; - double wx = q[WW] * xs, wy = q[WW] * ys, wz = q[WW] * zs; - double xx = q[XX] * xs, xy = q[XX] * ys, xz = q[XX] * zs; - double yy = q[YY] * ys, yz = q[YY] * zs, zz = q[ZZ] * zs; - - m[XX][XX] = 1.0 - (yy + zz); m[XX][YY] = xy + wz; m[XX][ZZ] = xz - wy; - m[YY][XX] = xy - wz; m[YY][YY] = 1.0 - (xx + zz); m[YY][ZZ] = yz + wx; - m[ZZ][XX] = xz + wy; m[ZZ][YY] = yz - wx; m[ZZ][ZZ] = 1.0 - (xx + yy); - - m[XX][WW] = m[YY][WW] = m[ZZ][WW] = m[WW][XX] = m[WW][YY] = m[WW][ZZ] = 0.0; - m[WW][WW] = 1.0; -} - -static void rotate_matrix_by_quat(double m[][4], const Quat q) -{ - /* append a quaternion rotation to a matrix */ - - double n[4][4]; - - quat_to_matrix(q, n); - - append_matrix(m, n); /* append matrix 'n' onto matrix 'm' */ -} - -void rotate_matrix_axis_angle (double m[][4], const double* axis, double angle) -{ - Quat q; - - make_quaternion(q, axis, angle); - rotate_matrix_by_quat(m, q); -} - -void lerp_pt(double start[3], double end[3], double t, double result[3]) -{ - result[0] = start[0] + t * (end[0] - start[0]); - result[1] = start[1] + t * (end[1] - start[1]); - result[2] = start[2] + t * (end[2] - start[2]); -} - -void slerp(const dpCoord3D* axisStart, double angleStart, - const dpCoord3D* axisEnd, double angleEnd, - double t, - dpCoord3D* axisResult, double* angleResult) -{ - Quat from, to, res; - double to1[4], omega, cosom, sinom, scale0, scale1; - - make_quaternion(from, axisStart->xyz, angleStart); - make_quaternion(to, axisEnd->xyz, angleEnd); - - /* calc cosine */ - cosom = from[XX] * to[XX] + from[YY] * to[YY] + from[ZZ] * to[ZZ] + from[WW] * to[WW]; - - /* adjust signs (if necessary) */ - if (cosom < 0.0) - { - cosom = -cosom; to1[0] = - to[XX]; - to1[1] = - to[YY]; - to1[2] = - to[ZZ]; - to1[3] = - to[WW]; - } - else - { - to1[0] = to[XX]; - to1[1] = to[YY]; - to1[2] = to[ZZ]; - to1[3] = to[WW]; - } - - /* calculate coefficients */ - if ((1.0 - cosom) > TINY_NUMBER) - { - /* standard case (slerp) */ - omega = acos(cosom); - sinom = sin(omega); - scale0 = sin((1.0 - t) * omega) / sinom; - scale1 = sin(t * omega) / sinom; - } - else - { - /* "from" and "to" quaternions are very close, do a linear interpolation */ - scale0 = 1.0 - t; - scale1 = t; - } - /* calculate final values */ - res[XX] = scale0 * from[XX] + scale1 * to1[XX]; - res[YY] = scale0 * from[YY] + scale1 * to1[YY]; - res[ZZ] = scale0 * from[ZZ] + scale1 * to1[ZZ]; - res[WW] = scale0 * from[WW] + scale1 * to1[WW]; - - quat_to_axis_angle_rot(res, axisResult, angleResult); -} - -smAxes find_primary_direction(double vec[]) -{ - double x_abs = ABS(vec[0]); - double y_abs = ABS(vec[1]); - double z_abs = ABS(vec[2]); - - if (x_abs >= y_abs) - { - if (x_abs >= z_abs) - { - if (vec[0] > 0.0) - return smX; - else - return smNegX; - } - else - { - if (vec[2] > 0.0) - return smZ; - else - return smNegZ; - } - } - else - { - if (y_abs >= z_abs) - { - if (vec[1] > 0.0) - return smY; - else - return smNegY; - } - else - { - if (vec[2] > 0.0) - return smZ; - else - return smNegZ; - } - } -} - - -/* COPY_4X4MATRIX: copies a 4x4 matrix */ -void copy_4x4matrix(double from[][4], double to[][4]) -{ - to[0][0] = from[0][0]; - to[0][1] = from[0][1]; - to[0][2] = from[0][2]; - to[0][3] = from[0][3]; - to[1][0] = from[1][0]; - to[1][1] = from[1][1]; - to[1][2] = from[1][2]; - to[1][3] = from[1][3]; - to[2][0] = from[2][0]; - to[2][1] = from[2][1]; - to[2][2] = from[2][2]; - to[2][3] = from[2][3]; - to[3][0] = from[3][0]; - to[3][1] = from[3][1]; - to[3][2] = from[3][2]; - to[3][3] = from[3][3]; -} diff --git a/OpenSim/Utilities/simmToOpenSim/mathtools.h b/OpenSim/Utilities/simmToOpenSim/mathtools.h deleted file mode 100644 index 68dd8ca2e3..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/mathtools.h +++ /dev/null @@ -1,84 +0,0 @@ -/******************************************************************************* - - MATHTOOLS.H - - Author: Peter Loan - - Date: 28-OCT-98 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - -*******************************************************************************/ - -void append_4x4matrix(double mat1[][4], double mat2[][4]); -void make_translation_matrix(double mat[][4], int axis, double trans); -void make_scale_matrix(double mat[][4], int axis, double scale); -int get_array_length(int list[]); -void mult_4x4matrix_by_vector(double mat[][4], double vector[], double result[]); -void mult_4x4matrices(double mat1[][4], double mat2[][4], double result[][4]); -void transpose_4x4matrix(double mat[][4], double mat_transpose[][4]); -void cross_vectors(double vector1[], double vector2[], double result[]); -double normalize_vector(double vector[], double norm_vector[]); -double normalize_vector_f(float vector[], float norm_vector[]); -void format_double(double number, char format[]); -void reset_4x4matrix(double mat[][4]); -void make_4x4dircos_matrix(double angle, double axis[], double mat[][4]); -SBoolean vector_intersects_polyhedron(double pt[], double vec[], PolyhedronStruct* ph, double inter[]); -SBoolean intersect_line_plane(double pt1[], double pt2[], - double plane[], double d, double inter[], double* t); -SBoolean intersect_line_plane01(double pt1[], double pt2[], - double plane[], double d, double inter[], double* t); -SBoolean point_in_polygon3D(double pt[], PolyhedronStruct* ph, int polygon_index); -int point_in_polygon2D(double point[], double pgon[][2], int numverts); -int polygon_ray_inter3d(PolyhedronStruct* newph, int poly_index, double pt[3], int axes); -int polygon_ray_inter_jordanstheorem(double** poly_pts, int numpts, - double ptray[3], int axes); -int point_ray_relation(double* pt, double ptray[], int axes); -void make_rotation_matrix(double** mat, double normal[]); -int intersect_lines(double p1[], double p2[], double p3[], double p4[], - double p_int1[], double* t, double p_int2[], double* s); -int intersect_lines_scaled(double p1[], double p2[], double p3[], double p4[], - double p_int1[], double* t, double* mag1, - double p_int2[], double* s, double* mag2); -void clear_vector(double a[], int n); -void mult_3x3matrix_by_vector(double mat[][3], double vector[], double result[]); -void make_3x3_xrot_matrix(double a, double m[][3]); -void make_3x3dircos_matrix(double angle, double axis[], double mat[][3]); -void reset_3x3matrix(double matrix[][3]); -void mult_3x3matrices(double mat1[][3], double mat2[][3], double result[][3]); -void transpose_3x3matrix(double mat[][3], double mat_transpose[][3]); -void copy_3x3matrix(double from[][3], double to[][3]); -void mult_3x3_by_vector(double mat[][3], double vec[]); -int intersect_lines(double p1[], double p2[], double p3[], double p4[], - double p_int1[], double* t, double p_int2[], double* s); -void find_plane_normal_to_line(PlaneStruct* plane, double pt1[], double pt2[]); -double compute_angle_between_vectors(double vector1[], double vector2[]); -void project_point_onto_plane(double pt[], PlaneStruct* plane, double projpt[]); -double distance_between_vertices(double vertex1[], double vertex2[]); -double distancesqr_between_vertices(double vertex1[], double vertex2[]); -double get_distsqr_point_line(double point[], double pl[], double vl[]); -void get_point_from_point_line(double point[], double pt[], double vec[], - double closest_pt[]); -double atan3(double y, double x); -void identity_matrix(double m[][4]); -void x_rotate_matrix_bodyfixed(double m[][4], double radians); /* body-fixed rotation */ -void y_rotate_matrix_bodyfixed(double m[][4], double radians); -void z_rotate_matrix_bodyfixed(double m[][4], double radians); -void x_rotate_matrix_spacefixed(double m[][4], double radians); -void y_rotate_matrix_spacefixed(double m[][4], double radians); -void z_rotate_matrix_spacefixed(double m[][4], double radians); -void rotate_matrix_axis_angle(double m[][4], const double* axis, double angle); -void translate_matrix(double m[][4], const double* delta); -void scale_matrix(double m[][4], const double* scaleBy); -void append_matrix(DMatrix m, const DMatrix b); -void transform_pt(double m[][4], double* pt); -void transform_vec(double m[][4], double* vec); -void extract_rotation(double m[][4], dpCoord3D* axis, double* angle); -void extract_xyz_rot_spacefixed(double m[][4], double xyz_rot[3]); -void extract_xyz_rot_bodyfixed(double m[][4], double xyz_rot[3]); -void lerp_pt(double start[3], double end[3], double t, double result[3]); -void slerp(const dpCoord3D* axisStart, double angleStart, - const dpCoord3D* axisEnd, double angleEnd, - double t, dpCoord3D* axisResult, double* angleResult); - diff --git a/OpenSim/Utilities/simmToOpenSim/mocap.h b/OpenSim/Utilities/simmToOpenSim/mocap.h deleted file mode 100644 index 2d6ea176f1..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/mocap.h +++ /dev/null @@ -1,188 +0,0 @@ -/******************************************************************************* - - MOCAP.H - public api for the mocap to SIMM tranlator. This translator - currently handles Motion Analysis HTR & HTR2 files as well as realtime - motion received via. network connection to EVa Realtime. In addition - this translator can import analog data that may accompany an HTR or HTR2 - file in the form of ANB, ANC, or OrthoTrak XLS file(s). - - Author: Kenny Smith - - Date: 25-JUN-99 - - Copyright (c) 1999 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ -#ifndef MOCAP_H -#define MOCAP_H - -/* --------------------------------------------------------------------------- - PUBLIC MOCAP IMPORT API: ------------------------------------------------------------------------------- */ -#if ! OPENSIM_BUILD -public ReturnCode mocap_to_simm(glutMocapOptions*, int* modelIndex); -#endif - -public ReturnCode save_pose_file(ModelStruct*, const char* fileName); - -/* --------------------------------------------------------------------------- - PUBLIC REALTIME MOCAP IMPORT API: ------------------------------------------------------------------------------- */ -public void start_stop_realtime_mocap_stream(ModelStruct*); -public void stop_realtime_mocap_stream(); -public void realtime_exit(); -public void refresh_realtime_display(); - -public const char* get_realtime_hz(); - -/* =========================================================================== - PRIVATE API - the following declarations are included in this header simply - so that the mocap importer implementation can be divided into seperate - modules (currently mocap.c and analoc.c). Client code elsewhere in SIMM - should not make use of any of the following code. ------------------------------------------------------------------------------- */ - -#define REALTIME_MOTION_NAME "realtime" - -enum { /* htr & htr2 column identifiers */ - _TX, _TY, _TZ, - _RX, _RY, _RZ, - _BONE_SCALE, - HTR_FRAME_NUM_COLS, - - _HTR2_BONE_LEN = _BONE_SCALE -}; - -STRUCT { /* ---- mocap model segment record: */ - char name[128]; /* mocap segment name */ - char parentName[128]; /* mocap parent segment name */ - int parentIndex; /* mocap parent segment index */ - dpCoord3D offset; /* base position: starting translation */ - dpCoord3D rotation; /* base position: starting rotation */ - DMatrix base_transform; /* base position: as a transform matrix */ - double boneLength; /* mocap segment length */ - double htr2TranslationParam; /* htr2 translation parameter */ -} MocapSeg; - -enum { XYZ, ZYX }; /* eulerRotationOrder */ - -enum { MM }; /* calibrationUnits */ - -enum { DEGREES, RADIANS }; /* rotationUnits */ - -STRUCT { /* ---- mocap file info record: */ - int fileVersion; /* mocap file version number */ - int numSegments; /* number of body segments */ - int numFrames; /* number of mocap frames to follow */ - int dataFrameRate; /* mocap frames per second */ - int eulerRotationOrder; /* XYZ, or ZYX */ - int calibrationUnits; /* MM */ - int rotationUnits; /* DEGREES or RADIANS */ - int globalAxisOfGravity; /* up direction (XX, YY, or ZZ) */ - int boneLengthAxis; /* local long axis for mocap segments */ - int analogSyncFrameIndex;/* index to sync motion & analog data */ - double scaleFactor; /* ?? */ - MocapSeg* segments; /* array of mocap body segments */ - int rootSegment; /* index of root mocap segment */ - long byteOffsetToFrames; /* offset to beginning of motion frame data */ - ModelStruct* model; /* SIMM model that is created from this mocap data */ -} MocapInfo; - - -#if OLD_ANALOG - -enum { FX, FY, FZ, MX, MY, MZ, NUM_AMTI_BERTEC_CHANNELS }; - -enum { FX12, FX34, FY14, FY23, FZ1, FZ2, FZ3, FZ4, NUM_KISTLER_CHANNELS }; - -enum { /* ---- analog channel type */ - UNSPECIFIED_ANALOG_CHANNEL, - ELAPSED_TIME, - FORCE_PLATE_FORCE, - FORCE_PLATE_MOMENT, - FORCE_LOCATION, - EMG, - OTHER_DATA -}; - -STRUCT { /* ---- forceplate channel info: */ - int index; /* forceplate index (zero based) */ - int component; /* forceplate channel componant (see enums above) */ - double baseline; -} FPChannel; - -STRUCT { /* ---- emg channel info: */ - int numMuscles; - char** muscleNames; /* array of SIMM muscle names */ - int* muscleIndices; /* array of SIMM muscle indices */ - double maxVoluntaryContraction;/* user-specified value for scaling this emg channel */ - double maxSample; /* observed maximum value for this channel */ -} EMGMuscles; - -STRUCT { /* ---- other data channel info: */ - char* name; /* name to use for SIMM motion column title */ -} OtherData; - -STRUCT { /* ---- import variable info: */ - char* name; /* imported variable name */ - int type; /* force plate, emg, or other data */ - int frequency; /* sample rate, Hz */ - int range; /* channel range (ie. maximum?) */ - int numMotionColumns; /* number of motion data columns channel maps to */ - int* motionColumn; /* columns of motion data that this channel maps to */ - union { - FPChannel forcePlate; - EMGMuscles emg; - OtherData otherData; - } u; -} AnalogChannel; - -typedef struct { /* ---- gait events (read from OrthoTrak XLS file) */ - char name[32]; /* event name */ - double time; /* event frame number */ -} GaitEvent; - -STRUCT { /* ---- analog data record */ - int numChannels; - int numRows; - AnalogChannel* channels; - double* data; - int numGaitEvents; - GaitEvent* gaitEvents; -} AnalogData; - -STRUCT { /* ---- forceplate record: */ - SBoolean inited; /* has this record been initialized? */ - int manufacturer; /* manufacturer id (see enum above) */ - double scaleFactor; /* overall channel scale factor? */ - double length; /* forceplate length (optional) */ - double width; /* forceplate width (optional) */ - double calibrationMatrix[64]; /* forceplate calibration matrix */ - DMatrix localToWorldXform; /* forceplate placement matrix */ - - struct { /* -- Kistler forceplate parameters: */ - double a, b, az0; /* transducer placement: a, b, az0 */ - double p[6]; /* COP correction coefficients (optional) */ - SBoolean hasCOPCorrectionValues; /* are COP correction coefficients inited? */ - } kistler; -} ForcePlateSpec; - -#endif - -#if ! OPENSIM_BUILD -smAnalogStruct* _init_channel_mapping(SBoolean includeMuscles, const char analogFile[]); -smC3DStruct* read_analog_data_files(int numFiles, char* const* files, const MocapInfo*, const glutMocapOptions*); -SBoolean _map_channel(const char* name, smAnalogChannel* channel, const smAnalogStruct* m); -void _rectify_emg_samples(smAnalogStruct* ad); -void _scale_emg_samples(smAnalogStruct* ad); -int _init_forceplates(smForcePlateSpec fp[], int n, const char analogFile[], ModelStruct* ms); -void _nuke_forceplate_samples(smAnalogStruct* ad); -void _autocalibrate_forceplate_channel(smAnalogStruct* ad, int col); -void free_analog_data(smAnalogStruct*); -void post_process_c3d_analog_data(smC3DStruct* c3d, const MocapInfo* mi, const glutMocapOptions* mo); -void read_critical_marker_names(char* names[], const char dataFile[]); -#endif - -#endif /* MOCAP_H */ diff --git a/OpenSim/Utilities/simmToOpenSim/modelplot.h b/OpenSim/Utilities/simmToOpenSim/modelplot.h deleted file mode 100644 index bba474cd38..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/modelplot.h +++ /dev/null @@ -1,1619 +0,0 @@ -/******************************************************************************* - - MODELPLOT.H - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef MODELPLOT_H -#define MODELPLOT_H - -#define MAX_GENCOORDS 5000 -#define MAX_ELEMENTS 16384 // for pop-up menu tags - -#define R1 0 -#define R2 1 -#define R3 2 -#define TX 3 -#define TY 4 -#define TZ 5 -#define TRANS 0 -#define ROT1 1 -#define ROT2 2 -#define ROT3 3 -#define AXIS1 0 -#define AXIS2 1 -#define AXIS3 2 - -#define DEFAULT_TITLE 0 -#define USER_DEFINED_TITLE 1 - -/* For object picking */ -#define OBJECT_BITS ((PickIndex)56) -#define MAX_OBJECTS ((PickIndex)2< tolerance, loop is broken*/ -#define DEFAULT_LOOP_TOLERANCE 1e-4 -#define DEFAULT_LOOP_WEIGHT 10.0 -#define DEFAULT_SOLVER_ACCURACY 1e-4 - -#define TIME ((void*)0xcafebeef) - -#define BOOL_EPSILON 0.00001 -#define BOOL_EPSILON_SQR 0.0000000001 -#define BOOL_APX_EQ(x, y) (DABS((x) - (y)) <= BOOL_EPSILON) - -#define BOOL_EPSILON2 0.00001 -#define BOOL_EPSILON2_SQR 0.0000000001 -#define BOOL_APX_EQ2(x, y) (DABS((x) - (y)) <= BOOL_EPSILON2) - -#define BOOL_EPSILON3 0.002 -#define BOOL_EPSILON3_SQR 0.000004 -#define BOOL_APX_EQ3(x, y) (DABS((x) - (y)) <= BOOL_EPSILON3) - -#define ANGLES_APX_EQ(x, y) (DABS((x) - (y)) <= ANGLE_EPSILON) - -#define FALSE 0 -#define TRUE 1 -#define BOOL_INFINITY 1000000 -#define ON_RAY 0 -#define ABOVE_RAY 1 -#define BELOW_RAY 2 - -#define GLOBAL_SEGMENT -1 -#define UNDEFINED_SEGMENT -2 -#define UNDEFINED_MARKER -2 - -#define FUNCTION_MENU_START 1000 - -ENUM { - rtMocap, - rtSimulation, - rtNotConnected -} RTConnection; - - -ENUM { - draw_skin_only, - draw_bones_only, - draw_skin_and_bones -} SkinMode; - - -ENUM { - rotation_gencoord, - translation_gencoord -} GencoordType; /* type of a gencoord */ - - -ENUM { - TFL_CURVE = 0, - AFL_CURVE, - PFL_CURVE, - FV_CURVE, - EXC_CURVE -} MuscleFunctionType; - - -ENUM { - constant_dof, /* constant */ - function_dof /* function */ -} DofType; /* type of elements in a dof definition */ - - -ENUM { - r1, /* rotation about axis 1 */ - r2, /* rotation about axis 2 */ - r3, /* rotation about axis 3 */ - tx, /* translation along the x axis */ - ty, /* translation along the y axis */ - tz, /* translation along the z axis */ - t /* used by the Gait Loader */ -} DofKey; /* the 6 dofs in every joint */ - - -ENUM { - MODEL_ADD, /* add a new model */ - MODEL_DELETE /* delete an existing model */ -} ModelAction; /* possible actions on model windows */ - - -ENUM { - ASCII_FORMAT, /* columns of numbers (raw data) */ - POSTSCRIPT_FORMAT, /* plain, Postscript format */ - ILLUSTRATOR_FORMAT /* Illustrator-readable Postscript */ -} PLOTFILEFORMAT; /* format for output plot file */ - - -ENUM { - found_none, - found_some -} VerticesFound; - - -ENUM { - unspecified_order, - clockwise, - counterclockwise -} VertexOrder; - - -ENUM { - no_proc, - yes_proc, - yes_and_propagated -} PolygonProcess; - - -ENUM { - old_ascii, - new_ascii, - binary, - wavefront, - stl_ascii, - stl_binary, - unknown, - file_not_found -} FileType; - -ENUM { - file_good, - file_bad, - file_read_only, - file_missing -} FileReturnCode; - -ENUM { - didNotSolveLoop, - loopUnchanged, - loopChanged, - loopBroken, - largeResidinLoop -} LoopStatus; - -ENUM { - didNotSolveConstraint, - constraintUnchanged, - constraintChanged, - constraintBroken, - gcOutOfRange -} ConstraintStatus; - -enum { /* bit flags for display_animation_hz */ - DISPLAY_HZ_IN_WINDOW = 0x01, - DISPLAY_HZ_ON_CONSOLE = 0x02, - DISPLAY_POLYS_PER_SEC = 0x04 -}; - -STRUCT { - double start; /* starting value of range */ - double end; /* ending value of range */ -} Range; /* a generalized coordinate range */ - -ENUM { - constraint_sphere = 0, - constraint_cylinder, - constraint_ellipsoid, - constraint_plane, - constraint_none -} ConstraintObjectType; - -STRUCT { - double a, b, c, d; -} PlaneStruct; - - -STRUCT { - dpCoord3D rotation_axis; /* local-to-parent transform parameter */ - double rotation_angle; /* local-to-parent transform parameter */ - dpCoord3D translation; /* local-to-parent transform parameter */ - SBoolean xforms_valid; /* do xform matrices need recalculation? */ - int ref_count; /* used to track a specific instance of an xform */ - DMatrix from_local_xform; /* wrapobj-to-parent transform matrix */ - DMatrix to_local_xform; /* parent-to-wrapobj transform matrix */ -} XForm; - -STRUCT { - char* name; /* name of deform object */ - int segment; /* body segment object is fixed to */ - SBoolean active; /* is the deform object active? */ - SBoolean visible; /* is the deform object visible? */ - SBoolean autoReset; /* special case -- auto-reset deformation */ - SBoolean translationOnly; /* auto-reset translations only? */ - dpCoord3D innerMin, innerMax; /* inner deform box */ - dpCoord3D outerMin, outerMax; /* outer deform box */ - XForm position; /* outer box placement (in segment frame) */ - XForm deform_start; /* inner box starting placement (in segment frame) */ - XForm deform_end; /* inner box ending placement (in segment frame) */ - double deform_factor; /* deformation parameter (0.0 to 1.0) */ - DMatrix delta_xform; /* transform for deforming vertices */ - DMatrix delta_xform2; /* transform for displaying deform values in DE */ - DMatrix delta_xform3; /* transform for displaying deform boxes and axes */ - float* innerBox; /* inner box vertices */ - float* innerBoxUndeformed; /* inner box vertices (undeformed) */ - float* outerBox; /* outer box vertices */ - float* outerBoxUndeformed; /* outer box vertices (undeformed) */ -} DeformObject; - -STRUCT { - char* name; - double value; - double default_value; - Range range; - int keys[2]; /* keys to change deformity's value */ - int num_deforms; - DeformObject** deform; - char** deform_name; -} Deformity; - -STRUCT { - int segmentnum; /* number of the segment this polygon is in */ - int bonenum; /* number of the bone this polygon is in */ - int polynum; /* the polygon number */ -} SelectedPolygon; /* properties of the highlighted polygon */ - - -STRUCT { - Condition condition; /* condition (dirty,clean) of matrices */ - DMatrix forward; /* forward transform for ref frames */ - DMatrix inverse; /* backward transform for ref frames */ -#if ! ENGINE - GLfloat gl_forward[16]; /* float matrix for use in GL calls */ - GLfloat gl_inverse[16]; /* float matrix for use in GL calls */ -#endif -} Conversion; /* joint transformation matrices */ - - -STRUCT { - int genc; /* gencoord number */ - double start; /* starting value of range-of-motion */ - double end; /* ending value of range-of-motion */ -} PointRange; /* muscle point range (wrapping pts) */ - - -STRUCT { - char* name; /* name of muscle group */ - int num_muscles; /* number of muscles in group */ - int muscindex_array_size; /* current size of muscle_index array */ - int* muscle_index; /* list of muscles in group */ - int number_of_columns; /* width of menu, in columns */ - int column_width; /* width of a column, in pixels */ - Menu menu; /* menu structure for this group */ -} MuscleGroup; /* properties of a muscle group */ - - -STRUCT { - char* name; - int num_segments; - int seg_array_size; - int* segment; - int displaymodemenu; -} SegmentGroup; - - -STRUCT { - int v_num; - double distance; - int side; -} PListStruct; - - -STRUCT { - double ptseg[2][3]; - int vert_edge[2]; - int vertex_index[2]; /* vertices of edge (in pl1) if seg runs over an edge */ - int vindices[2][2]; /* vertices of edges (in pl2) that made this segment */ - int poly_index; - int marked; /* used in boolean extraction routines */ -} PInterSegmentStruct; - -STRUCT { - int num_inter_seg; /* segments in each polygon */ - PInterSegmentStruct *seg; - PInterSegmentStruct *segmaxx; -} PInterSegmentListStruct; - -STRUCT { - int seglst_num; - PInterSegmentListStruct *seglst; /* used for clipping loops */ -} PTInterSegmentListStruct; - -STRUCT { - double key; - int num_inter_seg; - PInterSegmentListStruct *seglst; /* used for sorting loops */ -} SortOpenStruct; - -STRUCT { - int num; /* always be an even number */ - SortOpenStruct *sos; /* used for sorting loops */ -} SortOpenListStruct; - -STRUCT { - int coplanar_flag; - int polygon_mark; /* polygon used or has an intersection */ - int poly_output; /* polygon is in output list */ - int poly_adjpush; /* polygon is being pushed onto adj list */ - int num_inters; - int inters[MAX_POLY_INTERS]; - PInterSegmentListStruct seg_list; /* segment lists from intersection */ - int seglst_num; - PInterSegmentListStruct *seglst; /* used for clipping loops */ -} BooleanInfo; - - -STRUCT { - int num_vertices; - int *vertex_index; - double normal[3]; - int normal_computed; - SBoolean selected; -#if POLYGON_DISPLAY_HACK - SBoolean displayed; -#endif - int polyhedron_number; - int thrown_out; - int old; - double ordering_value; - double d; - BoundingCube bc; - BooleanInfo boolcode; -} PolygonStruct; - - -STRUCT { - double coord[3]; - double normal[3]; - float texture[2]; - int polyhedron_number; - int thrown_out; - int old; - int new_index; - int polygon_count; - int* polygons; -} VertexStruct; - - -STRUCT { - char* name; - GLuint gl_display; - BoundingCube bc; - VertexStruct* vertex; - int num_vertices; - PolygonStruct* polygon; - int num_polygons; - int num_removed_vertices; - int num_removed_polygons; - float tx, ty, tz; - float sx, sy, sz; - double transform_matrix[4][4]; - SBoolean selected; - int selected_vertex; - int selected_polygon; - int selected_edge; - DrawingMode drawmode; - int material; - SBoolean show_normals; - VertexStruct* undeformed; - GLuint texture; -} PolyhedronStruct; - - -STRUCT { - char* name; - SBoolean defined_yet; - SBoolean defined_in_file; - SBoolean ambient_defined; - GLfloat ambient[4]; - SBoolean diffuse_defined; - GLfloat diffuse[4]; - SBoolean specular_defined; - GLfloat specular[4]; - SBoolean shininess_defined; - GLfloat shininess; - SBoolean emission_defined; - GLfloat emission[4]; - SBoolean alpha_defined; - GLuint normal_list; - GLuint highlighted_list; -} MaterialStruct; - - -STRUCT { - char* name; - char* filename; - char* texture_filename; - char* texture_coords_filename; - PolyhedronStruct* wobj; - DrawingMode drawmode; - SBoolean selected; - int material; - DMatrix transform; - long drawmodemenu; -} WorldObject; - - -STRUCT { - char *name; /* name of point */ - int segment; /* index of segment point is on */ - int floorSegment; /* index of segment floor is attached to */ - double point[3]; /* xyz coordinates of the point */ - double friction; /* coefficient of friction with floor */ - double param_a; /* parameters */ - double param_b; - double param_c; - double param_d; - double param_e; - double param_f; - SBoolean visible; /* is spring point visible? */ - SBoolean active; /* is spring point active? - not used yet */ - double radius; /* radius used to display/draw point */ -} SpringPoint; - -STRUCT { - char *name; /* name of floor */ - char *filename; /* filename for floor */ - PolyhedronStruct *poly; /* polygons in floor */ - SBoolean visible; /* is floor visible? */ - int numPoints; - int pointArraySize; - SpringPoint **points; /* list of points used with floor */ -} SpringFloor; - -STRUCT { - char* name; - char* filename; - PolyhedronStruct* poly; - SBoolean visible; -} ContactObject; - - -STRUCT { - char* body1; - char* body2; - double restitution; - double mu_static; - double mu_dynamic; -} ContactPair; - - -STRUCT { - char* name; - int numElements; - char** element; -} ContactGroup; - -STRUCT { - char* name; - double offset[3]; - double weight; - SBoolean visible; - SBoolean selected; - SBoolean fixed; /* is marker not solved for when using OrthoTrak algorithms? */ -} Marker; - -STRUCT { - double accuracy; - smSolverMethod method; - int max_iterations; - smBoolean joint_limits; - smBoolean orient_body; - smBoolean fg_contact; -} SolverOptions; - -STRUCT { - int mode; - SBoolean draw_world_objects; - SBoolean draw_bones; - SBoolean draw_muscles; - SBoolean draw_selection_box; - SkinMode skin_mode; - int field1; - int field2; -} ModelDrawOptions; - - -STRUCT { - char* name; /* name of segment */ - int numBones; /* number of bone files */ - int boneArraySize; /* size of bone[] array */ - PolyhedronStruct* bone; /* list of bones for this segment */ - SBoolean defined; /* is segment defined in joint file? */ - SBoolean shadow; /* draw shadow of this bone? */ - SBoolean draw_axes; /* draw axes along with segment? */ - SBoolean shadow_color_spec; /* was shadow color specified in file? */ - SBoolean show_masscenter; /* draw masscenter along with bone */ - SBoolean show_inertia; /* draw inertia along with bone */ - double axis_length; /* length of each axis */ - long drawmodemenu; /* pop-up menu for changing draw mode */ - float shadow_scale[3]; /* 1 0 1 means show y-axis shadow */ - GLfloat shadow_trans[3]; /* where to draw shadow */ - ColorRGB shadow_color; /* color of the shadow */ - DrawingMode drawmode; /* how to display this segment */ - double mc_radius; /* radius of sphere for mass display */ - int material; /* material this segment is made of */ - double mass; /* mass of this segment */ - double inertia[3][3]; /* moment of inertia of this segment */ - double masscenter[3]; /* location of mass center */ - SBoolean mass_specified; /* was mass specified in joint file? */ - SBoolean inertia_specified; /* was inertia specified in joint file? */ - SBoolean masscenter_specified; /* was masscenter specified in joint file? */ - float bone_scale[3]; /* bone scale factor for patty */ - Condition ground_condition; /* are matrices currently valid? */ - DMatrix from_ground; /* transformation from ground to this seg */ - DMatrix to_ground; /* transformation from this seg to ground */ -#if ! ENGINE - GLfloat float_from_ground[16]; /* transformation from ground to this seg */ - GLfloat float_to_ground[16]; /* transformation from this seg to ground */ -#endif - int numgroups; /* number of groups to which segment belongs*/ - int* group; /* list of segment group numbers */ - int numSpringPoints; - int springPointArraySize; - SpringPoint* springPoint; /* spring points for use with spring-based contact det. */ - SpringFloor* springFloor; /* spring floor */ - int numContactObjects; - int contactObjectArraySize; - ContactObject* contactObject; /* array of [rigid-body] contact objects */ - ContactObject* forceMatte; /* force matte (for applying ground forces in DP) */ - int numMarkers; - int markerArraySize; - Marker** marker; /* markers for Solver model */ - double htr_o[3]; /* origin of htr segment in solver */ - double htr_x[3]; /* direction of htr seg Xaxis in solver */ - double htr_y[3]; /* direction of htr seg Yaxis in solver */ -#if INCLUDE_BONE_EDITOR_EXTRAS - char* pts_file; - int num_raw; - double* raw_vertices; -#endif - int num_deforms; - int deform_obj_array_size; - DeformObject* deform; -#if INCLUDE_MOCAP_MODULE - double lengthstart[3]; /* starting point of reference for calcing [HTR] segment length */ - double lengthend[3]; /* ending point of reference for calcing [HTR] segment length */ - SBoolean lengthstartend_specified; /* whether or not lengthstart and lengthend were specified in file */ - char* gait_scale_segment; /* name of segment in OrthoTrak model to compare length to */ - double gait_scale_factor[3]; /* scale of this segment, is compared to size of gait_scale_segment */ - char* mocap_segment; /* associated mocap segment name */ - int mocap_seg_index; /* associated mocap segment index */ - int mocap_scaling_method; /* method used to scale this segment */ - char* mocap_scale_chain_end1; /* distal segment to use for scaling (SIMM or Mocap) */ - char* mocap_scale_chain_end2; /* distal segment to use for scaling (Mocap or SIMM) */ - DMatrix mocap_adjustment_xform; /* SIMM-to-mocap segment transformation */ -#endif -} SegmentStruct; /* properties of a segment */ - - -enum { INHERIT_SCALE, SCALE_ONE_TO_ONE, SCALE_CHAIN, NUM_SCALING_METHODS }; - - -STRUCT { - char* name; /* name as dof appears in SD/FAST code */ - char* con_name; /* if constrained, additional name */ - double initial_value; /* value of dof at start of simulation */ - SBoolean constrained; /* is this dof constrained by a joint func? */ - SBoolean fixed; /* is this dof fixed (prescribed in SD/FAST)? */ - int state_number; /* element of state vector that holds this dof */ - int error_number; /* if constrained, index into error array */ - int joint; /* SD/FAST joint which contains this dof */ - int axis; /* axis in SD/FAST joint which cor. to this dof*/ - double conversion; /* for printing (RTOD for rots, 1.0 for trans) */ - double conversion_sign; /* set to -1 if DOF has - slope function */ -} SDDof; /* holds stuff for SD/FAST */ - - -STRUCT { - char* name; /* name of gencoord */ - double value; /* current value of this gencoord */ - double velocity; /* current velocity of this gencoord */ - GencoordType type; /* type of gencoord (rotation, translation) */ - SBoolean wrap; /* can values wrap across range boundaries? */ - double tolerance; /* tolerance for changing gencoord value */ - SBoolean clamped; /* clamp gencoord within range of motion? */ - SBoolean clamped_save; /* saved value of clamped state */ - SBoolean locked; /* lock gencoord at its current value */ - SBoolean locked_save; /* saved value of locked state */ - int numjoints; /* number of joints this gencoord is in */ - int* jointnum; /* list of joints this gencoord is in */ - int keys[2]; /* keys to change gencoord's value */ - Range range; /* range of acceptable values */ - SBoolean defined; /* is gencoord defined in joint file? */ - SBoolean find_moment_arm; /* used when calculating moment arms in PM */ - double default_value; /* initial value from joints file */ - SBoolean default_value_specified; /* was default_value specified in joint file? */ - SBoolean used_in_model; /* is this gencoord used in the model? */ - dpFunction* restraint_function; /* torque func that restrains gc throughout ROM */ - int restraint_sdcode_num; /* func number in SD/FAST code */ - SBoolean restraintFuncActive; /* is restraint function active? */ - dpFunction* min_restraint_function; /* torque func that restrains gc at min ROM */ - int min_restraint_sdcode_num; /* func number in SD/FAST code */ - dpFunction* max_restraint_function; /* torque func that restrains gc at max ROM */ - int max_restraint_sdcode_num; /* func number in SD/FAST code */ - int numgroups; /* number of groups to which gencoord belongs*/ - int* group; /* list of gencoord group numbers */ -#if INCLUDE_MOCAP_MODULE - char* mocap_segment; /* associated mocap segment name */ - int mocap_seg_index; /* associated mocap segment index */ - int mocap_column; /* associated mocap channel index */ - double mocap_adjust; /* mocap base position value */ -#endif - SBoolean used_in_loop; /* is the gencoord used in a loop? */ - SBoolean used_in_constraint; /* is the gencoord used in a constraint? */ - SBoolean slider_visible; /* is slider visible in the Model Viewer? */ - double pd_stiffness; /* stiffness of gc for inverse simulation */ -} GeneralizedCoord; /* properties of a generalized coordinate*/ - - -STRUCT { - char* name; - int num_gencoords; - int genc_array_size; - GeneralizedCoord** gencoord; -} GencoordGroup; - - -STRUCT { - DofType type; /* type (constant,function) of dof */ - DofKey key; /* one of: tx, ty, tz, r1, r2, r3 */ - double value; /* current value of this dof */ - dpFunction* function; /* if type=function, pointer to constraint function */ - GeneralizedCoord* gencoord; /* if type=function, pointer to gencoord */ - SDDof sd; /* holds stuff for SD/FAST */ -} DofStruct; /* description of a dof */ - - -STRUCT { - char* name; /* name of joint */ - char* sd_name; /* name of joint in SD/FAST code */ - int sd_num; /* number of joint in SD/FAST code */ - dpJointType type; /* type of joint, used in SD/FAST code */ - SBoolean defined; /* has this joint been defined in joint file? */ - char* solverType; /* type of joint, used in Solver code */ - SBoolean user_loop; /* loop joint defined by user */ - SBoolean loop_joint; /* does this joint close a loop? */ - int from; /* starting segment of joint (parent) */ - int to; /* ending segment of joint (child) */ - SBoolean show_axes[3]; /* draw rotation axes in model window? */ - double axis_length[3]; /* length of rotation axis if displayed */ - int order[4]; /* order to perform transformations */ - double parentframe[3][4]; /* reference frame of parent (x,y,z) */ - double parentrotaxes[3][4]; /* rotation axes in parent frame */ - double parentinterframe[3][4]; /* parent axes after a translation */ - double parentinterrotaxes[3][4]; /* rotation axes (parent) after rotations*/ - double childframe[3][4]; /* child axes expressed in parent frame */ - double childrotaxes[3][4]; /* rotation axes expressed in child frame*/ - double childinterframe[3][4]; /* child axes after a translation */ - double childinterrotaxes[3][4]; /* rotation axes (child) after rotations */ - DofStruct dofs[6]; /* equations defining the 6 dofs */ - Conversion conversion; /* transformation matrices for joint */ - double reaction_force[3]; /* joint reaction force */ - int jrf_segment; /* ref frame that reaction force is in */ - SBoolean* in_seg_ground_path; /* is this joint in the path from seg to ground? */ - SBoolean pretransform_active; - Condition pretransform_condition; - DMatrix pretransform; /* optional joint pretransformation */ - DMatrix pretransform_inverse; /* optional joint pretransformation inverse */ -#if INCLUDE_MOCAP_MODULE - char* mocap_segment; /* associated mocap segment name */ - int mocap_seg_index; /* associated mocap segment index */ -#endif -} JointStruct; /* properties of a joint */ - - -STRUCT { - char *name; /* name of constraint point */ - int segment; /* segment the point is attached to */ - double offset[3]; /* point's offset in segment frame from segment origin */ - double weight; /* weight of the point */ - double tolerance; /* tolerance to error between point and surface of constraint */ - // SBoolean visible; /* is the point visible */ - //SBoolean active; /* is the point active */ - SBoolean broken; /* is the point/surface constraint good or broken */ -} ConstraintPoint; - - -STRUCT { - char *name; /* name of constraint object */ - ConstraintObjectType constraintType; /* type of constraint object */ - int segment; /* body segment object is fixed to */ - SBoolean active; /* is the constraint object active? */ - SBoolean visible; /* is the constraint object visible? */ - dpCoord3D radius; /* constraint object radius */ - double height; /* constraint object height (cylinder only) */ - PlaneStruct plane; /* plane parameters (plane only) */ - int constraintAxis; /* which axis to constrain over */ - int constraintSign; /* which side of constraint axis to use */ - dpCoord3D rotationAxis; /* local-to-parent transform parameter */ - double rotationAngle; /* local-to-parent transform parameter */ - dpCoord3D translation; /* local-to-parent transform parameter */ - SBoolean xforms_valid; /* do xform matrices need recalculation? */ - DMatrix from_local_xform; /* consobj-to-parent transform matrix */ - DMatrix to_local_xform; /* parent-to-consobj transform matrix */ - dpCoord3D undeformed_translation; - int cp_array_size; /* size of array of constraint points */ - int numPoints; /* number of constraint points */ - ConstraintPoint *points; /* constraint points */ - int num_qs; /* number of qs affected by constraint */ - int num_jnts; /* number of joints affected by constraint */ - int *joints; /* list of joints affected by constraint */ - GeneralizedCoord** qs; /* list of gencoords affected by constraint */ - long display_list; /* only used for cylinder */ - SBoolean display_list_is_stale; -} ConstraintObject; - - -STRUCT { - char* name; /* name of ligament */ - SBoolean display; /* whether or not to display this ligament */ - SBoolean selected; /* whether or not this ligament is selected */ - double activation; /* for color display */ - int numlines; /* number of ligament lines */ - int line_array_size; /* current size of ligament line array */ - dpMusclePathStruct* line; /* array of lines which define ligament border */ - DrawingMode drawmode; /* how to display this ligament */ - SBoolean show_normals; /* display surface normal vectors? */ - SBoolean wrap_calced; /* has wrapping been calculated? */ -} LigamentStruct; /* properties of a musculotendon unit */ - - -STRUCT{ - double num1[3]; - double num2[3]; - double vec[3]; - SBoolean flag; - char* musc_name; - char* segment_name; -} DglValueStruct; - - -STRUCT { /* ---- motion object record: */ - char* name; /* the motion object's name */ - char* filename; /* the motion object's file name */ - SBoolean defined_in_file; - double startingPosition[3]; - double startingScale[3]; - double startingXYZRotation[3]; - DrawingMode drawmode; /* the motion object's draw mode */ - int vector_axis; /* axis to use for vx,vy,vz vector animation */ - char* materialname; - int material; /* the motion object's starting material */ - PolyhedronStruct shape; /* the motion object's polyhedron */ -} MotionObject; - -enum { /* ---- motion object channel componant ids: */ - MO_TX, MO_TY, MO_TZ, /* translation or position */ - MO_VX, MO_VY, MO_VZ, /* vector */ - MO_SX, MO_SY, MO_SZ, /* scale */ - MO_CR, MO_CG, MO_CB, /* color */ - MO_X, MO_Y, MO_Z /* axis */ -}; - -ENUM { - UnknownMotionObject = 0, - MarkerMotionObject, - ForceMotionObject, - FootPrintObject, - ForcePlateObject -} MotionObjectType; - -STRUCT { /* ---- motion object animation channel record: */ - int component; /* animated componant (see componant ids above) */ - int column; /* motion column index */ -} MotionObjectChannel; - - -STRUCT { /* ---- motion object instance record: */ - char* name; - MotionObjectType type; /* marker, force, etc. */ - int object; /* the motion object's definition */ - int segment; /* the body segment to which the motion object is attached */ - int num_channels; /* the number of animated channels for this motion object */ - MotionObjectChannel* channels; /* the motion object's channel descriptions */ - DMatrix currentXform; /* the motion object's current parent-to-local xform */ - MaterialStruct currentMaterial; /* the motion object's current material */ - double current_value; /* */ - DrawingMode drawmode; /* the motion object's current draw mode */ - SBoolean visible; - GLuint aux_display_obj; -} MotionObjectInstance; - - -STRUCT { - double current_value; /* */ - int current_frame; /* */ - double** gencoords; /* */ - double** genc_velocities; /* */ - int num_motion_object_instances; - MotionObjectInstance** motion_object_instance; - double** muscles; /* for activation levels of muscles */ - double** ligaments; /* for activation levels of ligaments */ - double** other_data; /* */ - long curvemenu; /* parent menu for data columns (plot curves) */ - long gencoordmenu; /* popup menu of gencoord data columns */ - long musclemenu; /* popup menu of muscle activations */ - long forceplatemenu; /* popup menu of force plate components */ - long segmentforcemenu; /* popup menu of segment forces */ - long markermenu; /* popup menu of markers */ - long motionobjectmenu; /* popup menu of motion objects */ - long othermenu; /* popup menu of other data columns */ - SBoolean show_markers; /* */ - SBoolean show_marker_trails; /* */ - SBoolean show_forces; /* */ - SBoolean show_force_trails; /* */ - SBoolean show_foot_prints; /* */ - SBoolean show_force_plates; -} MotionModelOptions; /* */ - - -STRUCT { - char* name; /* name of motion */ - SBoolean used; /* is this motion structure being used? */ - SBoolean wrap; /* can values wrap across motion range? */ - SBoolean is_realtime; /* should this motion receive realtime data? */ - SBoolean sliding_time_scale; /* scroll the time scale during realtime import? */ - SBoolean calc_derivatives; /* should SIMM calculate genc_vels? */ - SBoolean enforce_loops; /* enforce loops while playing motion? */ - SBoolean enforce_constraints; /* enforce constraints while playing motion? */ - char** deriv_names; /* names of SIMM-calculated derivs */ - double** deriv_data; /* SIMM-caluclated derivs of motion data */ - double time_step; /* time between steps in motion */ - int keys[2]; /* keys to move model through motion */ - int number_of_datarows; /* number of rows of data */ - int number_of_datacolumns; /* number of columns of data */ - double min_value; /* min value (time step) of motion range */ - double max_value; /* max value (time step) of motion range */ - char* units; /* units of motion range */ - SBoolean show_cursor; /* track motion with cursor in plot windows? */ - GLfloat cursor_color[3]; /* color of cursor in plot windows */ - MotionModelOptions mopt; /* some properties that are tied to a specific model */ - char** columnname; /* names of the columns of data */ - double** motiondata; /* the actual motion data */ - double** data_std_dev; /* standard deviations */ - int num_events; /* number of motion events */ - smMotionEvent* event; /* array of motion events */ - GLfloat event_color[3]; /* color for event lines and text in plots */ - int realtime_circular_index; /* magical index used to continuously add realtime samples */ - int simulation_index; /* index of latest results from a simulation DLL */ - smAxes walk_direction; /* for gait motions */ - smAxes vertical_direction; /* primarily for gait motions */ - int time_column; /* index of column that contains time values */ - int x_column; /* index of column that serves as X variable (time, percent, etc.) */ -} MotionSequence; /* holds a motion sequence, from a file */ - - -ENUM { - XVAR_GENCOORD_TYPE, /* gencoord, a model's degree of freedom */ - XVAR_MOTION_CURVE_TYPE, /* a single data column from a motion sequence */ - XVAR_MOTION_TYPE /* motion (gencoords taken from motion sequence) */ -} XvarType; /* type of x-var used in plotting */ - - -ENUM { - YVAR_MOMENT_TYPE, /* y-var is moment or moment arm */ - YVAR_MOTION_CURVE_TYPE, /* a single data column from a motion sequence */ - YVAR_OTHER_TYPE /* y-var is anything else */ -} YvarType; /* type of y-var used in plotting */ - - -STRUCT { - XvarType type; /* type of xvar (GENCOORD_TYPE, MOTION_TYPE) */ - double start; /* starting value of x-variable for curve */ - double end; /* ending value of x-variable for curve */ - MotionSequence* motion; /* if xvar is a motion, pointer to the motion */ - struct ModelStruct* ms; /* and model structures are stored here. */ - GeneralizedCoord* gencoord; /* gencoord number, if xvar is a gencoord */ - int motion_column; /* index of data column, if xvar is a motion curve */ - char name[150]; /* name of x variable */ -} XvarStruct; /* contains plotmaker xvar information */ - - -STRUCT { - YvarType type; /* type of yvar (MOMENT_TYPE, OTHER_TYPE) */ - int yvariable; /* the y variable chosen for plotting */ - GeneralizedCoord* gencoord; /* if MOMENT_TYPE, the gencoord to use */ - MotionSequence* motion; /* if yvar is a motion curve, pointer to the motion */ - int motion_column; /* if a motion variable, the motion column */ - double min; /* minimum value for clipping */ - double max; /* maximum value for clipping */ - char name[150]; /* name of y variable */ - int* musc; /* list of muscle states (on or off) */ - int* musc_index; - int nummuscles; /* number of muscles in list */ - int muscle_array_size; - double activation; /* activation level for all muscles */ - double stepsize; /* number of degrees between data points */ - SBoolean active; /* include active force? */ - SBoolean passive; /* include passive force? */ - SBoolean sum; /* whether or not to sum muscle curves */ - SBoolean rectify; /* whether or not to rectify curve data */ - SBoolean override_activation; /* whether or not to use global activation */ - double scale; /* scale Y-values by this amount */ - double offset; /* offset Y-values by this amount */ - int change_segment; /* segment for muscle orientations */ - SBoolean isometric; /* compute only isometric force? */ -} YvarStruct; /* contains plotmaker yvar information */ - - -STRUCT { - char* name; /* */ - int color; /* color of this curve */ - XvarStruct xvar; /* contains xvar information */ - YvarStruct yvar; /* contains yvar information */ - void *modelPtr; /* model used to make this curve */ - int numvalues; /* */ - void* xvalues; /* was double* */ - void* yvalues; /* was double* */ - void* y_std_dev; /* standard deviations (was double*) */ - SBoolean selected; /* */ - void* musc_values; /* was DglValueStruct* */ - int realtime_circular_index; /* index used to circularize curve arrays */ - int realtime_update_index; /* index used to update curves in realtime */ - int simulation_index; /* index of latest frame from simulation DLL */ - int num_events; /* number of events in array */ - smMotionEvent* event; /* array of motion events, if curve is from a motion */ - GLfloat event_color[3]; /* color for displaying event lines and text */ -} CurveStruct; /* */ - - -STRUCT { - char* name; - double x_coord; - GLfloat color[3]; -} PlotFileEvent; - - -STRUCT { - int plotnum; /* */ - int numcurves; /* */ - SBoolean zoomed_yet; /* */ - struct pks* plotkey; /* */ - CurveStruct* curve[MAX_PLOT_CURVES]; /* */ - IntBox datavp; /* */ - Ortho dataortho; /* */ - Ortho savedataortho; /* */ - double xmin; /* */ - double xmax; /* */ - double ymin; /* */ - double ymax; /* */ - char xformat[6]; /* */ - char yformat[6]; /* */ - char *title; /* */ - char *xname; /* */ - char *yname; /* */ - int title_len; /* */ - int xname_len; /* */ - int yname_len; /* */ - int numxticks; /* */ - int numyticks; /* */ - double xinc; /* */ - double yinc; /* */ - double xstart; /* */ - double ystart; /* */ - int numpoints; /* */ - XYCoord pt[30]; /* */ - int key_window_x; /* x loc. of plot key when plot is iconified */ - int key_window_y; /* y loc. of plot key when plot is iconified */ - double cursor_x; /* x location of cursor for tracking motion */ - GLfloat cursor_color[3]; /* color of cursor */ - MotionSequence* cursor_motion; /* motion that is tied to this cursor */ - void *cursor_modelPtr; /* model whose motion is tied to this cursor */ - SBoolean show_cursor; /* */ - SBoolean show_events; /* */ - SBoolean needs_bounding; - int num_file_events; /* number of plot file events */ - PlotFileEvent* file_event; /* events read in from a plot file */ -} PlotStruct; /* */ - - -STRUCT pks { - int plotnum; /* */ - int numcurves; /* */ - PlotStruct* plot; /* */ -} PlotKeyStruct; - - -STRUCT { - int null_material; /* */ - int default_bone_material; /* */ - int default_world_object_material;/* */ - int default_muscle_min_material; /* */ - int default_muscle_max_material; /* */ - int def_muscle_point_material; /* */ - int sel_muscle_point_material; /* */ - int bone_outline_material; /* */ - int highlighted1_polygon_material; /* */ - int highlighted2_polygon_material; /* */ - int marker_material; /* */ - int sel_marker_material; /* */ - int cp_current_sel_ok_material; /* */ - int cp_current_sel_broken_material; /* */ - int cp_current_ok_material; /* */ - int cp_current_broken_material; /* */ - int cp_current_inactive_material; - int cp_back_ok_material; - int cp_back_broken_material; - int cp_back_inactive_material; - int co_current_active_material; - int co_current_inactive_material; - int co_back_active_material; - int co_back_inactive_material; -// int co_broken_material; - int masscenter_material1; - int masscenter_material2; - int num_materials; /* */ - int material_array_size; /* */ - MaterialStruct* materials; /* */ -} ModelMaterials; - - -STRUCT { - int default_view; /* which of the saved views is the default */ - int num_file_views; /* number of views defined in joints file */ - double saved_view[MAXSAVEDVIEWS][4][4]; /* array of saved view transformations */ - char* view_name[MAXSAVEDVIEWS]; /* names of saved views */ - SBoolean view_used[MAXSAVEDVIEWS]; /* has this view been saved by user? */ - long view_menu; - long maindrawmodemenu; /* */ - long allsegsdrawmodemenu; /* */ - long allligsdrawmodemenu; /* */ - long allworlddrawmodemenu; /* */ - long alldrawmodemenu; /* */ - long eachsegdrawmodemenu; /* */ - SBoolean continuous_motion; /* animate the model continuously? */ - unsigned display_animation_hz; /* bit flags for displaying animation hz */ - clock_t time_of_last_frame; /* time that previous frame of motion was displayed */ - SBoolean display_motion_info; /* display info about current motion? */ - MotionSequence* applied_motion; /* motion that is currently applied to model */ - MotionSequence* current_motion; /* motion linked to hot keys, continuous motion */ - int muscle_array_size; /* */ - int nummuscleson; /* */ - int* muscleson; /* */ - MuscleMenu mgroup[GROUPBUFFER]; /* */ - int menucolumns[COLUMNBUFFER]; /* */ - long muscle_cylinder_id; /* */ -#if INCLUDE_MSL_LENGTH_COLOR - double muscle_color_factor; /* controls muscle length colorization */ -#endif - double motion_speed; /* */ - SBoolean show_selected_coords; /* */ - SBoolean show_all_muscpts; /* */ - SBoolean fast_muscle_drawing; - SBoolean show_highlighted_polygon;/* */ - SBoolean show_shadow; /* */ - GLfloat background_color[3]; /* */ - GLfloat crosshairs_color[3]; /* */ - GLfloat vertex_label_color[3]; /* */ - GLfloat rotation_axes_color[3]; /* */ - SBoolean background_color_spec; /* */ - SBoolean crosshairs_color_spec; /* */ - SBoolean vertex_label_color_spec; /* */ - SBoolean rotation_axes_color_spec;/* */ - SelectedPolygon hpoly; /* */ - float muscle_point_radius; /* */ - long muscle_point_id; /* */ - int numdevs; /* */ - int* devs; /* button nums to control joint movement */ - int* dev_values; /* */ - ModelMaterials mat; -} ModelDisplayStruct; - -enum { - SNAPSHOT_INACTIVE, - SNAPSHOT_INSTANT, - SNAPSHOT_CONTINUOUS, - SNAPSHOT_MOTION -}; - -enum { - MAKEMOVIE_INACTIVE, - MAKEMOVIE_CONTINUOUS, - MAKEMOVIE_MOTION -}; - -STRUCT { - int shadem; /* */ - float tx, ty, tz; /* */ - short rx, ry, rz; /* */ - int nummuscleson; /* */ - int musclepts; /* */ - int showpoly; /* */ - SelectedPolygon hpoly; /* */ - SBoolean marker_visibility; -} SaveDisplay; /* */ - - -STRUCT { - char* name; /* joint name, not used yet */ - int order[4]; /* transformation order */ - DofStruct dofs[6]; /* the six dofs (rx, ry, rz, tx, ty, tz) */ - double parentrotaxes[3][4]; /* rotation axes in parent frame */ -} SaveJoints; /* holds a copy of a joint */ - -STRUCT { - char *name; /* */ - double mass; /* mass of this segment */ - double inertia[3][3]; /* moment of inertia of this segment */ - double masscenter[3]; /* location of mass center */ - SBoolean mass_specified; /* was mass specified in joint file? */ - SBoolean inertia_specified; /* was inertia specified in joint file? */ - SBoolean masscenter_specified; /* was masscenter specified in joint file? */ - SBoolean show_masscenter; - SBoolean show_inertia; - int numSpringPoints; - SpringPoint* springPoint; /* spring points for use with spring-based contact det. */ - SpringFloor* springFloor; /* spring floor */ - int numContactObjects; - ContactObject* contactObject; /* array of [rigid-body] contact objects */ - ContactObject* forceMatte; /* force matte (for applying ground forces in DP) */ -} SaveSegments; /* holds a copy of a segment */ - -STRUCT { - double value; - double velocity; - double tolerance; - double pd_stiffness; - double default_value; -// SBoolean default_value_specified; - SBoolean restraintFuncActive; - SBoolean clamped; - SBoolean locked; - Range range; - dpFunction* restraint_function; - SBoolean used_in_model; -} SaveGencoords; - - -STRUCT { - char* name; - int segment; - double offset[3]; - double weight; - SBoolean visible; - SBoolean selected; -} SaveMarker; - -STRUCT { - int muscle; - int musc_wrap_index; - dpWrapObject* wrap_object; - int start_pt; - int end_pt; - int wrap_algorithm; -} MuscWrapAssoc; - -STRUCT { - int constraint_obj; - int numPoints; - ConstraintPoint *savedPoints; -} SaveConstraintPointAssoc; - -STRUCT { - int numsavedjnts; /* */ - int numsavedgencs; /* */ - int numsavedbones; /* */ - int numsavedsegments; - int numsavedmuscgroups; - SaveSegments *segment; - SaveJoints* joint; /* */ - SaveGencoords *gencoord; - dpFunction** function; /* */ - SaveDisplay disp; /* */ - MuscleGroup *muscgroup; - int num_wrap_objects; - dpWrapObject* wrap_object; - int num_muscwrap_associations; - MuscWrapAssoc* muscwrap_associations; - int num_deforms; - DeformObject* deform; - int num_markers; /* */ - SaveMarker* marker; /* */ - int num_conspt_associations; - SaveConstraintPointAssoc *conspt_associations; - int num_constraint_objects; - ConstraintObject *constraintobj; -} ModelSave; /* */ - - -STRUCT { - char* name; /* */ - char* forceUnits; /* units of muscle force */ - char* lengthUnits; /* length units of model */ - char* HTRtranslationUnits; /* units for HTR output, used in Solver only */ - SBoolean is_demo_model; /* is this the demo model? */ - SBoolean useIK; /* */ - SBoolean defaultGCApproved; /* has user approved current default GC values? */ - SBoolean defaultLoopsOK; /* are the loops closed in the default configuration? */ - SBoolean defaultConstraintsOK; /* are the constraints satisfied for the default values? */ - SBoolean constraintsOK; /* are the constraints satisfied in current configuration? */ - SBoolean loopsOK; /* are the loops closed in the current configuration? */ - int modelnum; /* */ - int numgencoords; /* */ - int numunusedgencoords; /* */ - int nummuscles; /* number of muscles in this model */ - int numligaments; /* number of ligaments in this model */ - int numjoints; /* */ - int numsegments; /* */ - int numgroups; /* number of muscle groups */ - int numseggroups; /* */ - int numgencgroups; /* */ - int numworldobjects; /* */ - int numclosedloops; /* */ -// int dis_muscle_array_size; - int muscle_array_size; /* current size of dpMuscleStruct array */ - int ligament_array_size; /* current size of LigamentStruct array */ - int muscgroup_array_size; /* current size of MuscleGroup array */ - int seggroup_array_size; /* current size of SegmentGroup array */ - int gencgroup_array_size; /* current size of GencoordGroup array */ - int genc_array_size; /* current size of GeneralizedCoord array */ - int segment_array_size; /* current size of SegmentStruct array */ - int joint_array_size; /* current size of JointStruct array */ - int func_array_size; /* current size of dpFunction array */ - int currentframe; /* */ - int pushedframe; /* */ - int longest_genc_name; /* length, in pixels, of longest gencoord name */ - int longest_dynparam_name; /* length, in pixels, of longest gencoord name */ - int** pathptrs; /* */ - int ground_segment; /* index of the ground segment */ - int initial_ground_segment; /* index of the segment which starts as ground */ - int* segment_drawing_order; /* order in which to draw the segments */ - SBoolean specified_min_thickness; /* did user specify min_thickness of def musc? */ - SBoolean specified_max_thickness; /* did user specify max_thickness of def musc? */ - long musclegroupmenu; /* pop-up muscle group menu */ - long jointmenu; /* pop-up joint menu */ - long xvarmenu; /* pop-up x-var menu (gencoords and motions) */ - long gencoordmenu; /* pop-up gencoord menu */ - long gencoordmenu2; /* pop-up gencoord menu */ - long gencoord_group_menu; /* pop-up gencoord group menu */ - long momentgencmenu; /* gencoord menu used for moment y-var */ - long momentarmgencmenu; /* gencoord menu used for moment arm y-var */ - long momentarmnumgencmenu; /* gencoord menu used for moment arm y-var */ - long maxmomentgencmenu; /* gencoord menu used for maxmoment y-var */ - long segmentmenu; /* pop-up menu of segment names */ - long motionplotmenu; /* pop-up menu of motions linked to this model */ - long motionwithrealtimemenu; /* pop-up menu of motions, with RT option, no column data */ - long motionmenu; /* pop-up menu of motions, no column data */ - long material_menu; /* pop-up menu of materials */ - long markerMenu; /* pop-up menu of segment-based marker links */ - long functionMenu; /* pop-up menu of functions */ - double max_dimension; /* size of largest dimension of model (segments only) */ - double max_dimension2; /* size of largest dimension of model (segments + world objects) */ - double max_diagonal; /* diagonal dimension of scene bounding box (segments + world objects) */ - SBoolean max_diagonal_needs_recalc;/* do we need to recompute the model's max diagonal? */ - SliderArray gencslider; /* array of gencoord sliders */ - Form gencform; /* form for gencoords and current values */ - CheckBoxPanel gc_chpanel; /* checkbox panel for gencoord clamp/unclamp */ - CheckBoxPanel gc_lockPanel; /* checkbox panel for gc lock/unlock */ - Form dynparamsform; - int dynparamsSize; - char* jointfilename; /* file used to generate joints, segments */ - char* bonepathname; /* directory used to find bones (optional) */ - char* musclefilename; /* file used to generate the muscles */ - char* motionfilename[100]; /* motion files specified within joints file */ - char* mocap_dir; /* folder with static pose that scaled this model */ - int num_motion_files; /* number of motion files named in joints file */ - WorldObject* worldobj; /* */ - int world_array_size; /* */ - JointStruct* joint; /* */ - SegmentStruct* segment; /* */ - MuscleGroup* muscgroup; /* array of muscle group menus */ - SegmentGroup* seggroup; /* array of segment groups */ - GencoordGroup* gencgroup; /* array of gencoord groups */ - GeneralizedCoord** gencoord; /* */ - dpFunction** function; /* functions used for dofs and gencoords and muscle points */ - dpMuscleStruct** muscle; /* array of pointers to muscles */ - dpMuscleStruct* default_muscle; /* */ - LigamentStruct* ligament; /* array of ligament structures */ - ModelDisplayStruct dis; /* */ - ModelSave save; /* */ - TextLine genc_help[2*GENBUFFER]; /* help text for controlling gencs with keys*/ - SBoolean dynamics_ready; /* were all required dynamics params specified? */ - int numContactPairs; /* */ - int contactPairArraySize; /* */ - ContactPair* contactPair; /* */ - int numContactGroups; /* */ - int contactGroupArraySize; /* */ - ContactGroup* contactGroup; /* */ - int num_motion_objects; - MotionObject* motion_objects; - int num_wrap_objects; - int wrap_object_array_size; - dpWrapObject** wrapobj; - int num_deformities; - int deformity_array_size; - Deformity* deformity; - struct loopstruct* loop; - SBoolean GEFuncOK; - SBoolean marker_visibility; - double marker_radius; - int constraint_object_array_size; - int num_constraint_objects; - ConstraintObject *constraintobj; -// double constraint_tolerance; - double loop_tolerance; - double loop_weight; - SolverOptions solver; - smAxes gravity; - SBoolean global_show_masscenter; - SBoolean global_show_inertia; - int num_motions; - int motion_array_size; - MotionSequence** motion; -#if ! ENGINE - glut_mutex_t modelLock; -#endif - SBoolean realtimeState; -} ModelStruct; - -STRUCT { - int window_index; /* index into array of all SIMM windows */ - int window_glut_id; /* identifier returned by glueOpenWindow() */ - int scene_num; - int num_models; - ModelStruct** model; /* models in this scene */ - int windowX1, windowY1; /* coordinates of lower left corner of model window */ - int windowHeight, windowWidth; /* height and width of model window */ - float tx, ty, tz; /* */ - float start_tx; /* */ - float start_ty; /* */ - float start_tz; /* */ - short rx, ry, rz; /* */ - ModelStruct* camera_segment_model; - int camera_segment; /* the segment the camera is attached to (or -1) */ - double max_diagonal; - double transform_matrix[4][4]; /* */ - GLfloat z_vector[3]; /* world coords of vector out of screen */ - GLint viewport[4]; /* viewport as returned by OpenGL */ - GLdouble modelview_matrix[16]; /* model view matrix as returned by OpenGL */ - GLdouble projection_matrix[16]; /* projection matrix as returned by OpenGL */ - GLdouble fov_angle; /* */ - GLdouble aspect_ratio; /* */ - GLdouble near_clip_plane; /* */ - GLdouble far_clip_plane; /* */ - double x_zoom_constant; /* */ - double y_zoom_constant; /* */ - double model_move_increment; /* */ - SBoolean show_crosshairs; /* */ - SBoolean trackball; /* */ - GLfloat background_color[3]; /* */ - GLfloat crosshairs_color[3]; /* */ - int snapshot_mode; - int snapshot_counter; - char* snapshot_file_suffix; - SBoolean snapshot_include_depth; - int movie_mode; - char* movie_file; -} Scene; - -typedef struct loopstruct { - ModelStruct *ms; - int model_num; - GeneralizedCoord* locked_q; - double q_value; - int num_qs; - int num_resids; - GeneralizedCoord** qs; - int *segs; - int *joints; - int num_segs; - int num_jnts; - SBoolean first_iter; -} LoopStruct; - -STRUCT { - ModelStruct *ms; - SBoolean *loopUsed; - GeneralizedCoord** gencoord_list; - GeneralizedCoord* controlled_gc; - double controlled_value; - SBoolean first_iter; -} IKStruct; - -STRUCT { - ModelStruct *model; - GeneralizedCoord** gencoord_list; - GeneralizedCoord* controlled_gc; - double controlled_value; - SBoolean first_iter; - SBoolean test; - double tolerance; - SBoolean *consUsed; -} ConstraintSolverStruct; - -STRUCT { - ModelStruct *model; - SBoolean first_iter; - GeneralizedCoord* controlled_gc; - double controlled_value; - int numQs; - GeneralizedCoord** gencoord_list; - IKStruct *loopInfo; - ConstraintSolverStruct *constraintInfo; - int numConstraintQs; - int numConstraintResids; - int numLoopQs; - int numLoopResids; - SBoolean largeGenCoordResids; - SBoolean loopClosed; - SBoolean constraintClosed; -} LCStruct; - - -ENUM { - no_tri, - simple_tri, - complex_tri, - hyper_complex_tri -} TriangulateOption; - - -ENUM { - x_ordering, - y_ordering, - z_ordering, - no_ordering -} OrderFormat; - - -STRUCT { - SBoolean verbose_output; - FileType output_format; - const char* output_dir; - SBoolean write_separate_polyhedra; - SBoolean convexify_polygons; - SBoolean remove_collinear; - SBoolean clip_vertices; - SBoolean tol_box_set; - SBoolean fill_holes; - TriangulateOption triangulate; - VertexOrder vertex_order; - int vertex_offset; - double vertex_tolerance; - double max_edge_length; - double tol_x_min; - double tol_x_max; - double tol_y_min; - double tol_y_max; - double tol_z_min; - double tol_z_max; - double clip_x_min; - double clip_x_max; - double clip_y_min; - double clip_y_max; - double clip_z_min; - double clip_z_max; - double reference_normal[3]; -} NormOptions; - -STRUCT { - char* name; - double mass; - double mass_center[3]; - double inertia[3][3]; - double body_to_joint[3]; - double inboard_to_joint[3]; - int simm_segment; -} SDSegment; - -STRUCT { - SBoolean used; - Direction dir; - char* inbname; - char* outbname; - SBoolean closes_loop; -} JointSDF; - -STRUCT { - SBoolean used; - int times_split; - double mass_factor; -} SegmentSDF; - -STRUCT { - SBoolean preserveMassDist; -} ScaleModelOptions; - -STRUCT { - Scene* scene; - ModelStruct* model; - int pan_mx_old; - int pan_my_old; - double pan_wx_old; - double pan_wy_old; - double pan_wz_old; -} MoveObjectTracker; - -STRUCT { - Scene* scene; - ModelStruct* model; - PickIndex object; -} SelectedObject; - -#endif /*MODELPLOT_H*/ - diff --git a/OpenSim/Utilities/simmToOpenSim/modelutils.c b/OpenSim/Utilities/simmToOpenSim/modelutils.c deleted file mode 100644 index 4d19f4d8ed..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/modelutils.c +++ /dev/null @@ -1,559 +0,0 @@ -/******************************************************************************* - - MODELUTILS.C - - Author: Peter Loan - - Date: 03-JAN-91 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: - - Routines: - delete_model : deletes a model - check_gencoord_wrapping : checks boundaries of gencoord value - makegencform : makes the gencoord form for a model - getjointvarnum : given a dof name, returns a dof number - getjointvarname : given dof number, returns dof name (e.g. tx) - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" - -/*************** DEFINES (for this file only) *********************************/ - - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static void delete_display_lists(ModelStruct* model); -static void destroy_model_menus(ModelStruct* model); - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ - -#if OPENSMAC -#undef ENGINE -#define ENGINE 1 -#endif - -#if ! ENGINE - -void delete_scene(Scene* scene) -{ - int i; - - // TODO_SCENE - delete_model(scene->model[0]); - - FREE_IFNOTNULL(scene->model); - FREE_IFNOTNULL(scene->snapshot_file_suffix); - FREE_IFNOTNULL(scene->movie_file); - free(scene); - - for (i=0; imodelLock); - - glutSetWindow(scene->window_glut_id); - - /* Post the MODEL_DELETED event before deleting the motions - * (which will post MOTION_DELETED events for each one). This - * requires more checking by each tool when handling a - * MOTION_DELETED event (to see if the model still exists or - * not), but is more efficient because the tool will not - * update itself for each motion deleted and then update itself - * again when the whole model is deleted. - * The model pointer is included in the MODEL_DELETED event so - * that the tools can check to see if they are currently set to - * the deleted model. But the modelnum is also needed to index - * into the array of tool/model options (e.g., meop, pmop). - */ - make_and_queue_simm_event(MODEL_DELETED, ms->modelnum, ms, NULL, ZERO, ZERO); - - for (i = 0; i < ms->motion_array_size; i++) - { - if (ms->motion[i]) - { - delete_motion(ms, ms->motion[i], no); - FREE_IFNOTNULL(ms->motion[i]); - } - } - - destroy_model_menus(ms); - - // In order to delete the window from the GUI as quickly as possible, - // only the tasks that must be done before the window is - // removed are done before calling delete_window(). This - // means that deleting the bone display lists, which used - // to be done inside free_model(), is done separately, here. - // These display lists can only be deleted when the window - // in which they were created is current. - delete_display_lists(ms); - - delete_window(scene->window_glut_id); - - sprintf(buf, "Deleted model %s.", ms->name); - - purge_simm_events_for_struct(scene, SCENE_INPUT_EVENT); - purge_simm_events_for_model(ms->modelnum, MODEL_ADDED); - - free_model(ms->modelnum); - - root.nummodels--; - - updatemodelmenu(); - - message(buf, 0, DEFAULT_MESSAGE_X_OFFSET); -} - -static void delete_display_lists(ModelStruct* model) -{ - int i, j; - Scene* scene = get_first_scene_containing_model(model); - if (scene && scene->window_glut_id != -1) - { - int savedID = glutGetWindow(); - glutSetWindow(scene->window_glut_id); - - // Delete the muscle display lists. - glDeleteLists(model->dis.muscle_cylinder_id, 1); - glDeleteLists(model->dis.muscle_point_id, 1); - - // Delete the bone, springfloor, contact object, and force matte display lists. - for (i=0; inumsegments; i++) - { - if (model->segment[i].defined == yes) - { - for (j=0; jsegment[i].numBones; j++) - delete_polyhedron_display_list(&model->segment[i].bone[j], NULL); - for (j=0; jsegment[i].numContactObjects; j++) - delete_polyhedron_display_list(model->segment[i].contactObject[j].poly, NULL); - if (model->segment[i].springFloor) - delete_polyhedron_display_list(model->segment[i].springFloor->poly, NULL); - if (model->segment[i].forceMatte) - delete_polyhedron_display_list(model->segment[i].forceMatte->poly, NULL); - } - } - - // Delete the wrap object display lists. - for (i=0; inum_wrap_objects; i++) - glDeleteLists(model->wrapobj[i]->display_list, 1); - - // Delete the world object display lists. - for (i=0; inumworldobjects; i++) - delete_polyhedron_display_list(model->worldobj[i].wobj, NULL); - - // Delete the constraint object display lists. - for (i=0; inum_constraint_objects; i++) - glDeleteLists(model->constraintobj[i].display_list, 1); - - // Delete the materials. - for (i=0; idis.mat.num_materials; i++) - { - if (model->dis.mat.materials[i].normal_list) - glDeleteLists(model->dis.mat.materials[i].normal_list,1); - if (model->dis.mat.materials[i].highlighted_list) - glDeleteLists(model->dis.mat.materials[i].highlighted_list,1); - FREE_IFNOTNULL(model->dis.mat.materials[i].name); - } - FREE_IFNOTNULL(model->dis.mat.materials); - - glutSetWindow(savedID); - } -} - -static void destroy_model_menus(ModelStruct* model) -{ - int i; - - glutDestroyMenu(model->musclegroupmenu); - glutDestroyMenu(model->jointmenu); - glutDestroyMenu(model->xvarmenu); - glutDestroyMenu(model->gencoordmenu); - glutDestroyMenu(model->gencoordmenu2); - if (model->gencoord_group_menu) - glutDestroyMenu(model->gencoord_group_menu); - glutDestroyMenu(model->momentgencmenu); - glutDestroyMenu(model->momentarmgencmenu); - glutDestroyMenu(model->momentarmnumgencmenu); - glutDestroyMenu(model->maxmomentgencmenu); - glutDestroyMenu(model->segmentmenu); - glutDestroyMenu(model->motionplotmenu); - glutDestroyMenu(model->motionmenu); - glutDestroyMenu(model->motionwithrealtimemenu); - glutDestroyMenu(model->material_menu); - if (model->markerMenu) - glutDestroyMenu(model->markerMenu); - - for (i = 0; i < model->numsegments; i++) - glutDestroyMenu(model->segment[i].drawmodemenu); - - for (i = 0; i < model->numseggroups; i++) - glutDestroyMenu(model->seggroup[i].displaymodemenu); - - for (i = 0; i < model->numworldobjects; i++) - glutDestroyMenu(model->worldobj[i].drawmodemenu); - - glutDestroyMenu(model->dis.view_menu); - glutDestroyMenu(model->dis.allsegsdrawmodemenu); - glutDestroyMenu(model->dis.allligsdrawmodemenu); - glutDestroyMenu(model->dis.allworlddrawmodemenu); - glutDestroyMenu(model->dis.alldrawmodemenu); - if (model->dis.eachsegdrawmodemenu > 0) - glutDestroyMenu(model->dis.eachsegdrawmodemenu); - glutDestroyMenu(model->dis.maindrawmodemenu); -} - -#endif /* ENGINE */ - - -double check_gencoord_wrapping(GeneralizedCoord* gc, double change) -{ - double new_value, range; - - new_value = gc->value + change; - - /* If the gencoord is not allowed to wrap, then clamp - * it within its range of motion. You do not need to - * check if the gencoord is unclamped because that - * variable is only relevant when reading gencoord values - * from a motion file or typing them into the gencoord - * field in the ModelViewer (and in neither case is this - * function called). - */ - if (gc->wrap == no) - { - if (new_value < gc->range.start) - return (gc->range.start); - else if (new_value > gc->range.end) - return (gc->range.end); - else - return (new_value); - } - - range = gc->range.end - gc->range.start; - - while (new_value > gc->range.end) - new_value -= range; - - while (new_value < gc->range.start) - new_value += range; - - return (new_value); -} - - -#if ! ENGINE - -/* MAKEGENCFORM: this routine makes the form for changing gencoord values. - * It also makes the checkbox panel for setting clamp/unclamp - * for the gencoords and lock/unlock for the gencoords. - */ - -ReturnCode makegencform(ModelStruct* ms) -{ - - int i, name_len; - Form* form; - CheckBoxPanel* check; - - form = &ms->gencform; - form->numoptions = ms->numgencoords; - form->selected_item = -1; - form->cursor_position = 0; - form->highlight_start = 0; - form->title = NULL; - - ms->longest_genc_name = 0; - - form->option = (FormItem*)simm_malloc(form->numoptions*sizeof(FormItem)); - if (form->option == NULL) - return code_bad; - - for (i=0; inumoptions; i++) - { - form->option[i].justify = yes; - form->option[i].active = yes; - form->option[i].visible = yes; - form->option[i].editable = yes; - form->option[i].use_alternate_colors = no; - form->option[i].decimal_places = 3; - form->option[i].data = NULL; - mstrcpy(&form->option[i].name, ms->gencoord[i]->name); - name_len = glueGetStringWidth(root.gfont.defaultfont, ms->gencoord[i]->name); - if (name_len > ms->longest_genc_name) - ms->longest_genc_name = name_len; - SET_BOX1221(form->option[i].box,0,75,-FORM_FIELD_YSPACING*i, - form->option[i].box.y2-FORM_FIELD_HEIGHT); - } - - /* make the gencoord checkbox panel */ - check = &ms->gc_chpanel; - check->title = "C"; - check->type = normal_checkbox; - check->numoptions = ms->numgencoords; - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - if (ms->gencoord[i]->clamped == no) - check->checkbox[i].state = off; - else - check->checkbox[i].state = on; - check->checkbox[i].name = NULL; - check->checkbox[i].box.x1 = form->option[i].box.x1; - check->checkbox[i].box.x2 = check->checkbox[i].box.x1 + CHECKBOX_XSIZE*2/3; - check->checkbox[i].box.y1 = form->option[i].box.y1; - check->checkbox[i].box.y2 = check->checkbox[i].box.y1 + CHECKBOX_YSIZE*2/3; - check->checkbox[i].use_alternate_colors = no; - } - - /* make the gencoord lock checkbox panel */ - check = &ms->gc_lockPanel; - check->title = "L"; //NULL; - check->type = normal_checkbox; - check->numoptions = ms->numgencoords; - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - if (ms->gencoord[i]->locked == no) - check->checkbox[i].state = off; - else - check->checkbox[i].state = on; - check->checkbox[i].name = NULL; - check->checkbox[i].box.x1 = form->option[i].box.x1; - check->checkbox[i].box.x2 = check->checkbox[i].box.x1 + CHECKBOX_XSIZE*2/3; - check->checkbox[i].box.y1 = form->option[i].box.y1; - check->checkbox[i].box.y2 = check->checkbox[i].box.y1 + CHECKBOX_YSIZE*2/3; - } - - return code_fine; - -} - - -ReturnCode make_dynparams_form(ModelStruct* ms) -{ - int i, name_len, num_options; - Form* form; - - if (ms->default_muscle) - num_options = ms->default_muscle->num_dynamic_params + 1; // one extra for muscle model - else - num_options = 1; // one extra for muscle model - - form = &ms->dynparamsform; - form->numoptions = num_options; - form->selected_item = -1; - form->cursor_position = 0; - form->highlight_start = 0; - form->title = NULL; - - ms->longest_dynparam_name = 0; - - form->option = (FormItem*)simm_malloc(form->numoptions*sizeof(FormItem)); - if (form->option == NULL) - return code_bad; - - ms->dynparamsSize = 0; - for (i=0; inumoptions; i++) - { - form->option[i].justify = yes; - form->option[i].active = yes; - form->option[i].visible = yes; - form->option[i].editable = yes; - form->option[i].use_alternate_colors = no; - form->option[i].data = NULL; - if (i == num_options - 1) - { - form->option[i].decimal_places = 0; - mstrcpy(&form->option[i].name, "muscle_model"); - } - else - { - form->option[i].decimal_places = 3; - mstrcpy(&form->option[i].name, ms->default_muscle->dynamic_param_names[i]); - } - name_len = glueGetStringWidth(root.gfont.defaultfont, form->option[i].name); - if (name_len > ms->longest_dynparam_name) - ms->longest_dynparam_name = name_len; - SET_BOX1221(form->option[i].box, 0, 90,-FORM_FIELD_YSPACING*i, - form->option[i].box.y2-FORM_FIELD_HEIGHT); - ms->dynparamsSize = form->option[i].box.y2 - FORM_FIELD_HEIGHT; - } - - ms->longest_dynparam_name -= 30; - - return code_fine; -} - -#endif /* ENGINE */ - - -/* GETJOINTVARNUM: */ - -int getjointvarnum(char string[]) -{ - - if (STRINGS_ARE_EQUAL(string,"r1")) - return (0); - if (STRINGS_ARE_EQUAL(string,"r2")) - return (1); - if (STRINGS_ARE_EQUAL(string,"r3")) - return (2); - if (STRINGS_ARE_EQUAL(string,"tx")) - return (3); - if (STRINGS_ARE_EQUAL(string,"ty")) - return (4); - if (STRINGS_ARE_EQUAL(string,"tz")) - return (5); - if (STRINGS_ARE_EQUAL(string,"order")) - return (6); - if (STRINGS_ARE_EQUAL(string,"axis1")) - return (7); - if (STRINGS_ARE_EQUAL(string,"axis2")) - return (8); - if (STRINGS_ARE_EQUAL(string,"axis3")) - return (9); - if (STRINGS_ARE_EQUAL(string,"segments")) - return (10); - - return (-1); - -} - - - -/* GETJOINTVARNAME: */ - -char* getjointvarname(int num) -{ - - switch (num) - { - case 0: - return ("r1"); - case 1: - return ("r2"); - case 2: - return ("r3"); - case 3: - return ("tx"); - case 4: - return ("ty"); - case 5: - return ("tz"); - case 6: - return ("order"); - case 7: - return ("axis1"); - case 8: - return ("axis2"); - case 9: - return ("axis3"); - case 10: - return ("name"); - default: - return (""); - } - -} - - -#if ! ENGINE - -SBoolean isVisible(double pt[]) -{ - if (pt[0] >= UNDEFINED_DOUBLE) - return no; - if (pt[1] >= UNDEFINED_DOUBLE) - return no; - if (pt[2] >= UNDEFINED_DOUBLE) - return no; - return yes; -} - -void hack_tool_updates(ModelStruct* model, int model_index) -{ - int i; - SimmEvent se; - - /* Hack: update all the tools so they know the model was deleted. - * This is done in cases where the updates must happen immediately, - * rather than just pushing a SIMM event on the queue. - */ - se.event_code = MODEL_DELETED; - se.model_index = model_index; - se.struct_ptr1 = (void*)model; - se.struct_ptr2 = NULL; - se.field1 = ZERO; - se.field2 = ZERO; - se.mouse_x = 0; - se.mouse_y = 0; - se.key_modifiers = 0; - se.window_id = -1; - se.object = 0; - - for (i = 0; i < TOOLBUFFER; i++) - { - if (tool[i].used == yes && (se.event_code & tool[i].simm_event_mask) == se.event_code) - (*tool[i].simm_event_handler)(se); - } -} - -#endif /* ENGINE */ diff --git a/OpenSim/Utilities/simmToOpenSim/motion.c b/OpenSim/Utilities/simmToOpenSim/motion.c deleted file mode 100644 index ff60b01412..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/motion.c +++ /dev/null @@ -1,2412 +0,0 @@ -/******************************************************************************* - - MOTION.C - - Author: Peter Loan - - Date: 10-SEP-90 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: - - Routines: - apply_motion_to_model : moves a model according to a motion - link_model_to_motion : checks motion column names - check_motion_wrapping : checks boundaries of motion variable - find_nth_motion : returns the appropriate motion - -*******************************************************************************/ -#include - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "normio.h" -#include "normtools.h" -//#include "password.h" - -/*************** DEFINES (for this file only) *********************************/ -#define MOTION_OFFSET 0.0001 -#define OTHER_NAMES_SIZE 3000 - -typedef struct { - int segment; - ContactObject* forceMatte; - double pointOfApplication[3]; - double force[3]; - double freeTorque[3]; - double magnitude; - int column[9]; -} ForceMatteForce; - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -// default motion objects -static MotionObject sDefaultMotionObjects[] = { - { - "force" , /* name */ - "arrow.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1.0, 0.002, 1.0 }, /* startingScale (see NOTE below for more info) */ - { 0,0,0 }, /* starting XYZRotation */ - flat_shading, /* drawmode */ - YY, /* vector_axis */ - "def_motion" /* materialname */ - }, - { - "ball" , /* name */ - "unit_sphere.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1,1,1 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - gouraud_shading, /* drawmode */ - YY, /* vector_axis */ - "def_motion" /* materialname */ - }, - { - "contact" , /* name */ - "arrow.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1.0, 1.0, 1.0 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - flat_shading, /* drawmode */ - YY, /* vector_axis */ - "def_motion" /* materialname */ - }, - { - "joint_force" , /* name */ - "arrow.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1.0, 1.0, 1.0 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - flat_shading, /* drawmode */ - YY, /* vector_axis */ - "def_joint_vector" /* materialname */ - }, - { - "joint_torque" , /* name */ - "arrow.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1.0, 1.0, 1.0 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - flat_shading, /* drawmode */ - YY, /* vector_axis */ - "def_joint_vector" /* materialname */ - }, - { - "spring_force" , /* name */ - "arrow.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 0.25, 1.0, 0.25 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - flat_shading, /* drawmode */ - YY, /* vector_axis */ - "def_muscle_point" /* materialname */ - }, - { - "torque" , /* name */ - "arrow.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 0.0, 0.0, 0.0 }, /* startingScale---------> free torques in OpenSim MOT files map to this object, */ - { 0,0,0 }, /* starting XYZRotation but you normally don't want to see them. So make the */ - flat_shading, /* drawmode starting scale 0, 0, 0. */ - YY, /* vector_axis */ - "def_joint_vector" /* materialname */ - }, - { - "rightFootPrint", /* name */ - "rightFootPrint.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1.0, 1.0, 1.0 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - solid_fill, /* drawmode */ - YY, /* vector_axis */ - "foot_print_mat" /* materialname */ - }, - { - "leftFootPrint", /* name */ - "leftFootPrint.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1.0, 1.0, 1.0 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - solid_fill, /* drawmode */ - YY, /* vector_axis */ - "foot_print_mat" /* materialname */ - }, - { - "forceplate", /* name */ - "forceplate.asc", /* filename */ - no, /* defined_in_file */ - { 0,0,0 }, /* startingPosition */ - { 1.0, 1.0, 1.0 }, /* startingScale */ - { 0,0,0 }, /* starting XYZRotation */ - outlined_polygons, /* drawmode */ - YY, /* vector_axis */ - "def_world_object" /* materialname */ - } -}; -/* NOTE: the starting scale factors (0.75, 0.002, 0.75) for the default "force" - * arrow motion object were chosen to match SIMM's previous behavior. -- KMS 12/20/99 - * NOTE: arrow.asc was changed to include the X and Z 0.75 factors. Thus the - * "force" scale factor was changed to 1.0, 0.002, 1.0 -- JPL 9/6/00 - */ - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ -extern char natural_cubic_text[]; -extern char gcv_text[]; - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void update_current_motion_frame(MotionSequence* motion); -static SBoolean link_model_to_motion(ModelStruct* model, MotionSequence* motion, - int* num_other_data, char* otherNames); -static MotionObjectInstance* store_motion_object_instance(ModelStruct *model, MotionModelOptions* mopt, - int seg, int motion_object, int compenant, int column); -static void add_vector(double pt[], double vec[], double freeTorque[], ForceMatteForce* fmf); -static void transformMotion(MotionSequence* newMotion, MotionSequence* oldMotion, - ModelStruct* model, ForceMatteForce forces[], int numForceMattes); -static SBoolean columnIsGroundForce(MotionSequence* motion, ModelStruct* model, int index); -static int column_is_marker_data(MotionSequence* motion, int column); - - -/* ------------------------------------------------------------------------- - add_default_motion_objects - ----------------------------------------------------------------------------- */ -void add_default_motion_objects(ModelStruct* model) -{ - int i, n = sizeof(sDefaultMotionObjects) / sizeof(MotionObject); - - for (i = 0; i < n; i++) - { - MotionObject* mo; - ReturnCode rc; - - int j = model->num_motion_objects++; - - if (model->motion_objects == NULL) - model->motion_objects = (MotionObject*) simm_malloc(model->num_motion_objects * sizeof(MotionObject)); - else - model->motion_objects = (MotionObject*) simm_realloc(model->motion_objects, - model->num_motion_objects * sizeof(MotionObject), &rc); - if (model->motion_objects == NULL) - { - model->num_motion_objects = 0; - return; - } - mo = &model->motion_objects[j]; - - memset(mo, 0, sizeof(MotionObject)); - - mstrcpy(&mo->name, sDefaultMotionObjects[i].name); - mstrcpy(&mo->filename, sDefaultMotionObjects[i].filename); - rc = lookup_polyhedron(&mo->shape, mo->filename, model); - mstrcpy(&mo->materialname, sDefaultMotionObjects[i].materialname); - mo->material = enter_material(model, mo->materialname, declaring_element); - mo->drawmode = sDefaultMotionObjects[i].drawmode; - mo->vector_axis = sDefaultMotionObjects[i].vector_axis; - - for (j = 0; j < 3; j++) - { - mo->startingPosition[j] = sDefaultMotionObjects[i].startingPosition[j]; - mo->startingScale[j] = sDefaultMotionObjects[i].startingScale[j]; - mo->startingXYZRotation[j] = sDefaultMotionObjects[i].startingXYZRotation[j]; - } - } -} - - -int get_motion_number(ModelStruct* model, MotionSequence* motion) -{ - int i; - - if (model && motion) - { - for (i = 0; i < model->motion_array_size; i++) - { - if (model->motion[i] == motion) - return i; - } - } - - return -1; -} - - -MotionSequence* get_motion_by_name(ModelStruct* model, const char name[]) -{ - if (model) - { - int i; - - for (i=0; imotion_array_size; i++) - { - if (model->motion[i] && STRINGS_ARE_EQUAL(model->motion[i]->name, name)) - return model->motion[i]; - } - } - - return NULL; -} - - -int get_motion_frame_number(MotionSequence* motion, double value) -{ - double percent; - int frame; - - percent = (motion->mopt.current_value - motion->min_value) / - (motion->max_value - motion->min_value); - - frame = percent * (motion->number_of_datarows - 1) + MOTION_OFFSET; - - return frame; -} - -SBoolean add_motion_to_model(MotionSequence* motion, ModelStruct* model, SBoolean showTopLevelMessages) -{ - int i, j, name_len, num_others, motion_num; - char otherNames[OTHER_NAMES_SIZE]; - IntBox bbox; - Form* form; - ReturnCode rc = code_fine, rc2 = code_fine; - SliderArray* sa; - - motion->mopt.current_value = motion->min_value; - motion->mopt.current_frame = 0; - - motion->mopt.gencoords = (double**)simm_malloc(model->numgencoords * sizeof(double*)); - motion->mopt.genc_velocities = (double**)simm_malloc(model->numgencoords * sizeof(double*)); - motion->mopt.muscles = (double**)simm_malloc(model->nummuscles * sizeof(double*)); - motion->mopt.ligaments = (double**)simm_malloc(model->numligaments * sizeof(double*)); - if ((motion->mopt.gencoords == NULL && model->numgencoords > 0) || - (motion->mopt.muscles == NULL && model->nummuscles > 0) || - (motion->mopt.ligaments == NULL && model->numligaments > 0)) - { - error(none,"Cannot link motion to model."); - return no; - } - - NULLIFY_STRING(otherNames); - - if (link_model_to_motion(model, motion, &num_others, otherNames) == no) - { - FREE_IFNOTNULL(motion->mopt.gencoords); - FREE_IFNOTNULL(motion->mopt.genc_velocities); - FREE_IFNOTNULL(motion->mopt.muscles); - FREE_IFNOTNULL(motion->mopt.ligaments); - FREE_IFNOTNULL(motion->mopt.other_data); - - if (motion->mopt.motion_object_instance) - { - for (j = 0; j < motion->mopt.num_motion_object_instances; j++) - free_motion_object_instance(motion->mopt.motion_object_instance[j], model); - } - FREE_IFNOTNULL(motion->mopt.motion_object_instance); - motion->mopt.num_motion_object_instances = 0; - - return no; - } - - /* Make sure the motion has a unique name */ - for (i = 0; i < model->motion_array_size; i++) - { - if (model->motion[i] && model->motion[i] != motion && - STRINGS_ARE_EQUAL(model->motion[i]->name, motion->name)) - break; - } - if (i < model->motion_array_size) - { - (void)sprintf(buffer,"%s (%d)", motion->name, get_motion_number(model, motion) + 1); - FREE_IFNOTNULL(motion->name); - mstrcpy(&motion->name, buffer); - } - - // Sort the events by x_coord. - sort_events(motion, NULL); - - motion_num = get_motion_number(model, motion); -#if ! OPENSMAC && ! ENGINE - make_marker_trails(model, motion); - make_force_trails(model, motion); - - make_motion_curve_menu(model, motion, motion_num); - - // Before adding the first motion, remove the "none loaded" entry. - if (model->num_motions == 1) - { - glueRemoveMenuItem(model->motionplotmenu, 1); - glueRemoveMenuItem(model->motionmenu, 1); - } - glueAddSubmenuEntry(model->motionplotmenu, motion->name, motion->mopt.curvemenu); - glueAddMenuEntryWithValue(model->motionmenu, motion->name, motion_num); - glueAddMenuEntryWithValue(model->motionwithrealtimemenu, motion->name, motion_num); - - /* Add a field to the gencoord form */ - form = &model->gencform; - form->option = (FormItem*)simm_realloc(form->option, (form->numoptions+1)*sizeof(FormItem), &rc); - if (rc == code_bad) - { - error(none,"Cannot link motion to the model."); - return no; - } - - mstrcpy(&form->option[form->numoptions].name, motion->name); - SET_BOX1221(form->option[form->numoptions].box,0,75, - -FORM_FIELD_YSPACING*form->numoptions, - form->option[form->numoptions].box.y2-FORM_FIELD_HEIGHT); - storeDoubleInForm(&form->option[form->numoptions], motion->mopt.current_value, 3); - form->option[form->numoptions].justify = yes; - form->option[form->numoptions].active = yes; - form->option[form->numoptions].visible = yes; - form->option[form->numoptions].editable = yes; - form->option[form->numoptions].use_alternate_colors = no; - form->option[form->numoptions].decimal_places = 3; - form->option[form->numoptions].data = motion; - form->numoptions++; - name_len = glueGetStringWidth(root.gfont.defaultfont, motion->name); - if (name_len > model->longest_genc_name) - model->longest_genc_name = name_len; - - /* Add a slider to the gencoord slider array */ - sa = &model->gencslider; - sa->sl = (Slider*)simm_realloc(sa->sl,(sa->numsliders+1)*sizeof(Slider),&rc); - if (rc == code_bad) - { - error(none,"Cannot link motion to the model."); - return no; - } - - SET_BOX1221(bbox, 0, 170 + 2 * FORM_FIELD_HEIGHT, -FORM_FIELD_YSPACING * sa->numsliders, - bbox.y2 - FORM_FIELD_HEIGHT); - make_slider(&sa->sl[sa->numsliders], horizontal_slider, bbox, FORM_FIELD_HEIGHT, - motion->min_value, motion->min_value, motion->max_value, - (motion->max_value - motion->min_value)/ - (motion->number_of_datarows - 1), NULL, motion); - sa->numsliders++; - - reposition_gencoord_sliders(model); - - glueAddMenuEntryWithValue(model->xvarmenu, motion->name, model->numgencoords + motion_num); - model->dis.devs = (int*)simm_realloc(model->dis.devs, (model->dis.numdevs + 2) * sizeof(int), &rc); - model->dis.dev_values = (int*)simm_realloc(model->dis.dev_values, (model->dis.numdevs + 2) * sizeof(int), &rc2); - if (rc == code_bad || rc2 == code_bad) - { - error(none,"Cannot link motion to the model."); - return no; - } - - model->dis.devs[model->dis.numdevs++] = motion->keys[0]; - model->dis.devs[model->dis.numdevs++] = motion->keys[1]; - - if (showTopLevelMessages == yes) - { - (void)sprintf(buffer,"Linked motion %s to model %s (%d \'other\' data columns).", - motion->name, model->name, num_others); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); - if (num_others > 0) - { - simm_printf(no, "The following have been loaded as \'otherdata\' in motion %s:\n", motion->name); - simmPrintMultiLines(otherNames, no, 80, 50); - } - } - - make_and_queue_simm_event(MOTION_ADDED, model->modelnum, model->motion[motion_num], NULL, ZERO, ZERO); - -#endif /* !OPENSMAC && ! ENGINE */ - - return yes; -} - -static SBoolean link_model_to_motion(ModelStruct* model, MotionSequence* motion, int* num_other_data, char* otherNames) -{ - int i, j, ref, seg, musc_index, lig_index, cur_len = 1; - dpFunctionType sType; - double cutoffFreq; - SBoolean full = no; - char* ptr = NULL; - GeneralizedCoord* gencoord = NULL; - - // In case this function is being called on a motion already added to a model - // (e.g., after normalizing or smoothing), free the pointers that are about to - // be re-allocated. - for (i=0; imopt.num_motion_object_instances; ) - { - if (motion->mopt.motion_object_instance[i]->num_channels > 0) - { - free_motion_object_instance(motion->mopt.motion_object_instance[i], model); - for (j=i; jmopt.num_motion_object_instances - 1; j++) - motion->mopt.motion_object_instance[j] = motion->mopt.motion_object_instance[j+1]; - motion->mopt.num_motion_object_instances--; - } - else - { - i++; - } - } - if (motion->mopt.num_motion_object_instances == 0) - FREE_IFNOTNULL(motion->mopt.motion_object_instance); - - FREE_IFNOTNULL(motion->mopt.other_data); - *num_other_data = 0; - - for (i = 0; i < model->numgencoords; i++) - { - motion->mopt.gencoords[i] = NULL; - motion->mopt.genc_velocities[i] = NULL; - } - - for (i = 0; i < model->nummuscles; i++) - motion->mopt.muscles[i] = NULL; - - for (i = 0; i < model->numligaments; i++) - motion->mopt.ligaments[i] = NULL; - - // Since you don't yet know how many columns are other data, make room - // for the maximum number, and realloc later. - motion->mopt.other_data = (double**)simm_malloc(motion->number_of_datacolumns*sizeof(double*)); - if (motion->mopt.other_data == NULL) - { - error(none,"Cannot link motion to the model."); - return no; - } - - for (i = 0; i < motion->number_of_datacolumns; i++) - motion->mopt.other_data[i] = NULL; - - for (i = 0; i < motion->number_of_datacolumns; i++) - { - int motion_object; - double cutoffFreq; - dpFunctionType sType; - - if ((gencoord = name_is_gencoord(motion->columnname[i], model, NULL, &sType, &cutoffFreq, yes))) - motion->mopt.gencoords[getGencoordIndex(model, gencoord)] = motion->motiondata[i]; - else if ((gencoord = name_is_gencoord(motion->columnname[i], model, "_vel", &sType, &cutoffFreq, yes))) - motion->mopt.genc_velocities[getGencoordIndex(model, gencoord)] = motion->motiondata[i]; - else if ((musc_index = name_is_muscle(model, motion->columnname[i], NULL, &sType, &cutoffFreq, yes)) >= 0) - motion->mopt.muscles[musc_index] = motion->motiondata[i]; - else if ((lig_index = getLigamentIndex(model, motion->columnname[i])) >= 0) - motion->mopt.ligaments[lig_index] = motion->motiondata[i]; - else if ((seg = name_is_marker(model, motion->columnname[i], &motion_object, &ref, &sType, &cutoffFreq, yes)) >= 0 && motion_object >= 0 && ref >= 0) - { - MotionObjectInstance* mi = store_motion_object_instance(model, &motion->mopt, seg, motion_object, ref, i); - mi->type = MarkerMotionObject; - if (mi->name == NULL) - { - char* p = NULL; - strcpy(buffer, motion->columnname[i]); - p = strrchr(buffer, '_'); - if (p) - *p = STRING_TERMINATOR; - mstrcpy(&mi->name, buffer); - } - } - else if ((seg = name_is_forceplate(model, motion->columnname[i], &motion_object, &ref, &sType, &cutoffFreq, yes)) >= 0 && motion_object >= 0 && ref >= 0) - { - MotionObjectInstance* mi = store_motion_object_instance(model, &motion->mopt, seg, motion_object, ref, i); - mi->type = ForceMotionObject; - if (mi->name == NULL) - { - char* p = NULL; - strcpy(buffer, motion->columnname[i]); - p = strrchr(buffer, '_'); - if (p) - *p = STRING_TERMINATOR; - mstrcpy(&mi->name, buffer); - } - } - else if ((seg = name_is_body_segment(model, motion->columnname[i], &motion_object, &ref, &sType, &cutoffFreq, yes)) >= 0 && motion_object >= 0 && ref >= 0) - { - MotionObjectInstance* mi = store_motion_object_instance(model, &motion->mopt, seg, motion_object, ref, i); - mi->type = ForceMotionObject; - if (mi->name == NULL) - { - char* p = NULL; - strcpy(buffer, motion->columnname[i]); - p = strrchr(buffer, '_'); - if (p) - *p = STRING_TERMINATOR; - mstrcpy(&mi->name, buffer); - } - } - else - { - motion->mopt.other_data[(*num_other_data)++] = motion->motiondata[i]; - // Build up a character string list of 'otherdata' names so you can - // inform the user which ones you found. - if (otherNames) - addNameToString(motion->columnname[i], otherNames, OTHER_NAMES_SIZE); - } - } - - return yes; -} - - -/* ------------------------------------------------------------------------- - name_is_marker - parse the specified 'name' to - discover its encoded marker name and animation component. If there is - a match, use the "ball" motion object and return "ground" as the segment - because marker data in a motion is always w.r.t. ground. ----------------------------------------------------------------------------- */ -int name_is_marker(ModelStruct* model, char name[], int* motion_object, int* component, - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd) -{ - int i, j, seg, maxLen, Mlen = 0, len = strlen(name); - SBoolean foundOne; - char *ptr, *newEnd; - - /* First check to see if the string begins with the name of a marker. - * To handle models which have overlapping marker names, like - * "L.Knee" and "L.Knee.Med", check for a match with all markers, and take - * the longest match. - */ - for (i = 0, foundOne = no, maxLen = -1; i < model->numsegments; i++) - { - for (j = 0; j < model->segment[i].numMarkers; j++) - { - Mlen = strlen(model->segment[i].marker[j]->name); - if (len >= Mlen) - { - if (strings_equal_n_case_insensitive(name, model->segment[i].marker[j]->name, Mlen)) - { - if (Mlen > maxLen) - { - foundOne = yes; - maxLen = Mlen; - } - } - } - } - } - - if (foundOne == no) - return -1; - - /* Move ptr past the marker name. */ - ptr = &name[maxLen]; - len -= maxLen; - - /* The name is of a marker, so assume a segment of "ground" and then - * look for the component name. - */ - seg = model->ground_segment; - - /* The motion object is the pre-defined "ball" object. */ - if (motion_object) - { - *motion_object = -1; - - for (i = 0; i < model->num_motion_objects; i++) - { - if (STRINGS_ARE_EQUAL(model->motion_objects[i].name, "ball")) - { - *motion_object = i; - break; - } - } - if (i == model->num_motion_objects) - return -1; - } - - /* Now look for the component. */ - if (component) - { - *component = -1; - - /* determine the motion component from the next part of the name. */ - if (!strncmp(ptr, "_tx", 3) || !strncmp(ptr, "_px", 3)) - *component = MO_TX; - else if (!strncmp(ptr, "_ty", 3) || !strncmp(ptr, "_py", 3)) - *component = MO_TY; - else if (!strncmp(ptr, "_tz", 3) || !strncmp(ptr, "_pz", 3)) - *component = MO_TZ; - else if (!strncmp(ptr, "_vx", 3)) - *component = MO_VX; - else if (!strncmp(ptr, "_vy", 3)) - *component = MO_VY; - else if (!strncmp(ptr, "_vz", 3)) - *component = MO_VZ; - else if (!strncmp(ptr, "_sx", 3)) - *component = MO_SX; - else if (!strncmp(ptr, "_sy", 3)) - *component = MO_SY; - else if (!strncmp(ptr, "_sz", 3)) - *component = MO_SZ; - else if (!strncmp(ptr, "_cr", 3)) - *component = MO_CR; - else if (!strncmp(ptr, "_cg", 3)) - *component = MO_CG; - else if (!strncmp(ptr, "_cb", 3)) - *component = MO_CB; - else - return -1; - } - else - { - return -1; - } - - /* Move ptr past the component name. */ - ptr += 3; - len -= 3; - - /* Store a pointer to the character right after the component. - * This will be the new end of the string if stripEnd == yes. - */ - newEnd = ptr; - - /* If functionType and cutoffFrequency are not NULL, check to see if the name ends in - * natural_cubic_text or gcv_text (followed by an optional cutoff frequency). If it - * does, set *functionType to the appropriate type. If no function label is found, set - * the type to step_func. - */ - if (functionType && cutoffFrequency) - { - int matched_spl = 0; - int spl_len = strlen(natural_cubic_text); - int gcv_len = strlen(gcv_text); - - *functionType = dpStepFunction; - *cutoffFrequency = -1.0; // by default there is no smoothing - - if (len >= spl_len) - { - if (!strncmp(ptr, natural_cubic_text, spl_len)) - { - *functionType = dpNaturalCubicSpline; - ptr += spl_len; - len -= spl_len; - matched_spl = 1; - } - } - - if (!matched_spl && len >= gcv_len) - { - if (!strncmp(ptr, gcv_text, gcv_len)) - { - ptr += gcv_len; - len -= gcv_len; - *functionType = dpGCVSpline; - if (len > 0) - { - char* intPtr = buffer; - - /* Move over the underscore and look for an integer. */ - if (*(ptr++) != '_') - { - return -1; - } - else - { - len--; /* for the underscore character */ - while (*ptr >= '0' && *ptr <= '9') - { - *(intPtr++) = *(ptr++); - len--; - } - *intPtr = STRING_TERMINATOR; - *cutoffFrequency = atof(buffer); - } - } - } - } - } - - /* If there are extra characters after the suffixes, return an error. */ - if (len > 0) - return -1; - - /* Strip off the text for the function type and cutoff frequency. */ - if (stripEnd == yes) - *newEnd = STRING_TERMINATOR; - - return seg; - -} - - -/* ------------------------------------------------------------------------- - name_is_forceplate - parse the specified 'name' to - discover its encoded forceplate name and animation component. If there is - a match, use the "force" motion object and return "ground" as the segment - because forceplate data in a motion is always w.r.t. ground. ----------------------------------------------------------------------------- */ -int name_is_forceplate(ModelStruct* model, char name[], int* motion_object, int* component, - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd) -{ - int i, j, seg, plateNum, Flen = 0, len = strlen(name); - char *ptr, *newEnd; - - /* If the column name starts with "forceplate" and a number, then - * it is forceplate data. - */ - if (sscanf(name, "forceplate%d", &plateNum) != 1) - return -1; - - /* The name is of a forceplate, so assume a segment of "ground" and then - * look for the component name. - */ - seg = model->ground_segment; - - sprintf(buffer, "forceplate%d", plateNum); - Flen = strlen(buffer); - - /* Move ptr past the forceplate name and number. */ - ptr = &name[Flen]; - len -= Flen; - - /* The motion object is the pre-defined "force" object. */ - if (motion_object) - { - *motion_object = -1; - - for (i = 0; i < model->num_motion_objects; i++) - { - if (STRINGS_ARE_EQUAL(model->motion_objects[i].name, "force")) - { - *motion_object = i; - break; - } - } - if (i == model->num_motion_objects) - return -1; - } - - /* Now look for the component. */ - if (component) - { - *component = -1; - - /* determine the motion component from the next part of the name. */ - if (!strncmp(ptr, "_tx", 3) || !strncmp(ptr, "_px", 3)) - *component = MO_TX; - else if (!strncmp(ptr, "_ty", 3) || !strncmp(ptr, "_py", 3)) - *component = MO_TY; - else if (!strncmp(ptr, "_tz", 3) || !strncmp(ptr, "_pz", 3)) - *component = MO_TZ; - else if (!strncmp(ptr, "_vx", 3)) - *component = MO_VX; - else if (!strncmp(ptr, "_vy", 3)) - *component = MO_VY; - else if (!strncmp(ptr, "_vz", 3)) - *component = MO_VZ; - else if (!strncmp(ptr, "_sx", 3)) - *component = MO_SX; - else if (!strncmp(ptr, "_sy", 3)) - *component = MO_SY; - else if (!strncmp(ptr, "_sz", 3)) - *component = MO_SZ; - else if (!strncmp(ptr, "_cr", 3)) - *component = MO_CR; - else if (!strncmp(ptr, "_cg", 3)) - *component = MO_CG; - else if (!strncmp(ptr, "_cb", 3)) - *component = MO_CB; - else if (!strncmp(ptr, "_torque_x", 9)) // free torque - *component = MO_X; - else if (!strncmp(ptr, "_torque_y", 9)) // free torque - *component = MO_Y; - else if (!strncmp(ptr, "_torque_z", 9)) // free torque - *component = MO_Z; - else - return -1; - } - else - { - return -1; - } - - // Move ptr past the component name. - if (*component == MO_X || *component == MO_Y || *component == MO_Z) - { - ptr += 9; - len -= 9; - } - else - { - ptr += 3; - len -= 3; - } - - /* Store a pointer to the character right after the component. - * This will be the new end of the string if stripEnd == yes. - */ - newEnd = ptr; - - /* If functionType and cutoffFrequency are not NULL, check to see if the name ends in - * natural_cubic_text or gcv_text (followed by an optional cutoff frequency). If it - * does, set *functionType to the appropriate type. If no function label is found, set - * the type to step_func. - */ - if (functionType && cutoffFrequency) - { - int matched_spl = 0; - int spl_len = strlen(natural_cubic_text); - int gcv_len = strlen(gcv_text); - - *functionType = dpStepFunction; - *cutoffFrequency = -1.0; // by default there is no smoothing - - if (len >= spl_len) - { - if (!strncmp(ptr, natural_cubic_text, spl_len)) - { - *functionType = dpNaturalCubicSpline; - ptr += spl_len; - len -= spl_len; - matched_spl = 1; - } - } - - if (!matched_spl && len >= gcv_len) - { - if (!strncmp(ptr, gcv_text, gcv_len)) - { - ptr += gcv_len; - len -= gcv_len; - *functionType = dpGCVSpline; - if (len > 0) - { - char* intPtr = buffer; - - /* Move over the underscore and look for an integer. */ - if (*(ptr++) != '_') - { - return -1; - } - else - { - len--; /* for the underscore character */ - while (*ptr >= '0' && *ptr <= '9') - { - *(intPtr++) = *(ptr++); - len--; - } - *intPtr = STRING_TERMINATOR; - *cutoffFrequency = atof(buffer); - } - } - } - } - } - - /* If there are extra characters after the suffixes, return an error. */ - if (len > 0) - return -1; - - /* Strip off the text for the function type and cutoff frequency. */ - if (stripEnd == yes) - *newEnd = STRING_TERMINATOR; - - return seg; - -} - - -/* ------------------------------------------------------------------------- - motion_object_instance_contains_component - ----------------------------------------------------------------------------- */ -static SBoolean motion_object_instance_contains_component(MotionObjectInstance* mi, int component) -{ - if (mi->channels) - { - int i; - - for (i = 0; i < mi->num_channels; i++) - if (mi->channels[i].component == component) - return yes; - } - return no; -} - -/* ------------------------------------------------------------------------- - store_motion_object_instance - ----------------------------------------------------------------------------- */ -static MotionObjectInstance* store_motion_object_instance(ModelStruct* model, - MotionModelOptions* mopt, int seg, int motion_object, int motion_component, int column) -{ - ReturnCode rc; - int i; - MotionObjectInstance* mi = NULL; - - // Scan the motion's object instance array to see if we need to create - // a new motion object instance, or use an existing one. There could be - // multiple incomplete instances, in which case you want to add the new - // column to the first one that is missing this component. - for (i=0; inum_motion_object_instances; i++) - { - if (mopt->motion_object_instance[i]->segment == seg && - mopt->motion_object_instance[i]->object == motion_object && - motion_object_instance_contains_component(mopt->motion_object_instance[i], motion_component) == no) - { - // You found an existing motion object instance that does not yet have - // this component defined. 'i' is now the index of this instance. - break; - } - } - - if (i == mopt->num_motion_object_instances) - { - // Add a new element to the array of motion object instances. - i = mopt->num_motion_object_instances++; - - if (mopt->motion_object_instance == NULL) - { - mopt->motion_object_instance = (MotionObjectInstance**) - simm_malloc(mopt->num_motion_object_instances * sizeof(MotionObjectInstance*)); - } - else - { - mopt->motion_object_instance = (MotionObjectInstance**) - simm_realloc(mopt->motion_object_instance, - mopt->num_motion_object_instances * sizeof(MotionObjectInstance*), &rc); - } - - // Initialize a new motion object instance. - if (mopt->motion_object_instance) - { - MotionObject* mo = &model->motion_objects[motion_object]; - - mi = mopt->motion_object_instance[i] = (MotionObjectInstance*)simm_calloc(1, sizeof(MotionObjectInstance)); - - mi->type = UnknownMotionObject; - mi->object = motion_object; - mi->segment = seg; - mi->drawmode = mo->drawmode; - mi->current_value = -1.0; - mi->visible = yes; - - mi->currentMaterial.name = NULL; - copy_material(&model->dis.mat.materials[mo->material], &mi->currentMaterial); - } - else - mopt->num_motion_object_instances--; - } - else - mi = mopt->motion_object_instance[i]; - - // Add a new animation channel to the motion object instance. - if (mi) - { - i = mi->num_channels++; - - if (mi->channels == NULL) - { - mi->channels = (MotionObjectChannel*) - simm_malloc(mi->num_channels * sizeof(MotionObjectChannel)); - } - else - { - mi->channels = (MotionObjectChannel*) - simm_realloc(mi->channels, mi->num_channels * sizeof(MotionObjectChannel), &rc); - } - - if (mi->channels) - { - mi->channels[i].component = motion_component; - mi->channels[i].column = column; - } - } - - return mi; -} - - -double check_motion_wrapping(ModelStruct* model, MotionSequence* motion, double change) -{ - double new_value, range; - - if (model == NULL || motion == NULL) - return 0.0; - - new_value = motion->mopt.current_value + change; - - if (motion->wrap == no) - return new_value; - - range = motion->max_value - motion->min_value; - - while (new_value > motion->max_value) - new_value -= range; - - while (new_value < motion->min_value) - new_value += range; - - return new_value; -} - - -/* ------------------------------------------------------------------------- - circularize_motion_index - this interesting routine was added to allow - realtime motion to continuously stream into a fixed-duration - MotionSequence, by wrapping the samples around the end of the buffer. - - -- KMS 2/23/00 ----------------------------------------------------------------------------- */ -static int circularize_motion_index(MotionSequence* motion, int i) -{ - if (motion->is_realtime && motion->realtime_circular_index > 0) - { - i += motion->realtime_circular_index; - - if (i >= motion->number_of_datarows) - i -= motion->number_of_datarows; - } - - return i; -} - -void free_motion(MotionSequence* motion, ModelStruct* model) -{ - if (motion) - { - int i; - - FREE_IFNOTNULL(motion->name); - FREE_IFNOTNULL(motion->units); - - if (motion->motiondata) - { - for (i = 0; i < motion->number_of_datacolumns; i++) - FREE_IFNOTNULL(motion->motiondata[i]); - FREE_IFNOTNULL(motion->motiondata); - } - - if (motion->columnname) - { - for (i = 0; i < motion->number_of_datacolumns; i++) - FREE_IFNOTNULL(motion->columnname[i]); - FREE_IFNOTNULL(motion->columnname); - } - - if (motion->deriv_data) - { - for (i = 0; i < motion->number_of_datacolumns; i++) - FREE_IFNOTNULL(motion->deriv_data[i]); - FREE_IFNOTNULL(motion->deriv_data); - } - - if (motion->deriv_names) - { - for (i = 0; i < motion->number_of_datacolumns; i++) - FREE_IFNOTNULL(motion->deriv_names[i]); - FREE_IFNOTNULL(motion->deriv_names); - } - - if (motion->data_std_dev) - { - for (i = 0; i < motion->number_of_datacolumns; i++) - FREE_IFNOTNULL(motion->data_std_dev[i]); - FREE_IFNOTNULL(motion->data_std_dev); - } - - if (motion->event) - { - for (i = 0; i < motion->num_events; i++) - FREE_IFNOTNULL(motion->event[i].name); - FREE_IFNOTNULL(motion->event); - } - - FREE_IFNOTNULL(motion->mopt.gencoords); - FREE_IFNOTNULL(motion->mopt.genc_velocities); - FREE_IFNOTNULL(motion->mopt.muscles); - FREE_IFNOTNULL(motion->mopt.ligaments); - FREE_IFNOTNULL(motion->mopt.other_data); - - if (motion->mopt.motion_object_instance) - { - for (i = 0; i < motion->mopt.num_motion_object_instances; i++) - free_motion_object_instance(motion->mopt.motion_object_instance[i], model); - FREE_IFNOTNULL(motion->mopt.motion_object_instance); - } - } -} - -int apply_motion_to_model(ModelStruct* model, MotionSequence* motion, double value, SBoolean update_modelview, SBoolean draw_plot) -{ - int i, item, motion_num; - double percent; - - if (!motion) - return 0; - - motion->mopt.current_value = value; - - if (motion->mopt.current_value < motion->min_value) - motion->mopt.current_value = motion->min_value; - else if (motion->mopt.current_value > motion->max_value) - motion->mopt.current_value = motion->max_value; - - if (EQUAL_WITHIN_ERROR(motion->max_value, motion->min_value) || motion->number_of_datarows == 1) - { - motion->mopt.current_value = motion->min_value; - motion->mopt.current_frame = 0; - } - else - { -#if 0 // can't use this method with variable spacing in the X column - percent = (motion->mopt.current_value - motion->min_value) / (motion->max_value - motion->min_value); - - motion->mopt.current_frame = percent * (motion->number_of_datarows - 1) + MOTION_OFFSET; - - motion->mopt.current_value = motion->min_value + motion->mopt.current_frame * - (motion->max_value - motion->min_value) / (motion->number_of_datarows - 1); - - //printf("percent = %lf, frame = %d, value = %lf\n", percent, motion->mopt.current_frame, motion->mopt.current_value); -#else - update_current_motion_frame(motion); - //printf("frame = %d, value = %lf, frameValue = %lf\n", motion->mopt.current_frame, motion->mopt.current_value, motion->motiondata[motion->x_column][motion->mopt.current_frame]); -#endif - } - - motion->mopt.current_frame = circularize_motion_index(motion, motion->mopt.current_frame); - - if ((model->numclosedloops > 0) && (model->useIK == yes) && motion->enforce_loops == yes) - { - LoopStatus loopStatus; - ConstraintStatus consStatus; - /* if the gencoord is in the motion file, set it to the value in the - * motion file. If it's not in the motion file and is in a loop, set it - * to zero. Then invalidate all the joint matrices, and solve all the - * loops. Gencoords get set to whatever new values are calculated by - * the IK solver (may not be the same as those in the motion file. - */ - for (i = 0; i < model->numgencoords; i++) - { - int j; - if (motion->mopt.gencoords[i] != NULL) - model->gencoord[i]->value = motion->mopt.gencoords[i][motion->mopt.current_frame]; - else if (model->gencoord[i]->used_in_loop == yes) - model->gencoord[i]->value = 0.0; - for (j = 0; j < model->gencoord[i]->numjoints; j++) - invalidate_joint_matrix(model, &model->joint[model->gencoord[i]->jointnum[j]]); - } - solveAllLoopsAndConstraints(model, &loopStatus, &consStatus, yes); - if (loopStatus == loopChanged) - { -#if 0 - sprintf(buffer, "Loops solved using motion file values, values changed\n"); - error(none, buffer); -#endif - } - } - else if (model->num_constraint_objects > 0)// && motion->enforce_constraints == yes) - { - ConstraintStatus constraintStatus; - LoopStatus loopStatus; - /* if the gencoord is in the motion file, copy its value into the gc struct. - * then invalidate all the joints that use it. - * Solve all the constraints with the new values. Gencoords are set - * to whatever values are calculated by the constraint solver (may not - * be the values in the motion file. Don't change any values if the - * gencoord is locked. If the constraints are unchanged, the gencoord - * values must be set (set_gencoord_value was not called in solveall. - */ - for (i = 0; i < model->numgencoords; i++) - { - int j; - if ((motion->mopt.gencoords[i] != NULL) && - (model->gencoord[i]->locked == no)) - { - model->gencoord[i]->value = motion->mopt.gencoords[i][motion->mopt.current_frame]; - for (j = 0; j < model->gencoord[i]->numjoints; j++) - invalidate_joint_matrix(model, &model->joint[model->gencoord[i]->jointnum[j]]); - } - } - - solveAllLoopsAndConstraints(model, &loopStatus, &constraintStatus, motion->enforce_constraints);//yes); - if (constraintStatus == constraintUnchanged) - { - for (i = 0; i < model->numgencoords; i++) - { - set_gencoord_value(model, model->gencoord[i], model->gencoord[i]->value, no); - } - } - else if (constraintStatus == constraintChanged) - { -#if 0 - sprintf(buffer, "Constraints solved using motion file values, values changed\n"); - error(none, buffer); -#endif - } - //dkb aug 19, 2002 - for (i = 0; i < model->numgencoords; i++) - { - set_gencoord_value(model, model->gencoord[i], model->gencoord[i]->value, no); - } - } - else - { - /* if solver is off or there are no loops, and there are no constraints, - * set the gencoords to the values in the motion file */ - for (i = 0; i < model->numgencoords; i++) - { - if (motion->mopt.gencoords[i] != NULL) - { - if (model->gencoord[i]->locked == no) - { - set_gencoord_value(model, model->gencoord[i], motion->mopt.gencoords[i][motion->mopt.current_frame], no); - } - else if (update_modelview == yes) - { - /* If the gencoord is locked, but still appears in the motion file, - * print a warning message if the motion file value is not within - * a tolerance equal to the default precision of motion file data. - */ - if (NOT_EQUAL_WITHIN_TOLERANCE(motion->mopt.gencoords[i][motion->mopt.current_frame], model->gencoord[i]->value, 0.000001)) - { - sprintf(errorbuffer, "Gencoord %s is locked at %f\n", - model->gencoord[i]->name, model->gencoord[i]->value); - error(none, errorbuffer); - } - } - } - } - } - -#if ! OPENSMAC && ! ENGINE - for (i=0; inummuscles; i++) - { - if (motion->mopt.muscles[i] != NULL) - model->muscle[i]->dynamic_activation = motion->mopt.muscles[i][motion->mopt.current_frame]; - } - - for (i=0; inumligaments; i++) - { - if (motion->mopt.ligaments[i] != NULL) - model->ligament[i].activation = motion->mopt.ligaments[i][motion->mopt.current_frame]; - } - - for (i=0; inumgencoords; i++) - { - if (motion->mopt.genc_velocities[i] != NULL) - set_gencoord_velocity(model, model->gencoord[i], motion->mopt.genc_velocities[i][motion->mopt.current_frame]); - else - set_gencoord_velocity(model, model->gencoord[i], 0.0); - } - - if (update_modelview == yes) - { - /* Find out which form item is linked to this motion and update it. */ - for (item = 0; item < model->gencform.numoptions; item++) - { - if (model->gencform.option[item].data == motion) - { - storeDoubleInForm(&model->gencform.option[item], motion->mopt.current_value, 3); - break; - } - } - - /* Find out which slider is linked to this motion and update it. */ - for (item = 0; item < model->gencform.numoptions; item++) - { - if (model->gencform.option[item].data == motion) - { - model->gencslider.sl[item].value = motion->mopt.current_value; - break; - } - } - - model->dis.applied_motion = motion; - } - - if (draw_plot == yes && motion->show_cursor == yes) - set_plot_cursors(model, motion); -#endif - - /* If there is no gencoord data in this motion (all columns of - * data in motion file are "othercurves," then return 0, meaning you didn't - * change the model configuration by trying to apply the motion to it. Otherwise - * return 1, so the calling routine knows that it should redraw the model. - * JPL/TODO 7/24/96: This doesn't work since standard deviations have been - * added to motion files, because num_datacolumns gets decremented by num_std_dev, - * but num_other does not (so they may happen to be equal) - - if (motion->number_of_datacolumns == motion->number_of_othercurves) - return 0; - else - */ - - return 1; -} - -static void update_current_motion_frame(MotionSequence* motion) -{ - int i; - - // Current_value has changed. For efficiency, first check to see if it is still - // near current_frame. - if (motion->mopt.current_frame == 0) - { - // See if current value is between frames 0 and 1. - // If it is, figure out which one it is closer to. - double leftDiff = motion->mopt.current_value - motion->motiondata[motion->x_column][0] - TINY_NUMBER; - double rightDiff = motion->motiondata[motion->x_column][1] + TINY_NUMBER - motion->mopt.current_value; - - if (leftDiff >= 0.0 && rightDiff >= 0.0) - { - if (leftDiff < rightDiff) - motion->mopt.current_frame = 0; - else - motion->mopt.current_frame = 1; - return; - } - } - else if (motion->mopt.current_frame == motion->number_of_datarows - 1) - { - // See if current value is between last and second-to-last frames. - // If it is, figure out which one it is closer to. - double leftDiff = motion->mopt.current_value - motion->motiondata[motion->x_column][motion->number_of_datarows - 2] - TINY_NUMBER; - double rightDiff = motion->motiondata[motion->x_column][motion->number_of_datarows - 1] + TINY_NUMBER - motion->mopt.current_value; - - if (leftDiff >= 0.0 && rightDiff >= 0.0) - { - if (leftDiff < rightDiff) - motion->mopt.current_frame = motion->number_of_datarows - 2; - else - motion->mopt.current_frame = motion->number_of_datarows - 1; - return; - } - } - else - { - // See if current value is between current frame and previous frame. - // If it is, figure out which one it is closer to. - int frame = motion->mopt.current_frame; - double leftDiff = motion->mopt.current_value - motion->motiondata[motion->x_column][frame - 1] - TINY_NUMBER; - double rightDiff = motion->motiondata[motion->x_column][frame] + TINY_NUMBER - motion->mopt.current_value; - - if (leftDiff >= 0.0 && rightDiff >= 0.0) - { - if (leftDiff < rightDiff) - motion->mopt.current_frame = frame - 1; - else - motion->mopt.current_frame = frame; - return; - } - - // See if current value is between current frame and next frame. - // If it is, figure out which one it is closer to. - leftDiff = motion->mopt.current_value - motion->motiondata[motion->x_column][frame] - TINY_NUMBER; - rightDiff = motion->motiondata[motion->x_column][frame + 1] + TINY_NUMBER - motion->mopt.current_value; - - if (leftDiff >= 0.0 && rightDiff >= 0.0) - { - if (leftDiff < rightDiff) - motion->mopt.current_frame = frame; - else - motion->mopt.current_frame = frame + 1; - return; - } - } - - // Find the first frame that is greater than current value, then - // figure out whether current value is closer to that frame or the - // previous frame. - for (i=0; inumber_of_datarows; i++) - { - if (motion->motiondata[motion->x_column][i] + TINY_NUMBER >= motion->mopt.current_value) - { - if (i == 0) - { - motion->mopt.current_frame = 0; - return; - } - else - { - double leftDiff = motion->mopt.current_value - motion->motiondata[motion->x_column][i - 1] - TINY_NUMBER; - double rightDiff = motion->motiondata[motion->x_column][i] + TINY_NUMBER - motion->mopt.current_value; - - if (leftDiff >= 0.0 && rightDiff >= 0.0) - { - if (leftDiff < rightDiff) - motion->mopt.current_frame = i - 1; - else - motion->mopt.current_frame = i; - return; - } - } - } - } -} - -ReturnCode write_motion(MotionSequence *motion, const char filename[]) -{ - int i, j; - FILE* file = simm_fopen(filename, "w"); - - if (file == NULL) - return code_bad; - - fprintf(file, "/* %s\n */\n\n", filename); - fprintf(file, "name %s\n", motion->name); - fprintf(file, "datacolumns %d\n", motion->number_of_datacolumns); - fprintf(file, "datarows %d\n", motion->number_of_datarows); - fprintf(file, "x_column %d\n", motion->x_column + 1); - //fprintf(file, "range %lf %f\n", motion->min_value, motion->max_value); - if (motion->units) - fprintf(file, "units %s\n", motion->units); - if (motion->wrap) - fprintf(file, "wrap\n"); - - if (motion->enforce_loops == yes) - fprintf(file, "enforce_loops yes\n"); - else - fprintf(file, "enforce_loops no\n"); - if (motion->enforce_constraints == yes) - fprintf(file, "enforce_constraints yes\n"); - else - fprintf(file, "enforce_constraints no\n"); - - if (motion->show_cursor) - fprintf(file, "cursor %.2f %.2f %.2f\n", motion->cursor_color[0], motion->cursor_color[1], motion->cursor_color[2]); - - for (i = 0; i < motion->num_events; i++) - fprintf(file, "event %lf %s\n", motion->event[i].xCoord, motion->event[i].name); - - fprintf(file, "event_color %f %f %f\n", motion->event_color[0], motion->event_color[1], motion->event_color[2]); - - if (motion->calc_derivatives) - fprintf(file, "calc_derivatives yes\n"); - - for (i=0; imopt.num_motion_object_instances; i++) - { - MotionObjectInstance* mi = motion->mopt.motion_object_instance[i]; - - if (mi->type == FootPrintObject) - { - // The scale factor is the magnitude of any row or column in the rotation matrix. - double eulerRot[3], nvec[3]; - memset(nvec, 0, 3 * sizeof(double)); - extract_xyz_rot_bodyfixed(mi->currentXform, eulerRot); - fprintf(file, "%s %lf %lf %lf %lf %lf %lf %lf\n", mi->name, - normalize_vector(mi->currentXform[0], nvec), // scale factor - mi->currentXform[3][0], mi->currentXform[3][1], mi->currentXform[3][2], // translations - eulerRot[0] * RTOD, eulerRot[1] * RTOD, eulerRot[2] * RTOD); // XYZ Euler rotations - } - else if (mi->type == ForcePlateObject) - { - double eulerRot[3], nvec[3]; - memset(nvec, 0, 3 * sizeof(double)); - extract_xyz_rot_bodyfixed(mi->currentXform, eulerRot); - fprintf(file, "%s", mi->name); - // Scale factors are the magnitudes of the rows. - fprintf(file, " %lf %lf %lf", VECTOR_MAGNITUDE(mi->currentXform[0]), VECTOR_MAGNITUDE(mi->currentXform[1]), - VECTOR_MAGNITUDE(mi->currentXform[2])); - fprintf(file, " %lf %lf %lf", mi->currentXform[3][0], mi->currentXform[3][1], mi->currentXform[3][2]); - fprintf(file, " %lf %lf %lf\n", eulerRot[0] * RTOD, eulerRot[1] * RTOD, eulerRot[2] * RTOD); - } - } - - fprintf(file, "endheader\n\n"); - - for (j = 0; j < motion->number_of_datacolumns; j++) - fprintf(file, "%s\t", motion->columnname[j]); - fprintf(file, "\n"); - - for (i = 0; i < motion->number_of_datarows; i++) - { - for (j = 0; j < motion->number_of_datacolumns; j++) - fprintf(file, "%lf\t", motion->motiondata[j][i]); - - fprintf(file, "\n"); - } - fclose(file); - - return code_fine; -} - - -MotionSequence* createMotionStruct(ModelStruct* model) -{ - int i; - ReturnCode rc = code_fine; - MotionSequence* motion; - - /* Find, or make, an empty slot for this new motion. */ - for (i = 0; i < model->motion_array_size; i++) - { - if (model->motion[i] == NULL) - break; - } - - if (i == model->motion_array_size) - { - int j, old_size = model->motion_array_size; - model->motion_array_size += MOTION_ARRAY_INCREMENT; - model->motion = (MotionSequence**)simm_realloc(model->motion, - (unsigned)((model->motion_array_size)*sizeof(MotionSequence*)), &rc); - if (rc == code_bad) - return NULL; - - for (j = old_size; j < model->motion_array_size; j++) - model->motion[j] = NULL; - } - - model->motion[i] = (MotionSequence*)simm_calloc(1, sizeof(MotionSequence)); - if (model->motion[i] == NULL) - return NULL; - - motion = model->motion[i]; - model->num_motions++; - - return motion; -} - -#if OPENSMAC -MotionSequence* applyForceMattesToMotion(ModelStruct* model, MotionSequence* motionKin, MotionSequence* motionAnalog, SBoolean addToModel) -{ - int i, j, k, numForceMatteForces = 0; - double value, increment; - ForceMatteForce forces[10]; - MotionSequence* newMotion = NULL; - - if (motionAnalog->number_of_datarows < 2) - return newMotion; - - for (i=0; inumsegments; i++) - { - if (model->segment[i].forceMatte) - { - forces[numForceMatteForces].segment = i; - forces[numForceMatteForces].forceMatte = model->segment[i].forceMatte; - numForceMatteForces++; - } - } - - if (numForceMatteForces == 0) - return newMotion; - - if (addToModel == yes) - newMotion = createMotionStruct(model); - else - newMotion = (MotionSequence*)simm_calloc(1, sizeof(MotionSequence)); - transformMotion(newMotion, motionAnalog, model, forces, numForceMatteForces); - add_motion_to_model(newMotion, model, no); - - increment = (motionAnalog->max_value - motionAnalog->min_value) / (motionAnalog->number_of_datarows - 1); - for (i=0; inumber_of_datarows; i++) - { - value = motionAnalog->min_value + i * increment; - apply_motion_to_model(model, motionKin, value, no, no); - motionAnalog->mopt.current_frame = i; - for (k=0; kmopt.num_motion_object_instances; j++) - { - MotionObjectInstance* moi = motionAnalog->mopt.motion_object_instance[j]; - if (moi->segment == model->ground_segment && STRINGS_ARE_EQUAL(model->motion_objects[moi->object].name, "force")) - { - double pt[3], pt2[3], vec[3], vec2[3], inter[3], freeTorque[3]; - for (k=0; knum_channels; k++) - { - if (moi->channels[k].component == MO_TX) - pt[0] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_TY) - pt[1] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_TZ) - pt[2] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_VX) - vec[0] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_VY) - vec[1] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_VZ) - vec[2] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_X) - freeTorque[0] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_Y) - freeTorque[1] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - else if (moi->channels[k].component == MO_Z) - freeTorque[2] = motionAnalog->motiondata[moi->channels[k].column][motionAnalog->mopt.current_frame]; - } - if (VECTOR_MAGNITUDE(vec) > 5.0) - { - for (k=0; kground_segment, forces[k].segment); - convert_vector(model, vec2, model->ground_segment, forces[k].segment); - if (vector_intersects_polyhedron(pt2, vec2, forces[k].forceMatte->poly, inter)) - { - add_vector(pt, vec, freeTorque, &forces[k]); - break; - } - } - } - } - } - for (k=0; kmotiondata[forces[k].column[0]][motionAnalog->mopt.current_frame] = forces[k].force[0]; - newMotion->motiondata[forces[k].column[1]][motionAnalog->mopt.current_frame] = forces[k].force[1]; - newMotion->motiondata[forces[k].column[2]][motionAnalog->mopt.current_frame] = forces[k].force[2]; - newMotion->motiondata[forces[k].column[3]][motionAnalog->mopt.current_frame] = forces[k].pointOfApplication[0]; - newMotion->motiondata[forces[k].column[4]][motionAnalog->mopt.current_frame] = forces[k].pointOfApplication[1]; - newMotion->motiondata[forces[k].column[5]][motionAnalog->mopt.current_frame] = forces[k].pointOfApplication[2]; - newMotion->motiondata[forces[k].column[6]][motionAnalog->mopt.current_frame] = forces[k].freeTorque[0]; - newMotion->motiondata[forces[k].column[7]][motionAnalog->mopt.current_frame] = forces[k].freeTorque[1]; - newMotion->motiondata[forces[k].column[8]][motionAnalog->mopt.current_frame] = forces[k].freeTorque[2]; - } - } - - return newMotion; -} - - -/* Add a force vector to one already stored in a ForceMatteForce. */ -static void add_vector(double pt[], double vec[], double freeTorque[], ForceMatteForce* fmf) -{ - int i; - double oldMagnitude = fmf->magnitude; - double vecMagnitude = VECTOR_MAGNITUDE(vec); - double newMagnitude = oldMagnitude + vecMagnitude; - - for (i=0; i<3; i++) - { - // Add in the force. - fmf->force[i] += vec[i]; - // The new point of application is the weighted sum of the two vectors. - fmf->pointOfApplication[i] = (fmf->pointOfApplication[i]*oldMagnitude + pt[i]*vecMagnitude) / newMagnitude; - // Add in the free torque. TODO: 2 free torques may complicate the calculations for force and COP addition. - fmf->freeTorque[i] += freeTorque[i]; - } - fmf->magnitude = newMagnitude; -} - -static void transformMotion(MotionSequence* newMotion, MotionSequence* oldMotion, ModelStruct* model, ForceMatteForce* forces, int numForceMattes) -{ - int i, j, count, numGroundForceColumns; - - // Copy over all of the components that do not change. - // Change the name of the old motion so that the new one - // (which initially has the same name) will not get a "(2)" - // appended to it when it is added to the model. - memcpy(newMotion, oldMotion, sizeof(MotionSequence)); - mstrcpy(&newMotion->name, oldMotion->name); - if (oldMotion->name[0] == '_') - oldMotion->name[0] = '*'; - else - oldMotion->name[0] = '_'; - mstrcpy(&newMotion->units, oldMotion->units); - newMotion->deriv_names = NULL; - newMotion->deriv_data = NULL; - memset(&newMotion->mopt, 0, sizeof(MotionModelOptions)); - newMotion->mopt.num_motion_object_instances = 0; - newMotion->mopt.motion_object_instance = NULL; - newMotion->event_color[0] = 1.0f; - newMotion->event_color[1] = 0.0f; - newMotion->event_color[2] = 1.0f; - - if (oldMotion->num_events > 0) - { - newMotion->event = (smMotionEvent*)simm_malloc(sizeof(smMotionEvent)*oldMotion->num_events); - memcpy(newMotion->event, oldMotion->event, sizeof(smMotionEvent)*oldMotion->num_events); - for (j=0; jnum_events; j++) - mstrcpy(&newMotion->event[j].name, oldMotion->event[j].name); - } - - // Count how many columns in the old motion correspond to ground-based forces. - for (i=0, numGroundForceColumns=0; inumber_of_datacolumns; i++) - { - if (columnIsGroundForce(oldMotion, model, i)) - numGroundForceColumns++; - } - - newMotion->number_of_datacolumns = oldMotion->number_of_datacolumns - numGroundForceColumns + numForceMattes * 9; - newMotion->columnname = (char**)simm_malloc(newMotion->number_of_datacolumns * sizeof(char*)); - newMotion->motiondata = (double**)simm_malloc(newMotion->number_of_datacolumns*sizeof(double*)); - for (i=0; inumber_of_datacolumns; i++) - newMotion->motiondata[i] = (double*)simm_calloc(newMotion->number_of_datarows, sizeof(double)); - newMotion->data_std_dev = (double**)simm_calloc(newMotion->number_of_datacolumns, sizeof(double*)); - - // Copy over column names and data, except for the ground-based forces - for (i=0, count=0; inumber_of_datacolumns; i++) - { - if (!columnIsGroundForce(oldMotion, model, i)) - { - mstrcpy(&newMotion->columnname[count], oldMotion->columnname[i]); - memcpy(newMotion->motiondata[count], oldMotion->motiondata[i], oldMotion->number_of_datarows*sizeof(double)); - count++; - } - } - - // Fill in the names for the force matte-based forces. The data will be filled in later. - // There will be one force for each force matte, but they are kept in the ground frame - // because that's how OpenSim expects them. Also, the order must be vx, vy, vz, px, py, pz, torquex, torquey, torquez. -#if 0 - for (i=0; icolumnname[count], "ground_force_vx"); - forces[i].column[0] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_vy"); - forces[i].column[1] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_vz"); - forces[i].column[2] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_px"); - forces[i].column[3] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_py"); - forces[i].column[4] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_pz"); - forces[i].column[5] = count++; - mstrcpy(&newMotion->columnname[count], "ground_torque_x"); - forces[i].column[6] = count++; - mstrcpy(&newMotion->columnname[count], "ground_torque_y"); - forces[i].column[7] = count++; - mstrcpy(&newMotion->columnname[count], "ground_torque_z"); - forces[i].column[8] = count++; - } -#else - for (i=0; icolumnname[count], "ground_force_vx"); - forces[i].column[0] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_vy"); - forces[i].column[1] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_vz"); - forces[i].column[2] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_px"); - forces[i].column[3] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_py"); - forces[i].column[4] = count++; - mstrcpy(&newMotion->columnname[count], "ground_force_pz"); - forces[i].column[5] = count++; - } - for (i=0; icolumnname[count], "ground_torque_x"); - forces[i].column[6] = count++; - mstrcpy(&newMotion->columnname[count], "ground_torque_y"); - forces[i].column[7] = count++; - mstrcpy(&newMotion->columnname[count], "ground_torque_z"); - forces[i].column[8] = count++; - } -#endif - - // Fill in the mopt structure. - //link_model_to_motion(model, newMotion, &numOtherData, NULL); -} - -static SBoolean columnIsGroundForce(MotionSequence* motion, ModelStruct* model, int index) -{ - int i, j; - - for (i=0; imopt.num_motion_object_instances; i++) - { - if ((STRINGS_ARE_EQUAL(model->motion_objects[motion->mopt.motion_object_instance[i]->object].name, "force") || - STRINGS_ARE_EQUAL(model->motion_objects[motion->mopt.motion_object_instance[i]->object].name, "torque")) && - motion->mopt.motion_object_instance[i]->segment == model->ground_segment) - { - for (j=0; jmopt.motion_object_instance[i]->num_channels; j++) - { - if (motion->mopt.motion_object_instance[i]->channels[j].column == index) - return yes; - } - } - } - - return no; -} - -#endif - -void sort_events(MotionSequence* motion, int* index) -{ - int i, j, index_of_min; - double min; - smMotionEvent save; - - // Sort the events to have increasing xCoords. This uses - // a simple N*2 bubble sort, but is OK since there are - // not usually more than 10-12 events in a motion. - for (i=0; inum_events; i++) - { - for (j=i, min = MAXMDOUBLE; jnum_events; j++) - { - if (motion->event[j].xCoord < min) - { - min = motion->event[j].xCoord; - index_of_min = j; - } - } - if (index_of_min != i) - { - memcpy(&save, &motion->event[i], sizeof(smMotionEvent)); - memcpy(&motion->event[i], &motion->event[index_of_min], sizeof(smMotionEvent)); - memcpy(&motion->event[index_of_min], &save, sizeof(smMotionEvent)); - // Keep track of the event pointed to by index. - if (index && *index == index_of_min) - *index = i; - } - } -} - - -void normalize_motion(MotionSequence* motion, ModelStruct* model) -{ - int i, j, num_other, new_num_rows = 101; - int new_num_columns = motion->number_of_datacolumns + 1; - double **deriv_data = NULL, **motiondata = NULL, **data_std_dev = NULL; - double abscissa, start, step_size, percent; - char** columnname; - dpFunction function; - static const char percent_label[] = "percent"; - - // Don't normalize if the motion already has new_num_rows rows and - // min and max values of 0.0 and 100.0. - if (EQUAL_WITHIN_ERROR(motion->min_value, 0.0) && EQUAL_WITHIN_ERROR(motion->max_value, 100.0) && - motion->number_of_datarows == new_num_rows) - { - (void)sprintf(errorbuffer, "Motion %s is already normalized.", motion->name); - error(abort_action, errorbuffer); - return; - } - - start = motion->min_value; - step_size = (motion->max_value - motion->min_value) / (double)(new_num_rows - 1); - - // In case the motion is currently being displayed, calculate and store the percent - // so you can later set the motion to [about] the same time frame. - percent = (motion->mopt.current_value - motion->min_value) / (motion->max_value - motion->min_value); - - // Make space for the new data (new_num_rows * new_num_columns). - // new_num_columns should be 1 more than the motion currently has. - // This extra column will be put in the first slot, and will contain - // 'percent' data. - columnname = (char**)simm_calloc(new_num_columns, sizeof(char*)); - mstrcpy(&columnname[0], percent_label); - motiondata = (double**)simm_calloc(new_num_columns, sizeof(double*)); - if (motion->deriv_data) - deriv_data = (double**)simm_calloc(new_num_columns, sizeof(double*)); - if (motion->data_std_dev) - data_std_dev = (double**)simm_calloc(new_num_columns, sizeof(double*)); - for (i=0; i 0) - { - columnname[i] = motion->columnname[i-1]; - if (motion->deriv_data && motion->deriv_data[i-1]) - deriv_data[i] = (double*)simm_calloc(new_num_rows, sizeof(double)); - if (motion->data_std_dev && motion->data_std_dev[i-1]) - data_std_dev[i] = (double*)simm_calloc(new_num_rows, sizeof(double)); - } - } - - malloc_function(&function, motion->number_of_datarows); - function.numpoints = motion->number_of_datarows; - function.source = dpJointFile; - function.status = dpFunctionSimmDefined; - function.type = dpNaturalCubicSpline; - function.used = dpYes; - - // Fill in the [new] first column with 'percent'. - for (j=0; jnumber_of_datacolumns; i++) - { - int markerMotionObject = column_is_marker_data(motion, i); - function.numpoints = 0; - for (j=0; jnumber_of_datarows; j++) - { - if (markerMotionObject < 0 || is_marker_data_visible(motion, markerMotionObject, j) == yes) - { - function.x[function.numpoints] = motion->motiondata[motion->x_column][j]; - function.y[function.numpoints] = motion->motiondata[i][j]; - function.numpoints++; - } - } - calc_function_coefficients(&function); - for (j=0, abscissa = start; jnumber_of_datarows; - for (j=0; jnumber_of_datarows; j++) - { - function.x[j] = motion->data_std_dev[0][j]; - function.y[j] = motion->data_std_dev[i][j]; - } - calc_function_coefficients(&function); - for (j=0, abscissa = start; jcolumnname); - for (i=0; inumber_of_datacolumns; i++) - FREE_IFNOTNULL(motion->motiondata[i]); - FREE_IFNOTNULL(motion->motiondata); - if (motion->deriv_data) - { - for (i=0; inumber_of_datacolumns; i++) - FREE_IFNOTNULL(motion->deriv_data[i]); - free(motion->deriv_data); - } - if (motion->data_std_dev) - { - for (i=0; inumber_of_datacolumns; i++) - FREE_IFNOTNULL(motion->data_std_dev[i]); - free(motion->data_std_dev); - } - - motion->columnname = columnname; - motion->motiondata = motiondata; - motion->deriv_data = deriv_data; - motion->data_std_dev = data_std_dev; - - motion->number_of_datacolumns = new_num_columns; - motion->number_of_datarows = new_num_rows; - - motion->x_column = 0; - motion->min_value = motion->motiondata[motion->x_column][0]; - motion->max_value = motion->motiondata[motion->x_column][motion->number_of_datarows-1]; - // If time_column is defined, increment it (because the percent column was inserted - // at the beginning) and recalculate the event xCoords. If time_column is not defined, - // there is no way to recalculate the xCoords and the events will not work properly. - if (motion->time_column >= 0) - { - double oldRange, newRange; - motion->time_column++; - oldRange = motion->motiondata[motion->time_column][motion->number_of_datarows-1] - - motion->motiondata[motion->time_column][0]; - newRange = motion->motiondata[motion->x_column][motion->number_of_datarows-1] - - motion->motiondata[motion->x_column][0]; - for (i=0; inum_events; i++) - { - double per = (motion->event[i].xCoord - motion->motiondata[motion->time_column][0]) / oldRange; - motion->event[i].xCoord = motion->motiondata[motion->x_column][0] + per * newRange; - } - } - - link_model_to_motion(model, motion, &num_other, NULL); - - // Recalculate the current frame, using the previously calculated percent. - motion->mopt.current_value = percent * 100.0; - motion->mopt.current_frame = percent * (motion->number_of_datarows - 1) + MOTION_OFFSET; - -#if ! OPENSMAC && ! ENGINE - make_marker_trails(model, motion); - make_force_trails(model, motion); - - queue_model_redraw(model); -#endif - - // After interpolating, force plate forces can sometimes go negative. - //TODO5.0: there's got to be a better way than this... -#if 1 - for (i=0; imopt.num_motion_object_instances; i++) - { - for (j=0; jmopt.motion_object_instance[i]->num_channels; j++) - { - int comp = motion->mopt.motion_object_instance[i]->channels[j].component; - if (comp == MO_VX || comp == MO_VY || comp == MO_VZ) - { - int k, col = motion->mopt.motion_object_instance[i]->channels[j].column; - for (k=0; knumber_of_datarows; k++) - { - double value = ABS(motion->motiondata[col][k]); - if (value < 1.0) - motion->motiondata[col][k] = 0.0; - } - } - } - } -#endif - -#if ! OPENSMAC && ! ENGINE - // Generate an event so the tools can update themselves. - make_and_queue_simm_event(MOTION_CHANGED, model->modelnum, motion, NULL, ZERO, ZERO); - - (void)sprintf(buffer, "Normalized motion %s to have %d time steps.", motion->name, new_num_rows); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); -#endif -} - - -void smooth_motion(MotionSequence* motion, ModelStruct* model, double cutoff_frequency) -{ - int i, j, num_other; - double **deriv_data = NULL, **motiondata = NULL; - dpFunction function; - - if (motion->number_of_datarows < 6 || cutoff_frequency <= 0.0) //TODO5.0 - return; - - // Make space for the new data (new_num_rows * motion->number_of_datacolumns). - motiondata = (double**)simm_calloc(motion->number_of_datacolumns, sizeof(double*)); - if (motion->deriv_data) - deriv_data = (double**)simm_calloc(motion->number_of_datacolumns, sizeof(double*)); - for (i=0; inumber_of_datacolumns; i++) - { - motiondata[i] = (double*)simm_calloc(motion->number_of_datarows, sizeof(double)); - if (motion->deriv_data && motion->deriv_data[i]) - deriv_data[i] = (double*)simm_calloc(motion->number_of_datarows, sizeof(double)); - } - - malloc_function(&function, motion->number_of_datarows); - function.numpoints = 0; //motion->number_of_datarows; - function.source = dpJointFile; - function.status = dpFunctionSimmDefined; - function.type = dpGCVSpline; - function.cutoff_frequency = cutoff_frequency; - function.used = dpYes; - - for (i=0; inumber_of_datarows; i++) - motiondata[motion->x_column][i] = motion->motiondata[motion->x_column][i]; - - // Fit splines to the data. - // TODO5.0: maybe it would be better to access the data through the mopt pointers - // so you can more easily deal with it as gencoords, markers, etc. - for (i=0; inumber_of_datacolumns; i++) - { - if (i != motion->x_column) - { - int markerMotionObject = column_is_marker_data(motion, i); - function.numpoints = 0; - for (j=0; jnumber_of_datarows; j++) - { - if (markerMotionObject < 0 || is_marker_data_visible(motion, markerMotionObject, j) == yes) - { - function.x[function.numpoints] = motion->motiondata[motion->x_column][j]; - function.y[function.numpoints] = motion->motiondata[i][j]; - function.numpoints++; - } - } - calc_function_coefficients(&function); - for (j=0; jnumber_of_datarows; j++) - motiondata[i][j] = interpolate_function(motiondata[motion->x_column][j], &function, zeroth, 0.0, 0.0); - - if (deriv_data && deriv_data[i]) - { - for (j=0; jnumber_of_datarows; j++) - deriv_data[i][j] = interpolate_function(motiondata[motion->x_column][j], &function, first, 1.0, 1.0); - } - } - } - - // Free the space used by the old data. - for (i=0; inumber_of_datacolumns; i++) - FREE_IFNOTNULL(motion->motiondata[i]); - FREE_IFNOTNULL(motion->motiondata); - if (motion->deriv_data) - { - for (i=0; inumber_of_datacolumns; i++) - FREE_IFNOTNULL(motion->deriv_data[i]); - free(motion->deriv_data); - } - - motion->motiondata = motiondata; - motion->deriv_data = deriv_data; - - link_model_to_motion(model, motion, &num_other, NULL); - -#if ! OPENSMAC && ! ENGINE - make_marker_trails(model, motion); - make_force_trails(model, motion); - - queue_model_redraw(model); -#endif - - // After interpolating, force plate forces can sometimes go negative. - //TODO5.0: there's got to be a better way than this... -#if 1 - for (i=0; imopt.num_motion_object_instances; i++) - { - for (j=0; jmopt.motion_object_instance[i]->num_channels; j++) - { - int comp = motion->mopt.motion_object_instance[i]->channels[j].component; - if (comp == MO_VX || comp == MO_VY || comp == MO_VZ) - { - int k, col = motion->mopt.motion_object_instance[i]->channels[j].column; - for (k=0; knumber_of_datarows; k++) - { - double value = ABS(motion->motiondata[col][k]); - if (value < 1.0) - motion->motiondata[col][k] = 0.0; - } - } - } - } -#endif - - //TODO5.0: send MOTION_CHANGED event so model viewer can update? - // Also: how to figure out if motion is current, so it needs to be re-applied? - (void)sprintf(buffer, "Smoothed motion %s with a cutoff frequency of %lf.", motion->name, cutoff_frequency); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); -} - -static int column_is_marker_data(MotionSequence* motion, int column) -{ - int i, j; - - for (i=0; imopt.num_motion_object_instances; i++) - { - if (motion->mopt.motion_object_instance[i]->type == MarkerMotionObject) - { - for (j=0; jmopt.motion_object_instance[i]->num_channels; j++) - { - if (motion->mopt.motion_object_instance[i]->channels[j].column == column) - return i; - } - } - } - - return -1; -} - -/* If any coordinate of a point is UNDEFINED, the point is not visible. - * JPL 9/12/03: C3D files seem to use 0.0 as undefined, so if all three - * coordinates are 0.0, return false as well. - */ -SBoolean is_marker_visible(double pt[]) -{ - if (EQUAL_WITHIN_ERROR(pt[0], UNDEFINED_DOUBLE)) - return no; - if (EQUAL_WITHIN_ERROR(pt[1], UNDEFINED_DOUBLE)) - return no; - if (EQUAL_WITHIN_ERROR(pt[2], UNDEFINED_DOUBLE)) - return no; - - if (EQUAL_WITHIN_ERROR(pt[0], 0.0) && EQUAL_WITHIN_ERROR(pt[1], 0.0) && EQUAL_WITHIN_ERROR(pt[2], 0.0)) - return no; - - return yes; -} - -SBoolean is_marker_visible_meters(double pt[]) -{ - double bad_data = UNDEFINED_DOUBLE * 0.001; - - if (EQUAL_WITHIN_ERROR(pt[0], bad_data)) - return no; - if (EQUAL_WITHIN_ERROR(pt[1], bad_data)) - return no; - if (EQUAL_WITHIN_ERROR(pt[2], bad_data)) - return no; - - if (EQUAL_WITHIN_ERROR(pt[0], 0.0) && EQUAL_WITHIN_ERROR(pt[1], 0.0) && EQUAL_WITHIN_ERROR(pt[2], 0.0)) - return no; - - return yes; -} - -SBoolean is_marker_data_visible(MotionSequence* motion, int motionObject, int row) -{ - double pt[3]; - MotionObjectInstance* moi = motion->mopt.motion_object_instance[motionObject]; - - pt[0] = motion->motiondata[moi->channels[0].column][row]; - pt[1] = motion->motiondata[moi->channels[1].column][row]; - pt[2] = motion->motiondata[moi->channels[2].column][row]; - - return is_marker_visible_meters(pt); -} - - -int get_frame_number(smC3DStruct* c3d, double time) -{ - int i; - - for (i=0; imotionData->header.numFrames; i++) - { - if (c3d->motionData->frameList[i].time >= time) - return i; - } - - return c3d->motionData->header.numFrames - 1; -} - -MotionObjectInstance* add_foot_print(ModelStruct* model, MotionSequence* motion, const char name[], double scale, - double translation[], double eulerRotation[]) -{ - int i, j, index, motion_object = -1; - MotionObjectInstance* mi = NULL; - ReturnCode rc; - - index = motion->mopt.num_motion_object_instances++; - - if (motion->mopt.motion_object_instance == NULL) - { - motion->mopt.motion_object_instance = (MotionObjectInstance**) - simm_malloc(motion->mopt.num_motion_object_instances * sizeof(MotionObjectInstance*)); - } - else - { - motion->mopt.motion_object_instance = (MotionObjectInstance**) - simm_realloc(motion->mopt.motion_object_instance, motion->mopt.num_motion_object_instances * sizeof(MotionObjectInstance*), &rc); - } - - if (motion->mopt.motion_object_instance == NULL) - { - motion->mopt.num_motion_object_instances--; - return NULL; - } - - for (i=0; inum_motion_objects; i++) - { - if (STRINGS_ARE_EQUAL(model->motion_objects[i].name, name)) - { - motion_object = i; - break; - } - } - if (motion_object == -1) - return NULL; - - // Initialize a new motion object instance. - mi = motion->mopt.motion_object_instance[index] = (MotionObjectInstance*)simm_calloc(1, sizeof(MotionObjectInstance)); - - mstrcpy(&mi->name, model->motion_objects[motion_object].name); - mi->type = FootPrintObject; - mi->object = motion_object; - mi->segment = model->ground_segment; - mi->drawmode = model->motion_objects[motion_object].drawmode; - mi->current_value = -1.0; - mi->visible = yes; - identity_matrix(mi->currentXform); - copy_material(&model->dis.mat.materials[model->motion_objects[motion_object].material], &mi->currentMaterial); - - if (eulerRotation) - { - x_rotate_matrix_bodyfixed(mi->currentXform, eulerRotation[0]); - y_rotate_matrix_bodyfixed(mi->currentXform, eulerRotation[1]); - z_rotate_matrix_bodyfixed(mi->currentXform, eulerRotation[2]); - } - - for (i=0; i<3; i++) - for (j=0; j<3; j++) - mi->currentXform[i][j] *= scale; - - if (translation) - { - mi->currentXform[3][0] = translation[0]; - mi->currentXform[3][1] = translation[1]; - mi->currentXform[3][2] = translation[2]; - } - - return mi; -} - -MotionObjectInstance* add_force_plate(ModelStruct* model, MotionSequence* motion, const char name[], double scale[], - double translation[], double eulerRotation[]) -{ - int i, j, index, motion_object = -1; - MotionObjectInstance* mi = NULL; - ReturnCode rc; - - index = motion->mopt.num_motion_object_instances++; - - if (motion->mopt.motion_object_instance == NULL) - { - motion->mopt.motion_object_instance = (MotionObjectInstance**) - simm_malloc(motion->mopt.num_motion_object_instances * sizeof(MotionObjectInstance*)); - } - else - { - motion->mopt.motion_object_instance = (MotionObjectInstance**) - simm_realloc(motion->mopt.motion_object_instance, motion->mopt.num_motion_object_instances * sizeof(MotionObjectInstance*), &rc); - } - - if (motion->mopt.motion_object_instance == NULL) - { - motion->mopt.num_motion_object_instances--; - return NULL; - } - - for (i=0; inum_motion_objects; i++) - { - if (STRINGS_ARE_EQUAL(model->motion_objects[i].name, name)) - { - motion_object = i; - break; - } - } - if (motion_object == -1) - return NULL; - - // Initialize a new motion object instance. - mi = motion->mopt.motion_object_instance[index] = (MotionObjectInstance*)simm_calloc(1, sizeof(MotionObjectInstance)); - - mstrcpy(&mi->name, model->motion_objects[motion_object].name); - mi->type = ForcePlateObject; - mi->object = motion_object; - mi->segment = model->ground_segment; - mi->drawmode = model->motion_objects[motion_object].drawmode; - mi->current_value = -1.0; - mi->visible = yes; - identity_matrix(mi->currentXform); - copy_material(&model->dis.mat.materials[model->motion_objects[motion_object].material], &mi->currentMaterial); - - if (eulerRotation) - { - x_rotate_matrix_bodyfixed(mi->currentXform, eulerRotation[0]); - y_rotate_matrix_bodyfixed(mi->currentXform, eulerRotation[1]); - z_rotate_matrix_bodyfixed(mi->currentXform, eulerRotation[2]); - } - - if (scale) - { - for (i=0; i<3; i++) - for (j=0; j<3; j++) - mi->currentXform[i][j] *= scale[i]; - } - - if (translation) - { - mi->currentXform[3][0] = translation[0]; - mi->currentXform[3][1] = translation[1]; - mi->currentXform[3][2] = translation[2]; - } - - if (0) - { - double mat[4][4]; - printf("add_force_plate, rot = %lf %lf %lf\n", eulerRotation[0], eulerRotation[1], eulerRotation[2]); - print_4x4matrix(mi->currentXform); - for (i=0; i<3; i++) - printf("row %d mag = %lf\n", i, VECTOR_MAGNITUDE(mi->currentXform[i])); - transpose_4x4matrix(mi->currentXform, mat); - for (i=0; i<3; i++) - printf("col %d mag = %lf\n", i, VECTOR_MAGNITUDE(mat[i])); - } - return mi; -} diff --git a/OpenSim/Utilities/simmToOpenSim/normio.c b/OpenSim/Utilities/simmToOpenSim/normio.c deleted file mode 100644 index ec8717afcf..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/normio.c +++ /dev/null @@ -1,1705 +0,0 @@ -/******************************************************************************* - - NORMIO.C - - Author: Peter Loan - - Date: 23-OCT-98 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - -*******************************************************************************/ -#include "universal.h" - -#define _POSIX_ 1 - -#include -#include - -#include "globals.h" -#include "functions.h" -#include "normio.h" -#include "normtools.h" - -// Linux doesn't use O_BINARY -#ifndef O_BINARY -#define O_BINARY 0 -#endif - - -/*************** DEFINES (for this file only) *********************************/ - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static int filekey = 133; -char new_ascii_label[] = "NORM_ASCII"; -char stl_ascii_label[] = "solid"; -static SBoolean verbose = no; - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void name_polyhedron(PolyhedronStruct* ph, char filename[]); - - -/* --------------------------------------------------------------------------- - BINARY COMPATIBILITY ROUTINES: the following 3 routines are used to support - reading binary bone files on little-endian Win32 machines. -- KMS 12/17/98 ------------------------------------------------------------------------------- */ -static void flip_bytes(char* buf, int size) -{ - char* p = buf; - char* q = buf + size - 1; - - for ( ; p < q; p++, q--) - { - char c = *p; - - *p = *q; - *q = c; - } -} - - -static int read_binary(int fd, char* buf, int size) -{ - int bytes_read = read(fd, buf, size); - -#ifdef WIN32 - flip_bytes(buf, size); -#endif - - return bytes_read; -} - - -static int read_binary_array(int fd, char* buf, int n, int size) -{ - int nBytes = n * size; - int bytes_read = read(fd, buf, nBytes); - -#ifdef WIN32 - { - char *p = buf, *end = buf + nBytes; - - for ( ; p < end; p += size) - flip_bytes(p, size); - } -#endif - - return bytes_read; -} - - -static int write_binary(int fd, char* buf, int size) -{ - int bytes_written; - -#ifdef WIN32 - flip_bytes(buf, size); -#endif - - bytes_written = write(fd, buf, size); - -#ifdef WIN32 - flip_bytes(buf, size); -#endif - - return bytes_written; -} - - -static int write_binary_array(int fd, char* buf, int n, int size) -{ - int bytes_written, nBytes = n * size; - -#ifdef WIN32 - char *p = buf, *end = buf + nBytes; - - for ( ; p < end; p += size) - flip_bytes(p, size); -#endif - - bytes_written = write(fd, buf, nBytes); - -#ifdef WIN32 - p = buf; - - for ( ; p < end; p += size) - flip_bytes(p, size); -#endif - - return bytes_written; -} - - -/* PARSE_STRING: this routine scans a string and extracts a variable from - * it. The type of variable that it extracts is specified in one of the - * arguments. It returns the unused portion of the string so that more - * variables can be extracted from it. - */ - -static char* parse_string(char str_buffer[], VariableType var_type, void* dest_var) -{ - char tmp_buffer[CHARBUFFER], *buffer_ptr; - - if (str_buffer == NULL) - return NULL; - - buffer_ptr = tmp_buffer; - - while (CHAR_IS_WHITE_SPACE(*str_buffer)) - str_buffer++; - - if (var_type == type_char) - { - *((char*)dest_var) = *str_buffer; - return str_buffer + 1; - } - - if (var_type == type_string) - { - if (STRING_IS_NULL(str_buffer)) - *((char*)dest_var) = STRING_TERMINATOR; - else - (void)sscanf(str_buffer,"%s", (char*)dest_var); - return str_buffer + strlen((char*)dest_var); - } - - if (var_type == type_double) - { - *((double*)dest_var) = ERROR_DOUBLE; - if (STRING_IS_NOT_NULL(str_buffer)) - { - while (*str_buffer == '-' || *str_buffer == '.' || *str_buffer == '+' || - *str_buffer == 'e' || *str_buffer == 'E' || - *str_buffer == 'd' || *str_buffer == 'D' || - (*str_buffer >= '0' && *str_buffer <= '9')) - *(buffer_ptr++) = *(str_buffer++); - *buffer_ptr = STRING_TERMINATOR; - if (sscanf(tmp_buffer,"%lg",(double*)dest_var) != 1) - *((double*)dest_var) = ERROR_DOUBLE; - } - return str_buffer; - } - - if (var_type == type_int) - { - *((int*)dest_var) = ERRORINT; - if (STRING_IS_NOT_NULL(str_buffer)) - { - while (*str_buffer == '-' || (*str_buffer >= '0' && *str_buffer <= '9')) - *(buffer_ptr++) = *(str_buffer++); - *buffer_ptr = STRING_TERMINATOR; - if (sscanf(tmp_buffer,"%d",(int*)dest_var) != 1) - *((int*)dest_var) = ERRORINT; - } - return str_buffer; - } - - (void)sprintf(errorbuffer,"Unknown variable type (%d) passed to parse_string().", - (int)var_type); - error(none,errorbuffer); - - return NULL; -} - -FileType check_file_type(char filename[]) -{ - int fpb, fkb, len; - char fkey[CHARBUFFER]; - FILE* fpa; - FileType ft=unknown; - - // First try to open the file. - if ((fpb = simm_open(filename, O_RDONLY | O_BINARY | O_RAW)) == -1) - return file_not_found; - else - close(fpb); - - len = strlen(filename); - - // Check suffix to see if it's a Wavefront .obj file. - if (len > 4) - { - if (STRINGS_ARE_EQUAL(&filename[len-4], ".obj")) - return wavefront; - } - - // Binary STL files should not start with "solid" in the header, - // but some do anyway. To load these, their suffixes must be - // changed to .stlb. - if (len > 5) - { - if (STRINGS_ARE_EQUAL(&filename[len-5], ".stlb")) - return stl_binary; - } - - // Check suffix to see if it's an STL file. - if (len > 4) - { - if (STRINGS_ARE_EQUAL(&filename[len-4], ".stl")) - { - if ((fpa = simm_fopen(filename, "r")) == NULL) - { - return file_not_found; - } - else - { - fscanf(fpa, "%s", fkey); - if (STRINGS_ARE_EQUAL(fkey, stl_ascii_label)) - ft = stl_ascii; - else - ft = stl_binary; - fclose(fpa); - return ft; - } - } - } - - // The file exists and it's not a Wavefront or STL, so - // read from it to see if it's a binary SIMM bone file. - if ((fpb = simm_open(filename, O_RDONLY | O_BINARY | O_RAW)) == -1) - { - return file_not_found; - } - else - { - read_binary(fpb, (char*)&fkb, sizeof(int)); - close(fpb); - } - - if (fkb == filekey) - ft = binary; - else - ft = unknown; - - if (ft == unknown) - { - if ((fpa = simm_fopen(filename, "r")) == NULL) - { - return file_not_found; - } - else - { - fscanf(fpa, "%s", fkey); - if (STRINGS_ARE_EQUAL(fkey, new_ascii_label)) - ft = new_ascii; - else - ft = old_ascii; - fclose(fpa); - } - } - - return ft; -} - - -FileReturnCode read_polyhedron(PolyhedronStruct* ph, char filename[], SBoolean run_norm) -{ - FileType input_file_type; - ReturnCode rc; - - preread_init_polyhedron(ph); - - // Always name the polyhedron so it can be written back to JNT file. - name_polyhedron(ph, filename); - - input_file_type = check_file_type(filename); - - if (input_file_type == file_not_found) - { - return file_missing; - } - if (input_file_type == binary) - { - rc = read_binary_file(ph,filename); - if (rc == code_bad || check_polyhedron(ph) == code_bad) - { - sprintf(errorbuffer, "Validity check of bone %s failed.", ph->name); - error(none,errorbuffer); - return file_bad; - } - postread_init_polyhedron(ph,no); - } - else if (input_file_type == new_ascii) - { - rc = read_ascii_file(ph,filename); - if (rc == code_bad || check_polyhedron(ph) == code_bad) - { - sprintf(errorbuffer, "Validity check of bone %s failed.", ph->name); - error(none,errorbuffer); - return file_bad; - } - postread_init_polyhedron(ph,no); - } - else if (input_file_type == wavefront) - { - rc = read_wavefront_file(ph, filename, run_norm); - if (rc == code_bad || check_polyhedron(ph) == code_bad) - { - sprintf(errorbuffer, "Validity check of bone %s failed.", ph->name); - error(none,errorbuffer); - return file_bad; - } - } - else if (input_file_type == stl_ascii) - { - rc = read_stl_ascii_file(ph, filename, run_norm); - if (rc == code_bad || check_polyhedron(ph) == code_bad) - { - sprintf(errorbuffer, "Validity check of bone %s failed.", ph->name); - error(none,errorbuffer); - return file_bad; - } - } - else if (input_file_type == stl_binary) - { - rc = read_stl_binary_file(ph, filename, run_norm); - if (rc == code_bad || check_polyhedron(ph) == code_bad) - { - sprintf(errorbuffer, "Validity check of bone %s failed.", ph->name); - error(none,errorbuffer); - return file_bad; - } - } - else /* input_file_type == old_ascii or unknown */ - { - rc = read_old_ascii_file(ph,filename); - if (rc == code_bad || check_polyhedron(ph) == code_bad) - { - sprintf(errorbuffer, "Validity check of bone %s failed.", ph->name); - error(none,errorbuffer); - return file_bad; - } - postread_init_polyhedron(ph,no); - if (run_norm == yes) - { - int num_out; - NormOptions opt; - PolyhedronStruct* ph_out; - - opt.verbose_output = no; - opt.clip_vertices = no; - opt.output_format = new_ascii; - opt.write_separate_polyhedra = no; - opt.convexify_polygons = no; - opt.remove_collinear = no; - opt.vertex_offset = 0; - opt.tol_box_set = no; - opt.fill_holes = no; - opt.triangulate = no_tri; - opt.vertex_order = unspecified_order; - opt.vertex_tolerance = 0.0; - opt.max_edge_length = -50.0; - opt.reference_normal[0] = 0.0; - opt.reference_normal[1] = 1.0; - opt.reference_normal[2] = 0.0; - norm(ph, &opt, &num_out, &ph_out); - if (num_out == 0) - return file_bad; - free_polyhedron(ph, no, NULL); - copy_polyhedron(&ph_out[0], ph); - free_polyhedron(&ph_out[0], no, NULL); - } - } - - return file_good; -} - -/* Make a suitable name for a polyhedron, based on the name of - * the file it was loaded from. - */ -static void name_polyhedron(PolyhedronStruct* ph, char filename[]) -{ - // Form the bone name from the base of the filename. - int len, end = strlen(filename); - int start = end - 1; - while (start >= 0 && filename[start--] != DIR_SEP_CHAR) - ; - - if (start == -1) - start = 0; - else - start += 2; - len = end - start; - ph->name = (char *)simm_malloc((len+1)*sizeof(char)); - - (void)strncpy(ph->name,&filename[start],len); - ph->name[len] = STRING_TERMINATOR; -} - -ReturnCode read_binary_file(PolyhedronStruct* ph, char filename[]) -{ - int i, fd, fk, not_needed, num_edges; - long correct_num_bytes, bytes_read = 0; - float fnormal[3]; - - if ((fd = simm_open(filename,O_RDONLY | O_BINARY | O_RAW)) == -1) - { - (void)sprintf(errorbuffer,"Unable to open file %s", filename); - error(none,errorbuffer); - return code_bad; - } - - bytes_read += read_binary(fd,(char*)&fk,sizeof(int)); - if (fk != filekey) - { - (void)sprintf(errorbuffer,"File %s is not a properly formatted binary file", filename); - error(none,errorbuffer); - return code_bad; - } - - bytes_read += read_binary(fd,(char*)&ph->num_vertices,sizeof(int)); - bytes_read += read_binary(fd,(char*)&ph->num_polygons,sizeof(int)); - bytes_read += read_binary(fd,(char*)¬_needed,sizeof(int)); - bytes_read += read_binary(fd,(char*)¬_needed,sizeof(int)); - bytes_read += read_binary(fd,(char*)&ph->bc.x1,sizeof(double)); - bytes_read += read_binary(fd,(char*)&ph->bc.x2,sizeof(double)); - bytes_read += read_binary(fd,(char*)&ph->bc.y1,sizeof(double)); - bytes_read += read_binary(fd,(char*)&ph->bc.y2,sizeof(double)); - bytes_read += read_binary(fd,(char*)&ph->bc.z1,sizeof(double)); - bytes_read += read_binary(fd,(char*)&ph->bc.z2,sizeof(double)); - - ph->vertex = (VertexStruct*)simm_malloc(ph->num_vertices*sizeof(VertexStruct)); - ph->polygon = (PolygonStruct*)simm_malloc(ph->num_polygons*sizeof(PolygonStruct)); - - if (ph->vertex == NULL || ph->polygon == NULL) - return code_bad; - - for (i=0; inum_vertices; i++) - { - preread_init_vertex(&ph->vertex[i],i); - bytes_read += read_binary_array(fd, (char*) ph->vertex[i].coord, - 3, sizeof(double)); - } - - for (i=0; inum_vertices; i++) - { - bytes_read += read_binary_array(fd, (char*) fnormal, 3, sizeof(float)); - ph->vertex[i].normal[0] = fnormal[0]; - ph->vertex[i].normal[1] = fnormal[1]; - ph->vertex[i].normal[2] = fnormal[2]; - } - - for (i=0, num_edges=0; inum_polygons; i++) - { - preread_init_polygon(&ph->polygon[i]); - bytes_read += read_binary(fd, (char*) &ph->polygon[i].num_vertices, sizeof(int)); - num_edges += ph->polygon[i].num_vertices; - ph->polygon[i].vertex_index = (int*)simm_malloc(ph->polygon[i].num_vertices* - sizeof(int)); - } - - for (i=0; inum_polygons; i++) - { - bytes_read += read_binary_array(fd, (char*) ph->polygon[i].vertex_index, - ph->polygon[i].num_vertices, sizeof(int)); - } - close(fd); - - correct_num_bytes = 5*sizeof(int) + 6*sizeof(double) + - 3*ph->num_vertices*sizeof(double) + 3*ph->num_vertices*sizeof(float) + - ph->num_polygons*sizeof(int) + num_edges*sizeof(int); - - if (bytes_read != correct_num_bytes) - { - ph->num_vertices = ph->num_polygons = 0; - (void)sprintf(errorbuffer,"Error reading file %s. Only %ld of %ld bytes read.", - filename, bytes_read, correct_num_bytes); - error(none,errorbuffer); - return code_bad; - } - return code_fine; -} - - -ReturnCode read_ascii_file(PolyhedronStruct* ph, char filename[]) -{ - int i, j, rc; - FILE* fp; - - fp = simm_fopen(filename,"r"); - if (fp == NULL) - { -#if 0 /* this should never get called */ - (void)sprintf(errorbuffer,"Unable to open file %s", filename); - error(none,errorbuffer); -#endif - return code_bad; - } - - /* For now, support old ASCII file type. Eventually (perhaps for SIMM/PC 1.0, - * we'll want to remove support for this filetype (as well as for binary), - * since the new norm will only output the new ASCII format. - */ - rc = fscanf(fp,"%s", buffer); - if (rc != 1 || STRINGS_ARE_NOT_EQUAL(buffer,new_ascii_label)) - { - fclose(fp); - (void)sprintf(errorbuffer,"Error reading header information from file %s.", - filename); - error(none,errorbuffer); - return code_bad; - } - - rc = fscanf(fp,"%d %d", &ph->num_vertices, &ph->num_polygons); - - if (rc != 2) - { - fclose(fp); - (void)sprintf(errorbuffer,"Error reading header information from file %s.", - filename); - error(none,errorbuffer); - return code_bad; - } - - rc = fscanf(fp,"%lf %lf %lf %lf %lf %lf", - &ph->bc.x1, &ph->bc.x2, - &ph->bc.y1, &ph->bc.y2, - &ph->bc.z1, &ph->bc.z2); - - if (rc != 6) - { - fclose(fp); - (void)sprintf(errorbuffer,"Error reading bounding box from file %s.", - filename); - error(none,errorbuffer); - return code_bad; - } - - ph->vertex = (VertexStruct*)simm_malloc(ph->num_vertices*sizeof(VertexStruct)); - ph->polygon = (PolygonStruct*)simm_malloc(ph->num_polygons*sizeof(PolygonStruct)); - - if (ph->vertex == NULL || ph->polygon == NULL) - { - fclose(fp); - return code_bad; - } - - /* read-in the vertices and vertex normals */ - - for (i=0; inum_vertices; i++) - { - preread_init_vertex(&ph->vertex[i],i); - fscanf(fp,"%lf %lf %lf", &ph->vertex[i].coord[0], - &ph->vertex[i].coord[1], &ph->vertex[i].coord[2]); - fscanf(fp,"%lf %lf %lf", &ph->vertex[i].normal[0], - &ph->vertex[i].normal[1], &ph->vertex[i].normal[2]); - } - - /* read-in the polygons (vertex indices) */ - - for (i=0; inum_polygons; i++) - { - preread_init_polygon(&ph->polygon[i]); - fscanf(fp,"%d", &ph->polygon[i].num_vertices); - ph->polygon[i].vertex_index = (int*)simm_malloc(ph->polygon[i].num_vertices* - sizeof(int)); - for (j=0; jpolygon[i].num_vertices; j++) - fscanf(fp,"%d", &ph->polygon[i].vertex_index[j]); - } - - fclose(fp); - - return code_fine; -} - - -ReturnCode read_old_ascii_file(PolyhedronStruct* ph, char filename[]) -{ - int i, j, rc; - FILE* fp; - - fp = simm_fopen(filename,"r"); - if (fp == NULL) - { - (void)sprintf(errorbuffer,"Unable to open file %s", filename); - error(none,errorbuffer); - return code_bad; - } - - rc = fscanf(fp,"%d %d", &ph->num_vertices, &ph->num_polygons); - - if (rc != 2) - { - fclose(fp); - (void)sprintf(errorbuffer,"Error reading header information from file %s.", - filename); - error(none,errorbuffer); - return code_bad; - } - - ph->vertex = (VertexStruct*)simm_malloc(ph->num_vertices*sizeof(VertexStruct)); - ph->polygon = (PolygonStruct*)simm_malloc(ph->num_polygons*sizeof(PolygonStruct)); - //dkb polygon malloc boolcode.seg_list here??? - - if (ph->vertex == NULL || ph->polygon == NULL) - { - fclose(fp); - return code_bad; - } - - /* read-in the vertices */ - - for (i=0; inum_vertices; i++) - { - preread_init_vertex(&ph->vertex[i],i); - fscanf(fp,"%lf %lf %lf", &ph->vertex[i].coord[0], - &ph->vertex[i].coord[1], &ph->vertex[i].coord[2]); - } - - /* read-in the polygons (vertex indices) */ - - for (i=0; inum_polygons; i++) - { - preread_init_polygon(&ph->polygon[i]); - fscanf(fp,"%d", &ph->polygon[i].num_vertices); - ph->polygon[i].vertex_index = (int*)simm_malloc(ph->polygon[i].num_vertices* - sizeof(int)); - for (j=0; jpolygon[i].num_vertices; j++) - { - fscanf(fp,"%d", &ph->polygon[i].vertex_index[j]); - /* Old ASCII vertex numbers start at 1 */ - ph->polygon[i].vertex_index[j]--; - } - } - - fclose(fp); - - return code_fine; -} - -ReturnCode read_wavefront_file(PolyhedronStruct* ph, char filename[], SBoolean run_norm) -{ - int i, num_normals = 0, v_count = 0, n_count = 0, p_count = 0; - double* vertex_normals; - FILE* fp; - - fp = simm_fopen(filename, "r"); - if (fp == NULL) - { -#if 0 /* this should never get called */ - (void)sprintf(errorbuffer,"Unable to open file %s", filename); - error(none,errorbuffer); -#endif - return code_bad; - } - - /* Scan ahead to find num_vertices, num_normals, num_polygons. */ - while (fscanf(fp, "%s", buffer) > 0) - { - if (STRINGS_ARE_EQUAL(buffer, "v")) - ph->num_vertices++; - else if (STRINGS_ARE_EQUAL(buffer, "vn")) - num_normals++; - else if (STRINGS_ARE_EQUAL(buffer, "f")) - ph->num_polygons++; - else if (STRINGS_ARE_EQUAL(buffer, "#")) - read_line(fp, buffer); - } - - // Make a temporary array for the vertex normals, which will - // be copied to the vertex structures as the polygons - // are read. In a Wavefront file it is possible to use a vertex - // in one polygon with one normal, and the same vertex in a - // different polygon with a different normal. This is not allowed - // in SIMM; each time a vertex is used in a polygon, the normal - // specified for that usage will be copied into the vertex's - // normal vector, overwriting any previous specifications. - vertex_normals = (double*)simm_malloc(num_normals * 3 * sizeof(double)); - -#if 0 - if (num_normals != ph->num_vertices) - { - fclose(fp); - (void)sprintf(errorbuffer, "Error: number of vertices (%d) not equal to number of vertex normals (%d) in file %s.", - ph->num_vertices, num_normals, filename); - error(none,errorbuffer); - ph->num_vertices = ph->num_polygons = 0; - return code_bad; - } -#endif - - ph->vertex = (VertexStruct*)simm_malloc(ph->num_vertices * sizeof(VertexStruct)); - ph->polygon = (PolygonStruct*)simm_malloc(ph->num_polygons * sizeof(PolygonStruct)); - - if (ph->vertex == NULL || ph->polygon == NULL || vertex_normals == NULL) - { - fclose(fp); - ph->num_vertices = ph->num_polygons = 0; - return code_bad; - } - - for (i=0; inum_vertices; i++) - preread_init_vertex(&ph->vertex[i], i); - - for (i=0; inum_polygons; i++) - preread_init_polygon(&ph->polygon[i]); - - /* Rewind file to beginning. */ - rewind(fp); - - while (fscanf(fp, "%s", buffer) > 0) - { - if (STRINGS_ARE_EQUAL(buffer, "#")) - { - read_line(fp, buffer); - } - else if (STRINGS_ARE_EQUAL(buffer, "v")) - { - fscanf(fp, "%lg %lg %lg", &ph->vertex[v_count].coord[0], - &ph->vertex[v_count].coord[1], &ph->vertex[v_count].coord[2]); - v_count++; - } - else if (STRINGS_ARE_EQUAL(buffer, "vn")) - { - fscanf(fp, "%lg %lg %lg", &vertex_normals[n_count * 3], &vertex_normals[n_count * 3 + 1], &vertex_normals[n_count * 3 + 2]); - n_count++; - } - else if (STRINGS_ARE_EQUAL(buffer, "f")) - { - int max_vertices, index, pv_count = 0; - ReturnCode rc; - char* line; - - read_line(fp, buffer); - max_vertices = strlen(buffer); - ph->polygon[p_count].vertex_index = (int*)simm_malloc(max_vertices * sizeof(int)); - line = buffer; - // indices are stored in this format: 102/95/101 - // vertex coordinates: 102nd element in 'v' array - // texture coordinates: 95th element in 'vt' array - // normal coordinates: 101st element in 'vn' array - // negative numbers mean to start at the end of the array and count backwards - while (line && strlen(line) > 0) - { - // Read the vertex index. - int v_index; - line = parse_string(line, type_int, &v_index); - if (v_index < 0) - v_index += ph->num_vertices; // count backwards from end - else - v_index--; // make index zero-based - ph->polygon[p_count].vertex_index[pv_count++] = v_index; - - // After each vertex index there can be normal and texture indices (each preceded by "/"). - // If they are present, skip over the texture index but use the normal index. TODO5.0: use the texure index - if (line[0] == '/') - { - // skip over the "/" - line++; - // read and ignore the texture index - line = parse_string(line, type_int, &index); - if (line[0] == '/') - { - // skip over the "/" - line++; - // read the normal index - line = parse_string(line, type_int, &index); - index--; // make index zero-based - // Now get that normal vector and copy it to the vertex. - memcpy(ph->vertex[v_index].normal, &vertex_normals[index * 3], 3 * sizeof(double)); - } - } - } - ph->polygon[p_count].num_vertices = pv_count; - ph->polygon[p_count].vertex_index = (int*)simm_realloc(ph->polygon[p_count].vertex_index, - ph->polygon[p_count].num_vertices * sizeof(int), &rc); - p_count++; - } - } - - fclose(fp); - - postread_init_polyhedron(ph, no); - - if (run_norm == yes) - { - int num_out; - NormOptions opt; - PolyhedronStruct* ph_out; - - opt.verbose_output = no; - opt.clip_vertices = no; - opt.output_format = new_ascii; - opt.write_separate_polyhedra = no; - opt.convexify_polygons = no; - opt.remove_collinear = no; - opt.vertex_offset = 0; - opt.tol_box_set = no; - opt.fill_holes = no; - opt.triangulate = no_tri; - opt.vertex_order = unspecified_order; - opt.vertex_tolerance = 0.0000002; - opt.max_edge_length = -50.0; - opt.reference_normal[0] = 0.0; - opt.reference_normal[1] = 1.0; - opt.reference_normal[2] = 0.0; - - //write_ascii_file("C:\\Products\\5.0Testing\\bones\\debug1.asc", &ph->bc, ph, 1, &opt); - - norm(ph, &opt, &num_out, &ph_out); - if (num_out == 0) - return file_bad; - free_polyhedron(ph, no, NULL); - copy_polyhedron(&ph_out[0], ph); - free_polyhedron(&ph_out[0], no, NULL); - - //write_ascii_file("C:\\Products\\5.0Testing\\bones\\debug2.asc", &ph->bc, ph, 1, &opt); - } - - return code_fine; -} - - -ReturnCode read_stl_binary_file(PolyhedronStruct* ph, char filename[], SBoolean run_norm) -{ - int i, j, fd, bytes_read = 0, index; - float number; - unsigned int count; - char header[80]; - - if ((fd = simm_open(filename, O_RDONLY | O_BINARY | O_RAW)) == -1) - { -#if 0 // This should never get called. - (void)sprintf(errorbuffer, "Unable to open file %s", filename); - error(none, errorbuffer); -#endif - return code_bad; - } - - bytes_read += read(fd, (void*)header, 80); - - bytes_read += read(fd, (void*)&count, sizeof(int)); - ph->num_polygons = count; - ph->num_vertices = 3 * ph->num_polygons; - - ph->vertex = (VertexStruct*)simm_malloc(ph->num_vertices * sizeof(VertexStruct)); - ph->polygon = (PolygonStruct*)simm_malloc(ph->num_polygons * sizeof(PolygonStruct)); - - if (ph->vertex == NULL || ph->polygon == NULL) - { - close(fd); - ph->num_vertices = ph->num_polygons = 0; - return code_bad; - } - - for (i=0; inum_vertices; i++) - preread_init_vertex(&ph->vertex[i], i); - - for (i=0; inum_polygons; i++) - preread_init_polygon(&ph->polygon[i]); - - for (i=0, index=0; inum_polygons; i++) - { - // read the polygon normal - bytes_read += read(fd, (void*)&number, sizeof(float)); - ph->polygon[i].normal[0] = number; - bytes_read += read(fd, (void*)&number, sizeof(float)); - ph->polygon[i].normal[1] = number; - bytes_read += read(fd, (void*)&number, sizeof(float)); - ph->polygon[i].normal[2] = number; - // read the 3 vertices - for (j=0; j<3; j++) - { - bytes_read += read(fd, (void*)&number, sizeof(float)); - ph->vertex[index].coord[0] = number; - bytes_read += read(fd, (void*)&number, sizeof(float)); - ph->vertex[index].coord[1] = number; - bytes_read += read(fd, (void*)&number, sizeof(float)); - ph->vertex[index].coord[2] = number; - index++; - } - bytes_read += read(fd, (void*)header, 2); // attribute byte count, not used - } - - close(fd); - - for (i=0, index=0; inum_polygons; i++) - { - ph->polygon[i].num_vertices = 3; - ph->polygon[i].vertex_index = (int*)simm_malloc(ph->polygon[i].num_vertices * sizeof(int)); - ph->polygon[i].vertex_index[0] = index++; - ph->polygon[i].vertex_index[1] = index++; - ph->polygon[i].vertex_index[2] = index++; - } - - postread_init_polyhedron(ph, no); - - if (run_norm == yes) - { - int num_out; - NormOptions opt; - PolyhedronStruct* ph_out; - - opt.verbose_output = no; - opt.clip_vertices = no; - opt.output_format = new_ascii; - opt.write_separate_polyhedra = no; - opt.convexify_polygons = no; - opt.remove_collinear = no; - opt.vertex_offset = 0; - opt.tol_box_set = no; - opt.fill_holes = no; - opt.triangulate = no_tri; - opt.vertex_order = unspecified_order; - opt.vertex_tolerance = 0.0000002; - opt.max_edge_length = -50.0; - opt.reference_normal[0] = 0.0; - opt.reference_normal[1] = 1.0; - opt.reference_normal[2] = 0.0; - - //write_ascii_file("C:\\Products\\5.0Testing\\bones\\debug1.asc", &ph->bc, ph, 1, &opt); - - norm(ph, &opt, &num_out, &ph_out); - if (num_out == 0) - return file_bad; - free_polyhedron(ph, no, NULL); - copy_polyhedron(&ph_out[0], ph); - free_polyhedron(&ph_out[0], no, NULL); - - //write_ascii_file("C:\\Products\\5.0Testing\\bones\\debug2.asc", &ph->bc, ph, 1, &opt); - } - - return code_fine; -} - - -ReturnCode read_stl_ascii_file(PolyhedronStruct* ph, char filename[], SBoolean run_norm) -{ - int i, j, index, count = 0; - FILE* fp; - float number; - char header[80]; - - fp = simm_fopen(filename, "r"); - if (fp == NULL) - { -#if 0 /* this should never get called */ - (void)sprintf(errorbuffer, "Unable to open file %s", filename); - error(none, errorbuffer); -#endif - return code_bad; - } - - fscanf(fp, "%s", header); - if (STRINGS_ARE_NOT_EQUAL_CI(header, "solid")) - { - (void)sprintf(errorbuffer, "Error reading ascii STL file %s. File must start with header \"solid\".", filename); - error(none, errorbuffer); - return code_bad; - } - - while (1) - { - if (fscanf(fp, "%s", header) != 1) - break; - if (STRINGS_ARE_EQUAL_CI(header, "facet")) - count++; - } - rewind(fp); - - ph->num_polygons = count; - ph->num_vertices = 3 * ph->num_polygons; - - ph->vertex = (VertexStruct*)simm_malloc(ph->num_vertices * sizeof(VertexStruct)); - ph->polygon = (PolygonStruct*)simm_malloc(ph->num_polygons * sizeof(PolygonStruct)); - - if (ph->vertex == NULL || ph->polygon == NULL) - { - fclose(fp); - ph->num_vertices = ph->num_polygons = 0; - return code_bad; - } - - for (i=0; inum_vertices; i++) - preread_init_vertex(&ph->vertex[i], i); - - for (i=0; inum_polygons; i++) - preread_init_polygon(&ph->polygon[i]); - - fscanf(fp, "%s", header); - - for (i=0, index=0; inum_polygons; i++) - { - // read the polygon normal - fscanf(fp, "%*s %*s %lg %lg %lg", &ph->polygon[i].normal[0], &ph->polygon[i].normal[1], &ph->polygon[i].normal[2]); - fscanf(fp, "%*s %*s"); // "outer loop" - // read the 3 vertices - for (j=0; j<3; j++) - { - fscanf(fp, "%*s %lg %lg %lg", &ph->vertex[index].coord[0], &ph->vertex[index].coord[1], &ph->vertex[index].coord[2]); - index++; - } - fscanf(fp, "%*s %*s"); // "endloop endfacet" - } - - for (i=0, index=0; inum_polygons; i++) - { - ph->polygon[i].num_vertices = 3; - ph->polygon[i].vertex_index = (int*)simm_malloc(ph->polygon[i].num_vertices * sizeof(int)); - ph->polygon[i].vertex_index[0] = index++; - ph->polygon[i].vertex_index[1] = index++; - ph->polygon[i].vertex_index[2] = index++; - } - - fclose(fp); - - postread_init_polyhedron(ph, no); - - if (run_norm == yes) - { - int num_out; - NormOptions opt; - PolyhedronStruct* ph_out; - - opt.verbose_output = no; - opt.clip_vertices = no; - opt.output_format = new_ascii; - opt.write_separate_polyhedra = no; - opt.convexify_polygons = no; - opt.remove_collinear = no; - opt.vertex_offset = 0; - opt.tol_box_set = no; - opt.fill_holes = no; - opt.triangulate = no_tri; - opt.vertex_order = unspecified_order; - opt.vertex_tolerance = 0.0000002; - opt.max_edge_length = -50.0; - opt.reference_normal[0] = 0.0; - opt.reference_normal[1] = 1.0; - opt.reference_normal[2] = 0.0; - - //write_ascii_file("C:\\Products\\5.0Testing\\bones\\debug1.asc", &ph->bc, ph, 1, &opt); - - norm(ph, &opt, &num_out, &ph_out); - if (num_out == 0) - return file_bad; - free_polyhedron(ph, no, NULL); - copy_polyhedron(&ph_out[0], ph); - free_polyhedron(&ph_out[0], no, NULL); - - //write_ascii_file("C:\\Products\\5.0Testing\\bones\\debug2.asc", &ph->bc, ph, 1, &opt); - } - - return code_fine; -} - - -ReturnCode write_binary_file(char filename[], BoundingCube* bc, - PolyhedronStruct polyhedron[], int num_polyhedra, - int vertex_offset) -{ - - int i, j, k, fd, vnum, num_vertices, num_polygons, num_edges; - int num_written, max_vertex_count; - float fnorm[3]; - SBoolean write_verbose = yes; - - if ((fd = simm_open(filename,O_CREAT | O_WRONLY | O_BINARY)) == -1) - { - fprintf(stderr,"Unable to open %s for output.\n", filename); - return code_bad; - } - - if (write_verbose == yes) - { - printf("Writing binary file %s ... ", filename); - fflush(stdout); - } - - num_vertices = 0; - num_polygons = 0; - num_edges = 0; - max_vertex_count = 0; - for (i=0; i max_vertex_count) - max_vertex_count = polyhedron[i].polygon[j].num_vertices; - } - } - } - - /* Filekey is used to mark the file as a SIMM/norm binary file */ - - write_binary(fd,(char*)&filekey,sizeof(int)); - write_binary(fd,(char*)&num_vertices,sizeof(int)); - write_binary(fd,(char*)&num_polygons,sizeof(int)); - write_binary(fd,(char*)&num_edges,sizeof(int)); - write_binary(fd,(char*)&max_vertex_count,sizeof(int)); - - write_binary(fd,(char*)&bc->x1,sizeof(double)); - write_binary(fd,(char*)&bc->x2,sizeof(double)); - write_binary(fd,(char*)&bc->y1,sizeof(double)); - write_binary(fd,(char*)&bc->y2,sizeof(double)); - write_binary(fd,(char*)&bc->z1,sizeof(double)); - write_binary(fd,(char*)&bc->z2,sizeof(double)); - - num_written = vertex_offset; - for (i=0; ix1, bc->x2, bc->y1, bc->y2, - bc->z1, bc->z2); - - num_written = opt->vertex_offset; - for (i=0; i= num_written) - printf("writing %d\n", polyhedron[i].vertex[polyhedron[i].polygon[j].vertex_index[k]].new_index); - } - fprintf(fpo,"\n"); - } - } - } - - fclose(fpo); - -#ifdef WIN32 - _chmod(filename, _S_IREAD | _S_IWRITE ); -#else - chmod(filename,00644); -#endif - - if (write_verbose == yes) - printf("Done.\n"); - - return code_fine; - -} - - - -ReturnCode write_old_ascii_file(char filename[], PolyhedronStruct polyhedron[], - int num_polyhedra, NormOptions* opt) -{ - - int i, j, k, num_vertices, num_polygons, num_written; - SBoolean write_verbose = yes; - FILE* fpo; - - if (polyhedron == NULL) - return code_fine; - - if ((fpo = simm_fopen(filename,"w")) == NULL) - { - fprintf(stderr,"Unable to open %s for output.\n", filename); - return code_bad; - } - - if (write_verbose == yes) - { - printf("Writing old ascii file %s ... ", filename); - fflush(stdout); - } - - num_vertices = 0; - num_polygons = 0; - for (i=0; ivertex_offset; - for (i=0; i= num_written) - printf("writing %d\n", polyhedron[i].vertex[polyhedron[i].polygon[j].vertex_index[k]].new_index); - } - fprintf(fpo,"\n"); - } - } - } - - fclose(fpo); - -#ifdef WIN32 - _chmod(filename, _S_IREAD | _S_IWRITE ); -#else - chmod(filename,00644); -#endif - - if (write_verbose == yes) - printf("Done.\n"); - - return code_fine; - -} - - - -ReturnCode write_binary_separates(char filename[], PolyhedronStruct polyhedron[], - int num_polyhedra, int vertex_offset) -{ - int i, j, k, fd, vnum, num_edges, max_vertex_count; - int num_vertices, num_polygons, num_written; - SBoolean write_verbose = yes; - float fnorm[3]; - char buffer[CHARBUFFER]; - BoundingCube bc; - - for (i=0; i max_vertex_count) - max_vertex_count = polyhedron[i].polygon[j].num_vertices; - } - } - - bc.x1 = bc.x2 = polyhedron[0].vertex[0].coord[0]; - bc.y1 = bc.y2 = polyhedron[0].vertex[0].coord[1]; - bc.z1 = bc.z2 = polyhedron[0].vertex[0].coord[2]; - - for (j=0; j bc.x2) - bc.x2 = polyhedron[i].vertex[j].coord[0]; - if (polyhedron[i].vertex[j].coord[1] < bc.y1) - bc.y1 = polyhedron[i].vertex[j].coord[1]; - if (polyhedron[i].vertex[j].coord[1] > bc.y2) - bc.y2 = polyhedron[i].vertex[j].coord[1]; - if (polyhedron[i].vertex[j].coord[2] < bc.z1) - bc.z1 = polyhedron[i].vertex[j].coord[2]; - if (polyhedron[i].vertex[j].coord[2] > bc.z2) - bc.z2 = polyhedron[i].vertex[j].coord[2]; - } - - /* Filekey is used to mark the file as a SIMM/norm binary file */ - - write(fd,(char*)&filekey,sizeof(int)); - - num_vertices = polyhedron[i].num_vertices - polyhedron[i].num_removed_vertices; - num_polygons = polyhedron[i].num_polygons - polyhedron[i].num_removed_polygons; - - write_binary(fd,(char*)&num_vertices,sizeof(int)); - write_binary(fd,(char*)&num_polygons,sizeof(int)); - write_binary(fd,(char*)&num_edges,sizeof(int)); - write_binary(fd,(char*)&max_vertex_count,sizeof(int)); - write_binary(fd,(char*)&bc.x1,sizeof(double)); - write_binary(fd,(char*)&bc.x2,sizeof(double)); - write_binary(fd,(char*)&bc.y1,sizeof(double)); - write_binary(fd,(char*)&bc.y2,sizeof(double)); - write_binary(fd,(char*)&bc.z1,sizeof(double)); - write_binary(fd,(char*)&bc.z2,sizeof(double)); - - for (j=0, num_written=0; j max_vertex_count) - max_vertex_count = polyhedron[i].polygon[j].num_vertices; - } - } - - fprintf(fpo,"%d %d\n", polyhedron[i].num_vertices - polyhedron[i].num_removed_vertices, - polyhedron[i].num_polygons - polyhedron[i].num_removed_polygons); - - fprintf(fpo,"%lf %lf %lf %lf %lf %lf\n", polyhedron[i].bc.x1, polyhedron[i].bc.x2, - polyhedron[i].bc.y1, polyhedron[i].bc.y2, - polyhedron[i].bc.z1, polyhedron[i].bc.z2); - - for (j=0, num_written=0; jvertex_offset); - fprintf(fpo,"\n"); - } - fclose(fpo); - -#ifdef WIN32 - _chmod(buffer, _S_IREAD | _S_IWRITE ); -#else - chmod(buffer,00644); -#endif - - if (write_verbose == yes) - printf("Done.\n"); - } - - return code_fine; - -} - -#if ! ENGINE -/* ------------------------------------------------------------------------- - build_file_list_from_pattern - given a file pathname with optional wildcard - characters, this routine returns an argv-style list of file pathnames - that match the specified pattern. - - NOTE: This routine can append to an existing list of file names. If *list - is non-NULL, then this routine assumes it already points to a previously - allocated argv-style list, and appends to it. If *list is NULL, then - this routine allocates the list and it is up to the caller to free it. ----------------------------------------------------------------------------- */ -void build_file_list_from_pattern (const char* pattern, char*** list, int* numFiles) -{ - int n; - - char* fileList = glutExpandFilenamePattern(pattern, &n); - - if (fileList) - { - if (n > 0) - { - ReturnCode rc; - - int i = *numFiles; - - *numFiles += n; - - if (list) - { - if (*list) - *list = simm_realloc(*list, *numFiles * sizeof(char*), &rc); - else - *list = simm_malloc(*numFiles * sizeof(char*)); - - if (*list) - { - char* p = fileList; - - /* build list of pointers to strings: - */ - for ( ; i < *numFiles; i++) - { - mstrcpy(&(*list)[i], p); - - p += strlen(p) + 1; - } - } - } - } - free(fileList); - } -} -#endif /* ENGINE */ - -SBoolean file_exists(const char filename[]) -{ - FILE* fp; - - if (filename == NULL || strlen(filename) == 0) - return no; - - if ((fp = simm_fopen(filename, "r")) == NULL) - return no; - - fclose(fp); - - return yes; -} - -SBoolean can_create_file(const char filename[]) -{ - FILE* fp; - - if (filename == NULL || strlen(filename) == 0) - return no; - - // If the file exists, see if it can be appended (overwritten). - if (file_exists(filename) == yes) - { - if ((fp = fopen(filename, "a")) == NULL) - return 0; - - fclose(fp); - return 1; - } - else - { - // The file doesn't exist. See if it can be created. - if ((fp = fopen(filename, "w")) == NULL) - return no; - - fclose(fp); - remove(filename); - return yes; - } -} diff --git a/OpenSim/Utilities/simmToOpenSim/normio.h b/OpenSim/Utilities/simmToOpenSim/normio.h deleted file mode 100644 index f18a348002..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/normio.h +++ /dev/null @@ -1,33 +0,0 @@ -/******************************************************************************* - - NORMIO.H - - Author: Peter Loan - - Date: 23-OCT-98 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - -*******************************************************************************/ - -FileType check_file_type(char filename[]); -ReturnCode read_polyhedron(PolyhedronStruct* ph, char filename[], SBoolean run_norm); -ReturnCode read_binary_file(PolyhedronStruct* ph, char filename[]); -ReturnCode read_ascii_file(PolyhedronStruct* ph, char filename[]); -ReturnCode read_old_ascii_file(PolyhedronStruct* ph, char filename[]); -ReturnCode read_wavefront_file(PolyhedronStruct* ph, char filename[], SBoolean run_norm); -ReturnCode read_stl_ascii_file(PolyhedronStruct* ph, char filename[], SBoolean run_norm); -ReturnCode read_stl_binary_file(PolyhedronStruct* ph, char filename[], SBoolean run_norm); -ReturnCode write_binary_file(char filename[], BoundingCube* bc, - PolyhedronStruct polyhedron[], int num_polyhedra, - int vertex_offset); -ReturnCode write_ascii_file(char filename[], BoundingCube* bc, - PolyhedronStruct polyhedron[], int num_polyhedra, - NormOptions* opt); -ReturnCode write_old_ascii_file(char filename[], PolyhedronStruct polyhedron[], - int num_polyhedra, NormOptions* opt); -ReturnCode write_binary_separates(char filename[], PolyhedronStruct polyhedron[], - int num_polyhedra, int vertex_offset); -ReturnCode write_ascii_separates(char filename[], PolyhedronStruct polyhedron[], - int num_polyhedra, NormOptions* opt); diff --git a/OpenSim/Utilities/simmToOpenSim/normtools.c b/OpenSim/Utilities/simmToOpenSim/normtools.c deleted file mode 100644 index 3c018f1ad4..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/normtools.c +++ /dev/null @@ -1,4240 +0,0 @@ -/******************************************************************************* - - NORMTOOLS.C - - Author: Peter Loan - - Date: 12-SEP-96 - - Copyright (c) 1996 MusculoGraphics, Inc. - All rights reserved. - -*******************************************************************************/ -#include "universal.h" -#include "functions.h" -#include "globals.h" - -#include "normio.h" -#include "normtools.h" - - -/*************** DEFINES (for this file only) *********************************/ -#define MAX_HOLE_EDGES 5000 -#define MAX_POLY_EDGES 500 -#define SKIP_ME -9999 -#define NORM_EPSILON 0.0000000001 -#define CONVEX_EPSILON 0.000001 -#define CONVEX_EPSILON_SQR 0.00000000001 -#define COLLINEAR_EPSILON 0.001 -#define ANGLE_EPSILON 0.001 - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static char* axis_names[] = {"X","Y","Z"}; - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ -SBoolean verbose = no; - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -/* rest in normtools.h */ - - - -void norm(PolyhedronStruct* ph_in, NormOptions* opt, int* num_output, - PolyhedronStruct** ph_out) -{ - - int i; - PolyhedronStruct *ph, *ph2; - - if (check_polyhedron(ph_in) == code_bad) - { - fprintf(stderr,"Unable to norm bone."); - *num_output = 0; - *ph_out = NULL; - } - - verbose = opt->verbose_output; - - /* For testing */ - verbose = opt->verbose_output = yes; - - remove_duplicate_vertices(ph_in, opt); - - *num_output = subdivide_input_polyhedron(ph_in, ph_out); - - for (i=0; i<*num_output; i++) - { - ph = &(*ph_out)[i]; - - remove_degenerate_polygons(ph); - - fill_holes(ph,opt->fill_holes); - - check_edge_lengths(ph,opt->max_edge_length); - - convexify_polygons(ph,opt); - - triangulate_polygons(ph,opt->triangulate); - - compress_polyhedron(ph); - - find_vertex_normals(ph,opt); - - find_bounding_cube(ph,&ph->bc); - } - - /* If opt.write_separate_polyhedra is yes, then just return - * because you've already got separate bones. If not, then - * you need to merge all of the polyhedra into one. - */ - if (opt->write_separate_polyhedra == no && *num_output > 1) - { - ph = combine_polyhedra(ph_out,*num_output); - for (i=0; i<*num_output; i++) - { - ph2 = &(*ph_out)[i]; - free_polyhedron(ph2, no, NULL); - } - free(*ph_out); - *ph_out = ph; - *num_output = 1; - } -} - - -PolyhedronStruct* combine_polyhedra(PolyhedronStruct** phs, int num_phs) -{ - int i, j, k, count; - PolyhedronStruct *ph, *ph2; - - ph = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - preread_init_polyhedron(ph); - - /* Copy the name and display parameters from the first polyhedron to the combined one. */ - if ((*phs)[0].name) - mstrcpy(&ph->name, (*phs)[0].name); - ph->drawmode = (*phs)[0].drawmode; - ph->material = (*phs)[0].material; - ph->show_normals = (*phs)[0].show_normals; - - for (i=0; inum_vertices += (ph2->num_vertices - ph2->num_removed_vertices); - ph->num_polygons += (ph2->num_polygons - ph2->num_removed_polygons); - } - - ph->vertex = (VertexStruct*)simm_malloc(ph->num_vertices*sizeof(VertexStruct)); - ph->polygon = (PolygonStruct*)simm_malloc(ph->num_polygons*sizeof(PolygonStruct)); - - for (i=0, count=0; inum_vertices; j++) - { - if (ph2->vertex[j].thrown_out == no) - { - copy_vertex_lite(&ph2->vertex[j],&ph->vertex[count]); - ph2->vertex[j].new_index = count++; - } - } - } - - for (i=0, count=0; inum_polygons; j++) - { - if (ph2->polygon[j].thrown_out == no) - { - copy_polygon_lite(&ph2->polygon[j],&ph->polygon[count]); - for (k=0; kpolygon[count].num_vertices; k++) - ph->polygon[count].vertex_index[k] = - ph2->vertex[ph2->polygon[j].vertex_index[k]].new_index; - count++; - } - } - } - - make_vert_poly_lists(ph); - - for (i=0; ibc.x1 < ph->bc.x1) - ph->bc.x1 = ph2->bc.x1; - if (ph2->bc.y1 < ph->bc.y1) - ph->bc.y1 = ph2->bc.y1; - if (ph2->bc.z1 < ph->bc.z1) - ph->bc.z1 = ph2->bc.z1; - if (ph2->bc.x2 > ph->bc.x2) - ph->bc.x2 = ph2->bc.x2; - if (ph2->bc.y2 > ph->bc.y2) - ph->bc.y2 = ph2->bc.y2; - if (ph2->bc.z2 > ph->bc.z2) - ph->bc.z2 = ph2->bc.z2; - } - - return ph; -} - - -ReturnCode check_polyhedron(PolyhedronStruct* ph) -{ - int i, j; - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].num_vertices <= 0) - ph->polygon[i].thrown_out = yes; - for (j=0; jpolygon[i].num_vertices; j++) - { - if (ph->polygon[i].vertex_index[j] >= ph->num_vertices || - ph->polygon[i].vertex_index[j] < 0) - { - fprintf(stderr,"Bad polyhedron: vertex index out of range.\n"); - return code_bad; - } - } - } - - return code_fine; -} - - -void preread_init_polygon(PolygonStruct* p) -{ - memset(p, 0, sizeof(PolygonStruct)); - - p->normal_computed = no_proc; - p->selected = no; -#if POLYGON_DISPLAY_HACK - p->displayed = yes; -#endif - p->old = no; -} - - -void preread_init_vertex(VertexStruct* v, int index) -{ - memset(v, 0, sizeof(VertexStruct)); - - v->thrown_out = no; - v->old = no; - v->new_index = index; -} - - -void preread_init_polyhedron(PolyhedronStruct* ph) -{ - memset(ph, 0, sizeof(PolyhedronStruct)); - - ph->selected = no; - ph->selected_vertex = -1; - ph->selected_polygon = -1; - ph->selected_edge = -1; - - ph->drawmode = gouraud_shading; - ph->material = -1; - ph->show_normals = no; - ph->bc.x1 = MAXMDOUBLE; - ph->bc.x2 = MINMDOUBLE; - ph->bc.y1 = MAXMDOUBLE; - ph->bc.y2 = MINMDOUBLE; - ph->bc.z1 = MAXMDOUBLE; - ph->bc.z2 = MINMDOUBLE; - - reset_4x4matrix(ph->transform_matrix); - ph->tx = 0.0; - ph->ty = 0.0; - ph->tz = 0.0; - ph->sx = 1.0; - ph->sy = 1.0; - ph->sz = 1.0; -} - - -/* This function assumes that the polyhedron has just been loaded from a file, - * or is otherwise completely uninitialized. When you are done, every member - * variable is correctly initialized except for polygon and vertex normals, - * which cannot be calculated until you subdivide the polyhedron (the polygon - * normals and d values are calculated, but may have the wrong sign). - */ -void postread_init_polyhedron(PolyhedronStruct* ph, SBoolean full_init) -{ - int i; - - for (i=0; inum_polygons; i++) - { - calc_polygon_normal(ph, &ph->polygon[i]); - calc_polygon_bounding_cube(ph, &ph->polygon[i]); - } - - make_vert_poly_lists(ph); - - if (full_init == yes) - calc_polyhedron_bounding_cube(ph); -} - - -PolyhedronStruct* clone_polyhedron(PolyhedronStruct* from) -{ - int i, j; - PolyhedronStruct* to; - - to = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - - mstrcpy(&to->name,from->name); - - to->num_vertices = from->num_vertices; - to->vertex = (VertexStruct*)simm_malloc(to->num_vertices*sizeof(VertexStruct)); - - to->num_polygons = from->num_polygons; - to->polygon = (PolygonStruct*)simm_malloc(to->num_polygons*sizeof(PolygonStruct)); - - for (i=0; inum_vertices; i++) - copy_vertex(&from->vertex[i],&to->vertex[i]); - - for (i=0; inum_polygons; i++) - copy_polygon(&from->polygon[i],&to->polygon[i]); - - to->num_removed_vertices = from->num_removed_vertices; - to->num_removed_polygons = from->num_removed_polygons; - - to->tx = from->tx; - to->ty = from->ty; - to->tz = from->tz; - - to->sx = from->sx; - to->sy = from->sy; - to->sz = from->sz; - - for (i=0; i<4; i++) - for (j=0; j<4; j++) - to->transform_matrix[i][j] = from->transform_matrix[i][j]; - - to->selected = from->selected; - to->selected_vertex = from->selected_vertex; - to->selected_polygon = from->selected_polygon; - to->selected_edge = from->selected_edge; - - to->drawmode = from->drawmode; - to->material = from->material; - to->show_normals = from->show_normals; - - to->bc.x1 = from->bc.x1; - to->bc.x2 = from->bc.x2; - to->bc.y1 = from->bc.y1; - to->bc.y2 = from->bc.y2; - to->bc.z1 = from->bc.z1; - to->bc.z2 = from->bc.z2; - - return to; -} - - -void copy_polyhedron(PolyhedronStruct* from, PolyhedronStruct* to) -{ - int i, j; - - mstrcpy(&to->name, from->name); - - to->num_vertices = from->num_vertices; - to->vertex = (VertexStruct*)simm_malloc(to->num_vertices*sizeof(VertexStruct)); - - to->num_polygons = from->num_polygons; - to->polygon = (PolygonStruct*)simm_malloc(to->num_polygons*sizeof(PolygonStruct)); - - for (i=0; inum_vertices; i++) - copy_vertex(&from->vertex[i],&to->vertex[i]); - - for (i=0; inum_polygons; i++) - copy_polygon(&from->polygon[i],&to->polygon[i]); - - to->num_removed_vertices = from->num_removed_vertices; - to->num_removed_polygons = from->num_removed_polygons; - - to->tx = from->tx; - to->ty = from->ty; - to->tz = from->tz; - - to->sx = from->sx; - to->sy = from->sy; - to->sz = from->sz; - - for (i=0; i<4; i++) - for (j=0; j<4; j++) - to->transform_matrix[i][j] = from->transform_matrix[i][j]; - - to->selected = from->selected; - to->selected_vertex = from->selected_vertex; - to->selected_polygon = from->selected_polygon; - to->selected_edge = from->selected_edge; - to->drawmode = from->drawmode; - to->material = from->material; - to->show_normals = from->show_normals; - to->gl_display = from->gl_display; - - to->bc.x1 = from->bc.x1; - to->bc.x2 = from->bc.x2; - to->bc.y1 = from->bc.y1; - to->bc.y2 = from->bc.y2; - to->bc.z1 = from->bc.z1; - to->bc.z2 = from->bc.z2; -} - - -void copy_vertex(VertexStruct* from, VertexStruct* to) -{ - memcpy(to, from, sizeof(VertexStruct)); - - if (to->polygon_count > 0) - { - int i; - - to->polygons = (int*)simm_malloc(to->polygon_count * sizeof(int)); - for (i=0; ipolygon_count; i++) - to->polygons[i] = from->polygons[i]; - } - else - { - to->polygons = NULL; - } -} - - -void copy_vertex_lite(VertexStruct* from, VertexStruct* to) -{ - memcpy(to, from, sizeof(VertexStruct)); - to->polygon_count = 0; - to->polygons = NULL; -} - - -void copy_pintersegmentstruct(PInterSegmentStruct* from, PInterSegmentStruct* to) -{ - to->ptseg[0][0] = from->ptseg[0][0]; - to->ptseg[0][1] = from->ptseg[0][1]; - to->ptseg[0][2] = from->ptseg[0][2]; - to->ptseg[1][0] = from->ptseg[1][0]; - to->ptseg[1][1] = from->ptseg[1][1]; - to->ptseg[1][2] = from->ptseg[1][2]; - to->vert_edge[0] = from->vert_edge[0]; - to->vert_edge[1] = from->vert_edge[1]; - to->vertex_index[0] = from->vertex_index[0]; - to->vertex_index[1] = from->vertex_index[1]; - to->vindices[0][0] = from->vindices[0][0]; - to->vindices[0][1] = from->vindices[0][1]; - to->vindices[1][0] = from->vindices[1][0]; - to->vindices[1][1] = from->vindices[1][1]; - to->poly_index = from->poly_index; - to->marked = from->marked; -} - - -void copy_pintersegmentliststruct(PInterSegmentListStruct* from, - PInterSegmentListStruct* to) -{ - int i; - - to->num_inter_seg = from->num_inter_seg; - if (to->num_inter_seg > 0) - { - to->seg = (PInterSegmentStruct*)simm_malloc(to->num_inter_seg*sizeof(PInterSegmentStruct)); - for (i=0; inum_inter_seg; i++) - copy_pintersegmentstruct(&from->seg[i],&to->seg[i]); - } - else - { - to->seg = NULL; - } - to->segmaxx = NULL; /* TODO: should this point to the last element? */ -} - - -void copy_polygon(PolygonStruct* from, PolygonStruct* to) -{ - int i; - - to->num_vertices = from->num_vertices; - to->vertex_index = (int*)simm_malloc(to->num_vertices*sizeof(int)); - for (i=0; inum_vertices; i++) - to->vertex_index[i] = from->vertex_index[i]; - to->normal[0] = from->normal[0]; - to->normal[1] = from->normal[1]; - to->normal[2] = from->normal[2]; - to->d = from->d; - to->normal_computed = from->normal_computed; - to->selected = from->selected; -#if POLYGON_DISPLAY_HACK - to->displayed = from->displayed; -#endif - to->polyhedron_number = from->polyhedron_number; - to->thrown_out = from->thrown_out; - to->old = from->old; - to->ordering_value = from->ordering_value; - to->bc.x1 = from->bc.x1; - to->bc.x2 = from->bc.x2; - to->bc.y1 = from->bc.y1; - to->bc.y2 = from->bc.y2; - to->bc.z1 = from->bc.z1; - to->bc.z2 = from->bc.z2; - to->boolcode.coplanar_flag = from->boolcode.coplanar_flag; - to->boolcode.polygon_mark = from->boolcode.polygon_mark; - to->boolcode.poly_output = from->boolcode.poly_output; - to->boolcode.poly_adjpush = from->boolcode.poly_adjpush; - to->boolcode.num_inters = from->boolcode.num_inters; - for (i=0; iboolcode.num_inters; i++) - to->boolcode.inters[i] = from->boolcode.inters[i]; - copy_pintersegmentliststruct(&from->boolcode.seg_list,&to->boolcode.seg_list); - to->boolcode.seglst_num = from->boolcode.seglst_num; - if (to->boolcode.seglst_num > 0) - { - to->boolcode.seglst = (PInterSegmentListStruct*)simm_malloc( - to->boolcode.seglst_num*sizeof(PInterSegmentListStruct)); - for (i=0; iboolcode.seglst_num; i++) - copy_pintersegmentliststruct(&from->boolcode.seglst[i],&to->boolcode.seglst[i]); - } - else - { - to->boolcode.seglst = NULL; - } -} - - -void copy_polygon_lite(PolygonStruct* from, PolygonStruct* to) -{ - int i; - - to->num_vertices = from->num_vertices; - to->vertex_index = (int*)simm_malloc(to->num_vertices*sizeof(int)); - for (i=0; inum_vertices; i++) - to->vertex_index[i] = from->vertex_index[i]; - to->normal[0] = from->normal[0]; - to->normal[1] = from->normal[1]; - to->normal[2] = from->normal[2]; - to->d = from->d; - to->normal_computed = from->normal_computed; - to->selected = from->selected; -#if POLYGON_DISPLAY_HACK - to->displayed = from->displayed; -#endif - to->polyhedron_number = from->polyhedron_number; - to->thrown_out = from->thrown_out; - to->old = from->old; - to->ordering_value = from->ordering_value; - - to->bc.x1 = from->bc.x1; - to->bc.x2 = from->bc.x2; - to->bc.y1 = from->bc.y1; - to->bc.y2 = from->bc.y2; - to->bc.z1 = from->bc.z1; - to->bc.z2 = from->bc.z2; - - to->boolcode.coplanar_flag = 0; - to->boolcode.polygon_mark = 0; - to->boolcode.poly_output = 0; - to->boolcode.poly_adjpush = 0; - to->boolcode.num_inters = 0; - to->boolcode.seg_list.num_inter_seg = 0; - to->boolcode.seglst_num = 0; - to->boolcode.seglst = NULL; -//dkb sept 2009 - to->boolcode.seg_list.seg = NULL; - to->boolcode.seg_list.segmaxx = NULL; -} - - -void calc_polygon_bounding_cube(PolyhedronStruct* ph, PolygonStruct* p) -{ - - int i; - VertexStruct* v; - - p->bc.x1 = MAXMDOUBLE; - p->bc.x2 = MINMDOUBLE; - p->bc.y1 = MAXMDOUBLE; - p->bc.y2 = MINMDOUBLE; - p->bc.z1 = MAXMDOUBLE; - p->bc.z2 = MINMDOUBLE; - - for (i=0; inum_vertices; i++) - { - v = &ph->vertex[p->vertex_index[i]]; - if (v->coord[0] < p->bc.x1) - p->bc.x1 = v->coord[0]; - if (v->coord[0] > p->bc.x2) - p->bc.x2 = v->coord[0]; - if (v->coord[1] < p->bc.y1) - p->bc.y1 = v->coord[1]; - if (v->coord[1] > p->bc.y2) - p->bc.y2 = v->coord[1]; - if (v->coord[2] < p->bc.z1) - p->bc.z1 = v->coord[2]; - if (v->coord[2] > p->bc.z2) - p->bc.z2 = v->coord[2]; - } - -} - - -void calc_polyhedron_bounding_cube(PolyhedronStruct* ph) -{ - - int i; - - ph->bc.x1 = MAXMDOUBLE; - ph->bc.x2 = MINMDOUBLE; - ph->bc.y1 = MAXMDOUBLE; - ph->bc.y2 = MINMDOUBLE; - ph->bc.z1 = MAXMDOUBLE; - ph->bc.z2 = MINMDOUBLE; - - for (i=0; inum_vertices; i++) - { - if (ph->vertex[i].coord[0] < ph->bc.x1) - ph->bc.x1 = ph->vertex[i].coord[0]; - if (ph->vertex[i].coord[0] > ph->bc.x2) - ph->bc.x2 = ph->vertex[i].coord[0]; - - if (ph->vertex[i].coord[1] < ph->bc.y1) - ph->bc.y1 = ph->vertex[i].coord[1]; - if (ph->vertex[i].coord[1] > ph->bc.y2) - ph->bc.y2 = ph->vertex[i].coord[1]; - - if (ph->vertex[i].coord[2] < ph->bc.z1) - ph->bc.z1 = ph->vertex[i].coord[2]; - if (ph->vertex[i].coord[2] > ph->bc.z2) - ph->bc.z2 = ph->vertex[i].coord[2]; - } - -} - - -/* This routine finds all the polygons that each vertex is a member of. - * It's not terribly efficient right now. First it scans all the polygons - * to find out how much space to malloc for each vertex, then it scans - * them again to fill in the polygons[] lists. - */ - -void make_vert_poly_lists(PolyhedronStruct* ph) -{ - int i, j; - VertexStruct* v; - - for (i=0; inum_vertices; i++) - { - FREE_IFNOTNULL(ph->vertex[i].polygons); - ph->vertex[i].polygon_count = 0; - } - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - for (j=0; jpolygon[i].num_vertices; j++) - ph->vertex[ph->polygon[i].vertex_index[j]].polygon_count++; - } - - for (i=0; inum_vertices; i++) - { - if (ph->vertex[i].thrown_out == yes) - continue; - if (ph->vertex[i].polygon_count == 0) - { - throw_out_vertex(ph,i); - ph->vertex[i].polygons = NULL; - } - else - { - ph->vertex[i].polygons = (int*)simm_malloc(ph->vertex[i].polygon_count*sizeof(int)); - ph->vertex[i].polygon_count = 0; - } - } - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - for (j=0; jpolygon[i].num_vertices; j++) - { - v = &ph->vertex[ph->polygon[i].vertex_index[j]]; - v->polygons[v->polygon_count++] = i; - } - } -} - - -/* This routine finds the other polygon (not p1) that shares the - * edge from vertices v1 to v2. - */ - -int find_other_polygon(PolyhedronStruct* ph, int p1, int v1, int v2) -{ - - int i, j; - VertexStruct *vs1, *vs2; - - vs1 = &ph->vertex[v1]; - vs2 = &ph->vertex[v2]; - - for (i=0; ipolygon_count; i++) - { - if (vs1->polygons[i] == p1) - continue; - for (j=0; jpolygon_count; j++) - { - if ((vs1->polygons[i] == vs2->polygons[j]) && - (ph->polygon[vs1->polygons[i]].thrown_out == no)) - return (vs1->polygons[i]); - } - } - - return (-1); - -} - - -int subdivide_input_polyhedron(PolyhedronStruct* ip, PolyhedronStruct** phs) -{ - int i, j, ph_num, polys_used_so_far, verts_used_so_far, num_polyhedra; - int read_pos, write_pos, *poly_list; - VertexStruct *v_new, *v_old; - PolyhedronStruct *ph; - PolygonStruct *p_new, *p_old; - ReturnCode rc; - - if (verbose == yes) - { - printf("Separating out polyhedra... "); - fflush(stdout); - } - - /* So that you don't have to keep checking for thrown_out polygons, - * go thru the polygon list once and give all thrown_out polygons a - * polyhedron_number of SKIP_ME. - */ - - for (i=0; inum_polygons; i++) - { - if (ip->polygon[i].thrown_out == yes) - ip->polygon[i].polyhedron_number = SKIP_ME; - else - ip->polygon[i].polyhedron_number = 0; - } - - for (i=0; inum_vertices; i++) - ip->vertex[i].polyhedron_number = 0; - - poly_list = (int*)simm_malloc(ip->num_polygons*sizeof(int)); - - num_polyhedra = 0; - polys_used_so_far = 0; - verts_used_so_far = ip->num_removed_vertices; - - while (1) - { - /* Find a polygon whose polyhedron_number is 0 (unprocessed) */ - - for (i=0; inum_polygons; i++) - { - if (ip->polygon[i].polyhedron_number == 0) - break; - } - - if (i == ip->num_polygons) - break; - - ph_num = ++num_polyhedra; - ip->polygon[i].polyhedron_number = ph_num; - poly_list[0] = i; - read_pos = 0; - write_pos = 1; - - if (num_polyhedra == 1) - (*phs) = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - else - (*phs) = (PolyhedronStruct*)simm_realloc((*phs),num_polyhedra*sizeof(PolyhedronStruct),&rc); - - ph = &(*phs)[ph_num-1]; - - preread_init_polyhedron(ph); - mstrcpy(&ph->name,ip->name); - ph->drawmode = ip->drawmode; - ph->material = ip->material; - ph->show_normals = ip->show_normals; - - ph->polygon = (PolygonStruct*)simm_malloc((ip->num_polygons-polys_used_so_far)* - sizeof(PolygonStruct)); - ph->vertex = (VertexStruct*)simm_malloc((ip->num_vertices-verts_used_so_far)* - sizeof(VertexStruct)); - - for (i=0; inum_polygons-polys_used_so_far; i++) - preread_init_polygon(&ph->polygon[i]); - for (i=0; inum_vertices-verts_used_so_far; i++) - preread_init_vertex(&ph->vertex[i],i); - - while (read_pos < write_pos) - { - p_old = &ip->polygon[poly_list[read_pos++]]; - - /* For each vertex in this polygon, - * 1. copy the vertex info to the new polyhedron structure - * 2. put all the polygons that use this vertex into poly_list. - */ - for (i=0; inum_vertices; i++) - { - v_old = &ip->vertex[p_old->vertex_index[i]]; - if (v_old->polyhedron_number != 0) - { - /* This vertex has already been added to the new polyhedron, - * so just copy its new index into the old polygon (so that - * it gets copied into the new polygon later). - */ - p_old->vertex_index[i] = v_old->new_index; - continue; - } - v_old->polyhedron_number = ph_num; - v_new = &ph->vertex[ph->num_vertices]; - copy_vertex(v_old,v_new); - v_new->polyhedron_number = 0; - v_new->new_index = ph->num_vertices; - v_old->new_index = ph->num_vertices; - p_old->vertex_index[i] = ph->num_vertices; - for (j=0; jpolygon_count; j++) - { - if (ip->polygon[v_old->polygons[j]].polyhedron_number == 0) // crash here when applying two deform objects and norming in BE - { - poly_list[write_pos++] = v_old->polygons[j]; - ip->polygon[v_old->polygons[j]].polyhedron_number = ph_num; - } - } - ph->num_vertices++; - } - - /* Now copy the polygon to the new polyhedron structure */ - p_new = &ph->polygon[ph->num_polygons++]; - copy_polygon(p_old,p_new); - p_new->polyhedron_number = 0; - } - - /* Now that you're done making this polyhedron, realloc the polygon and vertex arrays. */ - ph->polygon = (PolygonStruct*)simm_realloc(ph->polygon,ph->num_polygons*sizeof(PolygonStruct),&rc); - ph->vertex = (VertexStruct*)simm_realloc(ph->vertex,ph->num_vertices*sizeof(VertexStruct),&rc); - - make_vert_poly_lists(ph); - calc_polyhedron_bounding_cube(ph); - ph->num_removed_vertices = 0; - ph->num_removed_polygons = 0; - - polys_used_so_far += ph->num_polygons; - verts_used_so_far += ph->num_vertices; - } - - free(poly_list); - - if (verbose == yes) - printf("Done.\n"); - - return num_polyhedra; -} - - -SBoolean vertex_in_polygon(PolygonStruct* polygon, int v_num) -{ - int i; - - for (i=0; inum_vertices; i++) - if (polygon->vertex_index[i] == v_num) - return yes; - - return no; -} - - -/* This function assumes that the polygon normals have already been calculated */ - -void find_vertex_normals(PolyhedronStruct* ph, NormOptions* opt) -{ - - int i, poly_num; - - if (ph->num_polygons < 4 || ph->num_vertices < 4) - { - if (verbose == yes) - { -/* - printf("Bad polyhedron... "); - print_polyhedron(ph,""); -*/ - } - for (i=0; inum_vertices; i++) - { - if (ph->vertex[i].thrown_out == no && ph->vertex[i].polygon_count > 0) - { - poly_num = ph->vertex[i].polygons[0]; - ph->vertex[i].normal[0] = ph->polygon[poly_num].normal[0]; - ph->vertex[i].normal[1] = ph->polygon[poly_num].normal[1]; - ph->vertex[i].normal[2] = ph->polygon[poly_num].normal[2]; - } - else - { - ph->vertex[i].normal[0] = 0.0; - ph->vertex[i].normal[1] = 0.0; - ph->vertex[i].normal[2] = 1.0; - } - } - } - - if (opt->vertex_order == clockwise) - { - if (verbose == yes) - printf(" Assuming vertices have clockwise ordering within each polygon.\n"); - for (i=0; inum_polygons; i++) - reverse_vertex_order(&ph->polygon[i]); - } - else if (opt->vertex_order == counterclockwise) - { - /* order is already correct */ - if (verbose == yes) - printf(" Assuming vertices have counterclockwise ordering within each polygon.\n"); - } - else - { - make_polygons_consistent(ph,opt); - } - - /* Now that the vertex ordering is consistent for every polygon, - * use the polygon normals to find the vertex normals. - */ - - calc_vertex_normals(ph,opt); - -} - - -void make_polygons_consistent(PolyhedronStruct* ph, NormOptions* opt) -{ - - int top_vertex, shallowest_vertex, num_tries; - int polygon1, polygon2, poly_num; - static double yaxis[] = {0.0,1.0,0.0}; - double angle1, angle2, b_angle1, b_angle2, ref_mag; - double norm_edge1[3], norm_edge2[3], proj_yaxis[3]; - double proj_norm_edge1[3], proj_norm_edge2[3]; - PlaneStruct plane; - double rot_matrix[3][3]; - - num_tries = 0; - reset_3x3matrix(rot_matrix); - - while (1) - { - top_vertex = find_top_vertex(ph->vertex,ph->num_vertices); - - shallowest_vertex = find_shallowest_vertex(ph,top_vertex); - - find_polygons_sharing_edge(ph,top_vertex,shallowest_vertex, - &polygon1,&polygon2); - - if (polygon1 != -1 && polygon2 != -1) - { - break; - } - else if (num_tries > 21) - { - /* You just tried to rotate the polyhedron 20 times to find - * a convex section (two polygons sharing the shallowest edge - * descending from the topmost vertex), and failed. - */ - if (verbose == yes) - printf(" Couldn't find two faces sharing shallowest edge (%d %d).\n", - top_vertex+1, shallowest_vertex+1); - unorient_polyhedron(ph,rot_matrix); - if (polygon1 == -1 && polygon2 == -1) - { - if (verbose == yes) - { - printf(" ...badly formed polyhedron.\n"); - printf(" Assuming polygons are already in counterclockwise order.\n"); - printf(" Use \"-cw\" if they are clockwise.\n"); - } - } - else - { - ref_mag = VECTOR_MAGNITUDE(opt->reference_normal); - poly_num = (polygon1 == -1) ? polygon2 : polygon1; - if (EQUAL_WITHIN_ERROR(ref_mag,0.0)) - { - if (verbose == yes) - { - printf(" ...assuming that polygon %d faces up (+Y).\n", poly_num); - printf(" use \"-rn\" to specify other direction.\n"); - } - opt->reference_normal[0] = 0.0; - opt->reference_normal[1] = 1.0; - opt->reference_normal[2] = 0.0; - } - else - { - if (verbose == yes) - printf(" ...using reference normal to orient polygon %d.\n", poly_num); - } - if (DOT_VECTORS(ph->polygon[poly_num].normal,opt->reference_normal) < 0.0) - reverse_vertex_order(&ph->polygon[poly_num]); - order_all_vertex_lists(ph,poly_num); - } - return; - } - else - { - reorient_polyhedron(ph,rot_matrix); - num_tries++; - } - } - - match_vertex_orders(&ph->polygon[polygon1],&ph->polygon[polygon2], - top_vertex,shallowest_vertex); - - angle1 = compute_angle_between_vectors(ph->polygon[polygon1].normal,yaxis); - angle2 = compute_angle_between_vectors(ph->polygon[polygon2].normal,yaxis); - - if (angle1 <= M_PI_2 && angle2 <= M_PI_2) - { - order_all_vertex_lists(ph,polygon1); - } - else if (angle1 > M_PI_2 && angle2 > M_PI_2) - { - reverse_vertex_order(&ph->polygon[polygon1]); - order_all_vertex_lists(ph,polygon1); - } - else - { - find_other_edge(ph,polygon1,top_vertex,shallowest_vertex,norm_edge1); - find_other_edge(ph,polygon2,top_vertex,shallowest_vertex,norm_edge2); - find_plane_normal_to_line(&plane,ph->vertex[top_vertex].coord, - ph->vertex[shallowest_vertex].coord); - plane.d = 0.0; - project_point_onto_plane(yaxis,&plane,proj_yaxis); - project_point_onto_plane(norm_edge1,&plane,proj_norm_edge1); - project_point_onto_plane(norm_edge2,&plane,proj_norm_edge2); - b_angle1 = compute_angle_between_vectors(proj_norm_edge1,proj_yaxis); - b_angle2 = compute_angle_between_vectors(proj_norm_edge2,proj_yaxis); - if ((b_angle1 < b_angle2 && angle1 <= M_PI_2) || - (b_angle1 >= b_angle2 && angle1 > M_PI_2)) - { - order_all_vertex_lists(ph,polygon1); - } - else - { - reverse_vertex_order(&ph->polygon[polygon1]); - order_all_vertex_lists(ph,polygon1); - } - } - - /* Now that you have ordered all the polygons, rotate the vertices - * back to where they started from. - */ - - if (num_tries > 0) - unorient_polyhedron(ph,rot_matrix); - -} - - -void calc_vertex_normals(PolyhedronStruct* ph, NormOptions* opt) -{ - - int i, j, vertex; - double magnitude; - - /* A vertex normal is the average of the polygon normals for all polygons - * containing that vertex. Scan the polygon lists, and add that polygon's - * normal to all the vertices that are in the polygon. You don't need to - * keep track of how many you're adding since you're going to normalize - * the result anyway. This code assumes that the polygon normals have - * already been computed and made consistent with each other. - */ - - for (i=0; inum_vertices; i++) - { - ph->vertex[i].normal[0] = 0.0; - ph->vertex[i].normal[1] = 0.0; - ph->vertex[i].normal[2] = 0.0; - } - - for (i=0; inum_polygons; i++) - { - for (j=0; jpolygon[i].num_vertices; j++) - { - vertex = ph->polygon[i].vertex_index[j]; - ph->vertex[vertex].normal[0] += ph->polygon[i].normal[0]; - ph->vertex[vertex].normal[1] += ph->polygon[i].normal[1]; - ph->vertex[vertex].normal[2] += ph->polygon[i].normal[2]; - } - } - - for (i=0; inum_vertices; i++) - { - magnitude = normalize_vector(ph->vertex[i].normal,ph->vertex[i].normal); - if (EQUAL_WITHIN_ERROR(magnitude,0.0)) - { - ph->vertex[i].normal[0] = opt->reference_normal[0]; - ph->vertex[i].normal[1] = opt->reference_normal[1]; - ph->vertex[i].normal[2] = opt->reference_normal[2]; - } - } - -} - - -void order_all_vertex_lists(PolyhedronStruct* ph, int polygon) -{ - - int i, start_polygon, vertex2, p2; - PolygonStruct* poly; - VertexOrder order; - - ph->polygon[polygon].normal_computed = yes_proc; - - while (1) - { - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].normal_computed == yes_proc) - { - start_polygon = i; - break; - } - } - - if (i == ph->num_polygons) - break; - - poly = &ph->polygon[start_polygon]; - for (i=0; inum_vertices; i++) - { - if (i == poly->num_vertices-1) - vertex2 = poly->vertex_index[0]; - else - vertex2 = poly->vertex_index[i+1]; - p2 = find_other_polygon(ph,start_polygon, - poly->vertex_index[i],vertex2); - if (p2 != -1 && ph->polygon[p2].normal_computed == no_proc) - { - order = polygon_contains_edge(&ph->polygon[p2],poly->vertex_index[i],vertex2); - if (order == clockwise) - reverse_vertex_order(&ph->polygon[p2]); - ph->polygon[p2].normal_computed = yes_proc; - } - } - poly->normal_computed = yes_and_propagated; - } - -} - - -void calc_polygon_normal(PolyhedronStruct* ph, PolygonStruct* p) -{ - int i, v1, v2, v3, nv; - double vec1[3], vec2[3]; - double mag; - - // Find the first three vertices which are not collinear, - // and use them to find the surface normal. - nv = p->num_vertices; - - for (i=0; ivertex_index[i]; - v2 = p->vertex_index[(i+1)%nv]; - v3 = p->vertex_index[(i+2)%nv]; - - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v1].coord,vec2); - - normalize_vector(vec1,vec1); - normalize_vector(vec2,vec2); - cross_vectors(vec1,vec2,p->normal); - mag = normalize_vector(p->normal,p->normal); - if (mag >= COLLINEAR_EPSILON) - break; - } - - if (i == nv) - { - p->normal[0] = 1.0; - p->normal[1] = 0.0; - p->normal[2] = 0.0; - p->d = 0.0; - } - else - { - p->d = - ph->vertex[v1].coord[0] * p->normal[0] - - ph->vertex[v1].coord[1] * p->normal[1] - - ph->vertex[v1].coord[2] * p->normal[2]; - } -} - - -/* This routine makes consistent (matches) the vertex orders of the two polygons - * which share the edge between vertex1 and vertex2. - */ - -void match_vertex_orders(PolygonStruct* polygon1, PolygonStruct* polygon2, - int vertex1, int vertex2) -{ - - int order1, order2; - - order1 = polygon_contains_edge(polygon1,vertex1,vertex2); - order2 = polygon_contains_edge(polygon2,vertex2,vertex1); - - if (order1 != order2) - reverse_vertex_order(polygon2); - -} - - -void reverse_vertex_order(PolygonStruct* p) -{ - - int i, j, tmpvertex; - - for (i=0, j=p->num_vertices-1; ivertex_index[i]; - p->vertex_index[i] = p->vertex_index[j]; - p->vertex_index[j] = tmpvertex; - } - - /* Assuming that the polygon's normal has been computed, it needs to - * be reversed as well. - */ - - p->normal[0] = -p->normal[0]; - p->normal[1] = -p->normal[1]; - p->normal[2] = -p->normal[2]; - p->d = -p->d; - -} - - -void find_polygons_sharing_edge(PolyhedronStruct* ph, int vertex1, int vertex2, - int* polygon1, int* polygon2) -{ - - int i, j, polygons_found_so_far = 0; - - *polygon1 = *polygon2 = -1; - - for (i=0; ivertex[vertex1].polygon_count; i++) - { - for (j=0; jvertex[vertex2].polygon_count; j++) - { - if (ph->vertex[vertex1].polygons[i] == ph->vertex[vertex2].polygons[j]) - { - if (polygons_found_so_far == 0) - { - *polygon1 = ph->vertex[vertex1].polygons[i]; - polygons_found_so_far = 1; - } - else - { - *polygon2 = ph->vertex[vertex1].polygons[i]; - return; - } - } - } - } - -} - - -VertexOrder polygon_contains_edge(PolygonStruct* p, int vertex1, int vertex2) -{ - int i; - - for (i=0; inum_vertices; i++) - if (p->vertex_index[i] == vertex1) - break; - - if (i == p->num_vertices) - return unspecified_order; - - if (i == 0) - { - if (p->vertex_index[1] == vertex2) - return clockwise; - if (p->vertex_index[p->num_vertices-1] == vertex2) - return counterclockwise; - return unspecified_order; - } - - if (i == p->num_vertices-1) - { - if (p->vertex_index[0] == vertex2) - return clockwise; - if (p->vertex_index[i-1] == vertex2) - return counterclockwise; - return unspecified_order; - } - - if (p->vertex_index[i+1] == vertex2) - return clockwise; - if (p->vertex_index[i-1] == vertex2) - return counterclockwise; - return unspecified_order; -} - - -int find_top_vertex(VertexStruct* vertex, int num_vertices) -{ - int i, index_of_top_vertex; - double max_y_value; - - max_y_value = vertex[0].coord[YY]; - index_of_top_vertex = 0; - - for (i=1; i max_y_value) - { - max_y_value = vertex[i].coord[YY]; - index_of_top_vertex = i; - } - } - - return index_of_top_vertex; -} - - -int find_shallowest_vertex(PolyhedronStruct* ph, int top_vertex) -{ - int i, shallowest_vertex = 0, previous_vertex, next_vertex; - double x_length, y_length, z_length, y_slope, min_slope = MAXMDOUBLE; - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - if (get_adjacent_vertices(&ph->polygon[i], top_vertex, &previous_vertex, &next_vertex) == found_none) - continue; - - x_length = ph->vertex[top_vertex].coord[XX] - ph->vertex[previous_vertex].coord[XX]; - y_length = ph->vertex[top_vertex].coord[YY] - ph->vertex[previous_vertex].coord[YY]; - z_length = ph->vertex[top_vertex].coord[ZZ] - ph->vertex[previous_vertex].coord[ZZ]; - - y_slope = y_length/sqrt((x_length)*(x_length)+(z_length)*(z_length)); - if (y_slope < min_slope) - { - min_slope = y_slope; - shallowest_vertex = previous_vertex; - } - - x_length = ph->vertex[top_vertex].coord[XX] - ph->vertex[next_vertex].coord[XX]; - y_length = ph->vertex[top_vertex].coord[YY] - ph->vertex[next_vertex].coord[YY]; - z_length = ph->vertex[top_vertex].coord[ZZ] - ph->vertex[next_vertex].coord[ZZ]; - - y_slope = y_length / sqrt((x_length)*(x_length) + (z_length)*(z_length)); - if (y_slope < min_slope) - { - min_slope = y_slope; - shallowest_vertex = next_vertex; - } - } - - return shallowest_vertex; -} - - -VerticesFound get_adjacent_vertices(PolygonStruct* p, int seed_vertex, int* previous_vertex, int* next_vertex) -{ - int i; - - for (i=0; inum_vertices; i++) - if (p->vertex_index[i] == seed_vertex) - break; - - if (i == p->num_vertices) - return (found_none); - - if (i == 0) - *previous_vertex = p->vertex_index[p->num_vertices-1]; - else - *previous_vertex = p->vertex_index[i-1]; - - *next_vertex = p->vertex_index[(i+1)%p->num_vertices]; - - return (found_some); - -} - - -void find_other_edge(PolyhedronStruct* ph, int poly, int start_vertex, - int adj_vertex, double other_edge[]) -{ - - int i, v1, v2, other_vertex, first_vertex, second_vertex; - double the_edge[3]; - PolygonStruct* p; - - p = &ph->polygon[poly]; - - for (i=0; inum_vertices; i++) - if (p->vertex_index[i] == start_vertex) - break; - - if (i == p->num_vertices) - return; - - if (i == 0) - { - v1 = 1; - v2 = p->num_vertices - 1; - } - else if (i == p->num_vertices - 1) - { - v1 = 0; - v2 = i - 1; - } - else - { - v1 = i + 1; - v2 = i - 1; - } - - /* v1 and v2 are the vertices on either side of start_vertex. Find the one that - * is not equal to adj_vertex and set other_vertex equal to it. - */ - - if (p->vertex_index[v1] == adj_vertex) - other_vertex = v2; - else - other_vertex = v1; - - /* The edge will be from start_vertex (i) to other_vertex. */ - - first_vertex = p->vertex_index[other_vertex]; - second_vertex = p->vertex_index[i]; - - /* Now subtract second from first to form the edge */ - - the_edge[XX] = ph->vertex[first_vertex].coord[XX] - - ph->vertex[second_vertex].coord[XX]; - the_edge[YY] = ph->vertex[first_vertex].coord[YY] - - ph->vertex[second_vertex].coord[YY]; - the_edge[ZZ] = ph->vertex[first_vertex].coord[ZZ] - - ph->vertex[second_vertex].coord[ZZ]; - - normalize_vector(the_edge,other_edge); - -} - - -int compare_dist(PListStruct* item1, PListStruct* item2) -{ - - if (item1->distance < item2->distance) - return (-1); - - if (item1->distance > item2->distance) - return (1); - - return (0); - -} - - -void find_zero_area_polygons(PolyhedronStruct* ph) -{ - - int i, v1, v2, v3; - double vec1[3], vec2[3], vec3[3], cross_vec[3]; - double mag1, mag2, edge_length[3]; - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - - if (ph->polygon[i].num_vertices > 3) - { - find_zero_area_nontriangle(ph,i); - continue; - } - - v1 = ph->polygon[i].vertex_index[0]; - v2 = ph->polygon[i].vertex_index[1]; - v3 = ph->polygon[i].vertex_index[2]; - - MAKE_3DVECTOR(ph->vertex[v1].coord,ph->vertex[v2].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec2); - MAKE_3DVECTOR(ph->vertex[v3].coord,ph->vertex[v1].coord,vec3); - - edge_length[0] = normalize_vector(vec1,vec1); - edge_length[1] = normalize_vector(vec2,vec2); - edge_length[2] = normalize_vector(vec3,vec3); - - /* If two edges are collinear, then all three are. */ - cross_vectors(vec1,vec2,cross_vec); - mag1 = normalize_vector(cross_vec,cross_vec); - if ((mag1 > COLLINEAR_EPSILON) && (mag1 < 2.0 - COLLINEAR_EPSILON)) - continue; - - cross_vectors(vec2,vec3,cross_vec); - mag2 = normalize_vector(cross_vec,cross_vec); - if ((mag2 > COLLINEAR_EPSILON) && (mag2 < 2.0 - COLLINEAR_EPSILON)) - continue; -/* - printf("polygon[%d] (%d %d %d) has zero area (%.6lf, %.6lf) (old=%d)\n", - i, ph->polygon[i].vertex_index[0], - ph->polygon[i].vertex_index[1], ph->polygon[i].vertex_index[2], mag1, - mag2, ph->polygon[i].old); -*/ - } - -} - - -void remove_zero_area_polygons(PolyhedronStruct* ph) -{ - - int i, v1, v2, v3, e1, e2, v, p; - double vec1[3], vec2[3], vec3[3], cross_vec[3]; - double mag, edge_length[3]; - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - - if (ph->polygon[i].num_vertices > 3) - { - remove_zero_area_nontriangle(ph,i); - continue; - } - - v1 = ph->polygon[i].vertex_index[0]; - v2 = ph->polygon[i].vertex_index[1]; - v3 = ph->polygon[i].vertex_index[2]; - - MAKE_3DVECTOR(ph->vertex[v1].coord,ph->vertex[v2].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec2); - MAKE_3DVECTOR(ph->vertex[v3].coord,ph->vertex[v1].coord,vec3); - - edge_length[0] = normalize_vector(vec1,vec1); - edge_length[1] = normalize_vector(vec2,vec2); - edge_length[2] = normalize_vector(vec3,vec3); - - /* If two edges are collinear, then all three are. */ - cross_vectors(vec1,vec2,cross_vec); - mag = normalize_vector(cross_vec,cross_vec); - if ((mag > COLLINEAR_EPSILON) && (mag < 2.0 - COLLINEAR_EPSILON)) - continue; - - cross_vectors(vec2,vec3,cross_vec); - mag = normalize_vector(cross_vec,cross_vec); - if ((mag > COLLINEAR_EPSILON) && (mag < 2.0 - COLLINEAR_EPSILON)) - continue; - - /* If you make it to here, the triangle has [near]-zero area, so - * find the vertex that is between the other two. - */ - if (edge_length[0] > edge_length[1]) - { - if (edge_length[0] > edge_length[2]) - { - v = v3; - e1 = v1; - e2 = v2; - } - else - { - v = v2; - e1 = v1; - e2 = v3; - } - } - else - { - if (edge_length[1] > edge_length[2]) - { - v = v1; - e1 = v2; - e2 = v3; - } - else - { - v = v2; - e1 = v1; - e2 = v3; - } - } - - p = find_other_polygon(ph,i,e1,e2); - if (p == -1) - { - printf("zero area: no other polygon (%d) with %d %d\n", i, e1, e2); - } - else - { - add_vert_to_poly(ph,p,e1,e2,v); - } - throw_out_polygon(ph,i); - } - -} - - -void find_zero_area_nontriangle(PolyhedronStruct* ph, int pl) -{ - - int i, v1, v2, v3; - double vec1[3], vec2[3]; - double *v_angle; - PolygonStruct* ps; - - ps = &ph->polygon[pl]; - v_angle = (double*)simm_malloc(ps->num_vertices*sizeof(double)); - - for (i=0; inum_vertices; i++) - { - v1 = ps->vertex_index[(i+ps->num_vertices-1)%ps->num_vertices]; - v2 = ps->vertex_index[i]; - v3 = ps->vertex_index[(i+1)%ps->num_vertices]; - - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v1].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec2); - - v_angle[i] = compute_angle_between_vectors(vec1,vec2); - - if (v_angle[i] > ANGLE_EPSILON && v_angle[i] < M_PI - ANGLE_EPSILON) - { - free(v_angle); - return; - } - } -/* - printf("polygon[%d] has zero area ", pl); - for (i=0; inum_vertices; i++) - printf("%d ", ps->vertex_index[i]); - printf("angles: "); - for (i=0; inum_vertices; i++) - printf("%5.1lf ", v_angle[i]*RTOD); - printf("(old=%d)\n", ps->old); -*/ -} - - -void remove_zero_area_nontriangle(PolyhedronStruct* ph, int pl) -{ - - int i, j, count, index, e1, e2, v1, v2, v3, p, edge[2]; - double vec1[3], vec2[3]; - double *v_angle; - PolygonStruct* ps; - PListStruct* plist; - - ps = &ph->polygon[pl]; - v_angle = (double*)simm_malloc(ps->num_vertices*sizeof(double)); - - for (i=0; inum_vertices; i++) - { - v1 = ps->vertex_index[(i+ps->num_vertices-1)%ps->num_vertices]; - v2 = ps->vertex_index[i]; - v3 = ps->vertex_index[(i+1)%ps->num_vertices]; - - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v1].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec2); - - v_angle[i] = compute_angle_between_vectors(vec1,vec2); - - if (v_angle[i] > ANGLE_EPSILON && v_angle[i] < M_PI - ANGLE_EPSILON) - { - free(v_angle); - return; - } - } -/* - printf("polygon[%d] has zero area ", pl); - for (i=0; inum_vertices; i++) - printf("%d ", ps->vertex_index[i]); - printf("angles: "); - for (i=0; inum_vertices; i++) - printf("%5.1lf ", v_angle[i]*RTOD); - printf("(old=%d)\n", ps->old); -*/ - /* If you make it to here, the polygon must have [near]-zero area because - * all of its internal angles are either 0.0 or 180.0. So now you want to - * find the two vertices with an angle of 0.0, and sort the other vertices - * between these two. - */ - - for (i=0, count=0; inum_vertices; i++) - { - if (ANGLES_APX_EQ(v_angle[i],0.0)) - { - if (count > 1) - printf("Too many 0.0 angles in polygon %d\n", pl); - else - edge[count++] = i; - } - } - - if (count < 2) - { - printf("Could not find two 0.0 angles in polygon %d\n", pl); - free(v_angle); - return; - } - - plist = (PListStruct*)simm_malloc(ps->num_vertices*sizeof(PListStruct)); - - plist[0].v_num = ps->vertex_index[edge[0]]; - plist[0].distance = 0.0; - plist[0].side = 2; - - plist[ps->num_vertices-1].v_num = ps->vertex_index[edge[1]]; - plist[ps->num_vertices-1].distance = MAXMDOUBLE; - plist[ps->num_vertices-1].side = 2; - - /* plist[0] and plist[nv-1] define the min and max points of the collinear - * vertices. Move along one side of the polygon (side 0) from plist[0] to - * plist[nv-1] and fill in the plist structure. Then you will move along the - * other side (side 1) and fill in the rest of the plist structures, keeping - * them sorted by distance from plist[0]. - */ - - index = 1; - for (i=edge[0]+1; ivertex_index[i]; - plist[index].distance = - distancesqr_between_vertices(ph->vertex[plist[0].v_num].coord, - ph->vertex[plist[index].v_num].coord); - plist[index++].side = 0; - } - - for (i=edge[1]+1; inum_vertices; i++) - { - plist[index].v_num = ps->vertex_index[i]; - plist[index].distance = - distancesqr_between_vertices(ph->vertex[plist[0].v_num].coord, - ph->vertex[plist[index].v_num].coord); - plist[index++].side = 1; - } - - for (i=0; ivertex_index[i]; - plist[index].distance = - distancesqr_between_vertices(ph->vertex[plist[0].v_num].coord, - ph->vertex[plist[index].v_num].coord); - plist[index++].side = 1; - } - - qsort(plist,ps->num_vertices,sizeof(PListStruct), - (int(*)(const void*,const void*))compare_dist); -/* - for (i=0; inum_vertices; i++) - printf("plist[%d] = %d, %lf, %d\n", i, plist[i].v_num, - plist[i].distance, plist[i].side); -*/ - /* Now that you have a sorted list of vertices between plist[0] and plist[nv-1], - * use them to add vertices to the polygons that border this zero-area one. - * First scan the array looking for side1 vertices between side0 vertices, and - * then scan it again looking for side0 vertices between side1 vertices. - */ - - for (i=1; inum_vertices-1; i++) - { - if (plist[i].side != 1) - continue; - e1 = plist[i-1].v_num; - /* Find the first vertex after this one (plist[i]) not on side1 */ - for (j=i+1; jnum_vertices; j++) - if (plist[j].side != 1) - break; - e2 = plist[j].v_num; - p = find_other_polygon(ph,pl,e1,e2); - if (p == -1) - printf("zero area: no other polygon (%d) with %d %d\n", pl, e1, e2); - else - { -/* - printf("adding %d between %d and %d in polygon %d\n", plist[i].v_num, - e1, e2, p); -*/ - add_vert_to_poly(ph,p,e1,e2,plist[i].v_num); - } - } - - for (i=1; inum_vertices-1; i++) - { - if (plist[i].side != 0) - continue; - e1 = plist[i-1].v_num; - /* Find the first vertex after this one (plist[i]) not on side0 */ - for (j=i+1; jnum_vertices; j++) - if (plist[j].side != 0) - break; - e2 = plist[j].v_num; - p = find_other_polygon(ph,pl,e1,e2); - if (p == -1) - printf("zero area: no other polygon (%d) with %d %d\n", pl, e1, e2); - else - { -/* - printf("adding %d between %d and %d in polygon %d\n", plist[i].v_num, - e1, e2, p); -*/ - add_vert_to_poly(ph,p,e1,e2,plist[i].v_num); - } - } - - throw_out_polygon(ph,pl); - - free(plist); - free(v_angle); - -} - - -void remove_duplicate_vertices(PolyhedronStruct* ph, NormOptions* opt) -{ - int i, j, k, vertices_removed; - double distance; - - if (EQUAL_WITHIN_ERROR(0.0,opt->vertex_tolerance)) - return; - - if (opt->verbose_output == yes) - { - printf("Removing duplicate vertices (tol = %.10lf)\n", opt->vertex_tolerance); - fflush(stdout); - } - - for (i=0; inum_vertices; i++) - ph->vertex[i].new_index = i; - - distance = opt->vertex_tolerance * opt->vertex_tolerance; - - for (i=0, vertices_removed=0; inum_vertices; i++) - { - if (ph->vertex[i].thrown_out == yes) - continue; - for (j=i+1; jnum_vertices; j++) - { - if (ph->vertex[j].thrown_out == yes) - continue; - if (distancesqr_between_vertices(ph->vertex[i].coord, ph->vertex[j].coord) < distance) - { - // In the polygons that use vertex j, replace j with i. - // TODO: for now, assume that the normals of these polygons - // do not change much, so don't recalculate them. Also, - // assume that vertex[i].normal does not change much. - for (k=0; kvertex[j].polygon_count; k++) - replace_vertex_index(&ph->polygon[ph->vertex[j].polygons[k]], j, i); - // Now add all of these polygons to the poly_list for vertex i. - add_polygons_to_vert_poly_list(&ph->vertex[i], ph->vertex[j].polygons, ph->vertex[j].polygon_count); - throw_out_vertex(ph, j); - vertices_removed++; - } - } - } - - if (opt->verbose_output == yes) - printf(" %d duplicate vertices removed.\n", vertices_removed); -} - - -void add_polygons_to_vert_poly_list(VertexStruct* v, int* polygons, int polygon_count) -{ - int i, j, max_polys; - ReturnCode rc; - - if (v->thrown_out == yes) - printf("OOPS: adding to v[deleted].polygons\n"); - - max_polys = v->polygon_count + polygon_count; - - v->polygons = (int*)simm_realloc(v->polygons,max_polys*sizeof(int),&rc); - - for (i=0; ipolygon_count; j++) - { - if (v->polygons[j] == polygons[i]) - break; - } - if (j == v->polygon_count) - v->polygons[v->polygon_count++] = polygons[i]; - } - - if (v->polygon_count < max_polys) - v->polygons = (int*)simm_realloc(v->polygons,v->polygon_count*sizeof(int),&rc); -} - - -void add_polygon_to_vert_poly_list(VertexStruct* v, int poly_num) -{ - int i; - ReturnCode rc; - - for (i=0; ipolygon_count; i++) - if (v->polygons[i] == poly_num) - return; - - if (v->thrown_out == yes) - printf("OOPS: adding to v[deleted].polygons\n"); - - v->polygon_count++; - - if (v->polygon_count == 1) - v->polygons = (int*)simm_malloc(sizeof(int)); - else - v->polygons = (int*)simm_realloc(v->polygons,v->polygon_count*sizeof(int),&rc); - - v->polygons[v->polygon_count-1] = poly_num; -} - - -void remove_polygon_from_vert_poly_list(VertexStruct* v, int poly_num) -{ - - int i, index; - - for (i=0; ipolygon_count; i++) - if (v->polygons[i] == poly_num) - break; - - if (i == v->polygon_count) - return; - - index = i; - - for (i=index; ipolygon_count-1; i++) - v->polygons[i] = v->polygons[i+1]; - - v->polygon_count--; - - /* TODO: you might want to realloc the array here */ - -} - - -void replace_vertex_index(PolygonStruct* p, int old_index, int new_index) -{ - - int i; - - for (i=0; inum_vertices; i++) - { - if (p->vertex_index[i] == old_index) - p->vertex_index[i] = new_index; - } - -} - - -void remove_degenerate_polygons(PolyhedronStruct* ph) -{ - - int i, j, k, num_removed=0; - - /* Remove all degenerate polygons. A degenerate polygon is one - * which has identical indices for two or more of its vertices - * (e.g., a polygon with a vertex list of: 2 6 7 6). This is a really - * hard problem for polygons with 4 or more vertices, because you need - * to break the polygon into two or more pieces, then remove the degenerate - * pieces. For now, just remove the duplicate indices in the polygon, and - * if there are less than 3 unique indices left, throw out the polygon - * (hoping that the hole will get filled in later). - */ - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes || ph->polygon[i].old == yes) - continue; - for (j=0; jpolygon[i].num_vertices; j++) - { - for (k=j+1; kpolygon[i].num_vertices; k++) - { - if (ph->polygon[i].vertex_index[j] == ph->polygon[i].vertex_index[k]) - { - /* - printf("polygon %d is degenerate ", i); - for (m=0; mpolygon[i].num_vertices; m++) - printf("%d ", ph->polygon[i].vertex_index[m]); - printf("\n"); - */ - if ((ph->polygon[i].num_vertices < 4) || (fix_polygon(ph,i) == -1)) - { - throw_out_polygon(ph,i); - num_removed++; - } - /* major hack (setting j) to break out of both loops */ - j = ph->polygon[i].num_vertices; - break; - } - } - } - } - - if (verbose == yes) - printf(" %d degenerate polygons removed.\n", num_removed); - -} - - -void remove_overlapping_polygons(PolyhedronStruct* ph) -{ - int i, j, num_removed = 0; - - /* Remove all overlapping polygons. A polygon is overlapping if - * its vertex list is identical to some other polygon's list - * (if the order of the vertices is different the polygon is - * still overlapping). - */ - - for (i = 0; i < ph->num_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes || ph->polygon[i].old == yes) - continue; - - for (j = i+1; j < ph->num_polygons; j++) - { - if (ph->polygon[j].thrown_out == yes || ph->polygon[j].old == yes) - continue; - - if (vertex_lists_identical(ph, i, j)) - { - throw_out_polygon(ph, j); - num_removed++; - } - } - } - - if (verbose == yes) - printf(" %d overlapping polygons removed.\n", num_removed); -} - - -int vertex_lists_identical(PolyhedronStruct* ph, int poly1, int poly2) -{ - int i, j; - - if (ph->polygon[poly1].num_vertices != ph->polygon[poly2].num_vertices) - return 0; - - for (i = 0; i < ph->polygon[poly1].num_vertices; i++) - { - for (j = 0; j < ph->polygon[poly2].num_vertices; j++) - { - if (ph->polygon[poly1].vertex_index[i] == ph->polygon[poly2].vertex_index[j]) - break; - } - if (j == ph->polygon[poly2].num_vertices) - return 0; - } - - return 1; -} - - -void remove_extra_polygons(PolyhedronStruct* ph) -{ - int i, j, k, num_removed = 0; - int v1, v2, num_brothers, num_triplets; - - /* Remove all extra polygons. A polygon is extra if two or - * more of its edges are shared by 2 or more other polygons. - */ - - for (i = 0; i < ph->num_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes || ph->polygon[i].old == yes) - continue; - - num_triplets = 0; - - if (i == 838) - { - num_triplets = 0; - } - - for (j = 0; j < ph->polygon[i].num_vertices; j++) - { - v1 = ph->polygon[i].vertex_index[j]; - v2 = ph->polygon[i].vertex_index[(j+1) % ph->polygon[i].num_vertices]; - num_brothers = 0; - - for (k = 0; k < ph->num_polygons; k++) - { - if (k != i && ph->polygon[k].thrown_out == no && ph->polygon[k].old == no) - { - if (polygon_contains_edge(&ph->polygon[k], v1, v2) > 0) - { - if (++num_brothers > 1) - break; - } - } - } - - if (num_brothers > 1) - num_triplets++; - - //printf("p %d e %d: brothers = %d\n", i, j, num_brothers); - } - - if (num_triplets > 1) - { - throw_out_polygon(ph, i); - num_removed++; - } - } - - if (verbose == yes) - printf(" %d extra polygons removed.\n", num_removed); -} - - -/* Remove all duplicates from the vertex_index list. If there - * are 3 or more vertices left standing, save the fixed polygon. - */ - -int fix_polygon(PolyhedronStruct* ph, int pl) -{ - - int i, j, count=0; - int* vlist; - PolygonStruct* ps; - - ps = &ph->polygon[pl]; - - vlist = (int*)simm_malloc(ps->num_vertices*sizeof(int)); - - for (i=0; inum_vertices; i++) - { - for (j=0; jvertex_index[i] == vlist[j]) - break; - if (j == count) - vlist[count++] = ps->vertex_index[i]; - } - - /* If there are less than 3 vertices, the attempted fix failed */ - if (count < 3) - return (-1); - - ps->vertex_index = vlist; - ps->num_vertices = count; -/* - printf("fix_polygon succeeded: "); - for (i=0; inum_polygons; i++) - { - if (polyhedron->polygon[i].thrown_out == yes) - continue; - polyhedron->polygon[i].ordering_value = 0.0; - for (j=0; jpolygon[i].num_vertices; j++) - polyhedron->polygon[i].ordering_value += - polyhedron->vertex[polyhedron->polygon[i].vertex_index[j]].coord[axis]; - polyhedron->polygon[i].ordering_value /= polyhedron->polygon[i].num_vertices; - } - - for (i=0; inum_polygons; i++) - { - if (polyhedron->polygon[i].thrown_out == yes) - continue; - min_so_far = polyhedron->polygon[i].ordering_value; - index_of_min = i; - for (j=i+1; jnum_polygons; j++) - { - if (polyhedron->polygon[j].thrown_out == yes) - continue; - if (polyhedron->polygon[j].ordering_value < min_so_far) - { - min_so_far = polyhedron->polygon[j].ordering_value; - index_of_min = j; - } - } - if (index_of_min != i) - swap_polygons(polyhedron,i,index_of_min); - } - - if (verbose == yes) - printf("Done.\n"); - -} - - -void swap_polygons(PolyhedronStruct* ph, int index1, int index2) -{ - - PolygonStruct p; - - copy_polygon(&ph->polygon[index1],&p); - copy_polygon(&ph->polygon[index2],&ph->polygon[index1]); - copy_polygon(&p,&ph->polygon[index2]); - -} - - -void order_vertices(PolyhedronStruct* polyhedron, int order_format) -{ - - int i, j, axis, index_of_min; - double min_so_far; - - if (order_format == no_ordering) - return; - - axis = order_format; - - for (i=0; inum_vertices; i++) - { - if (polyhedron->vertex[i].thrown_out == yes) - continue; - min_so_far = polyhedron->vertex[i].coord[axis]; - index_of_min = i; - for (j=i+1; jnum_vertices; j++) - { - if (polyhedron->vertex[j].coord[axis] < min_so_far) - { - min_so_far = polyhedron->vertex[j].coord[axis]; - index_of_min = j; - } - } - if (index_of_min != i) - swap_vertex_indices(polyhedron,i,index_of_min); - } - -} - - -void swap_vertex_indices(PolyhedronStruct* ph, int index1, int index2) -{ - - int i, j; - VertexStruct v; - - copy_vertex(&ph->vertex[index1],&v); - copy_vertex(&ph->vertex[index2],&ph->vertex[index1]); - copy_vertex(&v,&ph->vertex[index2]); - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - for (j=0; jpolygon[i].num_vertices; j++) - { - if (ph->polygon[i].vertex_index[j] == index1) - ph->polygon[i].vertex_index[j] = index2; - else if (ph->polygon[i].vertex_index[j] == index2) - ph->polygon[i].vertex_index[j] = index1; - } - } - -} - - -/* This function finds all of the edges that appear in just one polygon, and attempts - * to string them together to form hole-filling polygons. It assumes that there are - * not more that MAX_HOLE_EDGES loose edges, and not more that MAX_POLY_EDGES edges - * in any one hole. - */ - -void fill_holes(PolyhedronStruct* ph, SBoolean fill_holes) -{ - - int i, j, v1, v2, nv, count=0, num_edges, start_v, other_end; - int edgelist[MAX_HOLE_EDGES][2], edgeused[MAX_HOLE_EDGES], num_holes_filled=0; - int new_p, e_num, v_num, vertex_index[MAX_POLY_EDGES]; - double normal[3]; - - if (fill_holes == no) - return; - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - for (j=0; jpolygon[i].num_vertices; j++) - { - v1 = ph->polygon[i].vertex_index[j]; - if (j == ph->polygon[i].num_vertices-1) - v2 = ph->polygon[i].vertex_index[0]; - else - v2 = ph->polygon[i].vertex_index[j+1]; - if (find_other_polygon(ph,i,v1,v2) == -1) - { - edgelist[count][0] = v1; - edgelist[count++][1] = v2; - } - if (count >= MAX_HOLE_EDGES) - { - i = ph->num_polygons; /* hack to break out of both loops */ - break; - } - } - } - - num_edges = count; - - for (i=0; i 0) - { - for (i=0; i= 3) - { - new_p = make_new_polygon(ph,v_num,vertex_index,0,normal,0.0,yes,yes); - num_holes_filled++; - } - break; - } - if (other_end == start_v) - { - /* Got a complete polygon (closed loop of edges) */ - new_p = make_new_polygon(ph,v_num,vertex_index,0,normal,0.0,yes,yes); - num_holes_filled++; -/* - printf("filled hole with (%d): ", new_p); - for (j=0; jnum_polygons; j++) - { - if (ph->polygon[j].thrown_out == yes) - continue; - for (k=0; kpolygon[j].num_vertices; k++) - { - v1 = ph->polygon[j].vertex_index[k]; - if (k == ph->polygon[j].num_vertices-1) - v2 = ph->polygon[j].vertex_index[0]; - else - v2 = ph->polygon[j].vertex_index[k+1]; - if (find_other_polygon(ph,j,v1,v2) == -1) - printf("edge %d %d is unshared (%d only)\n", v1, v2, j); - } - } - -} - - -void check_edge_lengths(PolyhedronStruct* ph, double max_edge_length) -{ - - int i, j, v1, v2, vi1, vi2, v_new, num_edges_split=0; - double len, vec[3], vert[3]; - PolygonStruct* p; - - if (max_edge_length < 0.0) - return; - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - p = &ph->polygon[i]; - for (j=0; jnum_vertices; j++) - { - vi1 = j; - vi2 = (j+1)%p->num_vertices; - v1 = ph->polygon[i].vertex_index[vi1]; - v2 = ph->polygon[i].vertex_index[vi2]; - vec[0] = ph->vertex[v1].coord[0] - ph->vertex[v2].coord[0]; - vec[1] = ph->vertex[v1].coord[1] - ph->vertex[v2].coord[1]; - vec[2] = ph->vertex[v1].coord[2] - ph->vertex[v2].coord[2]; - len = VECTOR_MAGNITUDE(vec); - if (len > max_edge_length) - { - vert[0] = (ph->vertex[v1].coord[0] + ph->vertex[v2].coord[0]) * 0.5; - vert[1] = (ph->vertex[v1].coord[1] + ph->vertex[v2].coord[1]) * 0.5; - vert[2] = (ph->vertex[v1].coord[2] + ph->vertex[v2].coord[2]) * 0.5; - v_new = make_new_vertex(ph,vert,ph->vertex[v1].polyhedron_number,no,yes); - add_vertex(ph,i,vi1,vi2,v_new); - i--; - num_edges_split++; - break; - } - } - } - - if (verbose == yes) - printf(" %d edges split.\n", num_edges_split); - -} - - -void triangulate_polygons(PolyhedronStruct* ph, TriangulateOption triangulate) -{ - - int i, polys_to_add, verts_to_add, v_split, num_orig_polys, v_index[3]; - ReturnCode rc; - - if (triangulate == no_tri) - return; - - /* First malloc space for the new polygons */ - - if (triangulate == simple_tri) - { -/* - if (verbose == yes) - { - printf("Triangulating polygons (simple technique)... "); - fflush(stdout); - } -*/ - for (i=0, polys_to_add=0, verts_to_add=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - if (ph->polygon[i].num_vertices == 3) - continue; - if (ph->polygon[i].num_vertices == 4) - polys_to_add++; - else if (ph->polygon[i].num_vertices > 6) - { - polys_to_add += (ph->polygon[i].num_vertices - 1); - verts_to_add++; - } - else - { - polys_to_add += (ph->polygon[i].num_vertices - 3); - } - } - } - else if (triangulate == complex_tri) - { -/* - if (verbose == yes) - { - printf("Triangulating polygons (complex technique)... "); - fflush(stdout); - } -*/ - for (i=0, polys_to_add=0, verts_to_add=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - if (ph->polygon[i].num_vertices == 3) - continue; - if (ph->polygon[i].num_vertices == 4) - { - polys_to_add += 3; - verts_to_add++; - } - else if (ph->polygon[i].num_vertices > 6) - { - polys_to_add += (ph->polygon[i].num_vertices - 1); - verts_to_add++; - } - else - { - polys_to_add += (ph->polygon[i].num_vertices - 3); - } - } - } - - num_orig_polys = ph->num_polygons; - - if (polys_to_add > 0) - ph->polygon = (PolygonStruct*)simm_realloc(ph->polygon, - (ph->num_polygons+polys_to_add)*sizeof(PolygonStruct),&rc); - - if (verts_to_add > 0) - ph->vertex = (VertexStruct*)simm_realloc(ph->vertex, - (ph->num_vertices+verts_to_add)*sizeof(VertexStruct),&rc); - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - continue; - - if (ph->polygon[i].num_vertices == 3) - continue; - - if (ph->polygon[i].num_vertices == 4) - { - if (triangulate == simple_tri) - { - v_split = find_largest_angle(ph,i); - v_index[0] = ph->polygon[i].vertex_index[v_split]; - v_index[1] = ph->polygon[i].vertex_index[(v_split+2)%4]; - v_index[2] = ph->polygon[i].vertex_index[(v_split+3)%4]; - make_new_polygon(ph,3,v_index,ph->polygon[i].polyhedron_number, - ph->polygon[i].normal,ph->polygon[i].d,no,no); - v_index[0] = ph->polygon[i].vertex_index[v_split]; - v_index[1] = ph->polygon[i].vertex_index[(v_split+1)%4]; - v_index[2] = ph->polygon[i].vertex_index[(v_split+2)%4]; - change_vertex_indices(ph,i,v_index,3); - } - else - { - quarter_quad(ph,i); - } - } - else if (ph->polygon[i].num_vertices == 5) - { - trisect_fiver(ph,i); - } - else if (ph->polygon[i].num_vertices == 6) - { - bisect_sixer(ph,i); - i--; - } - else - { - split_large_polygon(ph,i,no,no); - } - } - - if (verbose == yes) - printf(" added %d polygons by triangulation.\n", polys_to_add); - -} - - -void quarter_quad(PolyhedronStruct* ph, int poly_num) -{ - - int v1, v2, v3, v4, new_v, v_index[3]; - double pt[3]; - PolygonStruct* p; - - p = &ph->polygon[poly_num]; - - v1 = p->vertex_index[0]; - v2 = p->vertex_index[1]; - v3 = p->vertex_index[2]; - v4 = p->vertex_index[3]; - - pt[0] = (ph->vertex[v1].coord[0] + ph->vertex[v2].coord[0] + - ph->vertex[v3].coord[0] + ph->vertex[v4].coord[0]) / 4.0; - pt[1] = (ph->vertex[v1].coord[1] + ph->vertex[v2].coord[1] + - ph->vertex[v3].coord[1] + ph->vertex[v4].coord[1]) / 4.0; - pt[2] = (ph->vertex[v1].coord[2] + ph->vertex[v2].coord[2] + - ph->vertex[v3].coord[2] + ph->vertex[v4].coord[2]) / 4.0; - - new_v = make_new_vertex(ph,pt,ph->vertex[v1].polyhedron_number,no,no); - - v_index[0] = v1; - v_index[1] = v2; - v_index[2] = new_v; - change_vertex_indices(ph,poly_num,v_index,3); - - v_index[0] = v2; - v_index[1] = v3; - v_index[2] = new_v; - make_new_polygon(ph,3,v_index,p->polyhedron_number,p->normal,p->d,no,no); - - v_index[0] = v3; - v_index[1] = v4; - v_index[2] = new_v; - make_new_polygon(ph,3,v_index,p->polyhedron_number,p->normal,p->d,no,no); - - v_index[0] = v4; - v_index[1] = v1; - v_index[2] = new_v; - make_new_polygon(ph,3,v_index,p->polyhedron_number,p->normal,p->d,no,no); - -} - - -void split_triangle(PolyhedronStruct* ph, int poly_num) -{ - - int v1, v2, v3, v_index[3], new_v; - double pt[3]; - PolygonStruct* p; - - p = &ph->polygon[poly_num]; - - v1 = p->vertex_index[0]; - v2 = p->vertex_index[1]; - v3 = p->vertex_index[2]; - - pt[0] = (ph->vertex[v1].coord[0] + ph->vertex[v2].coord[0] + - ph->vertex[v3].coord[0]) / 3.0; - pt[1] = (ph->vertex[v1].coord[1] + ph->vertex[v2].coord[1] + - ph->vertex[v3].coord[1]) / 3.0; - pt[2] = (ph->vertex[v1].coord[2] + ph->vertex[v2].coord[2] + - ph->vertex[v3].coord[2]) / 3.0; - - new_v = make_new_vertex(ph,pt,ph->vertex[v1].polyhedron_number,no,no); - - v_index[0] = v1; - v_index[1] = v2; - v_index[2] = new_v; - change_vertex_indices(ph,poly_num,v_index,3); - - v_index[0] = v2; - v_index[1] = v3; - v_index[2] = new_v; - make_new_polygon(ph,3,v_index,p->polyhedron_number,p->normal,p->d,no,no); - - v_index[0] = v3; - v_index[1] = v1; - v_index[2] = new_v; - make_new_polygon(ph,3,v_index,p->polyhedron_number,p->normal,p->d,no,no); - -} - - -SBoolean polygon_very_concave(PolyhedronStruct* ph, int poly_num) -{ - - int i, v1, v2, v3; - double vec1[3], vec2[3], dot_product, angle_sum, convex_sum; - double offset, ext_angle; - - for (i=0, angle_sum=0.0; ipolygon[poly_num].num_vertices; i++) - { - v2 = ph->polygon[poly_num].vertex_index[i]; - if (i == 0) - v1 = ph->polygon[poly_num].vertex_index[ph->polygon[poly_num].num_vertices-1]; - else - v1 = ph->polygon[poly_num].vertex_index[i-1]; - if (i == ph->polygon[poly_num].num_vertices-1) - v3 = ph->polygon[poly_num].vertex_index[0]; - else - v3 = ph->polygon[poly_num].vertex_index[i+1]; - - vec1[0] = ph->vertex[v1].coord[0] - ph->vertex[v2].coord[0]; - vec1[1] = ph->vertex[v1].coord[1] - ph->vertex[v2].coord[1]; - vec1[2] = ph->vertex[v1].coord[2] - ph->vertex[v2].coord[2]; - - vec2[0] = ph->vertex[v3].coord[0] - ph->vertex[v2].coord[0]; - vec2[1] = ph->vertex[v3].coord[1] - ph->vertex[v2].coord[1]; - vec2[2] = ph->vertex[v3].coord[2] - ph->vertex[v2].coord[2]; - - normalize_vector(vec1,vec1); - normalize_vector(vec2,vec2); - - dot_product = vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]; - - if (dot_product < -1.0) - angle_sum += 180.0; - else if (dot_product > 1.0) - angle_sum += 0.0; - else - angle_sum += RTOD*acos(dot_product); - } - - ext_angle = 360.0/ph->polygon[poly_num].num_vertices; - convex_sum = ph->polygon[poly_num].num_vertices * (180.0 - ext_angle); - offset = (convex_sum - angle_sum) * 0.5; - - if ((offset > 45.0) && (offset > convex_sum*0.05)) - return (yes); - - return (no); - -} - - -void print_internal_angles(PolyhedronStruct* ph, int pl) -{ - - int i; - int count=0, v1, v2, v3; - double v_angle, vec1[3], vec2[3]; - PolygonStruct* ps; - - ps = &ph->polygon[pl]; - - printf("polygon %d ", pl); - for (i=0; inum_vertices; i++) - printf("%d ", ps->vertex_index[i]); - printf(", internal angles:\n"); - - for (i=0; inum_vertices; i++) - { - v1 = ps->vertex_index[(i+ps->num_vertices-1)%ps->num_vertices]; - v2 = ps->vertex_index[i]; - v3 = ps->vertex_index[(i+1)%ps->num_vertices]; - - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v1].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec2); - - v_angle = compute_angle_between_vectors(vec1,vec2); - printf("%5.1lf ", v_angle*RTOD); - } - printf("\n"); - -} - - -void trisect_fiver(PolyhedronStruct* ph, int old_polygon) -{ - - int i, v_index[3], v_split, max_index; - int count=0, v1, v2, v3, col[5]; - double v_angle[5], vec1[3], vec2[3], max_angle=MINMDOUBLE; - PolygonStruct* p; - - p = &ph->polygon[old_polygon]; - - /* In order to find the best trisection point, first find the internal - * angles at the vertices. col[] holds the indices of vertices whose - * internal angle is 180.0, and count is number of such vertices. - */ - - for (i=0; inum_vertices; i++) - { - v1 = p->vertex_index[(i+p->num_vertices-1)%p->num_vertices]; - v2 = p->vertex_index[i]; - v3 = p->vertex_index[(i+1)%p->num_vertices]; - - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v1].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec2); - - v_angle[i] = compute_angle_between_vectors(vec1,vec2); - - if (v_angle[i] > M_PI - ANGLE_EPSILON) - col[count++] = i; - if (v_angle[i] > max_angle) - { - max_angle = v_angle[i]; - max_index = i; - } - } - - if (count < 2) - { - v_split = max_index; - } - else if (count == 2) - { - if (col[0] == 0 && col[1] == p->num_vertices-1) - { - v_split = 2; - } - else if ((col[1] - col[0]) == 1) - { - v_split = (col[0] + 3) % p->num_vertices; - } - else - { - v_split = col[0]; - } - } - else - { - printf("Zero-area fiver found (%d)\n", old_polygon); - } - - v_index[0] = p->vertex_index[v_split]; - v_index[1] = p->vertex_index[(v_split+2)%5]; - v_index[2] = p->vertex_index[(v_split+3)%5]; - make_new_polygon(ph,3,v_index,p->polyhedron_number,p->normal,p->d,no,no); - - v_index[0] = p->vertex_index[v_split]; - v_index[1] = p->vertex_index[(v_split+3)%5]; - v_index[2] = p->vertex_index[(v_split+4)%5]; - make_new_polygon(ph,3,v_index,p->polyhedron_number,p->normal,p->d,no,no); - - v_index[0] = p->vertex_index[v_split]; - v_index[1] = p->vertex_index[(v_split+1)%5]; - v_index[2] = p->vertex_index[(v_split+2)%5]; - change_vertex_indices(ph,old_polygon,v_index,3); - -} - - -void bisect_sixer(PolyhedronStruct* ph, int old_polygon) -{ - - int i, v_index[4], v_split, max_index; - int count=0, v1, v2, v3, col[6]; - double v_angle[6], vec1[3], vec2[3], max_angle=MINMDOUBLE; - PolygonStruct* p; - - p = &ph->polygon[old_polygon]; - - /* In order to find the best bisection point, first find the internal - * angles at the vertices. col[] holds the indices of vertices whose - * internal angle is 180.0, and count is number of such vertices. - */ - - for (i=0; inum_vertices; i++) - { - v1 = p->vertex_index[(i+p->num_vertices-1)%p->num_vertices]; - v2 = p->vertex_index[i]; - v3 = p->vertex_index[(i+1)%p->num_vertices]; - - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v1].coord,vec1); - MAKE_3DVECTOR(ph->vertex[v2].coord,ph->vertex[v3].coord,vec2); - - v_angle[i] = compute_angle_between_vectors(vec1,vec2); - - if (v_angle[i] > M_PI - ANGLE_EPSILON) - col[count++] = i; - if (v_angle[i] > max_angle) - { - max_angle = v_angle[i]; - max_index = i; - } - } - - if (count < 3) - { - v_split = max_index; - } - else if (count == 3) - { - if ((col[0] == 0) && (col[1] == 1) && (col[2] == p->num_vertices-1)) - { - v_split = 3; - } - else if ((col[0] == 0) && (col[1] == p->num_vertices-2) && - (col[2] == p->num_vertices-1)) - { - v_split = 2; - } - else if ((col[0] == col[1] - 1) && (col[1] == col[2] - 1)) - { - v_split = (col[1] + 3) % p->num_vertices; - } - else - { - v_split = max_index; - } - } - else - { - printf("Zero-area sixer found (%d)\n", old_polygon); - } - - v_index[0] = p->vertex_index[v_split]; - v_index[1] = p->vertex_index[(v_split+3)%6]; - v_index[2] = p->vertex_index[(v_split+4)%6]; - v_index[3] = p->vertex_index[(v_split+5)%6]; - make_new_polygon(ph,4,v_index,p->polyhedron_number,p->normal,p->d,no,no); - - v_index[0] = ph->polygon[old_polygon].vertex_index[v_split]; - v_index[1] = ph->polygon[old_polygon].vertex_index[(v_split+1)%6]; - v_index[2] = ph->polygon[old_polygon].vertex_index[(v_split+2)%6]; - v_index[3] = ph->polygon[old_polygon].vertex_index[(v_split+3)%6]; - change_vertex_indices(ph,old_polygon,v_index,4); - -} - - -/* This routine finds the largest internal angle of the polygon - * and returns the vertex number where it occurs. - */ - -int find_largest_angle(PolyhedronStruct* ph, int poly_num) -{ - - int i, v1, v2, v3, max_v; - double angle[20], vec1[3], vec2[3], dot_product, max_angle; - - for (i=0; ipolygon[poly_num].num_vertices; i++) - { - v2 = ph->polygon[poly_num].vertex_index[i]; - if (i == 0) - v1 = ph->polygon[poly_num].vertex_index[ph->polygon[poly_num].num_vertices-1]; - else - v1 = ph->polygon[poly_num].vertex_index[i-1]; - if (i == ph->polygon[poly_num].num_vertices-1) - v3 = ph->polygon[poly_num].vertex_index[0]; - else - v3 = ph->polygon[poly_num].vertex_index[i+1]; - - vec1[0] = ph->vertex[v1].coord[0] - ph->vertex[v2].coord[0]; - vec1[1] = ph->vertex[v1].coord[1] - ph->vertex[v2].coord[1]; - vec1[2] = ph->vertex[v1].coord[2] - ph->vertex[v2].coord[2]; - - vec2[0] = ph->vertex[v3].coord[0] - ph->vertex[v2].coord[0]; - vec2[1] = ph->vertex[v3].coord[1] - ph->vertex[v2].coord[1]; - vec2[2] = ph->vertex[v3].coord[2] - ph->vertex[v2].coord[2]; - - normalize_vector(vec1,vec1); - normalize_vector(vec2,vec2); - - dot_product = vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]; - - if (dot_product < -1.0) - angle[i] = M_PI; - else if (dot_product > 1.0) - angle[i] = 0.0; - else - angle[i] = acos(dot_product); - } - - for (i=0, max_angle=MINMDOUBLE; ipolygon[poly_num].num_vertices; i++) - { - if (angle[i] > max_angle) - { - max_angle = angle[i]; - max_v = i; - } - } - -/* printf("largest angle is %lf at %d\n", max_angle*RTOD, max_v);*/ - return (max_v); - -} - - -void change_vertex_indices(PolyhedronStruct* ph, int p_num, int v_index[], int num_v) -{ - - int i, j; - ReturnCode rc; - PolygonStruct* p; - - p = &ph->polygon[p_num]; - - for (i=0; inum_vertices; i++) - { - for (j=0; jvertex_index[i] == v_index[j]) - break; - } - if (j == num_v) - remove_polygon_from_vert_poly_list(&ph->vertex[p->vertex_index[i]],p_num); - } - - p->vertex_index = (int*)simm_realloc(p->vertex_index,num_v*sizeof(int),&rc); - p->num_vertices = num_v; - - for (i=0; ivertex_index[i] = v_index[i]; - add_polygon_to_vert_poly_list(&ph->vertex[v_index[i]],p_num); - } - - calc_polygon_bounding_cube(ph,p); - -} - - -void split_large_polygon(PolyhedronStruct* ph, int poly_num, SBoolean realloc_vertices, - SBoolean realloc_polygons) -{ - - int i, ph_num, v_index[3], new_v; - double pt[3], normal[3]; - PolygonStruct* p; - - p = &ph->polygon[poly_num]; - ph_num = ph->vertex[p->vertex_index[0]].polyhedron_number; - - pt[0] = pt[1] = pt[2] = 0.0; - for (i=0; inum_vertices; i++) - { - pt[0] += ph->vertex[p->vertex_index[i]].coord[0]; - pt[1] += ph->vertex[p->vertex_index[i]].coord[1]; - pt[2] += ph->vertex[p->vertex_index[i]].coord[2]; - } - - pt[0] /= p->num_vertices; - pt[1] /= p->num_vertices; - pt[2] /= p->num_vertices; - - new_v = make_new_vertex(ph,pt,ph_num,no,realloc_vertices); - ph->vertex[new_v].normal[0] = ph->polygon[poly_num].normal[0]; - ph->vertex[new_v].normal[1] = ph->polygon[poly_num].normal[1]; - ph->vertex[new_v].normal[2] = ph->polygon[poly_num].normal[2]; - - normal[0] = p->normal[0]; - normal[1] = p->normal[1]; - normal[2] = p->normal[2]; - - for (i=0; inum_vertices-1; i++) - { - v_index[0] = p->vertex_index[i]; - v_index[1] = p->vertex_index[i+1]; - v_index[2] = new_v; - - make_new_polygon(ph,3,v_index,p->polyhedron_number, - normal,p->d,realloc_polygons,no); - - p = &ph->polygon[poly_num]; - } - - v_index[0] = p->vertex_index[0]; - v_index[1] = new_v; - v_index[2] = p->vertex_index[p->num_vertices-1]; - change_vertex_indices(ph,poly_num,v_index,3); - -} - - -void reorient_polyhedron(PolyhedronStruct* ph, double rot_mat[][3]) -{ - - int i; - double new_rot1[3][3], new_rot2[3][3], res_mat[3][3], res_mat2[3][3]; - static double x_axis[] = {1.0, 0.0, 0.0}; - static double y_axis[] = {0.0, 1.0, 0.0}; - - make_3x3dircos_matrix(30.0,x_axis,new_rot1); - make_3x3dircos_matrix(30.0,y_axis,new_rot2); - - mult_3x3matrices(new_rot1,new_rot2,res_mat); - - for (i=0; inum_vertices; i++) - mult_3x3_by_vector(res_mat,ph->vertex[i].coord); - - mult_3x3matrices(res_mat,rot_mat,res_mat2); - copy_3x3matrix(res_mat2,rot_mat); - -} - - -void unorient_polyhedron(PolyhedronStruct* ph, double rot_mat[][3]) -{ - - int i; - double inverse_mat[3][3]; - - transpose_3x3matrix(rot_mat,inverse_mat); - - for (i=0; inum_vertices; i++) - mult_3x3_by_vector(inverse_mat,ph->vertex[i].coord); - -} - - -int make_new_vertex(PolyhedronStruct* ph, double pt[], int ph_num, - SBoolean check_existing, SBoolean realloc_array) -{ - - int i, v_num; - double tmp; - VertexStruct* vs; - - /* Check to see if the vertex is already in the list. If so, just return. */ - - if (check_existing == yes) - { - for (i=ph->num_vertices-1; i>=0; i--) - { - tmp = pt[0] - ph->vertex[i].coord[0]; - if (tmp > BOOL_EPSILON || tmp < -BOOL_EPSILON) - continue; - tmp = pt[1] - ph->vertex[i].coord[1]; - if (tmp > BOOL_EPSILON || tmp < -BOOL_EPSILON) - continue; - tmp = pt[2] - ph->vertex[i].coord[2]; - if (tmp > BOOL_EPSILON || tmp < -BOOL_EPSILON) - continue; -/* printf("make_new_vertex(ph = %p, matched %d)\n", ph, i);*/ - return (i); - } - } - - v_num = ph->num_vertices++; - - if (realloc_array == yes) - { - ph->vertex = (VertexStruct*)realloc(ph->vertex,ph->num_vertices*sizeof(VertexStruct)); - } - - vs = &ph->vertex[v_num]; - - preread_init_vertex(vs,v_num); - vs->coord[0] = pt[0]; - vs->coord[1] = pt[1]; - vs->coord[2] = pt[2]; - vs->polyhedron_number = ph_num; -/* - printf("just made vertex %d\n", v_num); - print_vertex(vs); -*/ - return (v_num); - -} - - -int make_new_polygon(PolyhedronStruct* ph, int num_vertices, int v_index[], - int ph_num, double normal[], double d, SBoolean realloc_array, - SBoolean recalc_normal) -{ - - int i, poly_num; - ReturnCode rc; - PolygonStruct* p; - - poly_num = ph->num_polygons++; - - if (realloc_array == yes) - { - if (ph->num_polygons == 1) - ph->polygon = (PolygonStruct*)simm_malloc(sizeof(PolygonStruct)); - else - ph->polygon = (PolygonStruct*)simm_realloc(ph->polygon, - ph->num_polygons*sizeof(PolygonStruct),&rc); - } - - p = &ph->polygon[poly_num]; - - preread_init_polygon(p); -/* - printf("make_new_polygon "); - for (i=0; inum_vertices = num_vertices; - p->vertex_index = (int*)simm_malloc(num_vertices*sizeof(int)); - for (i=0; ivertex_index[i] = v_index[i]; - - p->polyhedron_number = ph_num; - p->old = no; - - calc_polygon_bounding_cube(ph,p); - - if (recalc_normal == yes) - { - calc_polygon_normal(ph,&ph->polygon[poly_num]); - } - else - { - COPY_1X3VECTOR(normal,p->normal); - p->d = d; - } - - /* Add this polygon to the right vert_poly lists */ - add_poly_to_vert_poly_lists(ph,p,poly_num); - - return (poly_num); - -} - - -void add_poly_to_vert_poly_lists(PolyhedronStruct* ph, PolygonStruct* p, int poly_num) -{ - - int i, j; - ReturnCode rc; - VertexStruct* v; - - for (i=0; inum_vertices; i++) - { - v = &ph->vertex[p->vertex_index[i]]; - if (v->thrown_out == yes) - printf("OOPS: adding to v[deleted].polygons\n"); - for (j=0; jpolygon_count; j++) - { - if (v->polygons[j] == poly_num) - { -/* printf("add_poly_to_vert_poly_lists: already there!\n");*/ - break; - } - } - if (j == v->polygon_count) - { - v->polygon_count++; - v->polygons = (int*)simm_realloc(v->polygons,v->polygon_count*sizeof(int),&rc); - v->polygons[v->polygon_count-1] = poly_num; - } - } - -} - - -void convexify_polygons(PolyhedronStruct* ph, NormOptions* opt) -{ - - int i, j, e1, e2, v1, v2, v_new, v_from, num_concave = 0; - double p_bisect[3]; - - if (opt->convexify_polygons == no && opt->triangulate == no_tri) - return; -/* - if (verbose == yes) - { - printf("Making all polygons convex... "); - fflush(stdout); - } -*/ - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].num_vertices < 4) - continue; - if (ph->polygon[i].thrown_out == yes) - continue; - else - { - if ((v1 = polygon_is_concave(ph,i)) != -1) - { - if (ph->polygon[i].num_vertices == 4) - { - v2 = (v1+2)%ph->polygon[i].num_vertices; - bisect_polygon(ph,i,v1,v2); - } - else - { - /* Make a vertex (on the other side of the polygon) to form - * the splitting edge with. This new vertex (v_new) gets inserted - * right after v2 in the polygon's vertex list. - */ - v2 = find_bisection_point(ph,i,v1,p_bisect); - if (v2 == -1) - continue; - - v_from = ph->polygon[i].vertex_index[v1]; - v_new = make_new_vertex(ph,p_bisect,ph->vertex[v2].polyhedron_number,no,yes); - - /* The edge you want to split (by putting v_new in the middle of it) - * is defined by v2 and the next vertex after v2. Store these indices - * in e1 and e2. - */ - e1 = v2; - e2 = (v2+1)%(ph->polygon[i].num_vertices); - add_vertex(ph,i,e1,e2,v_new); - - /* The bisection vertices may have moved when you added v_new to - * the polygon, so now you want to find their new indices and put - * them in e1 and e2. - */ - for (j=0; jpolygon[i].num_vertices; j++) - if (ph->polygon[i].vertex_index[j] == v_from) - break; - e1 = j; - for (j=0; jpolygon[i].num_vertices; j++) - if (ph->polygon[i].vertex_index[j] == v_new) - break; - e2 = j; - - /* Now do a simple bisection from e1 to e2 */ - bisect_polygon(ph,i,e1,e2); - - /* Backup the loop so you scan the original polygon again - * (because it may still be concave in some other region). - */ - i--; - } - num_concave++; - } - } - } - - if (verbose == yes) - printf(" made %d polygons convex.\n", num_concave); - -} - - -int polygon_is_concave(PolyhedronStruct* ph, int poly) -{ - - int i, sign_sum, min_index, axis; - int* dot_product; - double v1[3], v2[3], normal[3], ref_vector[3]; - PolygonStruct* p; - double mag, vec_dot, min_so_far; - - p = &ph->polygon[poly]; - dot_product = (int*)malloc(p->num_vertices*sizeof(int)); - - /* form a reference vector which will be compared to all subsequent - * vectors to see if they point in the same direction. If one of them - * does not, then the polygon is concave. - */ - - /* First find three adjacent, non-collinear vertices and use them to - * find the reference vector. - */ - for (i=0; inum_vertices; i++) - { - MAKE_3DVECTOR(ph->vertex[p->vertex_index[i]].coord, - ph->vertex[p->vertex_index[(i+1)%p->num_vertices]].coord,v2); - MAKE_3DVECTOR(ph->vertex[p->vertex_index[i]].coord, - ph->vertex[p->vertex_index[(i+p->num_vertices-1)% - p->num_vertices]].coord,v1); - normalize_vector(v1,v1); - normalize_vector(v2,v2); - cross_vectors(v2,v1,ref_vector); - mag = normalize_vector(ref_vector,ref_vector); - if (mag > COLLINEAR_EPSILON) - break; - } - - /* Now form normal vectors for each vertex, and dot them with the reference - * vector. - */ - for (i=0, sign_sum=0; inum_vertices; i++) - { - MAKE_3DVECTOR(ph->vertex[p->vertex_index[i]].coord, - ph->vertex[p->vertex_index[(i+1)%p->num_vertices]].coord,v2); - MAKE_3DVECTOR(ph->vertex[p->vertex_index[i]].coord, - ph->vertex[p->vertex_index[(i+p->num_vertices-1)% - p->num_vertices]].coord,v1); - normalize_vector(v1,v1); - normalize_vector(v2,v2); - cross_vectors(v2,v1,normal); - mag = normalize_vector(normal,normal); - if (EQUAL_WITHIN_ERROR(mag,0.0)) - { - dot_product[i] = 1; - } - else - { - vec_dot = DOT_VECTORS(normal,ref_vector); - dot_product[i] = SIGN(vec_dot); - } - sign_sum += dot_product[i]; - } - - /* If each of the dot product signs equals 1, then the polygon is convex */ - if (sign_sum == p->num_vertices) - return (-1); - - /* If you reach here, the polygon is concave, so now you want to find a - * vertex where concavity occurs. You do this by finding a vertex where - * convexity occurs and then checking its dot_product value to see if it - * is positive or negative. If it is positive, then all the vertices that - * have a negative dot_product are vertices of concavity, and vice versa. - * To find a vertex of convexity, find the vertex with the smallest coordinate - * along either the X, Y, or Z axis. To make sure that the polygon is not - * perpendicular to the axis, look at the reference vector and pick the - * coordinate that is closest to zero. - */ - axis = 0; - min_so_far = DABS(ref_vector[0]); - for (i=1; i<3; i++) - { - if (DABS(ref_vector[i]) < min_so_far) - { - min_so_far = DABS(ref_vector[i]); - axis = i; - } - } - - min_index = 0; - min_so_far = ph->vertex[p->vertex_index[0]].coord[axis]; - for (i=1; inum_vertices; i++) - { - if (ph->vertex[p->vertex_index[i]].coord[axis] < min_so_far) - { - min_so_far = ph->vertex[p->vertex_index[i]].coord[axis]; - min_index = i; - } - } - - /* Now you want to store the polygon normal in the polygon structure, - * because you'll need it later if you have to bisect the polygon - * in the middle of a string of collinear vertices. The normal is - * in ref_vector[], but you also need to know which way it is pointing. - * To determine that, look at the min_index vertex (which you know - * is convex). If its dot_product is positive, then the vertices are - * ordered counterclockwise, so the ref_vector is correct. If it is - * negative, then store the negative of the ref_vector in the polygon's - * normal. - */ - - ph->polygon[poly].normal[0] = ref_vector[0] * dot_product[min_index]; - ph->polygon[poly].normal[1] = ref_vector[1] * dot_product[min_index]; - ph->polygon[poly].normal[2] = ref_vector[2] * dot_product[min_index]; - - /* Now find the first concave vertex, which is the first vertex whose - * dot_product does not match the sign of dot_product[min_index]. - */ -/* - printf("polygon %d is concave ", poly); - for (i=0; inum_vertices; i++) - printf("%d ", p->vertex_index[i]); - printf("\n"); -*/ - if (dot_product[min_index] > 0) - { - for (i=0; inum_vertices; i++) - if (dot_product[i] == -1) - return (i); - } - else - { - for (i=0; inum_vertices; i++) - if (dot_product[i] == 1) - return (i); - } - - return (-1); - -} - - -void split_edge(PolyhedronStruct* bone, int poly_num, int edge_num) -{ - - int newv, lv1, lv2, v1, v2, poly2; - double pt[3]; - - if (poly_num == -1 || edge_num == -1) - return; - - lv1 = bone->selected_edge; - lv2 = (bone->selected_edge+1)%bone->polygon[poly_num].num_vertices; - - v1 = bone->polygon[poly_num].vertex_index[lv1]; - v2 = bone->polygon[poly_num].vertex_index[lv2]; - - poly2 = find_other_polygon(bone,poly_num,v1,v2); - - pt[0] = (bone->vertex[v1].coord[0] + bone->vertex[v2].coord[0]) * 0.5; - pt[1] = (bone->vertex[v1].coord[1] + bone->vertex[v2].coord[1]) * 0.5; - pt[2] = (bone->vertex[v1].coord[2] + bone->vertex[v2].coord[2]) * 0.5; - - newv = make_new_vertex(bone,pt,0,no,yes); - - if (poly2 != -1) - { - bone->vertex[newv].normal[0] = (bone->polygon[poly_num].normal[0] + - bone->polygon[poly2].normal[0]) * 0.5; - bone->vertex[newv].normal[1] = (bone->polygon[poly_num].normal[1] + - bone->polygon[poly2].normal[1]) * 0.5; - bone->vertex[newv].normal[2] = (bone->polygon[poly_num].normal[2] + - bone->polygon[poly2].normal[2]) * 0.5; - normalize_vector(bone->vertex[newv].normal,bone->vertex[newv].normal); - } - - add_vertex(bone,poly_num,lv1,lv2,newv); - -} - -int find_nth_polygon(PolyhedronStruct* bone, int vertex_num, int* n) -{ - - int i, j, count; - - for (i=0, count=0; inum_polygons; i++) - { - for (j=0; jpolygon[i].num_vertices; j++) - { - if (bone->polygon[i].vertex_index[j] == vertex_num) - count++; - if (count == *n) - { - (*n)++; - return (i); - } - } - } - - /* If you've reached the end of the array without finding the - * nth occurrence, go back and find the first, then set n = 2. - */ - - for (i=0; inum_polygons; i++) - { - for (j=0; jpolygon[i].num_vertices; j++) - { - if (bone->polygon[i].vertex_index[j] == vertex_num) - { - *n = 2; - return (i); - } - } - } - - return (-1); - -} - - -/* This routine adds a vertex to the polygons that share the edge from e1 to e2 - * - */ -void add_vertex(PolyhedronStruct* ph, int poly, int e1, int e2, int v_new) -{ - - int i, v1, v2, other_p; - PolygonStruct* p; - ReturnCode rc; - - p = &ph->polygon[poly]; - - /* First find and save the global indices of the two vertices */ - v1 = p->vertex_index[e1]; - v2 = p->vertex_index[e2]; - - /* Put the new vertex in the vertex list right after e1. */ - p->num_vertices++; - p->vertex_index = (int*)simm_realloc(p->vertex_index,p->num_vertices*sizeof(int),&rc); - - for (i=p->num_vertices-1; i>e1+1; i--) - p->vertex_index[i] = p->vertex_index[i-1]; - p->vertex_index[e1+1] = v_new; - p->old = no; - - /* Now find the other polygon which shares the edge from v1 to v2 */ - - other_p = find_other_polygon(ph,poly,v1,v2); - - if (other_p != -1) - add_vert_to_poly(ph,other_p,v1,v2,v_new); - - add_polygon_to_vert_poly_list(&ph->vertex[v_new],poly); - -} - - -/* This routine adds a vertex to the polygon's vertex list, - * between vert1 and vert2. - */ -void add_vert_to_poly(PolyhedronStruct* ph, int poly, int vert1, int vert2, int v_new) -{ - int i, j, index2; - PolygonStruct* p; - ReturnCode rc; - - p = &ph->polygon[poly]; - - if (p->thrown_out == yes) - printf("OOPS: adding to p[deleted].vertex_index\n"); - - for (i=0; inum_vertices; i++) - { - if (i == p->num_vertices-1) - index2 = 0; - else - index2 = i+1; - if ((p->vertex_index[i] == vert1 && p->vertex_index[index2] == vert2) || - (p->vertex_index[index2] == vert1 && p->vertex_index[i] == vert2)) - break; - } - - if (i == p->num_vertices) - return; - - p->num_vertices++; - p->vertex_index = (int*)simm_realloc(p->vertex_index,p->num_vertices*sizeof(int),&rc); - - for (j=p->num_vertices-1; j>i+1; j--) - p->vertex_index[j] = p->vertex_index[j-1]; - - p->vertex_index[i+1] = v_new; - - p->old = no; - - /* Now update the vertex's polygon list */ - - add_polygon_to_vert_poly_list(&ph->vertex[v_new],poly); -} - - -/* This routine bisects a polygon by adding an edge from v1 to v2, - * cutting across the polygon. - */ -void bisect_polygon(PolyhedronStruct* ph, int poly, int v1, int v2) -{ - int i, j, n1, n2, old_num_vert, num_v; - int* v_index; - PolygonStruct* p; - - if (v1 == v2 || v1 >= ph->polygon[poly].num_vertices || - v2 >= ph->polygon[poly].num_vertices) - { - printf("error trying to bisect polygon %d (v1=%d, v2=%d)\n", poly, v1, v2); - return; - } - - p = &ph->polygon[poly]; - - old_num_vert = p->num_vertices; - - /* It's easier to split the vertex list if you know which index - * is smaller, so copy the vertex indices into n1 and n2. Then - * one half of the polygon will be the vertices from n1 to n2, - * and the other half will be from n2 to n1, wrapping around thru 0. - */ - - if (v1 > v2) - { - n1 = v2; - n2 = v1; - } - else - { - n1 = v1; - n2 = v2; - } - - v_index = (int*)simm_malloc(old_num_vert*sizeof(int)); - - num_v = old_num_vert - n2 + n1 + 1; - for (i=n2,j=0; ivertex_index[i]; - for (i=0; i<=n1; i++,j++) - v_index[j] = p->vertex_index[i]; - - /* recalculate the normal because you may be splitting a concave polygon, - * which may mean that the original normal calculation was backwards. - */ - make_new_polygon(ph,num_v,v_index,p->polyhedron_number,NULL,0.0,yes,yes); - - num_v = n2 - n1 + 1; - for (i=n1, j=0; i<=n2; i++,j++) - v_index[j] = ph->polygon[poly].vertex_index[i]; - change_vertex_indices(ph,poly,v_index,num_v); - calc_polygon_normal(ph,&ph->polygon[poly]); - - free(v_index); -} - - -/* This routine determines how to bisect a concave polygon, given one - * end of the bisecting edge (v1). It projects a ray (ray) from v1 to - * the other side of the polygon, in the direction that bisects the - * polygon angle at v1. - */ -int find_bisection_point(PolyhedronStruct* ph, int poly, int v1, double p_bisect[]) -{ - int i, v2, p1, p2; - double t, t1, t2; - double ray[3], ray1[3], ray2[3], v1vec[3], v2vec[3], p1vec[3]; - double p2vec[3], p1p2[3], ptemp1[3], ptemp2[3]; - double mag, mag1, mag2, minT= MAXMDOUBLE; - PolygonStruct* p; - - p = &ph->polygon[poly]; - v2 = -1; - - /* Make the ray that you'll project to the other side of the polygon */ - MAKE_3DVECTOR(ph->vertex[p->vertex_index[(v1+p->num_vertices-1)%p->num_vertices]].coord, - ph->vertex[p->vertex_index[v1]].coord,ray1); - MAKE_3DVECTOR(ph->vertex[p->vertex_index[(v1+1)%p->num_vertices]].coord, - ph->vertex[p->vertex_index[v1]].coord,ray2); - mag1 = normalize_vector(ray1,ray1); - mag2 = normalize_vector(ray2,ray2); - ray[0] = ray1[0] + ray2[0]; - ray[1] = ray1[1] + ray2[1]; - ray[2] = ray1[2] + ray2[2]; - mag = normalize_vector(ray,ray); - - /* If ray1 and ray2 are parallel, you'll get zero when you add them. - * If this happens, cross the polygon normal with ray1, which should - * create a ray that is perpendicular to ray1 and that points into - * the polygon. - */ - if (mag < 0.1) - { - cross_vectors(ph->polygon[poly].normal,ray1,ray); - normalize_vector(ray,ray); - } - - COPY_1X3VECTOR(ph->vertex[p->vertex_index[v1]].coord,v1vec); - v2vec[0] = v1vec[0] + ray[0]; - v2vec[1] = v1vec[1] + ray[1]; - v2vec[2] = v1vec[2] + ray[2]; - - for (i=0; inum_vertices; i++) - { - p1 = i; - if (p1 == (p->num_vertices - 1)) - p2 = 0; - else - p2 = i + 1; - - if ((p1 != v1) && (p2 != v1)) - { - MAKE_3DVECTOR(ph->vertex[p->vertex_index[p1]].coord, - ph->vertex[p->vertex_index[p2]].coord,p1p2); - COPY_1X3VECTOR(ph->vertex[p->vertex_index[p1]].coord,p1vec); - COPY_1X3VECTOR(ph->vertex[p->vertex_index[p2]].coord,p2vec); - - /* check if the point the ray is shot from is not on the line - * from p1 to p2. - */ - if (get_distsqr_point_line(v1vec,p1vec,p1p2) > CONVEX_EPSILON_SQR) - { - /* - get_nearest_2points_from_line_line_old(v1vec,ray,p1vec,p1p2,ptemp1,&t1,ptemp2,&t2); - */ - intersect_lines(v1vec,v2vec,p1vec,p2vec,ptemp1,&t1,ptemp2,&t2); - /* - get_nearest_2points_from_line_line(v1vec,v2vec,p1vec,p2vec,ptemp1,&t1,ptemp2,&t2); - */ - t = distancesqr_between_vertices(ptemp1,ptemp2); - /* TODO: handle non-planar polygons (t is fairly large) */ - if ((t > -(0.000025) && t < 0.000025) && - t1 > CONVEX_EPSILON && t1 < minT && - (t2 <= 1.0 || EQUAL_WITHIN_ERROR(t2,1.0)) && - (t2 >= 0.0 || EQUAL_WITHIN_ERROR(t2,0.0))) - { - v2 = p1; - minT = t1; - p_bisect[0] = ptemp1[0]; - p_bisect[1] = ptemp1[1]; - p_bisect[2] = ptemp1[2]; - } - } - } - } - -/* - if (v2 == -1) - { - printf("\nHmmm... couldn't find bisection point.\n"); - printf("tried to bisect at %d\n", ph->polygon[poly].vertex_index[v1]+1); - printf("mag = %lf\n", mag); - printf("ray1= %lf %lf %lf\n", ray1[0], ray1[1], ray1[2]); - printf("ray2= %lf %lf %lf\n", ray2[0], ray2[1], ray2[2]); - printf("ray= %lf %lf %lf\n", ray[0], ray[1], ray[2]); - printf("%lf %lf %lf segment ground\n", ph->vertex[p->vertex_index[v1]].coord[0], - ph->vertex[p->vertex_index[v1]].coord[1], - ph->vertex[p->vertex_index[v1]].coord[2]); - printf("%lf %lf %lf segment ground\n\n", - ray1[0]+ph->vertex[p->vertex_index[v1]].coord[0], - ray1[1]+ph->vertex[p->vertex_index[v1]].coord[1], - ray1[2]+ph->vertex[p->vertex_index[v1]].coord[2]); - printf("%lf %lf %lf segment ground\n", ph->vertex[p->vertex_index[v1]].coord[0], - ph->vertex[p->vertex_index[v1]].coord[1], - ph->vertex[p->vertex_index[v1]].coord[2]); - printf("%lf %lf %lf segment ground\n\n", - ray2[0]+ph->vertex[p->vertex_index[v1]].coord[0], - ray2[1]+ph->vertex[p->vertex_index[v1]].coord[1], - ray2[2]+ph->vertex[p->vertex_index[v1]].coord[2]); - printf("%lf %lf %lf segment ground\n", ph->vertex[p->vertex_index[v1]].coord[0], - ph->vertex[p->vertex_index[v1]].coord[1], - ph->vertex[p->vertex_index[v1]].coord[2]); - printf("%lf %lf %lf segment ground\n\n", - 5.0*ray[0]+ph->vertex[p->vertex_index[v1]].coord[0], - 5.0*ray[1]+ph->vertex[p->vertex_index[v1]].coord[1], - 5.0*ray[2]+ph->vertex[p->vertex_index[v1]].coord[2]); - for (i=0; ipolygon[poly].num_vertices; i++) - { - printf("[%d] %lf %lf %lf\n", ph->polygon[poly].vertex_index[i]+1, - ph->vertex[ph->polygon[poly].vertex_index[i]].coord[0], - ph->vertex[ph->polygon[poly].vertex_index[i]].coord[1], - ph->vertex[ph->polygon[poly].vertex_index[i]].coord[2]); - } - fflush(stdout); - } -*/ - - return v2; -} - - -int polygon_contains_edges(PolygonStruct* p, int v1, int v2, int v3) -{ - int i; - - for (i=0; inum_vertices; i++) - { - if (p->vertex_index[i] == v2) - break; - } - - if (i == p->num_vertices) - return -1; - - if (p->vertex_index[(i+p->num_vertices-1)%p->num_vertices] == v1 && - p->vertex_index[(i+1)%p->num_vertices] == v3) - return i; - - if (p->vertex_index[(i+p->num_vertices-1)%p->num_vertices] == v3 && - p->vertex_index[(i+1)%p->num_vertices] == v1) - return i; - - return -1; -} - - -/* This routine removes thrown_out vertices and polygons from the structure */ - -void compress_polyhedron(PolyhedronStruct* ph) -{ - int i, j, count, num_verts, num_polys; - VertexStruct* vert; - PolygonStruct* poly; - - if (ph->num_removed_vertices == 0 && ph->num_removed_polygons == 0) - return; - - num_verts = ph->num_vertices - ph->num_removed_vertices; - num_polys = ph->num_polygons - ph->num_removed_polygons; - - vert = (VertexStruct*)simm_malloc(num_verts*sizeof(VertexStruct)); - for (i=0, count=0; inum_vertices; i++) - { - if (ph->vertex[i].thrown_out == no) - { - copy_vertex(&ph->vertex[i],&vert[count]); - vert[count].polygon_count = 0; - vert[count].polygons = NULL; - ph->vertex[i].new_index = count++; - } - } - - poly = (PolygonStruct*)simm_malloc(num_polys*sizeof(PolygonStruct)); - for (i=0, count=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == no) - { - copy_polygon(&ph->polygon[i],&poly[count]); - for (j=0; jvertex[ph->polygon[i].vertex_index[j]].new_index; - count++; - } - } - - free(ph->vertex); - free(ph->polygon); - - ph->vertex = vert; - ph->polygon = poly; - ph->num_vertices = num_verts; - ph->num_removed_vertices = 0; - ph->num_polygons = num_polys; - ph->num_removed_polygons = 0; - - make_vert_poly_lists(ph); -} - -#ifdef unfinished_new_code -void compress_polyhedron(PolyhedronStruct* ph) -{ - int i, j, far_end, count, num_verts, num_polys; - VertexStruct* vert; - PolygonStruct* poly; - ReturnCode rc; - - num_verts = ph->num_vertices - ph->num_removed_vertices; - num_polys = ph->num_polygons - ph->num_removed_polygons; - far_end = ph->num_polygons - 1; - - /* Compress the polygon list by filling in thrown-out entries with - * polygons from the end of the array. - */ - for (i=0; ipolygon[i].thrown_out == yes) - { - for (j=far_end; j>i; j--) - { - if (ph->polygon[j].thrown_out == no) - { - copy_polygon(&ph->polygon[j],&ph->polygon[i]); - break; - } - } - far_end = j-1; - } - } - - vert = (VertexStruct*)simm_malloc(num_verts*sizeof(VertexStruct)); - for (i=0, count=0; inum_vertices; i++) - { - if (ph->vertex[i].thrown_out == no) - { - copy_vertex(&ph->vertex[i],&vert[count]); - vert[count].polygon_count = 0; - vert[count].polygons = NULL; - ph->vertex[i].new_index = count++; - } - } - - poly = (PolygonStruct*)simm_malloc(num_polys*sizeof(PolygonStruct)); - for (i=0, count=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == no) - { - copy_polygon(&ph->polygon[i],&poly[count]); - for (j=0; jvertex[ph->polygon[i].vertex_index[j]].new_index; - count++; - } - } - - ph->num_vertices = num_verts; - ph->num_removed_vertices = 0; - ph->num_polygons = num_polys; - ph->num_removed_polygons = 0; - - ph->polygon = (PolygonStruct*)simm_realloc(ph->polygon, - ph->num_polygons*sizeof(PolygonStruct),&rc); - - ph->vertex = (VertexStruct*)simm_realloc(ph->vertex, - ph->num_vertices*sizeof(VertexStruct),&rc); - - - make_vert_poly_lists(ph); -} -#endif - - -void throw_out_vertex(PolyhedronStruct* ph, int v) -{ - if (ph->vertex[v].thrown_out == no) - { - FREE_IFNOTNULL(ph->vertex[v].polygons); - ph->vertex[v].thrown_out = yes; - ph->num_removed_vertices++; - } -} - - -void throw_out_polygon(PolyhedronStruct* ph, int p) -{ - int i; - - if (ph->polygon[p].thrown_out == no) - { - for (i=0; ipolygon[p].num_vertices; i++) - remove_polygon_from_vert_poly_list(&ph->vertex[ph->polygon[p].vertex_index[i]],p); - FREE_IFNOTNULL(ph->polygon[p].vertex_index); - ph->polygon[p].thrown_out = yes; - ph->num_removed_polygons++; - } -} - - -void print_polyhedron(PolyhedronStruct* ph, char pname[]) -{ - int i; - - printf("POLYHEDRON (%s)**************************\n", pname); - printf("vertex= 0x%p\n", ph->vertex); - printf("num_vertices= %d\n", ph->num_vertices); - printf("polygon= 0x%p\n", ph->polygon); - printf("num_polygons= %d\n", ph->num_polygons); - printf("num_removed_vertices= %d\n", ph->num_removed_vertices); - printf("num_removed_polygons= %d\n", ph->num_removed_polygons); -#ifdef TIM - printf("bc= %lf %lf %lf %lf %lf %lf\n", ph->bc.x1, ph->bc.x2, - ph->bc.y1, ph->bc.y2, ph->bc.z1, ph->bc.z2); - printf("poly_list= 0x%p\n", ph->poly_list); - printf("init_flag= %d\n", ph->init_flag); -#endif - - if (ph->vertex != NULL) - { - for (i=0; inum_vertices; i++) - { - printf(" VERTEX[%d]:\n", i); - print_vertex(&ph->vertex[i]); - } - } - - if (ph->polygon != NULL) - { - for (i=0; inum_polygons; i++) - { - printf(" POLYGON[%d]:\n", i); - print_polygon(&ph->polygon[i]); - } - } - - printf("*******************************************\n"); - fflush(stdout); -} - - -void print_vertex(VertexStruct* v) -{ - int i; - - printf(" coord= %lf %lf %lf\n", v->coord[0], v->coord[1], v->coord[2]); - printf(" normal= %lf %lf %lf\n", v->normal[0], v->normal[1], v->normal[2]); - printf(" polyhedron_number= %d\n", v->polyhedron_number); - printf(" thrown_out= %d, new_index= %d\n", v->thrown_out, v->new_index); - printf(" polygon_count= %d, old = %d\n", v->polygon_count, v->old); - printf(" polygons= 0x%p (", v->polygons); - if (v->polygons != NULL) - { - for (i=0; ipolygon_count; i++) - printf("%d ", v->polygons[i]); - } - printf(")\n"); -} - - -void print_polygon(PolygonStruct* p) -{ - int i; - - printf(" num_vertices= %d, old = %d\n", p->num_vertices, p->old); - printf(" vertex_index= 0x%p (", p->vertex_index); - if (p->vertex_index != NULL) - { - for (i=0; inum_vertices; i++) - printf("%d ", p->vertex_index[i]); - } - printf(")\n"); - - printf(" normal= %lf %lf %lf\n", p->normal[0], p->normal[1], p->normal[2]); - printf(" normal_computed= %d, ", p->normal_computed); - printf(" polyhedron_number= %d\n", p->polyhedron_number); - printf(" thrown_out= %d\n", p->thrown_out); -/* - printf(" ordering_value= %lf, ", p->ordering_value); - printf(" d= %lf, old = %d\n", p->d, p->old); - printf(" bc= %.4lf %.4lf %.4lf %.4lf %.4lf %.4lf\n", p->bc.x1, p->bc.x2, - p->bc.y1, p->bc.y2, p->bc.z1, p->bc.z2); - printf(" coplanar_flag= %d, polygon_mark= %d, poly_output= %d, poly_adjpush= %d\n", - p->boolcode.coplanar_flag, p->boolcode.polygon_mark, p->boolcode.poly_output, p->boolcode.poly_adjpush); - printf(" seg_list.num_inter_seg= %d\n", p->boolcode.seg_list.num_inter_seg); - printf(" seg_list.seg= 0x%p, seg_list.segmaxx= 0x%p\n", p->boolcode.seg_list.seg, - p->boolcode.seg_list.segmaxx); - printf(" seglst_num= %d, seglst= 0x%p\n", p->boolcode.seglst_num, p->boolcode.seglst); -*/ -} - - -void print_polyhedron_simple(PolyhedronStruct* ph, char pname[]) -{ - int i, j; - - printf("%s\n", pname); - - for (i=0; inum_vertices; i++) - { - if (ph->vertex[i].thrown_out == yes) - { - printf("[%d] thrown out\n", i); - } - else - { - printf("[%d] %lf %lf %lf\n", i, ph->vertex[i].coord[0], - ph->vertex[i].coord[1],ph->vertex[i].coord[2]); - } - fflush(stdout); - } - - for (i=0; inum_polygons; i++) - { - if (ph->polygon[i].thrown_out == yes) - { - printf("[%d] thrown out\n", i); - } - else - { - printf("[%d] %d ", i, ph->polygon[i].num_vertices); - for (j=0; jpolygon[i].num_vertices; j++) - printf("%d ", ph->polygon[i].vertex_index[j]); - printf("\n"); - } - fflush(stdout); - } -} - -void free_polyhedron(PolyhedronStruct* ph, SBoolean free_ph, ModelStruct* ms) -{ - int i; - - if (!ph) - return; - - FREE_IFNOTNULL(ph->name); - - for (i=0; inum_vertices; i++) - FREE_IFNOTNULL(ph->vertex[i].polygons); - - FREE_IFNOTNULL(ph->vertex); - - for (i=0; inum_polygons; i++) - { - FREE_IFNOTNULL(ph->polygon[i].vertex_index); - FREE_IFNOTNULL(ph->polygon[i].boolcode.seglst); -//dkb crash sept 15, 2009 - FREE_IFNOTNULL(ph->polygon[i].boolcode.seg_list.seg); - } - - FREE_IFNOTNULL(ph->polygon); - -#if ! NORM && ! ENGINE && ! OPENSMAC - delete_polyhedron_display_list(ph, ms); -#endif - - if (free_ph == yes) - free(ph); -} - -void find_bounding_cube(PolyhedronStruct* polyhedron, BoundingCube* bc) -{ - int i; - - bc->x1 = bc->y1 = bc->z1 = MAXMDOUBLE; - bc->x2 = bc->y2 = bc->z2 = MINMDOUBLE; - - for (i=0; inum_vertices; i++) - { - if (polyhedron->vertex[i].thrown_out == yes) - continue; - if (polyhedron->vertex[i].coord[0] < bc->x1) - bc->x1 = polyhedron->vertex[i].coord[0]; - if (polyhedron->vertex[i].coord[0] > bc->x2) - bc->x2 = polyhedron->vertex[i].coord[0]; - if (polyhedron->vertex[i].coord[1] < bc->y1) - bc->y1 = polyhedron->vertex[i].coord[1]; - if (polyhedron->vertex[i].coord[1] > bc->y2) - bc->y2 = polyhedron->vertex[i].coord[1]; - if (polyhedron->vertex[i].coord[2] < bc->z1) - bc->z1 = polyhedron->vertex[i].coord[2]; - if (polyhedron->vertex[i].coord[2] > bc->z2) - bc->z2 = polyhedron->vertex[i].coord[2]; - } -} - -/* Unscale the bones in a model, using the scale factors - * that were stored in each segment when the model was scaled. - * This is useful when you want to export the bones of a model - * (e.g., to OpenSim). - */ -void unscale_bones(ModelStruct* model) -{ - int i, j, k; - - for (i = 0; i < model->numsegments; i++) - { - for (j = 0; j < model->segment[i].numBones; j++) - { - PolyhedronStruct* ph = &model->segment[i].bone[j]; - - for (k = 0; k < ph->num_vertices; k++) - { - ph->vertex[k].coord[XX] /= model->segment[i].bone_scale[XX]; - ph->vertex[k].coord[YY] /= model->segment[i].bone_scale[YY]; - ph->vertex[k].coord[ZZ] /= model->segment[i].bone_scale[ZZ]; - } - ph->bc.x1 /= model->segment[i].bone_scale[XX]; - ph->bc.x2 /= model->segment[i].bone_scale[XX]; - ph->bc.y1 /= model->segment[i].bone_scale[YY]; - ph->bc.y2 /= model->segment[i].bone_scale[YY]; - ph->bc.z1 /= model->segment[i].bone_scale[ZZ]; - ph->bc.z2 /= model->segment[i].bone_scale[ZZ]; - } - } -} diff --git a/OpenSim/Utilities/simmToOpenSim/normtools.h b/OpenSim/Utilities/simmToOpenSim/normtools.h deleted file mode 100644 index c0b11f11b4..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/normtools.h +++ /dev/null @@ -1,119 +0,0 @@ -/******************************************************************************* - - NORMTOOLS.H - - Author: Peter Loan - - Date: 12-SEP-96 - - Copyright (c) 1996 MusculoGraphics, Inc. - All rights reserved. - -*******************************************************************************/ - -void norm(PolyhedronStruct* ph_in, NormOptions* opt, int* num_output, - PolyhedronStruct** ph_out); -PolyhedronStruct* combine_polyhedra(PolyhedronStruct** phs, int num_phs); -ReturnCode check_polyhedron(PolyhedronStruct* ph); -void preread_init_polygon(PolygonStruct* p); -void preread_init_vertex(VertexStruct* v, int index); -void preread_init_polyhedron(PolyhedronStruct* ph); -void postread_init_polyhedron(PolyhedronStruct* ph, SBoolean full_init); -PolyhedronStruct* clone_polyhedron(PolyhedronStruct* from); -void copy_polyhedron(PolyhedronStruct* from, PolyhedronStruct* to); -void copy_vertex(VertexStruct* from, VertexStruct* to); -void copy_vertex_lite(VertexStruct* from, VertexStruct* to); -void copy_pintersegmentstruct(PInterSegmentStruct* from, - PInterSegmentStruct* to); -void copy_pintersegmentliststruct(PInterSegmentListStruct* from, - PInterSegmentListStruct* to); -void copy_polygon(PolygonStruct* from, PolygonStruct* to); -void copy_polygon_lite(PolygonStruct* from, PolygonStruct* to); -void calc_polygon_bounding_cube(PolyhedronStruct* ph, PolygonStruct* p); -void calc_polyhedron_bounding_cube(PolyhedronStruct* ph); -void make_vert_poly_lists(PolyhedronStruct* ph); -int find_other_polygon(PolyhedronStruct* ph, int p1, int v1, int v2); -int subdivide_input_polyhedron(PolyhedronStruct* ip, PolyhedronStruct** phs); -SBoolean vertex_in_polygon(PolygonStruct* polygon, int v_num); -void find_vertex_normals(PolyhedronStruct* ph, NormOptions* opt); -void make_polygons_consistent(PolyhedronStruct* ph, NormOptions* opt); -void calc_vertex_normals(PolyhedronStruct* ph, NormOptions* opt); -void order_all_vertex_lists(PolyhedronStruct* ph, int polygon); -void calc_polygon_normal(PolyhedronStruct* ph, PolygonStruct* p); -void match_vertex_orders(PolygonStruct* polygon1, PolygonStruct* polygon2, - int vertex1, int vertex2); -void reverse_vertex_order(PolygonStruct* p); -void find_polygons_sharing_edge(PolyhedronStruct* ph, int vertex1, int vertex2, - int* polygon1, int* polygon2); -VertexOrder polygon_contains_edge(PolygonStruct* p, int vertex1, int vertex2); -int find_top_vertex(VertexStruct* vertex, int num_vertices); -int find_shallowest_vertex(PolyhedronStruct* ph, int top_vertex); -VerticesFound get_adjacent_vertices(PolygonStruct* p, int seed_vertex, - int* previous_vertex, int* next_vertex); -void find_other_edge(PolyhedronStruct* ph, int poly, int start_vertex, - int adj_vertex, double other_edge[]); -int compare_dist(PListStruct* item1, PListStruct* item2); -void find_zero_area_polygons(PolyhedronStruct* ph); -void remove_zero_area_polygons(PolyhedronStruct* ph); -void find_zero_area_nontriangle(PolyhedronStruct* ph, int pl); -void remove_zero_area_nontriangle(PolyhedronStruct* ph, int pl); -void remove_duplicate_vertices(PolyhedronStruct* ph, NormOptions* opt); -void add_polygons_to_vert_poly_list(VertexStruct* v, - int* polygons, int polygon_count); -void add_polygon_to_vert_poly_list(VertexStruct* v, int poly_num); -void remove_polygon_from_vert_poly_list(VertexStruct* v, int poly_num); -void replace_vertex_index(PolygonStruct* p, int old_index, int new_index); -void remove_degenerate_polygons(PolyhedronStruct* ph); -void remove_overlapping_polygons(PolyhedronStruct* ph); -int vertex_lists_identical(PolyhedronStruct* ph, int poly1, int poly2); -void remove_extra_polygons(PolyhedronStruct* ph); -int fix_polygon(PolyhedronStruct* ph, int pl); -void order_polygons(PolyhedronStruct* polyhedron, int order_format); -void swap_polygons(PolyhedronStruct* ph, int index1, int index2); -void order_vertices(PolyhedronStruct* polyhedron, int order_format); -void swap_vertex_indices(PolyhedronStruct* ph, int index1, int index2); -void fill_holes(PolyhedronStruct* ph, SBoolean fill_holes); -void check_edge_usage(PolyhedronStruct* ph); -void check_edge_lengths(PolyhedronStruct* ph, double max_edge_length); -void triangulate_polygons(PolyhedronStruct* ph, TriangulateOption triangulate); -void quarter_quad(PolyhedronStruct* ph, int poly_num); -void split_triangle(PolyhedronStruct* ph, int poly_num); -SBoolean polygon_very_concave(PolyhedronStruct* ph, int poly_num); -void print_internal_angles(PolyhedronStruct* ph, int pl); -void trisect_fiver(PolyhedronStruct* ph, int old_polygon); -void bisect_sixer(PolyhedronStruct* ph, int old_polygon); -int find_largest_angle(PolyhedronStruct* ph, int poly_num); -void change_vertex_indices(PolyhedronStruct* ph, int p_num, int v_index[], int num_v); -void split_large_polygon(PolyhedronStruct* ph, int poly_num, SBoolean realloc_vertices, - SBoolean realloc_polygons); -void reorient_polyhedron(PolyhedronStruct* ph, double rot_mat[][3]); -void unorient_polyhedron(PolyhedronStruct* ph, double rot_mat[][3]); -int make_new_vertex(PolyhedronStruct* ph, double pt[], int ph_num, - SBoolean check_existing, SBoolean realloc_array); -int make_new_polygon(PolyhedronStruct* ph, int num_vertices, int v_index[], - int ph_num, double normal[], double d, SBoolean realloc_array, - SBoolean recalc_normal); -void add_poly_to_vert_poly_lists(PolyhedronStruct* ph, PolygonStruct* p, int poly_num); -void convexify_polygons(PolyhedronStruct* ph, NormOptions* opt); -int polygon_is_concave(PolyhedronStruct* ph, int poly); -void split_edge(PolyhedronStruct* bone, int poly_num, int edge_num); -int find_nth_polygon(PolyhedronStruct* bone, int vertex_num, int* n); -void add_vertex(PolyhedronStruct* ph, int poly, int e1, int e2, int v_new); -void add_vert_to_poly(PolyhedronStruct* ph, int poly, int vert1, int vert2, int v_new); -void bisect_polygon(PolyhedronStruct* ph, int poly, int v1, int v2); -int find_bisection_point(PolyhedronStruct* ph, int poly, int v1, double p_bisect[]); -int polygon_contains_edges(PolygonStruct* p, int v1, int v2, int v3); -void compress_polyhedron(PolyhedronStruct* ph); -void throw_out_vertex(PolyhedronStruct* ph, int v); -void throw_out_polygon(PolyhedronStruct* ph, int p); -void print_polyhedron(PolyhedronStruct* ph, char pname[]); -void print_vertex(VertexStruct* v); -void print_polygon(PolygonStruct* p); -void print_polyhedron_simple(PolyhedronStruct* ph, char pname[]); -void free_polyhedron(PolyhedronStruct* ph, SBoolean free_ph, ModelStruct* ms); -void find_bounding_cube(PolyhedronStruct* polyhedron, BoundingCube* bc); -void unscale_bones(ModelStruct* model); - -#if ! NORM && ! ENGINE && ! OPENSMAC -void delete_polyhedron_display_list(PolyhedronStruct* ph, ModelStruct* ms); -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/orthotrak.h b/OpenSim/Utilities/simmToOpenSim/orthotrak.h deleted file mode 100644 index 0915f98014..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/orthotrak.h +++ /dev/null @@ -1,46 +0,0 @@ -/******************************************************************************* - - ORTHOTRAK.H - - Author: Krystyne Blaikie - Peter Loan - - Date: 14-Aug-02 - - Copyright (c) 2002-4 MusculoGraphics, Inc. - All rights reserved. - -*******************************************************************************/ - -STRUCT -{ - int numNames; - char **markerNames; -} MarkerNameStruct; - -STRUCT -{ - double inferior; - double lateral; - double posterior; -} HJCOffsetStruct; - -STRUCT -{ - HJCOffsetStruct ROffset; - HJCOffsetStruct LOffset; - double RFootLength; - double LFootLength; - double RKneeDiameter; - double LKneeDiameter; - double RAnkleDiameter; - double LAnkleDiameter; - double subjectMass; -} PersonalDataStruct; - - -void subPoints(smPoint3 a, smPoint3 b, smPoint3 c); -ReturnCode checkMarkerMovement(double movement, smUnit units, double threshold); -void initializeOrthoTrakStruct(smOrthoTrakModel *data); -ReturnCode createsmModel(ModelStruct *simmModel, smModel *smMod); -void deletesmModel(smModel *model); diff --git a/OpenSim/Utilities/simmToOpenSim/path.c b/OpenSim/Utilities/simmToOpenSim/path.c deleted file mode 100644 index fd4ae7d6b6..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/path.c +++ /dev/null @@ -1,314 +0,0 @@ -/******************************************************************************* - - PATH.C - - Author: Peter Loan - - Date: 8-MAY-89 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines which find paths between - the segments (bones) in a model. - - Routines: - makepaths : main routine to find paths between every pair of segments - printpaths : prints out the path list for debugging - enterpath : trys to add a joint onto an existing path - setpath : given a path, copies it into the path list - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static int enterpath(ModelStruct* ms, int j, int k, int newend); -static ReturnCode setpath(ModelStruct* ms, int list[]); - - - -/* MAKEPATHS: */ - -ReturnCode makepaths(ModelStruct* ms) -{ - int i, j, k, m, n, jnt, segments, old_count, np; - int* path = NULL; - int from, to, nsq, nj, index, total, numpaths = 0; - Direction dir; - - if (ms->numsegments == 1) - { - ms->pathptrs = (int **)simm_malloc(sizeof(int*)); - ms->pathptrs[0] = (int*)simm_malloc(sizeof(int)); - ms->pathptrs[0][0] = 0; - return code_fine; - } - - for (i=0, nj=0; inumjoints; i++) - { - if (ms->joint[i].loop_joint == no) - { - nj++; - } - } - segments = ms->numsegments; - nsq = segments*segments; - total = segments*segments - segments - 2*nj; - - if (ms->pathptrs != NULL) - { - for (i=0; ipathptrs[i]); - free(ms->pathptrs); - } - - ms->pathptrs = (int **)simm_malloc(nsq*sizeof(int*)); - if (ms->pathptrs == NULL) - return code_bad; - - for (i=0; ipathptrs[i] = (int *)0; - - for (i=0; inumjoints; i++) - { - if (ms->joint[i].loop_joint == no) - { - path = (int*)simm_malloc(3*sizeof(int)); - path[0] = ms->joint[i].from; - path[1] = ms->joint[i].to; - path[2] = END_OF_ARRAY; - if (setpath(ms, path) == code_bad) // setpath takes ownership of path - return code_bad; - - path = (int*)simm_malloc(3*sizeof(int)); - path[0] = ms->joint[i].to; - path[1] = ms->joint[i].from; - path[2] = END_OF_ARRAY; - if (setpath(ms, path) == code_bad) // setpath takes ownership of path - return code_bad; - } - } - - while (numpaths < total) - { - old_count = numpaths; - for (i=0; inumjoints; i++) - { - if (ms->joint[i].loop_joint == yes) - continue; - m = ms->joint[i].from; - n = ms->joint[i].to; - for (j=0; jpathptrs[index] != NULL) - { - if ((np = enterpath(ms, j, m, n)) == -1) - return code_bad; - else - { - numpaths += np; - // printf("added %d to path[%d]: %d -> %d\n", n, j*segments + n, j, k); - } - } - index = j*segments+n; - if (ms->pathptrs[index] != NULL) - { - if ((np = enterpath(ms, j, n, m)) == -1) - return code_bad; - else - { - numpaths += np; - // printf("added %d to path[%d]: %d -> %d\n", m, j*segments+m, j, k); - } - } - } - } - } - if (numpaths == old_count && numpaths < total) - { - message("Error forming model: cannot find joint path between the following pairs of segments:",0,DEFAULT_MESSAGE_X_OFFSET); - for (i=0; ipathptrs[index] == NULL) - { - (void)sprintf(buffer," (%s,%s)", ms->segment[i].name, - ms->segment[j].name); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET+20); - } - } - } - return code_bad; - } - } - - /* Now that the paths have been made, turn them into a list of joints, - * rather than body segments. A list of joints is faster to traverse, - * and this will improve the speed of the See system. In these joint - * lists, negative numbers refer to inverse joints, joint numbers start - * at 1 (because you can't have -0) and a value of numjoints+1 marks the - * end of each list. - */ - for (i=0; ipathptrs[index][k] != j; k++) - { - from = ms->pathptrs[index][k]; - to = ms->pathptrs[index][k+1]; - jnt = find_joint_between_frames(ms, from, to, &dir); - if (dir == FORWARD) - ms->pathptrs[index][k] = jnt+1; - else - ms->pathptrs[index][k] = -(jnt+1); - } - ms->pathptrs[index][k] = ms->numjoints+1; -// ms->pathptrs[index][j] = END_OF_ARRAY; - } - } - - /* For each joint, do the following: - * for each segment, check to see if the joint is used in the path from - * that segment to ground. - */ - for (k=0; knumjoints; k++) - { - if (ms->joint[k].in_seg_ground_path == NULL) - ms->joint[k].in_seg_ground_path = (SBoolean*)simm_malloc(ms->numsegments*sizeof(SBoolean)); - for (i=0; inumsegments; i++) - { - ms->joint[k].in_seg_ground_path[i] = no; - - if (i == ms->ground_segment) - continue; - - path = GET_PATH(ms->modelnum,ms->ground_segment,i); - - for (j=0; path[j] != ms->numjoints+1; j++) - if (ABS(path[j])-1 == k) - ms->joint[k].in_seg_ground_path[i] = yes; - } - } - return code_fine; -} - - -void printpaths(ModelStruct* ms) -{ - int i, j, k, index, jnt; - - printf("\n\n*******************\n"); - printf("from: to: \n"); - for (i=0; inumsegments; i++) - { - for (k=0; knumsegments; k++) - { - index = i*ms->numsegments+k; - printf("path from %s to %s: ", ms->segment[i].name, - ms->segment[k].name); - if (ms->pathptrs[index] == NULL) - { - printf("empty\n"); - continue; - } - if (ms->pathptrs[index][0] == ms->numjoints+1) - { - printf("empty\n"); - continue; - } - for (j=0; ms->pathptrs[index][j] != ms->numjoints+1; j++) -// for (j=0; ms->pathptrs[index][j] != END_OF_ARRAY; j++) - { - jnt = ABS(ms->pathptrs[index][j]) - 1; - printf("%s ", ms->joint[jnt].name); - // printf("%d ", ms->pathptrs[index][j]); - } - printf("\n"); - } - } - printf("\n*******************\n\n"); - -} - - -/* ENTERPATH: This routine finds the path from j to k in the pathptrs - * list, adds newend onto the end of it, and puts the result back in - * pathptrs as the path from j to newend. - */ -static int enterpath(ModelStruct* ms, int j, int k, int newend) -{ - int length, *oldPath, *newPath; - - // If there is already a path from j to newend, just return. - if (ms->pathptrs[j * ms->numsegments + newend] != NULL) - return 0; - - oldPath = ms->pathptrs[j * ms->numsegments + k]; - length = get_array_length(oldPath); - - newPath = (int*)simm_malloc((length+2) * sizeof(int)); - memcpy(newPath, oldPath, length * sizeof(int)); - newPath[length] = newend; - newPath[length+1] = END_OF_ARRAY; - - if (setpath(ms, newPath) == code_bad) // setpath takes ownership of newPath - return -1; - - return 1; -} - - -/* SETPATH: given a list of segments, this routine finds which pathpointer - * array should be equal to it, and sets it accordingly. For example, if - * the list is 2 5 4 7 3, then the list is the path for the segment pair (2,3). - * This function passes ownership of 'list' to model[]->pathptrs[]. - */ -static ReturnCode setpath(ModelStruct* ms, int list[]) -{ - int length, start, finish, index; - - if (list == NULL) - return code_bad; - - start = list[0]; - length = get_array_length(list); - finish = list[length - 1]; - index = start * ms->numsegments + finish; - - ms->pathptrs[index] = list; - - return code_fine; -} diff --git a/OpenSim/Utilities/simmToOpenSim/readmodel.c b/OpenSim/Utilities/simmToOpenSim/readmodel.c deleted file mode 100644 index 676bf41a3d..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/readmodel.c +++ /dev/null @@ -1,3494 +0,0 @@ -/******************************************************************************* - - READMODEL.C - - Author: Peter Loan - - Date: 8-DEC-88 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines that read in a model - definition from an input file. - - Routines: - read_model_file : controls the reading of a model definition - read_gencoord : reads in a gencoord definition - read_joint : controls the reading of all joint parameters - read_restraint : reads a gencoord restraint function - read_segment : reads in a segment description - read_axis : reads in a rotation axis for a joint - read_refeq : reads in a 'reference equation' - read_order : reads in the transformation order for a joint - read_function : reads in a set of x-y pairs that define a function - malloc_function : mallocs space for a splined function - -*******************************************************************************/ -#include - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "defunctions.h" - -#include "wefunctions.h" - -/*************** DEFINES (for this file only) *********************************/ -#define INIT_SEGMENT_FILE_ARRAY_SIZE 5 -#define SEGMENT_FILE_ARRAY_INCREMENT 10 - -static char* beginposeString = "beginpose"; -static char* endposeString = "endpose"; -static char* ignoreString1 = "read_script_file"; -static char* ignoreString2 = "write_script_file"; -static char* ignoreString3 = "crosshair_visibility"; -static char* ignoreString4 = "muscle_point_visibility"; -static char* ignoreString5 = "shadow_visibility"; - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static ReturnCode read_gencoord(ModelStruct* ms, FILE* fp); -static ReturnCode set_wrap_specs(dpWrapObject* wo, char str[]); -static ReturnCode read_wrap_object(ModelStruct* ms, FILE* fp); -static ReturnCode read_restraint(ModelStruct* ms, FILE* fp, GeneralizedCoord* gc, int minmax); -static ReturnCode read_joint(ModelStruct* ms, FILE* fp); -static ReturnCode read_segment(ModelStruct* ms, FILE* fp); -static ReturnCode read_axis(ModelStruct* ms, FILE* fp, int joint, int axis); -static ReturnCode read_refeq(ModelStruct* ms, FILE* fp, DofStruct* jointvar); -static ReturnCode read_order(FILE* fp, int order[]); -static ReturnCode read_show_axis(ModelStruct* ms, FILE* fp, JointStruct* jnt); -static ReturnCode read_modelview(Scene* scene, ModelStruct* ms, FILE* fp); - -static ReturnCode read_gencoord_groups(ModelStruct*, FILE*, GeneralizedCoord* gencoord); -static ReturnCode read_segment_groups(ModelStruct*, FILE*, int segnum); -static DrawingMode get_drawmode(char buffer[]); - -static ReturnCode read_contact(ModelStruct* ms, FILE* fp); -static ReturnCode read_spring(ModelStruct* ms, FILE* fp); -static ReturnCode read_spring_floor(ModelStruct* ms, FILE* fp); -static void add_spring_point_to_floor(SegmentStruct* segment, SpringPoint* point); -static ReturnCode read_force_matte(ModelStruct* ms, SegmentStruct* seg, FILE* fp); -static ReturnCode read_contact_object(ModelStruct* ms, FILE* fp); -static ReturnCode read_contact_pair(ModelStruct* ms, FILE* fp); -static ReturnCode read_contact_group(ModelStruct* ms, FILE* fp); -static PolyhedronStruct* make_bone(ModelStruct* ms, SegmentStruct* seg, char filename[]); -static ReturnCode read_marker(ModelStruct* ms, SegmentStruct* seg, FILE* fp); - -static ReturnCode read_constraint(ModelStruct *ms, FILE* fp); -static ReturnCode set_constraint_specs(ConstraintObject* co, char str[]); -static ConstraintPoint* read_constraint_points(ModelStruct* ms, FILE* fp, int *numpoints, int *cp_array_size); -static ReturnCode read_motion_object(ModelStruct* ms, FILE* fp); - -//TODO_SCENE: ideally, this function should take a model argument, not a scene. -// The model should be added to the scene later. But for now, you need the scene -// to store some of the display info that is specified in the model file. -ReturnCode read_model_file(Scene* scene, ModelStruct* ms, char filename[], SBoolean showTopLevelMessages) -{ - char tempFileName[CHARBUFFER]; - int model_errors = 0; - FILE* fp; - ReturnCode status = code_fine; - -#if ENGINE - strcpy(tempFileName, ".model"); -#else - strcpy(tempFileName, glutGetTempFileName("simm-model")); -#endif - - if ((fp = preprocess_file(filename, tempFileName)) == NULL) - { - if (showTopLevelMessages == yes) - { - (void)sprintf(errorbuffer, "Unable to open model input file %s", filename); - error(none, errorbuffer); - } - return code_bad; - } - - /* Now that you know the joint file exists, store the full path in the - * model struct because the code to read bone files needs it. - */ - mstrcpy(&ms->jointfilename,filename); - - while (1) - { - if (read_string(fp,buffer) == EOF) - break; - - if (buffer[0] == '#') - { - read_nonempty_line(fp,buffer); - continue; - } - /* Skip over unsupported options */ - if (STRINGS_ARE_EQUAL(buffer, ignoreString1) || - STRINGS_ARE_EQUAL(buffer, ignoreString2) || - STRINGS_ARE_EQUAL(buffer, ignoreString3) || - STRINGS_ARE_EQUAL(buffer, ignoreString4) || - STRINGS_ARE_EQUAL(buffer, ignoreString5)) - { - _read_til(fp, '\n'); - continue; - } - if (STRINGS_ARE_EQUAL(buffer, beginposeString)){ - // skip until endposeString - while(STRINGS_ARE_NOT_EQUAL(buffer, endposeString)) - read_line(fp,buffer); - _read_til(fp, '\n'); - continue; - } - if (STRINGS_ARE_EQUAL(buffer,"name")) - { - read_line(fp,buffer); - ms->name = (char*)simm_malloc(250*sizeof(char)); - if (ms->name == NULL) - goto input_error; - (void)strcpy(ms->name,buffer); - strip_trailing_white_space(ms->name); - } - else if (STRINGS_ARE_EQUAL(buffer,"force_units")) - { - if (read_string(fp, buffer) == EOF) - break; - mstrcpy(&ms->forceUnits, buffer); - } - else if ((STRINGS_ARE_EQUAL(buffer,"length_units")) || STRINGS_ARE_EQUAL(buffer, "translation_units")) - { - if (read_string(fp, buffer) == EOF) - break; - mstrcpy(&ms->lengthUnits, buffer); - } - else if (STRINGS_ARE_EQUAL(buffer,"HTR_translation_units")) - { - if (read_string(fp, buffer) == EOF) - break; - mstrcpy(&ms->HTRtranslationUnits, buffer); - } - else if (STRINGS_ARE_EQUAL(buffer, "MV_gear") || STRINGS_ARE_EQUAL(buffer, "motion_speed")) - { - if (fscanf(fp, "%lf", &ms->dis.motion_speed) != 1) - { - error(recover,"Error reading MV_gear value in joint file."); - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"solver_accuracy")) - { - if (fscanf(fp, "%lf", &ms->solver.accuracy) != 1) - { - error(recover,"Error reading solver_accuracy value in joint file."); - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"solver_max_iterations")) - { - if (fscanf(fp, "%d", &ms->solver.max_iterations) != 1) - { - error(recover,"Error reading solver_max_iterations value in joint file."); - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"solver_method")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "LM") || STRINGS_ARE_EQUAL(buffer, "lm")) - ms->solver.method = smLevenbergMarquart; - else if (STRINGS_ARE_EQUAL(buffer, "GN") || STRINGS_ARE_EQUAL(buffer, "gn")) - ms->solver.method = smGaussNewton; - else - { - error(recover,"Error reading solver_method in joint file (must be LM or GN)."); - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"solver_joint_limits")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "YES") - || STRINGS_ARE_EQUAL(buffer, "true") || STRINGS_ARE_EQUAL(buffer, "TRUE")) - ms->solver.joint_limits = smYes; - else if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "NO") - || STRINGS_ARE_EQUAL(buffer, "false") || STRINGS_ARE_EQUAL(buffer, "FALSE")) - ms->solver.joint_limits = smNo; - else - { - error(recover,"Error reading solver_joint_limits in joint file (must be yes/true or no/false)."); - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"solver_orient_body")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "YES") - || STRINGS_ARE_EQUAL(buffer, "true") || STRINGS_ARE_EQUAL(buffer, "TRUE")) - ms->solver.orient_body = smYes; - else if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "NO") - || STRINGS_ARE_EQUAL(buffer, "false") || STRINGS_ARE_EQUAL(buffer, "FALSE")) - ms->solver.orient_body = smNo; - else - { - error(recover,"Error reading solver_orient_body in joint file (must be yes/true or no/false)."); - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"solver_fg_contact")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "YES") - || STRINGS_ARE_EQUAL(buffer, "true") || STRINGS_ARE_EQUAL(buffer, "TRUE")) - ms->solver.fg_contact = smYes; - else if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "NO") - || STRINGS_ARE_EQUAL(buffer, "false") || STRINGS_ARE_EQUAL(buffer, "FALSE")) - ms->solver.fg_contact = smNo; - else - { - error(recover,"Error reading solver_fg_contact in joint file (must be yes/true or no/false)."); - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"window_position")) - { - if (fscanf(fp, "%d %d", &scene->windowX1, &scene->windowY1) != 2) - { - error(recover,"Error reading window_position (X, Y) in joint file."); - break; - } - else - { -#if ! ENGINE - int scrLeft, scrRight, scrTop, scrBottom; - glutGetWorkArea(&scrLeft, &scrRight, &scrTop, &scrBottom); - if (scene->windowX1 >= scrRight) - scene->windowX1 = scrRight - 100; -#endif - } - } - else if (STRINGS_ARE_EQUAL(buffer,"window_size")) - { - if (fscanf(fp, "%d %d", &scene->windowWidth, &scene->windowHeight) != 2) - { - error(recover,"Error reading window_size (width, height) in joint file."); - break; - } - else - { - if (scene->windowWidth < 50) - scene->windowWidth = 50; - if (scene->windowHeight < 50) - scene->windowHeight = 50; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "gravity")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - error(recover, "Error reading gravity value in joint file."); - break; - } - if (STRINGS_ARE_EQUAL(buffer,"x") || STRINGS_ARE_EQUAL(buffer,"X") || - STRINGS_ARE_EQUAL(buffer,"+x") || STRINGS_ARE_EQUAL(buffer,"+X")) - ms->gravity = smX; - else if (STRINGS_ARE_EQUAL(buffer,"-x") || STRINGS_ARE_EQUAL(buffer,"-X")) - ms->gravity = smNegX; - else if (STRINGS_ARE_EQUAL(buffer,"y") || STRINGS_ARE_EQUAL(buffer,"Y") || - STRINGS_ARE_EQUAL(buffer,"+y") || STRINGS_ARE_EQUAL(buffer,"+Y")) - ms->gravity = smY; - else if (STRINGS_ARE_EQUAL(buffer,"-y") || STRINGS_ARE_EQUAL(buffer,"-Y")) - ms->gravity = smNegY; - else if (STRINGS_ARE_EQUAL(buffer,"z") || STRINGS_ARE_EQUAL(buffer,"Z") || - STRINGS_ARE_EQUAL(buffer,"+z") || STRINGS_ARE_EQUAL(buffer,"+Z")) - ms->gravity = smZ; - else if (STRINGS_ARE_EQUAL(buffer,"-z") || STRINGS_ARE_EQUAL(buffer,"-Z")) - ms->gravity = smNegZ; - else if (STRINGS_ARE_EQUAL(buffer,"none") || STRINGS_ARE_EQUAL(buffer,"zero")) - ms->gravity = smNoAlign; - else - { - error(abort_action, "Bad axis specified for gravity (must be X, -X, Y, -Y, Z, -Z, zero, or none"); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"beginjoint")) - { - if (read_joint(ms,fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"begingencoord")) - { - if (read_gencoord(ms, fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"beginsegment")) - { - if (read_segment(ms, fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpFunctionTypeUndefined, 0))) // old-style function definitions - { - if (read_function(ms, fp, no, dpNaturalCubicSpline, get_function_tag(dpFunctionTypeUndefined, 1)) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpStepFunction, 0))) - { - if (read_function(ms, fp, no, dpStepFunction, get_function_tag(dpStepFunction, 1)) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpLinearFunction, 0))) - { - if (read_function(ms, fp, no, dpLinearFunction, get_function_tag(dpLinearFunction, 1)) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpNaturalCubicSpline, 0))) - { - if (read_function(ms, fp, no, dpNaturalCubicSpline, get_function_tag(dpNaturalCubicSpline, 1)) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpGCVSpline, 0))) - { - if (read_function(ms, fp, no, dpGCVSpline, get_function_tag(dpGCVSpline, 1)) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"beginworldobject")) - { - if (read_world_object(ms, fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"beginmotionobject")) - { - if (read_motion_object(ms, fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"begincontact")) - { - if (read_contact(ms,fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"beginwrapobject")) - { - if (read_wrap_object(ms,fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"begindeformity")) - { - if (read_deformity(ms,fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer, "constraint_tolerance")) - { - double value; - fscanf(fp, "%lg", &value); - /* if (fscanf(fp,"%lg", &ms->constraint_tolerance) != 1) - { - error(recover,"Error reading constraint_tolerance."); - ms->constraint_tolerance = DEFAULT_CONSTRAINT_TOLERANCE; - model_errors++; - }*/ - } - else if (STRINGS_ARE_EQUAL(buffer, "loop_tolerance")) - { - if (fscanf(fp,"%lg", &ms->loop_tolerance) != 1) - { - error(recover,"Error reading loop_tolerance."); - ms->loop_tolerance = DEFAULT_LOOP_TOLERANCE; - model_errors++; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "loop_weight")) - { - if (fscanf(fp,"%lg", &ms->loop_weight) != 1) - { - error(recover,"Error reading loop_weight."); - ms->loop_weight = DEFAULT_LOOP_WEIGHT; - model_errors++; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "beginconstraintobject")) - { - if (read_constraint(ms, fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"beginmaterial")) - { - if (read_material(ms, fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"begincamera") || STRINGS_ARE_EQUAL(buffer,"beginview")) - { - if (read_modelview(scene, ms, fp) == code_bad) - goto input_error; - } - else if (STRINGS_ARE_EQUAL(buffer,"no_trackball")) - { - scene->trackball = no; - } - else if (STRINGS_ARE_EQUAL(buffer,"bone_path")) - { - if (read_line(fp, buffer) == EOF) - { - error(recover,"Error reading name of bone path within joint file."); - model_errors++; - } - else - { - strip_trailing_white_space(buffer); - mstrcpy(&ms->bonepathname,buffer); - } - } - else if (STRINGS_ARE_EQUAL(buffer,"muscle_file")) - { - if (read_line(fp, buffer) == EOF) - { - error(recover,"Error reading name of muscle file within joint file."); - model_errors++; - } - else - { - strip_trailing_white_space(buffer); - mstrcpy(&ms->musclefilename,buffer); - } - } - else if (STRINGS_ARE_EQUAL(buffer,"motion_file")) - { - if (read_line(fp, buffer) == EOF) - { - error(recover,"Error reading name of motion file within joint file."); - model_errors++; - } - else - { - if (ms->num_motion_files >= 100) - { - error(recover,"You can specify only 100 motion files in a joint file."); - } - else - { - strip_trailing_white_space(buffer); - mstrcpy(&ms->motionfilename[ms->num_motion_files],buffer); - ms->num_motion_files++; - } - } - } - else if (STRINGS_ARE_EQUAL(buffer, "inverse_kinematics_solver") || STRINGS_ARE_EQUAL(buffer, "IK_solver")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - ms->useIK = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - ms->useIK = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"background_color")) - { - GLfloat col[3]; - if (fscanf(fp,"%f %f %f", &col[0], &col[1], &col[2]) != 3) - { - error(recover,"Error reading background color for model window."); - model_errors++; - } - else - { - ms->dis.background_color[0] = col[0]; - ms->dis.background_color[1] = col[1]; - ms->dis.background_color[2] = col[2]; - ms->dis.background_color_spec = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"rotation_axes_color")) - { - GLfloat col[3]; - if (fscanf(fp,"%f %f %f", &col[0], &col[1], &col[2]) != 3) - { - error(recover,"Error reading color for joint rotation axes."); - model_errors++; - } - else - { - ms->dis.rotation_axes_color[0] = col[0]; - ms->dis.rotation_axes_color[1] = col[1]; - ms->dis.rotation_axes_color[2] = col[2]; - ms->dis.rotation_axes_color_spec = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "segment_axes_color")) - { - GLfloat col[3]; - fscanf(fp, "%f %f %f", &col[0], &col[1], &col[2]); - error(none, "Warning: segment_axes_color is no longer supported. Axes are now colored red/green/blue."); - } - else if (STRINGS_ARE_EQUAL(buffer,"vertex_label_color")) - { - GLfloat col[3]; - if (fscanf(fp,"%f %f %f", &col[0], &col[1], &col[2]) != 3) - { - error(recover,"Error reading color for vertex labels."); - model_errors++; - } - else - { - ms->dis.vertex_label_color[0] = col[0]; - ms->dis.vertex_label_color[1] = col[1]; - ms->dis.vertex_label_color[2] = col[2]; - ms->dis.vertex_label_color_spec = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"crosshairs_color")) - { - GLfloat col[3]; - if (fscanf(fp,"%f %f %f", &col[0], &col[1], &col[2]) != 3) - { - error(recover,"Error reading color for model window crosshairs."); - model_errors++; - } - else - { - ms->dis.crosshairs_color[0] = col[0]; - ms->dis.crosshairs_color[1] = col[1]; - ms->dis.crosshairs_color[2] = col[2]; - ms->dis.crosshairs_color_spec = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"marker_visibility")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - ms->marker_visibility = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - ms->marker_visibility = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"marker_radius")) - { - if (fscanf(fp,"%lg", &ms->marker_radius) != 1) - { - error(recover,"Error reading marker_radius."); - ms->marker_radius = DEFAULT_MARKER_RADIUS; - model_errors++; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"masscenter_visibility")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - ms->global_show_masscenter = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - ms->global_show_masscenter = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"inertia_visibility")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - ms->global_show_inertia = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - ms->global_show_inertia = yes; - } - else - { - (void)sprintf(errorbuffer,"Unrecognized string \"%s\" found in joint file.", buffer); - error(recover,errorbuffer); - model_errors++; - } - if (model_errors > 50) - { - error(none,"Too many errors to continue."); - goto input_error; - } - } - - goto cleanup; - - input_error: - status = code_bad; - - cleanup: - (void)fclose(fp); - (void)unlink((const char*)tempFileName); - - return status; -} - - - -/* READ_GENCOORD: */ - -static ReturnCode read_gencoord(ModelStruct* ms, FILE* fp) -{ - int nk; - char key1[64], key2[64]; - SBoolean range_specified = no, restraintFuncSpecified = no; - double rangeval1, rangeval2, gc_tolerance, pd_stiffness; - GeneralizedCoord* gc; - - if (read_string(fp, buffer) == EOF) - { - sprintf(errorbuffer,"Unexpected EOF found reading gencoord definition"); - error(abort_action, errorbuffer); - return code_bad; - } - - gc = enter_gencoord(ms, buffer, yes); - - if (gc == NULL) - { - sprintf(errorbuffer, "Unable to allocate space for gencoord %s", buffer); - error(abort_action, errorbuffer); - return code_bad; - } - - if (gc->defined == yes) - { - (void)sprintf(errorbuffer,"Error: redefinition of gencoord %s", buffer); - error(abort_action,errorbuffer); - return code_bad; - } - - while (1) - { - if (read_string(fp,buffer) == EOF) - { - (void)sprintf(errorbuffer,"Unexpected EOF found reading gencoord definition"); - error(abort_action,errorbuffer); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer,"endgencoord")) - { - gc->defined = yes; - break; - } - - if (STRINGS_ARE_EQUAL(buffer,"keys")) - { - read_line(fp,buffer); - nk = sscanf(buffer,"%s %s", key1, key2); - if (nk == 1) - gc->keys[0] = gc->keys[1] = lookup_simm_key(key1); - else if (nk == 2) - { - gc->keys[0] = lookup_simm_key(key1); - gc->keys[1] = lookup_simm_key(key2); - } - else - { - (void)sprintf(errorbuffer,"Error reading keys for gencoord %s", - gc->name); - error(recover,errorbuffer); - gc->keys[0] = gc->keys[1] = null_key; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"wrap")) - { - gc->wrap = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"range")) - { - if (fscanf(fp,"%lg %lg", &rangeval1, &rangeval2) != 2) - { - (void)sprintf(errorbuffer, "Error reading range for gencoord %s", - gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - gc->range.start = _MIN(rangeval1,rangeval2); - gc->range.end = _MAX(rangeval1,rangeval2); - range_specified = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"tolerance")) - { - if (fscanf(fp,"%lg", &gc_tolerance) != 1) - { - (void)sprintf(errorbuffer, "Error reading tolerance for gencoord %s", - gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - gc->tolerance = gc_tolerance; - } - else if (STRINGS_ARE_EQUAL(buffer,"default_value")) - { - if (fscanf(fp,"%lg", &gc->default_value) != 1) - { - (void)sprintf(errorbuffer, "Error reading default value for gencoord %s", - gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - gc->default_value_specified = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"unclamped")) - { - gc->clamped = gc->clamped_save = no; - } - else if (STRINGS_ARE_EQUAL(buffer, "clamped")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - gc->clamped = gc->clamped_save = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - gc->clamped = gc->clamped_save = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"locked")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - gc->locked = gc->locked_save = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - gc->locked = gc->locked_save = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"restraint")) - { - if (read_restraint(ms,fp,gc,2) == code_bad) - { - (void)sprintf(errorbuffer, "Error reading restraint for gencoord %s", - gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - restraintFuncSpecified = yes; - } - else if (STRINGS_ARE_EQUAL(buffer, "active")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - gc->restraintFuncActive = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - gc->restraintFuncActive = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"minrestraint")) - { - if (read_restraint(ms,fp,gc,0) == code_bad) - { - (void)sprintf(errorbuffer, "Error reading minrestraint for gencoord %s", - gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"maxrestraint")) - { - if (read_restraint(ms,fp,gc,1) == code_bad) - { - (void)sprintf(errorbuffer, "Error reading maxrestraint for gencoord %s", - gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"begingroups")) - { - if (read_gencoord_groups(ms, fp, gc) != code_fine) - { - gc->numgroups = 0; - sprintf(errorbuffer,"Error reading groups for gencoord %s.", gc->name); - error(recover,errorbuffer); - } - } - else if (STRINGS_ARE_EQUAL(buffer,"visible")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - gc->slider_visible = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - gc->slider_visible = yes; - } -#if INCLUDE_MOCAP_MODULE - else if (STRINGS_ARE_EQUAL(buffer,"mocap_value")) - { - char componentName[32]; - - if (fscanf(fp, "%s %s", buffer, componentName) != 2) - { - sprintf(errorbuffer, "Error reading mocap info for %s gencoord.", gc->name); - error(abort_action, errorbuffer); - return code_bad; - } - mstrcpy(&gc->mocap_segment, buffer); - - gc->mocap_seg_index = -1; - gc->mocap_column = -1; - - switch (tolower(componentName[0])) - { - case 't': - switch (tolower(componentName[1])) - { - case 'x': gc->mocap_column = _TX; break; - case 'y': gc->mocap_column = _TY; break; - case 'z': gc->mocap_column = _TZ; break; - } - break; - - case 'r': - switch (tolower(componentName[1])) - { - case 'x': gc->mocap_column = _RX; break; - case 'y': gc->mocap_column = _RY; break; - case 'z': gc->mocap_column = _RZ; break; - } - break; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"mocap_adjust")) - { - if (fscanf(fp,"%lg", &gc->mocap_adjust) != 1) - { - (void)sprintf(errorbuffer, "Error reading \'mocap_adjust\' for gencoord %s", - gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - } -#endif - else if (STRINGS_ARE_EQUAL(buffer,"pd_stiffness") || STRINGS_ARE_EQUAL(buffer,"PD_stiffness")) - { - if (fscanf(fp,"%lg", &pd_stiffness) != 1) - { - (void)sprintf(errorbuffer, "Error reading pd_stiffness for gencoord %s", gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - gc->pd_stiffness = pd_stiffness; - } - else - { - (void)sprintf(errorbuffer, "Unknown string (%s) in definition of gencoord %s", - buffer, gc->name); - error(recover,errorbuffer); - } - - } - - if (range_specified == no) - { - (void)sprintf(errorbuffer,"Range not specified for gencoord %s", gc->name); - error(abort_action,errorbuffer); - return code_bad; - } - - if (restraintFuncSpecified == no) - gc->restraintFuncActive = no; - - return code_fine; -} - - - -static ReturnCode set_wrap_specs(dpWrapObject* wo, char str[]) -{ - if (STRINGS_ARE_EQUAL(str,"none") || STRINGS_ARE_EQUAL(str,"full")) - { - wo->wrap_axis = 0; - wo->wrap_sign = 0; - return code_fine; - } - if (STRINGS_ARE_EQUAL(str,"-x") || STRINGS_ARE_EQUAL(str,"-X")) - { - wo->wrap_axis = 0; - wo->wrap_sign = -1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"x") || STRINGS_ARE_EQUAL(str,"X")) - { - wo->wrap_axis = 0; - wo->wrap_sign = 1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"-y") || STRINGS_ARE_EQUAL(str,"-Y")) - { - wo->wrap_axis = 1; - wo->wrap_sign = -1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"y") || STRINGS_ARE_EQUAL(str,"Y")) - { - wo->wrap_axis = 1; - wo->wrap_sign = 1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"-z") || STRINGS_ARE_EQUAL(str,"-Z")) - { - wo->wrap_axis = 2; - wo->wrap_sign = -1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"z") || STRINGS_ARE_EQUAL(str,"Z")) - { - wo->wrap_axis = 2; - wo->wrap_sign = 1; - return code_fine; - } - - return code_bad; -} - -static ReturnCode set_constraint_specs(ConstraintObject* co, char str[]) -{ - if (STRINGS_ARE_EQUAL(str,"none") || STRINGS_ARE_EQUAL(str,"full")) - { - co->constraintAxis = 0; - co->constraintSign = 0; - return code_fine; - } - if (STRINGS_ARE_EQUAL(str,"-x") || STRINGS_ARE_EQUAL(str,"-X")) - { - co->constraintAxis = 0; - co->constraintSign = -1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"x") || STRINGS_ARE_EQUAL(str,"X")) - { - co->constraintAxis = 0; - co->constraintSign = 1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"-y") || STRINGS_ARE_EQUAL(str,"-Y")) - { - co->constraintAxis = 1; - co->constraintSign = -1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"y") || STRINGS_ARE_EQUAL(str,"Y")) - { - co->constraintAxis = 1; - co->constraintSign = 1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"-z") || STRINGS_ARE_EQUAL(str,"-Z")) - { - co->constraintAxis = 2; - co->constraintSign = -1; - return code_fine; - } - else if (STRINGS_ARE_EQUAL(str,"z") || STRINGS_ARE_EQUAL(str,"Z")) - { - co->constraintAxis = 2; - co->constraintSign = 1; - return code_fine; - } - - return code_bad; -} - - -static ReturnCode read_wrap_object(ModelStruct* ms, FILE* fp) -{ - ReturnCode rc = code_fine; - dpWrapObject* wo; - double xyz[3]; - DMatrix m; - - if (ms->num_wrap_objects == ms->wrap_object_array_size) - { - ms->wrap_object_array_size += WRAP_OBJECT_ARRAY_INCREMENT; - ms->wrapobj = (dpWrapObject**)simm_realloc(ms->wrapobj, ms->wrap_object_array_size*sizeof(dpWrapObject*), &rc); - - if (rc == code_bad) - { - ms->wrap_object_array_size -= WRAP_OBJECT_ARRAY_INCREMENT; - return code_bad; - } - } - - wo = ms->wrapobj[ms->num_wrap_objects] = (dpWrapObject*)simm_malloc(sizeof(dpWrapObject)); - - initwrapobject(wo); - - if (fscanf(fp, "%s", buffer) != 1) - { - error(abort_action, "Error reading name in wrap object definition"); - return code_bad; - } - else - mstrcpy(&wo->name, buffer); - - while (1) - { - if (read_string(fp, buffer) == EOF) - break; - - if (buffer[0] == '#') - { - read_nonempty_line(fp, buffer); - continue; - } - - if (STRINGS_ARE_EQUAL(buffer, "wraptype")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "sphere")) - wo->wrap_type = dpWrapSphere; - else if (STRINGS_ARE_EQUAL(buffer, "cylinder")) - wo->wrap_type = dpWrapCylinder; - else if (STRINGS_ARE_EQUAL(buffer, "ellipsoid")) - wo->wrap_type = dpWrapEllipsoid; - else if (STRINGS_ARE_EQUAL(buffer, "torus")) - wo->wrap_type = dpWrapTorus; - } - else if (STRINGS_ARE_EQUAL(buffer, "wrapmethod")) - { - int i; - - if (read_string(fp, buffer) == EOF) - break; - - for (i = 0; i < WE_NUM_WRAP_ALGORITHMS; i++) - { - if (STRINGS_ARE_EQUAL(buffer, get_wrap_algorithm_name(i))) - { - wo->wrap_algorithm = i; - break; - } - } - } - else if (STRINGS_ARE_EQUAL(buffer, "segment")) - { - if (fscanf(fp,"%s", buffer) != 1) - { - (void)sprintf(errorbuffer, "Error reading segment in definition of wrap object %s", wo->name); - error(abort_action, errorbuffer); - return code_bad; - } - else - { - wo->segment = enter_segment(ms, buffer, yes); - - if (wo->segment == -1) - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "active")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "false")) - wo->active = no; - } - else if (STRINGS_ARE_EQUAL(buffer, "visible")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "false")) - wo->visible = no; - } - else if (STRINGS_ARE_EQUAL(buffer, "show_wrap_pts")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "true")) - wo->show_wrap_pts = yes; - } - else if (STRINGS_ARE_EQUAL(buffer, "rotation")) - { - sprintf(errorbuffer, "Error reading rotation for wrap object %s:", wo->name); - error(none, errorbuffer); - error(abort_action, " The \'rotation\' keyword has been changed to \'xyz_body_rotation\'."); - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "xyz_body_rotation")) - { - if (fscanf(fp, "%lg %lg %lg", &xyz[0], &xyz[1], &xyz[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading rotation for wrap object %s", wo->name); - error(abort_action, errorbuffer); - return code_bad; - } - identity_matrix(m); - x_rotate_matrix_bodyfixed(m, xyz[0] * DTOR); - y_rotate_matrix_bodyfixed(m, xyz[1] * DTOR); - z_rotate_matrix_bodyfixed(m, xyz[2] * DTOR); - extract_rotation(m, &wo->rotation_axis, &wo->rotation_angle); - } - else if (STRINGS_ARE_EQUAL(buffer, "xyz_space_rotation")) - { - if (fscanf(fp, "%lg %lg %lg", &xyz[0], &xyz[1], &xyz[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading rotation for wrap object %s", wo->name); - error(abort_action,errorbuffer); - return code_bad; - } - identity_matrix(m); - x_rotate_matrix_spacefixed(m, xyz[0] * DTOR); - y_rotate_matrix_spacefixed(m, xyz[1] * DTOR); - z_rotate_matrix_spacefixed(m, xyz[2] * DTOR); - extract_rotation(m, &wo->rotation_axis, &wo->rotation_angle); - } - else if (STRINGS_ARE_EQUAL(buffer, "rotationaxis")) - { - fscanf(fp, "%lg %lg %lg", &wo->rotation_axis.xyz[0], &wo->rotation_axis.xyz[1], &wo->rotation_axis.xyz[2]); - } - else if (STRINGS_ARE_EQUAL(buffer, "rotationangle")) - { - if (fscanf(fp, "%lg", &wo->rotation_angle) != 1) - { - (void)sprintf(errorbuffer, "Error reading rotation for wrap object %s", wo->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "translation") || STRINGS_ARE_EQUAL(buffer, "translate")) - { - if (fscanf(fp,"%lg %lg %lg", &wo->translation.xyz[0], &wo->translation.xyz[1], &wo->translation.xyz[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading translation for wrap object %s", wo->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "radius")) - { - if (wo->wrap_type == dpWrapEllipsoid) - { - rc = ((fscanf(fp, "%lg %lg %lg", &wo->radius[0], &wo->radius[1], &wo->radius[2]) == 3) ? code_fine : code_bad); - if (wo->radius[0] <= 0.0 || wo->radius[1] <= 0.0 || wo->radius[2] <= 0.0) - { - error(none, "Value must be greater than zero."); - rc = code_bad; - } - } - else if (wo->wrap_type == dpWrapTorus) - { - rc = ((fscanf(fp, "%lg %lg", &wo->radius[0], &wo->radius[1]) == 2) ? code_fine : code_bad); - if (wo->radius[0] <= 0.0 || wo->radius[1] <= 0.0) - { - error(none, "Value must be greater than zero."); - rc = code_bad; - } - } - else - { - rc = ((fscanf(fp, "%lg", &wo->radius[0]) == 1) ? code_fine : code_bad); - if (wo->radius[0] <= 0.0) - { - error(none, "Value must be greater than zero."); - rc = code_bad; - } - } - - if (rc == code_bad) - { - (void)sprintf(errorbuffer, "Error reading radius for wrap object %s", wo->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "height")) - { - int num = fscanf(fp, "%lg", &wo->height); - - if (num == 1 && wo->height <= 0.0) - { - error(none, "Value must be greater than zero."); - num = -1; - } - - if (num != 1) - { - (void)sprintf(errorbuffer, "Error reading cylinder height for wrap object %s", wo->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "quadrant")) - { - if (read_string(fp, buffer) == EOF) - break; - - set_wrap_specs(wo, buffer); - } - else if (STRINGS_ARE_EQUAL(buffer, "endwrapobject")) - break; - } - wo->undeformed_translation = wo->translation; - - wo->xforms_valid = no; - - ms->num_wrap_objects++; - - return code_fine; -} - - -static ReturnCode read_restraint(ModelStruct* ms, FILE* fp, GeneralizedCoord* gc, int minmax) -{ - int user_num; - - if (read_string(fp, buffer) == EOF) - return code_bad; - - // The string just read should be of the form "f", such as "f32". - - if (buffer[0] != 'f') - return code_bad; - - if (minmax == 2) - { - if (sscanf(&buffer[1], "%d", &user_num) != 1) - return code_bad; - gc->restraint_function = ms->function[enter_function(ms, user_num, yes)]; - } - else if (minmax == 0) - { - if (sscanf(&buffer[1], "%d", &user_num) != 1) - return code_bad; - gc->min_restraint_function = ms->function[enter_function(ms, user_num, yes)]; - } - else - { - if (sscanf(&buffer[1], "%d", &user_num) != 1) - return code_bad; - gc->max_restraint_function = ms->function[enter_function(ms, user_num, yes)]; - } - - return code_fine; -} - - -/* READ_JOINT: */ - -static ReturnCode read_joint(ModelStruct* ms, FILE* fp) -{ - int i, jointnum; - char from[CHARBUFFER], to[CHARBUFFER], jointname[CHARBUFFER]; - char check = 0; - int jointvarnum; - JointStruct* jnt; - - if (fscanf(fp, "%s", jointname) != 1) - { - error(abort_action,"Error reading name in joint definition"); - return code_bad; - } - - jointnum = enter_joint(ms, jointname, yes); - if (jointnum == -1) - return code_bad; - - jnt = &ms->joint[jointnum]; - - if (jnt->defined == yes) - { - (void)sprintf(errorbuffer,"Error: redefinition of joint %s", jointname); - error(abort_action,errorbuffer); - return code_bad; - } - - while (1) - { - if (read_string(fp,buffer) == EOF) - { - (void)sprintf(errorbuffer,"Unexpected EOF found reading joint definition"); - error(abort_action,errorbuffer); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer,"endjoint")) - { - jnt->defined = yes; - break; - } - - if (STRINGS_ARE_EQUAL(buffer,"loopjoint")) - { - /* IK solver, Nov. 2000: - * the loopjoint keyword in the joint file no longer sets the loop joint, - * since the Inverse Kinematics routines take care to keep the loop unbroken. - * You no longer have to specify the joint at which the display gets broken. - */ - jnt->user_loop = yes; - continue; - } - - if (STRINGS_ARE_EQUAL(buffer,"show")) - { - if (read_show_axis(ms,fp,jnt) == code_bad) - return code_bad; - continue; - } - if (STRINGS_ARE_EQUAL(buffer,"type")) - { - if (fscanf(fp,"%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading type for joint %s", jnt->name); - error(abort_action,errorbuffer); - return code_bad; - } - mstrcpy(&jnt->solverType, buffer); - continue; - } - if (STRINGS_ARE_EQUAL(buffer,"pretransform")) - { - /* NOTE: typically the joint pretransform is computed dynamically to - * support deformations (and possibly other effects), and is not - * specified in a joint file. However I left this code here because - * it is useful for testing the joint pretransform mechanism. -- KMS - */ - int i,j; - - for (i=0; i<4; i++) - { - for (j=0; j<4; j++) - { - if (fscanf(fp,"%lg", &jnt->pretransform[i][j]) != 1) - { - sprintf(errorbuffer, "Error reading pretransform for joint %s", jnt->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - } - invert_4x4transform(jnt->pretransform, jnt->pretransform_inverse); - continue; - } - -#if INCLUDE_MOCAP_MODULE - if (STRINGS_ARE_EQUAL(buffer,"mocap_segment")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading \'mocap_segment\' for joint %s.", jnt->name); - error(abort_action, errorbuffer); - return code_bad; - } - mstrcpy(&jnt->mocap_segment, buffer); - continue; - } -#endif - - if ((jointvarnum = getjointvarnum(buffer)) == -1) - { - (void)sprintf(errorbuffer, - "Unknown string (%s) in definition of joint %s", - buffer, jnt->name); - error(recover,errorbuffer); - continue; - } - - if (jointvarnum >= 0 && jointvarnum < 6) - { - if (read_refeq(ms,fp,&jnt->dofs[jointvarnum]) == code_bad) - { - (void)sprintf(errorbuffer,"Error reading definition of %s in joint %s", - getjointvarname(jointvarnum), jnt->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - check |= 1 << jointvarnum; - } - else if (jointvarnum == 6) - { - if (read_order(fp,jnt->order) == code_bad) - { - (void)sprintf(errorbuffer, - "Error reading order of transformations in joint %s", jnt->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (jointvarnum > 6 && jointvarnum < 10) - { - if (read_axis(ms,fp,jointnum,jointvarnum-7) == code_bad) - return code_bad; - } - else if (jointvarnum == 10) - { - if (fscanf(fp,"%s %s", from, to) != 2) - { - (void)sprintf(errorbuffer,"Error reading segments in definition of joint %s", jnt->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - jnt->from = enter_segment(ms,from,yes); - jnt->to = enter_segment(ms,to,yes); - if (jnt->from == -1 || jnt->to == -1) - return code_bad; - } - } - } - - /* Each time a dof is properly defined, the corresponding bit in the 'check' - * variable is set to 1. Thus when we see 'endjoint,' we can examine this - * variable to see if the lower 6 bits are all 1s (which means check = 63). - * If check != 63, then not all 6 of the dofs have been properly defined, - * so print an error message. - */ - - if (check < 63) - { - (void)sprintf(errorbuffer, "Incomplete definition of joint %s:", jnt->name); - error(none,errorbuffer); - for (i=0; i<6; i++) - { - if (!(check & (1<segment[segmentnum]; - - if (seg->defined == yes) - { - (void)sprintf(errorbuffer,"Error: redefinition of segment %s", segmentname); - error(abort_action,errorbuffer); - return code_bad; - } - - while (1) - { - if (read_string(fp,buffer) == EOF) - { - (void)sprintf(errorbuffer, "Unexpected EOF found reading segment definition"); - error(abort_action,errorbuffer); - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"endsegment")) - { - seg->defined = yes; - break; - } - else if (STRINGS_ARE_EQUAL(buffer,"beginfiles")) - { - while (1) - { - if (read_string(fp,buffer) == EOF) - { - (void)sprintf(errorbuffer, "Error reading bone files for %s segment.", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"endfiles")) - { - break; - } - else - { - PolyhedronStruct* ph; - - strcpy(name,buffer); - ph = make_bone(ms,seg,name); - if (ph != NULL) - { - ph->drawmode = -1; - ph->material = -1; - } - } - } - } - else if (STRINGS_ARE_EQUAL(buffer,"bone")) - { - PolyhedronStruct* ph; - - fscanf(fp,"%s", name); - ph = make_bone(ms,seg,name); - read_line(fp,buffer); - if (ph != NULL) - { - int m, ns; - ph->material = -1; - ph->drawmode = -1; - ns = sscanf(buffer,"%s %s", buf[0], buf[1]); - for (ns--; ns >= 0; ns--) - { - m = get_drawmode(buf[ns]); - if (m != -1) - { - ph->drawmode = m; - } - else - { - m = enter_material(ms, buf[ns], declaring_element); - if (m != -1) - { - ph->material = m; - } - else - { - sprintf(errorbuffer,"Error reading material and drawmode for bone %s.", ph->name); - error(recover,errorbuffer); - } - } - } - } - } - else if (STRINGS_ARE_EQUAL(buffer,"begingroups")) - { - if (read_segment_groups(ms, fp, segmentnum) != code_fine) - { - seg->numgroups = 0; - - sprintf(errorbuffer,"Error reading groups for segment %s.", seg->name); - error(recover,errorbuffer); - } - } -#if INCLUDE_BONE_EDITOR_EXTRAS - else if (STRINGS_ARE_EQUAL(buffer,"pts_file")) - { - FILE* fp_pts; - - fscanf(fp,"%s", buffer); - mstrcpy(&seg->pts_file,buffer); - fp_pts = simm_fopen(seg->pts_file,"r"); - if (fp_pts != NULL) - { - fscanf(fp_pts,"%d", &seg->num_raw); - seg->raw_vertices = (double*)simm_malloc(seg->num_raw*3*sizeof(double)); - for (i=0; inum_raw; i++) - fscanf(fp_pts,"%*s %lf %lf %lf", &seg->raw_vertices[i*3], - &seg->raw_vertices[i*3+1], &seg->raw_vertices[i*3+2]); - fclose(fp_pts); - } - else - { - sprintf(errorbuffer,"Cannot open %s", buffer); - error(none,errorbuffer); - } - } -#endif - else if (STRINGS_ARE_EQUAL(buffer,"force_matte")) - { - if (read_force_matte(ms, seg, fp) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"marker")) - { - if (read_marker(ms, seg, fp) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"shadow")) - { - if (fscanf(fp,"%s %lg", buffer, &shadow_trans) != 2) - { - (void)sprintf(errorbuffer,"Error reading shadow axis and/or shadow translation for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - if (STRINGS_ARE_EQUAL(buffer,"x") || STRINGS_ARE_EQUAL(buffer,"X")) - shadow_axis = 0; - else if (STRINGS_ARE_EQUAL(buffer,"y") || STRINGS_ARE_EQUAL(buffer,"Y")) - shadow_axis = 1; - else if (STRINGS_ARE_EQUAL(buffer,"z") || STRINGS_ARE_EQUAL(buffer,"Z")) - shadow_axis = 2; - else - { - (void)sprintf(errorbuffer,"Bad axis specified in shadow for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - seg->shadow_scale[shadow_axis] = 0.0; - seg->shadow_trans[shadow_axis] = shadow_trans; - seg->shadow = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"shadowcolor")) - { - if (fscanf(fp,"%f %f %f", &seg->shadow_color.rgb[RD], - &seg->shadow_color.rgb[GR], &seg->shadow_color.rgb[BL]) != 3) - { - (void)sprintf(errorbuffer,"Error reading shadow color for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - /* Support for 'old style' colors (range: 0 to 255). If any of the - * color components is greater than 1.0, assume that it's an old - * style color definition. - */ - if (seg->shadow_color.rgb[RD] > 1.0 || - seg->shadow_color.rgb[GR] > 1.0 || - seg->shadow_color.rgb[BL] > 1.0) - { - seg->shadow_color.rgb[RD] /= 255.0; - seg->shadow_color.rgb[GR] /= 255.0; - seg->shadow_color.rgb[BL] /= 255.0; - } - for (i=0; i<3; i++) - { - if (seg->shadow_color.rgb[i] < 0.0) - seg->shadow_color.rgb[i] = 0.0; - } - seg->shadow_color_spec = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"material")) - { - if (fscanf(fp,"%s", name) != 1) - { - (void)sprintf(errorbuffer,"Error reading material for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - seg->material = enter_material(ms,name,declaring_element); - } - } - else if (STRINGS_ARE_EQUAL(buffer,"scale")) - { - if (fscanf(fp,"%f %f %f", &seg->bone_scale[0], &seg->bone_scale[1], - &seg->bone_scale[2]) != 3) - { - (void)sprintf(errorbuffer,"Error reading scale factors for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"mass")) - { - if (fscanf(fp,"%lg", &seg->mass) != 1) - { - (void)sprintf(errorbuffer,"Error reading mass for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - seg->mass_specified = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"inertia")) - { - for (i=0; i<9; i++) - { - if (fscanf(fp,"%lg", &seg->inertia[i/3][i%3]) != 1) - { - (void)sprintf(errorbuffer,"Error reading inertia for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - seg->inertia_specified = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"masscenter")) - { - for (i=0; i<3; i++) - { - if (fscanf(fp,"%lg", &seg->masscenter[i]) != 1) - { - (void)sprintf(errorbuffer,"Error reading masscenter for %s segment", - seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - seg->masscenter_specified = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"show_masscenter")) - { - if (fscanf(fp, "%s", buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"off") - || STRINGS_ARE_EQUAL(buffer, "false")) - seg->show_masscenter = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - seg->show_masscenter = yes; - } - else if (STRINGS_ARE_EQUAL(buffer, "show_inertia")) - { - if (fscanf(fp, "%s", buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "off") - || STRINGS_ARE_EQUAL(buffer, "false")) - seg->show_inertia = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "on") - ||STRINGS_ARE_EQUAL(buffer, "true")) - seg->show_inertia = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"drawmode")) - { - if (read_drawmode(fp, &seg->drawmode) != code_fine) - { - sprintf(errorbuffer, "Error reading drawmode for %s segment", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"axes")) - { - if (fscanf(fp, "%lg", &seg->axis_length) != 1) - { - (void)sprintf(errorbuffer, "Error reading axis length for %s segment", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - else - seg->draw_axes = yes; - } - else if (STRINGS_ARE_EQUAL(buffer, "axes_length")) - { - if (fscanf(fp,"%lg", &seg->axis_length) != 1) - { - (void)sprintf(errorbuffer,"Error reading axis length for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"show_axes")) - { - seg->draw_axes = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"begindeform") || - STRINGS_ARE_EQUAL(buffer,"deform")) - { - if (read_deform(fp, seg, segmentnum) != code_fine) - return code_bad; - } -#if INCLUDE_MOCAP_MODULE - else if (STRINGS_ARE_EQUAL(buffer,"lengthstart")) - { - if (fscanf(fp,"%f %f %f", &seg->lengthstart[0], &seg->lengthstart[1], - &seg->lengthstart[2]) != 3) - { - (void)sprintf(errorbuffer,"Error reading lengthstart for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - seg->lengthstartend_specified = yes; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"lengthend")) - { - if (fscanf(fp,"%f %f %f", &seg->lengthend[0], &seg->lengthend[1], - &seg->lengthend[2]) != 3) - { - (void)sprintf(errorbuffer,"Error reading lengthend for %s segment", seg->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - seg->lengthstartend_specified = yes; - } - } -#endif - else if (STRINGS_ARE_EQUAL(buffer, "htr_o")) - { - if (fscanf(fp, "%lg %lg %lg", &seg->htr_o[0], &seg->htr_o[1], &seg->htr_o[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading htr_o for %s segment", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "htr_x")) - { - if (fscanf(fp, "%lg %lg %lg", &seg->htr_x[0], &seg->htr_x[1], &seg->htr_x[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading htr_x for %s segment", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "htr_y")) - { - if (fscanf(fp, "%lg %lg %lg", &seg->htr_y[0], &seg->htr_y[1], &seg->htr_y[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading htr_y for %s segment", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - } -#if INCLUDE_MOCAP_MODULE - else if (STRINGS_ARE_EQUAL(buffer,"gait_scale")) - { - if (fscanf(fp, "%s %lf %lf %lf", buffer, &seg->gait_scale_factor[0], - &seg->gait_scale_factor[1], &seg->gait_scale_factor[2]) != 4) - { - sprintf(errorbuffer, "Error reading \'gait_scale\' for %s segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - mstrcpy(&seg->gait_scale_segment, buffer); - } - else if (STRINGS_ARE_EQUAL(buffer,"mocap_segment")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading \'mocap_segment\' for %s segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - mstrcpy(&seg->mocap_segment, buffer); - - if (fscanf(fp, "%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading \'mocap_segment\' for %s segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer, "inherit_scale")) - seg->mocap_scaling_method = INHERIT_SCALE; - else if (STRINGS_ARE_EQUAL(buffer, "scale_one_to_one")) - seg->mocap_scaling_method = SCALE_ONE_TO_ONE; - else if (STRINGS_ARE_EQUAL(buffer, "scale_chain")) - { - seg->mocap_scaling_method = SCALE_CHAIN; - - /* read the first mocap scale chain end name: - */ - if (fscanf(fp, "%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading \'mocap_segment\' for %s segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - mstrcpy(&seg->mocap_scale_chain_end1, buffer); - - /* read the second mocap scale chain end name: - */ - if (fscanf(fp, "%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading \'mocap_segment\' for %s segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - mstrcpy(&seg->mocap_scale_chain_end2, buffer); - } - } - else if (STRINGS_ARE_EQUAL(buffer,"mocap_scale_to_segment")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading \'mocap_scale_to_segment\' for %s segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - mstrcpy(&seg->mocap_scale_chain_end1, buffer); - } - else if (STRINGS_ARE_EQUAL(buffer,"mocap_adjustment_xform")) - { - double axis[3], angle; - - if (fscanf(fp, "%lg %lg %lg %lg", &axis[XX], &axis[YY], &axis[ZZ], &angle) != 4) - { - sprintf(errorbuffer, "Error reading mocap adjustment for %s segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - - fprintf(stderr, "MOCAP ADJUST: %s [%.3f %.3f %.3f] %.1f\n", - seg->name, axis[XX], axis[YY], axis[ZZ], angle); - - angle *= DTOR; - - rotate_matrix_axis_angle(seg->mocap_adjustment_xform, axis, angle); - } -#endif - else - { - (void)sprintf(errorbuffer, "Unknown string (%s) in definition of segment %s", - buffer, seg->name); - error(recover,errorbuffer); - } - } - - for (i=0; inumBones; i++) - { - if (seg->bone[i].material == -1) - seg->bone[i].material = seg->material; - if (seg->bone[i].drawmode == -1) - seg->bone[i].drawmode = seg->drawmode; - } - - return code_fine; -} - - - -/* READ_AXIS: */ - -static ReturnCode read_axis(ModelStruct* ms, FILE* fp, int joint, int axis) -{ - int i; - double total; - - for (i=0; i<3; i++) - { - if (fscanf(fp, "%lg", &ms->joint[joint].parentrotaxes[axis][i]) != 1) - { - (void)sprintf(errorbuffer, "Error reading definition of axis%d in joint %s", axis, ms->joint[joint].name); - error(abort_action, errorbuffer); - return code_bad; - } - } - - ms->joint[joint].parentrotaxes[axis][3] = 0.0; - - total = sqrt(ms->joint[joint].parentrotaxes[axis][0] * ms->joint[joint].parentrotaxes[axis][0] + - ms->joint[joint].parentrotaxes[axis][1] * ms->joint[joint].parentrotaxes[axis][1] + - ms->joint[joint].parentrotaxes[axis][2] * ms->joint[joint].parentrotaxes[axis][2]); - - if (EQUAL_WITHIN_ERROR(total, 0.0)) - { - (void)sprintf(errorbuffer, "Error reading axis%d in joint %s (vector has zero length)", axis+1, ms->joint[joint].name); - error(abort_action, errorbuffer); - return code_bad; - } - - for (i=0; i<3; i++) - ms->joint[joint].parentrotaxes[axis][i] /= total; - - return code_fine; -} - - - -/* READ_REFEQ: */ - -static ReturnCode read_refeq(ModelStruct* ms, FILE* fp, DofStruct* jointvar) -{ - int j, k, n; - int userfuncnum; - char usergencname[CHARBUFFER]; - - read_string(fp,buffer); - - if (STRINGS_ARE_EQUAL(buffer,"constant")) - { - jointvar->type = constant_dof; - jointvar->function = NULL; - jointvar->gencoord = NULL; - if (fscanf(fp,"%lg", &jointvar->value) != 1) - return code_bad; - else - return code_fine; - } - - if (STRINGS_ARE_EQUAL(buffer,"function")) - { - jointvar->type = function_dof; - j = 0; - while (1) - { - if (j == CHARBUFFER-1) - return code_bad; - buffer[j] = getc(fp); - if (buffer[j] == (char) EOF) - return code_bad; - if (buffer[j] == ')') - break; - j++; - } - buffer[j+1] = STRING_TERMINATOR; - j = 0; - while (buffer[j++] != 'f') - ; - k = sscanf(&buffer[j],"%d", &userfuncnum); - while (buffer[j++] != '(') /* remove the open paren */ - ; - n = j + strlen(&buffer[j]) - 1; /* the next four lines remove the */ - while (buffer[n] != ')') /* close paren at the end */ - n--; - buffer[n] = STRING_TERMINATOR; - k += sscanf(&buffer[j],"%s", usergencname); /* now read the gencoord name */ - if (k != 2) - return code_bad; - else - { - jointvar->function = ms->function[enter_function(ms, userfuncnum, yes)]; - jointvar->gencoord = enter_gencoord(ms, usergencname, yes); - if (jointvar->gencoord == NULL) - return code_bad; - return code_fine; - } - } - - return code_bad; -} - - -static ReturnCode read_order(FILE* fp, int order[]) -{ - int i; - char buf[4][CHARBUFFER]; - - if (fscanf(fp,"%s %s %s %s", buf[0],buf[1],buf[2],buf[3]) != 4) - return code_bad; - - /* order[] holds the position of each matrix in the multiplication order. - * Order[0] contains the order of the translation matrix, order[1] contains - * the order of the axis1--rotation matrix, etc. - */ - for (i=0; i<4; i++) - { - if (STRINGS_ARE_EQUAL(buf[i],"t")) - order[0] = i; - else if (STRINGS_ARE_EQUAL(buf[i],"r1")) - order[1] = i; - else if (STRINGS_ARE_EQUAL(buf[i],"r2")) - order[2] = i; - else if (STRINGS_ARE_EQUAL(buf[i],"r3")) - order[3] = i; - } - - return code_fine; -} - - -static ReturnCode read_show_axis(ModelStruct* ms, FILE* fp, JointStruct* jnt) -{ - double length; - - if (fscanf(fp,"%s %lg", buffer, &length) != 2) - { - sprintf(errorbuffer,"Error reading axis name and/or length after \"show\" keyword in joint %s", jnt->name); - error(abort_action,errorbuffer); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer,"axis1")) - { - jnt->axis_length[0] = length; - jnt->show_axes[0] = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"axis2")) - { - jnt->axis_length[1] = length; - jnt->show_axes[1] = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"axis3")) - { - jnt->axis_length[2] = length; - jnt->show_axes[2] = yes; - } - - return code_fine; -} - - -static ReturnCode read_modelview(Scene* scene, ModelStruct* ms, FILE* fp) -{ - int i, j; - - if (ms->dis.num_file_views >= MAXSAVEDVIEWS) - { - sprintf(errorbuffer,"Only %d cameras are allowed in the joints file", - MAXSAVEDVIEWS); - error(recover,errorbuffer); - return code_bad; - } - - if (fscanf(fp,"%s", buffer) != 1) - { - error(abort_action,"Error reading name of camera"); - return code_bad; - } - - mstrcpy(&ms->dis.view_name[ms->dis.num_file_views], buffer); - - for (i=0; i<4; i++) - { - for (j=0; j<4; j++) - { - if (fscanf(fp,"%lg", &ms->dis.saved_view[ms->dis.num_file_views][i][j]) != 1) - { - sprintf(errorbuffer,"Error reading matrix for camera %s", - ms->dis.view_name[ms->dis.num_file_views]); - error(abort_action,errorbuffer); - return code_bad; - } - } - } - - if (fscanf(fp,"%s", buffer) != 1) - { - error(abort_action,"Error reading camera"); - return code_bad; - } - else if (STRINGS_ARE_NOT_EQUAL(buffer,"endcamera") && STRINGS_ARE_NOT_EQUAL(buffer,"endview")) - { - error(abort_action,"Error reading camera"); - return code_bad; - } - - /* If the camera is named "default," make it the default camera, - * which is the one that the model is set to when loaded. - */ - if (STRINGS_ARE_EQUAL(ms->dis.view_name[ms->dis.num_file_views], "default")) - ms->dis.default_view = ms->dis.num_file_views; - - ms->dis.view_used[ms->dis.num_file_views] = yes; - ms->dis.num_file_views++; - - return code_fine; -} - - -static ReturnCode read_gencoord_groups (ModelStruct* ms, FILE* fp, GeneralizedCoord* gencoord) -{ - int num_malloced_so_far = 10; - - ReturnCode rc = code_fine; - - gencoord->numgroups = 0; - - gencoord->group = (int*)simm_malloc(num_malloced_so_far * sizeof(int)); - - if (gencoord->group == NULL) - return code_bad; - - while (1) - { - if (fscanf(fp, "%s", buffer) != 1) - return code_bad; - - if (STRINGS_ARE_EQUAL(buffer, "endgroups")) - { - gencoord->group = (int*)simm_realloc(gencoord->group, gencoord->numgroups * sizeof(int), &rc); - break; - } - - if (gencoord->numgroups > num_malloced_so_far) - { - num_malloced_so_far += 10; - - gencoord->group = (int*)simm_realloc(gencoord->group, num_malloced_so_far * sizeof(int), &rc); - - if (rc == code_bad) - break; - } - - gencoord->group[gencoord->numgroups] = enter_gencoord_group(ms, buffer, gencoord); - - if (gencoord->group[gencoord->numgroups] == -1) - return code_bad; - - gencoord->numgroups++; - } - return rc; -} - - -static ReturnCode read_segment_groups (ModelStruct* ms, FILE* fp, int segnum) -{ - int num_malloced_so_far = 10; - SegmentStruct* seg = &ms->segment[segnum]; - ReturnCode rc = code_fine; - - seg->numgroups = 0; - - seg->group = (int*) simm_malloc(num_malloced_so_far * sizeof(int)); - - if (seg->group == NULL) - return code_bad; - - while (1) - { - if (fscanf(fp, "%s", buffer) != 1) - return code_bad; - - if (STRINGS_ARE_EQUAL(buffer,"endgroups")) - { - seg->group = (int*) simm_realloc(seg->group, seg->numgroups * sizeof(int), &rc); - - break; - } - - if (seg->numgroups > num_malloced_so_far) - { - num_malloced_so_far += 10; - - seg->group = (int*) simm_realloc(seg->group, num_malloced_so_far * sizeof(int), &rc); - - if (rc == code_bad) - break; - } - - seg->group[seg->numgroups] = enter_segment_group(ms, buffer, segnum); - - if (seg->group[seg->numgroups] == -1) - return code_bad; - - seg->numgroups++; - } - return rc; -} - - -ReturnCode read_drawmode (FILE* fp, DrawingMode* dm) -{ - if (fscanf(fp,"%s", buffer) != 1) - return code_bad; - - *dm = get_drawmode(buffer); - - if (*dm == -1) - *dm = gouraud_shading; - - return code_fine; -} - - -static DrawingMode get_drawmode(char buffer[]) -{ - if (STRINGS_ARE_EQUAL(buffer,"wireframe")) - return wireframe; - else if (STRINGS_ARE_EQUAL(buffer,"solid_fill")) - return solid_fill; - else if (STRINGS_ARE_EQUAL(buffer,"flat_shading")) - return flat_shading; - else if (STRINGS_ARE_EQUAL(buffer,"gouraud_shading")) - return gouraud_shading; - else if (STRINGS_ARE_EQUAL(buffer,"outlined_polygons") || - STRINGS_ARE_EQUAL(buffer,"outlined")) - return outlined_polygons; - else if (STRINGS_ARE_EQUAL(buffer,"bounding_box")) - return bounding_box; - else if (STRINGS_ARE_EQUAL(buffer,"none")) - return no_surface; - else - return -1; -} - - -static ReturnCode read_contact(ModelStruct* ms, FILE* fp) -{ - - while (1) - { - if (read_string(fp,buffer) == EOF) - { - (void)sprintf(errorbuffer,"Unexpected EOF found reading contact parameters"); - error(abort_action,errorbuffer); - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"endcontact")) - { - return code_fine; - } - else if (STRINGS_ARE_EQUAL(buffer,"spring")) - { - if (read_spring(ms, fp) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"spring_floor")) - { - if (read_spring_floor(ms, fp) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"object")) - { - if (read_contact_object(ms, fp) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"contact_pair")) - { - if (read_contact_pair(ms, fp) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"begin_group")) - { - if (read_contact_group(ms, fp) == code_bad) - return code_bad; - } - } - return code_fine; -} - - -static ReturnCode read_spring(ModelStruct* ms, FILE* fp) -{ - int i, segnum; - char floorName[CHARBUFFER]; - SpringPoint* sp; - SegmentStruct* seg; - - if (fscanf(fp,"%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading segment name for spring"); - error(abort_action,errorbuffer); - return code_bad; - } - segnum = enter_segment(ms,buffer,yes); - seg = &ms->segment[segnum]; - - if (seg->numSpringPoints >= seg->springPointArraySize) - { - ReturnCode rc = code_fine; - - seg->springPointArraySize += SEGMENT_ARRAY_INCREMENT; - if (seg->springPoint == NULL) - { - seg->springPoint = (SpringPoint*) simm_malloc( - seg->springPointArraySize * sizeof(SpringPoint)); - } - else - { - seg->springPoint = (SpringPoint*) simm_realloc(seg->springPoint, - seg->springPointArraySize * sizeof(SpringPoint), &rc); - } - if (rc == code_bad || seg->springPoint == NULL) - { - seg->springPointArraySize -= SEGMENT_ARRAY_INCREMENT; - return code_bad; - } - } - - sp = &seg->springPoint[seg->numSpringPoints]; - - if (fscanf(fp,"%lf %lf %lf %s %lf %lf %lf %lf %lf %lf %lf", &sp->point[0], &sp->point[1], &sp->point[2], - floorName, &sp->friction, &sp->param_a, &sp->param_b, &sp->param_c, - &sp->param_d, &sp->param_e, &sp->param_f) != 11) - { - sprintf(errorbuffer, "Error reading spring point definition in segment %s", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - else - { - sp->name = NULL; - sp->visible = yes; - sp->segment = segnum; - - // Find the corresponding floor object and add the spring to it. - for (i=0; inumsegments; i++) - { - if (ms->segment[i].springFloor && STRINGS_ARE_EQUAL(floorName, ms->segment[i].springFloor->name)) - break; - } - if (i == ms->numsegments) - { - sprintf(errorbuffer, "Unknown spring floor (%s) referenced by spring point (was point defined before floor?)", floorName); - error(abort_action, errorbuffer); - return code_bad; - } - else - { - sp->floorSegment = i; - add_spring_point_to_floor(seg, sp); - seg->numSpringPoints++; - } - } - - return code_fine; -} - - -static ReturnCode read_spring_floor(ModelStruct* ms, FILE* fp) -{ - int segnum, count; - SegmentStruct* seg; - ReturnCode frc; - char str1[CHARBUFFER], str2[CHARBUFFER], str3[CHARBUFFER]; - - count = fscanf(fp, "%s", str1); - count += _read_til_tokens(fp, str2, "\t\r\n"); - _strip_outer_whitespace(str2); - read_line(fp, buffer); - count += sscanf(buffer, "%s", str3); - - segnum = enter_segment(ms, str3, yes); - if (segnum < 0) - { - sprintf(errorbuffer, "Error reading segment name for spring floor"); - error(abort_action,errorbuffer); - return code_bad; - } - - seg = &ms->segment[segnum]; - - if (seg->springFloor) - { - (void)sprintf(errorbuffer,"Second spring_floor found in segment %s. Only one is allowed per segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - - if ((seg->springFloor = (SpringFloor *)simm_malloc(sizeof(SpringFloor))) == NULL) - { - error(abort_action,"Ran out of memory. Unable to create spring floor."); - return code_bad; - } - else - { - mstrcpy(&seg->springFloor->name, str1); - mstrcpy(&seg->springFloor->filename, str2); - seg->springFloor->visible = yes; - seg->springFloor->points = NULL; - seg->springFloor->numPoints = 0; - seg->springFloor->pointArraySize = 0; -#if ! ENGINE - seg->springFloor->poly = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - frc = lookup_polyhedron(seg->springFloor->poly, seg->springFloor->filename, ms); - if (frc == file_missing) - { - (void)sprintf(errorbuffer,"Unable to locate spring_floor file %s", seg->springFloor->filename); - error(none,errorbuffer); - FREE_IFNOTNULL(seg->springFloor->poly); - } - else if (frc == file_bad) - { - (void)sprintf(errorbuffer,"Unable to read spring_floor from file %s", seg->springFloor->filename); - error(none,errorbuffer); - FREE_IFNOTNULL(seg->springFloor->poly); - } -#endif - } - - return code_fine; -} - -static void add_spring_point_to_floor(SegmentStruct* segment, SpringPoint* point) -{ - if (segment && segment->springFloor) - { - SpringFloor* floor = segment->springFloor; - - // Make the list longer if necessary. - if (floor->numPoints >= floor->pointArraySize) - { - ReturnCode rc = code_fine; - - floor->pointArraySize += SEGMENT_ARRAY_INCREMENT; - if (floor->points == NULL) - floor->points = (SpringPoint**)simm_malloc(floor->pointArraySize * sizeof(SpringPoint*)); - else - floor->points = (SpringPoint**)simm_realloc(floor->points, floor->pointArraySize * sizeof(SpringPoint*), &rc); - if (rc == code_bad || floor->points == NULL) - { - floor->pointArraySize -= SEGMENT_ARRAY_INCREMENT; - return; - } - } - - // Add the point to the list. - sprintf(buffer, "%s_pt%d", segment->springFloor->name, floor->numPoints+1); - if (!point->name) - mstrcpy(&point->name, buffer); - floor->points[floor->numPoints++] = point; - } -} - - -static ReturnCode read_force_matte(ModelStruct* ms, SegmentStruct* seg, FILE* fp) -{ - int segnum; - FileReturnCode frc; - char str1[CHARBUFFER], str2[CHARBUFFER]; - int count; - char bufs[3][CHARBUFFER]; - - if (seg->forceMatte) - { - (void)sprintf(errorbuffer, "Second force_matte found in segment %s. Only one is allowed per segment.", seg->name); - error(abort_action, errorbuffer); - return code_bad; - } - - count = fscanf(fp, "%s", str1); - count += _read_til_tokens(fp, str2, "\t\r\n"); - _strip_outer_whitespace(str2); - /* If both the name and filename were specified, look for the visible/invisible keyword. */ - if (count == 2) - { - read_line(fp, buffer); - if (sscanf(buffer, "%s", bufs[0]) > 0) - count++; - } - - if (count < 2) - { - if (count == 1) - { - sprintf(errorbuffer, "Filename undefined for force_matte %s in segment %s.", str1, seg->name); - } - error(abort_action,errorbuffer); - return code_bad; - } - else - { - if ((seg->forceMatte = (ContactObject*)simm_malloc(sizeof(ContactObject))) == NULL) - { - error(abort_action,"Ran out of memory. Unable to create force matte."); - return code_bad; - } - else - { - seg->forceMatte->visible = yes; - mstrcpy(&seg->forceMatte->name, str1); - mstrcpy(&seg->forceMatte->filename, str2); - -#if ! ENGINE || OPENSMAC - seg->forceMatte->poly = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - frc = lookup_polyhedron(seg->forceMatte->poly, seg->forceMatte->filename, ms); - if (frc == file_missing) - { - (void)sprintf(errorbuffer,"Unable to locate force_matte file %s", seg->forceMatte->filename); - error(none,errorbuffer); - FREE_IFNOTNULL(seg->forceMatte->poly); - } - else if (frc == file_bad) - { - (void)sprintf(errorbuffer,"Unable to read force_matte from file %s", seg->forceMatte->filename); - error(none,errorbuffer); - FREE_IFNOTNULL(seg->forceMatte->poly); - } -#endif - } - /* If visible/invisible keyword specified ... */ - if (count == 3) - { - if (STRINGS_ARE_EQUAL(bufs[0], "invisible") || STRINGS_ARE_EQUAL(bufs[0], "INVISIBLE") || - STRINGS_ARE_EQUAL(bufs[0], "hidden") || STRINGS_ARE_EQUAL(bufs[0], "HIDDEN")) - seg->forceMatte->visible = no; - if (STRINGS_ARE_EQUAL(bufs[0], "visible") || STRINGS_ARE_EQUAL(bufs[0], "VISIBLE")) - seg->forceMatte->visible = yes; - } - } - - return code_fine; -} - - -static ReturnCode read_contact_object(ModelStruct* ms, FILE* fp) -{ - int segnum, count; - ContactObject* co; - SegmentStruct* seg; - char segname[CHARBUFFER], co_name[CHARBUFFER], co_file[CHARBUFFER]; - char bufs[3][CHARBUFFER]; - ReturnCode rc; - FileReturnCode frc; - - count = fscanf(fp, "%s", co_name); - count += _read_til_tokens(fp, co_file, "\t\r\n"); - _strip_outer_whitespace(co_file); - read_line(fp, buffer); - count += sscanf(buffer, "%s %s", segname, bufs[0]); - - if (count < 3) - { - if (count == 1) - sprintf(errorbuffer, "No filename or segment defined for contact object %s.", co_name); - else if (count == 2) - sprintf(errorbuffer, "No segment defined for contact object %s.", co_name); - else - sprintf(errorbuffer, "Contact object undefined."); - error(abort_action,errorbuffer); - return code_bad; - } - else //count = 3 or 4 - { - segnum = enter_segment(ms, segname, yes); - seg = &ms->segment[segnum]; - - if (seg->numContactObjects >= seg->contactObjectArraySize) - { - rc = code_fine; - - seg->contactObjectArraySize += SEGMENT_ARRAY_INCREMENT; - if (seg->contactObject == NULL) - { - seg->contactObject = (ContactObject*) simm_malloc( - seg->contactObjectArraySize * sizeof(ContactObject)); - } - else - { - seg->contactObject = (ContactObject*) simm_realloc(seg->contactObject, - seg->contactObjectArraySize * sizeof(ContactObject), &rc); - } - if (rc == code_bad || seg->contactObject == NULL) - { - seg->contactObjectArraySize -= SEGMENT_ARRAY_INCREMENT; - return code_bad; - } - } - - co = &seg->contactObject[seg->numContactObjects]; - - co->visible = yes; - mstrcpy(&co->name, co_name); - mstrcpy(&co->filename, co_file); -#if ! ENGINE - co->poly = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - frc = lookup_polyhedron(co->poly, co->filename, ms); - if (frc == file_bad) - { - (void)sprintf(errorbuffer, "Unable to read contact object from file %s", co->filename); - error(none, errorbuffer); - FREE_IFNOTNULL(co->poly); - } - else if (frc == file_missing) - { - (void)sprintf(errorbuffer, "Unable to locate contact object file %s", co->filename); - error(none, errorbuffer); - FREE_IFNOTNULL(co->poly); - } -#endif - /* If visible/invisible keyword was specified... */ - if (count == 4) - { - if (STRINGS_ARE_EQUAL(bufs[0], "invisible") || STRINGS_ARE_EQUAL(bufs[0], "INVISIBLE") || - STRINGS_ARE_EQUAL(bufs[0], "hidden") || STRINGS_ARE_EQUAL(bufs[0], "HIDDEN")) - co->visible = no; - if (STRINGS_ARE_EQUAL(bufs[0], "visible") || STRINGS_ARE_EQUAL(bufs[0], "VISIBLE")) - co->visible = yes; - } - seg->numContactObjects++; - } - - return code_fine; -} - - -static ReturnCode read_contact_pair(ModelStruct* ms, FILE* fp) -{ - ReturnCode rc; - ContactPair* cp; - double restitution, mu_static, mu_dynamic; - - if (fscanf(fp,"%s %s %lf %lf %lf", buffer, msg, &restitution, &mu_static, &mu_dynamic) != 5) - { - sprintf(errorbuffer, "Error reading contact pair"); - error(abort_action,errorbuffer); - return code_bad; - } - - if (ms->numContactPairs >= ms->contactPairArraySize) - { - rc = code_fine; - - ms->contactPairArraySize += SEGMENT_ARRAY_INCREMENT; - if (ms->contactPair == NULL) - { - ms->contactPair = (ContactPair*) simm_malloc( - ms->contactPairArraySize * sizeof(ContactPair)); - } - else - { - ms->contactPair = (ContactPair*) simm_realloc(ms->contactPair, - ms->contactPairArraySize * sizeof(ContactPair), &rc); - } - if (rc == code_bad || ms->contactPair == NULL) - { - ms->contactPairArraySize -= SEGMENT_ARRAY_INCREMENT; - return code_bad; - } - } - - cp = &ms->contactPair[ms->numContactPairs]; - - mstrcpy(&cp->body1, buffer); - mstrcpy(&cp->body2, msg); - cp->restitution = restitution; - cp->mu_static = mu_static; - cp->mu_dynamic = mu_dynamic; - - ms->numContactPairs++; - - return code_fine; -} - - -static ReturnCode read_contact_group(ModelStruct* ms, FILE* fp) -{ - ReturnCode rc; - ContactGroup* cg; - - if (fscanf(fp,"%s", buffer) != 1) - { - sprintf(errorbuffer, "Error reading name of contact group"); - error(abort_action,errorbuffer); - return code_bad; - } - - if (ms->numContactGroups >= ms->contactGroupArraySize) - { - rc = code_fine; - - ms->contactGroupArraySize += SEGMENT_ARRAY_INCREMENT; - if (ms->contactGroup == NULL) - { - ms->contactGroup = (ContactGroup*) simm_malloc( - ms->contactGroupArraySize * sizeof(ContactGroup)); - } - else - { - ms->contactGroup = (ContactGroup*) simm_realloc(ms->contactGroup, - ms->contactGroupArraySize * sizeof(ContactGroup), &rc); - } - if (rc == code_bad || ms->contactGroup == NULL) - { - ms->contactGroupArraySize -= SEGMENT_ARRAY_INCREMENT; - return code_bad; - } - } - - cg = &ms->contactGroup[ms->numContactGroups]; - - mstrcpy(&cg->name, buffer); - cg->numElements = 0; - cg->element = NULL; - - while (1) - { - if (fscanf(fp,"%s", buffer) != 1) - { - sprintf(errorbuffer,"Error reading elements of group %s\n", cg->name); - error(abort_action,errorbuffer); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer,"end_group")) - { - break; - } - cg->numElements++; - if (cg->element == NULL) - cg->element = (char**)simm_malloc(sizeof(char*)); - else - cg->element = (char**)simm_realloc(cg->element, - cg->numElements*sizeof(char*),&rc); - - mstrcpy(&cg->element[cg->numElements-1], buffer); - } - - ms->numContactGroups++; - - return code_fine; -} - -// Note: If the bone file is missing or bad, the polyhedron is still added to the model -// so that its filename is preserved (so it can be written back to the JNT file). -static PolyhedronStruct* make_bone(ModelStruct* ms, SegmentStruct* seg, char filename[]) -{ - ReturnCode rc = code_fine; - FileReturnCode frc; - PolyhedronStruct* ph = NULL; - - if (seg->numBones >= seg->boneArraySize) - { - seg->boneArraySize += SEGMENT_FILE_ARRAY_INCREMENT; - if (seg->bone == NULL) - { - seg->bone = (PolyhedronStruct*)simm_malloc(seg->boneArraySize*sizeof(PolyhedronStruct)); - } - else - { - seg->bone = (PolyhedronStruct*)simm_realloc(seg->bone, seg->boneArraySize*sizeof(PolyhedronStruct), &rc); - if (rc == code_bad || seg->bone == NULL) - { - seg->boneArraySize -= SEGMENT_FILE_ARRAY_INCREMENT; - return NULL; - } - } - } - - ph = &seg->bone[seg->numBones++]; - - preread_init_polyhedron(ph); - - frc = lookup_polyhedron(ph, filename, ms); - if (frc == file_bad) - simm_printf(yes, "Unable to read bone from file %s\n", filename); - else if (frc == file_missing) - simm_printf(yes, "Unable to locate bone file %s\n", filename); - - return ph; -} - -static ReturnCode read_marker(ModelStruct* ms, SegmentStruct* seg, FILE* fp) -{ - int i, count; - Marker* m; - char bufs[3][CHARBUFFER]; - - _read_til_tokens(fp, buffer, "\t\r\n"); - _strip_outer_whitespace(buffer); - - if (strlen(buffer) < 1) - { - sprintf(errorbuffer, "Error reading marker name"); - error(abort_action,errorbuffer); - return code_bad; - } - - if (seg->numMarkers >= seg->markerArraySize) - { - ReturnCode rc = code_fine; - - seg->markerArraySize += SEGMENT_ARRAY_INCREMENT; - if (seg->marker == NULL) - { - seg->marker = (Marker**)simm_malloc(seg->markerArraySize * sizeof(Marker*)); - } - else - { - seg->marker = (Marker**)simm_realloc(seg->marker, seg->markerArraySize * sizeof(Marker*), &rc); - } - if (rc == code_bad || seg->marker == NULL) - { - seg->markerArraySize -= SEGMENT_ARRAY_INCREMENT; - return code_bad; - } - } - - m = seg->marker[seg->numMarkers] = (Marker*)simm_calloc(1, sizeof(Marker)); - m->visible = ms->marker_visibility; - m->selected = no; - m->fixed = no; - - read_line(fp, bufs[0]); - count = sscanf(bufs[0], "%lf %lf %lf %lf %s %s", &m->offset[XX], &m->offset[YY], &m->offset[ZZ], - &m->weight, bufs[1], bufs[2]); - if (count < 4) - { - if (count == 3) - { - sprintf(errorbuffer, "Weight undefined for marker %s in segment %s.", buffer, seg->name); - } - else - { - sprintf(errorbuffer, "Error reading definition of marker %s in segment %s.", buffer, seg->name); - } - error(abort_action,errorbuffer); - return code_bad; - } - else - { - /* The visible, invisible, and fixed keywords can be in any order after the marker weight. */ - for (i = 1; i < count - 3; i++) - { - if (STRINGS_ARE_EQUAL(bufs[i], "invisible") || STRINGS_ARE_EQUAL(bufs[i], "INVISIBLE") || - STRINGS_ARE_EQUAL(bufs[i], "hidden") || STRINGS_ARE_EQUAL(bufs[i], "HIDDEN")) - m->visible = no; - if (STRINGS_ARE_EQUAL(bufs[i], "visible") || STRINGS_ARE_EQUAL(bufs[i], "VISIBLE")) - m->visible = yes; - if (STRINGS_ARE_EQUAL(bufs[i], "fixed") || STRINGS_ARE_EQUAL(bufs[i], "FIXED")) - m->fixed = yes; - } - mstrcpy(&m->name, buffer); - m->visible = ms->marker_visibility; - seg->numMarkers++; - } - - return code_fine; - -} - - -static ReturnCode read_constraint(ModelStruct *ms, FILE* fp) -{ - ReturnCode rc = code_fine; - ConstraintObject *co; - double xyz[3]; - DMatrix m; - int i; - - if (ms->num_constraint_objects == ms->constraint_object_array_size) - { - ms->constraint_object_array_size += CONSTRAINT_OBJECT_ARRAY_INCREMENT; - ms->constraintobj = (ConstraintObject*)simm_realloc(ms->constraintobj, - ms->constraint_object_array_size*sizeof(ConstraintObject),&rc); - - if (rc == code_bad) - { - ms->constraint_object_array_size -= CONSTRAINT_OBJECT_ARRAY_INCREMENT; - return code_bad; - } - } - - co = &ms->constraintobj[ms->num_constraint_objects]; - - initconstraintobject(co); - - /* read constraint name */ - if (fscanf(fp,"%s", buffer) != 1) - { - error(abort_action,"Error reading name in constraint definition"); - return code_bad; - } - else - mstrcpy(&co->name,buffer); - - - while (1) - { - if (read_string(fp,buffer) == EOF) - break; - - if (buffer[0] == '#') - { - read_nonempty_line(fp,buffer); - continue; - } - - if (STRINGS_ARE_EQUAL(buffer, "constrainttype")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"sphere")) - co->constraintType = constraint_sphere; - else if (STRINGS_ARE_EQUAL(buffer,"cylinder")) - co->constraintType = constraint_cylinder; - else if (STRINGS_ARE_EQUAL(buffer,"ellipsoid")) - co->constraintType = constraint_ellipsoid; - else if (STRINGS_ARE_EQUAL(buffer,"plane")) - co->constraintType = constraint_plane; - } - else if (STRINGS_ARE_EQUAL(buffer,"segment")) - { - if (fscanf(fp,"%s", buffer) != 1) - { - (void)sprintf(errorbuffer,"Error reading segment in definition of constraint object %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - co->segment = enter_segment(ms,buffer,yes); - - if (co->segment == -1) - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"active")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"false")) - co->active = no; - } - else if (STRINGS_ARE_EQUAL(buffer,"visible")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"false")) - co->visible = no; - } - else if (STRINGS_ARE_EQUAL(buffer,"rotation")) - { - sprintf(errorbuffer, "Error reading rotation for constraint object %s:", co->name); - error(none,errorbuffer); - error(abort_action," The \'rotation\' keyword has been changed to \'xyz_body_rotation\'."); - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer,"xyz_body_rotation")) - { - if (fscanf(fp, "%lg %lg %lg", &xyz[0], &xyz[1], &xyz[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading rotation for constraint object %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - identity_matrix(m); - x_rotate_matrix_bodyfixed(m, xyz[0] * DTOR); - y_rotate_matrix_bodyfixed(m, xyz[1] * DTOR); - z_rotate_matrix_bodyfixed(m, xyz[2] * DTOR); - extract_rotation(m, &co->rotationAxis, &co->rotationAngle); - } - else if (STRINGS_ARE_EQUAL(buffer,"xyz_space_rotation")) - { - if (fscanf(fp, "%lg %lg %lg", &xyz[0], &xyz[1], &xyz[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading rotation for constraint object %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - identity_matrix(m); - x_rotate_matrix_spacefixed(m, xyz[0] * DTOR); - y_rotate_matrix_spacefixed(m, xyz[1] * DTOR); - z_rotate_matrix_spacefixed(m, xyz[2] * DTOR); - extract_rotation(m, &co->rotationAxis, &co->rotationAngle); - } - else if (STRINGS_ARE_EQUAL(buffer,"rotationaxis")) - { - fscanf(fp, "%lg %lg %lg", &co->rotationAxis.xyz[0], - &co->rotationAxis.xyz[1], &co->rotationAxis.xyz[2]); - } - else if (STRINGS_ARE_EQUAL(buffer,"rotationangle")) - { - if (fscanf(fp, "%lg", &co->rotationAngle) != 1) - { - (void)sprintf(errorbuffer, "Error reading rotation for constraint object %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"translation") || - STRINGS_ARE_EQUAL(buffer,"translate")) - { - if (fscanf(fp,"%lg %lg %lg", &co->translation.xyz[0], - &co->translation.xyz[1], &co->translation.xyz[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading translation for constraint object %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"radius")) - { - if (co->constraintType == constraint_ellipsoid) - { - rc = ((fscanf(fp,"%lg %lg %lg", &co->radius.xyz[0], - &co->radius.xyz[1], &co->radius.xyz[2]) == 3) ? code_fine : code_bad); - if (co->radius.xyz[0] <= 0.0 || co->radius.xyz[1] <= 0.0 || co->radius.xyz[2] <= 0.0) - { - error(none, "Value must be greater than zero."); - rc = code_bad; - } - } - else if (co->constraintType == constraint_plane) - { - rc = ((fscanf(fp,"%lg %lg", &co->radius.xyz[0], &co->radius.xyz[1]) == 2) ? code_fine : code_bad); - if (co->radius.xyz[0] <= 0.0 || co->radius.xyz[1] <= 0.0) - { - error(none, "Value must be greater than zero."); - rc = code_bad; - } - } - else - { - rc = ((fscanf(fp,"%lg", &co->radius.xyz[0]) == 1) ? code_fine : code_bad); - if (co->radius.xyz[0] <= 0.0) - { - error(none, "Value must be greater than zero."); - rc = code_bad; - } - } - - if (rc == code_bad) - { - (void)sprintf(errorbuffer, "Error reading radius for constraint object %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"height")) - { - int num = fscanf(fp,"%lg", &co->height); - - if (num == 1 && co->height <= 0.0) - { - error(none, "Value must be greater than zero."); - num = -1; - } - - if (num != 1) - { - (void)sprintf(errorbuffer, "Error reading cylinder height for constraint object %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"quadrant")) - { - if (read_string(fp,buffer) == EOF) - break; - - set_constraint_specs(co, buffer); - } -/* else if (STRINGS_ARE_EQUAL(buffer, "plane")) - { - double a, b, c, d; - if (fscanf(fp, "%s %lf %lf %lf %lf", buffer, &a, &b, &c, &d) != 4) - { - (void)sprintf(errorbuffer, - "Error reading definition of plane (segment a b c d) in definition of constraint %s", - co->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - co->constraintType = constraint_plane; - -// co->plane.segment = enter_segment(ms, buffer, yes); -// co->plane.plane.a = a; -// co->plane.plane.b = b; -// co->plane.plane.c = c; -// co->plane.plane.d = d; - } - }*/ - else if (STRINGS_ARE_EQUAL(buffer, "beginpoints")) - { - co->points = read_constraint_points(ms, fp, &co->numPoints, - &co->cp_array_size); - if (co->points == NULL) - { - (void)sprintf(errorbuffer, "Error reading points for constraint %s.", - co->name); - error(abort_action, errorbuffer); - return code_bad; - } - /* for (i = 0; i < co->numPoints; i++) - { - if (co->points[i].segment != co->points[0].segment) - { - (void)sprintf(errorbuffer, "Constraint points in %s must all be on same segment.", - co->name); - error(abort_action, errorbuffer); - return code_bad; - } - }*/ - } - else if (STRINGS_ARE_EQUAL(buffer,"endconstraintobject")) - break; - else - { - (void)sprintf(errorbuffer, "Unknown string %s in constraint %s.", - buffer, co->name); - error(none, errorbuffer); - } - } - co->undeformed_translation = co->translation; - - co->xforms_valid = no; - /* set constraint point activations and visibility to that of co if co is inactive/invisible*/ -//dkb nov 2009 - /* - for (i = 0; i < co->numPoints; i++) - { - if (co->visible == no) - co->points[i].visible = co->visible; - if (co->active == no) - co->points[i].active = co->active; - }*/ - - if (co->constraintType == constraint_none) - { - (void)sprintf(errorbuffer, "No constraint type (plane, constraint_object) specified for %s.", - co->name); - error(abort_action, errorbuffer); -// return code_bad; - } - ms->num_constraint_objects++; - - return code_fine; - -} - -static ConstraintPoint* read_constraint_points(ModelStruct* ms, FILE* fp, int *numpoints, - int *cp_array_size) -{ - int num; - double x, y, z, w, tol; - ConstraintPoint *cp; - char buf2[CHARBUFFER], point_name[CHARBUFFER], segment_name[CHARBUFFER]; - ReturnCode rc = code_fine; - - *numpoints = 0; - *cp_array_size = CP_ARRAY_INCREMENT; - - cp = (ConstraintPoint*)simm_malloc((*cp_array_size)*sizeof(ConstraintPoint)); - if (cp == NULL) - return (NULL); - - while (1) - { - if (read_nonempty_line(fp, buffer) == EOF) - { - error(none, "EOF while reading constraint points"); - return (NULL); - } - - sscanf(buffer, "%s", buf2); - if (STRINGS_ARE_EQUAL(buf2, "endpoints")) - return (cp); - - /* Check to see if you need to increase the size of the muscle-point - * array before reading any more points. - */ - if ((*numpoints) >= (*cp_array_size)) - { - (*cp_array_size) += CP_ARRAY_INCREMENT; - cp = (ConstraintPoint*)simm_realloc(cp,(*cp_array_size)*sizeof(ConstraintPoint),&rc); - if (rc == code_bad) - { - (*cp_array_size) -= CP_ARRAY_INCREMENT; - return (NULL); - } - } - initconstraintpoint(&cp[*numpoints]); - - /* If the string you just read was not "endpoints" then it must - * be the start of a new point. */ - /* read point name, offset, weight, and segment name and tolerance */ - num = sscanf(buffer, "%s %lf %lf %lf %lf %s %lf", point_name, &x, &y, &z, &w, - segment_name, &tol); - if ((num != 6) && (num != 7)) - return NULL; - - cp[*numpoints].offset[XX] = x; - cp[*numpoints].offset[YY] = y; - cp[*numpoints].offset[ZZ] = z; - cp[*numpoints].weight = w; -// cp[*numpoints].visible = yes; -// cp[*numpoints].active = yes; - if (num == 7) - cp[*numpoints].tolerance = tol; - mstrcpy(&cp[*numpoints].name, point_name); - cp[*numpoints].segment = enter_segment(ms, segment_name, yes); - if (cp[*numpoints].segment == -1) - { - (void)sprintf(errorbuffer,"Memory error while reading constraint points.", segment_name); - error(none,errorbuffer); - return (NULL); - } - (*numpoints)++; - } -} - -/* ------------------------------------------------------------------------- - read_motion_object - ----------------------------------------------------------------------------- */ -static ReturnCode read_motion_object(ModelStruct* ms, FILE* fp) -{ - int i; - MotionObject* mo; - ReturnCode rc = code_fine; - - /* read the motion object's name: - */ - if (fscanf(fp,"%s", buffer) != 1) - { - error(abort_action, "Error reading name in motion object definition"); - return code_bad; - } - - /* scan the list of existing motion objects to see if one already exists - * with the name we just read: - */ - for (i = 0; i < ms->num_motion_objects; i++) - if (STRINGS_ARE_EQUAL(buffer, ms->motion_objects[i].name)) - break; - - /* create a new motion object if necessary: - */ - if (i == ms->num_motion_objects) - { - i = ms->num_motion_objects++; - - if (ms->motion_objects == NULL) - ms->motion_objects = (MotionObject*) simm_malloc(ms->num_motion_objects * sizeof(MotionObject)); - else - ms->motion_objects = (MotionObject*) simm_realloc(ms->motion_objects, - ms->num_motion_objects * sizeof(MotionObject), - &rc); - if (ms->motion_objects == NULL) - { - ms->num_motion_objects = 0; - return code_bad; - } - mo = &ms->motion_objects[i]; - - memset(mo, 0, sizeof(MotionObject)); - - mstrcpy(&mo->name, buffer); - - mstrcpy(&mo->materialname, "def_motion"); - mo->material = enter_material(ms, mo->materialname, declaring_element); - - for (i = XX; i <= ZZ; i++) - { - mo->startingPosition[i] = 0.0; - mo->startingScale[i] = 1.0; - mo->startingXYZRotation[i] = 0.0; - } - mo->drawmode = gouraud_shading; - mo->vector_axis = YY; - } - else - mo = &ms->motion_objects[i]; - - mo->defined_in_file = yes; - - /* read the motion object's parameters: - */ - while (1) - { - if (read_string(fp, buffer) == EOF || STRINGS_ARE_EQUAL(buffer, "endmotionobject")) - break; - - if (buffer[0] == '#') - { - read_nonempty_line(fp,buffer); - continue; - } - - if (STRINGS_ARE_EQUAL(buffer, "filename")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - simm_printf(yes, "Error reading filename for motion object: %s.\n", mo->name); - return code_bad; - } - FREE_IFNOTNULL(mo->filename); - mstrcpy(&mo->filename, buffer); -#if ! OPENSMAC && ! ENGINE - free_polyhedron(&mo->shape, no, ms); - if (lookup_polyhedron(&mo->shape, mo->filename, ms) != code_fine) - { - simm_printf(yes, "Error reading motion object: %s\n", mo->filename); - return code_bad; - } -#endif - } - if (STRINGS_ARE_EQUAL(buffer, "position") || STRINGS_ARE_EQUAL(buffer,"origin")) - { - if (fscanf(fp, "%lg %lg %lg", &mo->startingPosition[0], - &mo->startingPosition[1], - &mo->startingPosition[2]) != 3) - { - simm_printf(yes, "Error reading position for motion object: %s.\n", mo->name); - return code_bad; - } - } - if (STRINGS_ARE_EQUAL(buffer, "scale")) - { - if (fscanf(fp, "%lg %lg %lg", &mo->startingScale[0], - &mo->startingScale[1], - &mo->startingScale[2]) != 3) - { - simm_printf(yes, "Error reading scale for motion object: %s.\n", mo->name); - return code_bad; - } - } - if (STRINGS_ARE_EQUAL(buffer, "xyzrotation")) - { - if (fscanf(fp, "%lg %lg %lg", &mo->startingXYZRotation[0], - &mo->startingXYZRotation[1], - &mo->startingXYZRotation[2]) != 3) - { - simm_printf(yes, "Error reading xyzrotation for motion object: %s.\n", mo->name); - return code_bad; - } - } - if (STRINGS_ARE_EQUAL(buffer, "material")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - simm_printf(yes, "Error reading material for motion object: %s.\n", mo->name); - return code_bad; - } - FREE_IFNOTNULL(mo->materialname); - free(mo->materialname); - - mstrcpy(&mo->materialname, buffer); - - mo->material = enter_material(ms, mo->materialname, declaring_element); - } - if (STRINGS_ARE_EQUAL(buffer, "drawmode")) - { - if (read_drawmode(fp, &mo->drawmode) != code_fine) - { - simm_printf(yes,"Error reading drawmode for motion object: %s.\n", mo->name); - return code_bad; - } - } - if (STRINGS_ARE_EQUAL(buffer, "vectoraxis")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - simm_printf(yes, "Error reading vectoraxis for motion object: %s.\n", mo->name); - return code_bad; - } - switch (buffer[0]) - { - case 'x': case 'X': mo->vector_axis = XX; break; - case 'y': case 'Y': mo->vector_axis = YY; break; - case 'z': case 'Z': mo->vector_axis = ZZ; break; - } - } - } - return rc; -} diff --git a/OpenSim/Utilities/simmToOpenSim/readmuscles.c b/OpenSim/Utilities/simmToOpenSim/readmuscles.c deleted file mode 100644 index a03d3287ad..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/readmuscles.c +++ /dev/null @@ -1,1393 +0,0 @@ -/******************************************************************************* - - READMUSCLES.C - - Author: Peter Loan - - Date: 8-DEC-88 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains routines that read-in muscle - descriptions from an input file. - - Routines: - readmuscles : main routine to read input file for muscle descriptions - getmuscle : a big if-else block to read in each muscle parameter - read_tendonflcurve : reads a tendon force-length curve from a muscle file - read_activeflcurve : reads an active force-length curve from a muscle file - read_passiveflcurve : reads a passive force-length curve from muscle file - read_forcecvelocitycurve : reads force-velocity curve from a muscle file - read_muscle_parameter : reads a muscle parameter (e.g. pennation) from file - read_dynamic_parameter_names : reads dynamic (muscle model) parameter names - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" - -#include "wefunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -dpFunctionType excitationType = dpFunctionTypeUndefined; - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static ReturnCode read_muscle(ModelStruct* ms, FILE* fp, dpMuscleStruct* muscle, int muscle_number); -static ReturnCode read_muscle_function(FILE* fp, ModelStruct* model, dpMuscleStruct* muscle, - dpFunction*** mfunc, dpFunction** default_mfunc, - char ending[], char description[]); -static void finish_muscle_definition(FILE* fp); -static ReturnCode read_muscle_parameter(FILE* fp, dpMuscleStruct* ms, - dpMuscleStruct* default_musc, double** param, - double* def_param, char param_name[], - double min_value, double max_value); -static ReturnCode read_excitation(FILE* fp, ModelStruct* ms, dpMuscleStruct* muscle); -static ReturnCode read_dynamic_parameter_names(FILE* fp, dpMuscleStruct* muscle); -static SBoolean check_dynamic_param_array(FILE* fp, dpMuscleStruct* muscle, char text_string[]); -static ReturnCode read_ligament(FILE* fp, ModelStruct* ms); -static void sort_muscle_groups(ModelStruct*); - - - - -/* READ_MUSCLE_FILE: this is the main routine for reading a muscle input - * file. It runs cpp on the muscle file, then reads it in. When it finds - * the string "beginmuscle" it calls read_muscle() read in a muscle - * definition. - */ -ReturnCode read_muscle_file(ModelStruct* ms, char filename[], SBoolean* file_exists, SBoolean showTopLevelMessages) -{ - char tempFileName[CHARBUFFER]; - int i, count, muscle_errors = 0; - SBoolean default_muscle_defined = no, any_errors = no; - FILE* fp; - ReturnCode rc; - -#if ENGINE - strcpy(tempFileName, ".muscles"); -#else - strcpy(tempFileName, glutGetTempFileName("simm-muscles")); -#endif - - if (file_exists) - *file_exists = no; - - if ((fp = preprocess_file(filename, tempFileName)) == NULL) - { - if (showTopLevelMessages == yes) - { - (void)sprintf(buffer, "Unable to open muscle input file %s:", filename); - message(buffer, HIGHLIGHT_TEXT,DEFAULT_MESSAGE_X_OFFSET); - message("Loading model without muscles.", HIGHLIGHT_TEXT, DEFAULT_MESSAGE_X_OFFSET+20); - } - return code_fine; - } - - ms->nummuscles = 0; - ms->default_muscle = (dpMuscleStruct*)simm_calloc(1, sizeof(dpMuscleStruct)); - - // To support deprecated parameter, reset this for each model loaded. - excitationType = dpStepFunction; - - while (1) - { - if (read_string(fp, buffer) == EOF) - break; - - if (buffer[0] == '#') - { - read_nonempty_line(fp, buffer); - continue; - } - - if (STRINGS_ARE_EQUAL(buffer, "beginmuscle")) - { - read_string(fp, buffer); - if (STRINGS_ARE_EQUAL(buffer, "defaultmuscle")) - { - if (default_muscle_defined == yes) - { - error(recover, "Default muscle already defined. Ignoring redefinition."); - while (1) - { - read_string(fp, buffer); - if (STRINGS_ARE_EQUAL(buffer, "endmuscle")) - break; - } - } - else if (ms->nummuscles > 0) - { - error(none, "Error reading default muscle:"); - error(abort_action, " You must define the default muscle before any other muscles."); - return code_bad; - } - else - { - default_muscle_defined = yes; - ms->default_muscle->display = yes; - ms->default_muscle->output = yes; - ms->default_muscle->excitation_abscissa = (void**)simm_malloc(sizeof(void*)); - *(ms->default_muscle->excitation_abscissa) = TIME; - ms->default_muscle->index = DEFAULT_MUSCLE_INDEX; - if (ms->default_muscle->num_dynamic_params > 0) - ms->default_muscle->dynamic_params = (double**)simm_calloc(ms->default_muscle->num_dynamic_params, sizeof(double*)); - mstrcpy(&ms->default_muscle->name, "defaultmuscle"); - (void)read_muscle(ms, fp, ms->default_muscle, -1); - } - } - else - { - // Whether or not the user defined a default muscle, you want to - // initialize certain elements of the default muscle structure - // before reading in any muscles. So do it here if you're about to - // read in the first muscle. - if (ms->nummuscles == 0) - { - if (init_default_muscle(ms) == code_bad) - return code_bad; - } - - /* Check for duplicate muscle names */ - for (i=0, count=0; inummuscles; i++) - { - if (STRINGS_ARE_EQUAL(buffer, ms->muscle[i]->name)) - count++; - } - if (count > 0) - { - (void)sprintf(errorbuffer, "Warning: %d muscles with name %s", count+1, buffer); - error(none,errorbuffer); - } - if (ms->nummuscles == ms->muscle_array_size) - { - ms->muscle_array_size += MUSCLE_ARRAY_INCREMENT; - ms->muscle = (dpMuscleStruct**)simm_realloc(ms->muscle, - ms->muscle_array_size*sizeof(dpMuscleStruct*), &rc); - if (rc == code_bad) - { - ms->muscle_array_size -= MUSCLE_ARRAY_INCREMENT; - return code_bad; - } - } - ms->muscle[ms->nummuscles] = (dpMuscleStruct*)simm_malloc(sizeof(dpMuscleStruct)); - if (init_muscle(ms, ms->muscle[ms->nummuscles], ms->default_muscle) == code_bad) - return code_bad; - ms->muscle[ms->nummuscles]->index = ms->nummuscles; - mstrcpy(&ms->muscle[ms->nummuscles]->name, buffer); - rc = read_muscle(ms, fp, ms->muscle[ms->nummuscles], ms->nummuscles); - if (rc == code_fine) - { - post_init_muscle_path(ms->muscle[ms->nummuscles]->path, ms->muscle[ms->nummuscles]->numWrapStructs); - ms->nummuscles++; - } - else - { - free(ms->muscle[ms->nummuscles]->momentarms); - error(none, "Cancelling this muscle definition."); - finish_muscle_definition(fp); - } - } - } - else if (STRINGS_ARE_EQUAL(buffer, "begindynamicparameters")) - { - if (default_muscle_defined == yes || ms->nummuscles > 0) - { - error(none, "Error reading dynamic parameter names:"); - error(abort_action, " You must define the names before the default muscle or any other muscles."); - return code_bad; - } - else - { - if (read_dynamic_parameter_names(fp, ms->default_muscle) == code_bad) - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "beginmusclesurface") || STRINGS_ARE_EQUAL(buffer, "beginligament")) - { - if (read_ligament(fp, ms) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpFunctionTypeUndefined, 0))) // old-style function definitions - { - if (read_function(ms, fp, yes, dpNaturalCubicSpline, get_function_tag(dpFunctionTypeUndefined, 1)) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpStepFunction, 0))) - { - if (read_function(ms, fp, yes, dpStepFunction, get_function_tag(dpStepFunction, 1)) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpLinearFunction, 0))) - { - if (read_function(ms, fp, yes, dpLinearFunction, get_function_tag(dpLinearFunction, 1)) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpNaturalCubicSpline, 0))) - { - if (read_function(ms, fp, yes, dpNaturalCubicSpline, get_function_tag(dpNaturalCubicSpline, 1)) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_tag(dpGCVSpline, 0))) - { - if (read_function(ms, fp, yes, dpGCVSpline, get_function_tag(dpGCVSpline, 1)) == code_bad) - return code_bad; - } - else - { - (void)sprintf(errorbuffer, "Unrecognized string \"%s\" found in muscle file.", - buffer); - error(recover,errorbuffer); - muscle_errors++; - } - if (muscle_errors > 10) - { - error(none, "Too many errors to continue."); - return code_bad; - } - } - - (void)fclose(fp); - (void)unlink((const char*)tempFileName); - - /* check that all functions used in muscle points are defined */ - for (i=0; ifunc_array_size; i++) - { - if (ms->function[i] && ms->function[i]->used == dpYes && ms->function[i]->status == dpFunctionUndefined) - { - (void)sprintf(errorbuffer, "Function f%d referenced in muscle file but not defined in muscle file.", ms->function[i]->usernum); - error(none,errorbuffer); - any_errors = yes; - } - } - if (any_errors == yes) - return code_bad; - - - /* If the muscle file contained a default muscle and no other - * muscles, then make sure that everything in the default muscle - * is initialized properly. This is usually done right before - * the first real muscle is read, but if there are none it is - * done here. - */ - if (ms->nummuscles == 0) - { - if (init_default_muscle(ms) == code_bad) - return code_bad; - } - - sort_muscle_groups(ms); - -#if ! ENGINE - if ( ! is_in_demo_mode()) - { - if (showTopLevelMessages == yes) - { - (void)sprintf(buffer, "Read muscle file %s", filename); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); - } - } -#endif - - if (file_exists) - *file_exists = yes; - - return code_fine; - -} - - -static ModelStruct* sSortingMuscGroupsForModel = NULL; - -static int _compare_muscle_names (const void* elem1, const void* elem2) -{ - return strcmp(sSortingMuscGroupsForModel->muscle[*(int*) elem1]->name, - sSortingMuscGroupsForModel->muscle[*(int*) elem2]->name); -} - -static void sort_muscle_groups (ModelStruct* ms) -{ - int i; - - sSortingMuscGroupsForModel = ms; - - for (i = 0; i < ms->numgroups; i++) - qsort(ms->muscgroup[i].muscle_index, - ms->muscgroup[i].num_muscles, sizeof(int), _compare_muscle_names); -} - - -/* READ_MUSCLE: this routine reads a muscle definition from a file. It is - * basically a big if-else block with one section for each muscle parameter. - */ -static ReturnCode read_muscle(ModelStruct* ms, FILE* fp, dpMuscleStruct* muscle, int muscle_number) -{ - char name[CHARBUFFER]; - int i, num_read, function_num, funcIndex; - - while (1) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "beginpoints")) - { - // don't read points for default muscle - if (muscle == ms->default_muscle || muscle_number < 0) - { - while (1) - { - read_string(fp, buffer); - if (STRINGS_ARE_EQUAL(buffer, "endpoints")) - break; - } - continue; - } - - muscle->path = (dpMusclePathStruct*)simm_calloc(1, sizeof(dpMusclePathStruct)); - if (read_muscle_path(ms, fp, muscle->path) == code_bad) - { - (void)sprintf(errorbuffer, "Error reading coordinates for muscle %s.", muscle->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "begingroups")) - { - muscle->group = read_muscle_groups(ms, fp, &muscle->numgroups, muscle_number); - if (muscle->group == NULL && muscle->numgroups != 0) - { - muscle->numgroups = 0; - (void)sprintf(errorbuffer, "Error reading groups for muscle %s.", muscle->name); - error(recover,errorbuffer); - } - } - else if (STRINGS_ARE_EQUAL(buffer, "begintendonforcelengthcurve")) // deprecated - { - if (read_muscle_function(fp, ms, muscle, - &muscle->tendon_force_len_func, ms->default_muscle->tendon_force_len_func, - "endtendonforcelengthcurve", "tendon_force_length_curve") == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "tendon_force_length_curve")) - { - read_string(fp, buffer); - if (sscanf(buffer, "f%d", &function_num) != 1) - { - (void)sprintf(errorbuffer, "Error reading tendon_force_length_curve for muscle %s.", muscle->name); - if (muscle != ms->default_muscle && ms->default_muscle != NULL) - { - muscle->tendon_force_len_func = ms->default_muscle->tendon_force_len_func; - error(none, errorbuffer); - error(none, "Using function from default muscle."); - return code_fine; - } - else - { - error(abort_action, errorbuffer); - return code_bad; - } - } - muscle->tendon_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (muscle->tendon_force_len_func == NULL) - return code_bad; - *muscle->tendon_force_len_func = ms->function[enter_function(ms, function_num, yes)]; - } - else if (STRINGS_ARE_EQUAL(buffer, "beginactiveforcelengthcurve")) // deprecated - { - if (read_muscle_function(fp, ms, muscle, - &muscle->active_force_len_func, ms->default_muscle->active_force_len_func, - "endactiveforcelengthcurve", "active_force_length_curve") == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "active_force_length_curve")) - { - read_string(fp, buffer); - if (sscanf(buffer, "f%d", &function_num) != 1) - { - (void)sprintf(errorbuffer, "Error reading active_force_length_curve for muscle %s.", muscle->name); - if (muscle != ms->default_muscle && ms->default_muscle != NULL) - { - muscle->active_force_len_func = ms->default_muscle->active_force_len_func; - error(none, errorbuffer); - error(none, "Using function from default muscle."); - return code_fine; - } - else - { - error(abort_action, errorbuffer); - return code_bad; - } - } - muscle->active_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (muscle->active_force_len_func == NULL) - return code_bad; - *muscle->active_force_len_func = ms->function[enter_function(ms, function_num, yes)]; - } - else if (STRINGS_ARE_EQUAL(buffer, "beginpassiveforcelengthcurve")) // deprecated - { - if (read_muscle_function(fp, ms, muscle, - &muscle->passive_force_len_func, ms->default_muscle->passive_force_len_func, - "endpassiveforcelengthcurve", "passive_force_length_curve") == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "passive_force_length_curve")) - { - read_string(fp, buffer); - if (sscanf(buffer, "f%d", &function_num) != 1) - { - (void)sprintf(errorbuffer, "Error reading passive_force_length_curve for muscle %s.", muscle->name); - if (muscle != ms->default_muscle && ms->default_muscle != NULL) - { - muscle->passive_force_len_func = ms->default_muscle->passive_force_len_func; - error(none, errorbuffer); - error(none, "Using function from default muscle."); - return code_fine; - } - else - { - error(abort_action, errorbuffer); - return code_bad; - } - } - muscle->passive_force_len_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (muscle->passive_force_len_func == NULL) - return code_bad; - *muscle->passive_force_len_func = ms->function[enter_function(ms, function_num, yes)]; - } - else if (STRINGS_ARE_EQUAL(buffer, "beginforcevelocitycurve")) // deprecated - { - if (read_muscle_function(fp, ms, muscle, - &muscle->force_vel_func, ms->default_muscle->force_vel_func, - "endforcevelocitycurve", "force_velocity_curve") == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "force_velocity_curve")) - { - read_string(fp, buffer); - if (sscanf(buffer, "f%d", &function_num) != 1) - { - (void)sprintf(errorbuffer, "Error reading force_velocity_curve for muscle %s.", muscle->name); - if (muscle != ms->default_muscle && ms->default_muscle != NULL) - { - muscle->force_vel_func = ms->default_muscle->force_vel_func; - error(none, errorbuffer); - error(none, "Using function from default muscle."); - return code_fine; - } - else - { - error(abort_action, errorbuffer); - return code_bad; - } - } - muscle->force_vel_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (muscle->force_vel_func == NULL) - return code_bad; - *muscle->force_vel_func = ms->function[enter_function(ms, function_num, yes)]; - } - else if (STRINGS_ARE_EQUAL(buffer, "max_contraction_velocity")) - { - if (read_muscle_parameter(fp, muscle, ms->default_muscle, - &muscle->max_contraction_vel, ms->default_muscle->max_contraction_vel, - "max_contraction_velocity", 0.0, DONT_CHECK_DOUBLE) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "max_force")) - { - if (read_muscle_parameter(fp, muscle, ms->default_muscle, - &muscle->max_isometric_force, - ms->default_muscle->max_isometric_force, - "max_force",0.0,DONT_CHECK_DOUBLE) == code_bad) - return code_bad; - muscle->force = *muscle->max_isometric_force; - } - else if (STRINGS_ARE_EQUAL(buffer, "tendon_slack_length")) - { - if (read_muscle_parameter(fp, muscle, ms->default_muscle, - &muscle->resting_tendon_length, - ms->default_muscle->resting_tendon_length, - "tendon_slack_length",0.0,DONT_CHECK_DOUBLE) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "optimal_fiber_length")) - { - if (read_muscle_parameter(fp, muscle, ms->default_muscle, - &muscle->optimal_fiber_length, - ms->default_muscle->optimal_fiber_length, - "optimal_fiber_length",0.0,DONT_CHECK_DOUBLE) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "pennation_angle")) - { - if (read_muscle_parameter(fp, muscle, ms->default_muscle, - &muscle->pennation_angle, - ms->default_muscle->pennation_angle, - "pennation_angle",0.0,90.0) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "min_thickness")) - { - if (read_muscle_parameter(fp, muscle, ms->default_muscle, - &muscle->min_thickness, - ms->default_muscle->min_thickness, - "min_thickness",0.0,DONT_CHECK_DOUBLE) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "max_thickness")) - { - if (read_muscle_parameter(fp, muscle, ms->default_muscle, - &muscle->max_thickness, - ms->default_muscle->max_thickness, - "max_thickness",0.0,DONT_CHECK_DOUBLE) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "min_material")) - { - if (fscanf(fp, "%s", name) != 1) - { - (void)sprintf(errorbuffer, "Error reading min_material for %s muscle", muscle->name); - error(abort_action, errorbuffer); - return code_bad; - } - else - { - muscle->min_material = (int*)simm_malloc(sizeof(int)); - *muscle->min_material = enter_material(ms,name,just_checking_element); - if (*muscle->min_material == -1) - { - free(muscle->min_material); - if (muscle == ms->default_muscle) - muscle->min_material = NULL; // will get reset later, in init_default_muscle() - else - muscle->min_material = ms->default_muscle->min_material; - (void)sprintf(errorbuffer, "Error reading min_material for muscle %s (%s is undefined)", - muscle->name, name); - error(none,errorbuffer); - error(none, "Using default material for min_material."); - } - } - } - else if (STRINGS_ARE_EQUAL(buffer, "max_material")) - { - if (fscanf(fp, "%s", name) != 1) - { - (void)sprintf(errorbuffer, "Error reading max_material for muscle %s", muscle->name); - error(abort_action,errorbuffer); - return code_bad; - } - else - { - muscle->max_material = (int*)simm_malloc(sizeof(int)); - *muscle->max_material = enter_material(ms,name,just_checking_element); - if (*muscle->max_material == -1) - { - free(muscle->max_material); - if (muscle == ms->default_muscle) - muscle->max_material = NULL; // will get reset later, in init_default_muscle() - else - muscle->max_material = ms->default_muscle->max_material; - (void)sprintf(errorbuffer, "Error reading max_material for muscle %s (%s is undefined)", - muscle->name, name); - error(none,errorbuffer); - error(none, "Using default material for max_material."); - } - } - } - else if (STRINGS_ARE_EQUAL(buffer, "activation")) - { - if (fscanf(fp, "%lg", &muscle->dynamic_activation) != 1) - { - (void)sprintf(errorbuffer, "Error reading activation for muscle %s.", muscle->name); - error(none,errorbuffer); - error(none, "Using a value of 1.0"); - muscle->dynamic_activation = 1.0; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "beginexcitation")) // deprecated - { - if (read_excitation(fp, ms, muscle) == code_bad) - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "excitation")) - { - read_line(fp, buffer); - if (sscanf(buffer, "f%d(%s)", &function_num, name) != 2) //TODO5.0: support spaces in here, like in read_refeq()? - { - (void)sprintf(errorbuffer, "Error reading excitation for muscle %s.", muscle->name); - if (muscle != ms->default_muscle && ms->default_muscle != NULL) - { - muscle->excitation_func = ms->default_muscle->excitation_func; - error(none, errorbuffer); - error(none, "Using function from default muscle."); - return code_fine; - } - else - { - error(abort_action, errorbuffer); - return code_bad; - } - } - muscle->excitation_abscissa = (void**)simm_malloc(sizeof(void*)); - if (muscle->excitation_abscissa == NULL) - return code_bad; - strip_brackets_from_string(name); - if (STRINGS_ARE_EQUAL(name, "time")) - *muscle->excitation_abscissa = TIME; - else - *muscle->excitation_abscissa = enter_gencoord(ms, name, no); - if (*muscle->excitation_abscissa == NULL) - { - (void)sprintf(errorbuffer, "Error reading abscissa of excitation function for muscle %s", muscle->name); - error(none, errorbuffer); - (void)sprintf(errorbuffer, "%s is neither \"time\" nor the name of a generalized coordinate", buffer); - error(abort_action, errorbuffer); - finish_muscle_definition(fp); - return code_bad; - } - muscle->excitation_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (muscle->excitation_func == NULL) - return code_bad; - *muscle->excitation_func = ms->function[enter_function(ms, function_num, yes)]; - } - else if (STRINGS_ARE_EQUAL(buffer, "excitation_format")) // deprecated - { - fscanf(fp, "%s", buffer); - // If the excitation format is defined after the excitation, then you can store - // the format directly in the excitation function. If it is defined before, store - // it in a static global for use later. - if (STRINGS_ARE_EQUAL(buffer, "spline_fit") || STRINGS_ARE_EQUAL(buffer, get_function_type_name(dpNaturalCubicSpline))) - { - if (muscle->excitation_func) - { - if ((*muscle->excitation_func)->type != dpNaturalCubicSpline) - { - (*muscle->excitation_func)->type = dpNaturalCubicSpline; - calc_function_coefficients(*muscle->excitation_func); - } - } - else - { - excitationType = dpNaturalCubicSpline; - } - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_type_name(dpGCVSpline))) - { - if (muscle->excitation_func) - { - if ((*muscle->excitation_func)->type != dpGCVSpline) - { - (*muscle->excitation_func)->type = dpGCVSpline; - calc_function_coefficients(*muscle->excitation_func); - } - } - else - { - excitationType = dpGCVSpline; - } - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_type_name(dpStepFunction))) - { - if (muscle->excitation_func) - { - if ((*muscle->excitation_func)->type != dpStepFunction) - { - (*muscle->excitation_func)->type = dpStepFunction; - calc_function_coefficients(*muscle->excitation_func); - } - } - else - { - excitationType = dpStepFunction; - } - } - else if (STRINGS_ARE_EQUAL(buffer, get_function_type_name(dpLinearFunction))) - { - if (muscle->excitation_func) - { - if ((*muscle->excitation_func)->type != dpLinearFunction) - { - (*muscle->excitation_func)->type = dpLinearFunction; - calc_function_coefficients(*muscle->excitation_func); - } - } - else - { - excitationType = dpLinearFunction; - } - } - else - { - sprintf(errorbuffer, "Error reading excitation_format for muscle %s.", muscle->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "muscle_model")) - { - muscle->muscle_model_index = (int*)simm_malloc(sizeof(int)); - if (muscle->muscle_model_index == NULL) - return code_bad; - num_read = fscanf(fp, "%d", muscle->muscle_model_index); - if (num_read != 1 || *(muscle->muscle_model_index) < 1) - { - if (muscle != ms->default_muscle && ms->default_muscle->muscle_model_index != NULL) - { - *(muscle->muscle_model_index) = *(ms->default_muscle->muscle_model_index); - (void)sprintf(errorbuffer, "Error reading muscle_model for muscle %s.", - muscle->name); - error(none,errorbuffer); - error(none, "Using value from default muscle."); - } - else - { - (void)sprintf(errorbuffer, "Error reading muscle_model for muscle %s.", - muscle->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - } - else if (STRINGS_ARE_EQUAL(buffer, "visible")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "false")) - muscle->display = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "true")) - muscle->display = yes; - } - else if (STRINGS_ARE_EQUAL(buffer, "output")) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "no") || STRINGS_ARE_EQUAL(buffer, "false")) - muscle->output = no; - else if (STRINGS_ARE_EQUAL(buffer, "yes") || STRINGS_ARE_EQUAL(buffer, "true")) - muscle->output = yes; - } - else if (STRINGS_ARE_EQUAL(buffer, "wrapobjects")) - { - sprintf(buffer, "Error reading wrap specification for muscle: %s", muscle->name); - - error(none, buffer); - error(none, " (1) The \"wrapobjects\" keyword has been changed to \"wrapobject\"."); - error(none, " (2) Space characters are no longer allowed in wrap object names."); - error(none, " (3) You may now specify the ellipsoid wrapping method following the wrap"); - error(none, " object\'s name with the keywords: \"hybrid\", \"midpoint\", or \"axial\"."); - error(none, " The wrapping method defaults to hybrid if none is specified."); - - read_line(fp, buffer); - } - else if (STRINGS_ARE_EQUAL(buffer, "wrapobject")) - { - char* p; - int i, j; - dpMuscleWrapStruct* wrap = NULL; - ReturnCode rc; - SBoolean algorithmSpecified = no; - - read_line(fp, buffer); - - p = strtok(buffer, " \t"); - - if (p) - { - wrap = (dpMuscleWrapStruct*)simm_malloc(sizeof(dpMuscleWrapStruct)); - - if (wrap) - { - /* initialize the muscle wrap association */ - wrap->wrap_object = get_wrap_object(ms, p); - wrap->startPoint = -1; - wrap->endPoint = -1; - - if (wrap->wrap_object == NULL) - { - sprintf(errorbuffer, "Unknown wrap object (%s) in muscle %s. Object will be ignored.", p, muscle->name); - error(none,errorbuffer); - FREE_IFNOTNULL(wrap); - } - else - { - wrap->wrap_algorithm = wrap->wrap_object->wrap_algorithm; - - for (i = 0; i < 2; i++) - init_musclepoint(&wrap->mp_wrap[i]); - - for (i = 0; i < 3; i++) - { - wrap->c[i] = MINMDOUBLE; - wrap->r1[i] = MINMDOUBLE; - wrap->r2[i] = MINMDOUBLE; - } - } - } - } - - /* if a wrap algorithm was specified, override the default algorithm */ - p = strtok(NULL, " \t"); - - if (p && wrap) - { - for (i = 0; i < WE_NUM_WRAP_ALGORITHMS; i++) - { - if (STRINGS_ARE_EQUAL(p, get_wrap_algorithm_name(i))) - { - wrap->wrap_algorithm = i; - algorithmSpecified = yes; - break; - } - } - } - - /* if a range of muscle points was specified, override the defaults */ - if (algorithmSpecified == yes) - p = strtok(NULL, " \t"); - - if (p && wrap) - { - if (STRINGS_ARE_EQUAL(p, "range")) - { - p = strtok(NULL, " \t"); - if (p) - { - wrap->startPoint = atoi(p); - if (wrap->startPoint == 0) - { - (void)sprintf(errorbuffer, "Error reading wrap starting range in muscle %s.", muscle->name); - error(recover,errorbuffer); - } - } - else - { - (void)sprintf(errorbuffer, "Error reading wrap starting range in muscle %s.", muscle->name); - error(recover,errorbuffer); - } - p = strtok(NULL, " \t"); - if (p) - { - wrap->endPoint = atoi(p); - if (wrap->endPoint == 0) - { - (void)sprintf(errorbuffer, "Error reading wrap ending range in muscle %s.", muscle->name); - error(recover,errorbuffer); - } - } - else - { - (void)sprintf(errorbuffer, "Error reading wrap ending range in muscle %s.", muscle->name); - error(recover,errorbuffer); - } - if (wrap->startPoint < 1) - wrap->startPoint = -1; - if (wrap->endPoint < 1) - wrap->endPoint = -1; - if (wrap->startPoint > wrap->endPoint && wrap->endPoint != -1) - { - wrap->startPoint = wrap->endPoint - 1; - (void)sprintf(errorbuffer, "Changing wrap range to (%d, %d) for muscle %s", - wrap->startPoint, wrap->endPoint, muscle->name); - error(recover,errorbuffer); - } - } - else - { - (void)sprintf(errorbuffer, "Unrecognized string \"%s\" found while reading muscle %s.", - p, muscle->name); - error(recover,errorbuffer); - } - } - - /* Now add the wrap structure to the muscle */ - if (wrap) - { - if (muscle->numWrapStructs == 0) - muscle->wrapStruct = (dpMuscleWrapStruct**)simm_malloc(sizeof(dpMuscleWrapStruct*)); - else - muscle->wrapStruct = (dpMuscleWrapStruct**)simm_realloc(muscle->wrapStruct, - (muscle->numWrapStructs+1) * sizeof(dpMuscleWrapStruct*), &rc); - muscle->wrapStruct[muscle->numWrapStructs++] = wrap; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "endmuscle")) - { - return code_fine; - } - else - { - SBoolean matched_string = check_dynamic_param_array(fp, muscle, buffer); - - if (matched_string == no) - { - (void)sprintf(errorbuffer, "Unrecognized string \"%s\" found while reading muscle %s.", - buffer, muscle->name); - error(recover, errorbuffer); - } - } - } - - return code_fine; -} - - -/* This function scans forward to the end of a muscle definition. - * It should be called only after encountering an error in the definition, - * as it skips over everything until "endmuscle" is found. - */ -static void finish_muscle_definition(FILE* fp) -{ - while (1) - { - fscanf(fp, "%s", buffer); - if (STRINGS_ARE_EQUAL(buffer, "endmuscle")) - break; - } -} - - -/* READ_MUSCLE_PARAMETER: this routine reads a single muscle parameter from - * a file. The parameter must be stored as a pointer to a double in the - * muscle structure. If there is an error when reading the parameter, the - * pointer will be set to the corresponding parameter in the default muscle. - */ -static ReturnCode read_muscle_parameter(FILE* fp, dpMuscleStruct* ms, - dpMuscleStruct* default_musc, double** param, - double* def_param, char param_name[], - double min_value, double max_value) -{ - *param = (double *)simm_malloc(sizeof(double)); - if (*param == NULL) - return code_bad; - - if (fscanf(fp, "%lg", *param) != 1) - { - if (ms != default_musc && def_param != NULL) - { - free(*param); - *param = def_param; - (void)sprintf(errorbuffer, "Error reading %s for muscle %s.", - param_name, ms->name); - error(none,errorbuffer); - error(none, "Using value from default muscle."); - return code_fine; - } - else - { - free(*param); - (void)sprintf(errorbuffer, "Error reading %s for muscle %s.", - param_name, ms->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else - { - if (min_value != DONT_CHECK_DOUBLE) - { - if (**param < min_value) - { - sprintf(errorbuffer, "Error reading %s for muscle %s:", - param_name, ms->name); - error(none,errorbuffer); - sprintf(errorbuffer, " value of %lf is below min allowable value (%lf).", - **param, min_value); - error(none,errorbuffer); - free(*param); - return code_bad; - } - } - - if (max_value != DONT_CHECK_DOUBLE) - { - if (**param > max_value) - { - sprintf(errorbuffer, "Error reading %s for muscle %s:\n", - param_name, ms->name); - error(none,errorbuffer); - sprintf(errorbuffer, " value of %lf is above max allowable value (%lf).", - **param, max_value); - error(none,errorbuffer); - free(*param); - return code_bad; - } - } - } - - return code_fine; -} - - - -/* READ_MUSCLE_FUNCTION: this routine reads a function for a muscle - * (e.g. active force-length curve). If there is an error when reading from - * the file, it sets the function equal to the corresponding function in the - * default muscle. - */ -static ReturnCode read_muscle_function(FILE* fp, ModelStruct* model, dpMuscleStruct* muscle, - dpFunction*** mfunc, dpFunction** default_mfunc, - char ending[], char description[]) -{ - dpFunction newFunc; - - *mfunc = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (*mfunc == NULL) - return code_bad; - - malloc_function(&newFunc, FUNCTION_ARRAY_INCREMENT); - - if (read_double_array(fp, ending, description, &newFunc) == code_bad) - { - free_function(&newFunc, no); - free(*mfunc); - - if (muscle != model->default_muscle && default_mfunc != NULL) - { - *mfunc = default_mfunc; - (void)sprintf(errorbuffer, "Error reading %s for muscle %s.", description, muscle->name); - error(none, errorbuffer); - error(none, "Using function from default muscle."); - return code_fine; - } - else - { - (void)sprintf(errorbuffer, "Error reading %s for muscle %s.", description, muscle->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - - newFunc.type = dpNaturalCubicSpline; - newFunc.cutoff_frequency = -1.0; - - calc_function_coefficients(&newFunc); - - *(*mfunc) = load_simm_function(model, &newFunc, yes); - - return code_fine; -} - - -static ReturnCode read_excitation(FILE* fp, ModelStruct* ms, dpMuscleStruct* muscle) -{ - int abscissa = -2; - dpFunction newFunc; - - /* First read the name of the abscissa of the function. It should be either - * the word "time" or the name of a generalized coordinate, of which this - * excitation pattern will be a function. - */ - if (fscanf(fp, "%s", buffer) != 1) - { - (void)sprintf(errorbuffer, "Error reading name of abscissa in excitation function for muscle %s", muscle->name); - error(abort_action, errorbuffer); - return code_bad; - } - - muscle->excitation_abscissa = (void**)simm_malloc(sizeof(void*)); - if (muscle->excitation_abscissa == NULL) - return code_bad; - - if (STRINGS_ARE_EQUAL(buffer, "time")) - { - *muscle->excitation_abscissa = TIME; - } - else - { - *muscle->excitation_abscissa = enter_gencoord(ms, buffer, no); - if (*muscle->excitation_abscissa == NULL) - { - (void)sprintf(errorbuffer, "Error reading abscissa of excitation function for muscle %s", muscle->name); - error(none,errorbuffer); - (void)sprintf(errorbuffer, "%s is neither \"time\" nor the name of a generalized coordinate", buffer); - error(abort_action, errorbuffer); - finish_muscle_definition(fp); - return code_bad; - } - } - - muscle->excitation_func = (dpFunction**)simm_malloc(sizeof(dpFunction*)); - if (muscle->excitation_func == NULL) - return code_bad; - - malloc_function(&newFunc, FUNCTION_ARRAY_INCREMENT); - newFunc.type = excitationType; - newFunc.cutoff_frequency = -1.0; - - if (read_double_array(fp, "endexcitation", "excitation-sequence", &newFunc) == code_bad) - { - free_function(&newFunc, no); - - if (muscle != ms->default_muscle && ms->default_muscle->excitation_func != NULL) - { - muscle->excitation_func = ms->default_muscle->excitation_func; - (void)sprintf(errorbuffer, "Error reading excitation sequence for muscle %s.", muscle->name); - error(none, errorbuffer); - error(none, "Using same sequence as for default muscle."); - return code_fine; - } - else - { - (void)sprintf(errorbuffer, "Error reading excitation sequence for muscle %s.", muscle->name); - error(abort_action, errorbuffer); - return code_bad; - } - } - - calc_function_coefficients(&newFunc); - - *muscle->excitation_func = load_simm_function(ms, &newFunc, yes); - - return code_fine; -} - - -/* INIT_DEFAULT_MUSCLE: this routine should be called after the default muscle - * has been read in. It makes sure that certain parameters are defined so that - * there are always defaults for them (e.g. min_thickness). - */ -ReturnCode init_default_muscle(ModelStruct* ms) -{ - /* If the user did not specify min and max thickness for the default muscle, - * define these two parameters. Then when you read in the other muscles, there - * are defaults already set-up for the muscles to point to. After reading in - * the complete model (bones and all), these default values may be changed - * to better correspond to the particular model. - */ - ms->default_muscle->index = DEFAULT_MUSCLE_INDEX; - ms->default_muscle->display = yes; - - if (ms->default_muscle->min_thickness == NULL) - { - ms->default_muscle->min_thickness = (double*)simm_malloc(sizeof(double)); - if (ms->default_muscle->min_thickness == NULL) - return code_bad; - *ms->default_muscle->min_thickness = 0.002; - ms->specified_min_thickness = no; - } - - if (ms->default_muscle->max_thickness == NULL) - { - ms->default_muscle->max_thickness = (double*)simm_malloc(sizeof(double)); - if (ms->default_muscle->max_thickness == NULL) - return code_bad; - *ms->default_muscle->max_thickness = 0.008; - ms->specified_max_thickness = no; - } - - if (ms->default_muscle->min_material == NULL) - { - ms->default_muscle->min_material = (int*)simm_malloc(sizeof(int)); - *ms->default_muscle->min_material = ms->dis.mat.default_muscle_min_material; - } - - if (ms->default_muscle->max_material == NULL) - { - ms->default_muscle->max_material = (int*)simm_malloc(sizeof(int)); - *ms->default_muscle->max_material = ms->dis.mat.default_muscle_max_material; - } - - return code_fine; -} - - -static ReturnCode read_dynamic_parameter_names(FILE* fp, dpMuscleStruct* muscle) -{ - int array_size = 10; - ReturnCode rc = code_fine; - - muscle->num_dynamic_params = 0; - muscle->dynamic_param_names = (char**)simm_malloc(array_size * sizeof(char*)); - - if (muscle->dynamic_param_names == NULL) - return code_bad; - - while (1) - { - if (fscanf(fp, "%s", buffer) != 1) - { - error(none, "EOF while reading dynamic parameters"); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer, "enddynamicparameters")) - break; - - // Check to see if you need to increase the size of the array before - // reading any more parameter names. - if (muscle->num_dynamic_params >= array_size) - { - array_size += 10; - muscle->dynamic_param_names = (char**)simm_realloc(muscle->dynamic_param_names, - array_size*sizeof(char*), &rc); - - if (rc == code_bad) - return code_bad; - } - - mstrcpy(&muscle->dynamic_param_names[muscle->num_dynamic_params], buffer); - - muscle->num_dynamic_params++; - } - - muscle->dynamic_param_names = (char**)simm_realloc(muscle->dynamic_param_names, - muscle->num_dynamic_params*sizeof(char*), &rc); - - return rc; -} - - -ReturnCode init_dynamic_param_array(dpMuscleStruct* muscle, dpMuscleStruct* default_muscle) -{ - int i; - - muscle->num_dynamic_params = default_muscle->num_dynamic_params; - muscle->dynamic_param_names = default_muscle->dynamic_param_names; - muscle->dynamic_params = (double**)simm_calloc(muscle->num_dynamic_params, sizeof(double*)); - - if (muscle->dynamic_params == NULL) - return code_bad; - - for (i=0; inum_dynamic_params; i++) - muscle->dynamic_params[i] = default_muscle->dynamic_params[i]; - - return code_fine; -} - - -static SBoolean check_dynamic_param_array(FILE* fp, dpMuscleStruct* muscle, char text_string[]) -{ - int i; - - for (i=0; inum_dynamic_params; i++) - { - if (STRINGS_ARE_EQUAL(text_string, muscle->dynamic_param_names[i])) - { - muscle->dynamic_params[i] = (double*)simm_malloc(sizeof(double)); - fscanf(fp, "%lg", muscle->dynamic_params[i]); - return yes; - } - } - - return no; -} - - -static ReturnCode read_ligament(FILE* fp, ModelStruct* ms) -{ - ReturnCode rc = code_fine; - LigamentStruct* lig; - dpMusclePathStruct* ligline; - - if (ms->numligaments == ms->ligament_array_size) - { - ms->ligament_array_size += MUSCLE_ARRAY_INCREMENT; - ms->ligament = (LigamentStruct*)simm_realloc(ms->ligament, - ms->ligament_array_size*sizeof(LigamentStruct),&rc); - if (rc == code_bad) - { - ms->ligament_array_size -= MUSCLE_ARRAY_INCREMENT; - return code_bad; - } - } - - lig = &ms->ligament[ms->numligaments]; - - lig->display = yes; - lig->numlines = 0; - lig->line_array_size = 10; - lig->activation = 1.0; - lig->drawmode = gouraud_shading; - lig->show_normals = no; - - lig->line = (dpMusclePathStruct*)simm_malloc(lig->line_array_size*sizeof(dpMusclePathStruct)); - if (lig->line == NULL) - return code_bad; - - if (fscanf(fp, "%s", buffer) != 1) - { - error(abort_action, "Error starting a ligament definition."); - return code_bad; - } - else - { - mstrcpy(&lig->name, buffer); - } - - while (1) - { - if (read_string(fp, buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer, "beginpoints")) - { - if (lig->numlines == lig->line_array_size) - { - lig->line_array_size += 10; - lig->line = (dpMusclePathStruct*)simm_realloc(lig->line, lig->line_array_size*sizeof(dpMusclePathStruct), &rc); - if (rc == code_bad) - { - lig->line_array_size -= 10; - return code_bad; - } - } - ligline = &lig->line[lig->numlines]; - if (read_muscle_path(ms, fp, ligline) == code_bad) - { - (void)sprintf(errorbuffer, "Error reading coordinates for muscle surface %s.", lig->name); - error(abort_action, errorbuffer); - return code_bad; - } - else - { - post_init_muscle_path(ligline, 0); //TODO5.0 once ligaments support wrap objects, change '0' to lig->numWrapStructs - } - lig->numlines++; - } - else if (STRINGS_ARE_EQUAL(buffer, "endmusclesurface") || STRINGS_ARE_EQUAL(buffer, "endligament")) - { - break; - } - else - { - (void)sprintf(errorbuffer, "Unrecognized string \"%s\" found while reading muscle surface %s.", buffer, lig->name); - error(recover, errorbuffer); - } - } - - ms->numligaments++; - - return code_fine; -} - - -/* set the array of mp points in the musclepoints array struct */ -ReturnCode post_init_muscle_path(dpMusclePathStruct* path, int numWrapStructs) -{ - if (path == NULL) - return code_fine; - - path->num_points = 0; - path->mp_array_size = path->num_orig_points + 2 * numWrapStructs; - path->mp = (dpMusclePoint**)simm_malloc(path->mp_array_size * sizeof(dpMusclePoint*)); - if (path->mp == NULL) - { - error(none, "Ran out of memory."); - return code_bad; - } - return code_fine; -} diff --git a/OpenSim/Utilities/simmToOpenSim/readtools.c b/OpenSim/Utilities/simmToOpenSim/readtools.c deleted file mode 100644 index d8aab68b2b..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/readtools.c +++ /dev/null @@ -1,2270 +0,0 @@ -/******************************************************************************* - - READTOOLS.C - - Author: Peter Loan - - Date: 8-DEC-88 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains tools for reading ascii text from - input files, and for extracting numbers and strings from the text. - - Routines: - preprocess_file : runs cpp on a file, then opens it - read_string : reads a string from a file - read_nonempty_line : reads a non-empty line of characters from a file - read_line : reads a possibly empty line from a file - get_groups : reads a set of muscle group names from muscle input file - getcoords : reads muscle points from the muscle input file - getdoublearray : reads an array of x-y pairs from a file - check_for_closing_paren : - get_xy_pair_from_string : reads x-y pair, format: ( 0.00 , 0.00 ) - getintpair : reads a pair of integers from a file, format: ( 0 , 0 ) - parse_string : parses a string using the specified format (%s, %f, ...) - get_stringpair : reads a pair of text strings from a file - name_is_gencoord : checks to see if a name is a gencoord name - -*******************************************************************************/ - -#include -#include -#include - -#include "universal.h" - -#include "globals.h" -#include "functions.h" -#include "defunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ -char natural_cubic_text[] = "_spl"; -char gcv_text[] = "_gcv"; -char linear_text[] = "_lin"; - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static SBoolean string_contains_closing_paren(char str_buffer[]); -static char* get_xypair_from_string(char str_buffer[], double* x, double* y); -static char* parse_string(char str_buffer[], VariableType var_type, - void* dest_var); -static void acpp(char in_file[], const char out_file[]); - - - - -/* PREPROCESS_FILE: this routine runs the C preprocessor on the specified - * input file, putting the output in the specified output file. It then - * opens the processed file and returns a pointer to a FILE structure for it. - */ - -FILE* preprocess_file(char infile[], const char outfile[]) -{ - - FILE* fp; - - if (file_exists(infile) == no) - return NULL; - - acpp(infile, outfile); - -#ifndef WIN32 - /* Change the protections so that anyone can overwrite this temporary file. - * Technically this is not needed, because the file is deleted right after - * it is closed, but I'm leaving it here anyway in case the file is not - * deleted properly. - */ - chmod(outfile, 438); -#endif - - fp = simm_fopen(outfile, "r"); - - if (fp == NULL) - perror("Unable to open acpp output"); - - return fp; -} - - - -/* READ_STRING: this routine reads a character string from a file. It skips over - * any white space at the beginning, and returns EOF if it hits the end of - * the file before completing the string. - */ -int read_string(FILE* fp, char str_buffer[]) -{ - int c; - - // Scan thru white space until you find the first character in the string. - while (1) - { - c = getc(fp); - if (c == EOF) - return EOF; - *str_buffer = c; - if (CHAR_IS_NOT_WHITE_SPACE(*str_buffer)) - break; - } - str_buffer++; - - // Now read characters until you find white space or EOF. - while (1) - { - c = getc(fp); - if (c == EOF) - return EOF; - *str_buffer = c; - if (CHAR_IS_WHITE_SPACE(*str_buffer)) - break; - str_buffer++; - } - - // Null-terminate the string. - *str_buffer = STRING_TERMINATOR; - - // You found a valid string without hitting EOF, so return a value that you - // know will never equal EOF, no matter how EOF is defined. - return EOF + 1; -} - - - -/* READ_LINE: reads a line (possibly empty) from a file */ - -int read_line(FILE* fp, char str_buffer[]) -{ - int c; - - /* Read characters until you hit a carriage return or EOF */ - while (1) - { - c = getc(fp); - if (c == EOF) - return EOF; - *str_buffer = c; - if (c == LINE_FEED) - break; - if (*str_buffer == CARRIAGE_RETURN) - break; - str_buffer++; - } - - /* Null-terminate the string */ - *str_buffer = STRING_TERMINATOR; - - /* You found a valid line without hitting EOF, so return a value that you - * know will never equal EOF, no matter how EOF is defined. - */ - return EOF + 1; -} - -/* COUNTTOKENS: returns the number of tokens in the string. */ -int countTokens(char str[]) -{ - int count = 0, previousWasSeparator = 1; - char* p = str; - - while (*p != STRING_TERMINATOR) - { - if (*p == ' ' || *p == '\t' || *p == '\n' || *p == '\r') - { - previousWasSeparator = 1; - } - else - { - if (previousWasSeparator) - count++; - previousWasSeparator = 0; - } - p++; - } - - return count; -} - - -/* READ_NONEMPTY_LINE: Reads the first non-empty line from a file. */ - -int read_nonempty_line(FILE* fp, char str_buffer[]) -{ - int c; - - *str_buffer = STRING_TERMINATOR; - - // Scan thru the white space until you find the first character. - while (1) - { - c = getc(fp); - if (c == EOF) - return EOF; - *str_buffer = c; - if (*str_buffer != SPACE && *str_buffer != TAB && - *str_buffer != CARRIAGE_RETURN && *str_buffer != '\r' && c != LINE_FEED) - break; - } - str_buffer++; - - // Now read characters until you find a carriage return or EOF. - while (1) - { - c = getc(fp); - if (c == EOF) - { - *str_buffer = STRING_TERMINATOR; - return EOF; - } - *str_buffer = c; - if (*str_buffer == CARRIAGE_RETURN || c == LINE_FEED) - break; - str_buffer++; - } - - *str_buffer = STRING_TERMINATOR; - - // You found a valid line without hitting EOF, so return a value that you - // know will never equal EOF, no matter how EOF is defined. - return EOF + 1; -} - - -/* READ_MUSCLE_GROUPS: This routine reads names of muscle groups from a file - * until the string "endgroups" is found. For each name it reads, it enters the - * name into the group-name database for the model. It then stores the - * database-index of this name in an array of muscle-group indices for this - * muscle. When done, this routine returns the array of group indices. - */ -int* read_muscle_groups(ModelStruct* ms, FILE* fp, int* num_groups, int muscle_number) -{ - int num_malloced_so_far = 50; - int* group_indices; - ReturnCode rc = code_fine; - - *num_groups = 0; - - group_indices = (int*)simm_malloc(num_malloced_so_far*sizeof(int)); - if (group_indices == NULL) - return NULL; - - while (1) - { - if (fscanf(fp, "%s", buffer) != 1) - return NULL; - - if (STRINGS_ARE_EQUAL(buffer, "endgroups")) - { - // rc should be code_fine since you're reallocing a smaller size. - group_indices = (int*)simm_realloc(group_indices, (*num_groups)*sizeof(int), &rc); - return group_indices; - } - - if (STRINGS_ARE_EQUAL(buffer, "all")) - { - error(none,"You cannot define a muscle group named \"all.\""); - error(none,"SIMM automatically creates a muscle group containing all muscles."); - } - else - { - int i, groupIndex = enter_muscle_group(ms, buffer, muscle_number); - - if (groupIndex == -1) - return NULL; - - // Make sure this group has not already been specified for this muscle. - for (i=0; i<*num_groups; i++) - { - if (groupIndex == group_indices[i]) - break; - } - if (i == *num_groups) - { - if (*num_groups > num_malloced_so_far) - { - num_malloced_so_far += 50; - group_indices = (int*)simm_realloc(group_indices, num_malloced_so_far*sizeof(int), &rc); - if (rc == code_bad) - return NULL; - } - - group_indices[*num_groups] = groupIndex; - (*num_groups)++; - } - } - } - - return NULL; -} - - -/* READ_MUSCLE_PATH: this routine reads muscle attachment points - * from a file. It keeps reading from the file until the string "endpoints" - * is encountered. It initially mallocs space for some points, and if it - * fills up that space and finds more points, it reallocs the muscle point - * array so that it can add the additional points. - */ -ReturnCode read_muscle_path(ModelStruct* ms, FILE* fp, dpMusclePathStruct* path) -{ - int numpoints, function_num; - dpMusclePoint* mp; - char *line; - char gencoord_name[CHARBUFFER], line2[CHARBUFFER], segment_name[CHARBUFFER], com[CHARBUFFER]; - double range1, range2; - ReturnCode rc = code_fine; - - // initialize the muscle path - if (initMusclePath(path) == code_bad) - return code_bad; - - numpoints = 0; - while (1) - { - if (fscanf(fp, "%s", buffer) != 1) - { - error(none, "End of file reached while reading muscle attachment points"); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer, "endpoints")) - { - path->num_orig_points = numpoints; - return code_fine; - } - - /* Check to see if you need to increase the size of the muscle-point - * array before reading any more points. - */ - if (numpoints >= path->mp_orig_array_size) - { - path->mp_orig_array_size += MUSCLEPOINT_ARRAY_INCREMENT; - - path->mp_orig = (dpMusclePoint*)simm_realloc(path->mp_orig, - path->mp_orig_array_size*sizeof(dpMusclePoint),&rc); - if (rc == code_bad) - { - path->mp_orig_array_size -= MUSCLEPOINT_ARRAY_INCREMENT; - return rc; - } - } - - mp = &path->mp_orig[numpoints]; - // initialize the point - init_musclepoint(mp); - - /* If the string you just read was not "endpoints" then it must - * be the start of a new point (so it's the x-coordinate), either - * a number (e.g. "0.0346"), or a function (e.g. "f2(gencoord_name)"). - */ - if (sscanf(buffer,"%lg", &mp->point[XX]) != 1) - { - if (sscanf(buffer, "f%d%s", &function_num, gencoord_name) != 2) - return code_bad; - strip_brackets_from_string(gencoord_name); - mp->function[XX] = ms->function[enter_function(ms, function_num, yes)]; - mp->gencoord[XX] = enter_gencoord(ms, gencoord_name, no); - mp->isMovingPoint = yes; - if (mp->gencoord[XX] == NULL) - { - (void)sprintf(errorbuffer, "Gencoord %s referenced in muscle file but not defined in joint file.", gencoord_name); - error(none,errorbuffer); - return code_bad; - } - if (mp->function[XX] == NULL) - { - (void)sprintf(errorbuffer, "Function %d referenced in muscle file but not defined in joint or muscle file.", function_num); - error(none,errorbuffer); - return code_bad; - } - } - - (void)read_nonempty_line(fp,line2); - - /* Read the y coordinate or function */ - line = parse_string(line2,type_string,(void*)buffer); - if (sscanf(buffer,"%lg", &mp->point[YY]) != 1) - { - if (sscanf(buffer,"f%d%s", &function_num, gencoord_name) != 2) - return code_bad; - strip_brackets_from_string(gencoord_name); - mp->function[YY] = ms->function[enter_function(ms, function_num, yes)]; - mp->gencoord[YY] = enter_gencoord(ms, gencoord_name, no); - mp->isMovingPoint = yes; - if (mp->gencoord[YY] == NULL) - { - (void)sprintf(errorbuffer, "Gencoord %s referenced in muscle file but not defined in joint file.", gencoord_name); - error(none,errorbuffer); - return code_bad; - } - if (mp->function[YY] == NULL) - { - (void)sprintf(errorbuffer, "Function %d referenced in muscle file but not defined in joint or muscle file.", function_num); - error(none,errorbuffer); - return code_bad; - } - } - else - { - if (mp->point[YY] == ERROR_DOUBLE) - return code_bad; - } - - /* Read the z coordinate or function */ - line = parse_string(line,type_string,(void*)buffer); - if (sscanf(buffer,"%lg", &mp->point[ZZ]) != 1) - { - if (sscanf(buffer,"f%d%s", &function_num, gencoord_name) != 2) - return code_bad; - strip_brackets_from_string(gencoord_name); - mp->function[ZZ] = ms->function[enter_function(ms, function_num, yes)]; - mp->gencoord[ZZ] = enter_gencoord(ms, gencoord_name, no); - mp->isMovingPoint = yes; - if (mp->function[ZZ] == NULL) - { - (void)sprintf(errorbuffer, "Function %d referenced in muscle file but not defined in joint file.", function_num); - error(none,errorbuffer); - return code_bad; - } - if (mp->gencoord[ZZ] == NULL) - { - (void)sprintf(errorbuffer, "Gencoord %s referenced in muscle file but not defined in joint or muscle file.", gencoord_name); - error(none,errorbuffer); - return code_bad; - } - } - else - { - if (mp->point[ZZ] == ERROR_DOUBLE) - return code_bad; - } - - /* read the keyword "segment" */ - line = parse_string(line,type_string,(void*)buffer); - - /* read the segment name */ - line = parse_string(line,type_string,(void*)segment_name); - if (segment_name == NULL) - return code_bad; - - /* read a string from the leftover part of the line. For most points, - * the leftover will be NULL. For wrapping points, the first string - * in the leftover should be "range". - */ - line = parse_string(line,type_string,(void*)com); - - if (STRINGS_ARE_EQUAL(com,"range")) // new keyword - { - line = parse_string(line,type_string,(void*)gencoord_name); - if (gencoord_name == NULL || name_is_gencoord(gencoord_name, ms, NULL, NULL, NULL, no) == NULL) - { - return code_bad; - } - - line = get_xypair_from_string(line, &range1, &range2); - if (line == NULL) - { - return code_bad; - } - else - { - mp->viaRange.start = _MIN(range1, range2); - mp->viaRange.end = _MAX(range1, range2); - } - mp->viaRange.gencoord = enter_gencoord(ms, gencoord_name, no); - if (mp->viaRange.gencoord == NULL) - { - (void)sprintf(errorbuffer,"Gencoord %s referenced in muscle file but not defined in joints file.", gencoord_name); - error(none,errorbuffer); - return code_bad; - } - mp->isVia = yes; - } - // support old files - if (STRINGS_ARE_EQUAL(com,"ranges")) - { - int i, temp = 0; - line = parse_string(line, type_int, (void*)&temp); - if (temp <= 0) - { - error(none,"The number of ranges must be greater than 0."); - return code_bad; - } - if (temp > 1) - { - error(none,"Multiple ranges no longer supported."); - return code_bad; - } - - for (i=0; iviaRange.start = _MIN(range1, range2); - mp->viaRange.end = _MAX(range1, range2); - mp->viaRange.gencoord = enter_gencoord(ms, gencoord_name, no); - mp->isVia = yes; - } - } - } - - mp->segment = enter_segment(ms, segment_name, no); - if (mp->segment == -1) - { - (void)sprintf(errorbuffer,"Segment %s referenced in muscle file but not defined in joints file.", segment_name); - error(none,errorbuffer); - return code_bad; - } - - mp->undeformed_point[0] = mp->point[0]; - mp->undeformed_point[1] = mp->point[1]; - mp->undeformed_point[2] = mp->point[2]; - - numpoints++; - } - - return code_fine; -} - -/* --------------------------------------------------------------------------- - count_remaining_lines - scan to the end of the file, counting the number - of lines remaining. ------------------------------------------------------------------------------- */ -public int count_remaining_lines (FILE* file, SBoolean countEmptyLines) -{ - SBoolean lineHasContent = no; - - long fpos = ftell(file); - - int c, n = 0; - - for (c = fgetc(file); c != EOF; c = fgetc(file)) - { - if (isgraph(c)) - lineHasContent = yes; - - if (c == '\n') - { - if (lineHasContent || countEmptyLines) - n++; - - lineHasContent = no; - } - } - if (lineHasContent) - n++; - - fseek(file, fpos, SEEK_SET); - - return n; -} - - -/* READ_DOUBLE_ARRAY: this routine reads an array of pairs-of-doubles - * (e.g. "(2.30, -0.05)"), as in the definition of a function. It keeps - * reading until it encounters the ending string (e.g. "endfunction") - * which is passed in. This routine is not trivial because it allows - * spaces to be placed liberally in the string that it extracts the - * doubles from (e.g. "( 2.30 , -0.05 )"). - */ -ReturnCode read_double_array(FILE* fp, const char ending[], const char name[], dpFunction* func) -{ - - int new_size; - - func->numpoints = 0; - - while (1) - { - if (read_string(fp,buffer) == EOF) - { - (void)sprintf(errorbuffer,"Unexpected EOF reading function %s.", name); - error(abort_action,errorbuffer); - return code_bad; - } - - if (STRINGS_ARE_EQUAL(buffer,ending)) - break; - - /* If the string just read is not the ending string (e.g. "endfunction"), - * then it must be part or all of an x-y pair "(x,y)". It cannot be more - * than one x-y pair because there must be a space between each x-y pair. - * Since there can be spaces within an x-y pair, you now want to continue - * reading strings from the file until you encounter a closing parenthesis. - */ - - /* Before parsing this next string, first make sure that there is enough - * space in the function structure for the next x-y pair. - */ - - if (func->numpoints == func->coefficient_array_size) - { - new_size = func->coefficient_array_size + FUNCTION_ARRAY_INCREMENT; - if (realloc_function(func,new_size) == code_bad) - return code_bad; - } - - while (1) - { - if (string_contains_closing_paren(buffer) == yes) - break; - - if (read_string(fp,&buffer[strlen(buffer)]) == EOF) - { - (void)sprintf(errorbuffer,"Unexpected EOF reading function %s.", name); - error(abort_action,errorbuffer); - return code_bad; - } - } - - if (get_xypair_from_string(buffer,&func->x[func->numpoints], - &func->y[func->numpoints]) == NULL) - { - (void)sprintf(errorbuffer,"Error reading x-y pair in function %s.", name); - error(abort_action,errorbuffer); - return code_bad; - } - - (func->numpoints)++; - } - - if (func->numpoints < 2) - { - (void)sprintf(errorbuffer,"Function %s contains fewer than 2 points.", name); - error(abort_action,errorbuffer); - return code_bad; - } - - return code_fine; - -} - - - -/* STRING_CONTAINS_CLOSING_PAREN: this routine scans a string to see if - * it contains a closing parenthesis. - */ - -static SBoolean string_contains_closing_paren(char str_buffer[]) -{ - - while (*str_buffer != STRING_TERMINATOR) - { - if (*(str_buffer++) == ')') - return (yes); - } - - return (no); - -} - - - -/* GET_XYPAIR_FROM_STRING: this routine parses a string to extract a pair - * of doubles from it. The string should be of the form: "(x, y)". - */ - -static char* get_xypair_from_string(char str_buffer[], double* x, double* y) -{ - - char junk; - char* buffer_ptr; - - *x = *y = ERROR_DOUBLE; - - buffer_ptr = parse_string(str_buffer,type_char,(void*)&junk); /* open paren */ - buffer_ptr = parse_string(buffer_ptr,type_double,(void*)x); /* x coord */ - buffer_ptr = parse_string(buffer_ptr,type_char,(void*)&junk); /* comma */ - buffer_ptr = parse_string(buffer_ptr,type_double,(void*)y); /* y coord */ - - buffer_ptr = parse_string(buffer_ptr,type_char,(void*)&junk); /* close paren*/ - - if (*x == ERROR_DOUBLE || *y == ERROR_DOUBLE) - return (NULL); - - return (buffer_ptr); - -} - - - -/* PARSE_STRING: this routine scans a string and extracts a variable from - * it. The type of variable that it extracts is specified in one of the - * arguments. It returns the unused portion of the string so that more - * variables can be extracted from it. - */ - -static char* parse_string(char str_buffer[], VariableType var_type, void* dest_var) -{ - - char tmp_buffer[CHARBUFFER], *buffer_ptr; - - if (str_buffer == NULL) - return (NULL); - - buffer_ptr = tmp_buffer; - - while (CHAR_IS_WHITE_SPACE(*str_buffer)) - str_buffer++; - - if (var_type == type_char) - { - *((char*)dest_var) = *str_buffer; - return (str_buffer+1); - } - - if (var_type == type_string) - { - if (STRING_IS_NULL(str_buffer)) - *((char*)dest_var) = STRING_TERMINATOR; - else - (void)sscanf(str_buffer,"%s", (char*)dest_var); - return (str_buffer + strlen((char*)dest_var)); - } - - if (var_type == type_double) - { - *((double*)dest_var) = ERROR_DOUBLE; - if (STRING_IS_NOT_NULL(str_buffer)) - { - while (*str_buffer == '-' || *str_buffer == '.' || *str_buffer == '+' || - *str_buffer == 'e' || *str_buffer == 'E' || - *str_buffer == 'd' || *str_buffer == 'D' || - (*str_buffer >= '0' && *str_buffer <= '9')) - *(buffer_ptr++) = *(str_buffer++); - *buffer_ptr = STRING_TERMINATOR; - if (sscanf(tmp_buffer,"%lg",(double*)dest_var) != 1) - *((double*)dest_var) = ERROR_DOUBLE; - } - return (str_buffer); - } - - if (var_type == type_int) - { - *((int*)dest_var) = ERRORINT; - if (STRING_IS_NOT_NULL(str_buffer)) - { - while (*str_buffer == '-' || (*str_buffer >= '0' && *str_buffer <= '9')) - *(buffer_ptr++) = *(str_buffer++); - *buffer_ptr = STRING_TERMINATOR; - if (sscanf(tmp_buffer,"%d",(int*)dest_var) != 1) - *((int*)dest_var) = ERRORINT; - } - return (str_buffer); - } - - (void)sprintf(errorbuffer,"Unknown variable type (%d) passed to parse_string().", - (int)var_type); - error(none,errorbuffer); - - return (NULL); - -} - - - -/* GET_STRING_PAIR: this routine takes a string of the form "(str1,str2)", - * which can have spaces between any of the 5 components, and returns the - * two internal strings, str1 and str2. - */ - -ReturnCode get_string_pair(char str_buffer[], char str1[], char str2[]) -{ - - /* scan past the opening parenthesis */ - - while (*str_buffer != '(' && *str_buffer != STRING_TERMINATOR) - str_buffer++; - str_buffer++; - - if (STRING_IS_NULL(str_buffer)) - return code_bad; - - /* scan upto the start of str1 */ - - while (*str_buffer == SPACE || *str_buffer == TAB) - str_buffer++; - - if (STRING_IS_NULL(str_buffer)) - return code_bad; - - /* copy the first string to the str1 array */ - - while (*str_buffer != ',' && *str_buffer != SPACE && *str_buffer != TAB) - *str1++ = *str_buffer++; - *str1 = STRING_TERMINATOR; - - /* scan upto the start of str2 */ - - while (*str_buffer == ',' || *str_buffer == SPACE || *str_buffer == TAB) - str_buffer++; - - if (STRING_IS_NULL(str_buffer)) - return code_bad; - - /* copy the second string to the str2 array */ - - while (*str_buffer != ')' && *str_buffer != SPACE && *str_buffer != TAB) - *str2++ = *str_buffer++; - *str2 = STRING_TERMINATOR; - - return code_fine; - -} - - -/* NAME_IS_GENCOORD: this function checks to see if a string is the name - * of one of the model's gencoords, with an optional suffix and also - * possibly the extra suffix "_spl" or "_gcv" (possibly followed by a cutoff - * frequency). It returns the index of the gencoord if there is a match. - * If stripEnd is yes, the optional functionType and cutoff frequency are - * removed from the name if a match is made. - */ -GeneralizedCoord* name_is_gencoord(char name[], ModelStruct* ms, char suffix[], - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd) -{ - int i, len, lenQ, Qnum, index, maxLen; - char *ptr, *newEnd; - - len = strlen(name); - - /* First check to see if the string begins with the name of a gencoord. - * To handle models which have overlapping gencoord names, like - * "q1" and "q10", check for a match with all gencoords, and take - * the longest match. - */ - for (i = 0, index = -1, maxLen = -1; i < ms->numgencoords; i++) - { - lenQ = strlen(ms->gencoord[i]->name); - if (len >= lenQ) - { - if (!strncmp(name, ms->gencoord[i]->name, lenQ)) - { - if (lenQ > maxLen) - { - index = i; - maxLen = lenQ; - } - } - } - } - - if (index == -1) - return NULL; - - /* You've found a matching gencoord name, so move ptr past the name and - * get ready to check the suffixes. - */ - Qnum = index; - ptr = &name[maxLen]; - len -= maxLen; - - /* If a suffix was passed in, check to see if the name ends in that suffix. - * If it does, remove the suffix and continue on. If it does not, return NULL - * because the passed-in suffix must be in the name. - */ - if (suffix) - { - int suffix_len = strlen(suffix); - - if (len >= suffix_len) - { - if (!strncmp(ptr, suffix, suffix_len)) - { - ptr += suffix_len; - len -= suffix_len; - } - else - { - return NULL; - } - } - else - { - return NULL; - } - } - - /* Store a pointer to the character right after the name (plus suffix, if specified). - * This will be the new end of the string if stripEnd == yes. - */ - newEnd = ptr; - - /* If functionType and cutoffFrequency are not NULL, check to see if the name ends in - * natural_cubic_text or gcv_text (followed by an optional cutoff frequency). If it - * does, set *functionType to the appropriate type. If no function label is found, set - * the type to step_func. - */ - if (functionType && cutoffFrequency) - { - int matched_spl = 0, matched_lin = 0; - int lin_len = strlen(linear_text); - int spl_len = strlen(natural_cubic_text); - int gcv_len = strlen(gcv_text); - - *functionType = dpStepFunction; - *cutoffFrequency = -1.0; // by default there is no smoothing - - if (len >= lin_len) - { - if (!strncmp(ptr, linear_text, lin_len)) - { - *functionType = dpLinearFunction; - ptr += lin_len; - len -= lin_len; - matched_lin = 1; - } - } - - if (!matched_lin && len >= spl_len) - { - if (!strncmp(ptr, natural_cubic_text, spl_len)) - { - *functionType = dpNaturalCubicSpline; - ptr += spl_len; - len -= spl_len; - matched_spl = 1; - } - } - - if (!matched_lin && !matched_spl && len >= gcv_len) - { - if (!strncmp(ptr, gcv_text, gcv_len)) - { - ptr += gcv_len; - len -= gcv_len; - *functionType = dpGCVSpline; - if (len > 0) - { - char* intPtr = buffer; - - /* Move over the underscore and look for an integer. */ - if (*(ptr++) != '_') - { - return NULL; - } - else - { - len--; /* for the underscore character */ - while (*ptr >= '0' && *ptr <= '9') - { - *(intPtr++) = *(ptr++); - len--; - } - *intPtr = STRING_TERMINATOR; - *cutoffFrequency = atof(buffer); - } - } - } - } - } - - /* If there are extra characters after the suffixes, return an error. */ - if (len > 0) - return NULL; - - /* Strip off the text for the function type and cutoff frequency. */ - if (stripEnd == yes) - *newEnd = STRING_TERMINATOR; - - return ms->gencoord[Qnum]; -} - - -/* NAME_IS_BODY_SEGMENT: this function checks to see if a string is the name - * of one of the model's body segments. The name can be followed by the name of - * any of the model's motion objects, plus an animation component (e.g., "_px" or "_vy"). - * Also, the string can contain a suffix of "_gcv" or "_spl", possibly followed - * by a cutoff frequency. It returns the index of the body segment if there is - * a match. If stripEnd is yes, the optional functionType and cutoff frequency are - * removed from the name if a match is made. - */ -int name_is_body_segment(ModelStruct* ms, char name[], int* motion_object, int* component, - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd) -{ - int i, index, len, lenS, maxLen, Snum; - char *ptr, *newEnd; - - len = strlen(name); - - /* First check to see if the string begins with the name of a segment. - * To handle models which have overlapping segment names, like - * "femur" and "femur1", check for a match with all segments, and take - * the longest match. - */ - for (i = 0, index = -1, maxLen = -1; i < ms->numsegments; i++) - { - lenS = strlen(ms->segment[i].name); - if (len >= lenS) - { - if (!strncmp(name, ms->segment[i].name, lenS)) - { - if (lenS > maxLen) - { - index = i; - maxLen = lenS; - } - } - } - } - - if (index == -1) - return -1; - - /* You've found a matching segment name, so move ptr past the name and - * get ready to check the suffixes. - */ - Snum = index; - ptr = &name[maxLen]; - len -= maxLen; - - if (motion_object) - { - int lenM; - *motion_object = -1; - - /* The motion object name must be preceded by an underscore. */ - if (*ptr == '_') - { - /* Temporarily move past the underscore. */ - ptr++; - len--; - - /* Find the first motion object which matches the name. - * This suffix is optional, so do not return if there is no match. - */ - for (i = 0; i < ms->num_motion_objects; i++) - { - lenM = strlen(ms->motion_objects[i].name); - if (len >= lenM) - { - if (!strncmp(ptr, ms->motion_objects[i].name, lenM)) - break; - } - } - if (i < ms->num_motion_objects) - { - *motion_object = i; - ptr += lenM; - len -= lenM; - } - else - { - /* Move backwards over the underscore. */ - ptr--; - len++; - } - } - } - - if (component) - { - *component = -1; - - /* Check to see if a component (e.g., "_px") is next in the string. - * This suffix is optional, and independent of the motion object name suffix. - */ - if (!strncmp(ptr, "_tx", 3) || !strncmp(ptr, "_px", 3)) - *component = MO_TX; - else if (!strncmp(ptr, "_ty", 3) || !strncmp(ptr, "_py", 3)) - *component = MO_TY; - else if (!strncmp(ptr, "_tz", 3) || !strncmp(ptr, "_pz", 3)) - *component = MO_TZ; - else if (!strncmp(ptr, "_vx", 3)) - *component = MO_VX; - else if (!strncmp(ptr, "_vy", 3)) - *component = MO_VY; - else if (!strncmp(ptr, "_vz", 3)) - *component = MO_VZ; - else if (!strncmp(ptr, "_sx", 3)) - *component = MO_SX; - else if (!strncmp(ptr, "_sy", 3)) - *component = MO_SY; - else if (!strncmp(ptr, "_sz", 3)) - *component = MO_SZ; - else if (!strncmp(ptr, "_cr", 3)) - *component = MO_CR; - else if (!strncmp(ptr, "_cg", 3)) - *component = MO_CG; - else if (!strncmp(ptr, "_cb", 3)) - *component = MO_CB; - else if (!strncmp(ptr, "_x", 2)) - *component = MO_X; - else if (!strncmp(ptr, "_y", 2)) - *component = MO_Y; - else if (!strncmp(ptr, "_z", 2)) - *component = MO_Z; - - if (*component >= MO_TX && *component <= MO_CB) - { - ptr += 3; - len -= 3; - } - else if (*component >= MO_X && *component <= MO_Z) - { - ptr += 2; - len -= 2; - } - } - - /* Store a pointer to the character right after the part of the name processed - * so far. This will be the new end of the string if stripEnd == yes. - */ - newEnd = ptr; - - /* If functionType and cutoffFrequency are not NULL, check to see if the name ends in - * natural_cubic_text or gcv_text (followed by an optional cutoff frequency). If it - * does, set *functionType to the appropriate type. If no function label is found, set - * the type to step_func. - */ - if (functionType && cutoffFrequency) - { - int matched_spl = 0, matched_lin = 0; - int lin_len = strlen(linear_text); - int spl_len = strlen(natural_cubic_text); - int gcv_len = strlen(gcv_text); - - *functionType = dpStepFunction; - *cutoffFrequency = -1.0; // by default there is no smoothing - - if (len >= lin_len) - { - if (!strncmp(ptr, linear_text, lin_len)) - { - *functionType = dpLinearFunction; - ptr += lin_len; - len -= lin_len; - matched_lin = 1; - } - } - - if (!matched_lin && len >= spl_len) - { - if (!strncmp(ptr, natural_cubic_text, spl_len)) - { - *functionType = dpNaturalCubicSpline; - ptr += spl_len; - len -= spl_len; - matched_spl = 1; - } - } - - if (!matched_lin && !matched_spl && len >= gcv_len) - { - if (!strncmp(ptr, gcv_text, gcv_len)) - { - ptr += gcv_len; - len -= gcv_len; - *functionType = dpGCVSpline; - if (len > 0) - { - char* intPtr = buffer; - - /* Move over the underscore and look for an integer. */ - if (*(ptr++) != '_') - { - return -1; - } - else - { - len--; /* for the underscore character */ - while (*ptr >= '0' && *ptr <= '9') - { - *(intPtr++) = *(ptr++); - len--; - } - *intPtr = STRING_TERMINATOR; - *cutoffFrequency = atof(buffer); - } - } - } - } - } - - /* If there are extra characters after the suffixes, return an error. */ - if (len > 0) - return -1; - - /* Strip off the text for the function type and cutoff frequency. */ - if (stripEnd == yes) - *newEnd = STRING_TERMINATOR; - - return Snum; -} - - -/* NAME_IS_MUSCLE: this function checks to see if a string is the name - * of one of the model's muscles, with an optional suffix and also - * possibly the extra suffix "_spl" or "_gcv" (possibly followed by a cutoff - * frequency). It returns the index of the muscle if there is a match. - * If stripEnd is yes, the optional functionType and cutoff frequency are - * removed from the name if a match is made. - */ -int name_is_muscle(ModelStruct* ms, char name[], char suffix[], - dpFunctionType* functionType, double* cutoffFrequency, SBoolean stripEnd) -{ - int i, len, lenM, Mnum, index, maxLen; - char *ptr, *newEnd; - - len = strlen(name); - - /* First check to see if the string begins with the name of a muscle. - * To handle models which have overlapping muscle names, like - * "trap1" and "trap10", check for a match with all muscles, and take - * the longest match. - */ - for (i = 0, index = -1, maxLen = -1; i < ms->nummuscles; i++) - { - lenM = strlen(ms->muscle[i]->name); - if (len >= lenM) - { - if (!strncmp(name, ms->muscle[i]->name, lenM)) - { - if (lenM > maxLen) - { - index = i; - maxLen = lenM; - } - } - } - } - - if (index == -1) - return -1; - - /* You've found a matching muscle name, so move ptr past the name and - * get ready to check the suffixes. - */ - Mnum = index; - ptr = &name[maxLen]; - len -= maxLen; - - /* If a suffix was passed in, check to see if the name ends in that suffix. - * If it does, remove the suffix and continue on. If it does not, return -1 - * because the suffix is not optional. - */ - if (suffix) - { - int suffix_len = strlen(suffix); - - if (len >= suffix_len) - { - if (!strncmp(ptr, suffix, suffix_len)) - { - ptr += suffix_len; - len -= suffix_len; - } - else - { - return -1; - } - } - else - { - return -1; - } - } - - /* Store a pointer to the character right after the name (plus suffix, if specified). - * This will be the new end of the string if stripEnd == yes. - */ - newEnd = ptr; - - /* If functionType and cutoffFrequency are not NULL, check to see if the name ends in - * natural_cubic_text or gcv_text (followed by an optional cutoff frequency). If it - * does, set *functionType to the appropriate type. If no function label is found, set - * the type to step_func. - */ - if (functionType && cutoffFrequency) - { - int matched_spl = 0, matched_lin = 0; - int lin_len = strlen(linear_text); - int spl_len = strlen(natural_cubic_text); - int gcv_len = strlen(gcv_text); - - *functionType = dpStepFunction; - *cutoffFrequency = -1.0; // by default there is no smoothing - - if (len >= lin_len) - { - if (!strncmp(ptr, linear_text, lin_len)) - { - *functionType = dpLinearFunction; - ptr += lin_len; - len -= lin_len; - matched_lin = 1; - } - } - - if (!matched_lin && len >= spl_len) - { - if (!strncmp(ptr, natural_cubic_text, spl_len)) - { - *functionType = dpNaturalCubicSpline; - ptr += spl_len; - len -= spl_len; - matched_spl = 1; - } - } - - if (!matched_lin && !matched_spl && len >= gcv_len) - { - if (!strncmp(ptr, gcv_text, gcv_len)) - { - ptr += gcv_len; - len -= gcv_len; - *functionType = dpGCVSpline; - if (len > 0) - { - char* intPtr = buffer; - - /* Move over the underscore and look for an integer. */ - if (*(ptr++) != '_') - { - return -1; - } - else - { - len--; /* for the underscore character */ - while (*ptr >= '0' && *ptr <= '9') - { - *(intPtr++) = *(ptr++); - len--; - } - *intPtr = STRING_TERMINATOR; - *cutoffFrequency = atof(buffer); - } - } - } - } - } - - /* If there are extra characters after the suffixes, return an error. */ - if (len > 0) - return -1; - - /* Strip off the text for the function type and cutoff frequency. */ - if (stripEnd == yes) - *newEnd = STRING_TERMINATOR; - - return Mnum; -} - - -SBoolean muscle_has_force_params(dpMuscleStruct* ms) -{ - if (ms->optimal_fiber_length == NULL || ms->resting_tendon_length == NULL || - ms->pennation_angle == NULL || ms->max_isometric_force == NULL || - ms->tendon_force_len_func == NULL || ms->active_force_len_func == NULL || - ms->passive_force_len_func == NULL || *ms->tendon_force_len_func == NULL || - *ms->active_force_len_func == NULL || *ms->passive_force_len_func == NULL) - return no; - - return yes; -} - -ENUM { - NoStringMarker=0, - WhiteSpace, - SingleQuote, - DoubleQuote -} StringMarker; - -StringMarker is_string_marker(char ch) -{ - if (ch == '\'') - return SingleQuote; - if (ch == '\"') - return DoubleQuote; - if (CHAR_IS_WHITE_SPACE(ch) || (ch == STRING_TERMINATOR)) - return WhiteSpace; - return NoStringMarker; -} - -int divide_string(char string[], char* word_array[], int max_words) -{ - int i, len, num_chars, word_start=0, count=0, last_one_white=1; - StringMarker marker = WhiteSpace; - - if (max_words <= 0) - return 0; - - len = strlen(string) + 1; - - // When you hit " x", set word_start to point to the 'x' - // When you hit "x ", copy the word ending in 'x' to the next - // slot in word_array. - for (i=0; i 1 && word_array[i][0] == '\"' && word_array[i][len-1] == '\"') - { - memmove(word_array[i], &word_array[i][1], len-1); - word_array[i][len-2] = STRING_TERMINATOR; - } - } - - return count; -} - -/* Returns the index of the last occurrence of a character in a string - * that belongs to a set of characters. - */ - -int strrcspn(const char* string, const char* strCharSet) -{ - int i, j, len = strlen(string); - int setLen = strlen(strCharSet); - - for (i = len - 1; i >= 0; i--) - { - for (j = 0; j < setLen; j++) - { - if (string[i] == strCharSet[j]) - return i; - } - } - - return -1; -} - - -/* Extracts a name and a value (double) from a specified character string. - * The name is assumed to be at the beginning of the string, and can have - * any number of tokens in it, followed by a colon (which may or may not - * be attached to the name or the value). This function modifies the string - * by inserting a NULL character after the name. - */ -int get_name_and_value_from_string(char* string, double* value) -{ - int len, pos; - char* ptr; - - /* The string may have a colon in it between the name and the value, - * so search for the last colon and replace it with a space. - */ - ptr = strrchr(string, ':'); - if (ptr) - *ptr = ' '; - - /* Strip trailing white space, then find the position of the - * last character of white space. The value is contained in the - * characters after this last white space. - */ - strip_trailing_white_space(string); - len = strlen(string); - pos = strrcspn(string, "\r\t "); - - if (pos > 0 && pos < len - 1) - { - if (sscanf(&string[pos+1], "%lg", value) != 1) - return 0; - string[pos] = STRING_TERMINATOR; - strip_trailing_white_space(string); - } - else - { - return 0; - } - - return 1; -} - - -void strip_trailing_white_space(char string[]) -{ - - int i; - - for (i=strlen(string)-1; i>=0; i--) - { - if (CHAR_IS_WHITE_SPACE(string[i])) - string[i] = STRING_TERMINATOR; - else - break; - } - -} - -/* ------------------------------------------------------------------------- - STATIC DATA ----------------------------------------------------------------------------- */ -static int sNumDefaultAcppOptions = 2; -static int sNumTotalAcppOptions = 2; -static char* sAcppOptions[128] = { "acpp", "-P" }; - -/* ------------------------------------------------------------------------- - clear_preprocessor_options - reset the options that are passed to acpp() - their defaults. ----------------------------------------------------------------------------- */ -public void clear_preprocessor_options () -{ - int i; - - for (i = sNumDefaultAcppOptions; i < sNumTotalAcppOptions; i++) - free(sAcppOptions[i]); - - sNumTotalAcppOptions = sNumDefaultAcppOptions; -} - -/* ------------------------------------------------------------------------- - add_preprocessor_option - format and append the specified string to the - list of command-line options passed to acpp(). - - NOTE: "default" options must precede "non-default" option in the list, - therefore be certain to call clear_preprocessor_options() before - calling this routine with 'isDefaultOption' equal to 'yes'. ----------------------------------------------------------------------------- */ -public void add_preprocessor_option (SBoolean isDefaultOption, const char* format, ...) -{ - va_list ap; - - va_start(ap, format); - vsprintf(buffer, format, ap); - va_end(ap); - -#if MEMORY_LEAK - { - int len=strlen(buffer)+1; - sAcppOptions[sNumTotalAcppOptions] = (char*)malloc(len); - strcpy(sAcppOptions[sNumTotalAcppOptions++], buffer); - } -#else - mstrcpy(&sAcppOptions[sNumTotalAcppOptions++], buffer); -#endif - - if (isDefaultOption) - sNumDefaultAcppOptions++; -} - -/* ------------------------------------------------------------------------- - acpp_message_proc - ----------------------------------------------------------------------------- */ -static int acpp_message_proc (const char* format, ...) -{ - va_list ap; - int n; - - va_start(ap, format); - n = vsprintf(buffer, format, ap); - va_end(ap); - - simm_printf(yes, buffer); - - return n; -} - -typedef int (*_msg_proc)(const char* format, ...); - -int acpp_main(int argc, char** argv, _msg_proc); - -/* ------------------------------------------------------------------------- - acpp - ----------------------------------------------------------------------------- */ -static void acpp (char in_file[], const char out_file[]) -{ - char* pure_path = NULL; - int argc; - - /* Add the folder containing the input file to the search path - * for acpp. This is so that if the file includes other files, - * its folder is searched for them. By default, acpp will search - * only SIMM\resources and the current directory (which may not - * be the directory the input file is in). - */ - get_pure_path_from_path(in_file, &pure_path); - if (pure_path) - { - add_preprocessor_option(no, "-I%s", pure_path); - free(pure_path); - } - - argc = sNumTotalAcppOptions; - sAcppOptions[argc++] = (char*) in_file; - sAcppOptions[argc++] = (char*) out_file; - -#if 0 - { - int i; - - fprintf(stderr, "\n"); - for (i = 0; i < argc; i++) - fprintf(stderr, "%s ", sAcppOptions[i]); - fprintf(stderr, "\n\n"); - } -#endif - - remove(out_file); /* required to work-around CodeWarrior bug */ - - if (acpp_main(argc, sAcppOptions, acpp_message_proc) != 0) - { - const char* infile = strrchr(in_file, DIR_SEP_CHAR); - - if (infile) - infile++; - else - infile = in_file; - - sprintf(errorbuffer, "Error running preproccesor on %s.", infile); - - error(none, errorbuffer); - } - -#if 0 - /* copy the preprocessed file so we can get a look at it: - */ - { - static int sCounter = 1; - sprintf(buffer, "cp %s /suture/usr/people/simm/ACPP-OUT-%d.txt", out_file, sCounter++); - glutSystem(buffer); - } -#endif - -} /* acpp */ - -/* ------------------------------------------------------------------------- - read_deform - ----------------------------------------------------------------------------- */ -ReturnCode read_deform (FILE* fp, SegmentStruct* seg, int segmentnum) -{ - DeformObject* dfm = NULL; - double xyz[3]; - int i = seg->num_deforms; - DMatrix m; - - if (seg->num_deforms >= seg->deform_obj_array_size) - { - ReturnCode rc = code_fine; - - /* expand deform object array if necessary */ - seg->deform_obj_array_size += 2; - - if (seg->deform == NULL) - { - seg->deform = (DeformObject*) simm_malloc( - seg->deform_obj_array_size * sizeof(DeformObject)); - } - else - { - seg->deform = (DeformObject*) simm_realloc(seg->deform, - seg->deform_obj_array_size * sizeof(DeformObject), &rc); - } - - if (rc == code_bad || seg->deform == NULL) - { - seg->deform_obj_array_size -= 2; - return code_bad; - } - } - - seg->num_deforms++; - - /* initialize the new deform */ - dfm = &seg->deform[i]; - - init_deform(dfm); - - dfm->segment = segmentnum; - - if (fscanf(fp,"%s", buffer) != 1) - { - error(abort_action,"Error reading name in deform object definition"); - return code_bad; - } - else - mstrcpy(&dfm->name,buffer); - - while (1) - { - if (read_string(fp,buffer) == EOF) - break; - - if (buffer[0] == '#') - { - read_nonempty_line(fp,buffer); - continue; - } - - if (STRINGS_ARE_EQUAL(buffer,"active")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"false")) - dfm->active = no; - } - else if (STRINGS_ARE_EQUAL(buffer,"visible")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"no") || STRINGS_ARE_EQUAL(buffer,"false")) - dfm->visible = no; - } - else if (STRINGS_ARE_EQUAL(buffer,"autoreset")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"yes") || STRINGS_ARE_EQUAL(buffer,"true")) - dfm->autoReset = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"translationonly")) - { - if (read_string(fp,buffer) == EOF) - break; - - if (STRINGS_ARE_EQUAL(buffer,"yes") || STRINGS_ARE_EQUAL(buffer,"true")) - dfm->translationOnly = yes; - } - else if (STRINGS_ARE_EQUAL(buffer,"innermin")) - { - if (fscanf(fp, "%lg %lg %lg", &dfm->innerMin.xyz[0], - &dfm->innerMin.xyz[1], &dfm->innerMin.xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading \'innermin\' for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"innermax")) - { - if (fscanf(fp, "%lg %lg %lg", &dfm->innerMax.xyz[0], - &dfm->innerMax.xyz[1], &dfm->innerMax.xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading \'innermax\' for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"outermin")) - { - if (fscanf(fp, "%lg %lg %lg", &dfm->outerMin.xyz[0], - &dfm->outerMin.xyz[1], &dfm->outerMin.xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading \'outermin\' for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"outermax")) - { - if (fscanf(fp, "%lg %lg %lg", &dfm->outerMax.xyz[0], - &dfm->outerMax.xyz[1], &dfm->outerMax.xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading \'outermax\' for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"xyz_body_rotation_POSITION")) - { - if (fscanf(fp, "%lg %lg %lg", &xyz[0], &xyz[1], &xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading rotation for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - identity_matrix(m); - x_rotate_matrix_bodyfixed(m, xyz[0] * DTOR); - y_rotate_matrix_bodyfixed(m, xyz[1] * DTOR); - z_rotate_matrix_bodyfixed(m, xyz[2] * DTOR); - extract_rotation(m, &dfm->position.rotation_axis, &dfm->position.rotation_angle); - } - else if (STRINGS_ARE_EQUAL(buffer,"translation_POSITION")) - { - if (fscanf(fp,"%lg %lg %lg", - &dfm->position.translation.xyz[0], - &dfm->position.translation.xyz[1], - &dfm->position.translation.xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading translation for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"xyz_body_rotation_DEFORM_START")) - { - if (fscanf(fp, "%lg %lg %lg", &xyz[0], &xyz[1], &xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading rotation for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - identity_matrix(m); - x_rotate_matrix_bodyfixed(m, xyz[0] * DTOR); - y_rotate_matrix_bodyfixed(m, xyz[1] * DTOR); - z_rotate_matrix_bodyfixed(m, xyz[2] * DTOR); - extract_rotation(m, &dfm->deform_start.rotation_axis, &dfm->deform_start.rotation_angle); - } - else if (STRINGS_ARE_EQUAL(buffer,"translation_DEFORM_START")) - { - if (fscanf(fp,"%lg %lg %lg", - &dfm->deform_start.translation.xyz[0], - &dfm->deform_start.translation.xyz[1], - &dfm->deform_start.translation.xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading translation for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"xyz_body_rotation_DEFORM_END")) - { - if (fscanf(fp, "%lg %lg %lg", &xyz[0], &xyz[1], &xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading rotation for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - identity_matrix(m); - x_rotate_matrix_bodyfixed(m, xyz[0] * DTOR); - y_rotate_matrix_bodyfixed(m, xyz[1] * DTOR); - z_rotate_matrix_bodyfixed(m, xyz[2] * DTOR); - extract_rotation(m, &dfm->deform_end.rotation_axis, &dfm->deform_end.rotation_angle); - } - else if (STRINGS_ARE_EQUAL(buffer,"translation_DEFORM_END")) - { - if (fscanf(fp,"%lg %lg %lg", - &dfm->deform_end.translation.xyz[0], - &dfm->deform_end.translation.xyz[1], - &dfm->deform_end.translation.xyz[2]) != 3) - { - sprintf(errorbuffer, "Error reading translation for deform object %s", dfm->name); - error(abort_action,errorbuffer); - return code_bad; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"enddeform")) - break; - else - { - sprintf(errorbuffer, "Unrecognized text in deform object %s: %s", dfm->name, buffer); - error(abort_action,errorbuffer); - return code_bad; - } - } - - /* Check the inner and outer boxes to see if they are well defined. */ - if (dfm->innerMin.xyz[0] > dfm->innerMax.xyz[0] || dfm->innerMin.xyz[0] < dfm->outerMin.xyz[0]) - { - sprintf(errorbuffer,"Error setting up deform object %s", dfm->name); - error(none, errorbuffer); - error(recover,"Inner min X must be greater than outer min X and less than inner max X"); - return code_bad; - } - if (dfm->innerMin.xyz[1] > dfm->innerMax.xyz[1] || dfm->innerMin.xyz[1] < dfm->outerMin.xyz[1]) - { - sprintf(errorbuffer,"Error setting up deform object %s", dfm->name); - error(none, errorbuffer); - error(recover,"Inner min Y must be greater than outer min Y and less than inner max Y"); - return code_bad; - } - if (dfm->innerMin.xyz[2] > dfm->innerMax.xyz[2] || dfm->innerMin.xyz[2] < dfm->outerMin.xyz[2]) - { - sprintf(errorbuffer,"Error setting up deform object %s", dfm->name); - error(none, errorbuffer); - error(recover,"Inner min Z must be greater than outer min Z and less than inner max Z"); - return code_bad; - } - if (dfm->innerMax.xyz[0] > dfm->outerMax.xyz[0]) - { - sprintf(errorbuffer,"Error setting up deform object %s", dfm->name); - error(none, errorbuffer); - error(recover,"Inner max X must be less than outer max X"); - return code_bad; - } - if (dfm->innerMax.xyz[1] > dfm->outerMax.xyz[1]) - { - sprintf(errorbuffer,"Error setting up deform object %s", dfm->name); - error(none, errorbuffer); - error(recover,"Inner max Y must be less than outer max Y"); - return code_bad; - } - if (dfm->innerMax.xyz[2] > dfm->outerMax.xyz[2]) - { - sprintf(errorbuffer,"Error setting up deform object %s", dfm->name); - error(none, errorbuffer); - error(recover,"Inner max Z must be less than outer max Z"); - return code_bad; - } - - /* NOTE: 'xyz_body_rotation_DEFORM' and 'translation_DEFORM' specify the - * *delta* transformation from the "position" frame to the "deform_start" - * frame. The code below converts this into the DeformObject's true - * "deform_start" transform which is measured relative to the parent segment's - * frame. - */ - dfm->position.xforms_valid = no; - dfm->deform_start.xforms_valid = no; - dfm->deform_end.xforms_valid = no; - recalc_deform_xforms(seg, dfm); - - /* deform start */ - copy_4x4matrix(dfm->deform_start.from_local_xform, m); - append_matrix(m, dfm->position.from_local_xform); - - extract_rotation(m, &dfm->deform_start.rotation_axis, &dfm->deform_start.rotation_angle); - dfm->deform_start.translation.xyz[0] = m[3][0]; - dfm->deform_start.translation.xyz[1] = m[3][1]; - dfm->deform_start.translation.xyz[2] = m[3][2]; - dfm->deform_start.xforms_valid = no; - - /* deform end */ - copy_4x4matrix(dfm->deform_end.from_local_xform, m); - append_matrix(m, dfm->position.from_local_xform); - - extract_rotation(m, &dfm->deform_end.rotation_axis, &dfm->deform_end.rotation_angle); - dfm->deform_end.translation.xyz[0] = m[3][0]; - dfm->deform_end.translation.xyz[1] = m[3][1]; - dfm->deform_end.translation.xyz[2] = m[3][2]; - dfm->deform_end.xforms_valid = no; - - recalc_deform_xforms(seg, dfm); - -#if ! ENGINE - init_deform_box_verts(dfm); -#endif - - return code_fine; - -} /* read_deform */ - - -/* ------------------------------------------------------------------------- - read_deformity - ----------------------------------------------------------------------------- */ -ReturnCode read_deformity (ModelStruct* ms, FILE* fp) -{ - ReturnCode rc = code_fine; - Deformity* dty; - - if (ms->num_deformities == ms->deformity_array_size) - { - ms->deformity_array_size += DEFORMITY_ARRAY_INCREMENT; - - ms->deformity = (Deformity*) simm_realloc(ms->deformity, - ms->deformity_array_size * sizeof(Deformity), &rc); - if (rc == code_bad) - { - ms->deformity_array_size -= DEFORMITY_ARRAY_INCREMENT; - return code_bad; - } - } - - dty = &ms->deformity[ms->num_deformities]; - - init_deformity(dty); - - if (fscanf(fp,"%s", buffer) != 1) - { - error(abort_action,"Error reading name in deformity definition"); - return code_bad; - } - else - mstrcpy(&dty->name, buffer); - - while (1) - { - if (read_string(fp, buffer) == EOF) - break; - - if (buffer[0] == '#') - { - read_nonempty_line(fp,buffer); - continue; - } - - if (STRINGS_ARE_EQUAL(buffer,"default_value") || STRINGS_ARE_EQUAL(buffer,"value")) - { - if (fscanf(fp, "%lg", &dty->default_value) != 1) - { - sprintf(errorbuffer, "Error reading value for deformity: %s.", dty->name); - error(none, errorbuffer); - return code_bad; - } - else - { - dty->value = dty->default_value; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"range")) - { - if (fscanf(fp, "%lg %lg", &dty->range.start, &dty->range.end) != 2) - { - sprintf(errorbuffer, "Error reading range for deformity: %s.", dty->name); - error(none, errorbuffer); - return code_bad; - } - - if (dty->range.start > dty->range.end) - { - double tmp = dty->range.start; - dty->range.start = dty->range.end; - dty->range.end = tmp; - } - } - else if (STRINGS_ARE_EQUAL(buffer,"keys")) - { - int nk; - char key1[64], key2[64]; - - read_line(fp, buffer); - - nk = sscanf(buffer,"%s %s", key1, key2); - - if (nk == 1) - dty->keys[0] = dty->keys[1] = lookup_simm_key(key1); - else if (nk == 2) - { - dty->keys[0] = lookup_simm_key(key1); - dty->keys[1] = lookup_simm_key(key2); - } - else - { - sprintf(errorbuffer, "Error reading keys for deformity: %s.", dty->name); - error(recover,errorbuffer); - } - } - else if (STRINGS_ARE_EQUAL(buffer,"deform")) - { - char* dfmName = buffer; - - read_line(fp, dfmName); - - _strip_outer_whitespace(dfmName); /* remove any leading or trailing whitespace */ - - if (*dfmName) - { - rc = code_fine; - - if (dty->deform == NULL) - { - dty->deform = (DeformObject**) simm_malloc(1 * sizeof(DeformObject*)); - dty->deform_name = (char**) simm_malloc(1 * sizeof(char*)); - } - else - { - dty->deform = (DeformObject**) simm_realloc(dty->deform, - (dty->num_deforms + 1) * sizeof(DeformObject*), &rc); - - dty->deform_name = (char**) simm_realloc(dty->deform_name, - (dty->num_deforms + 1) * sizeof(char*), &rc); - } - - if (dty->deform && dty->deform_name && rc == code_fine) - { - dty->deform[dty->num_deforms] = lookup_deform(ms, dfmName); - - if (dty->deform[dty->num_deforms]) - { - mstrcpy(&dty->deform_name[dty->num_deforms], dfmName); - dty->num_deforms++; - } - else - rc = code_bad; - } - - if (rc != code_fine) - { - if (dty->deform) - free(dty->deform); - - if (dty->deform_name) - free(dty->deform_name); - - dty->deform = NULL; - dty->num_deforms = 0; - - sprintf(errorbuffer, "Error reading deform \'%s\' for deformity: %s.", - dfmName, dty->name); - error(recover,errorbuffer); - - return code_bad; - } - } - } - else if (STRINGS_ARE_EQUAL(buffer,"enddeformity")) - break; - } - - /* Check the value and default_value to make sure they're in range. - * This code will also initialize them to range.start if they were not - * specified in the deformity definition. - */ - if (dty->value < dty->range.start) - dty->value = dty->range.start; - else if (dty->value > dty->range.end) - dty->value = dty->range.end; - - if (dty->default_value < dty->range.start) - dty->default_value = dty->range.start; - else if (dty->default_value > dty->range.end) - dty->default_value = dty->range.end; - - ms->num_deformities++; - - return code_fine; - -} /* read_deformity */ - - -/* ------------------------------------------------------------------------- - _read_til - read (and ignore) characters from the specified file until - the specified character 'c' is found. ----------------------------------------------------------------------------- */ -SBoolean _read_til (FILE* file, int c) -{ - int t = fgetc(file); - - while (t != c && t != EOF) - t = fgetc(file); - - return (SBoolean) (t == EOF); -} - - -/* ------------------------------------------------------------------------- - _read_til_tokens - Read characters from 'file' into 'buf' until one of the - characters specified in 'delimiters' is encountered. Put the delimiting - character back into 'file'. ----------------------------------------------------------------------------- */ -int _read_til_tokens (FILE* file, char* buf, const char* delimiters) -{ - char* p = buf; - - while (1) - { - char c = fgetc(file); - - if (feof(file) || ferror(file)) - break; - else - { - if (strchr(delimiters, c)) - { - ungetc(c, file); - break; - } - else - *p++ = c; - } - } - *p = '\0'; - - /* remove trailing whitespace if necessary */ - while (p > buf && isspace(p[-1])) - *(--p) = '\0'; - - /* Return 1 if the string is not empty. */ - if (p > buf) - return 1; - else - return 0; -} - -/* ------------------------------------------------------------------------- - _strip_outer_whitespace - ----------------------------------------------------------------------------- */ -void _strip_outer_whitespace (char* str) -{ - /* remove exterior (but not interior) whitespace from a string. - */ - - /* remove trailing whitespace */ - char* p = str + strlen(str) - 1; - - for ( ; p >= str && isspace(*p); p--) - *p = '\0'; - - /* remove leading whitespace */ - for (p = str; *p; p++) - if ( ! isspace(*p)) - break; - - if (*p && p != str) - memmove(str, p, strlen(p) + 1); -} - -void strip_brackets_from_string(char name[]) -{ - int i, j; - char buffer[CHARBUFFER]; - - // strip the () from the gencoord name - strcpy(buffer, name); - i = 0; - j = 0; - while (1) - { - if (buffer[i] == '(') - { - i++; - continue; - } - if (buffer[i] == ')') - { - name[j] = '\0'; - break; - } - name[j++] = buffer[i++]; - } -} diff --git a/OpenSim/Utilities/simmToOpenSim/sdf.c b/OpenSim/Utilities/simmToOpenSim/sdf.c deleted file mode 100644 index 96c76f7c7e..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/sdf.c +++ /dev/null @@ -1,2188 +0,0 @@ -/******************************************************************************* - - SDF.C - - Author: Peter Loan - - Date: 19-JUN-91 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: - - Routines: - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "sdfunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ -SDSegment* SDseg = NULL; -int num_SD_segs; -int* joint_order = NULL; - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void write_loop_joints(FILE* fp, char seg_name[], SegmentSDF* seg, int addQuestionMarks); -static int countConstraints(ModelStruct* ms, int* nq); - - -/* This function determines much of the information required to make - * an SD/FAST model from the SIMM model, such as figuring out how to - * convert each SIMM joint into an SD/FAST joint, and what the degrees - * of freedom should be. Optionally, it writes out this information to - * an SD/FAST input file. - */ -ReturnCode make_sdfast_model(ModelStruct* ms, char filename[], SBoolean write_file, int addQuestionMarks) -{ - int i, j, nq, dofcount, constrainedcount, numconstraints, jointnum, dofnum; - char* time_string; - char sdf_buffer[CHARBUFFER]; - FILE* fp = NULL; - JointSDF* jnts; - DofStruct* dof; - SegmentSDF* segs; - - /* Make the joint transformation matrices once you have changed the gencoords */ - - for (i=0; inumjoints; i++) - make_conversion(ms, i); - - /* Re-initialize the SD/FAST variables. Mark all dofs as - * constrained (not true degrees of freedom). - * Then for each gencoord, find the ONE dof which best - * corresponds to the gencoord and mark it unconstrained. - * That is, of all the dofs that are functions of a particular - * gencoord, one should be a direct mapping (between dof and - * gencoord) and the others should be non-trivial functions. - */ - - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - ms->joint[i].dofs[j].sd.name = NULL; - ms->joint[i].dofs[j].sd.con_name = NULL; - ms->joint[i].dofs[j].sd.initial_value = 0.0; - ms->joint[i].dofs[j].sd.constrained = yes; - ms->joint[i].dofs[j].sd.fixed = no; - ms->joint[i].dofs[j].sd.state_number = -1; - ms->joint[i].dofs[j].sd.error_number = -1; - ms->joint[i].dofs[j].sd.joint = -1; - ms->joint[i].dofs[j].sd.axis = -1; - ms->joint[i].dofs[j].sd.conversion = 0.0; - ms->joint[i].dofs[j].sd.conversion_sign = 1.0; - } - } - - for (i=0; inumgencoords; i++) - { - if (ms->gencoord[i]->used_in_model == yes) - { - if (!mark_unconstrained_dof(ms, ms->gencoord[i], &jointnum, &dofnum, NULL)) - { - sprintf(errorbuffer, "At least one DOF must be a \"simple\" function of gencoord %s (2 points, slope=1, passes thru zero).", - ms->gencoord[i]->name); - return code_bad; - } - } - } - - /* Now give the dofs names for use in the SD/FAST code. - * Names of unconstrained dofs will simply be the names of - * the gencoords to which they correspond. Names of - * constrained dofs will be formed from the joint name - * and the dof keyword (e.g. "hip_tx" and "knee_r1"). - */ - - name_dofs(ms); - - FREE_IFNOTNULL(joint_order); - joint_order = (int*)malloc(ms->numjoints*sizeof(int)); - jnts = (JointSDF*)simm_calloc(ms->numjoints, sizeof(JointSDF)); - segs = (SegmentSDF*)simm_calloc(ms->numsegments, sizeof(SegmentSDF)); - - for (i = 0; i < ms->numjoints; i++) - joint_order[i] = -1; - - find_sdfast_joint_order(ms, jnts, segs, joint_order, SDFAST_GROUND); - - /* Open the SD/FAST input file */ - - if (write_file == yes) - { - fp = simm_fopen(filename,"w"); - - if (fp == NULL) - { - sprintf(errorbuffer,"Error creating sdfast file %s", filename); - error(abort_action,errorbuffer); - return code_bad; - } - - make_time_string(&time_string); - - fprintf(fp,"# SD/FAST input file generated by %s\n", program_with_version); - fprintf(fp,"# Created %s\n", time_string); - fprintf(fp,"# Name of original SIMM joints file: %s\n#\n\n", - ms->jointfilename); - - fprintf(fp,"language = c\n\n"); - - if (ms->gravity == smX) - fprintf(fp,"gravity = 9.80665? 0.0? 0.0?\n\n"); - else if (ms->gravity == smNegX) - fprintf(fp,"gravity = -9.80665? 0.0? 0.0?\n\n"); - else if (ms->gravity == smY) - fprintf(fp,"gravity = 0.0? 9.80665? 0.0?\n\n"); - else if (ms->gravity == smNegY) - fprintf(fp,"gravity = 0.0? -9.80665? 0.0?\n\n"); - else if (ms->gravity == smZ) - fprintf(fp,"gravity = 0.0? 0.0? 9.80665?\n\n"); - else if (ms->gravity == smNegZ) - fprintf(fp,"gravity = 0.0? 0.0? -9.80665?\n\n"); - else if (ms->gravity == smNoAlign) - fprintf(fp,"gravity = 0.0? 0.0? 0.0?\n\n"); - else // -Y is the default - fprintf(fp,"gravity = 0.0? -9.80665? 0.0?\n\n"); - } - - /* Malloc space for the array of segment names. These names include $ground - * and the names of the "split" body segments. There can be at most - * (numjoints + 1) segment names. - */ - - /* Free the SDseg array, if it was used previously. */ - if (SDseg) - { - for (i = 0; i < num_SD_segs; i++) - FREE_IFNOTNULL(SDseg[i].name); - FREE_IFNOTNULL(SDseg); - } - - SDseg = (SDSegment*)simm_calloc(ms->numjoints + 1, sizeof(SDSegment)); - SDseg[0].name = (char*)simm_malloc(strlen(ms->segment[ms->ground_segment].name) + 2); - strcpy(SDseg[0].name, ms->segment[ms->ground_segment].name); - convert_string(SDseg[0].name, yes); - SDseg[0].simm_segment = ms->ground_segment; - SDseg[0].mass_center[0] = ms->segment[ms->ground_segment].masscenter[0]; - SDseg[0].mass_center[1] = ms->segment[ms->ground_segment].masscenter[1]; - SDseg[0].mass_center[2] = ms->segment[ms->ground_segment].masscenter[2]; - SDseg[0].mass = 0.0; - for (i = 0; i < 3; i++) - for (j = 0; j < 3; j++) - SDseg[0].inertia[i][j] = 0.0; - - num_SD_segs = 1; - - for (i=0, dofcount=0, constrainedcount=0; inumjoints; i++) - { - if (joint_order[i] < 0) - continue; - if (ms->joint[joint_order[i]].type != dpSkippable) - { - JointSDF* foo1 = &jnts[joint_order[i]]; - int foo2 = joint_order[i]; - int foo3 = ms->joint[i].sd_num; - make_sdfast_joint(ms, fp, &jnts[joint_order[i]], joint_order[i], ms->joint[i].sd_num,segs, &dofcount, - &constrainedcount, write_file, addQuestionMarks); - } - } - - if (write_file == yes) - { - for (i=0; inumsegments; i++) - { - if (segs[i].times_split > 0) - { - fprintf(fp,"# Loop Joints\n\n"); - break; - } - } - - for (i=0; inumsegments; i++) - { - if (segs[i].times_split > 0) - write_loop_joints(fp, ms->segment[i].name, &segs[i], addQuestionMarks); - } - } - - for (i=0; inumjoints; i++) - { - FREE_IFNOTNULL(jnts[i].inbname); - FREE_IFNOTNULL(jnts[i].outbname); - } - free(jnts); - free(segs); - - /* count the number of constraint functions, which is the number of q's - * minus the number of SIMM gencoords. - */ - - if (write_file == yes) - { - numconstraints = countConstraints(ms, &nq); - - if (numconstraints > 0) - fprintf(fp,"\n#constraints = %d\n\n", numconstraints); - - for (i=0; isd.constrained == yes) - fprintf(fp,"constraint = %s \n", dof->sd.con_name); - } - for (i = 0; i < ms->num_constraint_objects; i++) - { - for (j = 0; j < ms->constraintobj[i].numPoints; j++) - { - fprintf(fp, "constraint = %s_", ms->constraintobj[i].name); - fprintf(fp, "%s\n", ms->constraintobj[i].points[j].name); - } - } - fclose(fp); - - (void)sprintf(sdf_buffer,"Wrote SD/FAST input file %s", filename); - message(sdf_buffer,0,DEFAULT_MESSAGE_X_OFFSET); - } - - return code_fine; - -} - - -static int countConstraints(ModelStruct* ms, int* nq) -{ - int i, j, numconstraints; - - /* DKB added sd.fixed = yes otherwise can miss constraint if it is the last q in list */ - for (i = 0, *nq = 0; i < ms->numjoints; i++) - for (j = 0; j < 6; j++) - if (ms->joint[i].dofs[j].type == function_dof || ms->joint[i].dofs[j].sd.fixed == yes) - (*nq)++; - - numconstraints = *nq - (ms->numgencoords - ms->numunusedgencoords); - - for (i = 0; i < ms->num_constraint_objects; i++) - numconstraints += ms->constraintobj[i].numPoints; - - return numconstraints; - -} - - -char* make_sdfast_seg_name(char seg_name[], int times_split) -{ - - int i, new_len, prepend = 0; - char* new_name; - - if (seg_name[0] >= '0' && seg_name[0] <= '9') - prepend = 1; - - new_len = prepend + STRLEN(seg_name) + times_split; - - new_name = (char*)malloc(new_len * sizeof(char)); - - if (prepend) - { - new_name[0] = '_'; - strcpy(&new_name[1], seg_name); - } - else - { - strcpy(new_name, seg_name); - } - - for (i = new_len - 2; i > new_len - 2 - times_split; i--) - new_name[i] = 'p'; - - new_name[new_len - 1] = STRING_TERMINATOR; - - return new_name; -} - - -void make_sdfast_joint(ModelStruct* ms, FILE* fp, JointSDF* jntsdf, int jointnum, int sdnum, - SegmentSDF segs[], int* dofcount, - int* constrainedcount, SBoolean write_file, int addQuestionMarks) -{ - int i, ax, df, axisnum, axisname[3]; - double axis[3], bodytojoint[3], inbtojoint[3]; - SegmentStruct* seg1; - SegmentStruct* seg2; - SegmentSDF* segsdf; - JointStruct* jnt; - dpJointType type; - char prescribed_string[20]; - static char yesQuestionMark[] = "?"; - static char noQuestionMark[] = ""; - char* questionMark; - - if (addQuestionMarks) - questionMark = yesQuestionMark; - else - questionMark = noQuestionMark; - - jnt = &ms->joint[jointnum]; - type = ms->joint[jointnum].type; - - if (jntsdf->dir == FORWARD) - { - seg1 = &ms->segment[jnt->from]; - seg2 = &ms->segment[jnt->to]; - segsdf = &segs[jnt->to]; - } - else - { - seg1 = &ms->segment[jnt->to]; - seg2 = &ms->segment[jnt->from]; - segsdf = &segs[jnt->from]; - } - - if (write_file == yes) - { - fprintf(fp, "body = %s inb = %s joint = %s\n", jntsdf->outbname, - jntsdf->inbname, get_joint_type_name(type,jntsdf->dir)); - if (type == dpWeld) - fprintf(fp, "# this is really a weld joint implemented as a prescribed pin\n"); - } - - /* Add the body name to the list of segment names. This list contains 'real' - * segment names as well as the names of 'split' body segments. - */ - - mstrcpy(&(SDseg[num_SD_segs].name),jntsdf->outbname); - - /* If there are loops in the model, then SIMM segments get split and there - * will be more SD segments than SIMM segments. So that unsplittable segment - * parameters (like contact objects) can be assigned to the SD segments, - * each SD segment has an index of its corresponding SIMM segment. But for - * segments that were split, only one piece will have a valid index. - */ - if (jntsdf->closes_loop == no) - { - if (jntsdf->dir == FORWARD) - SDseg[num_SD_segs].simm_segment = jnt->to; - else - SDseg[num_SD_segs].simm_segment = jnt->from; - } - else - { - SDseg[num_SD_segs].simm_segment = -1; - } - - SDseg[num_SD_segs].mass = seg2->mass / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[0][0] = seg2->inertia[0][0] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[0][1] = seg2->inertia[0][1] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[0][2] = seg2->inertia[0][2] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[1][0] = seg2->inertia[1][0] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[1][1] = seg2->inertia[1][1] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[1][2] = seg2->inertia[1][2] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[2][0] = seg2->inertia[2][0] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[2][1] = seg2->inertia[2][1] / segsdf->mass_factor; - SDseg[num_SD_segs].inertia[2][2] = seg2->inertia[2][2] / segsdf->mass_factor; - SDseg[num_SD_segs].mass_center[0] = seg2->masscenter[0]; - SDseg[num_SD_segs].mass_center[1] = seg2->masscenter[1]; - SDseg[num_SD_segs].mass_center[2] = seg2->masscenter[2]; - - if (write_file == yes) - { - fprintf(fp, "mass = %20.16lf%s inertia = %.16lf%s %.16lf%s %.16lf%s\n", - SDseg[num_SD_segs].mass, questionMark, - SDseg[num_SD_segs].inertia[0][0], questionMark, - SDseg[num_SD_segs].inertia[0][1], questionMark, - SDseg[num_SD_segs].inertia[0][2], questionMark); - fprintf(fp, " %.16lf%s %.16lf%s %.16lf%s\n", - SDseg[num_SD_segs].inertia[1][0], questionMark, - SDseg[num_SD_segs].inertia[1][1], questionMark, - SDseg[num_SD_segs].inertia[1][2], questionMark); - fprintf(fp, " %.16lf%s %.16lf%s %.16lf%s\n", - SDseg[num_SD_segs].inertia[2][0], questionMark, - SDseg[num_SD_segs].inertia[2][1], questionMark, - SDseg[num_SD_segs].inertia[2][2], questionMark); - } - - /* The bodytojoint vector (inbtojoint vector for INVERSE joints) is always - * defined by the negative of the mass center vector. The inbtojoint vector - * (bodytojoint for INVERSE joints) can change depending on whether or not - * some of the translations in the joint are functions. For all translation - * components that are functions, you want to use just the mass center - * vector component so that the DOF value is the same as the SIMM gencoord - * or SIMM constraint function. For components that are constants, you want - * to add the DOF value to the mass center vector so that the origin ends - * up in the right place. 4/3/97. - * In general, the above method works only for joints in which the translations - * occur before the rotations. For cases in which the translations occur - * between two or more rotations, the joint cannot be modeled easily in - * SD/FAST. For cases in which the translations are after the rotations, - * SD/FAST has "reverse" joints (e.g., rbushing) that automatically handle - * the translations properly. Rplanar correctly handles this case because - * the rplanar joint itself handles two of the translations, and the third - * one is along the axis of rotation, so it does not need to be handled - * separately. For joints which do not have a reverse (e.g., pin, cylinder, - * universal, gimbal), you need to attach the translations to the "other" - * body segment than you normally would. 4/10/97. - */ - - if (type == dpReversePin || type == dpReverseGimbal || - type == dpReverseUniversal || type == dpReverseCylindrical) - { - if (jntsdf->dir == FORWARD) - { - if (jnt->dofs[TX].type == constant_dof) - bodytojoint[0] = -jnt->dofs[TX].value - seg2->masscenter[0]; - else - bodytojoint[0] = -seg2->masscenter[0]; - if (jnt->dofs[TY].type == constant_dof) - bodytojoint[1] = -jnt->dofs[TY].value - seg2->masscenter[1]; - else - bodytojoint[1] = -seg2->masscenter[1]; - if (jnt->dofs[TZ].type == constant_dof) - bodytojoint[2] = -jnt->dofs[TZ].value - seg2->masscenter[2]; - else - bodytojoint[2] = -seg2->masscenter[2]; - inbtojoint[0] = -seg1->masscenter[0]; - inbtojoint[1] = -seg1->masscenter[1]; - inbtojoint[2] = -seg1->masscenter[2]; - } - else - { - bodytojoint[0] = -seg2->masscenter[0]; - bodytojoint[1] = -seg2->masscenter[1]; - bodytojoint[2] = -seg2->masscenter[2]; - if (jnt->dofs[TX].type == constant_dof) - inbtojoint[0] = -jnt->dofs[TX].value - seg1->masscenter[0]; - else - inbtojoint[0] = -seg1->masscenter[0]; - if (jnt->dofs[TY].type == constant_dof) - inbtojoint[1] = -jnt->dofs[TY].value - seg1->masscenter[1]; - else - inbtojoint[1] = -seg1->masscenter[1]; - if (jnt->dofs[TZ].type == constant_dof) - inbtojoint[2] = -jnt->dofs[TZ].value - seg1->masscenter[2]; - else - inbtojoint[2] = -seg1->masscenter[2]; - } - } - else - { - if (jntsdf->dir == FORWARD) - { - bodytojoint[0] = -seg2->masscenter[0]; - bodytojoint[1] = -seg2->masscenter[1]; - bodytojoint[2] = -seg2->masscenter[2]; - if (jnt->dofs[TX].type == constant_dof) - inbtojoint[0] = jnt->dofs[TX].value - seg1->masscenter[0]; - else - inbtojoint[0] = -seg1->masscenter[0]; - if (jnt->dofs[TY].type == constant_dof) - inbtojoint[1] = jnt->dofs[TY].value - seg1->masscenter[1]; - else - inbtojoint[1] = -seg1->masscenter[1]; - if (jnt->dofs[TZ].type == constant_dof) - inbtojoint[2] = jnt->dofs[TZ].value - seg1->masscenter[2]; - else - inbtojoint[2] = -seg1->masscenter[2]; - } - else - { - if (jnt->dofs[TX].type == constant_dof) - bodytojoint[0] = jnt->dofs[TX].value - seg2->masscenter[0]; - else - bodytojoint[0] = -seg2->masscenter[0]; - if (jnt->dofs[TY].type == constant_dof) - bodytojoint[1] = jnt->dofs[TY].value - seg2->masscenter[1]; - else - bodytojoint[1] = -seg2->masscenter[1]; - if (jnt->dofs[TZ].type == constant_dof) - bodytojoint[2] = jnt->dofs[TZ].value - seg2->masscenter[2]; - else - bodytojoint[2] = -seg2->masscenter[2]; - inbtojoint[0] = -seg1->masscenter[0]; - inbtojoint[1] = -seg1->masscenter[1]; - inbtojoint[2] = -seg1->masscenter[2]; - } - } - - /* Store the vectors in the SDseg struct so they can be - * passed to the simulation DLL later. - */ - for (i = 0; i < 3; i++) - { - SDseg[num_SD_segs].body_to_joint[i] = bodytojoint[i]; - SDseg[num_SD_segs].inboard_to_joint[i] = inbtojoint[i]; - } - - if (write_file == yes) - { - fprintf(fp, "bodytojoint = %.16lf%s %.16lf%s %.16lf%s inbtojoint = %.16lf%s %.16lf%s %.16lf%s\n", - bodytojoint[0], questionMark, bodytojoint[1], questionMark, bodytojoint[2], questionMark, - inbtojoint[0], questionMark, inbtojoint[1], questionMark, inbtojoint[2], questionMark); - } - - num_SD_segs++; - - if (type == dpWeld) - { - if (write_file == yes) - { - fprintf(fp, "pin = 1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark); - fprintf(fp, "prescribed = 1\n"); - } - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[R1].sd.name,buffer); - jnt->dofs[R1].sd.state_number = (*dofcount)++; - jnt->dofs[R1].sd.constrained = no; - jnt->dofs[R1].sd.fixed = yes; - jnt->dofs[R1].sd.conversion = RTOD*jnt->dofs[R1].sd.conversion_sign; - jnt->dofs[R1].sd.joint = sdnum;//jointnum; - jnt->dofs[R1].sd.axis = 0; - jnt->dofs[R1].sd.initial_value = 0.0; - } - else if (type == dpPin || type == dpReversePin) - { - df = find_rotation_axis(jnt, axis); - if (jntsdf->dir == INVERSE) - { - axis[0] = -axis[0]; - axis[1] = -axis[1]; - axis[2] = -axis[2]; - } - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - if (jnt->dofs[df].type == constant_dof) - { - if (write_file == yes) - fprintf(fp, "prescribed = 1\n"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[df].sd.name,buffer); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.constrained = no; - jnt->dofs[df].sd.fixed = yes; - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 0; - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else - { - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - else if (write_file == yes) - fprintf(fp, "prescribed = 0?\n"); - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 0; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - } - else if (type == dpSlider) - { - df = find_translation_axis(jnt, axis, function_dof, 0); - if (jntsdf->dir == INVERSE) - { - axis[0] = -axis[0]; - axis[1] = -axis[1]; - axis[2] = -axis[2]; - } - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - else if (write_file == yes) - fprintf(fp, "prescribed = 0?\n"); - jnt->dofs[df].sd.conversion = jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 0; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else if (type == dpPlanar) - { - axisnum = 0; - NULLIFY_STRING(prescribed_string); - if (jntsdf->dir == FORWARD) - { - if (jnt->dofs[TX].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TX].sd.state_number = (*dofcount)++; - jnt->dofs[TX].sd.fixed = no; - if (jnt->dofs[TX].sd.constrained == yes) - { - jnt->dofs[TX].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign; - jnt->dofs[TX].sd.joint = sdnum;//jointnum; - jnt->dofs[TX].sd.axis = axisnum++; - } - if (jnt->dofs[TY].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s 1.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TY].sd.state_number = (*dofcount)++; - jnt->dofs[TY].sd.fixed = no; - if (jnt->dofs[TY].sd.constrained == yes) - { - jnt->dofs[TY].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign; - jnt->dofs[TY].sd.joint = sdnum;//jointnum; - jnt->dofs[TY].sd.axis = axisnum++; - } - if (jnt->dofs[TZ].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s 0.0%s 1.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TZ].sd.state_number = (*dofcount)++; - jnt->dofs[TZ].sd.fixed = no; - if (jnt->dofs[TZ].sd.constrained == yes) - { - jnt->dofs[TZ].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign; - jnt->dofs[TZ].sd.joint = sdnum;//jointnum; - jnt->dofs[TZ].sd.axis = axisnum++; - } - df = find_rotation_axis(jnt,axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - if (jnt->dofs[df].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[df].sd.name,buffer); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.constrained = no; - jnt->dofs[df].sd.fixed = yes; - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 2; - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else - { - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - { - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 2; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - } - else /* direction == INVERSE */ - { - df = find_rotation_axis(jnt,axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark); - if (jnt->dofs[df].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[df].sd.name,buffer); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.constrained = no; - jnt->dofs[df].sd.fixed = yes; - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = axisnum++; - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else - { - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - { - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = axisnum++; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - if (jnt->dofs[TZ].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s 0.0%s -1.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TZ].sd.state_number = (*dofcount)++; - jnt->dofs[TZ].sd.fixed = no; - if (jnt->dofs[TZ].sd.constrained == yes) - { - jnt->dofs[TZ].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign; - jnt->dofs[TZ].sd.joint = sdnum;//jointnum; - jnt->dofs[TZ].sd.axis = axisnum++; - } - if (jnt->dofs[TY].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s -1.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TY].sd.state_number = (*dofcount)++; - jnt->dofs[TY].sd.fixed = no; - if (jnt->dofs[TY].sd.constrained == yes) - { - jnt->dofs[TY].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign; - jnt->dofs[TY].sd.joint = sdnum;//jointnum; - jnt->dofs[TY].sd.axis = axisnum++; - } - if (jnt->dofs[TX].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = -1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TX].sd.state_number = (*dofcount)++; - jnt->dofs[TX].sd.fixed = no; - if (jnt->dofs[TX].sd.constrained == yes) - { - jnt->dofs[TX].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign; - jnt->dofs[TX].sd.joint = sdnum;//jointnum; - jnt->dofs[TX].sd.axis = axisnum++; - } - } - if (write_file == yes) - fprintf(fp, "prescribed =%s\n", prescribed_string); - } - else if (type == dpReversePlanar) - { - axisnum = 0; - NULLIFY_STRING(prescribed_string); - if (jntsdf->dir == FORWARD) - { - df = find_rotation_axis(jnt,axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - if (jnt->dofs[df].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[df].sd.name,buffer); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.constrained = no; - jnt->dofs[df].sd.fixed = yes; - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = axisnum++; - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else - { - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - { - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = axisnum++; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - if (jnt->dofs[TZ].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s 0.0%s 1.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TZ].sd.state_number = (*dofcount)++; - jnt->dofs[TZ].sd.fixed = no; - if (jnt->dofs[TZ].sd.constrained == yes) - { - jnt->dofs[TZ].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign; - jnt->dofs[TZ].sd.joint = sdnum;//jointnum; - jnt->dofs[TZ].sd.axis = axisnum++; - } - if (jnt->dofs[TY].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s 1.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TY].sd.state_number = (*dofcount)++; - jnt->dofs[TY].sd.fixed = no; - if (jnt->dofs[TY].sd.constrained == yes) - { - jnt->dofs[TY].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign; - jnt->dofs[TY].sd.joint = sdnum;//jointnum; - jnt->dofs[TY].sd.axis = axisnum++; - } - if (jnt->dofs[TX].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TX].sd.state_number = (*dofcount)++; - jnt->dofs[TX].sd.fixed = no; - if (jnt->dofs[TX].sd.constrained == yes) - { - jnt->dofs[TX].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign; - jnt->dofs[TX].sd.joint = sdnum;//jointnum; - jnt->dofs[TX].sd.axis = axisnum++; - } - } - else /* direction == INVERSE */ - { - if (jnt->dofs[TX].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = -1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TX].sd.state_number = (*dofcount)++; - jnt->dofs[TX].sd.fixed = no; - if (jnt->dofs[TX].sd.constrained == yes) - { - jnt->dofs[TX].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value; - } - jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign; - jnt->dofs[TX].sd.joint = sdnum;//jointnum; - jnt->dofs[TX].sd.axis = axisnum++; - } - if (jnt->dofs[TY].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s -1.0%s 0.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TY].sd.state_number = (*dofcount)++; - jnt->dofs[TY].sd.fixed = no; - if (jnt->dofs[TY].sd.constrained == yes) - { - jnt->dofs[TY].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value; - } - jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign; - jnt->dofs[TY].sd.joint = sdnum;//jointnum; - jnt->dofs[TY].sd.axis = axisnum++; - } - if (jnt->dofs[TZ].type == function_dof) - { - if (write_file == yes) - fprintf(fp, "pin = 0.0%s 0.0%s -1.0%s\n", questionMark, questionMark, questionMark); - jnt->dofs[TZ].sd.state_number = (*dofcount)++; - jnt->dofs[TZ].sd.fixed = no; - if (jnt->dofs[TZ].sd.constrained == yes) - { - jnt->dofs[TZ].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value; - } - jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign; - jnt->dofs[TZ].sd.joint = sdnum;//jointnum; - jnt->dofs[TZ].sd.axis = axisnum++; - } - df = find_rotation_axis(jnt,axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark); - if (jnt->dofs[df].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[df].sd.name,buffer); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.constrained = no; - jnt->dofs[df].sd.fixed = yes; - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = axisnum++; - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else - { - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - { - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = axisnum++; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - } - if (write_file == yes) - fprintf(fp, "prescribed =%s\n", prescribed_string); - } - else if (type == dpUniversal || type == dpReverseUniversal) - { - int axis_count = 0; - NULLIFY_STRING(prescribed_string); - for (i=0; i<3; i++) - { - if (jntsdf->dir == FORWARD) - ax = find_nth_rotation(jnt,i+1) - 1; - else - ax = find_nth_rotation(jnt,3-i) - 1; - /* TODO: after enhancing find_rotation_axis(), it should be called here instead - * of find_nth_rotation(), which doesn't check for zero rotations. - */ - if (jnt->dofs[ax].type == constant_dof && EQUAL_WITHIN_ERROR(jnt->dofs[ax].value,0.0)) - continue; - if (write_file == yes) - { - if (jntsdf->dir == FORWARD) - { - double axis[4]; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - } - else - { - double axis[4]; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark); - } - } - if (jnt->dofs[ax].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[ax].sd.name,buffer); - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.constrained = no; - jnt->dofs[ax].sd.fixed = yes; - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axis_count++; - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - } - else - { - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.fixed = no; - if (jnt->dofs[ax].sd.constrained == yes) - { - jnt->dofs[ax].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axis_count++; - if (jnt->dofs[ax].sd.constrained == no) - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value; - else - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - } - } - if (write_file == yes) - fprintf(fp, "prescribed =%s\n", prescribed_string); - } - else if (type == dpCylindrical || type == dpReverseCylindrical) - { - NULLIFY_STRING(prescribed_string); - if (jntsdf->dir == FORWARD) - { - for (i=TX; i<=TZ; i++) - { - if (jnt->dofs[i].type == function_dof) - { - jnt->dofs[i].sd.state_number = (*dofcount)++; - jnt->dofs[i].sd.fixed = no; - if (jnt->dofs[i].sd.constrained == yes) - { - jnt->dofs[i].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[i].sd.initial_value = jnt->dofs[i].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[i].sd.initial_value = jnt->dofs[i].value; - } - jnt->dofs[i].sd.conversion = jnt->dofs[i].sd.conversion_sign; - jnt->dofs[i].sd.joint = sdnum;//jointnum; - jnt->dofs[i].sd.axis = 0; - break; - } - } - df = find_rotation_axis(jnt,axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - if (jnt->dofs[df].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[df].sd.name,buffer); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.constrained = no; - jnt->dofs[df].sd.fixed = yes; - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 1; - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else - { - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - { - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 1; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - } - else /* direction == INVERSE */ - { - for (i=TX; i<=TZ; i++) - { - if (jnt->dofs[i].type == function_dof) - { - jnt->dofs[i].sd.state_number = (*dofcount)++; - jnt->dofs[i].sd.fixed = no; - if (jnt->dofs[i].sd.constrained == yes) - { - jnt->dofs[i].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[i].sd.initial_value = jnt->dofs[i].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[i].sd.initial_value = jnt->dofs[i].value; - } - jnt->dofs[i].sd.conversion = jnt->dofs[i].sd.conversion_sign; - jnt->dofs[i].sd.joint = sdnum;//jointnum; - jnt->dofs[i].sd.axis = 0; - break; - } - } - df = find_rotation_axis(jnt,axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark); - if (jnt->dofs[df].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[df].sd.name,buffer); - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.constrained = no; - jnt->dofs[df].sd.fixed = yes; - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 1; - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - else - { - jnt->dofs[df].sd.state_number = (*dofcount)++; - jnt->dofs[df].sd.fixed = no; - if (jnt->dofs[df].sd.constrained == yes) - { - jnt->dofs[df].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign; - jnt->dofs[df].sd.joint = sdnum;//jointnum; - jnt->dofs[df].sd.axis = 1; - if (jnt->dofs[df].sd.constrained == no) - jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value; - else - jnt->dofs[df].sd.initial_value = jnt->dofs[df].value; - } - } - if (write_file == yes) - fprintf(fp, "prescribed =%s\n", prescribed_string); - } - else if (type == dpGimbal || type == dpReverseGimbal) - { - NULLIFY_STRING(prescribed_string); - for (i=0; i<3; i++) - { - if (jntsdf->dir == FORWARD) - ax = find_nth_rotation(jnt,i+1) - 1; - else - ax = find_nth_rotation(jnt,3-i) - 1; - if (write_file == yes) - { - if (jntsdf->dir == FORWARD) - { - double axis[4]; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - } - else - { - double axis[4]; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark); - } - } - if (jnt->dofs[ax].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[ax].sd.name,buffer); - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.constrained = no; - jnt->dofs[ax].sd.fixed = yes; - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = i; - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - } - else - { - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.fixed = no; - if (jnt->dofs[ax].sd.constrained == yes) - { - jnt->dofs[ax].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = i; - if (jnt->dofs[ax].sd.constrained == yes) - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - else - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value; - } - } - if (write_file == yes) - fprintf(fp, "prescribed =%s\n", prescribed_string); - } - /* TODO: 4/1/97: bushing joints are defined by the 3 sliding axes (which - * in SIMM are always the X, Y, and Z axes. If the SIMM rotation axes are - * not the same, then there's trouble modeling the joint. For now, assume - * that the rotation axes are the same, and use them to write out axis - * coordinates, since their order matters. - */ - else if (type == dpBushing) - { - NULLIFY_STRING(prescribed_string); - axisnum = 0; - /* Assuming that the rotation axes are the X, Y, and Z axes (see comment - * above), you want to find out the order of the rotations in advance, - * so you can process the translations in that order as well, even if they - * come before the rotations. - */ - if (jntsdf->dir == FORWARD) - { - for (i=0; i<3; i++) - { - double axis[4]; - ax = find_nth_rotation(jnt, i+1) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (EQUAL_WITHIN_ERROR(axis[0],1.0)) - axisname[i] = TX; - else if (EQUAL_WITHIN_ERROR(axis[1],1.0)) - axisname[i] = TY; - else - axisname[i] = TZ; - } - for (i=0; i<3; i++) /* TX, TY, TZ */ - { - jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++; - jnt->dofs[axisname[i]].sd.fixed = no; - if (jnt->dofs[axisname[i]].sd.constrained == yes) - { - jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign; - jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum; - jnt->dofs[axisname[i]].sd.axis = axisnum++; - } - for (i=0; i<3; i++) /* R1, R2, R3 */ - { - double axis[4]; - ax = find_nth_rotation(jnt, i+1) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - if (jnt->dofs[ax].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[ax].sd.name,buffer); - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.constrained = no; - jnt->dofs[ax].sd.fixed = yes; - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - } - else - { - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.fixed = no; - if (jnt->dofs[ax].sd.constrained == yes) - { - jnt->dofs[ax].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - if (jnt->dofs[ax].sd.constrained == yes) - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - else - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value; - } - } - } - else /* direction == INVERSE */ - { - for (i=0; i<3; i++) - { - double axis[4]; - ax = find_nth_rotation(jnt, 3-i) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (EQUAL_WITHIN_ERROR(axis[0],1.0)) - axisname[i] = TX; - else if (EQUAL_WITHIN_ERROR(axis[1],1.0)) - axisname[i] = TY; - else - axisname[i] = TZ; - } - for (i=0; i<3; i++) - { - double axis[4]; - ax = find_nth_rotation(jnt, 3-i) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark); - if (jnt->dofs[ax].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[ax].sd.name,buffer); - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.constrained = no; - jnt->dofs[ax].sd.fixed = yes; - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - } - else - { - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.fixed = no; - if (jnt->dofs[ax].sd.constrained == yes) - { - jnt->dofs[ax].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - if (jnt->dofs[ax].sd.constrained == yes) - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - else - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value; - } - } - for (i=0; i<3; i++) /* TX, TY, TZ */ - { - jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++; - jnt->dofs[axisname[i]].sd.fixed = no; - if (jnt->dofs[axisname[i]].sd.constrained == yes) - { - jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign; - jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum; - jnt->dofs[axisname[i]].sd.axis = axisnum++; - } - } - if (write_file == yes) - fprintf(fp, "prescribed =%s\n", prescribed_string); - } - else if (type == dpReverseBushing) - { - NULLIFY_STRING(prescribed_string); - axisnum = 0; - if (jntsdf->dir == FORWARD) - { - for (i=0; i<3; i++) - { - double axis[4]; - ax = find_nth_rotation(jnt, i+1) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (EQUAL_WITHIN_ERROR(axis[0],1.0)) - axisname[i] = TX; - else if (EQUAL_WITHIN_ERROR(axis[1],1.0)) - axisname[i] = TY; - else - axisname[i] = TZ; - } - for (i=0; i<3; i++) - { - double axis[4]; - ax = find_nth_rotation(jnt, i+1) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark); - if (jnt->dofs[ax].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[ax].sd.name,buffer); - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.constrained = no; - jnt->dofs[ax].sd.fixed = yes; - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - } - else - { - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.fixed = no; - if (jnt->dofs[ax].sd.constrained == yes) - { - jnt->dofs[ax].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - if (jnt->dofs[ax].sd.constrained == yes) - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - else - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value; - } - } - for (i=0; i<3; i++) /* TX, TY, TZ */ - { - jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++; - jnt->dofs[axisname[i]].sd.fixed = no; - if (jnt->dofs[axisname[i]].sd.constrained == yes) - { - jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign; - jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum; - jnt->dofs[axisname[i]].sd.axis = axisnum++; - } - } - else - { - for (i=0; i<3; i++) - { - double axis[4]; - ax = find_nth_rotation(jnt, 3-i) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (EQUAL_WITHIN_ERROR(axis[0],1.0)) - axisname[i] = TX; - else if (EQUAL_WITHIN_ERROR(axis[1],1.0)) - axisname[i] = TY; - else - axisname[i] = TZ; - } - for (i=0; i<3; i++) /* TX, TY, TZ */ - { - jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++; - jnt->dofs[axisname[i]].sd.fixed = no; - if (jnt->dofs[axisname[i]].sd.constrained == yes) - { - jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - else - { - strcat(prescribed_string," 0?"); - jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value; - } - jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign; - jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum; - jnt->dofs[axisname[i]].sd.axis = axisnum++; - } - for (i=0; i<3; i++) - { - double axis[4]; - ax = find_nth_rotation(jnt, 3-i) - 1; - COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis); - normalize_vector(axis, axis); - if (write_file == yes) - fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark); - if (jnt->dofs[ax].type == constant_dof) - { - strcat(prescribed_string," 1"); - sprintf(buffer,"fixed%d", *dofcount); - mstrcpy(&jnt->dofs[ax].sd.name,buffer); - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.constrained = no; - jnt->dofs[ax].sd.fixed = yes; - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - } - else - { - jnt->dofs[ax].sd.state_number = (*dofcount)++; - jnt->dofs[ax].sd.fixed = no; - if (jnt->dofs[ax].sd.constrained == yes) - { - jnt->dofs[ax].sd.error_number = (*constrainedcount)++; - strcat(prescribed_string," 0"); - } - else - strcat(prescribed_string," 0?"); - jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign; - jnt->dofs[ax].sd.joint = sdnum;//jointnum; - jnt->dofs[ax].sd.axis = axisnum++; - if (jnt->dofs[ax].sd.constrained == yes) - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value; - else - jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value; - } - } - } - if (write_file == yes) - fprintf(fp, "prescribed =%s\n", prescribed_string); - } - - if (write_file == yes) - fprintf(fp, "\n"); - - /* Now that you're done making the SD/FAST joint... - * If the joint in the SD/FAST code is the reverse direction of the - * joint in SIMM, change its type on the SIMM side so that the code - * that checks for model consistency does not complain. This is done - * only for planar and bushing joints (the others are not implemented - * as reverse joints in SD/FAST). - */ - if (jntsdf->dir == FORWARD) - { -#if 0 // JPL 12/2/08: nothing should be done for FORWARD, right? - if (jnt->type == dpReversePlanar) - jnt->type = dpPlanar; - else if (jnt->type == dpReverseBushing) - jnt->type = dpBushing; -#endif - } - else - { - if (jnt->type == dpPlanar) - jnt->type = dpReversePlanar; - else if (jnt->type == dpBushing) - jnt->type = dpReverseBushing; - } -} - - -void static write_loop_joints(FILE* fp, char seg_name[], SegmentSDF* seg, int addQuestionMarks) -{ - int i; - char* new_name; - - for (i=0; itimes_split; i++) - { - new_name = make_sdfast_seg_name(seg_name, i+1); - fprintf(fp, "body = %s inb = %s joint = weld\n", seg_name, new_name); - fprintf(fp, "bodytojoint = 0.0 0.0 0.0 inbtojoint = 0.0 0.0 0.0\n\n"); - FREE_IFNOTNULL(new_name); - } -} - - -/* VALID_SDFAST_MODEL: this routine checks the model to make sure that the - * current implementation of SIMM can correctly represent it with an SD/FAST - * input file. - */ - -SBoolean valid_sdfast_model(ModelStruct* ms) -{ - int i, jointnum, dofnum; - - for (i = 0; i < ms->numjoints; i++) - { - if (ms->joint[i].type != dpPlanar && - ms->joint[i].type != dpReversePlanar && - ms->joint[i].type != dpPin && - ms->joint[i].type != dpReversePin && - ms->joint[i].type != dpUniversal && - ms->joint[i].type != dpReverseUniversal && - ms->joint[i].type != dpWeld && - ms->joint[i].type != dpSlider && - ms->joint[i].type != dpCylindrical && - ms->joint[i].type != dpReverseCylindrical && - ms->joint[i].type != dpGimbal && - ms->joint[i].type != dpReverseGimbal && - ms->joint[i].type != dpBushing && - ms->joint[i].type != dpReverseBushing && - ms->joint[i].type != dpSkippable) - { - sprintf(errorbuffer, "Joint %s cannot be converted to an SD/FAST joint.", ms->joint[i].name); - return no; - } - } - - for (i = 0; i < ms->numgencoords; i++) - { - if (ms->gencoord[i]->used_in_model == yes) - { - if (!mark_unconstrained_dof(ms, ms->gencoord[i], &jointnum, &dofnum, NULL)) - { - sprintf(errorbuffer, "At least one DOF must be a \"simple\" function of gencoord %s (2 points, slope=1, passes thru zero).", - ms->gencoord[i]->name); - return no; - } - } - } - - return yes; -} - - -void write_forparams(char filename[], ModelStruct* model) -{ - int i, j, nq; - FILE* file; - DofStruct* dof; - char buf[CHARBUFFER]; - char fullpath[CHARBUFFER]; - char* time_string; - - file = simm_fopen(filename, "w"); - - if (file == NULL) - return; - - if (model->musclefilename) - { - if (is_absolute_path(model->musclefilename)) - { - strcpy(buf, model->musclefilename); - } - else - { - char* p; - - strcpy(buf, model->jointfilename); - p = strrchr(buf, DIR_SEP_CHAR); - - if (p) - p++; - else - p = buf; - strcpy(p, model->musclefilename); - } - } - make_time_string(&time_string); - - fprintf(file, "# PARAMS.TXT\n"); - fprintf(file, "# Dynamics Pipeline parameters file generated by %s\n", program_with_version); - fprintf(file, "# for use with the stand-alone version of a dynamic simulation.\n"); - fprintf(file, "# Created %s\n", time_string); - fprintf(file, "# Name of original SIMM joints file: %s\n", model->jointfilename); - fprintf(file, "#\n"); - fprintf(file, "# This file contains all of the parameters that you can specify at\n"); - fprintf(file, "# run-time in a dynamic simulation (stand-alone version). To change\n"); - fprintf(file, "# one of them, remove the '#' from the beginning of the line (which\n"); - fprintf(file, "# identifies the line as a comment), and change the parameter to the\n"); - fprintf(file, "# desired value.\n\n"); - - fprintf(file, "############################# I/O Options #############################\n"); - if (model->musclefilename) - fprintf(file, "muscle_file %s\n", buf); - else - fprintf(file, "#muscle_file model.msl\n"); - if (model->bonepathname) - { - if (model->jointfilename) - { - char* purePath = NULL; - get_pure_path_from_path(model->jointfilename, &purePath); - build_full_path(purePath, model->bonepathname, fullpath); - FREE_IFNOTNULL(purePath); - } - else - { - build_full_path(NULL, model->bonepathname, fullpath); - } - fprintf(file, "bone_path %s\n", fullpath); - } - else - { - fprintf(file, "#bone_path path_to_bone_files\n"); - } - fprintf(file, "output_motion_file results.mot\n"); - fprintf(file, "#kinetics_file forces.ktx\n"); - fprintf(file, "output_kinetics_file results.ktx\n\n"); - - fprintf(file, "######################## Integration Parameters #######################\n"); - fprintf(file, "start_time 0.0 #start time of simulation\n"); - fprintf(file, "end_time 1.0 #end time of simulation\n"); - fprintf(file, "step_size 0.01 #reporting interval for integrator\n\n"); - - fprintf(file, "######################### Segment Parameters ##########################\n"); - fprintf(file, "# You can specify mass properties here if you add '?' after their\n"); - fprintf(file, "# values in model.sd. Mass centers should only be changed by editing\n"); - fprintf(file, "# your SIMM joint file and re-saving the dynamics files.\n"); - fprintf(file, "# segment_name mass value\n"); - fprintf(file, "# segment_name inertia I11 I12 I13 I21 I22 I23 I31 I32 I33\n"); - for (i=1; inumjoints; i++) - for (j=0; j<6; j++) - if (model->joint[i].dofs[j].type == function_dof || - model->joint[i].dofs[j].sd.fixed == yes) - nq++; - - for (i=0; isd.name, dof->sd.initial_value); - } - fprintf(file, "\n"); - - fprintf(file, "########################### Output Options ############################\n"); - fprintf(file, "# this one is for output printed to the shell window\n"); - fprintf(file, "verbose no\n"); - fprintf(file, "# these are for output written to output_motion_file\n"); - fprintf(file, "output_gencoord_values yes\n"); - fprintf(file, "output_muscle_activations yes\n"); - fprintf(file, "output_muscle_lengths no\n"); - fprintf(file, "output_muscle_forces yes\n"); - fprintf(file, "output_muscle_fiber_lengths no\n"); - fprintf(file, "output_muscle_fiber_velocities no\n"); - fprintf(file, "output_muscle_moment_arms no\n"); - fprintf(file, "output_muscle_joint_torques no\n"); - fprintf(file, "output_total_muscle_joint_torques no\n"); - fprintf(file, "output_joint_torques no\n"); - fprintf(file, "output_corrected_joint_torques no\n"); - fprintf(file, "output_joint_reaction_forces no\n"); - fprintf(file, "output_joint_reaction_torques no\n"); - fprintf(file, "output_optimized_muscle_activations no\n"); - fprintf(file, "output_mass_center_positions no\n"); - fprintf(file, "output_mass_center_velocities no\n"); - fprintf(file, "output_system_energy no\n"); - fprintf(file, "output_contact_forces no\n"); - fprintf(file, "output_spring_forces no\n"); - fprintf(file, "num_output_spring_forces 10\n"); - fprintf(file, "\n"); - - fprintf(file, "####################### Force Matte Definitions #######################\n"); - fprintf(file, "# force mattes can be used to apply forces defined in the ground\n"); - fprintf(file, "# reference frame to some other body segment.\n"); - fprintf(file, "# force_matte matte_name bone_file_name segment_name\n"); - for (i = 0; i < model->numsegments; i++) - { - if (model->segment[i].forceMatte) - { - fprintf(file,"force_matte %s %s\t%s\n", model->segment[i].forceMatte->name, - model->segment[i].forceMatte->filename, model->segment[i].name); - } - } - fprintf(file, "\n"); - - fprintf(file, "##################### Contact Detection Variables #####################\n"); - fprintf(file, "# object object_name bone_file_name segment_name\n"); - fprintf(file, "# begin_group group_name object_name1 ... object_nameN end_group\n"); - fprintf(file, "# contact_pair object_name1 object_name2 coef_rest mu_dynamic mu_static\n\n"); - for (i=0; inumsegments; i++) - { - for (j=0; jsegment[i].numContactObjects; j++) - { - fprintf(file,"object %s %s\t%s\n", model->segment[i].contactObject[j].name, - model->segment[i].contactObject[j].filename, model->segment[i].name); - } - } - for (i=0; inumContactGroups; i++) - { - fprintf(file,"begin_group %s\n", model->contactGroup[i].name); - for (j=0; jcontactGroup[i].numElements; j++) - fprintf(file,"%s ", model->contactGroup[i].element[j]); - fprintf(file,"\nend_group\n"); - } - for (i=0; inumContactPairs; i++) - { - fprintf(file,"contact_pair %s %s %lf %lf %lf\n", - model->contactPair[i].body1, - model->contactPair[i].body2, - model->contactPair[i].restitution, - model->contactPair[i].mu_static, - model->contactPair[i].mu_dynamic); - } - - fprintf(file, "######################## Spring-based Contacts ########################\n"); - fprintf(file, "# spring_floor spring_floor_name file_name segment_name\n"); - fprintf(file, "# spring segment_name Px Py Pz spring_floor_name friction param1 param2 param3 param4 param5 param6\n"); - for (i=0; inumsegments; i++) - { - if (model->segment[i].springFloor) - { - fprintf(file,"spring_floor %s %s\t%s\n", - model->segment[i].springFloor->name, - model->segment[i].springFloor->filename, - model->segment[i].name); - } - for (j=0; jsegment[i].numSpringPoints; j++) - { - fprintf(file,"spring %s % 10.4f % 10.4f % 10.4f " - "%s % 10.4f % 12.4f % 12.4f % 12.4f % 12.4f % 12.4f % 12.4f\n", - model->segment[i].name, - model->segment[i].springPoint[j].point[0], - model->segment[i].springPoint[j].point[1], - model->segment[i].springPoint[j].point[2], - model->segment[model->segment[i].springPoint[j].floorSegment].springFloor->name, - model->segment[i].springPoint[j].friction, - model->segment[i].springPoint[j].param_a, - model->segment[i].springPoint[j].param_b, - model->segment[i].springPoint[j].param_c, - model->segment[i].springPoint[j].param_d, - model->segment[i].springPoint[j].param_e, - model->segment[i].springPoint[j].param_f); - } - } - fprintf(file, "\n"); - - fprintf(file, "##################### Muscle State Initialization #####################\n"); - fprintf(file, "# You can specify the initial values of the muscle model states for each\n"); - fprintf(file, "# muscle by un-commenting the appropriate line[s] below and replacing\n"); - fprintf(file, "# the zeros with the initial values. You must specify the state values\n"); - fprintf(file, "# in the order that the states are numbered in the muscle derivative\n"); - fprintf(file, "# function for that muscle model (in derivs.c). You can specify fewer\n"); - fprintf(file, "# than the total number of states in the muscle model, but the N values\n"); - fprintf(file, "# that you specify will be used to initialize the *first* N states in the\n"); - fprintf(file, "# model. If you specify more states than there are in the muscle model,\n"); - fprintf(file, "# the extra values will be ignored.\n"); - fprintf(file, "# muscle_name state1_init_value state2_init_value ...\n"); - fprintf(file, "#\n"); - for (i = 0; i < model->nummuscles; i++) - fprintf(file, "#%s 0.0 0.0 0.0\n", model->muscle[i]->name); - fprintf(file, "\n"); - - fprintf(file, "##################### Wrap Object Initialization #####################\n"); - fprintf(file, "# You can specify the parameters of any of the wrap objects, in case\n"); - fprintf(file, "# you want to change them from their values when sdfor.c was created.\n"); - fprintf(file, "# If you specify a wrap object's parameters here, you must specify\n"); - fprintf(file, "# all of its parameters, and you must use the following format:\n"); - fprintf(file, "#\n"); - fprintf(file, "# name type active algorithm segment dim1 dim2 dim3 quadrant tx ty tz rx ry rz\n"); - fprintf(file, "#\n"); - fprintf(file, "# Explanation of parameters:\n"); - fprintf(file, "# name: the name of the wrap object, as defined in the model when sdfor.c\n"); - fprintf(file, "# was created.\n"); - fprintf(file, "# type: specify \"sphere\", \"ellipsoid\", \"cylinder\", or \"torus\".\n"); - fprintf(file, "# active: is the wrap object active? specify \"1\" for yes, \"0\" for no.\n"); - fprintf(file, "# algorithm: default algorithm to use for ellipsoid objects, in case\n"); - fprintf(file, "# algorithm is not specified in the muscle definition. Specify\n"); - fprintf(file, "# \"hybrid\", \"midpoint\", or \"axial\". For sphere, cylinder, and\n"); - fprintf(file, "# torus objects, specify \"0\".\n"); - fprintf(file, "# segment: the name of the body segment that the object is attached to.\n"); - fprintf(file, "# dim1, dim2, dim3: for spheres, specify \" 0.0 0.0\"\n"); - fprintf(file, "# for ellipsoids, specify \" \"\n"); - fprintf(file, "# for cylinders, specify \" 0.0\"\n"); - fprintf(file, "# for torii, specify \" 0.0\"\n"); - fprintf(file, "# quadrant: to constrain wrapping over half of the wrap object, define the\n"); - fprintf(file, "# active region by specifying \"x\", \"-x\", \"y\", \"-y\", \"z\", or \"-z\".\n"); - fprintf(file, "# For unconstrained wrapping, specify \"all\".\n"); - fprintf(file, "# tx, ty, tz: the location of the wrap object's origin in the body segment's\n"); - fprintf(file, "# reference frame.\n"); - fprintf(file, "# rx, ry, rz: the XYZ Euler angles (body-fixed) expressing the orientation of\n"); - fprintf(file, "# the wrap object in the body segment's reference frame.\n"); - fprintf(file, "#\n"); - for (i = 0; i < model->num_wrap_objects; i++) - { - double xyz[3], radius[] = {0.0, 0.0, 0.0}; - dpWrapObject* wo = model->wrapobj[i]; - const char *wrap_quadrant = "all"; - - strcpy(buffer, wo->name); - convert_string(buffer, yes); - - recalc_xforms(wo); - extract_xyz_rot_bodyfixed(wo->from_local_xform, xyz); - - memcpy(radius, wo->radius, 3*sizeof(double)); - if (wo->wrap_type == dpWrapSphere) { - radius[1] = radius[2] = 0.0; - } else if (wo->wrap_type == dpWrapCylinder) { - radius[1] = wo->height; - radius[2] = 0.0; - } else if (wo->wrap_type == dpWrapTorus) { - radius[2] = 0.0; - } - - if (wo->wrap_sign != 0) - { - switch ((wo->wrap_axis + 1) * wo->wrap_sign) - { - default: break; - case 1: wrap_quadrant = "x"; break; - case -1: wrap_quadrant = "-x"; break; - case 2: wrap_quadrant = "y"; break; - case -2: wrap_quadrant = "-y"; break; - case 3: wrap_quadrant = "z"; break; - case -3: wrap_quadrant = "-z"; break; - } - } - - fprintf(file,"#%s %s %d %s %s %.10lf %.10lf %.10lf %s %.10lf %.10lf %.10lf %.4lf %.4lf %.4lf\n", - buffer, - get_wrap_type_name(wo->wrap_type), - (wo->active == yes) ? 1 : 0, - (wo->wrap_type == dpWrapEllipsoid) ? (char*)get_wrap_algorithm_name(wo->wrap_algorithm) : "0", - SDseg[get_sd_seg_num(model->segment[wo->segment].name)+1].name, - radius[0], radius[1], radius[2], - wrap_quadrant, - wo->translation.xyz[0], wo->translation.xyz[1], wo->translation.xyz[2], - xyz[0]*RTOD, xyz[1]*RTOD, xyz[2]*RTOD); - } - - fclose(file); - - sprintf(buffer, "Created: %s.", filename); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); -} - - -void write_dllparams(char filename[], ModelStruct* model) -{ - int i, j, nq; - FILE* file; - DofStruct* dof; - char* time_string; - - file = simm_fopen(filename, "w"); - - if (file == NULL) - return; - - make_time_string(&time_string); - - fprintf(file, "# DLLPARAMS.TXT\n"); - fprintf(file, "# Dynamics Pipeline parameters file generated by %s\n", program_with_version); - fprintf(file, "# for use with the DLL version of a dynamic simulation.\n"); - fprintf(file, "# Created %s\n", time_string); - fprintf(file, "# Name of original SIMM joints file: %s\n", model->jointfilename); - fprintf(file, "#\n"); - fprintf(file, "# This file contains all of the parameters that you can specify at\n"); - fprintf(file, "# run-time in a dynamic simulation (DLL version). To change one\n"); - fprintf(file, "# of them, remove the '#' from the beginning of the line (which\n"); - fprintf(file, "# identifies the line as a comment), and change the parameter to the\n"); - fprintf(file, "# desired value. This file is read by the simulation when you press\n"); - fprintf(file, "# the \"initialize\" button in the Dynamics Tool.\n\n"); - - fprintf(file, "####################### Gencoord Initialization #######################\n"); - fprintf(file, "# gencoord_name initial_value initial_velocity\n"); - fprintf(file, "#\n"); - for (i=0, nq=0; inumjoints; i++) - for (j=0; j<6; j++) - if (model->joint[i].dofs[j].type == function_dof || - model->joint[i].dofs[j].sd.fixed == yes) - nq++; - - for (i=0; isd.name, dof->sd.initial_value); - } - fprintf(file, "\n"); - - fprintf(file, "##################### Muscle State Initialization #####################\n"); - fprintf(file, "# You can specify the initial values of the muscle model states for each\n"); - fprintf(file, "# muscle by un-commenting the appropriate line[s] below and replacing\n"); - fprintf(file, "# the zeros with the initial values. You must specify the state values\n"); - fprintf(file, "# in the order that the states are numbered in the muscle derivative\n"); - fprintf(file, "# function for that muscle model (in derivs.c). You can specify fewer\n"); - fprintf(file, "# than the total number of states in the muscle model, but the N values\n"); - fprintf(file, "# that you specify will be used to initialize the *first* N states in the\n"); - fprintf(file, "# model. If you specify more states than there are in the muscle model,\n"); - fprintf(file, "# the extra values will be ignored.\n"); - fprintf(file, "# muscle_name state1_init_value state2_init_value ...\n"); - fprintf(file, "#\n"); - for (i = 0; i < model->nummuscles; i++) - fprintf(file, "#%s 0.0 0.0 0.0\n", model->muscle[i]->name); - fprintf(file, "\n"); - - fclose(file); - - sprintf(buffer, "Created: %s.", filename); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); -} - - -void write_dllcontactparams(char filename[], ModelStruct* model) -{ - int i, j, nq; - FILE* file; - char bonepath[CHARBUFFER]; - char* time_string; - - file = simm_fopen(filename, "w"); - - if (file == NULL) - return; - - make_time_string(&time_string); - - /* Make a string to hold the bone path so it can be added to - * each contact object file name. - */ - if (model->bonepathname) - { - if (model->jointfilename) - { - char* purePath = NULL; - get_pure_path_from_path(model->jointfilename, &purePath); - build_full_path(purePath, model->bonepathname, bonepath); - FREE_IFNOTNULL(purePath); - } - else - { - build_full_path(NULL, model->bonepathname, bonepath); - } - } - else - { - strcpy(bonepath, "."); - } - - fprintf(file, "# DLLCONTACTPARAMS.TXT\n"); - fprintf(file, "# Dynamics Pipeline contact parameters file generated by %s\n", program_with_version); - fprintf(file, "# for use with the DLL version of a dynamic simulation.\n"); - fprintf(file, "# Created %s\n", time_string); - fprintf(file, "# Name of original SIMM joints file: %s\n", model->jointfilename); - fprintf(file, "#\n"); - fprintf(file, "# This file contains all of the parameters needed for rigid-body contact\n"); - fprintf(file, "# detection (the parameters specified between the keywords \"begincontact\"\n"); - fprintf(file, "# and \"endcontact\" in the joint file). They are written to this file\n"); - fprintf(file, "# every time the \"initialize\" button is pressed in the Dynamics Tool\n"); - fprintf(file, "# because they are not included in the dpModelStruct object that is passed\n"); - fprintf(file, "# to the DLL at that time.\n\n"); - - fprintf(file, "##################### Contact Detection Variables #####################\n"); - for (i=0; inumsegments; i++) - { - for (j=0; jsegment[i].numContactObjects; j++) - { - fprintf(file,"object %s %s\\%s\t%s\n", model->segment[i].contactObject[j].name, - bonepath, model->segment[i].contactObject[j].filename, model->segment[i].name); - } - } - for (i=0; inumContactGroups; i++) - { - fprintf(file,"begin_group %s\n", model->contactGroup[i].name); - for (j=0; jcontactGroup[i].numElements; j++) - fprintf(file,"%s ", model->contactGroup[i].element[j]); - fprintf(file,"\nend_group\n"); - } - for (i=0; inumContactPairs; i++) - { - fprintf(file,"contact_pair %s %s %lf %lf %lf\n", - model->contactPair[i].body1, - model->contactPair[i].body2, - model->contactPair[i].restitution, - model->contactPair[i].mu_static, - model->contactPair[i].mu_dynamic); - } - - fclose(file); - - sprintf(buffer, "Created: %s.", filename); - message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET); -} - diff --git a/OpenSim/Utilities/simmToOpenSim/sdftools.c b/OpenSim/Utilities/simmToOpenSim/sdftools.c deleted file mode 100644 index 66d5a73443..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/sdftools.c +++ /dev/null @@ -1,1845 +0,0 @@ -/******************************************************************************* - - SDFTOOLS.C - - Author: Peter Loan - - Date: 18-FEB-04 - - Copyright (c) 2004 MusculoGraphics, Inc. - All rights reserved. - - Description: - - Routines: - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "sdfunctions.h" - - -/*************** DEFINES (for this file only) *********************************/ - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static char constrained_suffix[] = "_con"; -char* ground_name[] = {"$ground", "ground"}; - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ -extern SDSegment* SDseg; -extern int num_SD_segs; -extern int* joint_order; - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static SBoolean is_xyz_joint(JointStruct* jnt); -static SBoolean axes_are_parallel(double axis1[], double axis2[], SBoolean oppositeDirAllowed); -static SBoolean segment_affects_dynamics(ModelStruct* ms, int segNum); -static void freeDPDefaultMuscle(dpMuscleStruct* dm); -static void freeDPMuscle(dpMuscleStruct* ms, dpMuscleStruct* dm); -static void freeDPMusclePaths(dpMusclePathStruct path[], int num); -static void freeDPFunction(dpFunction* sf); -static dpQStruct* get_q_for_gencoord(const char gencoord_name[], dpModelStruct* dp); -static int get_sd_floor_num(dpModelStruct* dp, char* name); -static void copyConstraintsToDPConstraints(ModelStruct* ms, dpModelStruct* dp); -static ReturnCode copyMusclesToDP(ModelStruct* ms, dpModelStruct* dp, int muscleList[]); -static dpWrapObject* getDPWrapObject(dpModelStruct* dp, char name[]); -static void copyWrapObjectsToDP(ModelStruct* ms, dpModelStruct* dp); -static void copyQsToDP(ModelStruct* ms, dpModelStruct* dp); -static void copySegmentsToDP(ModelStruct* ms, dpModelStruct* dp); - - - -dpJointType identify_joint_type(ModelStruct* ms, int jointnum) -{ - int i, rot_order_index; - int num_rot_constants = 0, num_rot_functions = 0; - int num_trans_constants = 0, num_trans_functions = 0, num_real_rot_constants = 0; - double axis1[3], axis2[3], axis3[3]; - JointStruct* jnt; - - jnt = &ms->joint[jointnum]; - - /* First give the joint a one-token name. In most cases, this is just - * the user-given name of the joint. However, if that name has special - * characters in it (e.g. -), those characters must be removed. Also, - * if the name starts with a number, then an underscore is prepended. - */ - FREE_IFNOTNULL(jnt->sd_name); - jnt->sd_name = (char*)simm_malloc(strlen(jnt->name) + 2); - strcpy(jnt->sd_name, jnt->name); - convert_string(jnt->sd_name, yes); - - /* Constant, non-zero rotations are not supported in SD/FAST. So to - * implement them, you treat them as gencoords, and then prescribe - * their motion to be constant (like weld joints). - */ - for (i=0; i<3; i++) - { - if (jnt->dofs[i].type == constant_dof && EQUAL_WITHIN_ERROR(jnt->dofs[i].value,0.0)) - num_rot_constants++; - else - num_rot_functions++; - } - - for (i=3; i<6; i++) - { - if (jnt->dofs[i].type == constant_dof) - num_trans_constants++; - else - num_trans_functions++; - } - - /* For the purposes of optimizing the pipeline code, you want to remove - * from the model segments that: - * (1) are leaf nodes (are used in only one joint and are not ground), - * (2) have no gencoords in their joint, - * (3) have zero mass, and - * (4) have no muscle points, wrap objects, or constraint objects. - * Such segments are often used for holding markers, and so are not needed - * in the dynamic simulation. - */ - for (i = 0; i < 3; i++) - { - if (jnt->dofs[i].type == constant_dof) - num_real_rot_constants++; - } - - /* If the joint has no degrees of freedom, check to see if one of - * the segments is a leaf node. - */ - if (num_real_rot_constants == 3 && num_trans_constants == 3) - { - int parent_count = 0, child_count = 0, leaf = -1; - - for (i = 0; i < ms->numjoints; i++) - { - if (jnt->from == ms->joint[i].from || jnt->from == ms->joint[i].to) - parent_count++; - if (jnt->to == ms->joint[i].from || jnt->to == ms->joint[i].to) - child_count++; - } - - /* If either of the segments in this joint is not ground and is - * used in exactly one joint, then it is potentially skippable. - */ - if (parent_count == 1 && jnt->from != ms->ground_segment) - leaf = jnt->from; - else if (child_count == 1 && jnt->to != ms->ground_segment) - leaf = jnt->to; - - if (leaf >= 0 && segment_affects_dynamics(ms, leaf) == no) - return dpSkippable; - } - - if (num_rot_functions == 0 && num_trans_functions == 0) - return dpWeld; - - if (num_rot_functions == 3 && num_trans_functions == 3) - { - /* In SD/FAST, bushing joints use the axes of rotation as - * the axes of translation as well. Thus for a SIMM joint - * to properly convert to an SD/FAST bushing, the rotation - * axes must be X, Y, and Z, since the translations are - * always w.r.t. these axes. - */ - if (is_xyz_joint(jnt) == no) - return dpUnknownJoint; - - if (jnt->order[TRANS] == 0) /* translation is first */ - return dpBushing; - else if (jnt->order[TRANS] == 3) /* translation is last */ - return dpReverseBushing; - else - return dpUnknownJoint; - } - - if (num_rot_functions == 1 && num_trans_functions == 0) - { - /* If the one rotation happens after the translations, - * then this is a [normal] pin joint. If it happens - * before, then this is a reverse pin joint, and the - * translations have to be added to the other body segment. - */ - for (i=0; i<3; i++) - { - if (jnt->dofs[i].type == function_dof || - NOT_EQUAL_WITHIN_ERROR(jnt->dofs[i].value,0.0)) - break; - } - if (i == 3) - return dpUnknownJoint; - rot_order_index = i + 1; - if (jnt->order[TRANS] < jnt->order[rot_order_index]) - return dpPin; - else - return dpReversePin; - } - - if (num_rot_functions == 0 && num_trans_functions == 1) - return dpSlider; - - if (num_rot_functions == 3 && num_trans_functions == 0) - { - if (jnt->order[TRANS] == 0) - return dpGimbal; - else if (jnt->order[TRANS] == 3) - return dpReverseGimbal; - else - return dpUnknownJoint; - } - - if (num_rot_functions == 2 && num_trans_functions == 0) - { - if (jnt->order[TRANS] == 0) - return dpUniversal; - else if (jnt->order[TRANS] == 3) - return dpReverseUniversal; - else - return dpUnknownJoint; - } - - if (num_rot_functions == 1 && num_trans_functions == 1) - { - (void)find_rotation_axis(jnt, axis1); - find_translation_axis(jnt, axis2, function_dof, 0); - if (axes_are_parallel(axis1, axis2, no) == yes) - { - /* If the one rotation happens after the translations, - * then this is a [normal] cylinder joint. If it happens - * before, then this is a reverse cylinder joint, and the - * translations have to be added to the other body segment. - */ - for (i=0; i<3; i++) - { - if (jnt->dofs[i].type == function_dof || NOT_EQUAL_WITHIN_ERROR(jnt->dofs[i].value,0.0)) - break; - } - if (i == 3) - return dpUnknownJoint; - rot_order_index = i + 1; - if (jnt->order[TRANS] < jnt->order[rot_order_index]) - return dpCylindrical; - else - return dpReverseCylindrical; - } - else - return dpUnknownJoint; - } - - if (num_rot_functions == 1 && num_trans_functions == 2) - { - (void)find_rotation_axis(jnt, axis1); - (void)find_translation_axis(jnt, axis2, function_dof, 0); - (void)find_translation_axis(jnt, axis3, function_dof, 1); - /* As long as the rotation axis is not parallel to either of - * the translation axes, and the translation is not in the - * middle of the transformation order, this is a valid planar joint. - */ - if (axes_are_parallel(axis1, axis2, yes) == yes || - axes_are_parallel(axis1, axis3, yes) == yes) - { - return dpUnknownJoint; - } - else - { - if (jnt->order[TRANS] == 0) /* translation is first */ - return dpPlanar; - else if (jnt->order[TRANS] == 3) /* translation is last */ - return dpReversePlanar; - else - return dpUnknownJoint; - } - } - - return dpUnknownJoint; - -} - - -void find_sdfast_joint_order(ModelStruct* ms, JointSDF jnts[], - SegmentSDF segs[], int joint_order[], int ground_name_index) -{ -#define UNPROCESSED -2 - - int i, joints_done = 0, num_skippable = 0, joints_used = 0; - - for (i=0; inumjoints; i++) - { - jnts[i].used = no; - ms->joint[i].sd_num = UNPROCESSED; - } - - for (i=0; inumsegments; i++) - { - segs[i].used = no; - segs[i].times_split = 0; - segs[i].mass_factor = 1.0; - } - - segs[ms->ground_segment].used = yes; - for (i=0; inumjoints; i++) - { - if (ms->joint[i].from == ms->ground_segment) - { - joint_order[joints_done] = i; - ms->joint[i].sd_num = joints_done++; - segs[ms->joint[i].to].used = yes; - jnts[i].used = yes; - jnts[i].dir = FORWARD; - jnts[i].inbname = (char*)simm_malloc(strlen(ground_name[ground_name_index]) + 2); - strcpy(jnts[i].inbname, ground_name[ground_name_index]); - jnts[i].outbname = (char*)simm_malloc(strlen(ms->segment[ms->joint[i].to].name) + 2); - strcpy(jnts[i].outbname, ms->segment[ms->joint[i].to].name); - jnts[i].closes_loop = no; - } - else if (ms->joint[i].to == ms->ground_segment) - { - joint_order[joints_done] = i; - ms->joint[i].sd_num = joints_done++; - segs[ms->joint[i].from].used = yes; - jnts[i].used = yes; - jnts[i].dir = INVERSE; - jnts[i].inbname = (char*)simm_malloc(strlen(ground_name[ground_name_index]) + 2); - strcpy(jnts[i].inbname, ground_name[ground_name_index]); - jnts[i].outbname = (char*)simm_malloc(strlen(ms->segment[ms->joint[i].from].name) + 2); - strcpy(jnts[i].outbname, ms->segment[ms->joint[i].from].name); - jnts[i].closes_loop = no; - } - } - - joints_used = joints_done; - while (joints_done + num_skippable < ms->numjoints) - { - for (i=0; inumjoints; i++) - { - if (jnts[i].used == yes) - continue; - if (segs[ms->joint[i].from].used == no && segs[ms->joint[i].to].used == no) - continue; - else if (ms->joint[i].type == dpSkippable) - { - if (ms->joint[i].sd_num == UNPROCESSED) - { - ms->joint[i].sd_num = -1; - joint_order[joints_done] = i; - num_skippable++; - jnts[i].used = no; - } - continue; - } - else if (segs[ms->joint[i].from].used == no) - { - joint_order[joints_done++] = i; - ms->joint[i].sd_num = joints_used++; - segs[ms->joint[i].from].used = yes; - jnts[i].used = yes; - jnts[i].dir = INVERSE; - jnts[i].inbname = (char*)simm_malloc(strlen(ms->segment[ms->joint[i].to].name) + 2); - strcpy(jnts[i].inbname, ms->segment[ms->joint[i].to].name); - jnts[i].outbname = (char*)simm_malloc(strlen(ms->segment[ms->joint[i].from].name) + 2); - strcpy(jnts[i].outbname, ms->segment[ms->joint[i].from].name); - jnts[i].closes_loop = no; - } - else if (segs[ms->joint[i].to].used == no) - { - joint_order[joints_done++] = i; - ms->joint[i].sd_num = joints_used++; - segs[ms->joint[i].to].used = yes; - jnts[i].used = yes; - jnts[i].dir = FORWARD; - jnts[i].inbname = (char*)simm_malloc(strlen(ms->segment[ms->joint[i].from].name) + 2); - strcpy(jnts[i].inbname, ms->segment[ms->joint[i].from].name); - jnts[i].outbname = (char*)simm_malloc(strlen(ms->segment[ms->joint[i].to].name) + 2); - strcpy(jnts[i].outbname, ms->segment[ms->joint[i].to].name); - jnts[i].closes_loop = no; - } - else - { - joint_order[joints_done++] = i; - ms->joint[i].sd_num = joints_used++; - jnts[i].used = yes; - jnts[i].dir = FORWARD; - segs[ms->joint[i].to].mass_factor += 1.0; - segs[ms->joint[i].to].times_split++; - jnts[i].inbname = (char*)simm_malloc(strlen(ms->segment[ms->joint[i].from].name) + 2); - strcpy(jnts[i].inbname, ms->segment[ms->joint[i].from].name); - jnts[i].outbname = make_sdfast_seg_name(ms->segment[ms->joint[i].to].name, - segs[ms->joint[i].to].times_split); - jnts[i].closes_loop = yes; - /* ms->numclosedloops++; loops are now counted by markLoopJoints() JPL 3/5/01 */ - } - } - } - -#if ! SIMMTOOPENSIM - /* Remove all special characters in the body segment names, so that the strings - * are valid one-token C strings. Do not touch the ground segment name ($ground) - * because SD/FAST requires exactly that name. - */ - for (i=0; inumjoints; i++) - { - if (ms->joint[i].type == dpSkippable) - continue; - if (STRINGS_ARE_NOT_EQUAL(jnts[i].inbname, ground_name[ground_name_index])) - convert_string(jnts[i].inbname, yes); - if (STRINGS_ARE_NOT_EQUAL(jnts[i].outbname, ground_name[ground_name_index])) - convert_string(jnts[i].outbname, yes); - } -#endif -} - - -static SBoolean is_xyz_joint(JointStruct* jnt) -{ - int i; - double axis[4]; - SBoolean x_taken = no, y_taken = no, z_taken = no; - - for (i=0; i<3; i++) - { - COPY_1X4VECTOR(jnt->parentrotaxes[i], axis); - normalize_vector(axis, axis); - if (x_taken == no && axes_are_parallel(axis, x_axis, no) == yes) - { - x_taken = yes; - break; - } - else if (y_taken == no && axes_are_parallel(axis, y_axis, no) == yes) - { - y_taken = yes; - break; - } - else if (z_taken == no && axes_are_parallel(axis, z_axis, no) == yes) - { - z_taken = yes; - break; - } - return no; - } - - return yes; -} - - -static SBoolean axes_are_parallel(double axis1[], double axis2[], SBoolean oppositeDirAllowed) -{ - - if (oppositeDirAllowed == yes) - { - if (EQUAL_WITHIN_ERROR(DABS(axis1[0]),DABS(axis2[0])) && - EQUAL_WITHIN_ERROR(DABS(axis1[1]),DABS(axis2[1])) && - EQUAL_WITHIN_ERROR(DABS(axis1[2]),DABS(axis2[2]))) - return yes; - else - return no; - } - else - { - if (EQUAL_WITHIN_ERROR(axis1[0],axis2[0]) && - EQUAL_WITHIN_ERROR(axis1[1],axis2[1]) && - EQUAL_WITHIN_ERROR(axis1[2],axis2[2])) - return yes; - else - return no; - } - -} - - - -/* Finds the first rotation-dof in a joint that is either a function, or - * a non-zero constant, then returns the axis of that rotation. - */ -int find_rotation_axis(JointStruct* jnt, double axis[]) -{ - int i; - - axis[0] = axis[1] = axis[2] = 0.0; - - for (i=0; i<3; i++) - { - if (jnt->dofs[i].type == function_dof) - break; - else if (NOT_EQUAL_WITHIN_ERROR(jnt->dofs[i].value,0.0)) - break; - } - - if (i == 3) - return 0; /* TODO: should return -1, but not ready to handle the error */ - - axis[0] = jnt->parentrotaxes[i][0]; - axis[1] = jnt->parentrotaxes[i][1]; - axis[2] = jnt->parentrotaxes[i][2]; - - normalize_vector(axis, axis); - - return i; -} - - - -/* Finds the Nth translation-dof in a joint that matches the DofType passed in, - * then returns the axis of that translation. - */ -int find_translation_axis(JointStruct* jnt, double axis[], DofType type, int num) -{ - int i, count = 0; - - axis[0] = axis[1] = axis[2] = 0.0; - - for (i = 3; i < 6; i++) - { - if (jnt->dofs[i].type == type) - { - if (count++ == num) - break; - } - } - - if (i == 6) - return 0; /* TODO: should return -1, but not ready to handle the error */ - - axis[i - 3] = 1.0; - - return i; -} - - -char* get_joint_type_name(dpJointType type, Direction dir) -{ - - if (type == dpPin || type == dpReversePin) - return ("pin"); - else if (type == dpCylindrical || type == dpReverseCylindrical) - return ("cylinder"); - else if (type == dpPlanar) - { - if (dir == FORWARD) - return ("planar"); - else - return ("rplanar"); - } - else if (type == dpReversePlanar) - { - if (dir == FORWARD) - return ("rplanar"); - else - return ("planar"); - } - else if (type == dpSlider) - return ("slider"); - else if (type == dpUniversal || type == dpReverseUniversal) - return ("ujoint"); - else if (type == dpGimbal || type == dpReverseGimbal) - return ("gimbal"); - else if (type == dpWeld) - return ("pin"); /* welds are prescribed pins */ - else if (type == dpBushing) - { - if (dir == FORWARD) - return ("bushing"); - else - return ("rbushing"); - } - else if (type == dpReverseBushing) - { - if (dir == FORWARD) - return ("rbushing"); - else - return ("bushing"); - } - - return ("unknown"); - -} - -static SBoolean segment_affects_dynamics(ModelStruct* ms, int segNum) -{ - int i, j; - SegmentStruct* seg; - - if (segNum < 0 || segNum >= ms->numsegments) - return yes; - - seg = &ms->segment[segNum]; - - if (NOT_EQUAL_WITHIN_ERROR(seg->mass, 0.0)) - return yes; - - for (i = 0; i < ms->num_wrap_objects; i++) - { - if (ms->wrapobj[i]->segment == segNum) - return yes; - } - - for (i = 0; i < ms->nummuscles; i++) - { - for (j = 0; j < ms->muscle[i]->path->num_points; j++) - if (ms->muscle[i]->path->mp[j]->segment == segNum) - return yes; - } - - return no; -} - - -void name_dofs(ModelStruct* ms) -{ - int i, j; - - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - if (ms->joint[i].dofs[j].sd.constrained == no) - { - GeneralizedCoord* gencoord = ms->joint[i].dofs[j].gencoord; - ms->joint[i].dofs[j].sd.name = (char*)simm_malloc(strlen(gencoord->name) + 2); - strcpy(ms->joint[i].dofs[j].sd.name, gencoord->name); - convert_string(ms->joint[i].dofs[j].sd.name, yes); - ms->joint[i].dofs[j].sd.con_name = NULL; - } - else - { - char namebuffer[CHARBUFFER]; - strcat3(namebuffer, ms->joint[i].name, "_", getjointvarname(j), CHARBUFFER); - convert_string(namebuffer, yes); - mstrcpy(&ms->joint[i].dofs[j].sd.name, namebuffer); - strcat(namebuffer, constrained_suffix); - mstrcpy(&ms->joint[i].dofs[j].sd.con_name, namebuffer); - } - } - } -} - - -void check_dynamic_params(ModelStruct* ms) -{ - int i, j; - - ms->dynamics_ready = yes; - - for (i=0; inumsegments; i++) - { - if (STRINGS_ARE_NOT_EQUAL(ms->segment[i].name,"ground")) - { - if (ms->segment[i].mass_specified == no && - ms->segment[i].masscenter_specified == no && - ms->segment[i].inertia_specified == no) - { - sprintf(errorbuffer, "No mass properties specified for segment %s.", ms->segment[i].name); - error(none, errorbuffer); - ms->dynamics_ready = no; - } - else - { - if (ms->segment[i].mass_specified == no) - { - sprintf(errorbuffer, "Mass not specified for segment %s.", ms->segment[i].name); - error(none, errorbuffer); - ms->dynamics_ready = no; - } - if (ms->segment[i].masscenter_specified == no) - { - sprintf(errorbuffer, "Masscenter not specified for segment %s.", ms->segment[i].name); - error(none, errorbuffer); - ms->dynamics_ready = no; - } - if (ms->segment[i].inertia_specified == no) - { - sprintf(errorbuffer, "Inertia matrix not specified for segment %s.", ms->segment[i].name); - error(none, errorbuffer); - ms->dynamics_ready = no; - } - } - } - for (j = 0; j < ms->segment[i].numContactObjects; j++) - { - if (ms->segment[i].contactObject[j].poly == NULL) - { - sprintf(errorbuffer, "The bone file for contact object %s was not found.", ms->segment[i].contactObject[j].name); - error(none, errorbuffer); - ms->dynamics_ready = no; - } - } - } -} - - -void freeDPModelStruct(dpModelStruct* dp) -{ - int i, j; - - FREE_IFNOTNULL(dp->name); - - FREE_IFNOTNULL(dp->contacts); - FREE_IFNOTNULL(dp->bilat_contacts); - - if (dp->spring) - { - for (i = 0; i < dp->num_springs; i++) - FREE_IFNOTNULL(dp->spring[i].floor_name); - FREE_IFNOTNULL(dp->spring); - } - - if (dp->spring_floor) - { - for (i = 0; i < dp->num_spring_floors; i++) - { - FREE_IFNOTNULL(dp->spring_floor[i].name); - if (dp->spring_floor[i].ph) - { - freeDPPolyhedron(dp->spring_floor[i].ph); - FREE_IFNOTNULL(dp->spring_floor[i].ph); - } - } - FREE_IFNOTNULL(dp->spring_floor); - } - - if (dp->force_matte) - { - for (i = 0; i < dp->num_force_mattes; i++) - { - FREE_IFNOTNULL(dp->force_matte[i].name); - if (dp->force_matte[i].ph) - { - freeDPPolyhedron(dp->force_matte[i].ph); - FREE_IFNOTNULL(dp->force_matte[i].ph); - } - } - FREE_IFNOTNULL(dp->force_matte); - } - - if (dp->q) - { - for (i = 0; i < dp->nq; i++) - { - FREE_IFNOTNULL(dp->q[i].name); - freeDPFunction(dp->q[i].restraint_function); - freeDPFunction(dp->q[i].min_restraint_function); - freeDPFunction(dp->q[i].max_restraint_function); - freeDPFunction(dp->q[i].constraint_function); - } - FREE_IFNOTNULL(dp->q); - } - - if (dp->body_segment) - { - for (i = 0; i < dp->num_body_segments; i++) - { - FREE_IFNOTNULL(dp->body_segment[i].name); - for (j = 0; j < dp->body_segment[i].num_objects; j++) - { - freeDPPolyhedron(dp->body_segment[i].object[j]); - FREE_IFNOTNULL(dp->body_segment[i].object[j]); - } - FREE_IFNOTNULL(dp->body_segment[i].object); - } - FREE_IFNOTNULL(dp->body_segment); - } - - if (dp->muscles) - { - for (i = 0; i < dp->num_muscles; i++) - freeDPMuscle(&dp->muscles[i], &dp->default_muscle); - FREE_IFNOTNULL(dp->muscles); - } - - freeDPDefaultMuscle(&dp->default_muscle); - - FREE_IFNOTNULL(dp->joint); - - if (dp->wrap_object) - { - for (i = 0; i < dp->num_wrap_objects; i++) - FREE_IFNOTNULL(dp->wrap_object[i].name); - FREE_IFNOTNULL(dp->wrap_object); - } - - if (dp->constraint_object) - { - for (i = 0; i < dp->num_constraint_objects; i++) - { - FREE_IFNOTNULL(dp->constraint_object[i].name); - for (j = 0; j < dp->constraint_object[i].numPoints; j++) - FREE_IFNOTNULL(dp->constraint_object[i].points[j].name); - FREE_IFNOTNULL(dp->constraint_object[i].points); - } - FREE_IFNOTNULL(dp->constraint_object); - } - - FREE_IFNOTNULL(dp); -} - -static void freeDPDefaultMuscle(dpMuscleStruct* dm) -{ - if (dm) - { - int i; - - FREE_IFNOTNULL(dm->name); - FREE_IFNOTNULL(dm->max_isometric_force); - FREE_IFNOTNULL(dm->pennation_angle); - FREE_IFNOTNULL(dm->optimal_fiber_length); - FREE_IFNOTNULL(dm->resting_tendon_length); - FREE_IFNOTNULL(dm->max_contraction_vel); - FREE_IFNOTNULL(dm->momentarms); - FREE_IFNOTNULL(dm->tendon_force_len_func); - FREE_IFNOTNULL(dm->active_force_len_func); - FREE_IFNOTNULL(dm->passive_force_len_func); - FREE_IFNOTNULL(dm->force_vel_func); - FREE_IFNOTNULL(dm->excitation_func); - - if (dm->dynamic_param_names) - { - for (i = 0; i < dm->num_dynamic_params; i++) - FREE_IFNOTNULL(dm->dynamic_param_names[i]); - FREE_IFNOTNULL(dm->dynamic_param_names); - } - - if (dm->dynamic_params) - { - for (i = 0; i < dm->num_dynamic_params; i++) - FREE_IFNOTNULL(dm->dynamic_params[i]); - FREE_IFNOTNULL(dm->dynamic_params); - } - - FREE_IFNOTNULL(dm->muscle_model_index); - if (dm->wrapStruct) - { - for (i = 0; i < dm->numWrapStructs; i++) - { - FREE_IFNOTNULL(dm->wrapStruct[i]->wrap_object); - FREE_IFNOTNULL(dm->wrapStruct[i]); - } - FREE_IFNOTNULL(dm->wrapStruct); - } - } -} - -/* does not free the path (path) */ -static void freeDPMuscle(dpMuscleStruct* ms, dpMuscleStruct* dm) -{ - if (ms && dm) - { - int i; - - if (ms->name != dm->name) - FREE_IFNOTNULL(ms->name); - if (ms->path) - { - for (i = 0; i < ms->path->num_orig_points; i++) - FREE_IFNOTNULL(ms->path->mp_orig[i].wrap_pts); - FREE_IFNOTNULL(ms->path->mp_orig); - FREE_IFNOTNULL(ms->path->mp); - } - FREE_IFNOTNULL(ms->path); - if (ms->max_isometric_force != dm->max_isometric_force) - FREE_IFNOTNULL(ms->max_isometric_force); - if (ms->pennation_angle != dm->pennation_angle) - FREE_IFNOTNULL(ms->pennation_angle); - if (ms->optimal_fiber_length != dm->optimal_fiber_length) - FREE_IFNOTNULL(ms->optimal_fiber_length); - if (ms->resting_tendon_length != dm->resting_tendon_length) - FREE_IFNOTNULL(ms->resting_tendon_length); - if (ms->max_contraction_vel != dm->max_contraction_vel) - FREE_IFNOTNULL(ms->max_contraction_vel); - FREE_IFNOTNULL(ms->momentarms); - if (ms->tendon_force_len_func != dm->tendon_force_len_func) - FREE_IFNOTNULL(ms->tendon_force_len_func); - if (ms->active_force_len_func != dm->active_force_len_func) - FREE_IFNOTNULL(ms->active_force_len_func); - if (ms->passive_force_len_func != dm->passive_force_len_func) - FREE_IFNOTNULL(ms->passive_force_len_func); - if (ms->force_vel_func != dm->force_vel_func) - FREE_IFNOTNULL(ms->force_vel_func); - if (ms->excitation_func != dm->excitation_func) - FREE_IFNOTNULL(ms->excitation_func); - - /* The dynamic_param_names should always be equal to the default muscle's, - * which should always be equal to the model's array of names. - */ - if (ms->dynamic_param_names && ms->dynamic_param_names != dm->dynamic_param_names) - { - for (i = 0; i < ms->num_dynamic_params; i++) - FREE_IFNOTNULL(ms->dynamic_param_names[i]); - FREE_IFNOTNULL(ms->dynamic_param_names); - } - /* The dynamic_params array (of pointers) is always unique to each muscle, - * but the doubles that they point to could be in the default muscle. - */ - if (ms->dynamic_params) - { - for (i = 0; i < ms->num_dynamic_params; i++) - { - if (ms->dynamic_params[i] != dm->dynamic_params[i]) - FREE_IFNOTNULL(ms->dynamic_params[i]); - } - FREE_IFNOTNULL(ms->dynamic_params); - } - - if (ms->muscle_model_index != dm->muscle_model_index) - FREE_IFNOTNULL(ms->muscle_model_index); - if (ms->wrapStruct && ms->wrapStruct != dm->wrapStruct) - { - for (i = 0; i < ms->numWrapStructs; i++) - { - //FREE_IFNOTNULL(ms->wrapStruct[i]->wrap_object); the wrap object is freed later - FREE_IFNOTNULL(ms->wrapStruct[i]); - } - FREE_IFNOTNULL(ms->wrapStruct); - } - } -} - -void freeDPPolyhedron(dpPolyhedronStruct* ph) -{ - if (ph) - { - int i; - - FREE_IFNOTNULL(ph->name); - for (i = 0; i < ph->num_vertices; i++) - FREE_IFNOTNULL(ph->vertex[i].polygons); - for (i = 0; i < ph->num_polygons; i++) - FREE_IFNOTNULL(ph->polygon[i].vertex_index); - FREE_IFNOTNULL(ph->vertex); - FREE_IFNOTNULL(ph->polygon); - } -} - - -static void freeDPFunction(dpFunction* sf) -{ - if (sf) - { - FREE_IFNOTNULL(sf->x); - FREE_IFNOTNULL(sf->y); - FREE_IFNOTNULL(sf->b); - FREE_IFNOTNULL(sf->c); - FREE_IFNOTNULL(sf->d); - } -} - -/* copy the SIMM model into the dp model structure */ - -dpModelStruct* copyModelToDPModel(ModelStruct* ms, int muscleList[]) -{ - int i, j, k, segNum, count; - ReturnCode rc; - dpQStruct* gencoord; - GeneralizedCoord *temp_gc; - dpQStruct* temp_q; - dpModelStruct* dp; - - dp = (dpModelStruct*)simm_calloc(1, sizeof(dpModelStruct)); - - dp->simmModel = (dpSimmModelID)ms; - - /* This is only set to yes by set_up_kinetics_input() in the - * simulation code. - */ - dp->newInverseSimulation = dpNo; - - /* To fill in the dpModelStruct, you need to know how - * the SIMM segments/joints are mapped to the SD/FAST segments/joints. - * If there are loops in the model, this is not a one-to-one mapping. - * So you need to call make_sdfast_model() to create the mapping. - */ - check_dynamic_params(ms); - if (ms->dynamics_ready == no) - { - sprintf(errorbuffer, "Unable to create dynamic simulation for %s", ms->name); - error(none, errorbuffer); - freeDPModelStruct(dp); - return NULL; - } - - for (i = 0; i < ms->numjoints; i++) - ms->joint[i].type = identify_joint_type(ms, i); - - if (valid_sdfast_model(ms) == no) - { - sprintf(buffer, "Unable to create dynamic simulation for %s", ms->name); - error(none, buffer); - error(none, errorbuffer); /* errorbuffer was filled in by valid_sdfast_model() */ - error(none, "Consult the SD/FAST manual or email technical support for more details."); - freeDPModelStruct(dp); - return NULL; - } - - if (make_sdfast_model(ms, NULL, no, 0) != code_fine) - { - sprintf(errorbuffer, "Unable to create dynamic simulation for %s", ms->name); - error(none, errorbuffer); - freeDPModelStruct(dp); - return NULL; - } - - /* If you made it to here, then SDseg[] was filled in with the segment info, - * and the Q info was stored in the model's joint array. So now you can copy - * the info to the dpModelStruct. - */ - - mstrcpy(&dp->name, ms->name); - - /* Set the gravity vector appropriately. */ - switch (ms->gravity) - { - case smNoAlign: - dp->gravity[0] = dp->gravity[1] = dp->gravity[2] = 0.0; - break; - case smX: - dp->gravity[1] = dp->gravity[2] = 0.0; - dp->gravity[0] = 9.80665; - break; - case smNegX: - dp->gravity[1] = dp->gravity[2] = 0.0; - dp->gravity[0] = -9.80665; - break; - case smY: - dp->gravity[0] = dp->gravity[2] = 0.0; - dp->gravity[1] = 9.80665; - break; - case smNegY: - dp->gravity[0] = dp->gravity[2] = 0.0; - dp->gravity[1] = -9.80665; - break; - case smZ: - dp->gravity[0] = dp->gravity[1] = 0.0; - dp->gravity[2] = 9.80665; - break; - case smNegZ: - default: - dp->gravity[0] = dp->gravity[1] = 0.0; - dp->gravity[2] = -9.80665; - break; - } - - dp->num_closed_loops = ms->numclosedloops; - - /* Most of the Q struct has to be copied after the constraint functions, because it contains - * pointers to functions. However, the number of Qs is needed for copying constraint - * objects, so it is calculated here. - */ - for (i = 0, dp->nq = 0; i < ms->numjoints; i++) - { - for (j = 0; j < 6; j++) - if (ms->joint[i].dofs[j].type == function_dof || ms->joint[i].dofs[j].sd.fixed == yes) - dp->nq++; - } - - /* Copy the segment info. */ - copySegmentsToDP(ms, dp); - - /* The entire array of joint structs is created and filled in by - * init_joints() in the simulation code, using the information - * returned by sdjnt(). However, some of the joint information is - * filled in here so that the simulation can make sure that the - * passed-in model matches the one built from model.sd. - */ - dp->num_joints = 0; - if (ms->numjoints > 0 && joint_order) - { - dp->joint = (dpJointStruct*)simm_calloc(ms->numjoints, sizeof(dpJointStruct)); - for (i = 0; i < ms->numjoints; i++) - { - if (joint_order[i] >= 0 && ms->joint[joint_order[i]].type != dpSkippable && - ms->joint[joint_order[i]].type != dpUnknownJoint) - dp->joint[dp->num_joints++].jnt_type = ms->joint[joint_order[i]].type; - } - dp->joint = (dpJointStruct*)simm_realloc(dp->joint, dp->num_joints * sizeof(dpJointStruct), &rc); - } - else - { - dp->joint = NULL; - } - - /* Copy the wrap objects. */ - copyWrapObjectsToDP(ms, dp); - - // DKB Nov. 2009 - moved copy muscles after copy Qs since muscles may have references to Qs - - /* Copy the spring floors. */ - /* First count how many floors there are, since they are not stored in one array. - * Each body segment has either 0 or 1 floors. Make sure the floor has a valid - * polyhedron, or else skip over it. - */ - for (i = 0, count = 0; i < ms->numsegments; i++) - if (ms->segment[i].springFloor && ms->segment[i].springFloor->poly) - count++; - - dp->num_spring_floors = dp->spring_floor_array_size = count; - if (dp->num_spring_floors > 0) - { - dp->spring_floor = (dpSpringFloor*)simm_malloc(dp->num_spring_floors * sizeof(dpSpringFloor)); - for (i = 0, count = 0; i < ms->numsegments; i++) - { - SpringFloor* floor = ms->segment[i].springFloor; - if (floor && floor->poly) - { - dp->spring_floor[count].segment = get_sd_seg_num(ms->segment[i].name); - mstrcpy(&dp->spring_floor[count].name, floor->name); - /* The plane information is taken from the polyhedron's first polygon. */ - copyPolyhedronToDPPolyhedron(floor->poly, &dp->spring_floor[count].ph, i, dp->spring_floor[count].segment, ms); - dp->spring_floor[count].plane.a = dp->spring_floor[count].ph->polygon[0].normal[0]; - dp->spring_floor[count].plane.b = dp->spring_floor[count].ph->polygon[0].normal[1]; - dp->spring_floor[count].plane.c = dp->spring_floor[count].ph->polygon[0].normal[2]; - dp->spring_floor[count].plane.d = dp->spring_floor[count].ph->polygon[0].d; - count++; - } - } - } - else - { - dp->spring_floor = NULL; - } - - /* Copy the spring points. - * First count how many points there are, since they are not stored in one array. - * But only count the spring points that are linked to a spring floor that has - * a valid polyhedron. - */ - for (i = 0, count = 0; i < ms->numsegments; i++) - { - for (j = 0; j < ms->segment[i].numSpringPoints; j++) - { - segNum = ms->segment[i].springPoint[j].floorSegment; - if (ms->segment[segNum].springFloor && ms->segment[segNum].springFloor->poly) - count++; - } - } - - dp->num_springs = dp->spring_array_size = count; - if (dp->num_springs > 0) - { - dp->spring = (dpSpringStruct*)simm_malloc(dp->num_springs * sizeof(dpSpringStruct)); - for (i = 0, count = 0; i < ms->numsegments; i++) - { - SegmentStruct* seg = &ms->segment[i]; - for (j = 0; j < seg->numSpringPoints; j++) - { - segNum = seg->springPoint[j].floorSegment; - if (ms->segment[segNum].springFloor && ms->segment[segNum].springFloor->poly) - { - dp->spring[count].segment = get_sd_seg_num(seg->name); - dp->spring[count].floor = get_sd_floor_num(dp, ms->segment[segNum].springFloor->name); - mstrcpy(&dp->spring[count].floor_name, ms->segment[segNum].springFloor->name); - for (k = 0; k < 3; k++) - { - dp->spring[count].point[k] = seg->springPoint[j].point[k] - seg->masscenter[k]; - dp->spring[count].force[k] = 0.0; - } - dp->spring[count].friction = seg->springPoint[j].friction; - dp->spring[count].param_a = seg->springPoint[j].param_a; - dp->spring[count].param_b = seg->springPoint[j].param_b; - dp->spring[count].param_c = seg->springPoint[j].param_c; - dp->spring[count].param_d = seg->springPoint[j].param_d; - dp->spring[count].param_e = seg->springPoint[j].param_e; - dp->spring[count].param_f = seg->springPoint[j].param_f; - count++; - } - } - } - } - else - { - dp->spring = NULL; - } - - /* Copy the force mattes. */ - /* First count how many mattes there are, since they are not stored in one array. - * Each body segment has either 0 or 1 mattes. If the matte's polyhedron is missing - * (e.g., because the bone file could not be found), skip the matte. - */ - for (i = 0, count = 0; i < ms->numsegments; i++) - if (ms->segment[i].forceMatte && ms->segment[i].forceMatte->poly) - count++; - - dp->num_force_mattes = dp->force_matte_array_size = count; - if (dp->num_force_mattes > 0) - { - dp->force_matte = (dpForceMatte*)simm_malloc(dp->num_force_mattes * sizeof(dpForceMatte)); - for (i = 0, count = 0; i < ms->numsegments; i++) - { - ContactObject* matte = ms->segment[i].forceMatte; - if (matte && matte->poly) - { - dp->force_matte[count].segment = get_sd_seg_num(ms->segment[i].name); - mstrcpy(&dp->force_matte[count].name, matte->name); - copyPolyhedronToDPPolyhedron(matte->poly, &dp->force_matte[count].ph, i, dp->force_matte[count].segment, ms); - count++; - } - } - } - else - { - dp->force_matte = NULL; - } - - /* Copy the constraint objects. */ - copyConstraintsToDPConstraints(ms, dp); - - /* Copy the functions, which are used for kinematic constraints, gencoord restraints - * and moving muscle point definitions. */ - dp->num_functions = countUsedFunctions(ms); - if (dp->num_functions > 0) - { - int index = 0; - dp->function = (dpFunction**)simm_calloc(dp->num_functions, sizeof(dpFunction*)); - for (i = 0; i < ms->func_array_size; i++) - { - if (ms->function[i] && ms->function[i]->used == dpYes) - { - dpFunction* from = ms->function[i]; - dpFunction *to = NULL; - dp->function[index] = (dpFunction*)simm_calloc(1, sizeof(dpFunction)); - to = dp->function[index++]; - // DKB Sept 17, 2009 added malloc because copy expects coeff_array_size to be set and memory to be allocated - malloc_function(to, from->coefficient_array_size); - copy_function(from, to); - } - } - } - else - { - dp->function = NULL; - } - - /* Copy the Q data. This has to be done after the constraint functions are copied - * because the Qs contain pointers to the functions. However, the number of Qs - * is calculated earlier, outside this function. - */ - copyQsToDP(ms, dp); - - /* Copy the muscles. */ - if (copyMusclesToDP(ms, dp, muscleList) == code_bad) //moved after the copy Qs because muscles may refer to Qs - { - sprintf(errorbuffer, "Unable to create dynamic simulation for %s - error in muscles", ms->name); - error(none, errorbuffer); - freeDPModelStruct(dp); - return NULL; - } - - for (i = 0; i < dp->num_muscles; i++) - { - for (j = 0; j < dp->muscles[i].path->num_orig_points; j++) - { - if (dp->muscles[i].path->mp_orig[j].isVia) - { - temp_q = (dpQStruct *)dp->muscles[i].path->mp_orig[j].viaRange.gencoord; - printf("muscle %s has via point for %s\n", dp->muscles[i].name, temp_q->name); - } - } - for (j = 0; j < dp->muscles[i].path->num_orig_points; j++) - { - if (dp->muscles[i].path->mp_orig[j].isMovingPoint) - { - if (dp->muscles[i].path->mp_orig[j].gencoord[0]) - { - temp_q = (dpQStruct *)dp->muscles[i].path->mp_orig[j].gencoord[0]; - printf("muscle %s has mmpX point for %s\n", dp->muscles[i].name, temp_q->name); - } - if (dp->muscles[i].path->mp_orig[j].gencoord[1]) - { - temp_q = (dpQStruct *)dp->muscles[i].path->mp_orig[j].gencoord[1]; - printf("muscle %s has mmpY point for %s\n", dp->muscles[i].name, temp_q->name); - } - if (dp->muscles[i].path->mp_orig[j].gencoord[2]) - { - temp_q = (dpQStruct *)dp->muscles[i].path->mp_orig[j].gencoord[2]; - printf("muscle %s has mmpZ point for %s\n", dp->muscles[i].name, temp_q->name); - } - } - } - } - - return dp; -} - -static dpQStruct* get_q_for_gencoord(const char gencoord_name[], dpModelStruct* dp) -{ - int i; - - for (i = 0; i < dp->nq; i++) - { - if (STRINGS_ARE_EQUAL(gencoord_name, dp->q[i].name)) - return &dp->q[i]; - } - - return NULL; -} - - -static int get_sd_floor_num(dpModelStruct* dp, char* name) -{ - int i; - - for (i = 0; i < dp->num_spring_floors; i++) - { - if (STRINGS_ARE_EQUAL(name, dp->spring_floor[i].name)) - return i; - } - - return -1; -} - - -static void copyConstraintsToDPConstraints(ModelStruct* ms, dpModelStruct* dp) -{ - int i, j, k, constraintNum; - double tmp_mat[4][4], tmp_inv[4][4]; - - /* Always turn on enforce_constraints for the dpModel (whether or not there - * are any active constraint objects). This parameter is only turned off by - * the simulation code if instructed to do so by a motion. - */ - dp->enforce_constraints = 1; - - /* The SD/FAST constraint numbers for the constraint points start after - * the last Q constraint, and are numbered consecutively. - */ - constraintNum = dp->nq - (ms->numgencoords - ms->numunusedgencoords); - - dp->num_constraint_objects = ms->num_constraint_objects; - if (dp->num_constraint_objects > 0) - { - dp->constraint_object = (dpConstraintObject*)simm_malloc(dp->num_constraint_objects * sizeof(dpConstraintObject)); - for (i = 0; i < dp->num_constraint_objects; i++) - { - ConstraintObject* from = &ms->constraintobj[i]; - dpConstraintObject* to = &dp->constraint_object[i]; - SegmentStruct* seg = &ms->segment[from->segment]; - mstrcpy(&to->name, from->name); - to->constraint_type = from->constraintType; - to->active = from->active; - to->segment = get_sd_seg_num(seg->name); - for (j = 0; j < 3; j++) - to->co_radius[j] = from->radius.xyz[j]; - to->height = from->height; - to->constraint_axis = from->constraintAxis; - to->constraint_sign = from->constraintSign; - to->numPoints = to->cp_array_size = from->numPoints; - - /* Form new transform matrices from the mass center of the segment to the - * constraint object, rather than from the origin of the segment. SD/FAST expects - * all segment-specific points (e.g., muscle points) to be specified w.r.t. - * the segment's mass center. - */ - copy_4x4matrix(from->from_local_xform, tmp_mat); - for (j = 0; j < 3; j++) - tmp_mat[3][j] -= seg->masscenter[j]; - invert_4x4transform(tmp_mat, tmp_inv); - copy_4x4matrix(tmp_mat, to->from_local_xform); - copy_4x4matrix(tmp_inv, to->to_local_xform); - to->plane.a = from->plane.a; - to->plane.b = from->plane.b; - to->plane.c = from->plane.c; - to->plane.d = from->plane.d; - - to->points = (dpConstraintPoint*)simm_malloc(to->numPoints * sizeof(dpConstraintPoint)); - for (j = 0; j < to->numPoints; j++) - { - seg = &ms->segment[from->points[j].segment]; - mstrcpy(&to->points[j].name, from->points[j].name); - to->points[j].segment = get_sd_seg_num(seg->name); - to->points[j].offset[XX] = from->points[j].offset[XX] - seg->masscenter[XX]; - to->points[j].offset[YY] = from->points[j].offset[YY] - seg->masscenter[YY]; - to->points[j].offset[ZZ] = from->points[j].offset[ZZ] - seg->masscenter[ZZ]; - to->points[j].weight = from->points[j].weight; - to->points[j].constraint_num = constraintNum++; - } - } - } - else - { - dp->constraint_object = NULL; - } -} - -/* copy muscles from the SIMM model structure to the DP structure - * when copied from the ModelStruct, all via point gencoords, moving muscle point gencoords, - * excitation abscissas , are refering to GENCOORDS (GeneralizedCoordStruct) they need - * to be converted to Qs (dpQStruct) - * Assuming the Qs have been copied to the dp struct, convert - * and ?? wrpa object gencoords*/ -static ReturnCode copyMusclesToDP(ModelStruct* ms, dpModelStruct* dp, int muscleList[]) -{ - int i, j, index; - - dp->num_muscles = 0; - dp->muscles = NULL; - - if (muscleList == NULL) - return code_fine; - - for (i = 0; i < ms->nummuscles; i++) - if (muscleList[i]) - dp->num_muscles++; - - /* If there are no muscles to copy, don't copy the default muscle; just return. */ - if (dp->num_muscles == 0) - return code_fine; - - if (copy_default_muscle_dp(ms->default_muscle, &dp->default_muscle, dp) == code_bad) - return code_bad; - - dp->muscles = (dpMuscleStruct*)simm_malloc(dp->num_muscles * sizeof(dpMuscleStruct)); - - for (i = 0, index = 0; i < ms->nummuscles; i++) - { - if (muscleList[i]) - { - if (copy_muscle_dp(ms->muscle[i], &dp->muscles[index++], ms->default_muscle, &dp->default_muscle, dp) == code_bad) - { - // if there's a problem, not going to create model, need to free muscle struct - FREE_IFNOTNULL(dp->muscles); - - return code_bad; - } - - /* DKB: Sept. 2009 - when making the pipeline model, some segments are skippable, - * they don't affect the dynamics, so they are not included in the model. Therefore, - * the order of the segments in SIMM (ms) and pipeline (dp) DO NOT CORRESPOND! All - * segment references need to be updated so they are correct in the DP model. - * As well, the pipeline assumes that all muscle points are relative to the segment's mass - * center, not it's origin. When the muscle file is read in in the stand-alone, the mass-center - * is subtracted. Make sure to do this here for all muscle points. - */ - for (j = 0; j < dp->muscles[index-1].path->num_orig_points; j++) - { - int simm_seg = dp->muscles[index-1].path->mp_orig[j].segment; - dp->muscles[index-1].path->mp_orig[j].segment = get_sd_seg_num(ms->segment[simm_seg].name); - dp->muscles[index-1].path->mp_orig[j].point[XX] -= ms->segment[simm_seg].masscenter[XX]; - dp->muscles[index-1].path->mp_orig[j].point[YY] -= ms->segment[simm_seg].masscenter[YY]; - dp->muscles[index-1].path->mp_orig[j].point[ZZ] -= ms->segment[simm_seg].masscenter[ZZ]; - } - /* Note: because the wrapStruct has a wrap_object pointer to the actual wrap_object in - * the dpModel, the segment stored in the dp model does not need to be adjusted. */ - - } - } - return code_fine; -} - -static dpWrapObject* getDPWrapObject(dpModelStruct* dp, char name[]) -{ - int i; - - if (name == NULL) - return NULL; - - for (i = 0; i < dp->num_wrap_objects; i++) - if (STRINGS_ARE_EQUAL(name, dp->wrap_object[i].name)) - return &dp->wrap_object[i]; - - return NULL; -} - - -static void copyWrapObjectsToDP(ModelStruct* ms, dpModelStruct* dp) -{ - int i; - - dp->num_wrap_objects = ms->num_wrap_objects; - - if (dp->num_wrap_objects == 0) - { - dp->wrap_object = NULL; - return; - } - - dp->wrap_object = (dpWrapObject*)simm_malloc(dp->num_wrap_objects * sizeof(dpWrapObject)); - - for (i = 0; i < dp->num_wrap_objects; i++) - { - dpWrapObject* from = ms->wrapobj[i]; - dpWrapObject* to = &dp->wrap_object[i]; - - memcpy(to, from, sizeof(dpWrapObject)); - mstrcpy(&to->name, from->name); -//check that xform is being copied properly, make sure that from->segment is the correct index dkb - to->segment = get_sd_seg_num(ms->segment[from->segment].name); - to->from_local_xform[3][XX] -= ms->segment[from->segment].masscenter[XX]; - to->from_local_xform[3][YY] -= ms->segment[from->segment].masscenter[YY]; - to->from_local_xform[3][ZZ] -= ms->segment[from->segment].masscenter[ZZ]; - invert_4x4transform(to->from_local_xform, to->to_local_xform); - } -} - - -static void copyQsToDP(ModelStruct* ms, dpModelStruct* dp) -{ - int i, j; - DofStruct* dof; - JointStruct* jnt; - dpQStruct* q; - GeneralizedCoord* gc; - - dp->nu = dp->nq; - dp->num_gencoords = 0; - - if (dp->nq == 0) - { - dp->q = NULL; - return; - } - - dp->q = (dpQStruct*)simm_malloc(dp->nq * sizeof(dpQStruct)); - - for (i = 0; i < dp->nq; i++) - { - dof = find_nth_q_dof(ms, i); - jnt = find_nth_q_joint(ms, i); - gc = dof->gencoord; - q = &dp->q[i]; - - mstrcpy(&q->name, dof->sd.name); - - if (dof->sd.fixed == yes) - { - q->type = dpFixedQ; - } - else if (dof->sd.constrained == no) - { - /* Locked gencoords are modeled as fixed Qs (as of version 4.1.1). */ - if (gc && gc->locked == yes) - q->type = dpFixedQ; - else - q->type = dpUnconstrainedQ; - } - else - { - q->type = dpConstrainedQ; - } - q->joint = jnt->sd_num; - q->axis = dof->sd.axis; - q->conversion = dof->sd.conversion; - q->initial_value = dof->sd.initial_value; - q->initial_velocity = 0.0; - - if (dof->sd.constrained == no && dof->sd.fixed == no) - { - q->range_start = gc->range.start; - q->range_end = gc->range.end; - q->pd_stiffness = gc->pd_stiffness; - } - else - { - q->range_start = -99999.9; - q->range_end = 99999.9; - q->pd_stiffness = 0.0; - } - if (dof->sd.fixed == yes || dof->sd.constrained == yes) - { - q->restraint_function = NULL; - q->min_restraint_function = NULL; - q->max_restraint_function = NULL; - q->function_active = dpNo; - } - else - { - if (gc->restraint_function) - { - // The index of the functions in the ModelStruct and the dpModelStruct should be the same. - q->restraint_function = dp->function[getFunctionIndex(ms, gc->restraint_function)]; - q->min_restraint_function = NULL; - q->max_restraint_function = NULL; - if (gc->restraintFuncActive == yes) - q->function_active = dpYes; - else - q->function_active = dpNo; - } - else - { - q->restraint_function = NULL; - q->function_active = dpNo; - - // The index of the functions in the ModelStruct and the dpModelStruct should be the same. - if (gc->min_restraint_function) - q->min_restraint_function = dp->function[getFunctionIndex(ms, gc->min_restraint_function)]; - else - q->min_restraint_function = NULL; - - if (gc->max_restraint_function) - q->max_restraint_function = dp->function[getFunctionIndex(ms, gc->max_restraint_function)]; - else - q->max_restraint_function = NULL; - } - } - if (dof->sd.constrained == yes) - { - DofStruct* ind_dof = find_unconstrained_sd_dof(ms, dof->gencoord); - // The index of the functions in the ModelStruct and the dpModelStruct should be the same. - q->constraint_function = dp->function[getFunctionIndex(ms, dof->function)]; - q->constraint_num = dof->sd.error_number; - q->q_ind = ind_dof->sd.state_number; - } - else - { - q->constraint_function = NULL; - q->constraint_num = -1; - q->q_ind = -1; - } - - if (dof->sd.fixed == yes || dof->sd.constrained == yes) - q->output = dpNo; - else - q->output = dpYes; - - q->torque = 0.0; - - if (q->type == dpUnconstrainedQ) - dp->num_gencoords++; - } - -} - - -static void copySegmentsToDP(ModelStruct* ms, dpModelStruct* dp) -{ - int i, j, k; - - dp->num_body_segments = num_SD_segs; - - dp->body_segment = (dpBodyStruct*)simm_malloc(dp->num_body_segments * sizeof(dpBodyStruct)); - - for (i = 0; i < num_SD_segs; i++) - { - mstrcpy(&dp->body_segment[i].name, SDseg[i].name); - dp->body_segment[i].output = (i > 0) ? dpYes : dpNo; - dp->body_segment[i].mass = SDseg[i].mass; - for (j = 0; j < 3; j++) - { - dp->body_segment[i].mass_center[j] = SDseg[i].mass_center[j]; - dp->body_segment[i].body_to_joint[j] = SDseg[i].body_to_joint[j]; - dp->body_segment[i].inboard_to_joint[j] = SDseg[i].inboard_to_joint[j]; - } - for (j = 0; j < 3; j ++) - for (k = 0; k < 3; k++) - dp->body_segment[i].inertia[j][k] = SDseg[i].inertia[j][k]; - - /* These fields are filled in later by the simulation code. */ - dp->body_segment[i].contactable = dpNo; - dp->body_segment[i].contact_joints = NULL; - for (j = 0; j < 3; j++) - { - dp->body_segment[i].contact_force[j] = 0.0; - dp->body_segment[i].impact_force[j] = 0.0; - dp->body_segment[i].impact_point[j] = 0.0; - } - - /* If a SIMM segment with contactable objects gets split into more - * than one SD segment, you want to copy the objects only to the - * [one] SD segment with a valid simm_segment value. - */ - if (SDseg[i].simm_segment >= 0) - { - SegmentStruct* seg = &ms->segment[SDseg[i].simm_segment]; - dp->body_segment[i].object = (dpPolyhedronStruct**)simm_malloc(seg->numContactObjects * sizeof(dpPolyhedronStruct*)); - dp->body_segment[i].num_objects = seg->numContactObjects; - for (j = 0; j < seg->numContactObjects; j++) - copyPolyhedronToDPPolyhedron(seg->contactObject[j].poly, &dp->body_segment[i].object[j], - SDseg[i].simm_segment, i - 1, ms); - } - else - { - dp->body_segment[i].num_objects = 0; - dp->body_segment[i].object = NULL; - } - } -} - - -void copyPolyhedronToDPPolyhedron(PolyhedronStruct* from, dpPolyhedronStruct** to, - int SimmSegNum, int SDSegNum, ModelStruct* ms) -{ - int i, j; - dpPolyhedronStruct* ph; - dpVertexStruct* v; - - ph = *to = (dpPolyhedronStruct*)simm_malloc(sizeof(dpPolyhedronStruct)); - - mstrcpy(&ph->name, from->name); - ph->body_segment = SDSegNum; - ph->bc.x1 = from->bc.x1 - ms->segment[SimmSegNum].masscenter[XX]; - ph->bc.x2 = from->bc.x2 - ms->segment[SimmSegNum].masscenter[XX]; - ph->bc.y1 = from->bc.y1 - ms->segment[SimmSegNum].masscenter[YY]; - ph->bc.y2 = from->bc.y2 - ms->segment[SimmSegNum].masscenter[YY]; - ph->bc.z1 = from->bc.z1 - ms->segment[SimmSegNum].masscenter[ZZ]; - ph->bc.z2 = from->bc.z2 - ms->segment[SimmSegNum].masscenter[ZZ]; - - ph->num_vertices = from->num_vertices; - ph->vertex = (dpVertexStruct*)simm_malloc(ph->num_vertices * sizeof(dpVertexStruct)); - for (i = 0; i < ph->num_vertices; i++) - { - for (j = 0; j < 3; j++) - { - ph->vertex[i].coord[j] = from->vertex[i].coord[j] - ms->segment[SimmSegNum].masscenter[j]; - ph->vertex[i].normal[j] = from->vertex[i].normal[j]; - } - ph->vertex[i].polygon_count = from->vertex[i].polygon_count; - ph->vertex[i].polygons = (int*)simm_malloc(ph->vertex[i].polygon_count * sizeof(int)); - for (j = 0; j < ph->vertex[i].polygon_count; j++) - ph->vertex[i].polygons[j] = from->vertex[i].polygons[j]; - } - - ph->num_polygons = from->num_polygons; - ph->polygon = (dpPolygonStruct*)simm_malloc(ph->num_polygons * sizeof(dpPolygonStruct)); - for (i = 0; i < ph->num_polygons; i++) - { - ph->polygon[i].normal[XX] = from->polygon[i].normal[XX]; - ph->polygon[i].normal[YY] = from->polygon[i].normal[YY]; - ph->polygon[i].normal[ZZ] = from->polygon[i].normal[ZZ]; - ph->polygon[i].num_vertices = from->polygon[i].num_vertices; - ph->polygon[i].vertex_index = (int*)simm_malloc(ph->polygon[i].num_vertices * sizeof(int)); - for (j = 0; j < ph->polygon[i].num_vertices; j++) - ph->polygon[i].vertex_index[j] = from->polygon[i].vertex_index[j]; - v = &ph->vertex[ph->polygon[i].vertex_index[0]]; - ph->polygon[i].d = -v->coord[XX] * ph->polygon[i].normal[XX] - - v->coord[YY] * ph->polygon[i].normal[YY] - - v->coord[ZZ] * ph->polygon[i].normal[ZZ]; - } -} - - -/* This routine finds the nth rotation [axis] in a joint. For example, - * if the order of the joint is: r1 t r3 r2, r1 is the first rotation, - * r3 is the second rotation, and r2 is the third rotation. order[0] - * holds the position of the translation, order[1] holds the position - * of the r1 rotation, etc. - */ -int find_nth_rotation(JointStruct* jnt, int n) -{ - int i, min = 9, min_index, max = -9, max_index; - - for (i=1; i<4; i++) - { - if (jnt->order[i] < min) - { - min = jnt->order[i]; - min_index = i; - } - if (jnt->order[i] > max) - { - max = jnt->order[i]; - max_index = i; - } - } - - if (n == 1) - return min_index; - - if (n == 3) - return max_index; - - for (i=1; i<4; i++) - if (i != min_index && i != max_index) - return i; - - return -1; -} - -/* returns the dof structure which corresponds to the nth Q in the SD/FAST code */ - -DofStruct* find_nth_q_dof(ModelStruct* ms, int n) -{ - int i, j; - - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - if ((ms->joint[i].dofs[j].type == function_dof || - ms->joint[i].dofs[j].sd.fixed == yes) && - ms->joint[i].dofs[j].sd.state_number == n) - return &ms->joint[i].dofs[j]; - } - } - - return NULL; -} - - -/* returns the joint structure which contains the nth Q in the SD/FAST code */ - -JointStruct* find_nth_q_joint(ModelStruct* ms, int n) -{ - int i, j; - - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - if (ms->joint[i].dofs[j].type == function_dof && - ms->joint[i].dofs[j].sd.state_number == n) - return &ms->joint[i]; - if (ms->joint[i].dofs[j].sd.fixed == yes && - ms->joint[i].dofs[j].sd.state_number == n) - return &ms->joint[i]; - } - } - - return NULL; -} - - -DofStruct* find_unconstrained_sd_dof(ModelStruct* ms, GeneralizedCoord* gencoord) -{ - int i, j; - - for (i=0; inumjoints; i++) - { - for (j=0; j<6; j++) - { - if (ms->joint[i].dofs[j].sd.constrained == no && - ms->joint[i].dofs[j].gencoord == gencoord) - return &ms->joint[i].dofs[j]; - } - } - - return NULL; -} - - -/* GET_SD_SEG_NUM: given a segment name, this function returns the - * number of that segment in the Pipeline code. It assumes that the - * SDseg[] array has already been filled (which happens as the - * SD/FAST model file is generated). - */ -int get_sd_seg_num(char simm_name[]) -{ - int i; - char* name; - - /* When SDseg[] was created, the body segment names were - * cleaned-up using convert_string(). So call it here too so - * that the names will match exactly. Malloc enough space for - * 'name' so that there's room for a leading underscore, if - * convert_string needs to add one. - */ - name = (char*)simm_malloc(strlen(simm_name) + 2); - strcpy(name, simm_name); - convert_string(name, yes); - - for (i=0; i -#endif -#ifdef WIN32 -#pragma warning( disable : 4996 ) -#endif - -static void printUsage(char programName[]); -ReturnCode write_opensim_model(ModelStruct* ms, char filename[], char geometryDirectory[], - const char* markerSetOut, int angleUnits); - - -static void printUsage(char programName[]) -{ - printf("Usage: %s -j joints_in [-m muscles_in] -x xml_out [-ms markerset_out] [-g geometry_directory] [-a degrees | radians]\n", programName); -} - - -int main(int argc, char* argv[]) -{ - Scene* scene; - ModelStruct* model; - char *jointIn = NULL, *muscleIn = NULL, *xmlOut = NULL, *geometryDirectory = NULL; - char* markerSetOut = NULL; - int angleUnits = RADIANS; - -#if OPENSIM_BUILD - printf("simmToOpenSim, version %s, build date %s %s\n", OpenSimVersion, __TIME__, __DATE__); -#else - printf("simmToOpenSim, version %s\n", program_version); -#endif - - if (argc < 5) - { - printUsage(argv[0]); - exit(0); - } - else - { - int i; - - for (i = 1; i < argc - 1; i++) - { - if (STRINGS_ARE_EQUAL_CI(argv[i], "-J")) - { - jointIn = argv[i+1]; - i++; - } - else if (STRINGS_ARE_EQUAL_CI(argv[i], "-M")) - { - muscleIn = argv[i+1]; - i++; - } - else if (STRINGS_ARE_EQUAL_CI(argv[i], "-X")) - { - xmlOut = argv[i+1]; - i++; - } - else if (STRINGS_ARE_EQUAL_CI(argv[i], "-MS")) - { - markerSetOut = argv[i+1]; - i++; - } - else if (STRINGS_ARE_EQUAL_CI(argv[i], "-G")) - { - geometryDirectory = argv[i+1]; - i++; - } - else if (STRINGS_ARE_EQUAL_CI(argv[i], "-A")) - { - if (STRINGS_ARE_EQUAL_CI(argv[i+1], "DEGREES")) - angleUnits = DEGREES; - else if (STRINGS_ARE_EQUAL_CI(argv[i+1], "RADIANS")) - angleUnits = RADIANS; - else - printf("Parameter after \"-a\" must be one of: degrees, DEGREES, radians, RADIANS.\n"); - i++; - } - else - { - printf("Unrecognized option (%s) on command line.", argv[i]); - printUsage(argv[0]); - //exit(0); - } - } - } - - if (!jointIn || !xmlOut) - { - printUsage(argv[0]); - exit(0); - } - - scene = get_scene(); - init_scene(scene); - - model = get_modelstruct(); - init_model(model); - - scene->model = (ModelStruct**)simm_malloc(sizeof(ModelStruct*)); - scene->model[0] = model; - scene->num_models = 1; - - mstrcpy(&model->jointfilename, jointIn); - - read_model_file(scene, model, model->jointfilename, yes); - - // If the muscle file name was specified on the command line, - // override the one specified in the joint file (if any). - if (muscleIn) - mstrcpy(&model->musclefilename, muscleIn); - - if (check_definitions(model) == code_bad) - exit(0); - - find_ground_joint(model); /* must be called before makepaths() ! */ - - if (makepaths(model) == code_bad) - exit(0); - - init_gencoords(model); - - set_gencoord_info(model); - - printf("Read joint file %s\n", model->jointfilename); - - if (model->musclefilename) - { - SBoolean foo; - read_muscle_file(model, model->musclefilename, &foo, yes); - } - - if (write_opensim_model(model, xmlOut, geometryDirectory, markerSetOut, angleUnits) == code_fine) - printf("Wrote OpenSim model file %s\n", xmlOut); - else - printf("Creation of OpenSim model from %s failed.\n", model->jointfilename); - - exit(0); -} diff --git a/OpenSim/Utilities/simmToOpenSim/simmkeys.c b/OpenSim/Utilities/simmToOpenSim/simmkeys.c deleted file mode 100644 index 1ae58224a5..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/simmkeys.c +++ /dev/null @@ -1,210 +0,0 @@ -/******************************************************************************* - - SIMMKEYS.C - - Author: Kenny Smith - - Date: 9-Nov-98 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - This file is copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#include -#include - -#include "universal.h" -#include "functions.h" - -typedef struct { - const char* keyname; - int keyid; -} KeyDef; - -static KeyDef simmkeys[] = { -{"null_key", 0}, -{"key_released", 0}, -{"key_pressed", 1}, -{"backspace_key", 8}, -{"tab_key", 9}, -{"return_key", 13}, -{"enter_key", 13}, -{"escape_key", 27}, -{"space_key", 32}, -{"bang_key", 33}, -{"double_quote_key", 34}, -{"hash_key", 35}, -{"pound_sign_key", 35}, -{"dollar_key", 36}, -{"percent_key", 37}, -{"ampersand_key", 38}, -{"single_quote_key", 39}, -{"open_paren_key", 40}, -{"left_paren_key", 40}, -{"close_paren_key", 41}, -{"right_paren_key", 41}, -{"asterisk_key", 42}, -{"plus_key", 43}, -{"comma_key", 44}, -{"dash_key", 45}, -{"minus_key", 45}, -{"period_key", 46}, -{"slash_key", 47}, -{"zero_key", 48}, -{"one_key", 49}, -{"two_key", 50}, -{"three_key", 51}, -{"four_key", 52}, -{"five_key", 53}, -{"six_key", 54}, -{"seven_key", 55}, -{"eight_key", 56}, -{"nine_key", 57}, -{"colon_key", 58}, -{"semicolon_key", 59}, -{"less_than_key", 60}, -{"equals_key", 61}, -{"greater_than_key", 62}, -{"question_mark_key", 63}, -{"at_sign_key", 64}, -{"A_key", 65}, -{"B_key", 66}, -{"C_key", 67}, -{"D_key", 68}, -{"E_key", 69}, -{"F_key", 70}, -{"G_key", 71}, -{"H_key", 72}, -{"I_key", 73}, -{"J_key", 74}, -{"K_key", 75}, -{"L_key", 76}, -{"M_key", 77}, -{"N_key", 78}, -{"O_key", 79}, -{"P_key", 80}, -{"Q_key", 81}, -{"R_key", 82}, -{"S_key", 83}, -{"T_key", 84}, -{"U_key", 85}, -{"V_key", 86}, -{"W_key", 87}, -{"X_key", 88}, -{"Y_key", 89}, -{"Z_key", 90}, -{"left_bracket_key", 91}, -{"open_bracket_key", 91}, -{"backslash_key", 92}, -{"right_bracket_key", 93}, -{"close_bracket_key", 93}, -{"carat_key", 94}, -{"underscore_key", 95}, -{"back_quote_key", 96}, -{"virgule_key", 96}, -{"a_key", 97}, -{"b_key", 98}, -{"c_key", 99}, -{"d_key", 100}, -{"e_key", 101}, -{"f_key", 102}, -{"g_key", 103}, -{"h_key", 104}, -{"i_key", 105}, -{"j_key", 106}, -{"k_key", 107}, -{"l_key", 108}, -{"m_key", 109}, -{"n_key", 110}, -{"o_key", 111}, -{"p_key", 112}, -{"q_key", 113}, -{"r_key", 114}, -{"s_key", 115}, -{"t_key", 116}, -{"u_key", 117}, -{"v_key", 118}, -{"w_key", 119}, -{"x_key", 120}, -{"y_key", 121}, -{"z_key", 122}, -{"left_brace_key", 123}, -{"open_brace_key", 123}, -{"vertical_bar_key", 124}, -{"right_brace_key", 125}, -{"close_brace_key", 125}, -{"tilde_key", 126}, -{"delete_key", 127}, -{"leftmouse_button", 128}, -{"middlemouse_button", 129}, -{"rightmouse_button", 130}, -{"select_button", 128}, -{"mouse_motion", 131}, -{"window_shut", 132}, -{"window_quit", 133}, -{"input_change", 134}, -{"depth_change", 135}, -{"window_thaw", 136}, -{"window_freeze", 137}, -{"f1_key", 138}, -{"f2_key", 139}, -{"f3_key", 140}, -{"f4_key", 141}, -{"f5_key", 142}, -{"f6_key", 143}, -{"f7_key", 144}, -{"f8_key", 145}, -{"f9_key", 146}, -{"f10_key", 147}, -{"f11_key", 148}, -{"f12_key", 149}, -{"left_arrow_key", 150}, -{"up_arrow_key", 151}, -{"right_arrow_key", 152}, -{"down_arrow_key", 153}, -{"page_up_key", 154}, -{"page_down_key", 155}, -{"home_key", 156}, -{"end_key", 157}, -{"insert_key", 158}, -{"shift_key", 159}, -{"control_key", 160}, -{"alt_key", 161}, -{"caps_lock_key", 162} -}; - - -int lookup_simm_key (const char* keyname) -{ - /* First check for an all-integer 'keyname'. If the joint file included - * simmkeys.h, then acpp will have already substituted integer key ids - * in place of symbolic key names. - */ - const char* p = keyname; - - for ( ; *p; p++) - if ( ! isdigit(*p)) - break; - - /* If 'keyname' has no alphabetic characters (ie. all digits), then - * convert it directly to an integer value. - */ - if (*p == '\0') - return atoi(keyname); - - /* Otherwise lookup the symbolic keyname and return its value. - */ - { - int i, n = sizeof(simmkeys) / sizeof(KeyDef); - - for (i = 0; i < n; i++) - { - if (strcmp(keyname, simmkeys[i].keyname) == 0) - return simmkeys[i].keyid; - } - } - return null_key; -} - diff --git a/OpenSim/Utilities/simmToOpenSim/simmkeys.h b/OpenSim/Utilities/simmToOpenSim/simmkeys.h deleted file mode 100644 index 55db5056af..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/simmkeys.h +++ /dev/null @@ -1,207 +0,0 @@ -/******************************************************************************* - - SIMMKEYS.H - - Author: Peter Loan - - Date: 29-JUL-98 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - This file is copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef SIMM_KEYS_H -#define SIMM_KEYS_H - -#define null_key 0 - -#define key_released 0 -#define key_pressed 1 - -// For some reason, CTRL-C and CTRL-V come through as -// ASCII codes 3 (end of text) and 22 (synchronous idle) -// instead of c_key and v_key with the CTRL modifier turned on. -// This is the case for both upper and lower case c and v. -#define copy_key 3 -#define paste_key 22 - -#define FIRST_ASCII_KEY 8 -#define backspace_key 8 -#define tab_key 9 -#define return_key 13 -#define enter_key 13 -#define escape_key 27 - -#define space_key 32 -#define bang_key 33 -#define double_quote_key 34 -#define hash_key 35 -#define pound_sign_key 35 -#define dollar_key 36 -#define percent_key 37 -#define ampersand_key 38 -#define single_quote_key 39 -#define open_paren_key 40 -#define left_paren_key 40 -#define close_paren_key 41 -#define right_paren_key 41 -#define asterisk_key 42 -#define plus_key 43 -#define comma_key 44 -#define dash_key 45 -#define minus_key 45 -#define period_key 46 -#define slash_key 47 -#define zero_key 48 -#define one_key 49 -#define two_key 50 -#define three_key 51 -#define four_key 52 -#define five_key 53 -#define six_key 54 -#define seven_key 55 -#define eight_key 56 -#define nine_key 57 -#define colon_key 58 -#define semicolon_key 59 -#define less_than_key 60 -#define equals_key 61 -#define greater_than_key 62 -#define question_mark_key 63 -#define at_sign_key 64 -#define A_key 65 -#define B_key 66 -#define C_key 67 -#define D_key 68 -#define E_key 69 -#define F_key 70 -#define G_key 71 -#define H_key 72 -#define I_key 73 -#define J_key 74 -#define K_key 75 -#define L_key 76 -#define M_key 77 -#define N_key 78 -#define O_key 79 -#define P_key 80 -#define Q_key 81 -#define R_key 82 -#define S_key 83 -#define T_key 84 -#define U_key 85 -#define V_key 86 -#define W_key 87 -#define X_key 88 -#define Y_key 89 -#define Z_key 90 -#define left_bracket_key 91 -#define open_bracket_key 91 -#define backslash_key 92 -#define right_bracket_key 93 -#define close_bracket_key 93 -#define carat_key 94 -#define underscore_key 95 -#define back_quote_key 96 -#define virgule_key 96 -#define a_key 97 -#define b_key 98 -#define c_key 99 -#define d_key 100 -#define e_key 101 -#define f_key 102 -#define g_key 103 -#define h_key 104 -#define i_key 105 -#define j_key 106 -#define k_key 107 -#define l_key 108 -#define m_key 109 -#define n_key 110 -#define o_key 111 -#define p_key 112 -#define q_key 113 -#define r_key 114 -#define s_key 115 -#define t_key 116 -#define u_key 117 -#define v_key 118 -#define w_key 119 -#define x_key 120 -#define y_key 121 -#define z_key 122 -#define left_brace_key 123 -#define open_brace_key 123 -#define vertical_bar_key 124 -#define right_brace_key 125 -#define close_brace_key 125 -#define tilde_key 126 -#define delete_key 127 -#define LAST_ASCII_KEY 127 - -#define FIRST_MOUSE_BUTTON 128 -#define leftmouse_button 128 -#define middlemouse_button 129 -#define rightmouse_button 130 -#define select_button 128 -#define mouse_motion 131 -#define LAST_MOUSE_BUTTON 131 - -#define FIRST_WINDOW_ACTION_KEY 132 -#define window_shut 132 -#define window_quit 133 -#define input_change 134 -#define depth_change 135 -#define window_thaw 136 -#define window_freeze 137 -#define LAST_WINDOW_ACTION_KEY 137 - -#define FIRST_SPECIAL_KEY 138 -#define f1_key 138 -#if 0 -#define f2_key 139 -#define f3_key 140 -#define f4_key 141 -#else -#define f2_key leftmouse_button -#define f3_key middlemouse_button -#define f4_key rightmouse_button -#endif -#define f5_key 142 -#define f6_key 143 -#define f7_key 144 -#define f8_key 145 -#define f9_key 146 -#define f10_key 147 -#define f11_key 148 -#define f12_key 149 - -#define left_arrow_key 150 -#define up_arrow_key 151 -#define right_arrow_key 152 -#define down_arrow_key 153 -#define page_up_key 154 -#define page_down_key 155 -#define home_key 156 -#define end_key 157 -#define insert_key 158 -#define LAST_SPECIAL_KEY 158 - -#define FIRST_MODIFIER_KEY 159 -#define shift_key 159 -#define control_key 160 -#define alt_key 161 -#define caps_lock_key 162 -#define LAST_MODIFIER_KEY 162 - -#define max_simm_keys 163 - -#define IS_ASCII_KEY(I) ((I) >= FIRST_ASCII_KEY && (I) <= LAST_ASCII_KEY) -#define IS_SPECIAL_KEY(I) ((I) >= FIRST_SPECIAL_KEY && (I) <= LAST_SPECIAL_KEY) -#define IS_MODIFIER_KEY(I) ((I) >= FIRST_MODIFIER_KEY && (I) <= LAST_MODIFIER_KEY) -#define IS_MOUSE_BUTTON(I) ((I) >= FIRST_MOUSE_BUTTON && (I) <= LAST_MOUSE_BUTTON) -#define IS_WINDOW_ACTION_KEY(I) ((I) >= FIRST_WINDOW_ACTION_KEY && (I) <= LAST_WINDOW_ACTION_KEY) - -#endif /* SIMM_KEYS_H */ diff --git a/OpenSim/Utilities/simmToOpenSim/sm.h b/OpenSim/Utilities/simmToOpenSim/sm.h deleted file mode 100644 index fd6c46b84f..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/sm.h +++ /dev/null @@ -1,1100 +0,0 @@ -/******************************************************************************* - - SM.H - - (c) Copyright 2001-9, MusculoGraphics, Inc. - a division of Motion Analysis Corp. - - This version of the Solver header file is for use with Solver 1.4.0, - released April 8, 2009. - - Authors: Peter Loan, Krystyne Blaikie - - This file contains definitions of enums and structures that are used in - the interface between the Solver library and its clients. The functions - comprising the interface have prototypes at the end of this file, and - are defined in client.cpp. All of the code in this file is C (not C++), - and the prototypes are within an "extern C" scope. Thus the Solver library - can be called from a C or a C++ client. - - Terminology: - DOF: not really a degree of freedom, but rather a "potential" degree - of freedom. Every joint has six DOFs, three translational and - three rotational. In a hinge joint, 5 of the DOFs are constants, - and one of the rotational DOFs is a function of a GenCoord. - GenCoord: (short for generalized coordinate) a variable with an - operating range that controls one or more DOFs. In the simplest - case, each GenCoord controls a single DOF, and the value of the - DOF equals the value of the GenCoord. For example, in a hinge - joint, one rotational DOF would be a linear function (with - slope = 1.0) of a GenCoord, and that GenCoord would not be used - in any other joints. In the general case, a GenCoord is an - abstract value that can control any number of DOFs. In the SIMM - knee for example, the GenCoord "knee_angle" controls not only - the flexion angle, but five other DOFs in two separate joints - are non-linear functions of knee_angle. - Q: another name for a GenCoord. -*******************************************************************************/ - -#ifndef _SM_H_ -#define _SM_H_ - -#ifdef _WINDOWS - #define DLL __declspec(dllexport) -#else - #define DLL -#endif - -#define SMDEBUG 0 - -#define UNDEFINED_DOUBLE 99999.99 - -typedef double smMatrix4x4[4][4]; -typedef double smPoint3[3]; -typedef void* smModelID; - -typedef enum { smNo = 0, smYes = 1 } smBoolean; - -typedef enum { smNoAxis = 0, smX, smNegX, smY, smNegY, smZ, smNegZ, smNoAlign } smAxes; - -typedef enum { smXYZ = 1, smXZY, smYXZ, smYZX, smZXY, smZYX } smEulerRotationOrder; - -typedef enum { PFX, PFY, PFZ, PPX, PPY, PMZ, NUM_TYPE1_CHANNELS } smForcePlate1Channels; - -typedef enum { FX, FY, FZ, MX, MY, MZ, NUM_TYPE24_CHANNELS } smForcePlate24Channels; - -typedef enum { FX12, FX34, FY14, FY23, FZ1, FZ2, FZ3, FZ4, NUM_TYPE3_CHANNELS } smForcePlate3Channels; - -typedef enum { FZC, FZD, FZA, FZB, FYAC, FXDC, FXAB, FYBD, NUM_TYPE5_CHANNELS } smForcePlate5Channels; - -typedef enum { smMale, smFemale, smNoGender } smGender; - -typedef enum { smTRBFile, smTRCFile, smC3DFile, smNoFileType } smMotionFileType; - -typedef enum { smFPForcePosition = 1, smFPSixChannel, smFPEightChannel, smFPCalSixChannel, smFPSixEightChannel, smKyowaDengyo } smForcePlateType; - -typedef enum { smUnsignedShort, smSignedShort } smIntegerFormat; - -typedef enum { smOwnedBySIMM, smOwnedByEVaRT, smOwnedBySolver, smOwnedByOther } smOwner; - -typedef enum { smSIMMFile, smMODFile, smOtherFile} smSource; - -typedef enum { smStepFunction, smLinearFunction, smNaturalCubicSpline, smGCVSpline} smFunctionType; - -typedef enum { - VSacral = 0, - RASIS, LASIS, RPSIS, LPSIS, - RKneeLat, LKneeLat, - RAnkleLat, LAnkleLat, - RHeel, LHeel, - RToe, LToe, - RGreaterTroc, LGreaterTroc, - RKneeMed, LKneeMed, - RAnkleMed, LAnkleMed, - RShoulder, LShoulder, - RElbowLat, LElbowLat, - RWristLat, LWristLat, RWristFront, LWristFront, - RElbowMed, LElbowMed, - RWristMed, LWristMed, RWristBack, LWristBack, - HeadRear, HeadTop, HeadFront, - HeadFrontRight, HeadFrontLeft, HeadBackRight, HeadBackLeft, - RMiddleFinger, LMiddleFinger, - RThumb, LThumb, - NumMarkers -} smOrthoTrakMarkerSet; - -typedef enum { - F1M1 = 0, F1M2, F1M3, - F2M1, F2M2, F2M3, - F3M1, F3M2, F3M3, - F4M1, F4M2, F4M3, - F5M1, F5M2, F5M3, - NumFingerMarkers -} smFingerMarkerSet; // used for both right and left hands - -typedef enum { - UNSPECIFIED_ANALOG_CHANNEL, - ELAPSED_TIME, - FORCE_PLATE_FORCE, - FORCE_PLATE_MOMENT, - FORCE_LOCATION, - EMG, - FORCE_PLATE_FREE_TORQUE, - OTHER_DATA -} smAnalogChannelType; - -typedef enum { - smPointGroup, - smAnalogGroup, - smForcePlateGroup, - smSubjectGroup, - smEventGroup, - smOtherGroup -} smC3DGroup; - -typedef enum { - smRadians = 0, - smDegrees, - smMillimeters, - smCentimeters, - smMeters, - smNewtons, - smNewtonMeters, - smNewtonCentimeters, - smNewtonMillimeters, - smVolts, - smMillivolts, - smOtherUnits -} smUnit; - -typedef enum { smUndefinedMarker = 0, smCriticalMarker, smSemiCriticalMarker, smOptionalMarker, smFixedMarker } smMarkerType; - -typedef enum -{ - smNoError = 0, - smNullPointer, - smImproperInput = 10, - smMaxIter = 15, - smUncontrolledGenCoord = 19, - smZeroFrames = 22, - smReadError = 24, - smInputError = 25, - smModelError, - smJNTFileError, - smFileError, - smFormatError, - smLeastSquaresSolverFailure, - smZeroMarkers, - smUncontrolledGCWarning, - smFileWarning -} smReturnCode; - -typedef enum -{ - smLevenbergMarquart, /* the default method, which is more robust */ - smGaussNewton /* the faster method, not guaranteed to converge */ -} smSolverMethod; - -typedef enum -{ - smHinge, /* one rotational DOF */ - smUniversal, /* two rotational DOFs */ - smSpherical, /* three rotational DOFs, axes = X, Y, Z */ - smGimbal, /* three rotational DOFs, user-specified axes */ - smSimm, /* joint as defined in SIMM */ - smSixdof, /* three translational, three rotational DOFs */ - smFixed, /* no DOFs, all translations and rotations are fixed */ - smSlider, /* one translational DOF */ - smPlanar, /* two translational, one rotational DOF */ - smSlaveHinge, /* slave, one rotational DOF */ - smSlaveUniversal, /* slave, two rotational DOFs */ - smSlaveSpherical, /* slave, three rotational DOFs, axes = X, Y, Z */ - smSlaveSlider, /* slave, one translational DOF */ - smPoint, /* two rotational, one translational DOF */ - smTranslational, /* three translational DOFs */ - smMixed, /* joint with three rotations that can be slaves or unique DOFs */ - smOtherJoint /* unknown type */ -} smJointType; - -typedef enum -{ - smOrderR1 = 0, /* where in the order the R1 rotation is */ - smOrderR2, /* where in the order the R2 rotation is */ - smOrderR3, /* where in the order the R3 rotation is */ - smOrderT /* where in the order the translation rotation is */ -} smTransformOrder; /* order of transforms in a SIMM joint */ - -typedef enum -{ - smConstant = 0, /* constant, number should be specified */ - smFunction /* function of a generalized coordinate */ -} smDOFType; - -typedef enum -{ - smSolveMe, /* solve this element (Q, segment) in the normal way */ - smDontSolveMe /* do not solve this element (point, translational segments) */ -} smSolveType; - -typedef enum -{ - smLocalAxes, /* axes are specified in local reference frame */ - smGlobalAxes /* axes are specified in global reference frame */ -} smAxisFrame; - -typedef enum -{ - smDofT1, /* for gencoords used only in a single TX component of a joint */ - smDofT2, /* for gencoords used only in a single TY component of a joint */ - smDofT3, /* for gencoords used only in a single TZ component of a joint */ - smDofR1, /* for gencoords used only in a single R1 component of a joint */ - smDofR2, /* for gencoords used only in a single R2 component of a joint */ - smDofR3, /* for gencoords used only in a single R3 component of a joint */ - smDofOther /* for [pre-defined] gencoords which may be used in more than one joint/DOF */ -} smDOFElement; /* which DOF[s] a certain gencoord is used in */ - -typedef enum -{ - smGCGenerated, /* created by Solver; used in only one DOF in one joint */ - smGCSIMM, /* defined in smModel.gencoord[] array, used in SIMM joints */ - smGCShared, /* created by Solver; used in master/slave joints */ - smGCOther /* other; is an error if gencoord remains this type */ -} smGenCoordType; - -typedef enum -{ - smGlobalPose, /* "global" pose, like htr2 files */ - smInitPose, /* the pose from the static init TRC file */ - smFirstFrame /* the first frame of data from the motion TRC file */ -} smBasePosition; - -typedef enum -{ - smConstraintPlane, - smConstraintEllipsoid, - smConstraintSphere, - smConstraintCylinder, - smOtherConstraint -} smConstraintObjectType; - -typedef struct { - smPoint3 normal; - double d; -} smPlaneStruct; - - -typedef struct -{ - int type; /* type not currently used */ - int numMarkers; /* number of markers in markerCoordList */ - int frameNum; /* client-defined frame number */ - double time; /* client-defined time of frame */ - smPoint3 *markerCoordList; /* array of 3D marker coordinates in global space */ - smUnit units; /* units of marker coordinates */ -} smTRCFrame; - -typedef struct -{ - int type; /* type not currently used */ - double translation[3]; /* translation of segment */ - double rotation[3]; /* rotation of segment */ - double scale; /* scale of segment; needed for smPoints */ -} smHTRSegmentTransform; - -typedef struct -{ - int type; /* type not currently used */ - int frameNum; /* frame number; copied from input TRC frame */ - double time; /* time of frame: copied from input TRC frame */ - int numSegments; /* number of segments in this frame of data */ - smHTRSegmentTransform* segmentTransformList; /* array of segment transforms */ -} smHTRFrame; - -typedef struct -{ - int type; /* type not currently used */ - int frameNum; /* frame number; copied from input TRC frame */ - double time; /* time of frame: copied from input TRC frame */ - int numSegments; /* number of segments in this frame of data */ - smMatrix4x4 *segmentTransformList; /* array of segment transforms */ -} smMatrixFrame; - -typedef struct -{ - int type; /* type not currently used */ - double accuracy; /* desired accuracy of solution */ - smSolverMethod method; /* which method to use when solving */ - int maxIterations; /* per frame, Solver will stop at this number */ - smEulerRotationOrder rotationOrder; /* rotation order for HTR output */ - smUnit htrRotationUnits; /* degrees or radians for HTR output */ - smUnit htrTranslationUnits; /* length units for HTR output */ - smBasePosition basePosition; /* which base position to use in HTR file */ - smAxes autoAlign; /* auto-align option when processing init pose */ - smAxisFrame axisFrame; /* in which frame are rotation axes specified */ - smBoolean orientBodyToFrame; /* do you want to orient the body before solving? */ - smBoolean jointLimitsOn; /* do you want to enforce joint limits? */ - double globalScale; /* global scale factor that was used to make this model */ - smBoolean loopsOn; /* do you want to enforce closed loops */ - smBoolean fgContactOn; /* do you want to turn on foot_ground contact? */ -} smOptions; - -typedef struct -{ - int type; /* type not currently used */ - double axis[3][3]; /* the three rotation axes for this joint */ - double range[6][2]; /* range limits for the six dofs */ - double stiffness[6][2]; /* stiffness to use at each range limit */ -} smSixDofJoint; - -typedef struct -{ - int type; /* type not currently used */ - double axis[3][3]; /* the three rotation axes for this joint */ -} smFixedJoint; - -typedef struct -{ - int type; /* type not currently used */ - double axis[3]; /* the translation axis for this joint */ - double range[2]; /* range limits for the dof */ - double stiffness[2]; /* stiffness to use at the range limit */ -} smSliderJoint; - -typedef struct -{ - int type; /* type not currently used */ - double axis[3]; /* rotation axis (translations are orthogonal to it) */ - double range[3][2]; /* range limits for the three dofs */ - double stiffness[3][2]; /* stiffness to use at the range limits */ -} smPlanarJoint; - -typedef struct -{ - int type; /* type not currently used */ - double axis[3]; /* the axis of the hinge */ - double range[2]; /* range limits for the dof */ - double stiffness[2]; /* stiffness to use at the range limit */ -} smHingeJoint; - -typedef struct -{ - int type; /* type not currently used */ - double axis[2][3]; /* the two rotation axes */ - double range[2][2]; /* range limits for the two dofs */ - double stiffness[2][2]; /* stiffness to use at the range limits */ -} smUniversalJoint; - -typedef struct -{ - int type; /* type not currently used */ - double range[3][2]; /* range limits for the three dofs */ - double stiffness[3][2]; /* stiffness to use at the range limits */ -} smSphericalJoint; - -typedef struct -{ - int type; /* type not currently used */ - double axis[3][3]; /* the three rotation axes */ - double range[3][2]; /* range limits for the three dofs */ - double stiffness[3][2]; /* stiffness to use at the range limits */ -} smGimbalJoint; - -typedef struct -{ - smDOFType type; /* constant or function */ - smPoint3 axis; /* axis of rotation or translation */ - int functionNum; /* index of function that controls this DOF */ - int gencoordNum; /* index of gencoord that controls this DOF */ - double value; /* current value in user units */ -} smDOF; /* a "potential" degree of freedom */ - -typedef struct -{ - int type; /* type not currently used */ - smTransformOrder transformOrder[4]; /* order of T, R1, R2, R3 */ - smDOF tx; /* the X-translation DOF */ - smDOF ty; /* the Y-translation DOF */ - smDOF tz; /* the Z-translation DOF */ - smDOF r1; /* a rotation DOF, not necessarily performed first */ - smDOF r2; /* a rotation DOF, not necessarily performed second */ - smDOF r3; /* a rotation DOF, not necessarily performed third */ - smJointType subtype; /* which type (e.g., slider) this SIMM joint represents */ -} smSIMMJoint; - -typedef struct -{ - int type; /* type not currently used */ - int master; /* index of the joint which controls this one */ - double ratio[3]; /* ratio slave gets from master DOFs (if 0, master DOF not part of slave) percentage that this slave gets, % = ratio / total */ - double axis[3]; /* the axis of the hinge */ -} smSlaveHingeJoint; - -typedef struct -{ - int type; /* type not currently used */ - int master; /* index of the joint which controls this one */ - double ratio[3]; /* ratio slave gets from master DOFs (if 0, master DOF not part of slave) percentages that this slave gets, % = ratio / total */ - double axis[2][3]; /* the two rotation axes */ -} smSlaveUniversalJoint; - -typedef struct -{ - int type; /* type not currently used */ - int master; /* index of the joint which controls this one */ - double ratio[3]; /* ratio slave gets from master DOFs (if 0, master DOF not part of slave) percentage that this slave gets, % = ratio / total */ - double axis[3]; /* the translation axis */ -} smSlaveSliderJoint; - -typedef struct -{ - int type; /* type not currently used */ - int master; /* index of the joint which controls this one */ - double ratio[3]; /* ratio slave gets from master DOFs (if 0, master DOF not part of slave) percentages that this slave gets, % = ratio / total */ -} smSlaveSphericalJoint; - -typedef struct -{ - int type; /* type not currently used */ - int master; /* index of the joint which controls this one */ - double ratio[3]; /* ratio slave gets from master DOFs (if 0, master DOF not part of slave) percentages that this slave gets, % = ratio / total */ - double range[3][2]; /* range limits for the three dofs */ - double stiffness[3][2]; /* stiffness to use at the range limits */ -// double axis[3][3]; /* the three rotation axes */ -} smMixedJoint; - -typedef struct -{ - int type; /* type not currently used */ -} smPointJoint; - -typedef struct -{ - int type; /* type not currently used */ -} smTranslationalJoint; - -typedef union -{ - smHingeJoint hinge; /* see smJointType for a description */ - smUniversalJoint universal; /* of each of these joints */ - smSphericalJoint spherical; - smGimbalJoint gimbal; - smSIMMJoint simm; - smSixDofJoint sixdof; - smFixedJoint fixed; - smSliderJoint slider; - smPlanarJoint planar; - smSlaveHingeJoint slaveHinge; - smSlaveUniversalJoint slaveUniversal; - smSlaveSliderJoint slaveSlider; - smSlaveSphericalJoint slaveSpherical; - smPointJoint point; - smTranslationalJoint translational; - smMixedJoint mixed; -} smJointUnion; - -typedef struct -{ - smMarkerType type; /* critical, optional, fixed, etc. */ - int index; /* index of marker in global marker array (smModel.marker[]) */ - double weight; /* marker weight for each segment */ - double localOffset[3]; /* offset in segment's local reference frame */ -} smMarkerLink; - -typedef struct -{ - int type; /* type not currently used */ - char *name; /* client-defined name of marker */ -} smMarker; - -typedef struct -{ - smFunctionType type; /* step, linear, natural cubic, GCV */ - int userNum; /* client-defined number of this function */ - int numPoints; /* number of control points */ - double *x; /* array of X coordinates of control points */ - double *y; /* array of Y coordinates of control points */ -} smConstraintFunction; - -typedef struct -{ - char *name; /* name of constraint point */ - int segmentIndex; /* index of segment on which it is defined */ - smPoint3 offset; /* offset of point from segment origin */ - double weight; /* weight used to calculate residual */ - double tolerance; /* tolerance of error in point distance from object - not for solving */ - smBoolean visible; /* is point visible */ - smBoolean active; /* is point active */ -} smConstraintPoint; - -typedef struct -{ - char *name; /* name of constraint object */ - smConstraintObjectType constraintType; /* type of constraint object */ - int segmentIndex; /* index of the segment the object is attached to */ - smBoolean active; /* is the constraint object active? */ - smBoolean visible; /* is the constraint object visible? */ - smPoint3 radius; /* constraint object radius */ - double height; /* constraint object height (cylinder only) */ - smPlaneStruct plane; /* plane parameters (plane only) */ - smAxes constraintAxis; /* which axis to constrain over */ - smPoint3 rotationAxis; /* local-to-parent transform parameter */ - double rotationAngle; /* local-to-parent transform parameter */ - smPoint3 translation; /* local-to-parent transform parameter */ - smMatrix4x4 from_local_xform; /* consobj-to-parent transform matrix */ - smMatrix4x4 to_local_xform; /* parent-to-consobj transform matrix */ - int numPoints; /* number of constraint points */ - smConstraintPoint *points; /* constraint points */ - int numQs; /* not used */ - int numJnts; /* not used */ - int *joints; /* not used */ - int *Qs; /* not used */ -} smConstraintObject; - -typedef struct -{ - int type; /* type not currently used */ - char *name; /* name of this gencoord */ - double rangeStart; /* minimum gencoord value */ - double rangeEnd; /* maximum gencoord value */ - double stiffness1; /* stiffness when gencoord goes below rangeStart */ - double stiffness2; /* stiffness when gencoord goes above rangeEnd */ - double tolerance; /* used to make setting the value more efficient */ - smBoolean locked; /* gencoord cannot be changed from default value */ - smBoolean clamped; /* gencoord constrained to remain within specified range (restraint func not used) */ - double defaultValue; /* more for SIMM, but used here to set initial value */ - smUnit units; /* units of this gencoord */ - int restraintFuncNum; /* index of the constraint/restraint function */ - smBoolean restraintFuncActive; /* is the restraintFunction active */ - smBoolean solve; /* is Solver used to find value, or is it calculated after? */ -} smGenCoord; - -typedef struct -{ - int type; /* type not currently used */ - char *name; /* name of this segment */ - int parent; /* index of segment with joint to this segment */ - int child; /* index of child segment in joint - unless loop or reverse, equal to index of current segment */ - int loopSeg; /* index of segment that, when jointed to current seg, closes a loop */ - double length; /* used only in HTR output */ - smJointType jointType; /* type of joint from parent to this segment */ - smJointUnion joint; /* joint from parent to this segment */ - double translation[3]; /* To specify the orientation of the segment with */ - double angle; /* respect to its parent, use translation, angle, */ - smPoint3 axis; /* and axis */ - int beginMarker; /* The orientation of the segment can also be */ - smPoint3 beginOffset; /* specified using beginMarker, endMarker, and */ - int endMarker; /* frontVector. In this case, an init pose file is */ - smPoint3 endOffset; /* needed as well. jointPositionMarker is just like */ - smPoint3 frontVector; /* beginMarker- they function the same, and only one */ - int jointPositionMarker; /* of them should be used. If both are specified, */ - smPoint3 jointPosition; /* jointPositionMarker will be used. */ - int numMarkerLinks; /* number of markers attached to this segment */ - smMarkerLink *markerLinkList; /* list of markers attached to this segment */ - smAxisFrame localAxis; /* are parent->segment axes specified locally or globally? */ - smPoint3 scaling; /* stored, but not used by Solver */ - smPoint3 endPoint; /* stored, but not used by Solver */ - char *gaitScaleSegment; /* name of the segment on which to base scaling */ - smPoint3 gaitScaleFactor; /* scale factor btwn simm and static trc pose */ - smPoint3 htr_o; /* origin of HTR segment */ - smPoint3 htr_x; /* direction of X axis of HTR segment */ - smPoint3 htr_y; /* direction of Y axis of HTR segment */ -} smSegment; - -typedef struct -{ - char *name; /* the name of the FG marker */ - int segmentIndex; /* the index of the SolverModel segment it is attached to */ - int markerIndex; /* the index of the SolverModel marker */ - int associatedMarkerIndex; /* the index of its associated marker (that has the same name without the fg. */ - int associatedMarkerTRCIndex; /* the TRC index of the associated marker */ - double threshold; /* the threshold for turning on the contact */ - smPoint3 staticPos; /* the original position of the marker */ -} fgMarkerStruct; - -typedef struct -{ - int type; /* type not currently used */ - smOwner owner; /* which program created the model: SIMM, EVaRT, Solver or Other? */ - smSource source; /* what format is the model derived from SIMM joint file or MOD file? */ - char *name; /* name of this model */ - int numSegments; /* number of segments in this model */ - smSegment *segmentList; /* list of segments in this model */ - int numMarkers; /* number of markers in this model */ - smMarker *markerList; /* list of markers in this model */ - int numConstraintFunctions; /* used by joint files, not mod files */ - smConstraintFunction *constraintFunctionList; /* used by joint files, not mod files */ - int numSIMMGenCoords; /* used by joint files, not mod files */ - smGenCoord *SIMMGenCoordList; /* used by joint files, not mod files */ - smUnit rotationUnits; /* units that model is defined in */ - smUnit translationUnits; /* units that model is defined in */ - smOptions options; /* list of Solver options for this model */ - int numConstraintObjects; /* number of constraint objects in this model */ - smConstraintObject *constraintObjectList; /* constraint objects */ - smAxes gravity; /* direction of gravity */ - smPlaneStruct groundPlane; /* description of the ground Plane */ - smBoolean gait; /* used for display in SIMM - set to true if using an OT model */ - /* used for SIMM display */ - char *bonefile; /* used for display in SIMM */ - char *bonepath; /* used for display in SIMM */ - char *musclefilename; /* used for display in SIMM */ - char *motionfilename; /* used for display in SIMM */ - double markerRadius; /* used for display in SIMM */ - double mvGear; /* used for display in SIMM */ - char *forceUnits; /* used for display in SIMM */ - char *lengthUnits; /* used for display in SIMM */ - smBoolean markerVisibility; /* used for display in SIMM */ - double loopTolerance; /* used to check status of loops */ - double loopWeight; /* used to calculate loop residuals when solving */ - smBoolean solveLoops; /* to turn loop solving on and off */ - int numFGMarkers; /* number of foot ground contact markers in the model */ - fgMarkerStruct *FGMarkerList; /* list of foot ground contact markers */ -} smModel; - -typedef struct -{ - int type; /* not currently used */ - int numGenCoords; /* the number of gencoords in the joint to this segment */ - int genCoordIndex[6]; /* the indices of these gencoords in the output Q array */ -} smSegmentMapping; /* for interpreting segment-based output */ - -typedef struct -{ - smGenCoordType type; /* type of gencoord: SIMM, Generated, Shared */ - int segment; /* segment whose joint this gencoord is in */ - smDOFElement dof; /* which dof this gencoord controls */ - double axis[3]; /* the instantaneous axis of rotation/translation */ -} smGenCoordMapping; /* for interpreting Q-based output */ - -typedef struct -{ - int type; /* type not currently used */ - int numGenCoords; /* number of gencoords in the Q array */ - char **nameList; /* names of gencoords as they appear in the Q array */ -} smGCNames; - -typedef struct -{ - int type; /* type not currently used */ - char *name; /* name of this segment */ - int numFrames; /* number of frames in this segment's HTR struct */ - smHTRSegmentTransform *htrDataList; /* list of frames of HTR data */ -} smHTRSegmentData; - -typedef struct -{ - int type; /* type not currently used */ - int numSegments; /* number of segments in this HTR model */ - int *parentList; /* parent indices for each segment */ - int *frameNumList; /* frame numbers for each frame of data */ - char **segmentNameList; /* names of segments in this HTR model */ - smOptions options; /* file output options */ - double dataFrameRate; /* frame rate of data */ - int framesSolved; /* number of frames of HTR data */ - smHTRSegmentData *segmentDataList; /* list of segments' data frames */ - smHTRFrame HTRBasePosition; /* base position from which frames are measured */ -} smHTRData; - -typedef struct -{ - double dataRate; /* data rate */ - double cameraRate; /* camera rate */ - double origDataRate; /* original data rate */ - int origStartFrame; /* original starting frame */ - int origNumFrames; /* original number of frames */ - int numFrames; /* number of frames in file */ - int firstFrameNum; /* user-defined number of first frame in file */ - int numMarkers; /* number of named markers */ - smUnit units; /* units of marker coordinates */ - char **markerNameList; /* list of marker names */ -} smTRCHeaderInfo; - -typedef struct -{ - int type; /* type not currently used */ - char *filename; /* name of file this TRC data came from */ - smTRCFrame* frameList; /* list of TRC frames */ - smTRCHeaderInfo header; /* information from file header */ -} smTRCStruct; - -typedef struct -{ - int numMarkersSolved; /* number of markers used to solve frame */ - double rmsTotal; /* total rms value of solved frame */ - int iterations; /* number of iterations needed to solve frame */ - int totalCalls; /* total number of calls to calculateResiduals */ - int inputFrameNum; /* actual frame number */ - int numQs; /* number of Qs to solve */ - double *qerr; /* Q error propagation for frame */ -} smSolveInfoStruct; - -typedef struct -{ - int firstFrameToSolve; /* index of the first frame to solve */ - int lastFrameToSolve; /* index of the last frame to solve */ - int frameIncrement; /* amount to increment when solving */ - smBoolean cropEnds; /* crop ends of TRC file when not all markers present? */ - smBoolean markerNamesFromDescriptions; /* used only for C3D files */ - smBoolean fgContactOn; /* is foot ground contact on or off */ -} smTRCOptions; - - -typedef struct -{ - smPoint3 translation; - double angle; - smPoint3 axis; -} smTransform; - -typedef enum { - smUnknownEventType = 0, - smHeelStrikeEvent, - smToeOffEvent -} smEventType; - -typedef enum { - smUnknownSegmentEvent = 0, - smRightFootEvent, - smLeftFootEvent, - smOtherSegmentEvent -} smEventSegment; - -typedef enum { - smUnknownEvent = 0, - smUserEvent, - smForceEvent, - smMarkerEvent -} smEventSource; - -typedef struct { - char* name; - double xCoord; - smEventType type; - smEventSegment segment; - smEventSource source; - smBoolean displayFlag; -} smMotionEvent; - -typedef struct { - smForcePlateType type; - double scaleFactor; - double origin[3]; // raw origin[] values from c3d file - double length; - double width; - int baselineRange[2]; - double calibrationMatrix[64]; - double transformMatrix[4][4]; // force plate reference frame w.r.t. lab frame - int channels[9]; -} smForcePlateSpec; - -typedef struct { - int index; - int component; - double baseline; -} smFPChannel; - -typedef struct { - int numMuscles; - char** muscleNames; - int* muscleIndices; - double specifiedMVC; - double actualMVC; -} smEMGChannel; - -typedef struct { - char* name; - int index; -} smOtherChannel; - -typedef struct { - char* name; - char* description; - smUnit units; - double frequency; - double range; - smAnalogChannelType type; - union { - smFPChannel force; - smEMGChannel emg; - smOtherChannel other; - } u; - int numMotionColumns; - int* motionColumn; -} smAnalogChannel; - -typedef struct { - int numChannels; - int numRows; - smAnalogChannel *channelList; - double* data; - int numForceEvents; - smMotionEvent* forceEvents; -} smAnalogStruct; - -typedef struct { - char* name; - int number; - smGender gender; - float height; - float weight; - char* dateOfBirth; - char* projectName; -} smSubjectStruct; - -typedef smTRCStruct smMotionStruct; - -typedef struct { - unsigned char firstParameterBlock; - unsigned char key; - float scaleFactor; - int numAnalogSamples; - short firstDataBlock; - short fourCharSupport; - double analogGlobalScale; - double* analogChannelScale; - int* analogChannelOffset; - smBoolean calMatricesSpecified; - int numAnalogChannels; - smIntegerFormat analogOffsetFormat; -} ExtraHeaderStuff; - -typedef struct { - smMotionStruct* motionData; - smAnalogStruct* analogData; - smSubjectStruct* subjectData; - int numHeaderEvents; // number of events specified in the header - int numEvents; // total number of events specified - smMotionEvent* eventList; - int numForcePlates; - smForcePlateSpec* forcePlateList; - int numOtherData; - smOtherChannel* otherDataList; - ExtraHeaderStuff* extraHeaderStuff; -} smC3DStruct; - -typedef struct { - int num3DPoints; - double pointFrameRate; - smUnit pointUnits; - int numAnalogChannels; - double analogFrameRate; - smUnit analogUnits; - int firstFrame; - int lastFrame; - int maxInterpolationGap; - smSubjectStruct subject; -} smC3DHeader; /* used for display, not for referencing data in file */ - - -typedef struct { - smBoolean readAnalogData; - smBoolean readOtherData; - smBoolean markerNamesFromDescriptions; - smBoolean leaveAnalogDataUnscaled; -} smReadC3DOptions; - -typedef struct { - smPoint3 X; - smPoint3 Y; - smPoint3 Z; - smMatrix4x4 dirCos; - smMatrix4x4 transp; -} smCoordSysStruct; - -typedef struct { - smPoint3 ASISmid, ShoulderMid; - smPoint3 LHipJC, RHipJC, LKneeJC, RKneeJC, LAnkleJC, RAnkleJC, PelvisCenter; - smPoint3 RShoulderJC, LShoulderJC, RElbowJC, LElbowJC, RWristJC, LWristJC; -} smJointCenterStruct; - -typedef struct { - smCoordSysStruct PelvisAxis, RThighAxis, LThighAxis, TrunkAxis; - smCoordSysStruct RShankAxis, LShankAxis, RFootAxis, LFootAxis; - smCoordSysStruct RUpArmAxis, LUpArmAxis, RLowArmAxis, LLowArmAxis, HeadAxis; -} smCoordSystemStruct; - -typedef struct { - smBoolean lowerBody; - smBoolean upperBody; - smBoolean rightHandFull; - smBoolean leftHandFull; - smBoolean rightArmOnly; - smBoolean leftArmOnly; - smBoolean pelvisPresent; - smBoolean headPresent; - smBoolean torsoPresent; - smBoolean armsUp; - int rightFingerMarkers[NumFingerMarkers]; - int leftFingerMarkers[NumFingerMarkers]; - int upAxis; - int index[NumMarkers]; - smPoint3 markerOffsets[NumMarkers]; - double RThighLength; - double RShankLength; - double RFootLength; - double LThighLength; - double LShankLength; - double LFootLength; - double PelvisLength; - double PelvisDepth; - double PelvisHeight; - double TrunkLength; - double TrunkWidth; - double RUpperArmLength; - double RLowerArmLength; - double LUpperArmLength; - double LLowerArmLength; - double HeadLength; - double RHandLength; - double LHandLength; - double RThumbLength; - double LThumbLength; - double RIndexFingerLength; - double LIndexFingerLength; - double RMiddleFingerLength; - double LMiddleFingerLength; - double RRingFingerLength; - double LRingFingerLength; - double RPinkyLength; - double LPinkyLength; - smJointCenterStruct globalJC; - smCoordSystemStruct coordSys; - smTRCStruct* trcInfo; - int numOtherData; - smOtherChannel* otherDataList; - double subjectMass; -} smOrthoTrakModel; - -/**************************** Function Prototypes *****************************/ - -#ifdef __cplusplus -extern "C" { -#endif - -/******************************** create model ********************************/ -DLL smReturnCode smCreateModel(smModel *model, smModelID *modelID); -DLL smReturnCode smCreateModelFromSIMMJointFile(smModel *model, const char filename[], - smModelID *modelID); -DLL smReturnCode smCreateModelFromMODFile(smModel *model, const char filename[], - smModelID *modelID, - smTRCOptions staticOptions); -DLL smReturnCode smCreateScaledModel(smModel *model, smModelID *modelID, - const char modelFilename[], - const char staticPoseFilename[], - const char personalDataFilename[], - smTRCOptions staticOptions); -DLL smReturnCode smCreateOrthoTrakModel(const char staticPoseFilename[], - const char personalDataFilename[], - smTRCOptions options, - smOrthoTrakModel *ortho); - -/******************************** delete structs ******************************/ -DLL smReturnCode smDeleteModel(smModelID modelID); -DLL smReturnCode smDeletesmModel(smModel *model); -DLL void smFreeTRCStruct(smTRCStruct *trc); -DLL void smFreeTRCHeader(smTRCHeaderInfo *header); -DLL void smFreeOrthoTrakModel(smOrthoTrakModel *ortho); -DLL void smFreeC3DStruct(smC3DStruct* c3d, smBoolean freeMotionData, smBoolean freeAnalogData, - smBoolean freeSubjectData, smBoolean freeEventData, smBoolean freeForcePlateData, - smBoolean freeOtherData); -DLL void smFreeC3DHeader(smC3DHeader* c3dHeader); -DLL void smFreeGenCoordNames(smGCNames *gcNames); - -/******************************** solve frames ********************************/ -DLL smReturnCode smSolveFrame(smModelID modelID, smTRCFrame *trcData, int nq, - double q[], smTRCFrame *trcPredicted, smSolveInfoStruct *info); -DLL smReturnCode smSolveFrameHTR(smModelID modelID, smTRCFrame *trcData, - int nq, double q[], smTRCFrame *trcPredicted, - smHTRFrame *htr, smSolveInfoStruct *info); -DLL smReturnCode smSolveFrameMatrix(smModelID modelID, smTRCFrame *trcData, - int nq, double q[], smTRCFrame *trcPredicted, - smMatrixFrame *mat, smSolveInfoStruct *info); -#if 0 -DLL smReturnCode smOptimizeFrame(smModelID modelID, smTRCFrame *trcData, - int nq, double q[], smTRCFrame *trcPredicted, - smSolveInfoStruct *info); -#endif -/******************************** read functions ******************************/ -DLL smReturnCode smReadTrackedFile(const char filename[], smTRCStruct *trc); -DLL smReturnCode smCropMotionEnds(smC3DStruct *c3d, int *firstFrame, int *lastFrame); -DLL smReturnCode smReadTrackedFileHeader(const char filename[], smTRCHeaderInfo *header); -DLL smReturnCode smReadC3DFile(const char filename[], smC3DStruct* c3d, smReadC3DOptions* options); -DLL smReturnCode smScanC3DHeader(const char filename[], smC3DHeader* head); - -/******************************** write functions *****************************/ -DLL smReturnCode smWriteMODFile(smModelID modelID, const char filename[]); -DLL smReturnCode smWriteJointFile(smModelID modelID, const char filename[]); -DLL smReturnCode smWriteHTRFile(const char filename[], smHTRData* data); -DLL smReturnCode smSetupMotionFile(smModelID modelID, const char filename[], - int first, int last, const char trcfilename[]); -DLL smReturnCode smSetupMotionWithTRCFile(smModelID modelID, const char filename[], - int first, int last, int numMarkers, - const char trcfilename[], - smTRCHeaderInfo *trcHeader); -DLL smReturnCode smWriteFrame(smModelID modelID); -DLL smReturnCode smWriteFrameWithTRC(smModelID modelID, smTRCFrame *trc, - smTRCFrame *predicted, smSolveInfoStruct *info); -DLL smReturnCode smCloseMotionFile(smModelID modelID, const char filename[]); -DLL smReturnCode smCloseMotionWithTRCFile(smModelID modelID, const char filename[]); - -/**************************** utility functions ***************************/ -DLL void smSetWorkingDirectory(const char path[]); -DLL int smGetVersionNumber(void); -DLL const char *smGetVersionString(void); -DLL const char *smGetWorkingDirectory(void); -DLL const char *smGetErrorMessage(smReturnCode code); -DLL const char *smGetMessage(smModelID modelID); -DLL const char *smGetErrorBuffer(smModelID modelID); -DLL void smClearMessage(smModelID modelID); -DLL void smClearErrorBuffer(smModelID modelID); -DLL void smUseGlobalMessages(smBoolean flag); -DLL const char *smGetGlobalMessage(void); -DLL const char *smGetGlobalErrorBuffer(void); -DLL void smSetStandAlone(void); -DLL double smGetConversion(smUnit from, smUnit to); -DLL void smSetCriticalMarkerNames(int numNames, const char *names[]); - -/**************************** set model parameters ****************************/ -DLL smReturnCode smResetModel(smModelID modelID); -DLL smReturnCode smSetPoseGenCoords(smModelID modelID, int numGenCoords, double gencoords[]); -DLL smReturnCode smSetPoseTransforms(smModelID modelID, int numSegments, smTransform segmentTransforms[]); -DLL smReturnCode smSetMarkerOrder(smModelID modelID, int numMarkers, char *markerNameList[]); -DLL smReturnCode smSetOptions(smModelID modelID, smOptions *options); -DLL smReturnCode smSetSolverMethod(smModelID modelID, smSolverMethod method); -DLL smReturnCode smSetMarkerWeight(smModelID modelID, const char segName[], - const char markerName[], double weight); -DLL smReturnCode smSetGravityDirection(smModelID modelID, smModel *smmodel, smAxes gravity); -DLL smReturnCode smSetAllSegmentSolveStates(smModelID modelID, int numSegments, smBoolean segmentStates[]); -DLL smReturnCode smSetSegmentSolveState(smModelID modelID, char *segmentName, smBoolean state); -DLL smReturnCode smSetSegmentSolveStateIndex(smModelID modelID, int index, smBoolean state); -DLL smReturnCode smSetAllGencoordSolveStates(smModelID modelID, int numGCs, smBoolean gcStates[]); -DLL smReturnCode smSetGencoordSolveState(smModelID modelID, char *gcName, smBoolean state); -DLL smReturnCode smSetGencoordSolveStateIndex(smModelID modelID, int index, smBoolean state); - -/**************************** get model parameters ****************************/ -DLL int smGetNumGenCoords(smModelID modelID); -DLL int smGetNumMarkers(smModelID modelID); -DLL int smGetNumMarkerLinks(smModelID modelID); -DLL smReturnCode smGetGenCoordMapping(smModelID modelID, smGenCoordMapping gcMap[]); -DLL smReturnCode smGetSegmentMapping(smModelID modelID, smSegmentMapping segMap[]); -DLL smReturnCode smGetOptions(smModelID, smOptions *options); -DLL smReturnCode smGetTRC(smModelID modelID, smTRCFrame *trc); -DLL smReturnCode smGetGlobalMarkerPositions(smModelID modelID, char **segmentNames, char **markerNames, smTRCFrame *trc); -DLL smReturnCode smGetGenCoordNames(smModelID modelID, smGCNames *gcNames); -DLL smReturnCode smGetHTRBasePosition(smModelID modelID, smHTRFrame *htr); -DLL smReturnCode smGetHTRPose(smModelID modelID, int nq, double q[], smHTRFrame *htr); -DLL smReturnCode smGetMatrixBasePosition(smModelID modelID, smMatrixFrame *matrix); -DLL smReturnCode smGetMatrixPose(smModelID modelID, int nq, double q[], - smMatrixFrame *matrix); -DLL smReturnCode smRemoveExtraMarkers(smModel *model, int numMarkersUsed, char *markerNamesUsed[]); -DLL smReturnCode smConvertPointFromGround(smModelID modelID, int smIndex, - smPoint3 *pt1, smPoint3 *pt2); -DLL smReturnCode smConvertPoint(smModelID modelID, const char fromName[], const char toName[], - smPoint3 *pt); -DLL smReturnCode smConvertVec(smModelID modelID, char fromName[], char toName[], - smPoint3 *vec); -DLL smReturnCode smGetGroundPlane(smModelID modelID, smPlaneStruct *plane); -DLL smReturnCode smSetGroundPlane(smModelID modelID, smPlaneStruct *plane); -DLL smReturnCode smGetPoseGenCoords(smModelID modelID, int numGenCoords, double gencoords[]); -DLL smReturnCode smGetPoseTransforms(smModelID modelID, int numSegments, smTransform segmentTransforms[]); -DLL int smGetNumSegments(smModelID modelID); -DLL smReturnCode smGetAllSegmentSolveStates(smModelID modelID, int numSegments, smBoolean segmentStates[]); -DLL smReturnCode smGetSegmentSolveState(smModelID modelID, char * segmentName, smBoolean * segmentState); -DLL smReturnCode smGetSegmentSolveStateIndex(smModelID modelID, int index, smBoolean * segmentState); -DLL smReturnCode smGetAllGencoordSolveStates(smModelID modelID, int numGCs, smBoolean gcStates[]); -DLL smReturnCode smGetGencoordSolveState(smModelID modelID, char * gcName, smBoolean * gcState); -DLL smReturnCode smGetGencoordSolveStateIndex(smModelID modelID, int index, smBoolean * gcState); - -#if SMDEBUG -/*** debug ***/ -DLL void displayModFileInfo(smModel model); -DLL smReturnCode writeModFileInfo(smModel model, char filename[], char modFilename[]); -DLL smReturnCode printsmModel(smModel *mod, char filename[]); -DLL smReturnCode smWriteFrameWithQs(smModelID modelID, int nq, double q[], smTRCFrame *trc, - smTRCFrame *predicted, smSolveInfoStruct *info); -#endif - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/test/arm_l.msl b/OpenSim/Utilities/simmToOpenSim/test/arm_l.msl deleted file mode 100644 index 5632e30fc7..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/arm_l.msl +++ /dev/null @@ -1,1132 +0,0 @@ -/******************************************************************************* - ARM MODEL - muscle file - - (c) Copyright 2001-5, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the muscle definitions for the left arm model of an average - adult male. It was developed by several people: the shoulder was modeled by - Kate Saul Holzbaur, M.S., the elbow was modeled by Wendy Murray, Ph.D., and the - wrist was modeled by Scott Delp, Ph.D. and Wendy Murray. - - This model is described in the following papers: - - Murray WM, Bryden, AM, Kilgore KL, and Keith MW. Factors influencing elbow - stability during wrist extension provided by brachioradialis tendon transfer. - Submitted to J Hand Surg. 2004. - - Murray, W.M., Arnold, A.S., Salinas, S., Durbhakula, M. Buchanan, T.S., - and Delp, S.L. "Building biomechanical models based on medical image - data: an assessment of model accuracy," Proc. First International Conference - on Medical Image Computing and Computer Assisted Interventions, - pp. 539-549, 1998. - - Murray, W. M., Buchanan, T. S., Delp, S. L. The isometric functional - capacity of elbow muscles, Journal of Biomechanics, vol. 33, - pp. 943-952, 2000. - - Murray, W.A., Delp, S. L., Buchanan, T. S. Variation of muscle moment arms - with elbow and forearm position, Journal of Biomechanics, vol. 28, - pp. 513-525, 1995. - - Delp, S. L., Grierson, A. E., Buchanan, T. S. Moments generated by the wrist - muscles in flexion-extension and radial-ulnar deviation, Journal of - Biomechanics, vol. 29, pp. 1371-1376, 1996. - - Gonzalez, R. V., Buchanan, T. S., Delp, S. L. How muscle architecture and - moment arms affect wrist flexion-extension moments, Journal of Biomechanics, - Vol. 30, pp. 705-712, 1997. - - Herrmann, A., Delp, S.L. Moment Arms and Force-Generating Capacity of the - Extensor Carpi Ulnaris After Transfer to the Extensor Carpi Radialis Brevis, - Journal of Hand Surgery, vol. 24A, pp. 1083-1090, 1999. - -*******************************************************************************/ - -#ifndef LEFT_HAND_FULL - #define LEFT_HAND_FULL 1 -#endif - -#ifndef DEFAULT_MUSCLE -beginmuscle defaultmuscle -begintendonforcelengthcurve -/* (tendon strain, normalized force) */ -(-1.000,0.0000) -(-0.0020,0.0000) -(-0.0010,0.0000) -(0.00000,0.0000) -(0.00131,0.0108) -(0.00281,0.0257) -(0.00431,0.0435) -(0.00581,0.0652) -(0.00731,0.0915) -(0.00881,0.123) -(0.01030,0.161) -(0.01180,0.208) -(0.01230,0.227) -(9.20000,345.0) -(9.20100,345.0) -(9.20200,345.0) -(12.0000,345.0) -endtendonforcelengthcurve - -beginactiveforcelengthcurve -/* (norm length, norm force) */ -(0.000000,0.000000) -(0.401000,0.000000) -(0.402000,0.000000) -(0.403500,0.000000) -(0.527250,0.226667) -(0.628750,0.636667) -(0.718750,0.856667) -(0.861250,0.950000) -(1.045000,0.993333) -(1.217500,0.770000) -(1.438750,0.246667) -(1.618750,0.000000) -(1.620000,0.000000) -(1.621000,0.000000) -(2.000000,0.000000) -endactiveforcelengthcurve - -beginpassiveforcelengthcurve -/* (norm length, norm force) */ -(0.80000,0.000000) -(0.998000,0.000000) -(0.999000,0.000000) -(1.000000,0.000000) -(1.100000,0.035) -(1.200000,0.120) -(1.300000,0.260) -(1.400000,0.550) -(1.500000,1.170) -(1.600000,2.000000) -(1.601000,2.000000) -(1.602000,2.000000) -(2.000000,2.000000) -endpassiveforcelengthcurve - -beginforcevelocitycurve -/* velocity, normalized force */ -(-1.001000,0.000000) -(-1.000000,0.000000) -(-0.950000,0.010417) -(-0.900000,0.021739) -(-0.850000,0.034091) -(-0.800000,0.047619) -(-0.750000,0.062500) -(-0.700000,0.078947) -(-0.650000,0.097222) -(-0.600000,0.117647) -(-0.550000,0.140625) -(-0.500000,0.166667) -(-0.450000,0.196429) -(-0.400000,0.230769) -(-0.350000,0.270833) -(-0.300000,0.318182) -(-0.250000,0.375000) -(-0.200000,0.444444) -(-0.150000,0.531250) -(-0.100000,0.642857) -(-0.050000,0.791667) -(0.000000,1.000000) -(0.050000,1.482014) -(0.100000,1.601571) -(0.150000,1.655791) -(0.200000,1.686739) -(0.250000,1.706751) -(0.300000,1.720753) -(0.350000,1.731099) -(0.400000,1.739055) -(0.450000,1.745365) -(0.500000,1.750490) -(0.550000,1.754736) -(0.600000,1.758312) -(0.650000,1.761364) -(0.700000,1.763999) -(0.750000,1.766298) -(0.800000,1.768321) -(0.850000,1.770115) -(0.900000,1.771717) -(0.950000,1.773155) -(1.000000,1.774455) -endforcevelocitycurve -max_contraction_velocity 10.0 /* fiberlengths/second */ -min_thickness 0.002 -max_thickness 0.005 -min_material muscle_min -max_material muscle_max -endmuscle -#endif - - -beginmuscle deltoid1_l -beginpoints - 0.00896 -0.11883 -0.00585 segment humerus_l - 0.01623 -0.11033 -0.00412 segment humerus_l - 0.04934 -0.03691 -0.00112 segment scapula_l --0.01589 0.01255 -0.09104 segment clavicle_l -endpoints -begingroups left shoulder_l endgroups -max_force 1142.600 -optimal_fiber_length 0.09760 -tendon_slack_length 0.10494 -pennation_angle 22.000 -activation 1.000 -visible yes -wrapobject delt2hum_l -wrapobject delt1hh_l range 2 3 -endmuscle - -beginmuscle deltoid2_l -beginpoints - 0.00461 -0.13611 -0.00560 segment humerus_l - 0.02235 0.00525 -0.05422 segment DELT2pt2_l --0.00847 0.00107 -0.03843 segment scapula_l --0.02536 0.00020 -0.00944 segment scapula_l -endpoints -begingroups left shoulder_l endgroups -max_force 1142.600 -optimal_fiber_length 0.10780 -tendon_slack_length 0.11442 -pennation_angle 15.000 -activation 1.000 -visible yes -wrapobject delt2_l -wrapobject delt2hum_l -wrapobject tmaj_lat_pec_corbhh_l -endmuscle - -beginmuscle deltoid3_l -beginpoints --0.06325 0.00138 0.02851 segment scapula_l --0.08225 -0.03728 -0.01399 segment scapula_l - 0.00206 -0.07602 -0.01045 segment humerus_l -endpoints -begingroups left shoulder_l endgroups -max_force 259.880 -optimal_fiber_length 0.13670 -tendon_slack_length 0.05263 -pennation_angle 18.000 -activation 1.000 -visible yes -wrapobject delt3_l -wrapobject delt3hum_l -wrapobject tmaj_lat_pec_corbhh_l -endmuscle - -beginmuscle supraspinatus_l -beginpoints - 0.00256 0.01063 -0.02593 segment humerus_l --0.02177 0.00144 0.01443 segment scapula_l --0.04994 -0.01716 0.06645 segment scapula_l -endpoints -begingroups left shoulder_l endgroups -max_force 487.820 -optimal_fiber_length 0.06820 -tendon_slack_length 0.05087 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject supsp_l -endmuscle - -beginmuscle infraspinatus_l -beginpoints --0.00887 0.00484 -0.02448 segment humerus_l --0.08379 -0.06215 0.05426 segment scapula_l -endpoints -begingroups left shoulder_l endgroups -max_force 1210.840 -optimal_fiber_length 0.07550 -tendon_slack_length 0.04436 -pennation_angle 18.500 -activation 1.000 -visible yes -wrapobject infsp_l -wrapobject infsp_tmin_hum_head_l -endmuscle - -beginmuscle subscapularis_l -beginpoints - 0.01403 0.00748 0.01331 segment humerus_l --0.01754 -0.04104 0.03092 segment scapula_l --0.08224 -0.04162 0.07349 segment scapula_l -endpoints -begingroups left shoulder_l endgroups -max_force 1377.810 -optimal_fiber_length 0.08730 -tendon_slack_length 0.05027 -pennation_angle 20.000 -activation 1.000 -visible yes -wrapobject subscap_l -endmuscle - -beginmuscle teres_minor_l -beginpoints --0.00110 -0.01264 -0.02156 segment humerus_l --0.10752 -0.09070 0.05376 segment scapula_l --0.10945 -0.09217 0.06013 segment scapula_l -endpoints -begingroups left shoulder_l endgroups -max_force 354.250 -optimal_fiber_length 0.07410 -tendon_slack_length 0.08975 -pennation_angle 24.000 -activation 1.000 -visible yes -wrapobject tmin_l range 1 2 -wrapobject infsp_tmin_hum_head_l range 1 2 -wrapobject tminhum_l range 1 2 -endmuscle - -beginmuscle teres_major_l -beginpoints - 0.00998 -0.05419 0.00568 segment humerus_l --0.11650 -0.11712 0.06616 segment scapula_l --0.11905 -0.12366 0.08078 segment scapula_l -endpoints -begingroups left shoulder_l endgroups -max_force 425.390 -optimal_fiber_length 0.16240 -tendon_slack_length 0.04507 -pennation_angle 16.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_l -wrapobject lat_tmajhh_l range 1 2 -wrapobject lat_tmaj2hh_l -endmuscle - -beginmuscle pectoralis_major1_l -beginpoints - 0.01169 -0.04191 -0.00780 segment humerus_l - 0.01615 -0.04045 0.00577 segment PEC1pt2_l - 0.01087 -0.01713 -0.14374 segment PEC1pt3_l - 0.00364 -0.00015 -0.05803 segment clavicle_l -endpoints -begingroups left shoulder_l endgroups -max_force 364.410 -optimal_fiber_length 0.14420 -tendon_slack_length 0.01761 -pennation_angle 17.000 -activation 1.000 -visible yes -wrapobject thorax_l -wrapobject pec12hum_l range 2 3 -wrapobject pec1hh_l -endmuscle - -beginmuscle pectoralis_major2_l -beginpoints - 0.01274 -0.04289 -0.00785 segment humerus_l - 0.01453 -0.04568 0.00687 segment PEC2pt2_l - 0.02290 -0.04727 -0.14403 segment PEC2pt3_l - 0.03308 0.27749 -0.11015 segment thorax - 0.03362 0.27095 -0.02578 segment thorax -endpoints -begingroups left shoulder_l endgroups -max_force 515.410 -optimal_fiber_length 0.13850 -tendon_slack_length 0.11784 -pennation_angle 25.000 -activation 1.000 -visible yes -wrapobject thorax_l -wrapobject pec12hum_l range 2 3 -wrapobject tmaj_lat_pec_corbhh_l -wrapobject pec23hh_l -endmuscle - -beginmuscle pectoralis_major3_l -beginpoints - 0.01269 -0.04375 -0.00750 segment humerus_l - 0.01243 -0.05157 0.01807 segment PEC3pt2_l - 0.03387 -0.08058 -0.13064 segment PEC3pt3_l - 0.05759 0.22647 -0.10141 segment thorax - 0.06772 0.19269 -0.04256 segment thorax -endpoints -begingroups left shoulder_l endgroups -max_force 390.550 -optimal_fiber_length 0.13850 -tendon_slack_length 0.17028 -pennation_angle 25.000 -activation 1.000 -visible yes -wrapobject thorax_l -wrapobject pec3hum_l range 2 3 -wrapobject pec23hh_l -endmuscle - -beginmuscle latissimus_dorsi1_l -beginpoints - 0.01050 -0.03415 0.00653 segment humerus_l --0.09020 -0.09739 0.02483 segment LAT1pt2_l --0.12729 -0.11099 0.06040 segment LAT1pt3_l --0.13625 0.20716 -0.03764 segment thorax --0.11071 0.18893 -0.01001 segment thorax -endpoints -begingroups left shoulder_l endgroups -max_force 389.100 -optimal_fiber_length 0.25400 -tendon_slack_length 0.17021 -pennation_angle 25.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_l -wrapobject lat_tmajhh_l range 1 2 -wrapobject lat_tmaj2hh_l -endmuscle - -beginmuscle latissimus_dorsi2_l -beginpoints - 0.00968 -0.04071 0.00611 segment humerus_l --0.08671 -0.11600 0.02052 segment LAT2pt2_l --0.11411 -0.15855 0.05968 segment LAT2pt3_l --0.12676 0.13009 -0.03267 segment thorax --0.09352 0.11701 -0.01485 segment thorax -endpoints -begingroups left shoulder_l endgroups -max_force 389.100 -optimal_fiber_length 0.23240 -tendon_slack_length 0.23279 -pennation_angle 19.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_l -wrapobject lat_tmajhh_l range 1 2 -wrapobject lat_tmaj2hh_l -endmuscle - -beginmuscle latissimus_dorsi3_l -beginpoints - 0.01208 -0.03922 0.00416 segment humerus_l --0.06445 -0.15099 0.02418 segment LAT3pt2_l --0.10725 -0.19923 0.05228 segment LAT3pt3_l --0.12863 0.10196 -0.06279 segment thorax --0.08278 0.03986 -0.01029 segment thorax -endpoints -begingroups left shoulder_l endgroups -max_force 281.660 -optimal_fiber_length 0.27890 -tendon_slack_length 0.20053 -pennation_angle 21.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_l -wrapobject lat_tmajhh_l range 1 2 -wrapobject lat_tmaj2hh_l -endmuscle - -beginmuscle coracobrachialis_l -beginpoints - 0.01419 -0.04684 0.03010 segment scapula_l - 0.00548 -0.07897 0.01774 segment scapula_l - 0.00743 -0.15048 0.00782 segment humerus_l -endpoints -begingroups left shoulder_l endgroups -max_force 242.736 -optimal_fiber_length 0.09320 -tendon_slack_length 0.09722 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject corbhum_l midpoint -wrapobject tmaj_lat_pec_corbhh_l -endmuscle - -beginmuscle triceps_longhead_l -beginpoints --0.05181 -0.04623 0.01563 segment scapula_l --0.02714 -0.11441 0.00664 segment humerus_l --0.03184 -0.22637 0.01217 segment humerus_l --0.01743 -0.26757 0.01208 segment humerus_l --0.02190 0.01046 0.00078 segment ulna_l -endpoints -begingroups left elbow_l endgroups -max_force 798.847 -optimal_fiber_length 0.13400 -tendon_slack_length 0.14311 -pennation_angle 12.000 -activation 1.000 -visible yes -wrapobject tri_l -wrapobject trilonghh_l -wrapobject trilongglen_l -endmuscle - -beginmuscle triceps_lateralis_l -beginpoints --0.00599 -0.12646 -0.00428 segment humerus_l --0.02344 -0.14528 -0.00928 segment humerus_l --0.03184 -0.22637 0.01217 segment humerus_l --0.01743 -0.26757 0.01208 segment humerus_l --0.02190 0.01046 0.00078 segment ulna_l -endpoints -begingroups left elbow_l endgroups -max_force 624.300 -optimal_fiber_length 0.11380 -tendon_slack_length 0.09800 -pennation_angle 9.000 -activation 1.000 -visible yes -wrapobject tri_l -endmuscle - -beginmuscle triceps_medialis_l -beginpoints --0.00838 -0.13695 0.00906 segment humerus_l --0.02601 -0.15139 0.01080 segment humerus_l --0.03184 -0.22637 0.01217 segment humerus_l --0.01743 -0.26757 0.01208 segment humerus_l --0.02190 0.01046 0.00078 segment ulna_l -endpoints -begingroups left elbow_l endgroups -max_force 624.300 -optimal_fiber_length 0.11380 -tendon_slack_length 0.09080 -pennation_angle 9.000 -activation 1.000 -visible yes -wrapobject tri_l -endmuscle - -beginmuscle anconeus_l -beginpoints --0.00744 -0.28359 -0.00979 segment humerus_l --0.02532 -0.00124 -0.00600 segment ulna_l -endpoints -begingroups left elbow_l endgroups -max_force 350.000 -optimal_fiber_length 0.02700 -tendon_slack_length 0.01800 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject anc_l -endmuscle - -beginmuscle supinator_brevis_l -beginpoints - 0.00996 -0.06096 -0.00075 segment radius_l - 0.01201 -0.05170 0.00107 segment radius_l --0.01360 -0.03384 -0.02013 segment ulna_l -endpoints -begingroups left forearm_l endgroups -max_force 476.000 -optimal_fiber_length 0.03300 -tendon_slack_length 0.02800 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject sup_l axial -endmuscle - -beginmuscle biceps_longhead_l -beginpoints --0.03545 -0.02671 0.01481 segment scapula_l --0.02377 -0.01486 0.00523 segment scapula_l - 0.02131 0.01793 -0.01028 segment humerus_l - 0.02378 -0.00511 -0.01201 segment humerus_l - 0.01345 -0.02827 -0.00136 segment humerus_l - 0.01068 -0.07736 0.00165 segment humerus_l - 0.01703 -0.12125 -0.00024 segment humerus_l - 0.02280 -0.17540 0.00630 segment humerus_l - 0.00000 0.00000 0.00000 segment bicpt2_l - 0.00000 0.00000 0.00000 segment bicpt_l ranges 1 pro_supination_l ( -36.60, 90.00) --0.00200 -0.03750 0.00200 segment radius_l -endpoints -begingroups left elbow_l endgroups -max_force 624.300 -optimal_fiber_length 0.11570 -tendon_slack_length 0.27571 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_bic_brd_l -wrapobject biclong_l range 2 3 -endmuscle - -beginmuscle biceps_shorthead_l -beginpoints - 0.01439 -0.04462 0.02979 segment scapula_l - 0.00106 -0.07609 0.01808 segment scapula_l - 0.01117 -0.07576 0.01101 segment humerus_l - 0.01703 -0.12125 0.01079 segment humerus_l - 0.02280 -0.17540 0.00630 segment humerus_l - 0.00000 0.00000 0.00000 segment bicpt2_l - 0.00000 0.00000 0.00000 segment bicpt_l ranges 1 pro_supination_l ( -36.60, 90.00) --0.00200 -0.03750 0.00200 segment radius_l -endpoints -begingroups left elbow_l endgroups -max_force 435.560 -optimal_fiber_length 0.13210 -tendon_slack_length 0.19856 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_bic_brd_l -endmuscle - -beginmuscle brachialis_l -beginpoints - 0.00680 -0.17390 0.00360 segment humerus_l - 0.01894 -0.28559 0.01105 segment humerus_l ranges 1 elbow_flexion_l ( 0.00, 46.59) - 0.00498 -0.01463 -0.00128 segment ulna_l ranges 1 elbow_flexion_l ( 0.00, 46.59) --0.00320 -0.02390 -0.00090 segment ulna_l -endpoints -begingroups left elbow_l endgroups -max_force 987.260 -optimal_fiber_length 0.08580 -tendon_slack_length 0.05350 -pennation_angle 0.000 -activation 1.000 -visible yes -endmuscle - -beginmuscle brachioradialis_l -beginpoints --0.00980 -0.19963 -0.00223 segment humerus_l - 0.03577 -0.12742 -0.02315 segment radius_l - 0.04190 -0.22100 -0.02240 segment radius_l -endpoints -begingroups left elbow_l endgroups -max_force 261.330 -optimal_fiber_length 0.17260 -tendon_slack_length 0.13300 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_bic_brd_l -endmuscle - -beginmuscle pronator_teres_l -beginpoints - 0.00360 -0.27590 0.03650 segment humerus_l - 0.00846 -0.03373 0.01432 segment ulna_l - 0.00464 -0.00026 0.00214 segment ptpt_l - 0.02360 -0.09340 -0.00940 segment radius_l ranges 1 pro_supination_l ( -90.00, 29.20) - 0.02540 -0.10880 -0.01980 segment radius_l -endpoints -begingroups left elbow_l forearm_l endgroups -max_force 566.220 -optimal_fiber_length 0.04920 -tendon_slack_length 0.09800 -pennation_angle 10.000 -activation 1.000 -visible yes -wrapobject elbow_pt_ecrl_l -endmuscle - -beginmuscle pron_quad_l -beginpoints - 0.03245 -0.19998 -0.01962 segment radius_l - 0.00193 -0.20972 -0.03632 segment ulna_l -endpoints -begingroups left forearm_l endgroups -max_force 75.480 -optimal_fiber_length 0.02820 -tendon_slack_length 0.00500 -pennation_angle 10.000 -activation 1.000 -visible yes -wrapobject pq2_l -wrapobject pq1_l -endmuscle - -#if LEFT_HAND_FULL - -beginmuscle ext_carp_rad_long_l -beginpoints --0.00730 -0.26090 -0.00910 segment humerus_l - 0.03195 -0.13463 -0.02779 segment radius_l - 0.04243 -0.23684 -0.03620 segment radius_l - 0.00465 -0.01363 -0.00522 segment index_metacarpal_l -endpoints -begingroups left elbow_l endgroups -max_force 304.890 -optimal_fiber_length 0.08100 -tendon_slack_length 0.22400 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_pt_ecrl_l -wrapobject ecrl_l -endmuscle - -beginmuscle ext_carp_rad_brev_l -beginpoints - 0.01349 -0.29048 -0.01698 segment humerus_l - 0.02905 -0.13086 -0.02385 segment radius_l - 0.03549 -0.22805 -0.03937 segment radius_l - 0.00492 -0.00439 -0.00646 segment middle_pre_l -endpoints -begingroups left forearm_l wrist_ext_l radial_l endgroups -max_force 100.520 -optimal_fiber_length 0.05850 -tendon_slack_length 0.22230 -pennation_angle 8.900 -activation 1.000 -visible yes -wrapobject ecrb_l -endmuscle - -beginmuscle ext_carp_ulnaris_l -beginpoints - 0.00083 -0.28955 -0.01880 segment humerus_l --0.01391 -0.03201 -0.02947 segment ulna_l --0.01705 -0.05428 -0.02868 segment ulna_l --0.01793 -0.09573 -0.03278 segment ulna_l --0.01421 -0.22696 -0.03481 segment radius_l --0.00368 -0.00667 -0.00398 segment pinky_metacarpal_l -endpoints -begingroups left forearm_l wrist_ext_l ulnar_l endgroups -max_force 93.170 -optimal_fiber_length 0.06220 -tendon_slack_length 0.22850 -pennation_angle 3.500 -activation 1.000 -visible yes -wrapobject ecu_l -endmuscle - -beginmuscle flex_carpi_rad_l -beginpoints - 0.00758 -0.27806 0.03705 segment humerus_l - 0.02110 -0.21943 -0.00127 segment radius_l - 0.00101 -0.00896 0.00518 segment index_metacarpal_l -endpoints -begingroups left forearm_l wrist_flex_l radial_l endgroups -max_force 73.960 -optimal_fiber_length 0.06280 -tendon_slack_length 0.24400 -pennation_angle 3.100 -activation 1.000 -visible yes -wrapobject elbow_pt_ecrl_l -wrapobject fcr_l -endmuscle - -beginmuscle flex_carpi_ulna_l -beginpoints - 0.00219 -0.27740 0.03880 segment humerus_l - 0.00549 0.00000 0.00000 segment FCUpt_l - 0.01082 -0.22327 -0.00969 segment radius_l --0.00258 -0.01017 0.00260 segment pinky_metacarpal_l -endpoints -begingroups left forearm_l wrist_flex_l ulnar_l endgroups -max_force 128.930 -optimal_fiber_length 0.05090 -tendon_slack_length 0.26500 -pennation_angle 12.100 -activation 1.000 -visible yes -wrapobject fcu_l -wrapobject elbow_pt_ecrl_l -endmuscle - -beginmuscle palmaris_longus_l -beginpoints - 0.00457 -0.27519 0.03865 segment humerus_l - 0.02531 -0.23915 0.00276 segment radius_l - 0.00917 -0.01898 0.01754 segment wrist_distal_l - 0.00186 -0.02325 -0.00020 segment middle_pre_l -endpoints -begingroups left forearm_l wrist_flex_l hand_l radial_l endgroups -max_force 26.720 -optimal_fiber_length 0.06380 -tendon_slack_length 0.26940 -pennation_angle 4.000 -activation 1.000 -visible yes -wrapobject pl_l -wrapobject elbow_pt_ecrl_l -endmuscle - -beginmuscle flex_dig_sup_lat_l -beginpoints - 0.00421 -0.27598 0.03864 segment humerus_l - 0.01377 -0.18718 -0.00205 segment radius_l --0.00235 -0.01393 0.01376 segment wrist_distal_l --0.00476 -0.02294 0.01020 segment ring_metacarpal_l - 0.00028 -0.04687 0.00634 segment pinky_metacarpal_l - 0.00158 -0.03659 0.00216 segment pinky_prox_l --0.00049 -0.02071 0.00305 segment pinky_mid_l -endpoints -begingroups left forearm_l wrist_flex_l ulnar_l hand_l endgroups -max_force 16.550 -optimal_fiber_length 0.05150 -tendon_slack_length 0.3383 -pennation_angle 5.000 -activation 1.000 -visible yes -wrapobject fds_l -wrapobject elbow_pt_ecrl_l -endmuscle - -beginmuscle flex_dig_sup_ring_l -beginpoints - 0.00479 -0.27880 0.03731 segment humerus_l - 0.01571 -0.18666 -0.00267 segment radius_l --0.00082 -0.01344 0.01359 segment wrist_distal_l --0.00910 -0.02470 0.00852 segment middle_pre_l --0.00186 -0.05480 0.00747 segment ring_metacarpal_l - 0.00051 -0.03981 0.00306 segment ring_prox_l - 0.00087 -0.02561 0.00273 segment ring_mid_l -endpoints -begingroups left forearm_l wrist_flex_l ulnar_l hand_l endgroups -max_force 57.900 -optimal_fiber_length 0.07360 -tendon_slack_length 0.328 -pennation_angle 4.000 -activation 1.000 -visible yes -wrapobject fds_l -wrapobject elbow_pt_ecrl_l -endmuscle - -beginmuscle flex_dig_sup_mid_l -beginpoints --0.00669 -0.02713 0.00191 segment ulna_l - 0.01899 -0.18706 -0.00424 segment radius_l - 0.00680 -0.01377 0.01399 segment wrist_distal_l - 0.00134 -0.02413 0.00707 segment middle_pre_l - 0.00213 -0.05608 0.00379 segment middle_pre_l - 0.00147 -0.04328 0.00205 segment middle_prox_l - 0.00074 -0.02717 0.00333 segment middle_mid_l -endpoints -begingroups left forearm_l wrist_flex_l radial_l hand_l endgroups -max_force 91.030 -optimal_fiber_length 0.07490 -tendon_slack_length 0.295 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fds_l -endmuscle - -beginmuscle flex_dig_sup_ind_l -beginpoints --0.00676 -0.02819 0.00137 segment ulna_l - 0.02145 -0.18623 -0.00552 segment radius_l - 0.00889 -0.01366 0.01387 segment wrist_distal_l --0.00243 -0.02854 0.00368 segment index_metacarpal_l --0.00199 -0.05267 0.00823 segment index_metacarpal_l - 0.00035 -0.03429 0.00029 segment index_prox_l - 0.00031 -0.02084 0.00328 segment index_mid_l -endpoints -begingroups left forearm_l wrist_flex_l radial_l hand_l endgroups -max_force 61.240 -optimal_fiber_length 0.08350 -tendon_slack_length 0.27500 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject fds_l -endmuscle - -beginmuscle flex_dig_prof_lat_l -beginpoints --0.00628 -0.03214 -0.00254 segment ulna_l - 0.01409 -0.18516 -0.00861 segment radius_l --0.00088 -0.01027 0.01188 segment wrist_distal_l --0.00779 -0.02305 0.00375 segment ring_metacarpal_l --0.00120 -0.04620 0.00546 segment pinky_metacarpal_l --0.00060 -0.03590 0.00353 segment pinky_prox_l --0.00034 -0.00485 0.00163 segment pinky_distal_l -endpoints -begingroups left forearm_l wrist_flex_l ulnar_l hand_l endgroups -max_force 79.650 -optimal_fiber_length 0.07490 -tendon_slack_length 0.28190 -pennation_angle 8.000 -activation 1.000 -visible yes -wrapobject fdp_l -endmuscle - -beginmuscle flex_dig_prof_ring_l -beginpoints --0.00502 -0.03374 -0.00273 segment ulna_l - 0.01580 -0.18629 -0.00803 segment radius_l - 0.00106 -0.01177 0.01184 segment wrist_distal_l --0.01037 -0.02577 0.00583 segment middle_pre_l --0.00164 -0.05450 0.00589 segment ring_metacarpal_l --0.00072 -0.04045 0.00263 segment ring_prox_l - 0.00021 -0.00738 0.00232 segment ring_distal_l -endpoints -begingroups left forearm_l wrist_flex_l ulnar_l hand_l endgroups -max_force 64.080 -optimal_fiber_length 0.07980 -tendon_slack_length 0.28200 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fdp_l -endmuscle - -beginmuscle flex_dig_prof_mid_l -beginpoints --0.00522 -0.03327 -0.00210 segment ulna_l - 0.01906 -0.18644 -0.00785 segment radius_l - 0.00335 -0.01280 0.01144 segment wrist_distal_l --0.00123 -0.02454 0.00598 segment middle_pre_l --0.00014 -0.05458 0.00480 segment middle_pre_l --0.00003 -0.04332 0.00330 segment middle_prox_l --0.00028 -0.00366 0.00352 segment middle_distal_l -endpoints -begingroups left forearm_l wrist_flex_l ulnar_l hand_l endgroups -max_force 81.650 -optimal_fiber_length 0.08350 -tendon_slack_length 0.29300 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject fdp_l -endmuscle - -beginmuscle flex_dig_prof_ind_l -beginpoints --0.00647 -0.03176 -0.00280 segment ulna_l - 0.02080 -0.18560 -0.00915 segment radius_l - 0.00896 -0.01168 0.01212 segment wrist_distal_l --0.00240 -0.02200 0.00051 segment index_metacarpal_l --0.00031 -0.05517 0.00787 segment index_metacarpal_l - 0.00192 -0.03615 0.00166 segment index_prox_l - 0.00070 -0.00422 0.00390 segment index_distal_l -endpoints -begingroups left forearm_l wrist_flex_l radial_l hand_l endgroups -max_force 68.270 -optimal_fiber_length 0.07490 -tendon_slack_length 0.29350 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fdp_l -endmuscle - -beginmuscle ext_dig_lat_l -beginpoints --0.00040 -0.28831 -0.01870 segment humerus_l - 0.00130 -0.03888 -0.01405 segment EDCpt_l - 0.00993 -0.22304 -0.03618 segment radius_l --0.00114 -0.04658 -0.00643 segment pinky_metacarpal_l --0.00175 -0.03423 -0.00191 segment pinky_prox_l --0.00150 -0.00018 -0.00169 segment pinky_distal_l -endpoints -begingroups left forearm_l wrist_ext_l ulnar_l hand_l endgroups -max_force 13.110 -optimal_fiber_length 0.06500 -tendon_slack_length 0.29650 -pennation_angle 2.000 -activation 1.000 -visible yes -wrapobject edcl_l range 3 4 -endmuscle - -beginmuscle ext_dig_ring_l -beginpoints --0.00156 -0.28936 -0.01782 segment humerus_l - 0.00260 -0.03991 -0.01552 segment EDCpt_l - 0.00882 -0.22366 -0.04284 segment radius_l --0.00047 -0.05534 -0.00526 segment ring_metacarpal_l --0.00248 -0.03947 -0.00260 segment ring_prox_l --0.00135 -0.00546 -0.00151 segment ring_distal_l -endpoints -begingroups left forearm_l wrist_ext_l ulnar_l hand_l endgroups -max_force 34.040 -optimal_fiber_length 0.06260 -tendon_slack_length 0.32700 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edcr_l -endmuscle - -beginmuscle ext_dig_mid_l -beginpoints - 0.00051 -0.28984 -0.01949 segment humerus_l - 0.00316 -0.03948 -0.01528 segment EDCpt_l - 0.01440 -0.22323 -0.04223 segment radius_l - 0.00081 -0.05603 -0.00578 segment middle_pre_l - 0.00074 -0.04351 -0.00385 segment middle_prox_l - 0.00021 -0.01047 0.00060 segment middle_distal_l -endpoints -begingroups left forearm_l wrist_ext_l radial_l hand_l endgroups -max_force 35.320 -optimal_fiber_length 0.07240 -tendon_slack_length 0.33500 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edcm_l -endmuscle - -beginmuscle ext_dig_ind_l -beginpoints - 0.00065 -0.28898 -0.01869 segment humerus_l - 0.00228 -0.03918 -0.01483 segment EDCpt_l - 0.01533 -0.22314 -0.04196 segment radius_l --0.00103 -0.06275 -0.01025 segment index_metacarpal_l --0.00068 -0.01251 -0.00847 segment index_prox_l - 0.00107 -0.04412 -0.00830 segment index_prox_l --0.00067 -0.01026 -0.00427 segment index_mid_l - 0.00003 -0.02014 -0.00079 segment index_mid_l --0.00041 -0.00411 -0.00135 segment index_distal_l -endpoints -begingroups left forearm_l wrist_ext_l radial_l hand_l endgroups -max_force 18.260 -optimal_fiber_length 0.07000 -tendon_slack_length 0.32200 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edci_l range 3 4 -wrapobject secondpm_l range 6 7 -wrapobject secondmd_l range 8 9 -endmuscle - -beginmuscle ext_dig_min_l -beginpoints - 0.00089 -0.28919 -0.01847 segment humerus_l --0.00977 -0.03907 -0.03085 segment ulna_l --0.00755 -0.08267 -0.03646 segment ulna_l - 0.00454 -0.22641 -0.03678 segment radius_l --0.00120 -0.00255 -0.00836 segment pinky_metacarpal_l --0.00213 -0.04605 -0.00531 segment pinky_metacarpal_l - 0.00010 -0.03330 -0.00245 segment pinky_prox_l --0.00180 -0.00427 -0.00110 segment pinky_distal_l -endpoints -begingroups left forearm_l wrist_ext_l ulnar_l hand_l endgroups -max_force 25.250 -optimal_fiber_length 0.06750 -tendon_slack_length 0.32200 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edm_l range 4 5 -endmuscle - -beginmuscle ext_indicis_l -beginpoints --0.00394 -0.16652 -0.03682 segment ulna_l - 0.00171 -0.17469 -0.04049 segment ulna_l - 0.01076 -0.22783 -0.03664 segment radius_l --0.00958 -0.01376 -0.00864 segment index_metacarpal_l --0.00128 -0.06217 -0.01019 segment index_metacarpal_l --0.00068 -0.01171 -0.00782 segment index_prox_l - 0.00055 -0.04350 -0.00843 segment index_prox_l --0.00073 -0.00986 -0.00448 segment index_mid_l - 0.00025 -0.02103 -0.00085 segment index_mid_l - 0.00099 -0.01294 -0.00063 segment index_distal_l -endpoints -begingroups left forearm_l wrist_ext_l radial_l hand_l endgroups -max_force 21.700 -optimal_fiber_length 0.05890 -tendon_slack_length 0.18600 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject ext_ellipse_l -wrapobject secondpm_l range 7 8 -wrapobject secondmd_l range 9 10 -endmuscle - -beginmuscle ext_poll_long_l -beginpoints --0.01412 -0.09706 -0.02952 segment ulna_l - 0.00124 -0.13668 -0.02270 segment radius_l - 0.02702 -0.21872 -0.03828 segment radius_l - 0.03382 -0.22754 -0.04607 segment radius_l - 0.04234 -0.23771 -0.04173 segment radius_l - 0.02428 0.00014 -0.00608 segment wrist_distal_l --0.00885 -0.00409 -0.00805 segment trapezium_l --0.00598 -0.00312 -0.00631 segment thumb_prox_l --0.00075 -0.03084 -0.00820 segment thumb_prox_l - 0.00102 -0.00713 -0.00893 segment thumb_mid_l - 0.00216 -0.03367 -0.00448 segment thumb_mid_l - 0.00187 -0.00930 -0.00012 segment thumb_distal_l -endpoints -begingroups left forearm_l wrist_ext_l radial_l hand_l endgroups -max_force 39.460 -optimal_fiber_length 0.05400 -tendon_slack_length 0.22050 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject mpthumb_l -endmuscle - -beginmuscle ext_poll_brev_l -beginpoints - 0.01924 -0.14695 -0.02114 segment radius_l - 0.02276 -0.15857 -0.02665 segment radius_l - 0.03831 -0.19722 -0.03834 segment radius_l - 0.05235 -0.23611 -0.02699 segment radius_l - 0.03123 0.00256 0.00327 segment wrist_distal_l - 0.00525 -0.00143 -0.00518 segment thumb_prox_l - 0.00502 -0.03200 -0.00628 segment thumb_prox_l - 0.00546 -0.01080 -0.00354 segment thumb_mid_l -endpoints -begingroups left forearm_l radial_l wrist_flex_l hand_l endgroups -max_force 14.200 -optimal_fiber_length 0.06750 -tendon_slack_length 0.11550 -pennation_angle 7.200 -activation 1.000 -visible yes -wrapobject mpthumb_l -endmuscle - -beginmuscle flex_poll_long_l -beginpoints - 0.00971 -0.09458 -0.01769 segment radius_l - 0.01658 -0.13525 -0.01915 segment radius_l - 0.01869 -0.17932 -0.01988 segment radius_l - 0.03800 -0.23171 -0.01580 segment radius_l - 0.01869 0.00882 0.01547 segment FPLpt_l - 0.01468 -0.01915 0.02110 segment FPLpt2_l - 0.00054 -0.01332 0.01504 segment thumb_prox_l --0.00074 -0.02257 0.01008 segment thumb_prox_l --0.00366 -0.01491 0.01195 segment thumb_mid_l - 0.00019 -0.02782 0.00735 segment thumb_mid_l --0.00116 -0.01087 0.00372 segment thumb_distal_l -endpoints -begingroups left forearm_l wrist_flex_l radial_l hand_l endgroups -max_force 77.200 -optimal_fiber_length 0.05520 -tendon_slack_length 0.19380 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fpl_l -wrapobject ipthumb_l -wrapobject mpthumb_l -endmuscle - -beginmuscle abd_poll_long_l -beginpoints - 0.01154 -0.09477 -0.01681 segment radius_l - 0.03030 -0.13970 -0.03507 segment radius_l - 0.03993 -0.16072 -0.03400 segment radius_l - 0.04397 -0.17845 -0.03134 segment radius_l - 0.05505 -0.23084 -0.02368 segment radius_l - 0.03385 0.00634 0.00745 segment APLpt_l - 0.01095 0.00781 -0.00408 segment thumb_prox_l - 0.00933 -0.00151 -0.00306 segment thumb_prox_l - 0.00561 -0.00996 -0.00193 segment thumb_prox_l -endpoints -begingroups left forearm_l radial_l wrist_flex_l hand_l endgroups -max_force 59.530 -optimal_fiber_length 0.07130 -tendon_slack_length 0.12950 -pennation_angle 7.500 -activation 1.000 -visible yes -wrapobject apl_l range 5 6 -endmuscle - -#endif diff --git a/OpenSim/Utilities/simmToOpenSim/test/arm_r.msl b/OpenSim/Utilities/simmToOpenSim/test/arm_r.msl deleted file mode 100644 index 1b88c9e9be..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/arm_r.msl +++ /dev/null @@ -1,1132 +0,0 @@ -/******************************************************************************* - ARM MODEL - muscle file - - (c) Copyright 2001-5, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the muscle definitions for the right arm model of an average - adult male. It was developed by several people: the shoulder was modeled by - Kate Saul Holzbaur, M.S., the elbow was modeled by Wendy Murray, Ph.D., and the - wrist was modeled by Scott Delp, Ph.D. and Wendy Murray. - - This model is described in the following papers: - - Murray WM, Bryden, AM, Kilgore KL, and Keith MW. Factors influencing elbow - stability during wrist extension provided by brachioradialis tendon transfer. - Submitted to J Hand Surg. 2004. - - Murray, W.M., Arnold, A.S., Salinas, S., Durbhakula, M. Buchanan, T.S., - and Delp, S.L. "Building biomechanical models based on medical image - data: an assessment of model accuracy," Proc. First International Conference - on Medical Image Computing and Computer Assisted Interventions, - pp. 539-549, 1998. - - Murray, W. M., Buchanan, T. S., Delp, S. L. The isometric functional - capacity of elbow muscles, Journal of Biomechanics, vol. 33, - pp. 943-952, 2000. - - Murray, W.A., Delp, S. L., Buchanan, T. S. Variation of muscle moment arms - with elbow and forearm position, Journal of Biomechanics, vol. 28, - pp. 513-525, 1995. - - Delp, S. L., Grierson, A. E., Buchanan, T. S. Moments generated by the wrist - muscles in flexion-extension and radial-ulnar deviation, Journal of - Biomechanics, vol. 29, pp. 1371-1376, 1996. - - Gonzalez, R. V., Buchanan, T. S., Delp, S. L. How muscle architecture and - moment arms affect wrist flexion-extension moments, Journal of Biomechanics, - Vol. 30, pp. 705-712, 1997. - - Herrmann, A., Delp, S.L. Moment Arms and Force-Generating Capacity of the - Extensor Carpi Ulnaris After Transfer to the Extensor Carpi Radialis Brevis, - Journal of Hand Surgery, vol. 24A, pp. 1083-1090, 1999. - -*******************************************************************************/ - -#ifndef RIGHT_HAND_FULL - #define RIGHT_HAND_FULL 1 -#endif - -#ifndef DEFAULT_MUSCLE -beginmuscle defaultmuscle -begintendonforcelengthcurve -/* (tendon strain, normalized force) */ -(-1.000,0.0000) -(-0.0020,0.0000) -(-0.0010,0.0000) -(0.00000,0.0000) -(0.00131,0.0108) -(0.00281,0.0257) -(0.00431,0.0435) -(0.00581,0.0652) -(0.00731,0.0915) -(0.00881,0.123) -(0.01030,0.161) -(0.01180,0.208) -(0.01230,0.227) -(9.20000,345.0) -(9.20100,345.0) -(9.20200,345.0) -(12.0000,345.0) -endtendonforcelengthcurve - -beginactiveforcelengthcurve -/* (norm length, norm force) */ -(0.000000,0.000000) -(0.401000,0.000000) -(0.402000,0.000000) -(0.403500,0.000000) -(0.527250,0.226667) -(0.628750,0.636667) -(0.718750,0.856667) -(0.861250,0.950000) -(1.045000,0.993333) -(1.217500,0.770000) -(1.438750,0.246667) -(1.618750,0.000000) -(1.620000,0.000000) -(1.621000,0.000000) -(2.000000,0.000000) -endactiveforcelengthcurve - -beginpassiveforcelengthcurve -/* (norm length, norm force) */ -(0.80000,0.000000) -(0.998000,0.000000) -(0.999000,0.000000) -(1.000000,0.000000) -(1.100000,0.035) -(1.200000,0.120) -(1.300000,0.260) -(1.400000,0.550) -(1.500000,1.170) -(1.600000,2.000000) -(1.601000,2.000000) -(1.602000,2.000000) -(2.000000,2.000000) -endpassiveforcelengthcurve - -beginforcevelocitycurve -/* velocity, normalized force */ -(-1.001000,0.000000) -(-1.000000,0.000000) -(-0.950000,0.010417) -(-0.900000,0.021739) -(-0.850000,0.034091) -(-0.800000,0.047619) -(-0.750000,0.062500) -(-0.700000,0.078947) -(-0.650000,0.097222) -(-0.600000,0.117647) -(-0.550000,0.140625) -(-0.500000,0.166667) -(-0.450000,0.196429) -(-0.400000,0.230769) -(-0.350000,0.270833) -(-0.300000,0.318182) -(-0.250000,0.375000) -(-0.200000,0.444444) -(-0.150000,0.531250) -(-0.100000,0.642857) -(-0.050000,0.791667) -(0.000000,1.000000) -(0.050000,1.482014) -(0.100000,1.601571) -(0.150000,1.655791) -(0.200000,1.686739) -(0.250000,1.706751) -(0.300000,1.720753) -(0.350000,1.731099) -(0.400000,1.739055) -(0.450000,1.745365) -(0.500000,1.750490) -(0.550000,1.754736) -(0.600000,1.758312) -(0.650000,1.761364) -(0.700000,1.763999) -(0.750000,1.766298) -(0.800000,1.768321) -(0.850000,1.770115) -(0.900000,1.771717) -(0.950000,1.773155) -(1.000000,1.774455) -endforcevelocitycurve -max_contraction_velocity 10.0 /* fiberlengths/second */ -min_thickness 0.002 -max_thickness 0.005 -min_material muscle_min -max_material muscle_max -endmuscle -#endif - - -beginmuscle deltoid1_r -beginpoints - 0.00896 -0.11883 0.00585 segment humerus_r - 0.01623 -0.11033 0.00412 segment humerus_r - 0.04934 -0.03691 0.00112 segment scapula_r --0.01589 0.01255 0.09104 segment clavicle_r -endpoints -begingroups right right shoulder_r endgroups -max_force 1142.600 -optimal_fiber_length 0.09760 -tendon_slack_length 0.10494 -pennation_angle 22.000 -activation 1.000 -visible yes -wrapobject delt2hum_r -wrapobject delt1hh_r range 2 3 -endmuscle - -beginmuscle deltoid2_r -beginpoints - 0.00461 -0.13611 0.00560 segment humerus_r - 0.02235 0.00525 0.05422 segment DELT2pt2_r --0.00847 0.00107 0.03843 segment scapula_r --0.02536 0.00020 0.00944 segment scapula_r -endpoints -begingroups right shoulder_r endgroups -max_force 1142.600 -optimal_fiber_length 0.10780 -tendon_slack_length 0.11442 -pennation_angle 15.000 -activation 1.000 -visible yes -wrapobject delt2_r -wrapobject delt2hum_r -wrapobject tmaj_lat_pec_corbhh_r -endmuscle - -beginmuscle deltoid3_r -beginpoints --0.06325 0.00138 -0.02851 segment scapula_r --0.08225 -0.03728 0.01399 segment scapula_r - 0.00206 -0.07602 0.01045 segment humerus_r -endpoints -begingroups right shoulder_r endgroups -max_force 259.880 -optimal_fiber_length 0.13670 -tendon_slack_length 0.05263 -pennation_angle 18.000 -activation 1.000 -visible yes -wrapobject delt3_r -wrapobject delt3hum_r -wrapobject tmaj_lat_pec_corbhh_r -endmuscle - -beginmuscle supraspinatus_r -beginpoints - 0.00256 0.01063 0.02593 segment humerus_r --0.02177 0.00144 -0.01443 segment scapula_r --0.04994 -0.01716 -0.06645 segment scapula_r -endpoints -begingroups right shoulder_r endgroups -max_force 487.820 -optimal_fiber_length 0.06820 -tendon_slack_length 0.05087 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject supsp_r -endmuscle - -beginmuscle infraspinatus_r -beginpoints --0.00887 0.00484 0.02448 segment humerus_r --0.08379 -0.06215 -0.05426 segment scapula_r -endpoints -begingroups right shoulder_r endgroups -max_force 1210.840 -optimal_fiber_length 0.07550 -tendon_slack_length 0.04436 -pennation_angle 18.500 -activation 1.000 -visible yes -wrapobject infsp_r -wrapobject infsp_tmin_hum_head_r -endmuscle - -beginmuscle subscapularis_r -beginpoints - 0.01403 0.00748 -0.01331 segment humerus_r --0.01754 -0.04104 -0.03092 segment scapula_r --0.08224 -0.04162 -0.07349 segment scapula_r -endpoints -begingroups right shoulder_r endgroups -max_force 1377.810 -optimal_fiber_length 0.08730 -tendon_slack_length 0.05027 -pennation_angle 20.000 -activation 1.000 -visible yes -wrapobject subscap_r -endmuscle - -beginmuscle teres_minor_r -beginpoints --0.00110 -0.01264 0.02156 segment humerus_r --0.10752 -0.09070 -0.05376 segment scapula_r --0.10945 -0.09217 -0.06013 segment scapula_r -endpoints -begingroups right shoulder_r endgroups -max_force 354.250 -optimal_fiber_length 0.07410 -tendon_slack_length 0.08975 -pennation_angle 24.000 -activation 1.000 -visible yes -wrapobject tmin_r range 1 2 -wrapobject infsp_tmin_hum_head_r range 1 2 -wrapobject tminhum_r range 1 2 -endmuscle - -beginmuscle teres_major_r -beginpoints - 0.00998 -0.05419 -0.00568 segment humerus_r --0.11650 -0.11712 -0.06616 segment scapula_r --0.11905 -0.12366 -0.08078 segment scapula_r -endpoints -begingroups right shoulder_r endgroups -max_force 425.390 -optimal_fiber_length 0.16240 -tendon_slack_length 0.04507 -pennation_angle 16.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_r -wrapobject lat_tmajhh_r range 1 2 -wrapobject lat_tmaj2hh_r -endmuscle - -beginmuscle pectoralis_major1_r -beginpoints - 0.01169 -0.04191 0.00780 segment humerus_r - 0.01615 -0.04045 -0.00577 segment PEC1pt2_r - 0.01087 -0.01713 0.14374 segment PEC1pt3_r - 0.00364 -0.00015 0.05803 segment clavicle_r -endpoints -begingroups right shoulder_r endgroups -max_force 364.410 -optimal_fiber_length 0.14420 -tendon_slack_length 0.01761 -pennation_angle 17.000 -activation 1.000 -visible yes -wrapobject thorax_r -wrapobject pec12hum_r range 2 3 -wrapobject pec1hh_r -endmuscle - -beginmuscle pectoralis_major2_r -beginpoints - 0.01274 -0.04289 0.00785 segment humerus_r - 0.01453 -0.04568 -0.00687 segment PEC2pt2_r - 0.02290 -0.04727 0.14403 segment PEC2pt3_r - 0.03308 0.27749 0.11015 segment thorax - 0.03362 0.27095 0.02578 segment thorax -endpoints -begingroups right shoulder_r endgroups -max_force 515.410 -optimal_fiber_length 0.13850 -tendon_slack_length 0.11784 -pennation_angle 25.000 -activation 1.000 -visible yes -wrapobject thorax_r -wrapobject pec12hum_r range 2 3 -wrapobject tmaj_lat_pec_corbhh_r -wrapobject pec23hh_r -endmuscle - -beginmuscle pectoralis_major3_r -beginpoints - 0.01269 -0.04375 0.00750 segment humerus_r - 0.01243 -0.05157 -0.01807 segment PEC3pt2_r - 0.03387 -0.08058 0.13064 segment PEC3pt3_r - 0.05759 0.22647 0.10141 segment thorax - 0.06742 0.19260 0.04254 segment thorax -endpoints -begingroups right shoulder_r endgroups -max_force 390.550 -optimal_fiber_length 0.13850 -tendon_slack_length 0.17028 -pennation_angle 25.000 -activation 1.000 -visible yes -wrapobject thorax_r -wrapobject pec3hum_r range 2 3 -wrapobject pec23hh_r -endmuscle - -beginmuscle latissimus_dorsi1_r -beginpoints - 0.01050 -0.03415 -0.00653 segment humerus_r --0.09020 -0.09739 -0.02483 segment LAT1pt2_r --0.12729 -0.11099 -0.06040 segment LAT1pt3_r --0.13625 0.20716 0.03764 segment thorax --0.11071 0.18893 0.01001 segment thorax -endpoints -begingroups right shoulder_r endgroups -max_force 389.100 -optimal_fiber_length 0.25400 -tendon_slack_length 0.17021 -pennation_angle 25.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_r -wrapobject lat_tmajhh_r range 1 2 -wrapobject lat_tmaj2hh_r -endmuscle - -beginmuscle latissimus_dorsi2_r -beginpoints - 0.00968 -0.04071 -0.00611 segment humerus_r --0.08671 -0.11600 -0.02052 segment LAT2pt2_r --0.11411 -0.15855 -0.05968 segment LAT2pt3_r --0.12676 0.13009 0.03267 segment thorax --0.09352 0.11701 0.01485 segment thorax -endpoints -begingroups right shoulder_r endgroups -max_force 389.100 -optimal_fiber_length 0.23240 -tendon_slack_length 0.23279 -pennation_angle 19.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_r -wrapobject lat_tmajhh_r range 1 2 -wrapobject lat_tmaj2hh_r -endmuscle - -beginmuscle latissimus_dorsi3_r -beginpoints - 0.01208 -0.03922 -0.00416 segment humerus_r --0.06445 -0.15099 -0.02418 segment LAT3pt2_r --0.10725 -0.19923 -0.05228 segment LAT3pt3_r --0.12863 0.10196 0.06279 segment thorax --0.08278 0.03986 0.01029 segment thorax -endpoints -begingroups right shoulder_r endgroups -max_force 281.660 -optimal_fiber_length 0.27890 -tendon_slack_length 0.20053 -pennation_angle 21.000 -activation 1.000 -visible yes -wrapobject tmaj_lathum_r -wrapobject lat_tmajhh_r range 1 2 -wrapobject lat_tmaj2hh_r -endmuscle - -beginmuscle coracobrachialis_r -beginpoints - 0.01419 -0.04684 -0.03010 segment scapula_r - 0.00548 -0.07897 -0.01774 segment scapula_r - 0.00743 -0.15048 -0.00782 segment humerus_r -endpoints -begingroups right shoulder_r endgroups -max_force 242.736 -optimal_fiber_length 0.09320 -tendon_slack_length 0.09722 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject corbhum_r midpoint -wrapobject tmaj_lat_pec_corbhh_r -endmuscle - -beginmuscle triceps_longhead_r -beginpoints --0.05181 -0.04623 -0.01563 segment scapula_r --0.02714 -0.11441 -0.00664 segment humerus_r --0.03184 -0.22637 -0.01217 segment humerus_r --0.01743 -0.26757 -0.01208 segment humerus_r --0.02190 0.01046 -0.00078 segment ulna_r -endpoints -begingroups right elbow_r endgroups -max_force 798.847 -optimal_fiber_length 0.13400 -tendon_slack_length 0.14311 -pennation_angle 12.000 -activation 1.000 -visible yes -wrapobject tri_r -wrapobject trilonghh_r -wrapobject trilongglen_r -endmuscle - -beginmuscle triceps_lateralis_r -beginpoints --0.00599 -0.12646 0.00428 segment humerus_r --0.02344 -0.14528 0.00928 segment humerus_r --0.03184 -0.22637 -0.01217 segment humerus_r --0.01743 -0.26757 -0.01208 segment humerus_r --0.02190 0.01046 -0.00078 segment ulna_r -endpoints -begingroups right elbow_r endgroups -max_force 624.300 -optimal_fiber_length 0.11380 -tendon_slack_length 0.09800 -pennation_angle 9.000 -activation 1.000 -visible yes -wrapobject tri_r -endmuscle - -beginmuscle triceps_medialis_r -beginpoints --0.00838 -0.13695 -0.00906 segment humerus_r --0.02601 -0.15139 -0.01080 segment humerus_r --0.03184 -0.22637 -0.01217 segment humerus_r --0.01743 -0.26757 -0.01208 segment humerus_r --0.02190 0.01046 -0.00078 segment ulna_r -endpoints -begingroups right elbow_r endgroups -max_force 624.300 -optimal_fiber_length 0.11380 -tendon_slack_length 0.09080 -pennation_angle 9.000 -activation 1.000 -visible yes -wrapobject tri_r -endmuscle - -beginmuscle anconeus_r -beginpoints --0.00744 -0.28359 0.00979 segment humerus_r --0.02532 -0.00124 0.00600 segment ulna_r -endpoints -begingroups right elbow_r endgroups -max_force 350.000 -optimal_fiber_length 0.02700 -tendon_slack_length 0.01800 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject anc_r -endmuscle - -beginmuscle supinator_brevis_r -beginpoints - 0.00996 -0.06096 0.00075 segment radius_r - 0.01201 -0.05170 -0.00107 segment radius_r --0.01360 -0.03384 0.02013 segment ulna_r -endpoints -begingroups right forearm_r endgroups -max_force 476.000 -optimal_fiber_length 0.03300 -tendon_slack_length 0.02800 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject sup_r axial -endmuscle - -beginmuscle biceps_longhead_r -beginpoints --0.03545 -0.02671 -0.01481 segment scapula_r --0.02377 -0.01486 -0.00523 segment scapula_r - 0.02131 0.01793 0.01028 segment humerus_r - 0.02378 -0.00511 0.01201 segment humerus_r - 0.01345 -0.02827 0.00136 segment humerus_r - 0.01068 -0.07736 -0.00165 segment humerus_r - 0.01703 -0.12125 0.00024 segment humerus_r - 0.02280 -0.17540 -0.00630 segment humerus_r - 0.00000 0.00000 0.00000 segment bicpt2_r - 0.00000 0.00000 0.00000 segment bicpt_r ranges 1 pro_supination_r ( -36.60, 90.00) --0.00200 -0.03750 -0.00200 segment radius_r -endpoints -begingroups right elbow_r endgroups -max_force 624.300 -optimal_fiber_length 0.11570 -tendon_slack_length 0.27571 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_bic_brd_r -wrapobject biclong_r range 2 3 -endmuscle - -beginmuscle biceps_shorthead_r -beginpoints - 0.01439 -0.04462 -0.02979 segment scapula_r - 0.00106 -0.07609 -0.01808 segment scapula_r - 0.01117 -0.07576 -0.01101 segment humerus_r - 0.01703 -0.12125 -0.01079 segment humerus_r - 0.02280 -0.17540 -0.00630 segment humerus_r - 0.00000 0.00000 0.00000 segment bicpt2_r - 0.00000 0.00000 0.00000 segment bicpt_r ranges 1 pro_supination_r ( -36.60, 90.00) --0.00200 -0.03750 -0.00200 segment radius_r -endpoints -begingroups right elbow_r endgroups -max_force 435.560 -optimal_fiber_length 0.13210 -tendon_slack_length 0.19856 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_bic_brd_r -endmuscle - -beginmuscle brachialis_r -beginpoints - 0.00680 -0.17390 -0.00360 segment humerus_r - 0.01894 -0.28559 -0.01105 segment humerus_r ranges 1 elbow_flexion_r ( 0.00, 46.59) - 0.00498 -0.01463 0.00128 segment ulna_r ranges 1 elbow_flexion_r ( 0.00, 46.59) --0.00320 -0.02390 0.00090 segment ulna_r -endpoints -begingroups right elbow_r endgroups -max_force 987.260 -optimal_fiber_length 0.08580 -tendon_slack_length 0.05350 -pennation_angle 0.000 -activation 1.000 -visible yes -endmuscle - -beginmuscle brachioradialis_r -beginpoints --0.00980 -0.19963 0.00223 segment humerus_r - 0.03577 -0.12742 0.02315 segment radius_r - 0.04190 -0.22100 0.02240 segment radius_r -endpoints -begingroups right elbow_r endgroups -max_force 261.330 -optimal_fiber_length 0.17260 -tendon_slack_length 0.13300 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_bic_brd_r -endmuscle - -beginmuscle pronator_teres_r -beginpoints - 0.00360 -0.27590 -0.03650 segment humerus_r - 0.00846 -0.03373 -0.01432 segment ulna_r - 0.00464 -0.00026 -0.00214 segment ptpt_r - 0.02360 -0.09340 0.00940 segment radius_r ranges 1 pro_supination_r ( -90.00, 29.20) - 0.02540 -0.10880 0.01980 segment radius_r -endpoints -begingroups right elbow_r forearm_r endgroups -max_force 566.220 -optimal_fiber_length 0.04920 -tendon_slack_length 0.09800 -pennation_angle 10.000 -activation 1.000 -visible yes -wrapobject elbow_pt_ecrl_r -endmuscle - -beginmuscle pron_quad_r -beginpoints - 0.03245 -0.19998 0.01962 segment radius_r - 0.00193 -0.20972 0.03632 segment ulna_r -endpoints -begingroups right forearm_r endgroups -max_force 75.480 -optimal_fiber_length 0.02820 -tendon_slack_length 0.00500 -pennation_angle 10.000 -activation 1.000 -visible yes -wrapobject pq2_r -wrapobject pq1_r -endmuscle - -#if RIGHT_HAND_FULL - -beginmuscle ext_carp_rad_long_r -beginpoints --0.00730 -0.26090 0.00910 segment humerus_r - 0.03195 -0.13463 0.02779 segment radius_r - 0.04243 -0.23684 0.03620 segment radius_r - 0.00465 -0.01363 0.00522 segment index_metacarpal_r -endpoints -begingroups right elbow_r endgroups -max_force 304.890 -optimal_fiber_length 0.08100 -tendon_slack_length 0.22400 -pennation_angle 0.000 -activation 1.000 -visible yes -wrapobject elbow_pt_ecrl_r -wrapobject ecrl_r -endmuscle - -beginmuscle ext_carp_rad_brev_r -beginpoints - 0.01349 -0.29048 0.01698 segment humerus_r - 0.02905 -0.13086 0.02385 segment radius_r - 0.03549 -0.22805 0.03937 segment radius_r - 0.00492 -0.00439 0.00646 segment middle_pre_r -endpoints -begingroups right forearm_r wrist_ext_r radial_r endgroups -max_force 100.520 -optimal_fiber_length 0.05850 -tendon_slack_length 0.22230 -pennation_angle 8.900 -activation 1.000 -visible yes -wrapobject ecrb_r -endmuscle - -beginmuscle ext_carp_ulnaris_r -beginpoints - 0.00083 -0.28955 0.01880 segment humerus_r --0.01391 -0.03201 0.02947 segment ulna_r --0.01705 -0.05428 0.02868 segment ulna_r --0.01793 -0.09573 0.03278 segment ulna_r --0.01421 -0.22696 0.03481 segment radius_r --0.00368 -0.00667 0.00398 segment pinky_metacarpal_r -endpoints -begingroups right forearm_r wrist_ext_r ulnar_r endgroups -max_force 93.170 -optimal_fiber_length 0.06220 -tendon_slack_length 0.22850 -pennation_angle 3.500 -activation 1.000 -visible yes -wrapobject ecu_r -endmuscle - -beginmuscle flex_carpi_rad_r -beginpoints - 0.00758 -0.27806 -0.03705 segment humerus_r - 0.02110 -0.21943 0.00127 segment radius_r - 0.00101 -0.00896 -0.00518 segment index_metacarpal_r -endpoints -begingroups right forearm_r wrist_flex_r radial_r endgroups -max_force 73.960 -optimal_fiber_length 0.06280 -tendon_slack_length 0.24400 -pennation_angle 3.100 -activation 1.000 -visible yes -wrapobject elbow_pt_ecrl_r -wrapobject fcr_r -endmuscle - -beginmuscle flex_carpi_ulna_r -beginpoints - 0.00219 -0.27740 -0.03880 segment humerus_r - 0.00549 0.00000 0.00000 segment FCUpt_r - 0.01082 -0.22327 0.00969 segment radius_r --0.00258 -0.01017 -0.00260 segment pinky_metacarpal_r -endpoints -begingroups right forearm_r wrist_flex_r ulnar_r endgroups -max_force 128.930 -optimal_fiber_length 0.05090 -tendon_slack_length 0.26500 -pennation_angle 12.100 -activation 1.000 -visible yes -wrapobject fcu_r -wrapobject elbow_pt_ecrl_r -endmuscle - -beginmuscle palmaris_longus_r -beginpoints - 0.00457 -0.27519 -0.03865 segment humerus_r - 0.02531 -0.23915 -0.00276 segment radius_r - 0.00917 -0.01898 -0.01754 segment wrist_distal_r - 0.00186 -0.02325 0.00020 segment middle_pre_r -endpoints -begingroups right forearm_r wrist_flex_r hand_r radial_r endgroups -max_force 26.720 -optimal_fiber_length 0.06380 -tendon_slack_length 0.26940 -pennation_angle 4.000 -activation 1.000 -visible yes -wrapobject pl_r -wrapobject elbow_pt_ecrl_r -endmuscle - -beginmuscle flex_dig_sup_lat_r -beginpoints - 0.00421 -0.27598 -0.03864 segment humerus_r - 0.01377 -0.18718 0.00205 segment radius_r --0.00235 -0.01393 -0.01376 segment wrist_distal_r --0.00476 -0.02294 -0.01020 segment ring_metacarpal_r - 0.00028 -0.04687 -0.00634 segment pinky_metacarpal_r - 0.00158 -0.03659 -0.00216 segment pinky_prox_r --0.00049 -0.02071 -0.00305 segment pinky_mid_r -endpoints -begingroups right forearm_r wrist_flex_r ulnar_r hand_r endgroups -max_force 16.550 -optimal_fiber_length 0.05150 -tendon_slack_length 0.3383 -pennation_angle 5.000 -activation 1.000 -visible yes -wrapobject fds_r -wrapobject elbow_pt_ecrl_r -endmuscle - -beginmuscle flex_dig_sup_ring_r -beginpoints - 0.00479 -0.27880 -0.03731 segment humerus_r - 0.01571 -0.18666 0.00267 segment radius_r --0.00082 -0.01344 -0.01359 segment wrist_distal_r --0.00910 -0.02470 -0.00852 segment middle_pre_r --0.00186 -0.05480 -0.00747 segment ring_metacarpal_r - 0.00051 -0.03981 -0.00306 segment ring_prox_r - 0.00087 -0.02561 -0.00273 segment ring_mid_r -endpoints -begingroups right forearm_r wrist_flex_r ulnar_r hand_r endgroups -max_force 57.900 -optimal_fiber_length 0.07360 -tendon_slack_length 0.328 -pennation_angle 4.000 -activation 1.000 -visible yes -wrapobject fds_r -wrapobject elbow_pt_ecrl_r -endmuscle - -beginmuscle flex_dig_sup_mid_r -beginpoints --0.00669 -0.02713 -0.00191 segment ulna_r - 0.01899 -0.18706 0.00424 segment radius_r - 0.00680 -0.01377 -0.01399 segment wrist_distal_r - 0.00134 -0.02413 -0.00707 segment middle_pre_r - 0.00213 -0.05608 -0.00379 segment middle_pre_r - 0.00147 -0.04328 -0.00205 segment middle_prox_r - 0.00074 -0.02717 -0.00333 segment middle_mid_r -endpoints -begingroups right forearm_r wrist_flex_r radial_r hand_r endgroups -max_force 91.030 -optimal_fiber_length 0.07490 -tendon_slack_length 0.295 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fds_r -endmuscle - -beginmuscle flex_dig_sup_ind_r -beginpoints --0.00676 -0.02819 -0.00137 segment ulna_r - 0.02145 -0.18623 0.00552 segment radius_r - 0.00889 -0.01366 -0.01387 segment wrist_distal_r --0.00243 -0.02854 -0.00368 segment index_metacarpal_r --0.00199 -0.05267 -0.00823 segment index_metacarpal_r - 0.00035 -0.03429 -0.00029 segment index_prox_r - 0.00031 -0.02084 -0.00328 segment index_mid_r -endpoints -begingroups right forearm_r wrist_flex_r radial_r hand_r endgroups -max_force 61.240 -optimal_fiber_length 0.08350 -tendon_slack_length 0.27500 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject fds_r -endmuscle - -beginmuscle flex_dig_prof_lat_r -beginpoints --0.00628 -0.03214 0.00254 segment ulna_r - 0.01409 -0.18516 0.00861 segment radius_r --0.00088 -0.01027 -0.01188 segment wrist_distal_r --0.00779 -0.02305 -0.00375 segment ring_metacarpal_r --0.00120 -0.04620 -0.00546 segment pinky_metacarpal_r --0.00060 -0.03590 -0.00353 segment pinky_prox_r --0.00034 -0.00485 -0.00163 segment pinky_distal_r -endpoints -begingroups right forearm_r wrist_flex_r ulnar_r hand_r endgroups -max_force 79.650 -optimal_fiber_length 0.07490 -tendon_slack_length 0.28190 -pennation_angle 8.000 -activation 1.000 -visible yes -wrapobject fdp_r -endmuscle - -beginmuscle flex_dig_prof_ring_r -beginpoints --0.00502 -0.03374 0.00273 segment ulna_r - 0.01580 -0.18629 0.00803 segment radius_r - 0.00106 -0.01177 -0.01184 segment wrist_distal_r --0.01037 -0.02577 -0.00583 segment middle_pre_r --0.00164 -0.05450 -0.00589 segment ring_metacarpal_r --0.00072 -0.04045 -0.00263 segment ring_prox_r - 0.00021 -0.00738 -0.00232 segment ring_distal_r -endpoints -begingroups right forearm_r wrist_flex_r ulnar_r hand_r endgroups -max_force 64.080 -optimal_fiber_length 0.07980 -tendon_slack_length 0.28200 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fdp_r -endmuscle - -beginmuscle flex_dig_prof_mid_r -beginpoints --0.00522 -0.03327 0.00210 segment ulna_r - 0.01906 -0.18644 0.00785 segment radius_r - 0.00335 -0.01280 -0.01144 segment wrist_distal_r --0.00123 -0.02454 -0.00598 segment middle_pre_r --0.00014 -0.05458 -0.00480 segment middle_pre_r --0.00003 -0.04332 -0.00330 segment middle_prox_r --0.00028 -0.00366 -0.00352 segment middle_distal_r -endpoints -begingroups right forearm_r wrist_flex_r ulnar_r hand_r endgroups -max_force 81.650 -optimal_fiber_length 0.08350 -tendon_slack_length 0.29300 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject fdp_r -endmuscle - -beginmuscle flex_dig_prof_ind_r -beginpoints --0.00647 -0.03176 0.00280 segment ulna_r - 0.02080 -0.18560 0.00915 segment radius_r - 0.00896 -0.01168 -0.01212 segment wrist_distal_r --0.00240 -0.02200 -0.00051 segment index_metacarpal_r --0.00031 -0.05517 -0.00787 segment index_metacarpal_r - 0.00192 -0.03615 -0.00166 segment index_prox_r - 0.00070 -0.00422 -0.00390 segment index_distal_r -endpoints -begingroups right forearm_r wrist_flex_r radial_r hand_r endgroups -max_force 68.270 -optimal_fiber_length 0.07490 -tendon_slack_length 0.29350 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fdp_r -endmuscle - -beginmuscle ext_dig_lat_r -beginpoints --0.00040 -0.28831 0.01870 segment humerus_r - 0.00130 -0.03888 0.01405 segment EDCpt_r - 0.00993 -0.22304 0.03618 segment radius_r --0.00114 -0.04658 0.00643 segment pinky_metacarpal_r --0.00175 -0.03423 0.00191 segment pinky_prox_r --0.00150 -0.00018 0.00169 segment pinky_distal_r -endpoints -begingroups right forearm_r wrist_ext_r ulnar_r hand_r endgroups -max_force 13.110 -optimal_fiber_length 0.06500 -tendon_slack_length 0.29650 -pennation_angle 2.000 -activation 1.000 -visible yes -wrapobject edcl_r range 3 4 -endmuscle - -beginmuscle ext_dig_ring_r -beginpoints --0.00156 -0.28936 0.01782 segment humerus_r - 0.00260 -0.03991 0.01552 segment EDCpt_r - 0.00882 -0.22366 0.04284 segment radius_r --0.00047 -0.05534 0.00526 segment ring_metacarpal_r --0.00248 -0.03947 0.00260 segment ring_prox_r --0.00135 -0.00546 0.00151 segment ring_distal_r -endpoints -begingroups right forearm_r wrist_ext_r ulnar_r hand_r endgroups -max_force 34.040 -optimal_fiber_length 0.06260 -tendon_slack_length 0.32700 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edcr_r -endmuscle - -beginmuscle ext_dig_mid_r -beginpoints - 0.00051 -0.28984 0.01949 segment humerus_r - 0.00316 -0.03948 0.01528 segment EDCpt_r - 0.01440 -0.22323 0.04223 segment radius_r - 0.00081 -0.05603 0.00578 segment middle_pre_r - 0.00074 -0.04351 0.00385 segment middle_prox_r - 0.00021 -0.01047 -0.00060 segment middle_distal_r -endpoints -begingroups right forearm_r wrist_ext_r radial_r hand_r endgroups -max_force 35.320 -optimal_fiber_length 0.07240 -tendon_slack_length 0.33500 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edcm_r -endmuscle - -beginmuscle ext_dig_ind_r -beginpoints - 0.00065 -0.28898 0.01869 segment humerus_r - 0.00228 -0.03918 0.01483 segment EDCpt_r - 0.01533 -0.22314 0.04196 segment radius_r --0.00103 -0.06275 0.01025 segment index_metacarpal_r --0.00068 -0.01251 0.00847 segment index_prox_r - 0.00107 -0.04412 0.00830 segment index_prox_r --0.00067 -0.01026 0.00427 segment index_mid_r - 0.00003 -0.02014 0.00079 segment index_mid_r --0.00041 -0.00411 0.00135 segment index_distal_r -endpoints -begingroups right forearm_r wrist_ext_r radial_r hand_r endgroups -max_force 18.260 -optimal_fiber_length 0.07000 -tendon_slack_length 0.32200 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edci_r range 3 4 -wrapobject secondpm_r range 6 7 -wrapobject secondmd_r range 8 9 -endmuscle - -beginmuscle ext_dig_min_r -beginpoints - 0.00089 -0.28919 0.01847 segment humerus_r --0.00977 -0.03907 0.03085 segment ulna_r --0.00755 -0.08267 0.03646 segment ulna_r - 0.00454 -0.22641 0.03678 segment radius_r --0.00120 -0.00255 0.00836 segment pinky_metacarpal_r --0.00213 -0.04605 0.00531 segment pinky_metacarpal_r - 0.00010 -0.03330 0.00245 segment pinky_prox_r --0.00180 -0.00427 0.00110 segment pinky_distal_r -endpoints -begingroups right forearm_r wrist_ext_r ulnar_r hand_r endgroups -max_force 25.250 -optimal_fiber_length 0.06750 -tendon_slack_length 0.32200 -pennation_angle 3.000 -activation 1.000 -visible yes -wrapobject edm_r range 4 5 -endmuscle - -beginmuscle ext_indicis_r -beginpoints --0.00394 -0.16652 0.03682 segment ulna_r - 0.00171 -0.17469 0.04049 segment ulna_r - 0.01076 -0.22783 0.03664 segment radius_r --0.00958 -0.01376 0.00864 segment index_metacarpal_r --0.00128 -0.06217 0.01019 segment index_metacarpal_r --0.00068 -0.01171 0.00782 segment index_prox_r - 0.00055 -0.04350 0.00843 segment index_prox_r --0.00073 -0.00986 0.00448 segment index_mid_r - 0.00025 -0.02103 0.00085 segment index_mid_r - 0.00099 -0.01294 0.00063 segment index_distal_r -endpoints -begingroups right forearm_r wrist_ext_r radial_r hand_r endgroups -max_force 21.700 -optimal_fiber_length 0.05890 -tendon_slack_length 0.18600 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject ext_ellipse_r -wrapobject secondpm_r range 7 8 -wrapobject secondmd_r range 9 10 -endmuscle - -beginmuscle ext_poll_long_r -beginpoints --0.01412 -0.09706 0.02952 segment ulna_r - 0.00124 -0.13668 0.02270 segment radius_r - 0.02702 -0.21872 0.03828 segment radius_r - 0.03382 -0.22754 0.04607 segment radius_r - 0.04234 -0.23771 0.04173 segment radius_r - 0.02428 0.00014 0.00608 segment wrist_distal_r --0.00885 -0.00409 0.00805 segment trapezium_r --0.00598 -0.00312 0.00631 segment thumb_prox_r --0.00075 -0.03084 0.00820 segment thumb_prox_r - 0.00102 -0.00713 0.00893 segment thumb_mid_r - 0.00216 -0.03367 0.00448 segment thumb_mid_r - 0.00187 -0.00930 0.00012 segment thumb_distal_r -endpoints -begingroups right forearm_r wrist_ext_r radial_r hand_r endgroups -max_force 39.460 -optimal_fiber_length 0.05400 -tendon_slack_length 0.22050 -pennation_angle 6.000 -activation 1.000 -visible yes -wrapobject mpthumb_r -endmuscle - -beginmuscle ext_poll_brev_r -beginpoints - 0.01924 -0.14695 0.02114 segment radius_r - 0.02276 -0.15857 0.02665 segment radius_r - 0.03831 -0.19722 0.03834 segment radius_r - 0.05235 -0.23611 0.02699 segment radius_r - 0.03123 0.00256 -0.00327 segment wrist_distal_r - 0.00525 -0.00143 0.00518 segment thumb_prox_r - 0.00502 -0.03200 0.00628 segment thumb_prox_r - 0.00546 -0.01080 0.00354 segment thumb_mid_r -endpoints -begingroups right forearm_r radial_r wrist_flex_r hand_r endgroups -max_force 14.200 -optimal_fiber_length 0.06750 -tendon_slack_length 0.11550 -pennation_angle 7.200 -activation 1.000 -visible yes -wrapobject mpthumb_r -endmuscle - -beginmuscle flex_poll_long_r -beginpoints - 0.00971 -0.09458 0.01769 segment radius_r - 0.01658 -0.13525 0.01915 segment radius_r - 0.01869 -0.17932 0.01988 segment radius_r - 0.03800 -0.23171 0.01580 segment radius_r - 0.01869 0.00882 -0.01547 segment FPLpt_r - 0.01468 -0.01915 -0.02110 segment FPLpt2_r - 0.00054 -0.01332 -0.01504 segment thumb_prox_r --0.00074 -0.02257 -0.01008 segment thumb_prox_r --0.00366 -0.01491 -0.01195 segment thumb_mid_r - 0.00019 -0.02782 -0.00735 segment thumb_mid_r --0.00116 -0.01087 -0.00372 segment thumb_distal_r -endpoints -begingroups right forearm_r wrist_flex_r radial_r hand_r endgroups -max_force 77.200 -optimal_fiber_length 0.05520 -tendon_slack_length 0.19380 -pennation_angle 7.000 -activation 1.000 -visible yes -wrapobject fpl_r -wrapobject ipthumb_r -wrapobject mpthumb_r -endmuscle - -beginmuscle abd_poll_long_r -beginpoints - 0.01154 -0.09477 0.01681 segment radius_r - 0.03030 -0.13970 0.03507 segment radius_r - 0.03993 -0.16072 0.03400 segment radius_r - 0.04397 -0.17845 0.03134 segment radius_r - 0.05505 -0.23084 0.02368 segment radius_r - 0.03385 0.00634 -0.00745 segment APLpt_r - 0.01095 0.00781 0.00408 segment thumb_prox_r - 0.00933 -0.00151 0.00306 segment thumb_prox_r - 0.00561 -0.00996 0.00193 segment thumb_prox_r -endpoints -begingroups right forearm_r radial_r wrist_flex_r hand_r endgroups -max_force 59.530 -optimal_fiber_length 0.07130 -tendon_slack_length 0.12950 -pennation_angle 7.500 -activation 1.000 -visible yes -wrapobject apl_r range 5 6 -endmuscle - -#endif /* RIGHT_HAND_FULL */ diff --git a/OpenSim/Utilities/simmToOpenSim/test/dynamic.jnt b/OpenSim/Utilities/simmToOpenSim/test/dynamic.jnt deleted file mode 100644 index d5769bca24..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/dynamic.jnt +++ /dev/null @@ -1,2644 +0,0 @@ -/******************************************************************************* - DYNAMIC MOCAP MODEL - joint file - - (c) Copyright 2003-4, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the segments joints, gencoords, and constraint functions - for the dynamic SIMM model of an average adult male. It is based on the full-body - model that was compiled from several sources, including: - - Scott Delp, Ph.D., Stanford University - Wendy Murray, Ph.D., Stanford University - Silvia Salinas Blemker, M.S., Stanford University - Anita Vasavada, Ph.D., Washington State University - Srikanth Suryanarayanan, M.S., Rehabilitation Institute of Chicago - Frans van der Helm, Ph.D., Delft University - -*******************************************************************************/ - -name Dynamic Mocap Model - -bone_path bones -muscle_file dynamic.msl -marker_visibility off -marker_radius 0.005000 -force_units N -length_units m -MV_gear 0.080000 -solver_accuracy 0.000100 - - -#ifndef LOWER_EXTREMITY - #define LOWER_EXTREMITY 1 -#endif - -#ifndef UPPER_EXTREMITY - #define UPPER_EXTREMITY 1 -#endif - -#ifndef GROUND_PLANE_DEFINED - #define GROUND_PLANE_XY 1 -#endif - - -#if GROUND_PLANE_XY - #define SHADOW_PARAMS shadow Z 0.001 - gravity -Z -#elif GROUND_PLANE_XZ - #define SHADOW_PARAMS shadow Y 0.001 - gravity -Y -#else - #define SHADOW_PARAMS shadow X 0.001 - gravity -X -#endif - - -/* To enhance the knee model to include varus/valgus motion, - * set VARUS_VALGUS to 1. - */ -#define VARUS_VALGUS 0 - - -/* Because the shoulder and wrist models have many degrees - * of freedom, it is often difficult to control with only - * one marker at the shoulder, one at the elbow, and one at - * the wrist. To reduce this problem for gait analysis, some - * of the degrees of freedom in the arms are restricted to - * small ranges of motion. These ranges are suitable for most - * gait patterns, but may not be appropriate for motions - * involving more movement of the arms, such as dancing. - * To remove these restrictions and allow greater movement - * of the arms, set FULL_ARM_MOTION to 1. For best results - * when doing this, you should also add more arm markers to - * the model. - */ -#define FULL_ARM_MOTION 0 - - -/* These are the weights of the various markers in the - * mocap model. Solver uses them to determine which markers - * it should try harder to fit to the marker cloud data. - * 1.0 is the default, and numbers greater than 1.0 tell - * Solver to try harder for those markers. - */ -#define FOOT_WEIGHT 2.0 -#define PELVIS_WEIGHT 2.0 -#define WRIST_WEIGHT 2.0 -#define ARM_WEIGHT 2.0 -#define OTHER_WEIGHT 1.0 - - -/* The following distribution of mass parameters was taken from: - * de Leva, P., "Adjustments to Zatsiorsky-Seluyanov's Segment - * Inertia Parameters," Journal of Biomechanics, Vol. 29, No. 9, - * pp. 1223-1230, 1996. - * Lafond, D., Prince, F., "Extended Adjustements of Zatsiorsky- - * Seluyanov's Mass Inertia Parameters," submitted for publication, - * 2003. - * The following changes were made: - * (1) the torso segment in the SIMM model is equivalent to the - * upper trunk and the middle trunk in the Zatsirosky-Seluyanov - * model. The parallel axis theorem was used to determine the - * inertial parameters for these two sections combined. - * (2) the foot in the Zatsirosky-Seluyanov was divided into the - * foot segment and the toes segment in the SIMM model, using - * the parallel axis theorm, and the assumption that the toes - * weigh 100 grams in the average male. - * (3) the mass of the patella in the average male was estimated to be - * 100 grams, and was taken out of the mass of the thigh. The - * inertial properties of the thigh were not modified. - * (4) the mass and inertial properties of the lower arm were split - * equally between the radius and ulna. - * (5) the mass of the talus in the average male was estimated to be - * 50 grams, and was taken out of the mass of the foot. The inertial - * properties of the foot were not modified. - */ -#define BODY_MASS 75.0 -#define TORSO_MASS {0.3229 * BODY_MASS} -#define PELVIS_MASS {0.1117 * BODY_MASS} -#define HEAD_MASS {0.0694 * BODY_MASS} -#define UPPER_ARM_MASS {0.0271 * BODY_MASS} -#define LOWER_ARM_MASS {0.0162 * BODY_MASS} -#define ULNA_MASS {0.5 * LOWER_ARM_MASS} -#define RADIUS_MASS {0.5 * LOWER_ARM_MASS} -#define HAND_MASS {0.0061 * BODY_MASS} -#define THIGH_MASS {0.1403 * BODY_MASS} -#define PATELLA_MASS {0.0013 * BODY_MASS} -#define SHANK_MASS {0.0433 * BODY_MASS} -#define FOOT_MASS {0.0118 * BODY_MASS} -#define TALUS_MASS {0.0006 * BODY_MASS} -#define TOES_MASS {0.0013 * BODY_MASS} -#define HAT_MASS {0.4911 * BODY_MASS} - -#define PELVIS_DEPTH 0.184400 /* mocap scaling: sacral marker to mid point of ASIS markers */ -#define PELVIS_HEIGHT 0.076200 /* mocap scaling: Y distance from hip center to mid point of ASIS markers */ -#define PELVIS_WIDTH 0.256000 /* mocap scaling: distance between ASIS markers */ -#define HIP_Y_PELVIS -0.0824 /* Y coordinate of hip center in pelvis frame */ -#define NAVEL_Y_PELVIS 0.0633 /* Y coordinate of navel in pelvis frame */ -#define PELVIS_CM_X -0.0893 /* estimated */ -#define PELVIS_CM_Y {NAVEL_Y_PELVIS - 0.6115 * (NAVEL_Y_PELVIS - HIP_Y_PELVIS)} -#define PELVIS_CM_Z 0.0 -#define PELVIS_GYR_X {0.551 * (NAVEL_Y_PELVIS - HIP_Y_PELVIS)} -#define PELVIS_GYR_Y {0.587 * (NAVEL_Y_PELVIS - HIP_Y_PELVIS)} -#define PELVIS_GYR_Z {0.615 * (NAVEL_Y_PELVIS - HIP_Y_PELVIS)} -#define PELVIS_INERTIA_X {PELVIS_MASS * PELVIS_GYR_X * PELVIS_GYR_X} -#define PELVIS_INERTIA_Y {PELVIS_MASS * PELVIS_GYR_Y * PELVIS_GYR_Y} -#define PELVIS_INERTIA_Z {PELVIS_MASS * PELVIS_GYR_Z * PELVIS_GYR_Z} - -#define TORSO_HEIGHT 0.5600 /* mocap scaling: mid point of ASIS markers to mid point of shoulder markers */ -#define TORSO_WIDTH 0.3632 /* mocap scaling: distance between shoulder markers */ -#define CERVICAL_Y_TORSO 0.4891 /* Y coordinate of C7 in torso frame */ -#define NAVEL_Y_TORSO 0.0302 /* Y coordinate of navel in torso frame */ -#define TORSO_CM_X 0.0145 /* estimated */ -#define TORSO_CM_Y {CERVICAL_Y_TORSO - 0.507 * (CERVICAL_Y_TORSO - NAVEL_Y_TORSO)} -#define TORSO_CM_Z 0.0 -#define TORSO_GYR_X {0.294 * (CERVICAL_Y_TORSO - NAVEL_Y_TORSO)} -#define TORSO_GYR_Y {0.233 * (CERVICAL_Y_TORSO - NAVEL_Y_TORSO)} -#define TORSO_GYR_Z {0.342 * (CERVICAL_Y_TORSO - NAVEL_Y_TORSO)} -#define TORSO_INERTIA_X {TORSO_MASS * TORSO_GYR_X * TORSO_GYR_X} -#define TORSO_INERTIA_Y {TORSO_MASS * TORSO_GYR_Y * TORSO_GYR_Y} -#define TORSO_INERTIA_Z {TORSO_MASS * TORSO_GYR_Z * TORSO_GYR_Z} - -#define HEAD_LENGTH 0.1890 /* mocap scaling: distance from rear head marker to front head marker */ -#define TOPHEAD_Y_HEAD 0.2830 /* Y coordinate of top of head in head frame */ -#define CERVICAL_Y_HEAD 0.0401 /* Y coordinate of C7 in head frame */ -#define HEAD_CM_X 0.0288 /* estimated */ -#define HEAD_CM_Y {TOPHEAD_Y_HEAD - 0.5002 * (TOPHEAD_Y_HEAD - CERVICAL_Y_HEAD)} -#define HEAD_CM_Z 0.0 -#define HEAD_GYR_X {0.315 * (TOPHEAD_Y_HEAD - CERVICAL_Y_HEAD)} -#define HEAD_GYR_Y {0.261 * (TOPHEAD_Y_HEAD - CERVICAL_Y_HEAD)} -#define HEAD_GYR_Z {0.303 * (TOPHEAD_Y_HEAD - CERVICAL_Y_HEAD)} -#define HEAD_INERTIA_X {HEAD_MASS * HEAD_GYR_X * HEAD_GYR_X} -#define HEAD_INERTIA_Y {HEAD_MASS * HEAD_GYR_Y * HEAD_GYR_Y} -#define HEAD_INERTIA_Z {HEAD_MASS * HEAD_GYR_Z * HEAD_GYR_Z} - -#define THIGH_LENGTH 0.3960 /* hip joint center to knee joint "center" */ -#define THIGH_CM_X 0.0 -#define THIGH_CM_Y {-0.4095 * THIGH_LENGTH} -#define THIGH_CM_Z 0.0 -#define THIGH_GYR_X {0.329 * THIGH_LENGTH} -#define THIGH_GYR_Y {0.149 * THIGH_LENGTH} -#define THIGH_GYR_Z {0.329 * THIGH_LENGTH} -#define THIGH_INERTIA_X {THIGH_MASS * THIGH_GYR_X * THIGH_GYR_X} -#define THIGH_INERTIA_Y {THIGH_MASS * THIGH_GYR_Y * THIGH_GYR_Y} -#define THIGH_INERTIA_Z {THIGH_MASS * THIGH_GYR_Z * THIGH_GYR_Z} - -#define PATELLA_CM_X 0.0018 /* estimated */ -#define PATELLA_CM_Y 0.0264 /* estimated */ -#define PATELLA_CM_Z 0.0 /* estimated */ -#define PAT_HEIGHT 0.025 /* estimated */ -#define PAT_RADIUS 0.020 /* estimated */ -#define PATELLA_INERTIA_X {PATELLA_MASS * PAT_RADIUS * PAT_RADIUS * 0.0833} -#define PATELLA_INERTIA_Y {PATELLA_MASS * ((PAT_RADIUS * PAT_RADIUS * 0.25) + (PAT_HEIGHT * PAT_HEIGHT * 0.08333))} -#define PATELLA_INERTIA_Z {PATELLA_MASS * ((PAT_RADIUS * PAT_RADIUS * 0.25) + (PAT_HEIGHT * PAT_HEIGHT * 0.08333))} - -#define SHANK_LENGTH 0.3907 /* knee joint "center" to ankle joint center */ -#define SHANK_CM_X 0.0 -#define SHANK_CM_Y {-0.4395 * SHANK_LENGTH} -#define SHANK_CM_Z 0.0 -#define SHANK_GYR_X {0.246 * SHANK_LENGTH} -#define SHANK_GYR_Y {0.102 * SHANK_LENGTH} -#define SHANK_GYR_Z {0.251 * SHANK_LENGTH} -#define SHANK_INERTIA_X {SHANK_MASS * SHANK_GYR_X * SHANK_GYR_X} -#define SHANK_INERTIA_Y {SHANK_MASS * SHANK_GYR_Y * SHANK_GYR_Y} -#define SHANK_INERTIA_Z {SHANK_MASS * SHANK_GYR_Z * SHANK_GYR_Z} - -#define TALUS_CM_X 0.0055 /* estimated */ -#define TALUS_CM_Y 0.0023 /* estimated */ -#define TALUS_CM_Z 0.0 -#define TAL_HEIGHT 0.016 -#define TAL_RADIUS 0.015 -#define TALUS_INERTIA_X {TALUS_MASS * ((TAL_RADIUS * TAL_RADIUS * 0.25) + (TAL_HEIGHT * TAL_HEIGHT * 0.08333))} -#define TALUS_INERTIA_Y {TALUS_MASS * TAL_RADIUS * TAL_RADIUS * 0.0833} -#define TALUS_INERTIA_Z {TALUS_MASS * ((TAL_RADIUS * TAL_RADIUS * 0.25) + (TAL_HEIGHT * TAL_HEIGHT * 0.08333))} - -#define FOOT_LENGTH 0.2670 /* mocap scaling: heel marker to toe marker */ -#define HEEL_X_FOOT -0.0080 /* X coordinate of back of heel in foot frame */ -#define TOES_START_X_FOOT 0.18865 /* X coordinate of start of toes segment */ -#define FOOT_CM_X {0.518 * (TOES_START_X_FOOT - HEEL_X_FOOT)} -#define FOOT_CM_Y 0.0156 /* estimated */ -#define FOOT_CM_Z 0.0 -#define FOOT_GYR_X {0.163 * (TOES_START_X_FOOT - (HEEL_X_FOOT))} -#define FOOT_GYR_Y {0.260 * (TOES_START_X_FOOT - (HEEL_X_FOOT))} -#define FOOT_GYR_Z {0.280 * (TOES_START_X_FOOT - (HEEL_X_FOOT))} -#define FOOT_INERTIA_X {FOOT_MASS * FOOT_GYR_X * FOOT_GYR_X} -#define FOOT_INERTIA_Y {FOOT_MASS * FOOT_GYR_Y * FOOT_GYR_Y} -#define FOOT_INERTIA_Z {FOOT_MASS * FOOT_GYR_Z * FOOT_GYR_Z} - -#define TOES_LENGTH 0.0614 /* estimated */ -#define TOES_CM_X {0.5000 * TOES_LENGTH} -#define TOES_CM_Y -0.0026 /* estimated */ -#define TOES_CM_Z 0.0105 /* estimated */ -#define TOES_GYR_X {0.521 * TOES_LENGTH} -#define TOES_GYR_Y {0.833 * TOES_LENGTH} -#define TOES_GYR_Z {0.894 * TOES_LENGTH} -#define TOES_INERTIA_X {TOES_MASS * TOES_GYR_X * TOES_GYR_X} -#define TOES_INERTIA_Y {TOES_MASS * TOES_GYR_Y * TOES_GYR_Y} -#define TOES_INERTIA_Z {TOES_MASS * TOES_GYR_Z * TOES_GYR_Z} - -#define UPPER_ARM_LENGTH 0.2850 /* shoulder joint center to elbow joint center */ -#define UPPER_ARM_CM_X 0.0 -#define UPPER_ARM_CM_Y {-0.5772 * UPPER_ARM_LENGTH} -#define UPPER_ARM_CM_Z 0.0 -#define UPPER_ARM_GYR_X {0.269 * UPPER_ARM_LENGTH} -#define UPPER_ARM_GYR_Y {0.158 * UPPER_ARM_LENGTH} -#define UPPER_ARM_GYR_Z {0.285 * UPPER_ARM_LENGTH} -#define UPPER_ARM_INERTIA_X {UPPER_ARM_MASS * UPPER_ARM_GYR_X * UPPER_ARM_GYR_X} -#define UPPER_ARM_INERTIA_Y {UPPER_ARM_MASS * UPPER_ARM_GYR_Y * UPPER_ARM_GYR_Y} -#define UPPER_ARM_INERTIA_Z {UPPER_ARM_MASS * UPPER_ARM_GYR_Z * UPPER_ARM_GYR_Z} - -#define LOWER_ARM_LENGTH 0.2635 /* elbow joint center to wrist joint center */ -#define LOWER_ARM_CM_X 0.0 -#define LOWER_ARM_CM_Y {-0.4574 * LOWER_ARM_LENGTH} -#define LOWER_ARM_CM_Z 0.0 -#define LOWER_ARM_GYR_X {0.265 * LOWER_ARM_LENGTH} -#define LOWER_ARM_GYR_Y {0.121 * LOWER_ARM_LENGTH} -#define LOWER_ARM_GYR_Z {0.276 * LOWER_ARM_LENGTH} -#define RADIUS_INERTIA_X {RADIUS_MASS * LOWER_ARM_GYR_X * LOWER_ARM_GYR_X} -#define RADIUS_INERTIA_Y {RADIUS_MASS * LOWER_ARM_GYR_Y * LOWER_ARM_GYR_Y} -#define RADIUS_INERTIA_Z {RADIUS_MASS * LOWER_ARM_GYR_Z * LOWER_ARM_GYR_Z} -#define ULNA_INERTIA_X {ULNA_MASS * LOWER_ARM_GYR_X * LOWER_ARM_GYR_X} -#define ULNA_INERTIA_Y {ULNA_MASS * LOWER_ARM_GYR_Y * LOWER_ARM_GYR_Y} -#define ULNA_INERTIA_Z {ULNA_MASS * LOWER_ARM_GYR_Z * LOWER_ARM_GYR_Z} - -#define HAND_LENGTH 0.1879 /* wrist joint center to distal middle finger marker */ -#define HAND_CM_X 0.0 -#define HAND_CM_Y {-0.3624 * HAND_LENGTH} -#define HAND_CM_Z 0.0 -#define HAND_GYR_X {0.235 * HAND_LENGTH} -#define HAND_GYR_Y {0.184 * HAND_LENGTH} -#define HAND_GYR_Z {0.288 * HAND_LENGTH} -#define HAND_INERTIA_X {HAND_MASS * HAND_GYR_X * HAND_GYR_X} -#define HAND_INERTIA_Y {HAND_MASS * HAND_GYR_Y * HAND_GYR_Y} -#define HAND_INERTIA_Z {HAND_MASS * HAND_GYR_Z * HAND_GYR_Z} - -#define HAT_HEIGHT 0.5600 /* mocap scaling: mid point of ASIS markers to mid point of shoulder markers */ -#define HAT_WIDTH 0.3632 /* mocap scaling: distance between shoulder markers */ -#define CERVICAL_Y_HAT 0.4891 /* Y coordinate of C7 in HAT frame */ -#define NAVEL_Y_HAT 0.0302 /* Y coordinate of navel in HAT frame */ -#define HAT_CM_X 0.0145 /* estimated */ -#define HAT_CM_Y {CERVICAL_Y_HAT - 0.507 * (CERVICAL_Y_HAT - NAVEL_Y_HAT)} -#define HAT_CM_Z 0.0 -#define HAT_GYR_X {0.294 * (CERVICAL_Y_HAT - NAVEL_Y_HAT)} -#define HAT_GYR_Y {0.233 * (CERVICAL_Y_HAT - NAVEL_Y_HAT)} -#define HAT_GYR_Z {0.342 * (CERVICAL_Y_HAT - NAVEL_Y_HAT)} -#define HAT_INERTIA_X {HAT_MASS * HAT_GYR_X * HAT_GYR_X} -#define HAT_INERTIA_Y {HAT_MASS * HAT_GYR_Y * HAT_GYR_Y} -#define HAT_INERTIA_Z {HAT_MASS * HAT_GYR_Z * HAT_GYR_Z} - - -/*********************************************/ -/* SEGMENTS */ -/*********************************************/ - -beginsegment ground -endsegment - -beginsegment pelvis -bone pelvis_rv.asc -bone pelvis_lv.asc -bone sacrumv.asc -material my_bone -SHADOW_PARAMS -begingroups spine torso right leg_r hip_r left leg_l hip_l endgroups -marker V.Sacral -0.17 0.0 0.0 PELVIS_WEIGHT -marker Sacral -0.17 0.0 0.0 PELVIS_WEIGHT -marker V.Sacrum -0.17 0.0 0.0 PELVIS_WEIGHT -marker Sacrum -0.17 0.0 0.0 PELVIS_WEIGHT -marker SACR -0.17 0.0 0.0 PELVIS_WEIGHT -marker VSAC -0.17 0.0 0.0 PELVIS_WEIGHT -marker R.ASIS 0.014 0.0115 0.128 PELVIS_WEIGHT -marker RASIS 0.014 0.0115 0.128 PELVIS_WEIGHT -marker RASI 0.014 0.0115 0.128 PELVIS_WEIGHT -marker R.PSIS -0.145 0.04 0.055 PELVIS_WEIGHT -marker RPSIS -0.145 0.04 0.055 PELVIS_WEIGHT -marker RPSI -0.145 0.04 0.055 PELVIS_WEIGHT -marker R.Trochanter -0.01 -0.02 0.08 PELVIS_WEIGHT -marker L.ASIS 0.014 0.0115 -0.128 PELVIS_WEIGHT -marker LASIS 0.014 0.0115 -0.128 PELVIS_WEIGHT -marker LASI 0.014 0.0115 -0.128 PELVIS_WEIGHT -marker L.PSIS -0.145 0.04 -0.055 PELVIS_WEIGHT -marker LPSIS -0.145 0.04 -0.055 PELVIS_WEIGHT -marker LPSI -0.145 0.04 -0.055 PELVIS_WEIGHT -marker L.Trochanter -0.01 -0.02 -0.08 PELVIS_WEIGHT -gait_scale PELVIS PELVIS_DEPTH PELVIS_HEIGHT PELVIS_WIDTH -mass PELVIS_MASS -masscenter PELVIS_CM_X PELVIS_CM_Y PELVIS_CM_Z -inertia PELVIS_INERTIA_X 0.0 0.0 - 0.0 PELVIS_INERTIA_Y 0.0 - 0.0 0.0 PELVIS_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 0.0 -1.0 0.0 -htr_y -1.0 0.0 0.0 -endsegment - -#if UPPER_EXTREMITY - -beginsegment torso -bone lumbar1sm.asc -bone lumbar2sm.asc -bone lumbar3sm.asc -bone lumbar4sm.asc -bone lumbar5.asc -bone ribcage_s.asc -bone thoracic1_s.asc -bone thoracic2_s.asc -bone thoracic3_s.asc -bone thoracic4_s.asc -bone thoracic5_s.asc -bone thoracic6_s.asc -bone thoracic7_s.asc -bone thoracic8_s.asc -bone thoracic9_s.asc -bone thoracic10_s.asc -bone thoracic11_s.asc -bone thoracic12_s.asc -bone clavicle_rvsm.asc -bone scapula_rvsm.asc -bone clavicle_lvsm.asc -bone scapula_lvsm.asc -material my_bone -SHADOW_PARAMS -begingroups torso spine endgroups -marker Offset -0.180000 0.220000 0.000000 OTHER_WEIGHT -marker Sternum 0.035 0.410 0.000 OTHER_WEIGHT -marker T10 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker CLAV 0.180000 0.220000 0.000000 OTHER_WEIGHT -marker STRN 0.180000 0.220000 0.000000 OTHER_WEIGHT -marker RBAK 0.180000 0.220000 0.000000 OTHER_WEIGHT -marker R.Scapula -0.120000 -0.100000 -0.080000 OTHER_WEIGHT -marker R.Scapula.Top 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker R.Scapula.Bottom 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker R.Angulus Acromialis -0.085 0.435 0.184 OTHER_WEIGHT -marker R.Trigonum Spinae -0.126 0.400 0.119 OTHER_WEIGHT -marker R.Angulus Inferior -0.135 0.270 0.135 OTHER_WEIGHT -marker C7 Spinous Process -0.080 0.455 0.000 OTHER_WEIGHT -marker L.Scapula -0.120000 -0.100000 0.080000 OTHER_WEIGHT -marker L.Scapula.Top 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker L.Scapula.Bottom 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker L.Angulus Acromialis -0.085 0.435 -0.184 OTHER_WEIGHT -marker L.Trigonum Spinae -0.126 0.400 -0.119 OTHER_WEIGHT -marker L.Angulus Inferior -0.135 0.270 -0.135 OTHER_WEIGHT -gait_scale TORSO 0.560000 0.560000 0.363200 -mass TORSO_MASS -masscenter TORSO_CM_X TORSO_CM_Y TORSO_CM_Z -inertia TORSO_INERTIA_X 0.0 0.0 - 0.0 TORSO_INERTIA_Y 0.0 - 0.0 0.0 TORSO_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment neckhead -bone cerv1sm.asc -bone cerv2sm.asc -bone cerv3sm.asc -bone cerv4sm.asc -bone cerv5sm.asc -bone cerv6sm.asc -bone cerv7.asc -bone skull_s.asc -bone jaw_s.asc -material my_bone -SHADOW_PARAMS -begingroups head spine endgroups -marker C7 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker C7 Spinous Process 0.000000 0.000000 0.000000 OTHER_WEIGHT -gait_scale HEAD HEAD_LENGTH HEAD_LENGTH HEAD_LENGTH -mass HEAD_MASS -masscenter HEAD_CM_X HEAD_CM_Y HEAD_CM_Z -inertia HEAD_INERTIA_X 0.0 0.0 - 0.0 HEAD_INERTIA_Y 0.0 - 0.0 0.0 HEAD_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.2 0.0 -endsegment - -beginsegment OT_head -material my_bone -SHADOW_PARAMS -begingroups head endgroups -marker Rear.Head 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker Head.Rear 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker RearHead 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker HeadRear 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker Front.Head 0.000000 0.190000 0.000000 OTHER_WEIGHT -marker Head.Front 0.000000 0.190000 0.000000 OTHER_WEIGHT -marker FrontHead 0.000000 0.190000 0.000000 OTHER_WEIGHT -marker HeadFront 0.000000 0.190000 0.000000 OTHER_WEIGHT -marker Top.Head -0.060000 0.095000 0.000000 OTHER_WEIGHT -marker Head.Top -0.060000 0.095000 0.000000 OTHER_WEIGHT -marker TopHead -0.060000 0.095000 0.000000 OTHER_WEIGHT -marker HeadTop -0.060000 0.095000 0.000000 OTHER_WEIGHT -marker R.Ear 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker L.Ear 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker RBHD 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker RFHD 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker LBHD 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker LFHD 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker HEDO 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker HEDP 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker HEDA 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker HEDL 0.000000 0.000000 0.000000 OTHER_WEIGHT -gait_scale HEAD HEAD_LENGTH HEAD_LENGTH HEAD_LENGTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.001 0.0 -endsegment - -beginsegment clavicle_marker_r -material my_bone -SHADOW_PARAMS -begingroups right shoulder_r endgroups -marker R.Shoulder 0.000000 0.000000 0.165000 OTHER_WEIGHT -marker RSHO 0.000000 0.000000 0.165000 OTHER_WEIGHT -gait_scale TORSO TORSO_HEIGHT TORSO_HEIGHT TORSO_WIDTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.001 0.0 -endsegment - -beginsegment humerus_r -bone humerus_rv.asc -material my_bone -SHADOW_PARAMS -begingroups right shoulder_r arm_r elbow_r endgroups -marker R.Bicep 0.04 -0.15 0.0 ARM_WEIGHT -marker R.Tricep -0.04 -0.15 0.0 ARM_WEIGHT -marker R.Biceps.Lateral 0.03 -0.15 0.02 ARM_WEIGHT -gait_scale R_UPPER_ARM UPPER_ARM_LENGTH UPPER_ARM_LENGTH UPPER_ARM_LENGTH -mass UPPER_ARM_MASS -masscenter UPPER_ARM_CM_X UPPER_ARM_CM_Y UPPER_ARM_CM_Z -inertia UPPER_ARM_INERTIA_X 0.0 0.0 - 0.0 UPPER_ARM_INERTIA_Y 0.0 - 0.0 0.0 UPPER_ARM_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment elbow_marker_r -material my_bone -begingroups right arm_r elbow_r endgroups -marker R.Elbow -0.01 -0.281 0.04 ARM_WEIGHT -marker R.Elbow.Lateral -0.01 -0.281 0.04 ARM_WEIGHT -marker R.Elbow.Lat -0.01 -0.281 0.04 ARM_WEIGHT -marker RELB -0.01 -0.281 0.04 ARM_WEIGHT -marker R.Elbow.Medial 0.0 -0.281 -0.04 ARM_WEIGHT -marker R.Elbow.Med 0.0 -0.281 -0.04 ARM_WEIGHT -gait_scale R_UPPER_ARM UPPER_ARM_LENGTH UPPER_ARM_LENGTH UPPER_ARM_LENGTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment ulna_r -bone ulna_rv.asc -material my_bone -SHADOW_PARAMS -begingroups right arm_r elbow_r endgroups -marker R.Forearm 0.0 -0.15 0.0 WRIST_WEIGHT -gait_scale R_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass ULNA_MASS -masscenter LOWER_ARM_CM_X LOWER_ARM_CM_Y LOWER_ARM_CM_Z -inertia ULNA_INERTIA_X 0.0 0.0 - 0.0 ULNA_INERTIA_Y 0.0 - 0.0 0.0 ULNA_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment radius_r -bone radius_rv.asc -material my_bone -SHADOW_PARAMS -begingroups right arm_r elbow_r endgroups -gait_scale R_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass RADIUS_MASS -masscenter LOWER_ARM_CM_X LOWER_ARM_CM_Y LOWER_ARM_CM_Z -inertia RADIUS_INERTIA_X 0.0 0.0 - 0.0 RADIUS_INERTIA_Y 0.0 - 0.0 0.0 RADIUS_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment wrist_marker_r -material my_bone -SHADOW_PARAMS -begingroups right arm_r elbow_r endgroups -marker R.Wrist -0.025 -0.245 0.006 WRIST_WEIGHT -marker R.Wrist.Lateral -0.025 -0.245 0.006 WRIST_WEIGHT -marker R.Wrist.Lat -0.025 -0.245 0.006 WRIST_WEIGHT -marker R.Wrist.Medial 0.035 -0.245 0.000 WRIST_WEIGHT -marker R.Wrist.Med 0.035 -0.245 0.000 WRIST_WEIGHT -marker R.Ulna 0.0 -0.245 -0.03 WRIST_WEIGHT -marker R.Radius 0.0 -0.245 0.04 WRIST_WEIGHT -marker RWRI -0.025 -0.245 0.006 WRIST_WEIGHT -marker RWRA -0.025 -0.245 0.006 WRIST_WEIGHT -marker RWRB 0.035 -0.245 0.000 WRIST_WEIGHT -gait_scale R_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment hand_r -bone pisiform_rvs.asc -bone lunate_rvs.asc -bone scaphoid_rvs.asc -bone triquetrum_rvs.asc -bone hamate_rvs.asc -bone capitate_rvs.asc -bone trapezoid_rvs.asc -bone trapezium_rvs.asc -bone metacarpal2_rvs.asc -bone index_proximal_rvs.asc -bone index_medial_rvs.asc -bone index_distal_rvs.asc -bone metacarpal3_rvs.asc -bone middle_proximal_rvs.asc -bone middle_medial_rvs.asc -bone middle_distal_rvs.asc -bone metacarpal4_rvs.asc -bone ring_proximal_rvs.asc -bone ring_medial_rvs.asc -bone ring_distal_rvs.asc -bone metacarpal5_rvs.asc -bone little_proximal_rvs.asc -bone little_medial_rvs.asc -bone little_distal_rvs.asc -bone metacarpal1_rvs.asc -bone thumb_proximal_rvs.asc -bone thumb_distal_rvs.asc -material my_bone -SHADOW_PARAMS -begingroups right arm_r endgroups -marker R.Hand -0.030000 -0.050000 0.000000 OTHER_WEIGHT -gait_scale R_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass HAND_MASS -masscenter HAND_CM_X HAND_CM_Y HAND_CM_Z -inertia HAND_INERTIA_X 0.0 0.0 - 0.0 HAND_INERTIA_Y 0.0 - 0.0 0.0 HAND_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.1 0.0 -endsegment - -beginsegment clavicle_marker_l -material my_bone -SHADOW_PARAMS -begingroups left shoulder_l endgroups -marker L.Shoulder 0.000000 0.000000 -0.165000 OTHER_WEIGHT -marker LSHO 0.000000 0.000000 -0.165000 OTHER_WEIGHT -gait_scale TORSO TORSO_HEIGHT TORSO_HEIGHT TORSO_WIDTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.0 -0.001 -endsegment - -beginsegment humerus_l -bone humerus_lv.asc -material my_bone -SHADOW_PARAMS -begingroups left shoulder_l arm_l elbow_l endgroups -marker L.Bicep 0.04 -0.15 0.0 ARM_WEIGHT -marker L.Tricep -0.04 -0.15 0.0 ARM_WEIGHT -marker L.Biceps.Lateral 0.03 -0.15 -0.02 ARM_WEIGHT -gait_scale L_UPPER_ARM UPPER_ARM_LENGTH UPPER_ARM_LENGTH UPPER_ARM_LENGTH -mass UPPER_ARM_MASS -masscenter UPPER_ARM_CM_X UPPER_ARM_CM_Y UPPER_ARM_CM_Z -inertia UPPER_ARM_INERTIA_X 0.0 0.0 - 0.0 UPPER_ARM_INERTIA_Y 0.0 - 0.0 0.0 UPPER_ARM_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment elbow_marker_l -material my_bone -begingroups left arm_l elbow_l endgroups -marker L.Elbow 0.01 -0.281 -0.04 ARM_WEIGHT -marker L.Elbow.Lateral 0.01 -0.281 -0.04 ARM_WEIGHT -marker L.Elbow.Lat 0.01 -0.281 -0.04 ARM_WEIGHT -marker LELB 0.01 -0.281 -0.04 ARM_WEIGHT -marker L.Elbow.Medial -0.02 -0.281 0.04 ARM_WEIGHT -marker L.Elbow.Med -0.02 -0.281 0.04 ARM_WEIGHT -gait_scale L_UPPER_ARM UPPER_ARM_LENGTH UPPER_ARM_LENGTH UPPER_ARM_LENGTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment ulna_l -bone ulna_lv.asc -material my_bone -SHADOW_PARAMS -begingroups left arm_l elbow_l endgroups -marker L.Forearm 0.0 -0.15 0.0 WRIST_WEIGHT -gait_scale L_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass ULNA_MASS -masscenter LOWER_ARM_CM_X LOWER_ARM_CM_Y LOWER_ARM_CM_Z -inertia ULNA_INERTIA_X 0.0 0.0 - 0.0 ULNA_INERTIA_Y 0.0 - 0.0 0.0 ULNA_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment radius_l -bone radius_lv.asc -material my_bone -SHADOW_PARAMS -begingroups left arm_l elbow_l endgroups -gait_scale L_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass RADIUS_MASS -masscenter LOWER_ARM_CM_X LOWER_ARM_CM_Y LOWER_ARM_CM_Z -inertia RADIUS_INERTIA_X 0.0 0.0 - 0.0 RADIUS_INERTIA_Y 0.0 - 0.0 0.0 RADIUS_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment wrist_marker_l -material my_bone -SHADOW_PARAMS -begingroups left arm_l elbow_l endgroups -marker L.Wrist -0.025 -0.245 -0.006 WRIST_WEIGHT -marker L.Wrist.Lateral -0.025 -0.245 -0.006 WRIST_WEIGHT -marker L.Wrist.Lat -0.025 -0.245 -0.006 WRIST_WEIGHT -marker L.Wrist.Medial 0.035 -0.245 0.000 WRIST_WEIGHT -marker L.Wrist.Med 0.035 -0.245 0.000 WRIST_WEIGHT -marker L.Ulna 0.0 -0.245 0.03 WRIST_WEIGHT -marker L.Radius 0.0 -0.245 -0.04 WRIST_WEIGHT -marker LWRI -0.025 -0.245 -0.006 WRIST_WEIGHT -marker LWRA -0.025 -0.245 -0.006 WRIST_WEIGHT -marker LWRB 0.025 -0.245 0.000 WRIST_WEIGHT -gait_scale L_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.001 0.0 -endsegment - -beginsegment hand_l -bone pisiform_lvs.asc -bone lunate_lvs.asc -bone scaphoid_lvs.asc -bone triquetrum_lvs.asc -bone hamate_lvs.asc -bone capitate_lvs.asc -bone trapezoid_lvs.asc -bone trapezium_lvs.asc -bone metacarpal2_lvs.asc -bone index_proximal_lvs.asc -bone index_medial_lvs.asc -bone index_distal_lvs.asc -bone metacarpal3_lvs.asc -bone middle_proximal_lvs.asc -bone middle_medial_lvs.asc -bone middle_distal_lvs.asc -bone metacarpal4_lvs.asc -bone ring_proximal_lvs.asc -bone ring_medial_lvs.asc -bone ring_distal_lvs.asc -bone metacarpal5_lvs.asc -bone little_proximal_lvs.asc -bone little_medial_lvs.asc -bone little_distal_lvs.asc -bone metacarpal1_lvs.asc -bone thumb_proximal_lvs.asc -bone thumb_distal_lvs.asc -material my_bone -SHADOW_PARAMS -begingroups left arm_l endgroups -marker L.Hand -0.030000 -0.050000 0.000000 OTHER_WEIGHT -gait_scale L_LOWER_ARM LOWER_ARM_LENGTH LOWER_ARM_LENGTH LOWER_ARM_LENGTH -mass HAND_MASS -masscenter HAND_CM_X HAND_CM_Y HAND_CM_Z -inertia HAND_INERTIA_X 0.0 0.0 - 0.0 HAND_INERTIA_Y 0.0 - 0.0 0.0 HAND_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -#else /* UPPER_EXTREMITY */ - -beginsegment HeadArmsTrunk -bone ribcage_s.asc -material my_bone -SHADOW_PARAMS -begingroups hat endgroups -marker Offset -0.180000 0.220000 0.000000 OTHER_WEIGHT -marker Sternum 0.180000 0.220000 0.000000 OTHER_WEIGHT -marker T10 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker CLAV 0.180000 0.220000 0.000000 OTHER_WEIGHT -marker STRN 0.180000 0.220000 0.000000 OTHER_WEIGHT -marker RBAK 0.180000 0.220000 0.000000 OTHER_WEIGHT -marker R.Scapula -0.120000 -0.100000 -0.080000 OTHER_WEIGHT -marker R.Scapula.Top 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker R.Scapula.Bottom 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker R.Angulus Acromialis -0.055000 -0.015000 0.015000 OTHER_WEIGHT -marker R.Trigonum Spinae -0.090000 -0.045000 -0.050000 OTHER_WEIGHT -marker R.Angulus Inferior -0.100000 -0.170000 -0.040000 OTHER_WEIGHT -marker L.Scapula -0.120000 -0.100000 0.080000 OTHER_WEIGHT -marker L.Scapula.Top 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker L.Scapula.Bottom 0.000000 0.000000 0.000000 OTHER_WEIGHT -marker L.Angulus Acromialis -0.055000 -0.015000 -0.015000 OTHER_WEIGHT -marker L.Trigonum Spinae -0.090000 -0.045000 0.050000 OTHER_WEIGHT -marker L.Angulus Inferior -0.100000 -0.170000 0.040000 OTHER_WEIGHT -/* There are no markers above the pelvis, so scale the HAT segment with femur length. */ -gait_scale R_THIGH 0.3960 0.3960 0.3960 -mass HAT_MASS -masscenter HAT_CM_X HAT_CM_Y HAT_CM_Z -inertia HAT_INERTIA_X 0.0 0.0 - 0.0 HAT_INERTIA_Y 0.0 - 0.0 0.0 HAT_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -#endif /* UPPER_EXTREMITY */ - -beginsegment femur_r -bone femur_r.asc -material my_bone -SHADOW_PARAMS -begingroups right leg_r hip_r knee_r endgroups -marker R.Knee 0.0 -0.404 0.05 OTHER_WEIGHT -marker R.Knee.Lateral 0.0 -0.404 0.05 OTHER_WEIGHT -marker R.Knee.Lat 0.0 -0.404 0.05 OTHER_WEIGHT -marker RKNE 0.0 -0.404 0.05 OTHER_WEIGHT -marker R.Knee.Medial 0.0 -0.404 -0.05 OTHER_WEIGHT -marker R.Knee.Med 0.0 -0.404 -0.05 OTHER_WEIGHT -marker R.Thigh 0.0179 -0.2240 0.1147 OTHER_WEIGHT -marker R.Thigh.Upper 0.0179 -0.2640 0.0647 OTHER_WEIGHT -marker R.Thigh.Front 0.0800 -0.3240 0.0047 OTHER_WEIGHT -marker R.Thigh.Rear -0.0479 -0.3240 0.0047 OTHER_WEIGHT -marker RTHI 0.0179 -0.2640 0.0647 OTHER_WEIGHT -gait_scale R_THIGH THIGH_LENGTH THIGH_LENGTH THIGH_LENGTH -mass THIGH_MASS -masscenter THIGH_CM_X THIGH_CM_Y THIGH_CM_Z -inertia THIGH_INERTIA_X 0.0 0.0 - 0.0 THIGH_INERTIA_Y 0.0 - 0.0 0.0 THIGH_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment tibia_r -bone tibia_r.asc -bone fibula_r.asc -material my_bone -SHADOW_PARAMS -begingroups right leg_r knee_r ankle_r endgroups -marker R.Ankle -0.005 -0.41 0.053 FOOT_WEIGHT -marker R.Ankle.Lateral -0.005 -0.41 0.053 FOOT_WEIGHT -marker R.Ankle.Lat -0.005 -0.41 0.053 FOOT_WEIGHT -marker RANK -0.005 -0.41 0.053 FOOT_WEIGHT -marker R.Ankle.Medial 0.006 -0.3888 -0.038 FOOT_WEIGHT -marker R.Ankle.Med 0.006 -0.3888 -0.038 FOOT_WEIGHT -marker R.Shank 0.0104 -0.2322 0.0748 OTHER_WEIGHT -marker R.Shank.Upper 0.0125 -0.3196 0.0600 OTHER_WEIGHT -marker R.Shank.Front 0.0425 -0.3596 0.0000 OTHER_WEIGHT -marker R.Shank.Rear -0.0325 -0.3596 0.0000 OTHER_WEIGHT -marker RTIB 0.0125 -0.3196 0.0600 OTHER_WEIGHT -gait_scale R_SHANK SHANK_LENGTH SHANK_LENGTH SHANK_LENGTH -mass SHANK_MASS -masscenter SHANK_CM_X SHANK_CM_Y SHANK_CM_Z -inertia SHANK_INERTIA_X 0.0 0.0 - 0.0 SHANK_INERTIA_Y 0.0 - 0.0 0.0 SHANK_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment patella_r -bone patella_r.asc -material my_bone -SHADOW_PARAMS -begingroups right leg_r knee_r endgroups -gait_scale R_SHANK SHANK_LENGTH SHANK_LENGTH SHANK_LENGTH -mass PATELLA_MASS -masscenter PATELLA_CM_X PATELLA_CM_Y PATELLA_CM_Z -inertia PATELLA_INERTIA_X 0.0 0.0 - 0.0 PATELLA_INERTIA_Y 0.0 - 0.0 0.0 PATELLA_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -#if VARUS_VALGUS -beginsegment patella_vv_tmp_r -material my_bone -SHADOW_PARAMS -begingroups right leg_r knee_r endgroups -gait_scale R_SHANK SHANK_LENGTH SHANK_LENGTH SHANK_LENGTH -mass 0.0000001 -masscenter 0.0 0.0 0.0 -inertia 0.0000001 0.0 0.0 - 0.0 0.0000001 0.0 - 0.0 0.0 0.0000001 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment -#endif - -beginsegment talus_r -bone talus_rv.asc -material my_bone -SHADOW_PARAMS -begingroups right leg_r foot_r ankle_r endgroups -gait_scale R_FOOT FOOT_LENGTH FOOT_LENGTH FOOT_LENGTH -mass TALUS_MASS -masscenter TALUS_CM_X TALUS_CM_Y TALUS_CM_Z -inertia TALUS_INERTIA_X 0.0 0.0 - 0.0 TALUS_INERTIA_Y 0.0 - 0.0 0.0 TALUS_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment foot_r -bone calcaneous_rv.asc -bone navicular_rv.asc -bone cuboid_rv.asc -bone first_cuneiform_rv.asc -bone second_cuneiform_rv.asc -bone third_cuneiform_rv.asc -bone metatarsal1_rv.asc -bone metatarsal2_rv.asc -bone metatarsal3_rv.asc -bone metatarsal4_rv.asc -bone metatarsal5_rv.asc -material my_bone -SHADOW_PARAMS -begingroups right leg_r ankle_r foot_r endgroups -gait_scale R_FOOT FOOT_LENGTH FOOT_LENGTH FOOT_LENGTH -mass FOOT_MASS -masscenter FOOT_CM_X FOOT_CM_Y FOOT_CM_Z -inertia FOOT_INERTIA_X 0.0 0.0 - 0.0 FOOT_INERTIA_Y 0.0 - 0.0 0.0 FOOT_INERTIA_Z -force_matte right_foot r_shoe.asc -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 1.0 0.0 0.0 -endsegment - -beginsegment foot_marker_r -material my_bone -SHADOW_PARAMS -begingroups right leg_r ankle_r foot_r endgroups -marker R.Toe 0.190 0.0 0.0 FOOT_WEIGHT -marker RTOE 0.190 0.0 0.0 FOOT_WEIGHT -marker R.MedFoot 0.13 0.0 -0.03 FOOT_WEIGHT -marker R.LatFoot 0.13 0.0 0.05 FOOT_WEIGHT -marker R.Heel 0.0 0.0 0.0 FOOT_WEIGHT -marker RHEE 0.0 0.0 0.0 FOOT_WEIGHT -gait_scale R_FOOT FOOT_LENGTH FOOT_LENGTH FOOT_LENGTH -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.001 0.0 0.0 -endsegment - -beginsegment toes_r -bone prox_phalanx1_rvs.asc -bone prox_phalanx2_rvs.asc -bone prox_phalanx3_rvs.asc -bone prox_phalanx4_rvs.asc -bone prox_phalanx5_rvs.asc -bone mid_phalanx2_rvs.asc -bone mid_phalanx3_rvs.asc -bone mid_phalanx4_rvs.asc -bone mid_phalanx5_rvs.asc -bone distal_phalanx1_rvs.asc -bone distal_phalanx2_rvs.asc -bone distal_phalanx3_rvs.asc -bone distal_phalanx4_rvs.asc -bone distal_phalanx5_rvs.asc -material my_bone -SHADOW_PARAMS -begingroups right leg_r foot_r endgroups -gait_scale R_FOOT FOOT_LENGTH FOOT_LENGTH FOOT_LENGTH -mass TOES_MASS -masscenter TOES_CM_X TOES_CM_Y TOES_CM_Z -inertia TOES_INERTIA_X 0.0 0.0 - 0.0 TOES_INERTIA_Y 0.0 - 0.0 0.0 TOES_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.05 0.0 0.0 -endsegment - -beginsegment femur_l -bone femur_l.asc -material my_bone -SHADOW_PARAMS -begingroups left leg_l hip_l knee_l endgroups -marker L.Knee 0.0 -0.404 -0.05 OTHER_WEIGHT -marker L.Knee.Lateral 0.0 -0.404 -0.05 OTHER_WEIGHT -marker L.Knee.Lat 0.0 -0.404 -0.05 OTHER_WEIGHT -marker LKNE 0.0 -0.404 -0.05 OTHER_WEIGHT -marker L.Knee.Medial 0.0 -0.404 0.05 OTHER_WEIGHT -marker L.Knee.Med 0.0 -0.404 0.05 OTHER_WEIGHT -marker L.Thigh 0.0179 -0.2240 -0.1147 OTHER_WEIGHT -marker L.Thigh.Upper 0.0179 -0.2640 -0.0647 OTHER_WEIGHT -marker L.Thigh.Front 0.0800 -0.3240 -0.0047 OTHER_WEIGHT -marker L.Thigh.Rear -0.0479 -0.3240 -0.0047 OTHER_WEIGHT -marker LTHI 0.0179 -0.2640 -0.0647 OTHER_WEIGHT -gait_scale L_THIGH THIGH_LENGTH THIGH_LENGTH THIGH_LENGTH -mass THIGH_MASS -masscenter THIGH_CM_X THIGH_CM_Y THIGH_CM_Z -inertia THIGH_INERTIA_X 0.0 0.0 - 0.0 THIGH_INERTIA_Y 0.0 - 0.0 0.0 THIGH_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -#if VARUS_VALGUS -beginsegment tibia_l_pre -begingroups left leg_l knee_l ankle_l endgroups -gait_scale L_SHANK SHANK_LENGTH SHANK_LENGTH SHANK_LENGTH -mass 0.0000001 -masscenter 0.0 0.0 0.0 -inertia 0.0000001 0.0 0.0 - 0.0 0.0000001 0.0 - 0.0 0.0 0.0000001 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment -#endif - -beginsegment tibia_l -bone tibia_l.asc -bone fibula_l.asc -material my_bone -SHADOW_PARAMS -begingroups left leg_l knee_l ankle_l endgroups -marker L.Ankle -0.005 -0.41 -0.053 FOOT_WEIGHT -marker L.Ankle.Lateral -0.005 -0.41 -0.053 FOOT_WEIGHT -marker L.Ankle.Lat -0.005 -0.41 -0.053 FOOT_WEIGHT -marker LANK -0.005 -0.41 -0.053 FOOT_WEIGHT -marker L.Ankle.Medial 0.006 -0.3888 0.038 FOOT_WEIGHT -marker L.Ankle.Med 0.006 -0.3888 0.038 FOOT_WEIGHT -marker L.Shank 0.0104 -0.2322 -0.0748 OTHER_WEIGHT -marker L.Shank.Upper 0.0125 -0.3196 -0.0600 OTHER_WEIGHT -marker L.Shank.Front 0.0425 -0.3596 0.0000 OTHER_WEIGHT -marker L.Shank.Rear -0.0325 -0.3596 0.0000 OTHER_WEIGHT -marker LTIB 0.0125 -0.3196 -0.0600 OTHER_WEIGHT -gait_scale L_SHANK SHANK_LENGTH SHANK_LENGTH SHANK_LENGTH -mass SHANK_MASS -masscenter SHANK_CM_X SHANK_CM_Y SHANK_CM_Z -inertia SHANK_INERTIA_X 0.0 0.0 - 0.0 SHANK_INERTIA_Y 0.0 - 0.0 0.0 SHANK_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment patella_l -bone patella_l.asc -material my_bone -SHADOW_PARAMS -begingroups left leg_l knee_l endgroups -gait_scale L_SHANK SHANK_LENGTH SHANK_LENGTH SHANK_LENGTH -mass PATELLA_MASS -masscenter PATELLA_CM_X PATELLA_CM_Y PATELLA_CM_Z -inertia PATELLA_INERTIA_X 0.0 0.0 - 0.0 PATELLA_INERTIA_Y 0.0 - 0.0 0.0 PATELLA_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -#if VARUS_VALGUS -beginsegment patella_vv_tmp_l -material my_bone -SHADOW_PARAMS -begingroups left leg_l knee_l endgroups -gait_scale L_SHANK SHANK_LENGTH SHANK_LENGTH SHANK_LENGTH -mass 0.0000001 -masscenter 0.0 0.0 0.0 -inertia 0.0000001 0.0 0.0 - 0.0 0.0000001 0.0 - 0.0 0.0 0.0000001 -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment -#endif - -beginsegment talus_l -bone talus_lv.asc -material my_bone -SHADOW_PARAMS -begingroups left leg_l foot_l ankle_l endgroups -gait_scale L_FOOT FOOT_LENGTH FOOT_LENGTH FOOT_LENGTH -mass TALUS_MASS -masscenter TALUS_CM_X TALUS_CM_Y TALUS_CM_Z -inertia TALUS_INERTIA_X 0.0 0.0 - 0.0 TALUS_INERTIA_Y 0.0 - 0.0 0.0 TALUS_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment foot_l -bone calcaneous_lv.asc -bone navicular_lv.asc -bone cuboid_lv.asc -bone first_cuneiform_lv.asc -bone second_cuneiform_lv.asc -bone third_cuneiform_lv.asc -bone metatarsal1_lv.asc -bone metatarsal2_lv.asc -bone metatarsal3_lv.asc -bone metatarsal4_lv.asc -bone metatarsal5_lv.asc -material my_bone -SHADOW_PARAMS -begingroups left leg_l ankle_l foot_l endgroups -gait_scale L_FOOT FOOT_LENGTH FOOT_LENGTH FOOT_LENGTH -mass FOOT_MASS -masscenter FOOT_CM_X FOOT_CM_Y FOOT_CM_Z -inertia FOOT_INERTIA_X 0.0 0.0 - 0.0 FOOT_INERTIA_Y 0.0 - 0.0 0.0 FOOT_INERTIA_Z -force_matte left_foot l_shoe.asc -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 1.0 0.0 0.0 -endsegment - -beginsegment foot_marker_l -material my_bone -SHADOW_PARAMS -begingroups left leg_l ankle_l foot_l endgroups -marker L.Toe 0.190 0.0 0.0 FOOT_WEIGHT -marker LTOE 0.190 0.0 0.0 FOOT_WEIGHT -marker L.MedFoot 0.13 0.0 0.03 FOOT_WEIGHT -marker L.LatFoot 0.13 0.0 -0.05 FOOT_WEIGHT -marker L.Heel 0.0 0.0 0.0 FOOT_WEIGHT -marker LHEE 0.0 0.0 0.0 FOOT_WEIGHT -gait_scale L_FOOT 0.267000 0.267000 0.267000 -mass 0.0 -masscenter 0.0 0.0 0.0 -inertia 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.001 0.0 0.0 -endsegment - -beginsegment toes_l -bone prox_phalanx1_lvs.asc -bone prox_phalanx2_lvs.asc -bone prox_phalanx3_lvs.asc -bone prox_phalanx4_lvs.asc -bone prox_phalanx5_lvs.asc -bone mid_phalanx2_lvs.asc -bone mid_phalanx3_lvs.asc -bone mid_phalanx4_lvs.asc -bone mid_phalanx5_lvs.asc -bone distal_phalanx1_lvs.asc -bone distal_phalanx2_lvs.asc -bone distal_phalanx3_lvs.asc -bone distal_phalanx4_lvs.asc -bone distal_phalanx5_lvs.asc -material my_bone -SHADOW_PARAMS -begingroups left leg_l foot_l endgroups -gait_scale L_FOOT 0.267000 0.267000 0.267000 -mass TOES_MASS -masscenter TOES_CM_X TOES_CM_Y -TOES_CM_Z -inertia TOES_INERTIA_X 0.0 0.0 - 0.0 TOES_INERTIA_Y 0.0 - 0.0 0.0 TOES_INERTIA_Z -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.05 0.0 0.0 -endsegment - - - -/*********************************************/ -/* JOINTS */ -/*********************************************/ - -beginjoint gnd_pelvis -segments ground pelvis -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx function f1(lower_torso_TX) -ty function f1(lower_torso_TY) -tz function f1(lower_torso_TZ) -r1 function f1(lower_torso_RX) -r2 function f1(lower_torso_RY) -r3 function f1(lower_torso_RZ) -endjoint - -#if UPPER_EXTREMITY - -beginjoint pelvis_torso -segments pelvis torso -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.087200 -ty constant 0.033100 -tz constant 0.000000 -r1 function f1(lumbar_roll) -r2 function f1(lumbar_yaw) -r3 function f1(lumbar_pitch) -endjoint - -beginjoint torso_neckhead -segments torso neckhead -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.0253 -ty constant 0.44895 -tz constant 0.000000 -r1 function f1(neck_roll) -r2 function f1(neck_yaw) -r3 function f1(neck_pitch) -endjoint - -beginjoint neckhead_OT_head -segments neckhead OT_head -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.07166 -ty constant 0.235900 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant -90.000000 -endjoint - -beginjoint clavicularmarker_r -segments torso clavicle_marker_r -order r1 r3 r2 t -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.0488 -ty constant 0.47065 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint acromial_r -segments torso humerus_r -order t r1 r3 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.0281 -ty constant 0.41645 -tz constant 0.199000 -r1 function f1(arm_add_r) -r2 function f1(arm_rot_r) -r3 function f1(arm_flex_r) -endjoint - -beginjoint elbowmarker_r -segments humerus_r elbow_marker_r -order t r3 r2 r1 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.226047 0.022269 0.973862 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 15.000000 -r3 constant 3.000000 -endjoint - -beginjoint elbow_r -segments humerus_r ulna_r -order t r3 r2 r1 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.226047 0.022269 0.973862 -tx constant 0.013144 -ty constant -0.286273 -tz constant -0.009595 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1(elbow_flex_r) -endjoint - -beginjoint radioulnar_r -segments ulna_r radius_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.056398 0.998406 0.001952 -axis3 0.000000 0.000000 1.000000 -tx constant -0.006727 -ty constant -0.013007 -tz constant 0.026083 -r1 constant 0.000000 -r2 function f1(pro_sup_r) -r3 constant 0.000000 -endjoint - -beginjoint wristmarker_r -segments radius_r wrist_marker_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.006727 -ty constant 0.013007 -tz constant -0.026083 -r1 constant -8.000000 -r2 constant 10.000000 -r3 constant -4.000000 -endjoint - -beginjoint radius_hand_r -segments radius_r hand_r -order t r3 r2 r1 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.008797 -ty constant -0.235841 -tz constant 0.01361 -r1 function f1(wrist_dev_r) -r2 constant 0.000000 -r3 function f1(wrist_flex_r) -endjoint - -beginjoint clavicularmarker_l -segments torso clavicle_marker_l -order r1 r3 r2 t -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.04880 -ty constant 0.47065 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint acromial_l -segments torso humerus_l -order t r1 r3 r2 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.0281 -ty constant 0.41645 -tz constant -0.199000 -r1 function f1(arm_add_l) -r2 function f1(arm_rot_l) -r3 function f1(arm_flex_l) -endjoint - -beginjoint elbowmarker_l -segments humerus_l elbow_marker_l -order t r3 r2 r1 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 -0.226047 -0.022269 0.973862 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant -15.000000 -r3 constant 3.000000 -endjoint - -beginjoint elbow_l -segments humerus_l ulna_l -order t r3 r2 r1 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 -0.226047 -0.022269 0.973862 -tx constant 0.013144 -ty constant -0.286273 -tz constant 0.009595 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1(elbow_flex_l) -endjoint - -beginjoint radioulnar_l -segments ulna_l radius_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 -0.056398 -0.998406 0.001952 -axis3 0.000000 0.000000 1.000000 -tx constant -0.006727 -ty constant -0.013007 -tz constant -0.026083 -r1 constant 0.000000 -r2 function f1(pro_sup_l) -r3 constant 0.000000 -endjoint - -beginjoint wristmarker_l -segments radius_l wrist_marker_l -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.006727 -ty constant 0.013007 -tz constant 0.026083 -r1 constant 8.000000 -r2 constant -10.000000 -r3 constant -4.000000 -endjoint - -beginjoint radius_hand_l -segments radius_l hand_l -order t r3 r2 r1 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.008797 -ty constant -0.235841 -tz constant -0.01361 -r1 function f1(wrist_dev_l) -r2 constant 0.000000 -r3 function f1(wrist_flex_l) -endjoint - -#else /* UPPER_EXTREMITY */ - -beginjoint pelvis_hat -segments pelvis HeadArmsTrunk -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.087200 -ty constant 0.033100 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -#endif /* UPPER_EXTREMITY */ - -beginjoint hip_r -segments pelvis femur_r -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.080000 -ty constant -0.082400 -tz constant 0.078500 -r1 function f1(hip_add_r) -r2 function f1(hip_rot_r) -r3 function f1(hip_flex_r) -endjoint - -#if VARUS_VALGUS - -beginjoint knee_flexion_r -segments femur_r tibia_r -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 -1.000000 -tx function f1600(knee_flex_r) -ty function f1601(knee_flex_r) -tz function f99(knee_flex_r) -r1 function f1(knee_vv_r) -r2 constant 0.000001 -r3 function f1(knee_flex_r) -endjoint - -beginjoint tib_pat_r -segments tibia_r patella_vv_tmp_r -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx function f1602(knee_flex_r) -ty function f1603(knee_flex_r) -tz constant 0.002400 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1604(knee_flex_r) -endjoint - -beginjoint patella_vv_corr_r -segments patella_vv_tmp_r patella_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 function f1612(knee_vv_r) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -#else - -beginjoint knee_flexion_r -segments femur_r tibia_r -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 -1.000000 -tx function f1600(knee_flex_r) -ty function f1601(knee_flex_r) -tz constant 0.0 -r1 constant 0.0 -r2 constant 0.0 -r3 function f1(knee_flex_r) -endjoint - -beginjoint tib_pat_r -segments tibia_r patella_r -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx function f1602(knee_flex_r) -ty function f1603(knee_flex_r) -tz constant 0.002400 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1604(knee_flex_r) -endjoint - -#endif - -beginjoint ankle_r -segments tibia_r talus_r -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 -0.104529 -0.173649 0.979244 -tx constant 0.000000 -ty constant -0.426000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1(ankle_flex_r) -endjoint - -beginjoint subtalar_r -segments talus_r foot_r -order t r1 r2 r3 -axis1 0.787180 0.604747 -0.120949 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.048770 -ty constant -0.041950 -tz constant 0.007920 -r1 function f1(subt_angle_r) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint foot_mark_r -segments foot_r foot_marker_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.020000 -ty constant 0.018000 -tz constant -0.010000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint toes_r -segments foot_r toes_r -order t r1 r2 r3 -axis1 0.541717 0.000000 -0.840561 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.176800 -ty constant -0.002000 -tz constant 0.001080 -r1 function f1(toe_angle_r) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint hip_l -segments pelvis femur_l -order t r3 r1 r2 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.080000 -ty constant -0.082400 -tz constant -0.078500 -r1 function f1(hip_add_l) -r2 function f1(hip_rot_l) -r3 function f1(hip_flex_l) -endjoint - -#if VARUS_VALGUS - -beginjoint knee_flexion_l_1 -segments femur_l tibia_l_pre -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx function f1700(knee_flex_l) -ty function f1701(knee_flex_l) -tz constant 0.0 -r1 constant 0.0 -r2 constant 0.0 -r3 constant 0.000001 -endjoint - -beginjoint knee_flexion_l_2 -segments tibia_l_pre tibia_l -order t r3 r1 r2 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 -1.000000 -tx constant 0.0 -ty constant 0.0 -tz constant 0.0 -r1 function f1(knee_vv_l) -r2 constant 0.0 -r3 function f1(knee_flex_l) -endjoint - -beginjoint tib_pat_l -segments tibia_l patella_vv_tmp_l -order t r3 r1 r2 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx function f1702(knee_flex_l) -ty function f1703(knee_flex_l) -tz constant -0.002400 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1704(knee_flex_l) -endjoint - -beginjoint patella_vv_corr_l -segments patella_vv_tmp_l patella_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 function f1612(knee_vv_l) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -#else - -beginjoint knee_flexion_l -segments femur_l tibia_l -order t r3 r1 r2 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 -1.000000 -tx function f1700(knee_flex_l) -ty function f1701(knee_flex_l) -tz constant 0.0 -r1 constant 0.0 -r2 constant 0.0 -r3 function f1(knee_flex_l) -endjoint - -beginjoint tib_pat_l -segments tibia_l patella_l -order t r3 r1 r2 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx function f1702(knee_flex_l) -ty function f1703(knee_flex_l) -tz constant -0.002400 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1704(knee_flex_l) -endjoint - -#endif - -beginjoint ankle_l -segments tibia_l talus_l -order t r3 r1 r2 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.104529 0.173649 0.979244 -tx constant 0.000000 -ty constant -0.426000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1(ankle_flex_l) -endjoint - -beginjoint subtalar_l -segments talus_l foot_l -order t r1 r2 r3 -axis1 -0.787180 -0.604747 -0.120949 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.048770 -ty constant -0.041950 -tz constant -0.007920 -r1 function f1(subt_angle_l) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint foot_mark_l -segments foot_l foot_marker_l -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.020000 -ty constant 0.018000 -tz constant 0.010000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint toes_l -segments foot_l toes_l -order t r1 r2 r3 -axis1 -0.541717 0.000000 -0.840561 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.176800 -ty constant -0.002000 -tz constant -0.001080 -r1 function f1(toe_angle_l) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - - - -/*********************************************/ -/* GENCOORDS */ -/*********************************************/ - -begingencoord lower_torso_TX -range -10.000000 10.000000 -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ - default_value 0.969 -#endif -begingroups ground_LT endgroups -clamped no -visible no -pd_stiffness 0.1 -endgencoord - -begingencoord lower_torso_TY -range -10.000000 10.000000 -#if GROUND_PLANE_XZ - default_value 0.969 -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ -#endif -begingroups ground_LT endgroups -clamped no -visible no -pd_stiffness 0.1 -endgencoord - -begingencoord lower_torso_TZ -range -10.000000 10.000000 -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY - default_value 0.969 -#elif GROUND_PLANE_YZ -#endif -begingroups ground_LT endgroups -clamped no -visible no -pd_stiffness 0.1 -endgencoord - -begingencoord lower_torso_RX -range -270.000000 270.000000 -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY - default_value 90.0 -#elif GROUND_PLANE_YZ -#endif -begingroups ground_LT endgroups -clamped no -pd_stiffness 0.1 -endgencoord - -begingencoord lower_torso_RY -range -270.000000 270.000000 -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ - default_value -90.0 -#endif -begingroups ground_LT endgroups -clamped no -pd_stiffness 0.1 -endgencoord - -begingencoord lower_torso_RZ -range -270.000000 270.000000 -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ - default_value -90.0 -#endif -begingroups ground_LT endgroups -clamped no -pd_stiffness 0.1 -endgencoord - -#if UPPER_EXTREMITY - -begingencoord lumbar_roll -range -45.0 45.0 -begingroups lumbar spine endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.1 -endgencoord - -begingencoord lumbar_yaw -range -60.0 60.0 -begingroups lumbar spine endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.1 -endgencoord - -begingencoord lumbar_pitch -range -45.0 45.0 -begingroups lumbar spine endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.1 -endgencoord - -begingencoord neck_roll -range -40.0 40.0 -begingroups neck spine endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.01 -endgencoord - -begingencoord neck_yaw -range -66.0 66.0 -begingroups neck spine endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.01 -endgencoord - -begingencoord neck_pitch -range -50.0 72.0 -begingroups neck spine endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.01 -endgencoord - -begingencoord arm_add_r -range -120.000000 90.000000 -#if ARMS_UP -default_value -90.0 -#endif -begingroups right shoulder_r arm_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord arm_rot_r -#if FULL_ARM_MOTION -range -90.0 90.0 -#else -range -10.0 10.0 -#endif -begingroups right shoulder_r arm_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord arm_flex_r -#if FULL_ARM_MOTION -range -90.0 90.0 -#else -range -45.0 90.0 -#endif -begingroups right shoulder_r arm_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord elbow_flex_r -range 0.000000 130.000000 -begingroups right arm_r elbow_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord pro_sup_r -#if FULL_ARM_MOTION -range 0.0 90.0 -#else -range 50.0 70.0 -#endif -default_value 60.0 -begingroups right arm_r elbow_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord wrist_flex_r -range -70.000000 70.000000 -begingroups right arm_r wrist_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.01 -endgencoord - -begingencoord wrist_dev_r -range -25.000000 35.000000 -begingroups right arm_r wrist_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.01 -endgencoord - -begingencoord arm_add_l -range -120.000000 90.000000 -#if ARMS_UP -default_value -90.0 -#endif -begingroups left shoulder_l arm_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord arm_rot_l -#if FULL_ARM_MOTION -range -90.0 90.0 -#else -range -10.0 10.0 -#endif -begingroups left shoulder_l arm_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord arm_flex_l -#if FULL_ARM_MOTION -range -90.0 90.0 -#else -range -45.0 90.0 -#endif -begingroups left shoulder_l arm_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord elbow_flex_l -range 0.000000 130.000000 -begingroups left arm_l elbow_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord pro_sup_l -#if FULL_ARM_MOTION -range 0.0 90.0 -#else -range 50.0 70.0 -#endif -default_value 60.0 -begingroups left arm_l elbow_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.01 -endgencoord - -begingencoord wrist_flex_l -range -70.000000 70.000000 -begingroups left arm_l wrist_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.01 -endgencoord - -begingencoord wrist_dev_l -range -25.000000 35.000000 -begingroups left arm_l wrist_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.01 -endgencoord - -#endif /* UPPER_EXTREMITY */ - -begingencoord hip_flex_r -range -11.000000 95.000000 -begingroups right leg_r hip_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.1 -endgencoord - -begingencoord hip_add_r -range -50.000000 20.000000 -begingroups right leg_r hip_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.1 -endgencoord - -begingencoord hip_rot_r -range -40.000000 40.000000 -begingroups right leg_r hip_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.1 -endgencoord - -begingencoord knee_flex_r -range -10.000000 120.000000 -begingroups right leg_r knee_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.2 -endgencoord - -#if VARUS_VALGUS -begingencoord knee_vv_r -range -25.000000 25.000000 -begingroups right leg_r knee_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.2 -endgencoord -#endif - -begingencoord ankle_flex_r -range -30.000000 30.000000 -begingroups right leg_r ankle_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 2.0 -endgencoord - -begingencoord subt_angle_r -range -20.000000 20.000000 -begingroups right leg_r ankle_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 2.0 -endgencoord - -begingencoord toe_angle_r -range -30.000000 30.000000 -begingroups right leg_r toes_r endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.1 -endgencoord - -begingencoord hip_flex_l -range -11.000000 95.000000 -begingroups left leg_l hip_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.1 -endgencoord - -begingencoord hip_add_l -range -50.000000 20.000000 -begingroups left leg_l hip_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.1 -endgencoord - -begingencoord hip_rot_l -range -40.000000 40.000000 -begingroups left leg_l hip_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.1 -endgencoord - -begingencoord knee_flex_l -range -10.000000 120.000000 -begingroups left leg_l knee_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.2 -endgencoord - -#if VARUS_VALGUS -begingencoord knee_vv_l -range -25.000000 25.000000 -begingroups left leg_l knee_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 0.2 -endgencoord -#endif - -begingencoord ankle_flex_l -range -30.000000 30.000000 -begingroups left leg_l ankle_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 2.0 -endgencoord - -begingencoord subt_angle_l -range -20.000000 20.000000 -begingroups left leg_l ankle_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -pd_stiffness 2.0 -endgencoord - -begingencoord toe_angle_l -range -30.000000 30.000000 -begingroups left leg_l toes_l endgroups -minrestraint f43 -maxrestraint f43 -clamped yes -visible no -pd_stiffness 0.1 -endgencoord - - -/*********************************************/ -/* FUNCTIONS */ -/*********************************************/ - -beginfunction f1 -(-360.000000,-360.000000) -( 360.000000, 360.000000) -endfunction - -beginfunction f99 -(-360.0, 0.0) -( 360.0, 0.0) -endfunction - -beginfunction f1600 -( 0.000000, -0.005250) -( 10.000000, -0.003100) -( 20.000000, -0.001000) -( 40.000000, 0.002120) -( 60.000000, 0.004100) -( 80.000000, 0.004110) -( 100.000000, 0.001790) -( 120.000000, -0.003200) -endfunction - -beginfunction f1601 -( 0.000000, -0.396000) -( 10.000000, -0.396600) -( 20.000000, -0.397600) -( 30.000000, -0.399000) -( 70.000000, -0.408200) -( 120.000000, -0.422600) -endfunction - -beginfunction f1602 -( 0.000000, 0.049600) -( 10.000000, 0.048400) -( 20.000000, 0.046900) -( 40.000000, 0.043000) -( 60.000000, 0.038100) -( 80.000000, 0.032400) -( 120.000000, 0.017300) -endfunction - -beginfunction f1603 -( 0.286000, -0.022700) -( 10.000000, -0.022300) -( 20.000000, -0.021900) -( 40.000000, -0.021100) -( 60.000000, -0.020400) -( 80.000000, -0.020000) -( 90.000000, -0.020200) -( 120.000000, -0.021900) -endfunction - -beginfunction f1604 -( -10.000000, -9.240000) -( -0.150000, -2.440000) -( 20.340000, 6.090000) -( 56.430000, 13.860000) -( 114.590000, 17.650000) -( 120.000000, 17.650000) -endfunction - -beginfunction f1612 -( -15.000000, 15.000000) -( -10.000000, 10.000000) -( -5.000000, 5.000000) -( 0.000000, 0.000000) -( 5.000000, -5.000000) -( 10.000000, -10.000000) -( 15.000000, -15.000000) -endfunction - -beginfunction f1700 -( 0.000000, -0.005250) -( 10.000000, -0.003100) -( 20.000000, -0.001000) -( 40.000000, 0.002120) -( 60.000000, 0.004100) -( 80.000000, 0.004110) -( 100.000000, 0.001790) -( 120.000000, -0.003200) -endfunction - -beginfunction f1701 -( 0.000000, -0.396000) -( 10.000000, -0.396600) -( 20.000000, -0.397600) -( 30.000000, -0.399000) -( 70.000000, -0.408200) -( 120.000000, -0.422600) -endfunction - -beginfunction f1702 -( 0.000000, 0.049600) -( 10.000000, 0.048400) -( 20.000000, 0.046900) -( 40.000000, 0.043000) -( 60.000000, 0.038100) -( 80.000000, 0.032400) -( 120.000000, 0.017300) -endfunction - -beginfunction f1703 -( 0.286000, -0.022700) -( 10.000000, -0.022300) -( 20.000000, -0.021900) -( 40.000000, -0.021100) -( 60.000000, -0.020400) -( 80.000000, -0.020000) -( 90.000000, -0.020200) -( 120.000000, -0.021900) -endfunction - -beginfunction f1704 -( -10.000000, -9.240000) -( -0.150000, -2.440000) -( 20.340000, 6.090000) -( 56.430000, 13.860000) -( 114.590000, 17.650000) -( 120.000000, 17.650000) -endfunction - -beginfunction f43 -( 0.000000, 0.000000) -(10.000000, 20.000000) -(30.000000, 70.000000) -endfunction - -/*********************************************/ -/* MATERIALS AND COLORS */ -/*********************************************/ - -beginmaterial my_bone -ambient 0.6000 0.6000 0.6000 -diffuse 0.6000 0.4500 0.4000 -specular 0.7000 0.5500 0.4000 -shininess 10.0000 -endmaterial - -beginmaterial cyan -ambient 0.1000 0.8000 1.0000 -diffuse 0.1000 0.3000 0.5000 -specular 0.1000 0.3000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial green -ambient 0.0100 0.9000 0.0100 -diffuse 0.1000 0.9000 0.1000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial orange -ambient 1.0000 0.3000 0.0100 -diffuse 0.5000 0.1000 0.1000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial yellow -ambient 1.0000 0.8000 0.1000 -diffuse 0.5000 0.4000 0.1000 -specular 0.5000 0.4000 0.1000 -shininess 10.0000 -endmaterial - -beginmaterial purple -ambient 0.5000 0.0100 0.5000 -diffuse 0.5000 0.0100 0.5000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial blue -ambient 0.1000 0.1000 1.0000 -diffuse 0.1000 0.1000 1.0000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial pink -ambient 0.9000 0.3000 0.2000 -diffuse 0.2000 0.3000 0.2000 -specular 0.1400 0.0000 0.2000 -shininess 25.0000 -endmaterial - -beginmaterial tan -ambient 0.7000 0.4000 0.1000 -diffuse 0.2000 0.3000 0.2000 -specular 0.1400 0.0000 0.2000 -shininess 10.0000 -endmaterial - -beginmaterial red -ambient 1.0000 0.1000 0.1000 -diffuse 0.5000 0.1000 0.1000 -specular 0.5000 0.1000 0.1000 -shininess 10.0000 -endmaterial - -beginmaterial arrow_mat -ambient 0.4000 0.4000 0.7000 -diffuse 0.3000 0.3000 0.5000 -specular 0.3000 0.3000 0.5000 -shininess 0.0000 -endmaterial - -beginmaterial ligament_mat -ambient 0.8000 0.3000 0.3000 -diffuse 0.7000 0.2000 0.2000 -specular 1.0000 0.5000 0.5000 -shininess 20.0000 -endmaterial - -beginmaterial bone_mat -ambient 0.6000 0.6000 0.6000 -diffuse 0.6000 0.4500 0.4000 -specular 0.7000 0.5500 0.4000 -shininess 10.0000 -endmaterial - -beginmaterial goldmetal -ambient 0.4000 0.2000 0.0000 -diffuse 0.2200 0.1800 0.0500 -specular 0.8000 0.8000 0.4000 -shininess 93.0000 -endmaterial - -beginmaterial graymetal -ambient 0.2000 0.2000 0.2000 -diffuse 0.0500 0.0500 0.0500 -specular 1.0000 1.0000 1.0000 -shininess 95.0000 -endmaterial - -beginmaterial rad_bone -ambient 0.6000 0.6000 0.8000 -diffuse 0.6000 0.4500 0.7000 -specular 0.9000 0.4000 0.4000 -shininess 20.0000 -endmaterial - -beginmaterial dist_bone -ambient 0.6000 0.6000 0.8000 -diffuse 0.6000 0.4500 0.7000 -specular 0.9000 0.4000 0.4000 -shininess 20.0000 -endmaterial - -beginmaterial floor_material -ambient 0.5000 0.4000 0.3000 -diffuse 0.5000 0.4000 0.3000 -specular 0.5000 0.4000 0.3000 -endmaterial - - -/*********************************************/ -/* MOTIONOBJECTS */ -/*********************************************/ - -beginmotionobject ball -material blue -scale 0.25 0.25 0.25 -endmotionobject - -beginmotionobject joint_force - filename arrow.asc - scale 1.0 0.003 1.0 - material blue - vectoraxis y -endmotionobject - -/*********************************************/ -/* WORLDOBJECTS */ -/*********************************************/ - -#if GROUND_PLANE_XZ - #define FLOOR floor_xz_plane.asc - #define FLOOR_ORIGIN 0.0 -0.002 0.0 -#elif GROUND_PLANE_XY - #define FLOOR floor_xy_plane.asc - #define FLOOR_ORIGIN 0.0 0.0 -0.002 -#elif GROUND_PLANE_YZ - #define FLOOR floor_yz_plane.asc - #define FLOOR_ORIGIN -0.002 0.0 0.0 -#endif - -beginworldobject floor -filename FLOOR -material floor_material -origin FLOOR_ORIGIN -drawmode solid_fill -endworldobject - - -/*********************************************/ -/* CAMERAS */ -/*********************************************/ - -#if GROUND_PLANE_XZ - beginview default - 0.0 0.0 1.0 0.0 - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview motion - -0.7046 -0.0060 0.7096 0.0000 - 0.0366 0.9983 0.0448 0.0000 - -0.7087 0.0575 -0.7032 0.0000 - 0.3181 -0.8978 -4.5020 1.0000 - endview - - beginview front - 0.0 0.0 1.0 0.0 - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview side - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview top - 1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -1.0 0.0 0.0 - 0.0 0.0 -5.0 1.0 - endview - -#elif GROUND_PLANE_XY - beginview default - 0.0 0.0 1.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview motion - -0.3906 -0.0806 0.9170 0.0000 - 0.9205 -0.0301 0.3895 0.0000 - -0.0038 0.9963 0.0860 0.0000 - 0.5573 -1.0410 -3.6453 1.0000 - endview - - beginview front - 0.0 0.0 1.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview side - 1.0 0.0 0.0 0.0 - 0.0 0.0 -1.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview top - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 0.0 -5.0 1.0 - endview - -#elif GROUND_PLANE_YZ - beginview default - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -0.5 -5.0 1.0 - endview - - beginview motion - -0.0016 0.9994 -0.0347 0.0000 - -0.7450 -0.0243 -0.6666 0.0000 - -0.6670 0.0248 0.7446 0.0000 - 0.5645 -0.8547 -6.0538 1.0000 - endview - - beginview front - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -0.5 -5.0 1.0 - endview - - beginview side - 0.0 1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 -0.5 -5.0 1.0 - endview - - beginview top - 0.0 0.0 1.0 0.0 - 0.0 -1.0 0.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 0.0 -5.0 1.0 - endview -#endif - diff --git a/OpenSim/Utilities/simmToOpenSim/test/dynamic.msl b/OpenSim/Utilities/simmToOpenSim/test/dynamic.msl deleted file mode 100644 index 0d3a496b70..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/dynamic.msl +++ /dev/null @@ -1,1752 +0,0 @@ -/******************************************************************************* - DYNAMIC MOCAP MODEL - muscle file - - (c) Copyright 2003-4, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the muscle definitions for the dynamic SIMM model of an average - adult male. It was developed by various biomechanics researchers, including: - - Scott Delp, Ph.D., Stanford University - Wendy Murray, Ph.D., Stanford University - Silvia Salinas Blemker, M.S., Stanford University - Anita Vasavada, Ph.D., Washington State University - Srikanth Suryanarayanan, M.S., Rehabilitation Institute of Chicago - Frans van der Helm, Ph.D., Delft University - -*******************************************************************************/ - -/* If MUSCLE_FORCES is set to 0, then the muscles will not - * contain force-generating properties, so you will not be - * able to plot muscle forces for this model. If MUSCLE_FORCES - * is set to 1, then the force-generating properties for - * an average adult male (which the mocap model represents) are - * included. However, since this mocap model is scaled to fit - * the motion capture subject, it is important to understand - * how force-generating properties are scaled before you plot - * muscle forces for a scaled model. Scaling of force-generating - * properties is currently an open research topic, so there is - * no generally accepted method for scaling them. The scalemodel - * utility in SIMM ASSUMES THAT FORCE-GENERATING PROPERTIES - * SCALE WITH TOTAL MUSCLE-TENDON LENGTH. For each muscle, SIMM - * computes the muscle-tendon length in the unscaled model (Lu) - * and in the scaled model (Ls). The force-generating properties - * of max_isometric_force, optimal_fiber_length, and - * resting_tendon_length are then scaled by the factor (Ls/Lu). - * If you do not accept this method of scaling, then you should - * not include force-generating muscle properties in your mocap - * model. - */ - -#define MUSCLE_FORCES 1 - -begindynamicparameters -timescale -mass -damping -activation1 -activation2 -enddynamicparameters - -beginmuscle defaultmuscle -#if MUSCLE_FORCES -begintendonforcelengthcurve -/* (tendon strain, normalized force) */ -(-1.000,0.0000) -(-0.0020,0.0000) -(-0.0010,0.0000) -(0.00000,0.0000) -(0.00131,0.0108) -(0.00281,0.0257) -(0.00431,0.0435) -(0.00581,0.0652) -(0.00731,0.0915) -(0.00881,0.123) -(0.01030,0.161) -(0.01180,0.208) -(0.01230,0.227) -(9.20000,345.0) -(9.20100,345.0) -(9.20200,345.0) -(12.0000,345.0) -endtendonforcelengthcurve - -beginactiveforcelengthcurve -/* (norm length, norm force) */ -(0.000000,0.000000) -(0.401000,0.000000) -(0.402000,0.000000) -(0.403500,0.000000) -(0.527250,0.226667) -(0.628750,0.636667) -(0.718750,0.856667) -(0.861250,0.950000) -(1.045000,0.993333) -(1.217500,0.770000) -(1.438750,0.246667) -(1.618750,0.000000) -(1.620000,0.000000) -(1.621000,0.000000) -(2.000000,0.000000) -endactiveforcelengthcurve - -beginpassiveforcelengthcurve -/* (norm length, norm force) */ -(0.80000,0.000000) -(0.998000,0.000000) -(0.999000,0.000000) -(1.000000,0.000000) -(1.100000,0.035) -(1.200000,0.120) -(1.300000,0.260) -(1.400000,0.550) -(1.500000,1.170) -(1.600000,2.000000) -(1.601000,2.000000) -(1.602000,2.000000) -(2.000000,2.000000) -endpassiveforcelengthcurve - -beginforcevelocitycurve -/* velocity, normalized force */ -(-1.001000,0.000000) -(-1.000000,0.000000) -(-0.950000,0.010417) -(-0.900000,0.021739) -(-0.850000,0.034091) -(-0.800000,0.047619) -(-0.750000,0.062500) -(-0.700000,0.078947) -(-0.650000,0.097222) -(-0.600000,0.117647) -(-0.550000,0.140625) -(-0.500000,0.166667) -(-0.450000,0.196429) -(-0.400000,0.230769) -(-0.350000,0.270833) -(-0.300000,0.318182) -(-0.250000,0.375000) -(-0.200000,0.444444) -(-0.150000,0.531250) -(-0.100000,0.642857) -(-0.050000,0.791667) -(0.000000,1.000000) -(0.050000,1.482014) -(0.100000,1.601571) -(0.150000,1.655791) -(0.200000,1.686739) -(0.250000,1.706751) -(0.300000,1.720753) -(0.350000,1.731099) -(0.400000,1.739055) -(0.450000,1.745365) -(0.500000,1.750490) -(0.550000,1.754736) -(0.600000,1.758312) -(0.650000,1.761364) -(0.700000,1.763999) -(0.750000,1.766298) -(0.800000,1.768321) -(0.850000,1.770115) -(0.900000,1.771717) -(0.950000,1.773155) -(1.000000,1.774455) -endforcevelocitycurve -max_contraction_velocity 10.0 /* fiberlengths/second */ -#endif -min_thickness 0.0025 -max_thickness 0.015 -timescale 0.1 -activation1 7.667 -activation2 1.459854 -mass 0.00286 -damping 0.1 -muscle_model 4 -excitation_format step_function -pennation_angle 0.0 -endmuscle - - -/*********** Right Leg ***********/ - -/* GLUTEUS MAXIMUS */ -beginmuscle glut_max1_r -beginpoints --0.1253 0.0425 0.0606 segment pelvis --0.1384 -0.0151 0.0836 segment pelvis --0.0457 -0.0248 0.0392 segment femur_r --0.0277 -0.0566 0.0470 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 382.0 /* source: Brand */ -optimal_fiber_length 0.1420 /* source: Friederich */ -tendon_slack_length 0.1250 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -#endif -beginexcitation time -(0.0, 0.2) -(1.0, 0.5) -endexcitation -endmuscle - -beginmuscle glut_max2_r -beginpoints --0.1442 0.0013 0.0513 segment pelvis --0.1469 -0.0683 0.0864 segment pelvis --0.0426 -0.0530 0.0293 segment femur_r --0.0156 -0.1016 0.0419 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 546.0 /* source: Brand */ -optimal_fiber_length 0.1470 /* source: Friederich */ -tendon_slack_length 0.1270 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_max3_r -beginpoints --0.1642 -0.0792 0.0074 segment pelvis --0.1622 -0.1215 0.0353 segment pelvis --0.0299 -0.1041 0.0135 segment femur_r --0.0060 -0.1419 0.0411 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 368.0 /* source: Brand */ -optimal_fiber_length 0.1440 /* source: Friederich */ -tendon_slack_length 0.1140 /* source: Delp/Loan */ -pennation_angle 5.0 /* source: Friederich */ -#endif -endmuscle - -/* GLUTEUS MEDIUS */ -beginmuscle glut_med1_r -beginpoints --0.0501 0.0141 0.1159 segment pelvis --0.0218 -0.0117 0.0555 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 546.0 /* units: Newtons, source: Brand */ -optimal_fiber_length 0.0535 /* units: meters, source: Friederich */ -tendon_slack_length 0.0780 /* units: meters, source: Delp */ -pennation_angle 8.0 /* units: degrees, source: Friederich */ -#endif -endmuscle - -beginmuscle glut_med2_r -beginpoints --0.0964 0.0137 0.0792 segment pelvis --0.0258 -0.0058 0.0527 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 382.0 /* source: Brand */ -optimal_fiber_length 0.0845 /* source: Friederich */ -tendon_slack_length 0.0370 /* source: Delp/Loan */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_med3_r -beginpoints --0.1316 -0.0058 0.0598 segment pelvis --0.0309 -0.0047 0.0518 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 435.0 /* source: Brand */ -optimal_fiber_length 0.0646 /* source: Friederich */ -tendon_slack_length 0.0530 /* source: Delp */ -pennation_angle 19.0 /* source: Friederich*/ -#endif -endmuscle - -/* GLUTEUS MINIMUS */ -beginmuscle glut_min1_r -beginpoints --0.0560 -0.0243 0.1006 segment pelvis --0.0072 -0.0104 0.0560 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 180.0 /* source: Brand */ -optimal_fiber_length 0.0680 /* source: Friederich */ -tendon_slack_length 0.0160 /* source: Delp */ -pennation_angle 10.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_min2_r -beginpoints --0.0726 -0.0228 0.0941 segment pelvis --0.0096 -0.0104 0.0560 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 190.0 /* source: Brand */ -optimal_fiber_length 0.0560 /* source: Friederich */ -tendon_slack_length 0.0260 /* source: LOAN (was 0.10) */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_min3_r -beginpoints --0.0927 -0.0226 0.0806 segment pelvis --0.0135 -0.0083 0.0550 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 215.0 /* source: Brand */ -optimal_fiber_length 0.0380 /* source: Friederich */ -tendon_slack_length 0.0510 /* source: Delp */ -pennation_angle 21.0 /* source: Friederich */ -#endif -endmuscle - -/* ADDUCTOR LONGUS */ -beginmuscle add_long_r -beginpoints --0.0434 -0.1058 0.0098 segment pelvis - 0.0050 -0.2111 0.0234 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 418.0 /* source: Wick */ -optimal_fiber_length 0.1380 /* source: Wick */ -tendon_slack_length 0.1105 /* source: Delp/Loan */ -pennation_angle 6.0 /* source: Wick */ -#endif -endmuscle - -/* ADDUCTOR BREVIS */ -beginmuscle add_brev_r -beginpoints --0.0685 -0.1124 0.0164 segment pelvis - 0.0009 -0.1196 0.0294 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 286.0 /* source: Wick */ -optimal_fiber_length 0.1330 /* source: Wick */ -tendon_slack_length 0.0130 /* source: Delp/Loan */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* ADDUCTOR MAGNUS */ -beginmuscle add_mag1_r -beginpoints --0.0825 -0.1337 0.0205 segment pelvis --0.0045 -0.1211 0.0339 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 346.0 /* source: Brand */ -optimal_fiber_length 0.0870 /* source: Friederich */ -tendon_slack_length 0.0600 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle add_mag2_r -beginpoints --0.0924 -0.1355 0.0258 segment pelvis - 0.0054 -0.2285 0.0227 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 312.0 /* source: Brand */ -optimal_fiber_length 0.1210 /* source: Friederich */ -tendon_slack_length 0.1300 /* source: Delp */ -pennation_angle 3.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle add_mag3_r -beginpoints --0.0864 -0.1344 0.0226 segment pelvis - 0.0070 -0.3837 -0.0266 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 444.0 /* source: Brand */ -optimal_fiber_length 0.1310 /* source: Friederich */ -tendon_slack_length 0.2600 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -#endif -endmuscle - -/* PECTINEUS */ -beginmuscle pectineus_r -beginpoints --0.0524 -0.0931 0.0401 segment pelvis --0.0122 -0.0822 0.0253 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 177.0 /* source: Wick */ -optimal_fiber_length 0.1330 /* source: Wick */ -tendon_slack_length 0.0010 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* ILIACUS */ -beginmuscle iliacus_r -beginpoints --0.0784 0.0194 0.0828 segment pelvis --0.0311 -0.0713 0.0801 segment pelvis - 0.0017 -0.0543 0.0057 segment femur_r --0.0193 -0.0621 0.0129 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 429.0 /* source: Brand */ -optimal_fiber_length 0.1000 /* source: Friederich */ -tendon_slack_length 0.0900 /* source: Delp */ -pennation_angle 7.0 /* source: Friederich */ -#endif -endmuscle - -/* PSOAS */ -beginmuscle psoas_r -beginpoints --0.07817 0.05129 0.02070 segment pelvis --0.03310 -0.07330 0.07090 segment pelvis - 0.00160 -0.05070 0.00380 segment femur_r --0.01880 -0.05970 0.01040 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 371.0 /* source: Brand */ -optimal_fiber_length 0.1040 /* source: Friederich */ -tendon_slack_length 0.1130 /* source: Delp/Loan */ -pennation_angle 8.0 /* source: Friederich */ -#endif -endmuscle - -/* QUADRATUS FEMORIS */ -beginmuscle quad_fem_r -beginpoints --0.1236 -0.1314 0.0470 segment pelvis --0.0381 -0.0359 0.0366 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 254.0 /* source: Brand */ -optimal_fiber_length 0.0540 /* source: Friederich */ -tendon_slack_length 0.0240 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -/* GEMELLUS */ -beginmuscle gemellus_r -beginpoints --0.1226 -0.0983 0.0664 segment pelvis --0.0142 -0.0033 0.0443 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 109.0 /* source: Brand */ -optimal_fiber_length 0.0240 /* source: Friederich */ -tendon_slack_length 0.0390 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -/* PIRIFORMIS */ -beginmuscle piri_r -beginpoints --0.1679 -0.0458 0.0291 segment pelvis --0.1333 -0.0531 0.0605 segment pelvis --0.0148 -0.0036 0.0437 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 296.0 /* source: Brand */ -optimal_fiber_length 0.0260 /* source: Friederich */ -tendon_slack_length 0.1060 /* source: Delp/Loan */ -pennation_angle 10.0 /* source: Friederich */ -#endif -endmuscle - -/* TENSOR FASCIAE LATAE */ -beginmuscle TFL_r -beginpoints --0.0404 0.0051 0.1191 segment pelvis - 0.0294 -0.0995 0.0597 segment femur_r - 0.0054 -0.4049 0.0357 segment femur_r - 0.0060 -0.0487 0.0297 segment tibia_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 155.0 /* source: Brand */ -optimal_fiber_length 0.0950 /* source: Friederich */ -tendon_slack_length 0.4250 /* source: Delp */ -pennation_angle 3.0 /* source: Friederich */ -#endif -endmuscle - -/* GRACILIS */ -beginmuscle gracilis_r -beginpoints --0.0656 -0.1201 0.0029 segment pelvis --0.0154 -0.0475 -0.0358 segment tibia_r - 0.0060 -0.0836 -0.0228 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 108.0 /* source: Wick */ -optimal_fiber_length 0.3520 /* source: Wick */ -tendon_slack_length 0.1400 /* source: Delp */ -pennation_angle 3.0 /* source: Wick */ -#endif -endmuscle - -/* SEMIMEMBRANOSUS */ -beginmuscle semimem_r -beginpoints --0.1285 -0.1178 0.0645 segment pelvis --0.0243 -0.0536 -0.0194 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1030.0 /* source: Wick */ -optimal_fiber_length 0.0800 /* source: Wick */ -tendon_slack_length 0.3590 /* source: Delp */ -pennation_angle 15.0 /* source: Wick */ -#endif -endmuscle - -/* SEMITENDINOSUS */ -beginmuscle semiten_r -beginpoints --0.1330 -0.1206 0.0553 segment pelvis --0.0314 -0.0545 -0.0146 segment tibia_r --0.0113 -0.0746 -0.0245 segment tibia_r - 0.0027 -0.0956 -0.0193 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 328.0 /* source: Wick */ -optimal_fiber_length 0.2010 /* source: Wick */ -tendon_slack_length 0.2620 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* BICEPS FEMORIS long head */ -beginmuscle bi_fem_lh_r -beginpoints --0.1337 -0.1164 0.0616 segment pelvis --0.0081 -0.0729 0.0423 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 717.0 /* source: Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.1090 /* source: Wick */ -tendon_slack_length 0.3410 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* BICEPS FEMORIS short head */ -beginmuscle bi_fem_sh_r -beginpoints - 0.0050 -0.2111 0.0234 segment femur_r --0.0101 -0.0725 0.0406 segment tibia_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 402.0 /* source:Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.1730 /* source: Wick */ -tendon_slack_length 0.1000 /* source: Delp */ -pennation_angle 23.0 /* source: Wick */ -#endif -endmuscle - -/* SARTORIUS */ -beginmuscle sartorius_r -beginpoints --0.0296 -0.0180 0.1198 segment pelvis --0.0030 -0.3568 -0.0421 segment femur_r --0.0056 -0.0419 -0.0399 segment tibia_r - 0.0060 -0.0589 -0.0383 segment tibia_r - 0.0243 -0.0840 -0.0252 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 104.0 /* source: Wick */ -optimal_fiber_length 0.5790 /* source: Wick */ -tendon_slack_length 0.0400 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* RECTUS FEMORIS */ -beginmuscle rectus_fem_r -beginpoints --0.0388 -0.0474 0.0918 segment pelvis - 0.0334 -0.4030 0.0019 segment femur_r ranges 1 knee_flex_r (83.65, 150.0) - 0.0121 0.0437 -0.0010 segment patella_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 779.0 /* source: Wick */ -optimal_fiber_length 0.0840 /* source: Wick */ -tendon_slack_length 0.3460 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* VASTUS MEDIALIS */ -beginmuscle vas_med_r -beginpoints -0.0140 -0.2099 0.0188 segment femur_r -0.0356 -0.2769 0.0009 segment femur_r -0.0370 -0.4048 -0.0125 segment femur_r ranges 1 knee_flex_r (69.32, 150.0) -0.0274 -0.4255 -0.0131 segment femur_r ranges 1 knee_flex_r (101.99, 150.0) -0.0063 0.0445 -0.0170 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1294.0 /* source: Wick */ -optimal_fiber_length 0.0890 /* source: Wick */ -tendon_slack_length 0.1260 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* VASTUS INTERMEDIUS */ -beginmuscle vas_int_r -beginpoints -0.0290 -0.1924 0.0310 segment femur_r -0.0335 -0.2084 0.0285 segment femur_r -0.0343 -0.4030 0.0055 segment femur_r ranges 1 knee_flex_r (81.36, 150.0) -0.0058 0.0480 -0.0006 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1365.0 /* source: Wick */ -optimal_fiber_length 0.0870 /* source: Wick */ -tendon_slack_length 0.1360 /* source: Delp */ -pennation_angle 3.0 /* source: Wick */ -#endif -endmuscle - -/* VASTUS LATERALIS */ -beginmuscle vas_lat_r -beginpoints -0.0048 -0.1854 0.0349 segment femur_r -0.0269 -0.2591 0.0409 segment femur_r -0.0361 -0.4030 0.0205 segment femur_r ranges 1 knee_flex_r (69.32, 150.0) -0.0253 -0.4243 0.0184 segment femur_r ranges 1 knee_flex_r (110.0, 150.0) -0.0103 0.0423 0.0141 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1871.0 /* source: Wick */ -optimal_fiber_length 0.0840 /* source: */ -tendon_slack_length 0.1570 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* PATELLAR LIGAMENT */ -beginmuscle pat_lig_r -beginpoints -0.0390 -0.0822 0.0000 segment tibia_r -0.0021 0.0015 0.0001 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1.0 /* source: Bogus */ -optimal_fiber_length 0.0500 /* source: Bogus */ -tendon_slack_length 0.0050 /* source: Bogus */ -pennation_angle 0.0 /* source: Bogus */ -#endif -endmuscle - -/* GASTROC medial head */ -beginmuscle gas_med_r -beginpoints --0.01270 -0.39290 -0.02350 segment femur_r --0.02390 -0.40220 -0.02580 segment femur_r ranges 1 knee_flex_r (-20.00, 44.12) --0.02170 -0.04870 -0.02950 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r knee_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 1113.0 /* source: Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.0450 /* source: Wick */ -tendon_slack_length 0.4080 /* source: Delp */ -pennation_angle 17.0 /* source: Wick */ -#endif -endmuscle - -/* GASTROC lateral head */ -beginmuscle gas_lat_r -beginpoints --0.01550 -0.39460 0.02720 segment femur_r --0.02540 -0.40180 0.02740 segment femur_r ranges 1 knee_flex_r (-20.00, 44.12) --0.02420 -0.04810 0.02350 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r knee_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 488.0 /* source:Brand(Wick lumps the two heads) */ -optimal_fiber_length 0.0640 /* source: Wick */ -tendon_slack_length 0.3850 /* source: Delp */ -pennation_angle 8.0 /* source: Wick */ -#endif -endmuscle - -/* SOLEUS */ -beginmuscle soleus_r -beginpoints --0.00240 -0.15330 0.00710 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 2839.0 /* 4176.0 source: Wick 3340 = 0.8 * 4176 */ -optimal_fiber_length 0.0300 /* Wick val=.025, Brand val= 0.03 */ -tendon_slack_length 0.2680 /* source: Delp */ -pennation_angle 25.0 /* source: Wick */ -#endif -endmuscle - -/* TIBIALIS POSTERIOR */ -beginmuscle tib_post_r -beginpoints --0.0094 -0.1348 0.0019 segment tibia_r --0.0144 -0.4051 -0.0229 segment tibia_r - 0.0417 0.0334 -0.0286 segment foot_r - 0.0772 0.0159 -0.0281 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 1270.0 /* source: Wick */ -optimal_fiber_length 0.0310 /* source: Wick */ -tendon_slack_length 0.3100 /* source: Delp */ -pennation_angle 12.0 /* source: Wick */ -#endif -endmuscle - -/* TIBIALIS ANTERIOR */ -beginmuscle tib_ant_r -beginpoints -0.0179 -0.1624 0.0115 segment tibia_r -0.0329 -0.3951 -0.0177 segment tibia_r -0.1166 0.0178 -0.0305 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 603.0 /* source: Wick */ -optimal_fiber_length 0.0980 /* source: Wick */ -tendon_slack_length 0.2230 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* EXTENSOR DIGITORUM */ -beginmuscle ext_dig_r -beginpoints - 0.00320 -0.13810 0.02760 segment tibia_r - 0.02890 -0.40070 0.00720 segment tibia_r - 0.09220 0.03880 -0.00010 segment foot_r - 0.16160 0.00550 0.01300 segment foot_r - 0.00030 0.00470 0.01530 segment toes_r - 0.04430 -0.00040 0.02500 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 341.0 /* source: Wick */ -optimal_fiber_length 0.1020 /* source: Wick */ -tendon_slack_length 0.3450 /* source: LOAN (was 0.30) */ -pennation_angle 8.0 /* source: Wick */ -#endif -endmuscle - -/* EXTENSOR HALLUCIS */ -beginmuscle ext_hal_r -beginpoints - 0.00120 -0.17670 0.02280 segment tibia_r - 0.03260 -0.39850 -0.00850 segment tibia_r - 0.09700 0.03890 -0.02110 segment foot_r - 0.12930 0.03090 -0.02570 segment foot_r - 0.17778 0.01196 -0.02730 segment foot_r - 0.01217 0.01039 -0.02633 segment toes_r - 0.05628 -0.00131 -0.01852 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 108.0 /* source: Wick */ -optimal_fiber_length 0.1110 /* source: Wick */ -tendon_slack_length 0.3050 /* source: Delp */ -pennation_angle 6.0 /* source: Wick */ -#endif -endmuscle - -/* FLEXOR DIGITORUM */ -beginmuscle flex_dig_r -beginpoints --0.00830 -0.20460 -0.00180 segment tibia_r --0.01540 -0.40510 -0.01960 segment tibia_r - 0.04360 0.03150 -0.02800 segment foot_r - 0.07080 0.01760 -0.02630 segment foot_r - 0.16361 -0.00798 0.01009 segment foot_r --0.00190 -0.00780 0.01470 segment toes_r - 0.02850 -0.00710 0.02150 segment toes_r - 0.04410 -0.00600 0.02420 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 310.0 /* source: Wick */ -optimal_fiber_length 0.0340 /* source: Wick */ -tendon_slack_length 0.4000 /* source: LOAN (was 0.39) */ -pennation_angle 7.0 /* source: Wick */ -#endif -endmuscle - -/* FLEXOR HALLUCIS */ -beginmuscle flex_hal_r -beginpoints --0.00790 -0.23340 0.02440 segment tibia_r --0.01860 -0.40790 -0.01740 segment tibia_r - 0.03740 0.02760 -0.02410 segment foot_r - 0.10380 0.00680 -0.02560 segment foot_r - 0.16956 -0.00501 -0.02761 segment foot_r - 0.00839 -0.00726 -0.02690 segment toes_r - 0.05607 -0.00839 -0.01817 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 322.0 /* source: Wick */ -optimal_fiber_length 0.0430 /* source: Wick */ -tendon_slack_length 0.3800 /* source: LOAN (was 0.40) */ -pennation_angle 10.0 /* source: Wick */ -#endif -endmuscle - -/* PERONEUS BREVIS */ -beginmuscle per_brev_r -beginpoints --0.00700 -0.26460 0.03250 segment tibia_r --0.01980 -0.41840 0.02830 segment tibia_r --0.01440 -0.42950 0.02890 segment tibia_r - 0.04710 0.02700 0.02330 segment foot_r - 0.07569 0.01934 0.02942 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 348.0 /* source: Wick */ -optimal_fiber_length 0.0500 /* source: Wick */ -tendon_slack_length 0.1610 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* PERONEUS LONGUS */ -beginmuscle per_long_r -beginpoints - 0.0005 -0.1568 0.0362 segment tibia_r --0.0207 -0.4205 0.0286 segment tibia_r --0.0162 -0.4319 0.0289 segment tibia_r - 0.0438 0.0230 0.0221 segment foot_r - 0.0681 0.0106 0.0284 segment foot_r - 0.0852 0.0069 0.0118 segment foot_r - 0.1203 0.0085 -0.0184 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 754.0 /* source: Wick */ -optimal_fiber_length 0.0490 /* source: Wick */ -tendon_slack_length 0.3450 /* source: Delp */ -pennation_angle 10.0 /* source: Wick */ -#endif -endmuscle - -/* PERONEUS TERTIUS */ -beginmuscle per_tert_r -beginpoints -0.0010 -0.2804 0.0231 segment tibia_r -0.0229 -0.4069 0.0159 segment tibia_r -0.0857 0.0228 0.0299 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 90.0 /* source: Brand (not reported by wick) */ -optimal_fiber_length 0.0790 /* source: Friederich */ -tendon_slack_length 0.1000 /* source: Delp */ -pennation_angle 13.0 /* source: Friederich */ -#endif -endmuscle - - -/*********** Left Leg ***********/ - -beginmuscle glut_max1_l -beginpoints --0.1253 0.0425 -0.0606 segment pelvis --0.1384 -0.0151 -0.0836 segment pelvis --0.0457 -0.0248 -0.0392 segment femur_l --0.0277 -0.0566 -0.0470 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 382.000 -optimal_fiber_length 0.14200 -tendon_slack_length 0.12500 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle glut_max2_l -beginpoints --0.1442 0.0013 -0.0513 segment pelvis --0.1469 -0.0683 -0.0864 segment pelvis --0.0426 -0.0530 -0.0293 segment femur_l --0.0156 -0.1016 -0.0419 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 546.000 -optimal_fiber_length 0.14700 -tendon_slack_length 0.12700 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle glut_max3_l -beginpoints --0.1642 -0.0792 -0.0074 segment pelvis --0.1622 -0.1215 -0.0353 segment pelvis --0.0299 -0.1041 -0.0135 segment femur_l --0.0060 -0.1419 -0.0411 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 368.000 -optimal_fiber_length 0.14400 -tendon_slack_length 0.11400 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle glut_med1_l -beginpoints --0.0501 0.0141 -0.1159 segment pelvis --0.0218 -0.0117 -0.0555 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 546.000 -optimal_fiber_length 0.05350 -tendon_slack_length 0.07800 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle glut_med2_l -beginpoints --0.0964 0.0137 -0.0792 segment pelvis --0.0258 -0.0058 -0.0527 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 382.000 -optimal_fiber_length 0.08450 -tendon_slack_length 0.03700 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle glut_med3_l -beginpoints --0.1316 -0.0058 -0.0598 segment pelvis --0.0309 -0.0047 -0.0518 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 435.000 -optimal_fiber_length 0.06460 -tendon_slack_length 0.05300 -pennation_angle 19.000 -#endif -endmuscle - -beginmuscle glut_min1_l -beginpoints --0.0560 -0.0243 -0.1006 segment pelvis --0.0072 -0.0104 -0.0560 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 180.000 -optimal_fiber_length 0.06800 -tendon_slack_length 0.01600 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle glut_min2_l -beginpoints --0.0726 -0.0228 -0.0941 segment pelvis --0.0096 -0.0104 -0.0560 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 190.000 -optimal_fiber_length 0.05600 -tendon_slack_length 0.02600 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle glut_min3_l -beginpoints --0.0927 -0.0226 -0.0806 segment pelvis --0.0135 -0.0083 -0.0550 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 215.000 -optimal_fiber_length 0.03800 -tendon_slack_length 0.05100 -pennation_angle 21.000 -#endif -endmuscle - -beginmuscle add_long_l -beginpoints --0.0434 -0.1058 -0.0098 segment pelvis - 0.0050 -0.2111 -0.0234 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 418.000 -optimal_fiber_length 0.13800 -tendon_slack_length 0.11050 -pennation_angle 6.000 -#endif -endmuscle - -beginmuscle add_brev_l -beginpoints --0.0685 -0.1124 -0.0164 segment pelvis - 0.0009 -0.1196 -0.0294 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 286.000 -optimal_fiber_length 0.13300 -tendon_slack_length 0.01300 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle add_mag1_l -beginpoints --0.0825 -0.1337 -0.0205 segment pelvis --0.0045 -0.1211 -0.0339 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 346.000 -optimal_fiber_length 0.08700 -tendon_slack_length 0.06000 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle add_mag2_l -beginpoints --0.0924 -0.1355 -0.0258 segment pelvis - 0.0054 -0.2285 -0.0227 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 312.000 -optimal_fiber_length 0.12100 -tendon_slack_length 0.13000 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle add_mag3_l -beginpoints --0.0864 -0.1344 -0.0226 segment pelvis - 0.0070 -0.3837 -0.0266 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 444.000 -optimal_fiber_length 0.13100 -tendon_slack_length 0.26000 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle pectineus_l -beginpoints --0.0524 -0.0931 -0.0401 segment pelvis --0.0122 -0.0822 -0.0253 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 177.000 -optimal_fiber_length 0.13300 -tendon_slack_length 0.00100 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle iliacus_l -beginpoints --0.0784 0.0194 -0.0828 segment pelvis --0.0311 -0.0713 -0.0801 segment pelvis - 0.0017 -0.0543 -0.0057 segment femur_l --0.0193 -0.0621 -0.0129 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 429.000 -optimal_fiber_length 0.10000 -tendon_slack_length 0.09000 -pennation_angle 7.000 -#endif -endmuscle - -beginmuscle psoas_l -beginpoints --0.07817 0.05129 -0.02070 segment pelvis --0.03310 -0.07330 -0.07090 segment pelvis - 0.00160 -0.05070 -0.00380 segment femur_l --0.01880 -0.05970 -0.01040 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 371.000 -optimal_fiber_length 0.10400 -tendon_slack_length 0.11300 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle quad_fem_l -beginpoints --0.1236 -0.1314 -0.0470 segment pelvis --0.0381 -0.0359 -0.0366 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 254.000 -optimal_fiber_length 0.05400 -tendon_slack_length 0.02400 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle gemellus_l -beginpoints --0.1226 -0.0983 -0.0664 segment pelvis --0.0142 -0.0033 -0.0443 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 109.000 -optimal_fiber_length 0.02400 -tendon_slack_length 0.03900 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle piri_l -beginpoints --0.1679 -0.0458 -0.0291 segment pelvis --0.1333 -0.0531 -0.0605 segment pelvis --0.0148 -0.0036 -0.0437 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 296.000 -optimal_fiber_length 0.02600 -tendon_slack_length 0.10600 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle TFL_l -beginpoints --0.0404 0.0051 -0.1191 segment pelvis - 0.0294 -0.0995 -0.0597 segment femur_l - 0.0054 -0.4049 -0.0357 segment femur_l - 0.0060 -0.0487 -0.0297 segment tibia_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 155.000 -optimal_fiber_length 0.09500 -tendon_slack_length 0.42500 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle gracilis_l -beginpoints --0.0656 -0.1201 -0.0029 segment pelvis --0.0154 -0.0475 0.0358 segment tibia_l - 0.0060 -0.0836 0.0228 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 108.000 -optimal_fiber_length 0.35200 -tendon_slack_length 0.14000 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle semimem_l -beginpoints --0.1285 -0.1178 -0.0645 segment pelvis --0.0243 -0.0536 0.0194 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1030.000 -optimal_fiber_length 0.08000 -tendon_slack_length 0.35900 -pennation_angle 15.000 -#endif -endmuscle - -beginmuscle semiten_l -beginpoints --0.1330 -0.1206 -0.0553 segment pelvis --0.0314 -0.0545 0.0146 segment tibia_l --0.0113 -0.0746 0.0245 segment tibia_l - 0.0027 -0.0956 0.0193 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 328.000 -optimal_fiber_length 0.20100 -tendon_slack_length 0.26200 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle bi_fem_lh_l -beginpoints --0.1337 -0.1164 -0.0616 segment pelvis --0.0081 -0.0729 -0.0423 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 717.000 -optimal_fiber_length 0.10900 -tendon_slack_length 0.34100 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle bi_fem_sh_l -beginpoints - 0.0050 -0.2111 -0.0234 segment femur_l --0.0101 -0.0725 -0.0406 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 402.000 -optimal_fiber_length 0.17300 -tendon_slack_length 0.10000 -pennation_angle 23.000 -#endif -endmuscle - -beginmuscle sartorius_l -beginpoints --0.0296 -0.0180 -0.1198 segment pelvis --0.0030 -0.3568 0.0421 segment femur_l --0.0056 -0.0419 0.0399 segment tibia_l - 0.0060 -0.0589 0.0383 segment tibia_l - 0.0243 -0.0840 0.0252 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 104.000 -optimal_fiber_length 0.57900 -tendon_slack_length 0.04000 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle rectus_fem_l -beginpoints --0.0388 -0.0474 -0.0918 segment pelvis - 0.0334 -0.4030 -0.0019 segment femur_l ranges 1 knee_flex_l (83.65, 150.00) - 0.0121 0.0437 0.0010 segment patella_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 779.000 -optimal_fiber_length 0.08400 -tendon_slack_length 0.34600 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle vas_med_l -beginpoints - 0.01400 -0.20990 -0.01880 segment femur_l - 0.03560 -0.27690 -0.00090 segment femur_l - 0.03700 -0.40480 0.01250 segment femur_l ranges 1 knee_flex_l (69.32, 150.00) - 0.02740 -0.42550 0.01310 segment femur_l ranges 1 knee_flex_l (101.99, 150.00) - 0.00630 0.04450 0.01700 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1294.000 -optimal_fiber_length 0.08900 -tendon_slack_length 0.12600 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle vas_int_l -beginpoints - 0.02900 -0.19240 -0.03100 segment femur_l - 0.03350 -0.20840 -0.02850 segment femur_l - 0.03430 -0.40300 -0.00550 segment femur_l ranges 1 knee_flex_l (81.36, 150.00) - 0.00580 0.04800 0.00060 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1365.000 -optimal_fiber_length 0.08700 -tendon_slack_length 0.13600 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle vas_lat_l -beginpoints - 0.00480 -0.18540 -0.03490 segment femur_l - 0.02690 -0.25910 -0.04090 segment femur_l - 0.03610 -0.40300 -0.02050 segment femur_l ranges 1 knee_flex_l (69.32, 150.00) - 0.02530 -0.42430 -0.01840 segment femur_l ranges 1 knee_flex_l (110.00, 150.00) - 0.01030 0.04230 -0.01410 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1871.000 -optimal_fiber_length 0.08400 -tendon_slack_length 0.15700 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle pat_lig_l -beginpoints - 0.03900 -0.08220 0.00000 segment tibia_l - 0.00210 0.00150 -0.00010 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 100.000 -optimal_fiber_length 0.05000 -tendon_slack_length 0.00500 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle gas_med_l -beginpoints --0.01270 -0.39290 0.02350 segment femur_l --0.02390 -0.40220 0.02580 segment femur_l ranges 1 knee_flex_l (-20.00, 44.12) --0.02170 -0.04870 0.02950 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l knee_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 1113.000 -optimal_fiber_length 0.04500 -tendon_slack_length 0.40800 -pennation_angle 17.000 -#endif -endmuscle - -beginmuscle gas_lat_l -beginpoints --0.01550 -0.39460 -0.02720 segment femur_l --0.02540 -0.40180 -0.02740 segment femur_l ranges 1 knee_flex_l (-20.00, 44.12) --0.02420 -0.04810 -0.02350 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l knee_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 488.000 -optimal_fiber_length 0.06400 -tendon_slack_length 0.38500 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle soleus_l -beginpoints --0.00240 -0.15330 -0.00710 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 2839.000 -optimal_fiber_length 0.03000 -tendon_slack_length 0.26800 -pennation_angle 25.000 -#endif -endmuscle - -beginmuscle tib_post_l -beginpoints --0.00940 -0.13480 -0.00190 segment tibia_l --0.01440 -0.40510 0.02290 segment tibia_l - 0.04170 0.03340 0.02860 segment foot_l - 0.07720 0.01590 0.02810 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 1270.000 -optimal_fiber_length 0.03100 -tendon_slack_length 0.31000 -pennation_angle 12.000 -#endif -endmuscle - -beginmuscle tib_ant_l -beginpoints - 0.01790 -0.16240 -0.01150 segment tibia_l - 0.03290 -0.39510 0.01770 segment tibia_l - 0.11660 0.01780 0.03050 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 603.000 -optimal_fiber_length 0.09800 -tendon_slack_length 0.22300 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle ext_dig_l -beginpoints - 0.00320 -0.13810 -0.02760 segment tibia_l - 0.02890 -0.40070 -0.00720 segment tibia_l - 0.09220 0.03880 0.00010 segment foot_l - 0.16160 0.00550 -0.01300 segment foot_l - 0.00030 0.00470 -0.01530 segment toes_l - 0.04430 -0.00040 -0.02500 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 341.000 -optimal_fiber_length 0.10200 -tendon_slack_length 0.34500 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle ext_hal_l -beginpoints - 0.00120 -0.17670 -0.02280 segment tibia_l - 0.03260 -0.39850 0.00850 segment tibia_l - 0.09700 0.03890 0.02110 segment foot_l - 0.12930 0.03090 0.02570 segment foot_l - 0.17778 0.01196 0.02730 segment foot_l - 0.01217 0.01039 0.02633 segment toes_l - 0.05628 -0.00131 0.01852 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 108.000 -optimal_fiber_length 0.11100 -tendon_slack_length 0.30500 -pennation_angle 6.000 -#endif -endmuscle - -beginmuscle flex_dig_l -beginpoints --0.00830 -0.20460 0.00180 segment tibia_l --0.01540 -0.40510 0.01960 segment tibia_l - 0.04360 0.03150 0.02800 segment foot_l - 0.07080 0.01760 0.02630 segment foot_l - 0.16361 -0.00798 -0.01009 segment foot_l --0.00190 -0.00780 -0.01470 segment toes_l - 0.02850 -0.00710 -0.02150 segment toes_l - 0.04410 -0.00600 -0.02420 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 310.000 -optimal_fiber_length 0.03400 -tendon_slack_length 0.40000 -pennation_angle 7.000 -#endif -endmuscle - -beginmuscle flex_hal_l -beginpoints --0.00790 -0.23340 -0.02440 segment tibia_l --0.01860 -0.40790 0.01740 segment tibia_l - 0.03740 0.02760 0.02410 segment foot_l - 0.10380 0.00680 0.02560 segment foot_l - 0.16956 -0.00501 0.02761 segment foot_l - 0.00839 -0.00726 0.02690 segment toes_l - 0.05607 -0.00839 0.01817 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 322.000 -optimal_fiber_length 0.04300 -tendon_slack_length 0.38000 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle per_brev_l -beginpoints --0.00700 -0.26460 -0.03250 segment tibia_l --0.01980 -0.41840 -0.02830 segment tibia_l --0.01440 -0.42950 -0.02890 segment tibia_l - 0.04710 0.02700 -0.02330 segment foot_l - 0.07569 0.01934 -0.02942 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 348.000 -optimal_fiber_length 0.05000 -tendon_slack_length 0.16100 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle per_long_l -beginpoints - 0.00050 -0.15680 -0.03620 segment tibia_l --0.02070 -0.42050 -0.02860 segment tibia_l --0.01620 -0.43190 -0.02890 segment tibia_l - 0.04380 0.02300 -0.02210 segment foot_l - 0.06810 0.01060 -0.02840 segment foot_l - 0.08520 0.00690 -0.01180 segment foot_l - 0.12030 0.00850 0.01840 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 754.000 -optimal_fiber_length 0.04900 -tendon_slack_length 0.34500 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle per_tert_l -beginpoints - 0.00100 -0.28040 -0.02310 segment tibia_l - 0.02290 -0.40690 -0.01590 segment tibia_l - 0.08570 0.02280 -0.02990 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 90.000 -optimal_fiber_length 0.07900 -tendon_slack_length 0.10000 -pennation_angle 13.000 -#endif -endmuscle - diff --git a/OpenSim/Utilities/simmToOpenSim/test/leg_l.msl b/OpenSim/Utilities/simmToOpenSim/test/leg_l.msl deleted file mode 100644 index ce99acf8b7..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/leg_l.msl +++ /dev/null @@ -1,843 +0,0 @@ -/*********************************************************************** - LEG MODEL - muscle file - - (c) Copyright 2001-5, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the muscle definitions for the left leg model of an average - adult male. It was developed by Scott Delp, Ph.D., Stanford University. - - This model is described in the following papers: - - Delp, S.L., Loan, J.P., Hoy, M.G., Zajac, F.E., Topp E.L., Rosen, J.M.: An - interactive graphics-based model of the lower extremity to study - orthopaedic surgical procedures, IEEE Transactions on Biomedical - Engineering, vol. 37, pp. 757-767, 1990. - - Delp, "Surgery Simulation: A computer graphics system to analyze and design - musculoskeletal reconstructions of the lower extremity," Ph.D. - Dissertation, Stanford University, 1990. - - LOCATION OF REFERENCE FRAMES - The locations of the frames are as follows. - - PELVS: The pelvic reference frame is fixed at the midpoint of the line - connecting the two ASIS. - FEMUR: The femoral frame is fixed at the center of the femoral head - TIBIA: The tibial segment is located at the mid point of the line between - the medial and lateral femoral epicondyles (see note below*). - PATELLA: The patellar frame is located at the most distal point of the patella. - TALUS: The talar frame is located at the midpoint of the line between the - apices of the medial and lateral maleoli. - CALCANEUS: The calcaneal frame is located at the most distal, inferior - point on the posterior surface of the calcaneus. - TOES: The toe frame is located at the base of the second metatarsal - - In the anatomical position, the X-axes point anteriorly, the Y-axes point - superiorly, and the Z-axes point laterally. Also note that this muscle - file must be used with a joint file that has the same reference segments. - - *The coordinates of the tibial tuberosity in the tibia frame are t = - (0.039, -0.082, 0.000). The origin of the tibia reference frame could be - moved to the tibial tuberosity using t. To do this, t would need to be - subtracted from all the muscle points in the tibia segment and from the - vertices in tibia and fibula bone files. Also, the translations in the - femur-tibia joint, the tibia-patella joint, and the tibia-talus joint would - need to be transformed. - -***********************************************************************/ - -#ifndef DEFAULT_MUSCLE -beginmuscle defaultmuscle -begintendonforcelengthcurve -/* (tendon strain, normalized force) */ -(-1.000,0.0000) -(-0.0020,0.0000) -(-0.0010,0.0000) -(0.00000,0.0000) -(0.00131,0.0108) -(0.00281,0.0257) -(0.00431,0.0435) -(0.00581,0.0652) -(0.00731,0.0915) -(0.00881,0.123) -(0.01030,0.161) -(0.01180,0.208) -(0.01230,0.227) -(9.20000,345.0) -(9.20100,345.0) -(9.20200,345.0) -(12.0000,345.0) -endtendonforcelengthcurve - -beginactiveforcelengthcurve -/* (norm length, norm force) */ -(0.000000,0.000000) -(0.401000,0.000000) -(0.402000,0.000000) -(0.403500,0.000000) -(0.527250,0.226667) -(0.628750,0.636667) -(0.718750,0.856667) -(0.861250,0.950000) -(1.045000,0.993333) -(1.217500,0.770000) -(1.438750,0.246667) -(1.618750,0.000000) -(1.620000,0.000000) -(1.621000,0.000000) -(2.000000,0.000000) -endactiveforcelengthcurve - -beginpassiveforcelengthcurve -/* (norm length, norm force) */ -(0.80000,0.000000) -(0.998000,0.000000) -(0.999000,0.000000) -(1.000000,0.000000) -(1.100000,0.035) -(1.200000,0.120) -(1.300000,0.260) -(1.400000,0.550) -(1.500000,1.170) -(1.600000,2.000000) -(1.601000,2.000000) -(1.602000,2.000000) -(2.000000,2.000000) -endpassiveforcelengthcurve - -beginforcevelocitycurve -/* velocity, normalized force */ -(-1.001000,0.000000) -(-1.000000,0.000000) -(-0.950000,0.010417) -(-0.900000,0.021739) -(-0.850000,0.034091) -(-0.800000,0.047619) -(-0.750000,0.062500) -(-0.700000,0.078947) -(-0.650000,0.097222) -(-0.600000,0.117647) -(-0.550000,0.140625) -(-0.500000,0.166667) -(-0.450000,0.196429) -(-0.400000,0.230769) -(-0.350000,0.270833) -(-0.300000,0.318182) -(-0.250000,0.375000) -(-0.200000,0.444444) -(-0.150000,0.531250) -(-0.100000,0.642857) -(-0.050000,0.791667) -(0.000000,1.000000) -(0.050000,1.482014) -(0.100000,1.601571) -(0.150000,1.655791) -(0.200000,1.686739) -(0.250000,1.706751) -(0.300000,1.720753) -(0.350000,1.731099) -(0.400000,1.739055) -(0.450000,1.745365) -(0.500000,1.750490) -(0.550000,1.754736) -(0.600000,1.758312) -(0.650000,1.761364) -(0.700000,1.763999) -(0.750000,1.766298) -(0.800000,1.768321) -(0.850000,1.770115) -(0.900000,1.771717) -(0.950000,1.773155) -(1.000000,1.774455) -endforcevelocitycurve -max_contraction_velocity 10.0 /* fiberlengths/second */ -min_thickness 0.002 -max_thickness 0.005 -min_material muscle_min -max_material muscle_max -endmuscle -#endif - - -beginmuscle glut_max1_l -beginpoints --0.11441 0.02747 -0.06060 segment pelvis --0.11422 -0.03160 -0.08360 segment pelvis --0.0457 -0.0248 -0.0392 segment femur_l --0.0277 -0.0566 -0.0470 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 382.000 -optimal_fiber_length 0.14200 -tendon_slack_length 0.12500 -pennation_angle 5.000 -endmuscle - -beginmuscle glut_max2_l -beginpoints --0.12356 -0.01693 -0.05130 segment pelvis --0.11053 -0.08535 -0.08640 segment pelvis --0.0426 -0.0530 -0.0293 segment femur_l --0.0156 -0.1016 -0.0419 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 546.000 -optimal_fiber_length 0.14700 -tendon_slack_length 0.12700 -pennation_angle 0.000 -endmuscle - -beginmuscle glut_max3_l -beginpoints --0.12494 -0.09986 -0.00740 segment pelvis --0.11347 -0.14063 -0.03530 segment pelvis --0.0299 -0.1041 -0.0135 segment femur_l --0.0060 -0.1419 -0.0411 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 368.000 -optimal_fiber_length 0.14400 -tendon_slack_length 0.11400 -pennation_angle 5.000 -endmuscle - -beginmuscle glut_med1_l -beginpoints --0.03475 0.01671 -0.11590 segment pelvis --0.0218 -0.0117 -0.0555 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 546.000 -optimal_fiber_length 0.05350 -tendon_slack_length 0.07800 -pennation_angle 8.000 -endmuscle - -beginmuscle glut_med2_l -beginpoints --0.07977 0.00591 -0.07920 segment pelvis --0.0258 -0.0058 -0.0527 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 382.000 -optimal_fiber_length 0.08450 -tendon_slack_length 0.03700 -pennation_angle 0.000 -endmuscle - -beginmuscle glut_med3_l -beginpoints --0.10968 -0.02101 -0.05980 segment pelvis --0.0309 -0.0047 -0.0518 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 435.000 -optimal_fiber_length 0.06460 -tendon_slack_length 0.05300 -pennation_angle 19.000 -endmuscle - -beginmuscle glut_min1_l -beginpoints --0.03186 -0.02203 -0.10060 segment pelvis --0.0072 -0.0104 -0.0560 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 180.000 -optimal_fiber_length 0.06800 -tendon_slack_length 0.01600 -pennation_angle 10.000 -endmuscle - -beginmuscle glut_min2_l -beginpoints --0.04837 -0.02430 -0.09410 segment pelvis --0.0096 -0.0104 -0.0560 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 190.000 -optimal_fiber_length 0.05600 -tendon_slack_length 0.02600 -pennation_angle 0.000 -endmuscle - -beginmuscle glut_min3_l -beginpoints --0.06800 -0.02863 -0.08060 segment pelvis --0.0135 -0.0083 -0.0550 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 215.000 -optimal_fiber_length 0.03800 -tendon_slack_length 0.05100 -pennation_angle 21.000 -endmuscle - -beginmuscle add_long_l -beginpoints --0.00125 -0.09861 -0.00980 segment pelvis - 0.0050 -0.2111 -0.0234 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 418.000 -optimal_fiber_length 0.13800 -tendon_slack_length 0.11050 -pennation_angle 6.000 -endmuscle - -beginmuscle add_brev_l -beginpoints --0.02422 -0.11069 -0.01640 segment pelvis - 0.0009 -0.1196 -0.0294 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 286.000 -optimal_fiber_length 0.13300 -tendon_slack_length 0.01300 -pennation_angle 0.000 -endmuscle - -beginmuscle add_mag1_l -beginpoints --0.03307 -0.13459 -0.02050 segment pelvis --0.0045 -0.1211 -0.0339 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 346.000 -optimal_fiber_length 0.08700 -tendon_slack_length 0.06000 -pennation_angle 5.000 -endmuscle - -beginmuscle add_mag2_l -beginpoints --0.04231 -0.13857 -0.02580 segment pelvis - 0.0054 -0.2285 -0.0227 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 312.000 -optimal_fiber_length 0.12100 -tendon_slack_length 0.13000 -pennation_angle 3.000 -endmuscle - -beginmuscle add_mag3_l -beginpoints --0.03671 -0.13615 -0.02260 segment pelvis - 0.0070 -0.3837 -0.0266 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 444.000 -optimal_fiber_length 0.13100 -tendon_slack_length 0.26000 -pennation_angle 5.000 -endmuscle - -beginmuscle pectineus_l -beginpoints --0.01288 -0.08826 -0.04010 segment pelvis --0.0122 -0.0822 -0.0253 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 177.000 -optimal_fiber_length 0.13300 -tendon_slack_length 0.00100 -pennation_angle 0.000 -endmuscle - -beginmuscle iliacus_l -beginpoints --0.06352 0.01551 -0.08280 segment pelvis - 0.00297 -0.06223 -0.08010 segment pelvis - 0.0017 -0.0543 -0.0057 segment femur_l --0.0193 -0.0621 -0.0129 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 429.000 -optimal_fiber_length 0.10000 -tendon_slack_length 0.09000 -pennation_angle 7.000 -endmuscle - -beginmuscle psoas_l -beginpoints --0.07047 0.04663 -0.02070 segment pelvis - 0.00148 -0.06462 -0.07090 segment pelvis - 0.00160 -0.05070 -0.00380 segment femur_l --0.01880 -0.05970 -0.01040 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 371.000 -optimal_fiber_length 0.10400 -tendon_slack_length 0.11300 -pennation_angle 8.000 -endmuscle - -beginmuscle quad_fem_l -beginpoints --0.07364 -0.14159 -0.04700 segment pelvis --0.0381 -0.0359 -0.0366 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 254.000 -optimal_fiber_length 0.05400 -tendon_slack_length 0.02400 -pennation_angle 0.000 -endmuscle - -beginmuscle gemellus_l -beginpoints --0.08011 -0.10912 -0.06640 segment pelvis --0.0142 -0.0033 -0.0443 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 109.000 -optimal_fiber_length 0.02400 -tendon_slack_length 0.03900 -pennation_angle 0.000 -endmuscle - -beginmuscle piri_l -beginpoints --0.13606 -0.06815 -0.02910 segment pelvis --0.10070 -0.06748 -0.06050 segment pelvis --0.0148 -0.0036 -0.0437 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 296.000 -optimal_fiber_length 0.02600 -tendon_slack_length 0.10600 -pennation_angle 10.000 -endmuscle - -beginmuscle TFL_l -beginpoints --0.02327 0.01012 -0.11910 segment pelvis - 0.0294 -0.0995 -0.0597 segment femur_l - 0.0054 -0.4049 -0.0357 segment femur_l - 0.0060 -0.0487 -0.0297 segment tibia_l -endpoints -begingroups - left leg_l hip_l -endgroups -max_force 155.000 -optimal_fiber_length 0.09500 -tendon_slack_length 0.42500 -pennation_angle 3.000 -endmuscle - -beginmuscle gracilis_l -beginpoints --0.01966 -0.11754 -0.00290 segment pelvis --0.0154 -0.0475 0.0358 segment tibia_l - 0.0060 -0.0836 0.0228 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -max_force 108.000 -optimal_fiber_length 0.35200 -tendon_slack_length 0.14000 -pennation_angle 3.000 -endmuscle - -beginmuscle semimem_l -beginpoints --0.08147 -0.12944 -0.06450 segment pelvis --0.0243 -0.0536 0.0194 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -max_force 1030.000 -optimal_fiber_length 0.08000 -tendon_slack_length 0.35900 -pennation_angle 15.000 -endmuscle - -beginmuscle semiten_l -beginpoints --0.08522 -0.13318 -0.05530 segment pelvis --0.0314 -0.0545 0.0146 segment tibia_l --0.0113 -0.0746 0.0245 segment tibia_l - 0.0027 -0.0956 0.0193 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -max_force 328.000 -optimal_fiber_length 0.20100 -tendon_slack_length 0.26200 -pennation_angle 5.000 -endmuscle - -beginmuscle bi_fem_lh_l -beginpoints --0.08685 -0.12925 -0.06160 segment pelvis --0.0081 -0.0729 -0.0423 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -max_force 717.000 -optimal_fiber_length 0.10900 -tendon_slack_length 0.34100 -pennation_angle 0.000 -endmuscle - -beginmuscle bi_fem_sh_l -beginpoints - 0.0050 -0.2111 -0.0234 segment femur_l --0.0101 -0.0725 -0.0406 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -max_force 402.000 -optimal_fiber_length 0.17300 -tendon_slack_length 0.10000 -pennation_angle 23.000 -endmuscle - -beginmuscle sartorius_l -beginpoints --0.00755 -0.00995 -0.11980 segment pelvis --0.0030 -0.3568 0.0421 segment femur_l --0.0056 -0.0419 0.0399 segment tibia_l - 0.0060 -0.0589 0.0383 segment tibia_l - 0.0243 -0.0840 0.0252 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -max_force 104.000 -optimal_fiber_length 0.57900 -tendon_slack_length 0.04000 -pennation_angle 0.000 -endmuscle - -beginmuscle rectus_fem_l -beginpoints --0.00990 -0.04067 -0.09180 segment pelvis - 0.0334 -0.4030 -0.0019 segment femur_l ranges 1 knee_flex_l (83.65, 150.00) - 0.0121 0.0437 0.0010 segment patella_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -max_force 779.000 -optimal_fiber_length 0.08400 -tendon_slack_length 0.34600 -pennation_angle 5.000 -endmuscle - -beginmuscle vas_med_l -beginpoints - 0.01400 -0.20990 -0.01880 segment femur_l - 0.03560 -0.27690 -0.00090 segment femur_l - 0.03700 -0.40480 0.01250 segment femur_l ranges 1 knee_flex_l (69.32, 150.00) - 0.02740 -0.42550 0.01310 segment femur_l ranges 1 knee_flex_l (101.99, 150.00) - 0.00630 0.04450 0.01700 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -max_force 1294.000 -optimal_fiber_length 0.08900 -tendon_slack_length 0.12600 -pennation_angle 5.000 -endmuscle - -beginmuscle vas_int_l -beginpoints - 0.02900 -0.19240 -0.03100 segment femur_l - 0.03350 -0.20840 -0.02850 segment femur_l - 0.03430 -0.40300 -0.00550 segment femur_l ranges 1 knee_flex_l (81.36, 150.00) - 0.00580 0.04800 0.00060 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -max_force 1365.000 -optimal_fiber_length 0.08700 -tendon_slack_length 0.13600 -pennation_angle 3.000 -endmuscle - -beginmuscle vas_lat_l -beginpoints - 0.00480 -0.18540 -0.03490 segment femur_l - 0.02690 -0.25910 -0.04090 segment femur_l - 0.03610 -0.40300 -0.02050 segment femur_l ranges 1 knee_flex_l (69.32, 150.00) - 0.02530 -0.42430 -0.01840 segment femur_l ranges 1 knee_flex_l (110.00, 150.00) - 0.01030 0.04230 -0.01410 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -max_force 1871.000 -optimal_fiber_length 0.08400 -tendon_slack_length 0.15700 -pennation_angle 5.000 -endmuscle - -beginmuscle pat_lig_l -beginpoints - 0.03900 -0.08220 0.00000 segment tibia_l - 0.00210 0.00150 -0.00010 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -max_force 100.000 -optimal_fiber_length 0.05000 -tendon_slack_length 0.00500 -pennation_angle 0.000 -endmuscle - -beginmuscle gas_med_l -beginpoints --0.01270 -0.39290 0.02350 segment femur_l --0.02390 -0.40220 0.02580 segment femur_l ranges 1 knee_flex_l (-20.00, 44.12) --0.02170 -0.04870 0.02950 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l knee_l ankle_l -endgroups -max_force 1113.000 -optimal_fiber_length 0.04500 -tendon_slack_length 0.40800 -pennation_angle 17.000 -endmuscle - -beginmuscle gas_lat_l -beginpoints --0.01550 -0.39460 -0.02720 segment femur_l --0.02540 -0.40180 -0.02740 segment femur_l ranges 1 knee_flex_l (-20.00, 44.12) --0.02420 -0.04810 -0.02350 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l knee_l ankle_l -endgroups -max_force 488.000 -optimal_fiber_length 0.06400 -tendon_slack_length 0.38500 -pennation_angle 8.000 -endmuscle - -beginmuscle soleus_l -beginpoints --0.00240 -0.15330 -0.00710 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 2839.000 -optimal_fiber_length 0.03000 -tendon_slack_length 0.26800 -pennation_angle 25.000 -endmuscle - -beginmuscle tib_post_l -beginpoints --0.00940 -0.13480 -0.00190 segment tibia_l --0.01440 -0.40510 0.02290 segment tibia_l - 0.04170 0.03340 0.02860 segment foot_l - 0.07720 0.01590 0.02810 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 1270.000 -optimal_fiber_length 0.03100 -tendon_slack_length 0.31000 -pennation_angle 12.000 -endmuscle - -beginmuscle tib_ant_l -beginpoints - 0.01790 -0.16240 -0.01150 segment tibia_l - 0.03290 -0.39510 0.01770 segment tibia_l - 0.11660 0.01780 0.03050 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 603.000 -optimal_fiber_length 0.09800 -tendon_slack_length 0.22300 -pennation_angle 5.000 -endmuscle - -beginmuscle ext_dig_l -beginpoints - 0.00320 -0.13810 -0.02760 segment tibia_l - 0.02890 -0.40070 -0.00720 segment tibia_l - 0.09220 0.03880 0.00010 segment foot_l - 0.16160 0.00550 -0.01300 segment foot_l - 0.00030 0.00470 -0.01530 segment toes_l - 0.04430 -0.00040 -0.02500 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 341.000 -optimal_fiber_length 0.10200 -tendon_slack_length 0.34500 -pennation_angle 8.000 -endmuscle - -beginmuscle ext_hal_l -beginpoints - 0.00120 -0.17670 -0.02280 segment tibia_l - 0.03260 -0.39850 0.00850 segment tibia_l - 0.09700 0.03890 0.02110 segment foot_l - 0.12930 0.03090 0.02570 segment foot_l - 0.17778 0.01196 0.02730 segment foot_l - 0.01217 0.01039 0.02633 segment toes_l - 0.05628 -0.00131 0.01852 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 108.000 -optimal_fiber_length 0.11100 -tendon_slack_length 0.30500 -pennation_angle 6.000 -endmuscle - -beginmuscle flex_dig_l -beginpoints --0.00830 -0.20460 0.00180 segment tibia_l --0.01540 -0.40510 0.01960 segment tibia_l - 0.04360 0.03150 0.02800 segment foot_l - 0.07080 0.01760 0.02630 segment foot_l - 0.16361 -0.00798 -0.01009 segment foot_l --0.00190 -0.00780 -0.01470 segment toes_l - 0.02850 -0.00710 -0.02150 segment toes_l - 0.04410 -0.00600 -0.02420 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 310.000 -optimal_fiber_length 0.03400 -tendon_slack_length 0.40000 -pennation_angle 7.000 -endmuscle - -beginmuscle flex_hal_l -beginpoints --0.00790 -0.23340 -0.02440 segment tibia_l --0.01860 -0.40790 0.01740 segment tibia_l - 0.03740 0.02760 0.02410 segment foot_l - 0.10380 0.00680 0.02560 segment foot_l - 0.16956 -0.00501 0.02761 segment foot_l - 0.00839 -0.00726 0.02690 segment toes_l - 0.05607 -0.00839 0.01817 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 322.000 -optimal_fiber_length 0.04300 -tendon_slack_length 0.38000 -pennation_angle 10.000 -endmuscle - -beginmuscle per_brev_l -beginpoints --0.00700 -0.26460 -0.03250 segment tibia_l --0.01980 -0.41840 -0.02830 segment tibia_l --0.01440 -0.42950 -0.02890 segment tibia_l - 0.04710 0.02700 -0.02330 segment foot_l - 0.07569 0.01934 -0.02942 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 348.000 -optimal_fiber_length 0.05000 -tendon_slack_length 0.16100 -pennation_angle 5.000 -endmuscle - -beginmuscle per_long_l -beginpoints - 0.00050 -0.15680 -0.03620 segment tibia_l --0.02070 -0.42050 -0.02860 segment tibia_l --0.01620 -0.43190 -0.02890 segment tibia_l - 0.04380 0.02300 -0.02210 segment foot_l - 0.06810 0.01060 -0.02840 segment foot_l - 0.08520 0.00690 -0.01180 segment foot_l - 0.12030 0.00850 0.01840 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 754.000 -optimal_fiber_length 0.04900 -tendon_slack_length 0.34500 -pennation_angle 10.000 -endmuscle - -beginmuscle per_tert_l -beginpoints - 0.00100 -0.28040 -0.02310 segment tibia_l - 0.02290 -0.40690 -0.01590 segment tibia_l - 0.08570 0.02280 -0.02990 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -max_force 90.000 -optimal_fiber_length 0.07900 -tendon_slack_length 0.10000 -pennation_angle 13.000 -endmuscle diff --git a/OpenSim/Utilities/simmToOpenSim/test/leg_r.msl b/OpenSim/Utilities/simmToOpenSim/test/leg_r.msl deleted file mode 100644 index 070fff128c..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/leg_r.msl +++ /dev/null @@ -1,880 +0,0 @@ -/*********************************************************************** - LEG MODEL - muscle file - - (c) Copyright 2001-5, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the muscle definitions for the right leg model of an average - adult male. It was developed by Scott Delp, Ph.D., Stanford University. - - This model is described in the following papers: - - Delp, S.L., Loan, J.P., Hoy, M.G., Zajac, F.E., Topp E.L., Rosen, J.M.: An - interactive graphics-based model of the lower extremity to study - orthopaedic surgical procedures, IEEE Transactions on Biomedical - Engineering, vol. 37, pp. 757-767, 1990. - - Delp, "Surgery Simulation: A computer graphics system to analyze and design - musculoskeletal reconstructions of the lower extremity," Ph.D. - Dissertation, Stanford University, 1990. - - LOCATION OF REFERENCE FRAMES - The locations of the frames are as follows. - - PELVS: The pelvic reference frame is fixed at the midpoint of the line - connecting the two ASIS. - FEMUR: The femoral frame is fixed at the center of the femoral head - TIBIA: The tibial segment is located at the mid point of the line between - the medial and lateral femoral epicondyles (see note below*). - PATELLA: The patellar frame is located at the most distal point of the patella. - TALUS: The talar frame is located at the midpoint of the line between the - apices of the medial and lateral maleoli. - CALCANEUS: The calcaneal frame is located at the most distal, inferior - point on the posterior surface of the calcaneus. - TOES: The toe frame is located at the base of the second metatarsal - - In the anatomical position, the X-axes point anteriorly, the Y-axes point - superiorly, and the Z-axes point laterally. Also note that this muscle - file must be used with a joint file that has the same reference segments. - - *The coordinates of the tibial tuberosity in the tibia frame are t = - (0.039, -0.082, 0.000). The origin of the tibia reference frame could be - moved to the tibial tuberosity using t. To do this, t would need to be - subtracted from all the muscle points in the tibia segment and from the - vertices in tibia and fibula bone files. Also, the translations in the - femur-tibia joint, the tibia-patella joint, and the tibia-talus joint would - need to be transformed. - -***********************************************************************/ - -#ifndef DEFAULT_MUSCLE -beginmuscle defaultmuscle -begintendonforcelengthcurve -/* (tendon strain, normalized force) */ -(-1.000,0.0000) -(-0.0020,0.0000) -(-0.0010,0.0000) -(0.00000,0.0000) -(0.00131,0.0108) -(0.00281,0.0257) -(0.00431,0.0435) -(0.00581,0.0652) -(0.00731,0.0915) -(0.00881,0.123) -(0.01030,0.161) -(0.01180,0.208) -(0.01230,0.227) -(9.20000,345.0) -(9.20100,345.0) -(9.20200,345.0) -(12.0000,345.0) -endtendonforcelengthcurve - -beginactiveforcelengthcurve -/* (norm length, norm force) */ -(0.000000,0.000000) -(0.401000,0.000000) -(0.402000,0.000000) -(0.403500,0.000000) -(0.527250,0.226667) -(0.628750,0.636667) -(0.718750,0.856667) -(0.861250,0.950000) -(1.045000,0.993333) -(1.217500,0.770000) -(1.438750,0.246667) -(1.618750,0.000000) -(1.620000,0.000000) -(1.621000,0.000000) -(2.000000,0.000000) -endactiveforcelengthcurve - -beginpassiveforcelengthcurve -/* (norm length, norm force) */ -(0.80000,0.000000) -(0.998000,0.000000) -(0.999000,0.000000) -(1.000000,0.000000) -(1.100000,0.035) -(1.200000,0.120) -(1.300000,0.260) -(1.400000,0.550) -(1.500000,1.170) -(1.600000,2.000000) -(1.601000,2.000000) -(1.602000,2.000000) -(2.000000,2.000000) -endpassiveforcelengthcurve - -beginforcevelocitycurve -/* velocity, normalized force */ -(-1.001000,0.000000) -(-1.000000,0.000000) -(-0.950000,0.010417) -(-0.900000,0.021739) -(-0.850000,0.034091) -(-0.800000,0.047619) -(-0.750000,0.062500) -(-0.700000,0.078947) -(-0.650000,0.097222) -(-0.600000,0.117647) -(-0.550000,0.140625) -(-0.500000,0.166667) -(-0.450000,0.196429) -(-0.400000,0.230769) -(-0.350000,0.270833) -(-0.300000,0.318182) -(-0.250000,0.375000) -(-0.200000,0.444444) -(-0.150000,0.531250) -(-0.100000,0.642857) -(-0.050000,0.791667) -(0.000000,1.000000) -(0.050000,1.482014) -(0.100000,1.601571) -(0.150000,1.655791) -(0.200000,1.686739) -(0.250000,1.706751) -(0.300000,1.720753) -(0.350000,1.731099) -(0.400000,1.739055) -(0.450000,1.745365) -(0.500000,1.750490) -(0.550000,1.754736) -(0.600000,1.758312) -(0.650000,1.761364) -(0.700000,1.763999) -(0.750000,1.766298) -(0.800000,1.768321) -(0.850000,1.770115) -(0.900000,1.771717) -(0.950000,1.773155) -(1.000000,1.774455) -endforcevelocitycurve -max_contraction_velocity 10.0 /* fiberlengths/second */ -min_thickness 0.002 -max_thickness 0.005 -min_material muscle_min -max_material muscle_max -endmuscle -#endif - - -/* GLUTEUS MAXIMUS */ -beginmuscle glut_max1_r -beginpoints --0.11441 0.02747 0.06060 segment pelvis --0.11422 -0.03160 0.08360 segment pelvis --0.0457 -0.0248 0.0392 segment femur_r --0.0277 -0.0566 0.0470 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 382.0 /* source: Brand */ -optimal_fiber_length 0.1420 /* source: Friederich */ -tendon_slack_length 0.1250 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -endmuscle - -beginmuscle glut_max2_r -beginpoints --0.12356 -0.01693 0.05130 segment pelvis --0.11053 -0.08535 0.08640 segment pelvis --0.0426 -0.0530 0.0293 segment femur_r --0.0156 -0.1016 0.0419 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 546.0 /* source: Brand */ -optimal_fiber_length 0.1470 /* source: Friederich */ -tendon_slack_length 0.1270 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -endmuscle - -beginmuscle glut_max3_r -beginpoints --0.12494 -0.09986 0.00740 segment pelvis --0.11347 -0.14063 0.03530 segment pelvis --0.0299 -0.1041 0.0135 segment femur_r --0.0060 -0.1419 0.0411 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 368.0 /* source: Brand */ -optimal_fiber_length 0.1440 /* source: Friederich */ -tendon_slack_length 0.1140 /* source: Delp/Loan */ -pennation_angle 5.0 /* source: Friederich */ -endmuscle - -/* GLUTEUS MEDIUS */ -beginmuscle glut_med1_r -beginpoints --0.03475 0.01671 0.11590 segment pelvis --0.0218 -0.0117 0.0555 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 546.0 /* units: Newtons, source: Brand */ -optimal_fiber_length 0.0535 /* units: meters, source: Friederich */ -tendon_slack_length 0.0780 /* units: meters, source: Delp */ -pennation_angle 8.0 /* units: degrees, source: Friederich */ -endmuscle - -beginmuscle glut_med2_r -beginpoints --0.07977 0.00591 0.07920 segment pelvis --0.0258 -0.0058 0.0527 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 382.0 /* source: Brand */ -optimal_fiber_length 0.0845 /* source: Friederich */ -tendon_slack_length 0.0370 /* source: Delp/Loan */ -pennation_angle 0.0 /* source: Friederich */ -endmuscle - -beginmuscle glut_med3_r -beginpoints --0.10968 -0.02101 0.05980 segment pelvis --0.0309 -0.0047 0.0518 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 435.0 /* source: Brand */ -optimal_fiber_length 0.0646 /* source: Friederich */ -tendon_slack_length 0.0530 /* source: Delp */ -pennation_angle 19.0 /* source: Friederich*/ -endmuscle - -/* GLUTEUS MINIMUS */ -beginmuscle glut_min1_r -beginpoints --0.03186 -0.02203 0.10060 segment pelvis --0.0072 -0.0104 0.0560 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 180.0 /* source: Brand */ -optimal_fiber_length 0.0680 /* source: Friederich */ -tendon_slack_length 0.0160 /* source: Delp */ -pennation_angle 10.0 /* source: Friederich */ -endmuscle - -beginmuscle glut_min2_r -beginpoints --0.04837 -0.02430 0.09410 segment pelvis --0.0096 -0.0104 0.0560 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 190.0 /* source: Brand */ -optimal_fiber_length 0.0560 /* source: Friederich */ -tendon_slack_length 0.0260 /* source: LOAN (was 0.10) */ -pennation_angle 0.0 /* source: Friederich */ -endmuscle - -beginmuscle glut_min3_r -beginpoints --0.06800 -0.02863 0.08060 segment pelvis --0.0135 -0.0083 0.0550 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 215.0 /* source: Brand */ -optimal_fiber_length 0.0380 /* source: Friederich */ -tendon_slack_length 0.0510 /* source: Delp */ -pennation_angle 21.0 /* source: Friederich */ -endmuscle - -/* ADDUCTOR LONGUS */ -beginmuscle add_long_r -beginpoints --0.00125 -0.09861 0.00980 segment pelvis - 0.0050 -0.2111 0.0234 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 418.0 /* source: Wick */ -optimal_fiber_length 0.1380 /* source: Wick */ -tendon_slack_length 0.1105 /* source: Delp/Loan */ -pennation_angle 6.0 /* source: Wick */ -endmuscle - -/* ADDUCTOR BREVIS */ -beginmuscle add_brev_r -beginpoints --0.02422 -0.11069 0.01640 segment pelvis - 0.0009 -0.1196 0.0294 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 286.0 /* source: Wick */ -optimal_fiber_length 0.1330 /* source: Wick */ -tendon_slack_length 0.0130 /* source: Delp/Loan */ -pennation_angle 0.0 /* source: Wick */ -endmuscle - -/* ADDUCTOR MAGNUS */ -beginmuscle add_mag1_r -beginpoints --0.03307 -0.13459 0.02050 segment pelvis --0.0045 -0.1211 0.0339 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 346.0 /* source: Brand */ -optimal_fiber_length 0.0870 /* source: Friederich */ -tendon_slack_length 0.0600 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -endmuscle - -beginmuscle add_mag2_r -beginpoints --0.04231 -0.13857 0.02580 segment pelvis - 0.0054 -0.2285 0.0227 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 312.0 /* source: Brand */ -optimal_fiber_length 0.1210 /* source: Friederich */ -tendon_slack_length 0.1300 /* source: Delp */ -pennation_angle 3.0 /* source: Friederich */ -endmuscle - -beginmuscle add_mag3_r -beginpoints --0.03671 -0.13615 0.02260 segment pelvis - 0.0070 -0.3837 -0.0266 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 444.0 /* source: Brand */ -optimal_fiber_length 0.1310 /* source: Friederich */ -tendon_slack_length 0.2600 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -endmuscle - -/* PECTINEUS */ -beginmuscle pectineus_r -beginpoints --0.01288 -0.08826 0.04010 segment pelvis --0.0122 -0.0822 0.0253 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 177.0 /* source: Wick */ -optimal_fiber_length 0.1330 /* source: Wick */ -tendon_slack_length 0.0010 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -endmuscle - -/* ILIACUS */ -beginmuscle iliacus_r -beginpoints --0.06352 0.01551 0.08280 segment pelvis - 0.00297 -0.06223 0.08010 segment pelvis - 0.0017 -0.0543 0.0057 segment femur_r --0.0193 -0.0621 0.0129 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 429.0 /* source: Brand */ -optimal_fiber_length 0.1000 /* source: Friederich */ -tendon_slack_length 0.0900 /* source: Delp */ -pennation_angle 7.0 /* source: Friederich */ -endmuscle - -/* PSOAS */ -beginmuscle psoas_r -beginpoints --0.07047 0.04663 0.02070 segment pelvis - 0.00148 -0.06462 0.07090 segment pelvis - 0.00160 -0.05070 0.00380 segment femur_r --0.01880 -0.05970 0.01040 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 371.0 /* source: Brand */ -optimal_fiber_length 0.1040 /* source: Friederich */ -tendon_slack_length 0.1130 /* source: Delp/Loan */ -pennation_angle 8.0 /* source: Friederich */ -endmuscle - -/* QUADRATUS FEMORIS */ -beginmuscle quad_fem_r -beginpoints --0.07364 -0.14159 0.04700 segment pelvis --0.0381 -0.0359 0.0366 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 254.0 /* source: Brand */ -optimal_fiber_length 0.0540 /* source: Friederich */ -tendon_slack_length 0.0240 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -endmuscle - -/* GEMELLUS */ -beginmuscle gemellus_r -beginpoints --0.08011 -0.10912 0.06640 segment pelvis --0.0142 -0.0033 0.0443 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 109.0 /* source: Brand */ -optimal_fiber_length 0.0240 /* source: Friederich */ -tendon_slack_length 0.0390 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -endmuscle - -/* PIRIFORMIS */ -beginmuscle piri_r -beginpoints --0.13606 -0.06815 0.02910 segment pelvis --0.10070 -0.06748 0.06050 segment pelvis --0.0148 -0.0036 0.0437 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 296.0 /* source: Brand */ -optimal_fiber_length 0.0260 /* source: Friederich */ -tendon_slack_length 0.1060 /* source: Delp/Loan */ -pennation_angle 10.0 /* source: Friederich */ -endmuscle - -/* TENSOR FASCIAE LATAE */ -beginmuscle TFL_r -beginpoints --0.02327 0.01012 0.11910 segment pelvis - 0.0294 -0.0995 0.0597 segment femur_r - 0.0054 -0.4049 0.0357 segment femur_r - 0.0060 -0.0487 0.0297 segment tibia_r -endpoints -begingroups - right leg_r hip_r -endgroups -max_force 155.0 /* source: Brand */ -optimal_fiber_length 0.0950 /* source: Friederich */ -tendon_slack_length 0.4250 /* source: Delp */ -pennation_angle 3.0 /* source: Friederich */ -endmuscle - -/* GRACILIS */ -beginmuscle gracilis_r -beginpoints --0.01966 -0.11754 0.00290 segment pelvis --0.0154 -0.0475 -0.0358 segment tibia_r - 0.0060 -0.0836 -0.0228 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -max_force 108.0 /* source: Wick */ -optimal_fiber_length 0.3520 /* source: Wick */ -tendon_slack_length 0.1400 /* source: Delp */ -pennation_angle 3.0 /* source: Wick */ -endmuscle - -/* SEMIMEMBRANOSUS */ -beginmuscle semimem_r -beginpoints --0.08147 -0.12944 0.06450 segment pelvis --0.0243 -0.0536 -0.0194 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -max_force 1030.0 /* source: Wick */ -optimal_fiber_length 0.3590 /* source: Wick */ -tendon_slack_length 0.0800 /* source: Delp */ -pennation_angle 15.0 /* source: Wick */ -endmuscle - -/* SEMITENDINOSUS */ -beginmuscle semiten_r -beginpoints --0.08522 -0.13318 0.05530 segment pelvis --0.0314 -0.0545 -0.0146 segment tibia_r --0.0113 -0.0746 -0.0245 segment tibia_r - 0.0027 -0.0956 -0.0193 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -max_force 328.0 /* source: Wick */ -optimal_fiber_length 0.2010 /* source: Wick */ -tendon_slack_length 0.2620 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -endmuscle - -/* BICEPS FEMORIS long head */ -beginmuscle bi_fem_lh_r -beginpoints --0.08685 -0.12925 0.06160 segment pelvis --0.0081 -0.0729 0.0423 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -max_force 717.0 /* source: Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.1090 /* source: Wick */ -tendon_slack_length 0.3410 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -endmuscle - -/* BICEPS FEMORIS short head */ -beginmuscle bi_fem_sh_r -beginpoints - 0.0050 -0.2111 0.0234 segment femur_r --0.0101 -0.0725 0.0406 segment tibia_r -endpoints -begingroups - right leg_r knee_r -endgroups -max_force 402.0 /* source:Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.1730 /* source: Wick */ -tendon_slack_length 0.1000 /* source: Delp */ -pennation_angle 23.0 /* source: Wick */ -endmuscle - -/* SARTORIUS */ -beginmuscle sartorius_r -beginpoints --0.00755 -0.00995 0.11980 segment pelvis --0.0030 -0.3568 -0.0421 segment femur_r --0.0056 -0.0419 -0.0399 segment tibia_r - 0.0060 -0.0589 -0.0383 segment tibia_r - 0.0243 -0.0840 -0.0252 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -max_force 104.0 /* source: Wick */ -optimal_fiber_length 0.5790 /* source: Wick */ -tendon_slack_length 0.0400 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -endmuscle - -/* RECTUS FEMORIS */ -beginmuscle rectus_fem_r -beginpoints --0.00990 -0.04067 0.09180 segment pelvis - 0.0334 -0.4030 0.0019 segment femur_r ranges 1 knee_flex_r (83.65, 150.0) - 0.0121 0.0437 -0.0010 segment patella_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -max_force 779.0 /* source: Wick */ -optimal_fiber_length 0.0840 /* source: Wick */ -tendon_slack_length 0.3460 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -endmuscle - -/* VASTUS MEDIALIS */ -beginmuscle vas_med_r -beginpoints -0.0140 -0.2099 0.0188 segment femur_r -0.0356 -0.2769 0.0009 segment femur_r -0.0370 -0.4048 -0.0125 segment femur_r ranges 1 knee_flex_r (69.32, 150.0) -0.0274 -0.4255 -0.0131 segment femur_r ranges 1 knee_flex_r (101.99, 150.0) -0.0063 0.0445 -0.0170 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -max_force 1294.0 /* source: Wick */ -optimal_fiber_length 0.0890 /* source: Wick */ -tendon_slack_length 0.1260 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -endmuscle - -/* VASTUS INTERMEDIUS */ -beginmuscle vas_int_r -beginpoints -0.0290 -0.1924 0.0310 segment femur_r -0.0335 -0.2084 0.0285 segment femur_r -0.0343 -0.4030 0.0055 segment femur_r ranges 1 knee_flex_r (81.36, 150.0) -0.0058 0.0480 -0.0006 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -max_force 1365.0 /* source: Wick */ -optimal_fiber_length 0.0870 /* source: Wick */ -tendon_slack_length 0.1360 /* source: Delp */ -pennation_angle 3.0 /* source: Wick */ -endmuscle - -/* VASTUS LATERALIS */ -beginmuscle vas_lat_r -beginpoints -0.0048 -0.1854 0.0349 segment femur_r -0.0269 -0.2591 0.0409 segment femur_r -0.0361 -0.4030 0.0205 segment femur_r ranges 1 knee_flex_r (69.32, 150.0) -0.0253 -0.4243 0.0184 segment femur_r ranges 1 knee_flex_r (110.0, 150.0) -0.0103 0.0423 0.0141 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -max_force 1871.0 /* source: Wick */ -optimal_fiber_length 0.0840 /* source: */ -tendon_slack_length 0.1570 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -endmuscle - -/* PATELLAR LIGAMENT */ -beginmuscle pat_lig_r -beginpoints -0.0390 -0.0822 0.0000 segment tibia_r -0.0021 0.0015 0.0001 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -max_force 100.0 /* source: Bogus */ -optimal_fiber_length 0.0500 /* source: Bogus */ -tendon_slack_length 0.0050 /* source: Bogus */ -pennation_angle 0.0 /* source: Bogus */ -endmuscle - -/* GASTROC medial head */ -beginmuscle gas_med_r -beginpoints --0.01270 -0.39290 -0.02350 segment femur_r --0.02390 -0.40220 -0.02580 segment femur_r ranges 1 knee_flex_r (-20.00, 44.12) --0.02170 -0.04870 -0.02950 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r knee_r ankle_r -endgroups -max_force 1113.0 /* source: Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.0450 /* source: Wick */ -tendon_slack_length 0.4080 /* source: Delp */ -pennation_angle 17.0 /* source: Wick */ -endmuscle - -/* GASTROC lateral head */ -beginmuscle gas_lat_r -beginpoints --0.01550 -0.39460 0.02720 segment femur_r --0.02540 -0.40180 0.02740 segment femur_r ranges 1 knee_flex_r (-20.00, 44.12) --0.02420 -0.04810 0.02350 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r knee_r ankle_r -endgroups -max_force 488.0 /* source:Brand(Wick lumps the two heads) */ -optimal_fiber_length 0.0640 /* source: Wick */ -tendon_slack_length 0.3850 /* source: Delp */ -pennation_angle 8.0 /* source: Wick */ -endmuscle - -/* SOLEUS */ -beginmuscle soleus_r -beginpoints --0.00240 -0.15330 0.00710 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 2839.0 /* 4176.0 source: Wick 3340 = 0.8 * 4176 */ -optimal_fiber_length 0.0300 /* Wick val=.025, Brand val= 0.03 */ -tendon_slack_length 0.2680 /* source: Delp */ -pennation_angle 25.0 /* source: Wick */ -endmuscle - -/* TIBIALIS POSTERIOR */ -beginmuscle tib_post_r -beginpoints --0.0094 -0.1348 0.0019 segment tibia_r --0.0144 -0.4051 -0.0229 segment tibia_r - 0.0417 0.0334 -0.0286 segment foot_r - 0.0772 0.0159 -0.0281 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 1270.0 /* source: Wick */ -optimal_fiber_length 0.0310 /* source: Wick */ -tendon_slack_length 0.3100 /* source: Delp */ -pennation_angle 12.0 /* source: Wick */ -endmuscle - -/* TIBIALIS ANTERIOR */ -beginmuscle tib_ant_r -beginpoints -0.0179 -0.1624 0.0115 segment tibia_r -0.0329 -0.3951 -0.0177 segment tibia_r -0.1166 0.0178 -0.0305 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 603.0 /* source: Wick */ -optimal_fiber_length 0.0980 /* source: Wick */ -tendon_slack_length 0.2230 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -endmuscle - -/* EXTENSOR DIGITORUM */ -beginmuscle ext_dig_r -beginpoints - 0.00320 -0.13810 0.02760 segment tibia_r - 0.02890 -0.40070 0.00720 segment tibia_r - 0.09220 0.03880 -0.00010 segment foot_r - 0.16160 0.00550 0.01300 segment foot_r - 0.00030 0.00470 0.01530 segment toes_r - 0.04430 -0.00040 0.02500 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 341.0 /* source: Wick */ -optimal_fiber_length 0.1020 /* source: Wick */ -tendon_slack_length 0.3450 /* source: LOAN (was 0.30) */ -pennation_angle 8.0 /* source: Wick */ -endmuscle - -/* EXTENSOR HALLUCIS */ -beginmuscle ext_hal_r -beginpoints - 0.00120 -0.17670 0.02280 segment tibia_r - 0.03260 -0.39850 -0.00850 segment tibia_r - 0.09700 0.03890 -0.02110 segment foot_r - 0.12930 0.03090 -0.02570 segment foot_r - 0.17778 0.01196 -0.02730 segment foot_r - 0.01217 0.01039 -0.02633 segment toes_r - 0.05628 -0.00131 -0.01852 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 108.0 /* source: Wick */ -optimal_fiber_length 0.1110 /* source: Wick */ -tendon_slack_length 0.3050 /* source: Delp */ -pennation_angle 6.0 /* source: Wick */ -endmuscle - -/* FLEXOR DIGITORUM */ -beginmuscle flex_dig_r -beginpoints --0.00830 -0.20460 -0.00180 segment tibia_r --0.01540 -0.40510 -0.01960 segment tibia_r - 0.04360 0.03150 -0.02800 segment foot_r - 0.07080 0.01760 -0.02630 segment foot_r - 0.16361 -0.00798 0.01009 segment foot_r --0.00190 -0.00780 0.01470 segment toes_r - 0.02850 -0.00710 0.02150 segment toes_r - 0.04410 -0.00600 0.02420 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 310.0 /* source: Wick */ -optimal_fiber_length 0.0340 /* source: Wick */ -tendon_slack_length 0.4000 /* source: LOAN (was 0.39) */ -pennation_angle 7.0 /* source: Wick */ -endmuscle - -/* FLEXOR HALLUCIS */ -beginmuscle flex_hal_r -beginpoints --0.00790 -0.23340 0.02440 segment tibia_r --0.01860 -0.40790 -0.01740 segment tibia_r - 0.03740 0.02760 -0.02410 segment foot_r - 0.10380 0.00680 -0.02560 segment foot_r - 0.16956 -0.00501 -0.02761 segment foot_r - 0.00839 -0.00726 -0.02690 segment toes_r - 0.05607 -0.00839 -0.01817 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 322.0 /* source: Wick */ -optimal_fiber_length 0.0430 /* source: Wick */ -tendon_slack_length 0.3800 /* source: LOAN (was 0.40) */ -pennation_angle 10.0 /* source: Wick */ -endmuscle - -/* PERONEUS BREVIS */ -beginmuscle per_brev_r -beginpoints --0.00700 -0.26460 0.03250 segment tibia_r --0.01980 -0.41840 0.02830 segment tibia_r --0.01440 -0.42950 0.02890 segment tibia_r - 0.04710 0.02700 0.02330 segment foot_r - 0.07569 0.01934 0.02942 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 348.0 /* source: Wick */ -optimal_fiber_length 0.0500 /* source: Wick */ -tendon_slack_length 0.1610 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -endmuscle - -/* PERONEUS LONGUS */ -beginmuscle per_long_r -beginpoints - 0.0005 -0.1568 0.0362 segment tibia_r --0.0207 -0.4205 0.0286 segment tibia_r --0.0162 -0.4319 0.0289 segment tibia_r - 0.0438 0.0230 0.0221 segment foot_r - 0.0681 0.0106 0.0284 segment foot_r - 0.0852 0.0069 0.0118 segment foot_r - 0.1203 0.0085 -0.0184 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 754.0 /* source: Wick */ -optimal_fiber_length 0.0490 /* source: Wick */ -tendon_slack_length 0.3450 /* source: Delp */ -pennation_angle 10.0 /* source: Wick */ -endmuscle - -/* PERONEUS TERTIUS */ -beginmuscle per_tert_r -beginpoints -0.0010 -0.2804 0.0231 segment tibia_r -0.0229 -0.4069 0.0159 segment tibia_r -0.0857 0.0228 0.0299 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -max_force 90.0 /* source: Brand (not reported by wick) */ -optimal_fiber_length 0.0790 /* source: Friederich */ -tendon_slack_length 0.1000 /* source: Delp */ -pennation_angle 13.0 /* source: Friederich */ -endmuscle - diff --git a/OpenSim/Utilities/simmToOpenSim/test/meTest.osim b/OpenSim/Utilities/simmToOpenSim/test/meTest.osim deleted file mode 100644 index fb078c066e..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/meTest.osim +++ /dev/null @@ -1,6252 +0,0 @@ - - - - - 0.000000000000 - 10.000000000000 - 4 - 0.100000000000 - 0.002860000000 - 0.100000000000 - 7.667000000000 - 1.459854000000 - - - -1.000000000000 -0.002000000000 -0.001000000000 0.000000000000 0.001310000000 0.002810000000 0.004310000000 0.005810000000 0.007310000000 0.008810000000 0.010300000000 0.011800000000 0.012300000000 9.200000000000 9.201000000000 9.202000000000 12.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.010800000000 0.025700000000 0.043500000000 0.065200000000 0.091500000000 0.123000000000 0.161000000000 0.208000000000 0.227000000000 345.000000000000 345.000000000000 345.000000000000 345.000000000000 - - - - - 0.000000000000 0.401000000000 0.402000000000 0.403500000000 0.527250000000 0.628750000000 0.718750000000 0.861250000000 1.045000000000 1.217500000000 1.438750000000 1.618750000000 1.620000000000 1.621000000000 2.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.226667000000 0.636667000000 0.856667000000 0.950000000000 0.993333000000 0.770000000000 0.246667000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - - - 0.800000000000 0.998000000000 0.999000000000 1.000000000000 1.100000000000 1.200000000000 1.300000000000 1.400000000000 1.500000000000 1.600000000000 1.601000000000 1.602000000000 2.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.035000000000 0.120000000000 0.260000000000 0.550000000000 1.170000000000 2.000000000000 2.000000000000 2.000000000000 2.000000000000 - - - - - -1.001000000000 -1.000000000000 -0.950000000000 -0.900000000000 -0.850000000000 -0.800000000000 -0.750000000000 -0.700000000000 -0.650000000000 -0.600000000000 -0.550000000000 -0.500000000000 -0.450000000000 -0.400000000000 -0.350000000000 -0.300000000000 -0.250000000000 -0.200000000000 -0.150000000000 -0.100000000000 -0.050000000000 0.000000000000 0.050000000000 0.100000000000 0.150000000000 0.200000000000 0.250000000000 0.300000000000 0.350000000000 0.400000000000 0.450000000000 0.500000000000 0.550000000000 0.600000000000 0.650000000000 0.700000000000 0.750000000000 0.800000000000 0.850000000000 0.900000000000 0.950000000000 1.000000000000 - 0.000000000000 0.000000000000 0.010417000000 0.021739000000 0.034091000000 0.047619000000 0.062500000000 0.078947000000 0.097222000000 0.117647000000 0.140625000000 0.166667000000 0.196429000000 0.230769000000 0.270833000000 0.318182000000 0.375000000000 0.444444000000 0.531250000000 0.642857000000 0.791667000000 1.000000000000 1.482014000000 1.601571000000 1.655791000000 1.686739000000 1.706751000000 1.720753000000 1.731099000000 1.739055000000 1.745365000000 1.750490000000 1.754736000000 1.758312000000 1.761364000000 1.763999000000 1.766298000000 1.768321000000 1.770115000000 1.771717000000 1.773155000000 1.774455000000 - - - - - 0.000000000000 - 10.000000000000 - 4 - 0.100000000000 - 0.002860000000 - 0.100000000000 - 7.667000000000 - 1.459854000000 - - - -1.000000000000 -0.002000000000 -0.001000000000 0.000000000000 0.001310000000 0.002810000000 0.004310000000 0.005810000000 0.007310000000 0.008810000000 0.010300000000 0.011800000000 0.012300000000 9.200000000000 9.201000000000 9.202000000000 12.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.010800000000 0.025700000000 0.043500000000 0.065200000000 0.091500000000 0.123000000000 0.161000000000 0.208000000000 0.227000000000 345.000000000000 345.000000000000 345.000000000000 345.000000000000 - - - - - 0.000000000000 0.401000000000 0.402000000000 0.403500000000 0.527250000000 0.628750000000 0.718750000000 0.861250000000 1.045000000000 1.217500000000 1.438750000000 1.618750000000 1.620000000000 1.621000000000 2.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.226667000000 0.636667000000 0.856667000000 0.950000000000 0.993333000000 0.770000000000 0.246667000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - - - 0.800000000000 0.998000000000 0.999000000000 1.000000000000 1.100000000000 1.200000000000 1.300000000000 1.400000000000 1.500000000000 1.600000000000 1.601000000000 1.602000000000 2.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.035000000000 0.120000000000 0.260000000000 0.550000000000 1.170000000000 2.000000000000 2.000000000000 2.000000000000 2.000000000000 - - - - - -1.001000000000 -1.000000000000 -0.950000000000 -0.900000000000 -0.850000000000 -0.800000000000 -0.750000000000 -0.700000000000 -0.650000000000 -0.600000000000 -0.550000000000 -0.500000000000 -0.450000000000 -0.400000000000 -0.350000000000 -0.300000000000 -0.250000000000 -0.200000000000 -0.150000000000 -0.100000000000 -0.050000000000 0.000000000000 0.050000000000 0.100000000000 0.150000000000 0.200000000000 0.250000000000 0.300000000000 0.350000000000 0.400000000000 0.450000000000 0.500000000000 0.550000000000 0.600000000000 0.650000000000 0.700000000000 0.750000000000 0.800000000000 0.850000000000 0.900000000000 0.950000000000 1.000000000000 - 0.000000000000 0.000000000000 0.010417000000 0.021739000000 0.034091000000 0.047619000000 0.062500000000 0.078947000000 0.097222000000 0.117647000000 0.140625000000 0.166667000000 0.196429000000 0.230769000000 0.270833000000 0.318182000000 0.375000000000 0.444444000000 0.531250000000 0.642857000000 0.791667000000 1.000000000000 1.482014000000 1.601571000000 1.655791000000 1.686739000000 1.706751000000 1.720753000000 1.731099000000 1.739055000000 1.745365000000 1.750490000000 1.754736000000 1.758312000000 1.761364000000 1.763999000000 1.766298000000 1.768321000000 1.770115000000 1.771717000000 1.773155000000 1.774455000000 - - - - - radians - N - m - - - - TFL_r add_brev_r add_long_r add_mag1_r add_mag2_r add_mag3_r bi_fem_lh_r bi_fem_sh_r ext_dig_r ext_hal_r flex_dig_r flex_hal_r gas_lat_r gas_med_r gemellus_r glut_max1_r glut_max2_r glut_max3_r glut_med1_r glut_med2_r glut_med3_r glut_min1_r glut_min2_r glut_min3_r gracilis_r iliacus_r pat_lig_r pectineus_r per_brev_r per_long_r per_tert_r piri_r psoas_r quad_fem_r rectus_fem_r sartorius_r semimem_r semiten_r soleus_r test_muscle tib_ant_r tib_post_r vas_int_r vas_lat_r vas_med_r - - - TFL_r add_brev_r add_long_r add_mag1_r add_mag2_r add_mag3_r bi_fem_lh_r bi_fem_sh_r ext_dig_r ext_hal_r flex_dig_r flex_hal_r gas_lat_r gas_med_r gemellus_r glut_max1_r glut_max2_r glut_max3_r glut_med1_r glut_med2_r glut_med3_r glut_min1_r glut_min2_r glut_min3_r gracilis_r iliacus_r pat_lig_r pectineus_r per_brev_r per_long_r per_tert_r piri_r psoas_r quad_fem_r rectus_fem_r sartorius_r semimem_r semiten_r soleus_r test_muscle tib_ant_r tib_post_r vas_int_r vas_lat_r vas_med_r - - - TFL_r add_brev_r add_long_r add_mag1_r add_mag2_r add_mag3_r bi_fem_lh_r gemellus_r glut_max1_r glut_max2_r glut_max3_r glut_med1_r glut_med2_r glut_med3_r glut_min1_r glut_min2_r glut_min3_r gracilis_r iliacus_r pectineus_r piri_r psoas_r quad_fem_r rectus_fem_r sartorius_r semimem_r semiten_r test_muscle - - - bi_fem_lh_r bi_fem_sh_r gas_lat_r gas_med_r gracilis_r pat_lig_r rectus_fem_r sartorius_r semimem_r semiten_r test_muscle vas_int_r vas_lat_r vas_med_r - - - ext_dig_r ext_hal_r flex_dig_r flex_hal_r gas_lat_r gas_med_r per_brev_r per_long_r per_tert_r soleus_r tib_ant_r tib_post_r - - - TFL_l add_brev_l add_long_l add_mag1_l add_mag2_l add_mag3_l bi_fem_lh_l bi_fem_sh_l ext_dig_l ext_hal_l flex_dig_l flex_hal_l gas_lat_l gas_med_l gemellus_l glut_max1_l glut_max2_l glut_max3_l glut_med1_l glut_med2_l glut_med3_l glut_min1_l glut_min2_l glut_min3_l gracilis_l iliacus_l pat_lig_l pectineus_l per_brev_l per_long_l per_tert_l piri_l psoas_l quad_fem_l rectus_fem_l sartorius_l semimem_l semiten_l soleus_l tib_ant_l tib_post_l vas_int_l vas_lat_l vas_med_l - - - TFL_l add_brev_l add_long_l add_mag1_l add_mag2_l add_mag3_l bi_fem_lh_l bi_fem_sh_l ext_dig_l ext_hal_l flex_dig_l flex_hal_l gas_lat_l gas_med_l gemellus_l glut_max1_l glut_max2_l glut_max3_l glut_med1_l glut_med2_l glut_med3_l glut_min1_l glut_min2_l glut_min3_l gracilis_l iliacus_l pat_lig_l pectineus_l per_brev_l per_long_l per_tert_l piri_l psoas_l quad_fem_l rectus_fem_l sartorius_l semimem_l semiten_l soleus_l tib_ant_l tib_post_l vas_int_l vas_lat_l vas_med_l - - - TFL_l add_brev_l add_long_l add_mag1_l add_mag2_l add_mag3_l bi_fem_lh_l bi_fem_sh_l gemellus_l glut_max1_l glut_max2_l glut_max3_l glut_med1_l glut_med2_l glut_med3_l glut_min1_l glut_min2_l glut_min3_l gracilis_l iliacus_l pectineus_l piri_l psoas_l quad_fem_l rectus_fem_l sartorius_l semimem_l semiten_l - - - bi_fem_lh_l bi_fem_sh_l gas_lat_l gas_med_l gracilis_l pat_lig_l rectus_fem_l sartorius_l semimem_l semiten_l vas_int_l vas_lat_l vas_med_l - - - ext_dig_l ext_hal_l flex_dig_l flex_hal_l gas_lat_l gas_med_l per_brev_l per_long_l per_tert_l soleus_l tib_ant_l tib_post_l - - - - - 100.00000000 - ground - 0.000 0.000 0.000 - -1.000 0.000 0.000 - pelvis - -0.0724376 0.00000000 0.00000000 - - - 200.00000000 - ground - 0.000 0.000 0.000 - 0.000 -1.000 0.000 - pelvis - -0.0724376 0.00000000 0.00000000 - - - 100.00000000 - ground - 0.000 0.000 0.000 - 0.000 0.000 -1.000 - pelvis - -0.0724376 0.00000000 0.00000000 - - - 80.00000000 - ground - -1.000 0.000 0.000 - pelvis - - - 80.00000000 - ground - 0.000 -1.000 0.000 - pelvis - - - 80.00000000 - ground - 0.000 0.000 -1.000 - pelvis - - - hip_flex_r - 1.0 - - - hip_add_r - 1.0 - - - hip_rot_r - 1.0 - - - knee_flex_r - 1.0 - - - ankle_flex_r - 1.0 - - - hip_flex_l - 1.0 - - - hip_add_l - 1.0 - - - hip_rot_l - 1.0 - - - knee_flex_l - 1.0 - - - ankle_flex_l - 1.0 - - - - - - -0.038800000000 -0.047400000000 0.091800000000 - pelvis - - - 0.033400000000 -0.403000000000 0.001900000000 - femur_r - knee_flex_r - 1.459967919293 2.617993877991 - - - 0.012100000000 0.043700000000 -0.001000000000 - patella_r - - - - 779.000000000000 - 0.084000000000 - 0.346000000000 - 0.087266462600 - 4 - - - - femur_ell - hybrid - - - femur_cyl - - - femur_sph - - - - - - - - - -0.125300000000 0.042500000000 0.060600000000 - pelvis - - - -0.138400000000 -0.015100000000 0.083600000000 - pelvis - - - -0.045700000000 -0.024800000000 0.039200000000 - femur_r - - - -0.027700000000 -0.056600000000 0.047000000000 - femur_r - - - - 382.000000000000 - 0.142000000000 - 0.125000000000 - 0.087266462600 - 4 - - - - - - -0.144200000000 0.001300000000 0.051300000000 - pelvis - - - -0.146900000000 -0.068300000000 0.086400000000 - pelvis - - - -0.042600000000 -0.053000000000 0.029300000000 - femur_r - - - -0.015600000000 -0.101600000000 0.041900000000 - femur_r - - - - 546.000000000000 - 0.147000000000 - 0.127000000000 - 0.000000000000 - 4 - - - - - - -0.164200000000 -0.079200000000 0.007400000000 - pelvis - - - -0.162200000000 -0.121500000000 0.035300000000 - pelvis - - - -0.029900000000 -0.104100000000 0.013500000000 - femur_r - - - -0.006000000000 -0.141900000000 0.041100000000 - femur_r - - - - 368.000000000000 - 0.144000000000 - 0.114000000000 - 0.087266462600 - 4 - - - - - - -0.050100000000 0.014100000000 0.115900000000 - pelvis - - - -0.021800000000 -0.011700000000 0.055500000000 - femur_r - - - - 546.000000000000 - 0.053500000000 - 0.078000000000 - 0.139626340160 - 4 - - - - - - -0.096400000000 0.013700000000 0.079200000000 - pelvis - - - -0.025800000000 -0.005800000000 0.052700000000 - femur_r - - - - 382.000000000000 - 0.084500000000 - 0.037000000000 - 0.000000000000 - 4 - - - - - - -0.131600000000 -0.005800000000 0.059800000000 - pelvis - - - -0.030900000000 -0.004700000000 0.051800000000 - femur_r - - - - 435.000000000000 - 0.064600000000 - 0.053000000000 - 0.331612557879 - 4 - - - - - - -0.056000000000 -0.024300000000 0.100600000000 - pelvis - - - -0.007200000000 -0.010400000000 0.056000000000 - femur_r - - - - 180.000000000000 - 0.068000000000 - 0.016000000000 - 0.174532925199 - 4 - - - - - - -0.072600000000 -0.022800000000 0.094100000000 - pelvis - - - -0.009600000000 -0.010400000000 0.056000000000 - femur_r - - - - 190.000000000000 - 0.056000000000 - 0.026000000000 - 0.000000000000 - 4 - - - - - - -0.092700000000 -0.022600000000 0.080600000000 - pelvis - - - -0.013500000000 -0.008300000000 0.055000000000 - femur_r - - - - 215.000000000000 - 0.038000000000 - 0.051000000000 - 0.366519142919 - 4 - - - - - - -0.043400000000 -0.105800000000 0.009800000000 - pelvis - - - 0.005000000000 -0.211100000000 0.023400000000 - femur_r - - - - 418.000000000000 - 0.138000000000 - 0.110500000000 - 0.104719755120 - 4 - - - - - - -0.068500000000 -0.112400000000 0.016400000000 - pelvis - - - 0.000900000000 -0.119600000000 0.029400000000 - femur_r - - - - 286.000000000000 - 0.133000000000 - 0.013000000000 - 0.000000000000 - 4 - - - - - - -0.082500000000 -0.133700000000 0.020500000000 - pelvis - - - -0.004500000000 -0.121100000000 0.033900000000 - femur_r - - - - 346.000000000000 - 0.087000000000 - 0.060000000000 - 0.087266462600 - 4 - - - - - - -0.092400000000 -0.135500000000 0.025800000000 - pelvis - - - 0.005400000000 -0.228500000000 0.022700000000 - femur_r - - - - 312.000000000000 - 0.121000000000 - 0.130000000000 - 0.052359877560 - 4 - - - - - - -0.086400000000 -0.134400000000 0.022600000000 - pelvis - - - 0.007000000000 -0.383700000000 -0.026600000000 - femur_r - - - - 444.000000000000 - 0.131000000000 - 0.260000000000 - 0.087266462600 - 4 - - - - - - -0.052400000000 -0.093100000000 0.040100000000 - pelvis - - - -0.012200000000 -0.082200000000 0.025300000000 - femur_r - - - - 177.000000000000 - 0.133000000000 - 0.001000000000 - 0.000000000000 - 4 - - - - - - -0.078400000000 0.019400000000 0.082800000000 - pelvis - - - -0.031100000000 -0.071300000000 0.080100000000 - pelvis - - - 0.001700000000 -0.054300000000 0.005700000000 - femur_r - - - -0.019300000000 -0.062100000000 0.012900000000 - femur_r - - - - 429.000000000000 - 0.100000000000 - 0.090000000000 - 0.122173047640 - 4 - - - - - - -0.078170000000 0.051290000000 0.020700000000 - pelvis - - - -0.033100000000 -0.073300000000 0.070900000000 - pelvis - - - 0.001600000000 -0.050700000000 0.003800000000 - femur_r - - - -0.018800000000 -0.059700000000 0.010400000000 - femur_r - - - - 371.000000000000 - 0.104000000000 - 0.113000000000 - 0.139626340160 - 4 - - - - - - -0.123600000000 -0.131400000000 0.047000000000 - pelvis - - - -0.038100000000 -0.035900000000 0.036600000000 - femur_r - - - - 254.000000000000 - 0.054000000000 - 0.024000000000 - 0.000000000000 - 4 - - - - - - -0.122600000000 -0.098300000000 0.066400000000 - pelvis - - - -0.014200000000 -0.003300000000 0.044300000000 - femur_r - - - - 109.000000000000 - 0.024000000000 - 0.039000000000 - 0.000000000000 - 4 - - - - - - -0.167900000000 -0.045800000000 0.029100000000 - pelvis - - - -0.133300000000 -0.053100000000 0.060500000000 - pelvis - - - -0.014800000000 -0.003600000000 0.043700000000 - femur_r - - - - 296.000000000000 - 0.026000000000 - 0.106000000000 - 0.174532925199 - 4 - - - - - - -0.040400000000 0.005100000000 0.119100000000 - pelvis - - - 0.029400000000 -0.099500000000 0.059700000000 - femur_r - - - 0.005400000000 -0.404900000000 0.035700000000 - femur_r - - - 0.006000000000 -0.048700000000 0.029700000000 - tibia_r - - - - 155.000000000000 - 0.095000000000 - 0.425000000000 - 0.052359877560 - 4 - - - - - - -0.065600000000 -0.120100000000 0.002900000000 - pelvis - - - -0.015400000000 -0.047500000000 -0.035800000000 - tibia_r - - - 0.006000000000 -0.083600000000 -0.022800000000 - tibia_r - - - - 108.000000000000 - 0.352000000000 - 0.140000000000 - 0.052359877560 - 4 - - - - - - -0.128500000000 -0.117800000000 0.064500000000 - pelvis - - - -0.024300000000 -0.053600000000 -0.019400000000 - tibia_r - - - - 1030.000000000000 - 0.080000000000 - 0.359000000000 - 0.261799387799 - 4 - - - - - - -0.133000000000 -0.120600000000 0.055300000000 - pelvis - - - -0.031400000000 -0.054500000000 -0.014600000000 - tibia_r - - - -0.011300000000 -0.074600000000 -0.024500000000 - tibia_r - - - 0.002700000000 -0.095600000000 -0.019300000000 - tibia_r - - - - 328.000000000000 - 0.201000000000 - 0.262000000000 - 0.087266462600 - 4 - - - - - - -0.133700000000 -0.116400000000 0.061600000000 - pelvis - - - -0.008100000000 -0.072900000000 0.042300000000 - tibia_r - - - - 717.000000000000 - 0.109000000000 - 0.341000000000 - 0.000000000000 - 4 - - - - - - 0.005000000000 -0.211100000000 0.023400000000 - femur_r - - - -0.010100000000 -0.072500000000 0.040600000000 - tibia_r - - - - 402.000000000000 - 0.173000000000 - 0.100000000000 - 0.401425727959 - 4 - - - - - - -0.029600000000 -0.018000000000 0.119800000000 - pelvis - - - -0.003000000000 -0.356800000000 -0.042100000000 - femur_r - - - -0.005600000000 -0.041900000000 -0.039900000000 - tibia_r - - - 0.006000000000 -0.058900000000 -0.038300000000 - tibia_r - - - 0.024300000000 -0.084000000000 -0.025200000000 - tibia_r - - - - 104.000000000000 - 0.579000000000 - 0.040000000000 - 0.000000000000 - 4 - - - - - - -0.038800000000 -0.047400000000 0.091800000000 - pelvis - - - 0.033400000000 -0.403000000000 0.001900000000 - femur_r - knee_flex_r - 1.459967919293 2.617993877991 - - - 0.012100000000 0.043700000000 -0.001000000000 - patella_r - - - - 779.000000000000 - 0.084000000000 - 0.346000000000 - 0.087266462600 - 4 - - - - - - 0.014000000000 -0.209900000000 0.018800000000 - femur_r - - - 0.035600000000 -0.276900000000 0.000900000000 - femur_r - - - 0.037000000000 -0.404800000000 -0.012500000000 - femur_r - knee_flex_r - 1.209862237482 2.617993877991 - - - 0.027400000000 -0.425500000000 -0.013100000000 - femur_r - knee_flex_r - 1.780061304109 2.617993877991 - - - 0.006300000000 0.044500000000 -0.017000000000 - patella_r - - - - 1294.000000000000 - 0.089000000000 - 0.126000000000 - 0.087266462600 - 4 - - - - - - 0.029000000000 -0.192400000000 0.031000000000 - femur_r - - - 0.033500000000 -0.208400000000 0.028500000000 - femur_r - - - 0.034300000000 -0.403000000000 0.005500000000 - femur_r - knee_flex_r - 1.419999879423 2.617993877991 - - - 0.005800000000 0.048000000000 -0.000600000000 - patella_r - - - - 1365.000000000000 - 0.087000000000 - 0.136000000000 - 0.052359877560 - 4 - - - - - - 0.004800000000 -0.185400000000 0.034900000000 - femur_r - - - 0.026900000000 -0.259100000000 0.040900000000 - femur_r - - - 0.036100000000 -0.403000000000 0.020500000000 - femur_r - knee_flex_r - 1.209862237482 2.617993877991 - - - 0.025300000000 -0.424300000000 0.018400000000 - femur_r - knee_flex_r - 1.919862177194 2.617993877991 - - - 0.010300000000 0.042300000000 0.014100000000 - patella_r - - - - 1871.000000000000 - 0.084000000000 - 0.157000000000 - 0.087266462600 - 4 - - - - - - 0.039000000000 -0.082200000000 0.000000000000 - tibia_r - - - 0.002100000000 0.001500000000 0.000100000000 - patella_r - - - - 1.000000000000 - 0.050000000000 - 0.005000000000 - 0.000000000000 - 4 - - - - - - -0.012700000000 -0.392900000000 -0.023500000000 - femur_r - - - -0.023900000000 -0.402200000000 -0.025800000000 - femur_r - knee_flex_r - -0.349065850399 0.770039265980 - - - -0.021700000000 -0.048700000000 -0.029500000000 - tibia_r - - - 0.003360000000 0.032040000000 -0.006340000000 - foot_r - - - - 1113.000000000000 - 0.045000000000 - 0.408000000000 - 0.296705972839 - 4 - - - - - - -0.015500000000 -0.394600000000 0.027200000000 - femur_r - - - -0.025400000000 -0.401800000000 0.027400000000 - femur_r - knee_flex_r - -0.349065850399 0.770039265980 - - - -0.024200000000 -0.048100000000 0.023500000000 - tibia_r - - - 0.003360000000 0.032040000000 -0.006340000000 - foot_r - - - - 488.000000000000 - 0.064000000000 - 0.385000000000 - 0.139626340160 - 4 - - - - - - -0.002400000000 -0.153300000000 0.007100000000 - tibia_r - - - 0.003360000000 0.032040000000 -0.006340000000 - foot_r - - - - 2839.000000000000 - 0.030000000000 - 0.268000000000 - 0.436332312999 - 4 - - - - - - -0.009400000000 -0.134800000000 0.001900000000 - tibia_r - - - -0.014400000000 -0.405100000000 -0.022900000000 - tibia_r - - - 0.041700000000 0.033400000000 -0.028600000000 - foot_r - - - 0.077200000000 0.015900000000 -0.028100000000 - foot_r - - - - 1270.000000000000 - 0.031000000000 - 0.310000000000 - 0.209439510239 - 4 - - - - - - 0.017900000000 -0.162400000000 0.011500000000 - tibia_r - - - 0.032900000000 -0.395100000000 -0.017700000000 - tibia_r - - - 0.116600000000 0.017800000000 -0.030500000000 - foot_r - - - - 603.000000000000 - 0.098000000000 - 0.223000000000 - 0.087266462600 - 4 - - - - - - 0.003200000000 -0.138100000000 0.027600000000 - tibia_r - - - 0.028900000000 -0.400700000000 0.007200000000 - tibia_r - - - 0.092200000000 0.038800000000 -0.000100000000 - foot_r - - - 0.161600000000 0.005500000000 0.013000000000 - foot_r - - - 0.000300000000 0.004700000000 0.015300000000 - toes_r - - - 0.044300000000 -0.000400000000 0.025000000000 - toes_r - - - - 341.000000000000 - 0.102000000000 - 0.345000000000 - 0.139626340160 - 4 - - - - - - 0.001200000000 -0.176700000000 0.022800000000 - tibia_r - - - 0.032600000000 -0.398500000000 -0.008500000000 - tibia_r - - - 0.097000000000 0.038900000000 -0.021100000000 - foot_r - - - 0.129300000000 0.030900000000 -0.025700000000 - foot_r - - - 0.177780000000 0.011960000000 -0.027300000000 - foot_r - - - 0.012170000000 0.010390000000 -0.026330000000 - toes_r - - - 0.056280000000 -0.001310000000 -0.018520000000 - toes_r - - - - 108.000000000000 - 0.111000000000 - 0.305000000000 - 0.104719755120 - 4 - - - - - - -0.008300000000 -0.204600000000 -0.001800000000 - tibia_r - - - -0.015400000000 -0.405100000000 -0.019600000000 - tibia_r - - - 0.043600000000 0.031500000000 -0.028000000000 - foot_r - - - 0.070800000000 0.017600000000 -0.026300000000 - foot_r - - - 0.163610000000 -0.007980000000 0.010090000000 - foot_r - - - -0.001900000000 -0.007800000000 0.014700000000 - toes_r - - - 0.028500000000 -0.007100000000 0.021500000000 - toes_r - - - 0.044100000000 -0.006000000000 0.024200000000 - toes_r - - - - 310.000000000000 - 0.034000000000 - 0.400000000000 - 0.122173047640 - 4 - - - - - - -0.007900000000 -0.233400000000 0.024400000000 - tibia_r - - - -0.018600000000 -0.407900000000 -0.017400000000 - tibia_r - - - 0.037400000000 0.027600000000 -0.024100000000 - foot_r - - - 0.103800000000 0.006800000000 -0.025600000000 - foot_r - - - 0.169560000000 -0.005010000000 -0.027610000000 - foot_r - - - 0.008390000000 -0.007260000000 -0.026900000000 - toes_r - - - 0.056070000000 -0.008390000000 -0.018170000000 - toes_r - - - - 322.000000000000 - 0.043000000000 - 0.380000000000 - 0.174532925199 - 4 - - - - - - -0.007000000000 -0.264600000000 0.032500000000 - tibia_r - - - -0.019800000000 -0.418400000000 0.028300000000 - tibia_r - - - -0.014400000000 -0.429500000000 0.028900000000 - tibia_r - - - 0.047100000000 0.027000000000 0.023300000000 - foot_r - - - 0.075690000000 0.019340000000 0.029420000000 - foot_r - - - - 348.000000000000 - 0.050000000000 - 0.161000000000 - 0.087266462600 - 4 - - - - - - 0.000500000000 -0.156800000000 0.036200000000 - tibia_r - - - -0.020700000000 -0.420500000000 0.028600000000 - tibia_r - - - -0.016200000000 -0.431900000000 0.028900000000 - tibia_r - - - 0.043800000000 0.023000000000 0.022100000000 - foot_r - - - 0.068100000000 0.010600000000 0.028400000000 - foot_r - - - 0.085200000000 0.006900000000 0.011800000000 - foot_r - - - 0.120300000000 0.008500000000 -0.018400000000 - foot_r - - - - 754.000000000000 - 0.049000000000 - 0.345000000000 - 0.174532925199 - 4 - - - - - - 0.001000000000 -0.280400000000 0.023100000000 - tibia_r - - - 0.022900000000 -0.406900000000 0.015900000000 - tibia_r - - - 0.085700000000 0.022800000000 0.029900000000 - foot_r - - - - 90.000000000000 - 0.079000000000 - 0.100000000000 - 0.226892802759 - 4 - - - - - - -0.125300000000 0.042500000000 -0.060600000000 - pelvis - - - -0.138400000000 -0.015100000000 -0.083600000000 - pelvis - - - -0.045700000000 -0.024800000000 -0.039200000000 - femur_l - - - -0.027700000000 -0.056600000000 -0.047000000000 - femur_l - - - - 382.000000000000 - 0.142000000000 - 0.125000000000 - 0.087266462600 - 4 - - - - - - -0.144200000000 0.001300000000 -0.051300000000 - pelvis - - - -0.146900000000 -0.068300000000 -0.086400000000 - pelvis - - - -0.042600000000 -0.053000000000 -0.029300000000 - femur_l - - - -0.015600000000 -0.101600000000 -0.041900000000 - femur_l - - - - 546.000000000000 - 0.147000000000 - 0.127000000000 - 0.000000000000 - 4 - - - - - - -0.164200000000 -0.079200000000 -0.007400000000 - pelvis - - - -0.162200000000 -0.121500000000 -0.035300000000 - pelvis - - - -0.029900000000 -0.104100000000 -0.013500000000 - femur_l - - - -0.006000000000 -0.141900000000 -0.041100000000 - femur_l - - - - 368.000000000000 - 0.144000000000 - 0.114000000000 - 0.087266462600 - 4 - - - - - - -0.050100000000 0.014100000000 -0.115900000000 - pelvis - - - -0.021800000000 -0.011700000000 -0.055500000000 - femur_l - - - - 546.000000000000 - 0.053500000000 - 0.078000000000 - 0.139626340160 - 4 - - - - - - -0.096400000000 0.013700000000 -0.079200000000 - pelvis - - - -0.025800000000 -0.005800000000 -0.052700000000 - femur_l - - - - 382.000000000000 - 0.084500000000 - 0.037000000000 - 0.000000000000 - 4 - - - - - - -0.131600000000 -0.005800000000 -0.059800000000 - pelvis - - - -0.030900000000 -0.004700000000 -0.051800000000 - femur_l - - - - 435.000000000000 - 0.064600000000 - 0.053000000000 - 0.331612557879 - 4 - - - - - - -0.056000000000 -0.024300000000 -0.100600000000 - pelvis - - - -0.007200000000 -0.010400000000 -0.056000000000 - femur_l - - - - 180.000000000000 - 0.068000000000 - 0.016000000000 - 0.174532925199 - 4 - - - - - - -0.072600000000 -0.022800000000 -0.094100000000 - pelvis - - - -0.009600000000 -0.010400000000 -0.056000000000 - femur_l - - - - 190.000000000000 - 0.056000000000 - 0.026000000000 - 0.000000000000 - 4 - - - - - - -0.092700000000 -0.022600000000 -0.080600000000 - pelvis - - - -0.013500000000 -0.008300000000 -0.055000000000 - femur_l - - - - 215.000000000000 - 0.038000000000 - 0.051000000000 - 0.366519142919 - 4 - - - - - - -0.043400000000 -0.105800000000 -0.009800000000 - pelvis - - - 0.005000000000 -0.211100000000 -0.023400000000 - femur_l - - - - 418.000000000000 - 0.138000000000 - 0.110500000000 - 0.104719755120 - 4 - - - - - - -0.068500000000 -0.112400000000 -0.016400000000 - pelvis - - - 0.000900000000 -0.119600000000 -0.029400000000 - femur_l - - - - 286.000000000000 - 0.133000000000 - 0.013000000000 - 0.000000000000 - 4 - - - - - - -0.082500000000 -0.133700000000 -0.020500000000 - pelvis - - - -0.004500000000 -0.121100000000 -0.033900000000 - femur_l - - - - 346.000000000000 - 0.087000000000 - 0.060000000000 - 0.087266462600 - 4 - - - - - - -0.092400000000 -0.135500000000 -0.025800000000 - pelvis - - - 0.005400000000 -0.228500000000 -0.022700000000 - femur_l - - - - 312.000000000000 - 0.121000000000 - 0.130000000000 - 0.052359877560 - 4 - - - - - - -0.086400000000 -0.134400000000 -0.022600000000 - pelvis - - - 0.007000000000 -0.383700000000 -0.026600000000 - femur_l - - - - 444.000000000000 - 0.131000000000 - 0.260000000000 - 0.087266462600 - 4 - - - - - - -0.052400000000 -0.093100000000 -0.040100000000 - pelvis - - - -0.012200000000 -0.082200000000 -0.025300000000 - femur_l - - - - 177.000000000000 - 0.133000000000 - 0.001000000000 - 0.000000000000 - 4 - - - - - - -0.078400000000 0.019400000000 -0.082800000000 - pelvis - - - -0.031100000000 -0.071300000000 -0.080100000000 - pelvis - - - 0.001700000000 -0.054300000000 -0.005700000000 - femur_l - - - -0.019300000000 -0.062100000000 -0.012900000000 - femur_l - - - - 429.000000000000 - 0.100000000000 - 0.090000000000 - 0.122173047640 - 4 - - - - - - -0.078170000000 0.051290000000 -0.020700000000 - pelvis - - - -0.033100000000 -0.073300000000 -0.070900000000 - pelvis - - - 0.001600000000 -0.050700000000 -0.003800000000 - femur_l - - - -0.018800000000 -0.059700000000 -0.010400000000 - femur_l - - - - 371.000000000000 - 0.104000000000 - 0.113000000000 - 0.139626340160 - 4 - - - - - - -0.123600000000 -0.131400000000 -0.047000000000 - pelvis - - - -0.038100000000 -0.035900000000 -0.036600000000 - femur_l - - - - 254.000000000000 - 0.054000000000 - 0.024000000000 - 0.000000000000 - 4 - - - - - - -0.122600000000 -0.098300000000 -0.066400000000 - pelvis - - - -0.014200000000 -0.003300000000 -0.044300000000 - femur_l - - - - 109.000000000000 - 0.024000000000 - 0.039000000000 - 0.000000000000 - 4 - - - - - - -0.167900000000 -0.045800000000 -0.029100000000 - pelvis - - - -0.133300000000 -0.053100000000 -0.060500000000 - pelvis - - - -0.014800000000 -0.003600000000 -0.043700000000 - femur_l - - - - 296.000000000000 - 0.026000000000 - 0.106000000000 - 0.174532925199 - 4 - - - - - - -0.040400000000 0.005100000000 -0.119100000000 - pelvis - - - 0.029400000000 -0.099500000000 -0.059700000000 - femur_l - - - 0.005400000000 -0.404900000000 -0.035700000000 - femur_l - - - 0.006000000000 -0.048700000000 -0.029700000000 - tibia_l - - - - 155.000000000000 - 0.095000000000 - 0.425000000000 - 0.052359877560 - 4 - - - - - - -0.065600000000 -0.120100000000 -0.002900000000 - pelvis - - - -0.015400000000 -0.047500000000 0.035800000000 - tibia_l - - - 0.006000000000 -0.083600000000 0.022800000000 - tibia_l - - - - 108.000000000000 - 0.352000000000 - 0.140000000000 - 0.052359877560 - 4 - - - - - - -0.128500000000 -0.117800000000 -0.064500000000 - pelvis - - - -0.024300000000 -0.053600000000 0.019400000000 - tibia_l - - - - 1030.000000000000 - 0.080000000000 - 0.359000000000 - 0.261799387799 - 4 - - - - - - -0.133000000000 -0.120600000000 -0.055300000000 - pelvis - - - -0.031400000000 -0.054500000000 0.014600000000 - tibia_l - - - -0.011300000000 -0.074600000000 0.024500000000 - tibia_l - - - 0.002700000000 -0.095600000000 0.019300000000 - tibia_l - - - - 328.000000000000 - 0.201000000000 - 0.262000000000 - 0.087266462600 - 4 - - - - - - -0.133700000000 -0.116400000000 -0.061600000000 - pelvis - - - -0.008100000000 -0.072900000000 -0.042300000000 - tibia_l - - - - 717.000000000000 - 0.109000000000 - 0.341000000000 - 0.000000000000 - 4 - - - - - - 0.005000000000 -0.211100000000 -0.023400000000 - femur_l - - - -0.010100000000 -0.072500000000 -0.040600000000 - tibia_l - - - - 402.000000000000 - 0.173000000000 - 0.100000000000 - 0.401425727959 - 4 - - - - - - -0.029600000000 -0.018000000000 -0.119800000000 - pelvis - - - -0.003000000000 -0.356800000000 0.042100000000 - femur_l - - - -0.005600000000 -0.041900000000 0.039900000000 - tibia_l - - - 0.006000000000 -0.058900000000 0.038300000000 - tibia_l - - - 0.024300000000 -0.084000000000 0.025200000000 - tibia_l - - - - 104.000000000000 - 0.579000000000 - 0.040000000000 - 0.000000000000 - 4 - - - - - - -0.038800000000 -0.047400000000 -0.091800000000 - pelvis - - - 0.033400000000 -0.403000000000 -0.001900000000 - femur_l - knee_flex_l - 1.459967919293 2.617993877991 - - - 0.012100000000 0.043700000000 0.001000000000 - patella_l - - - - 779.000000000000 - 0.084000000000 - 0.346000000000 - 0.087266462600 - 4 - - - - - - 0.014000000000 -0.209900000000 -0.018800000000 - femur_l - - - 0.035600000000 -0.276900000000 -0.000900000000 - femur_l - - - 0.037000000000 -0.404800000000 0.012500000000 - femur_l - knee_flex_l - 1.209862237482 2.617993877991 - - - 0.027400000000 -0.425500000000 0.013100000000 - femur_l - knee_flex_l - 1.780061304109 2.617993877991 - - - 0.006300000000 0.044500000000 0.017000000000 - patella_l - - - - 1294.000000000000 - 0.089000000000 - 0.126000000000 - 0.087266462600 - 4 - - - - - - 0.029000000000 -0.192400000000 -0.031000000000 - femur_l - - - 0.033500000000 -0.208400000000 -0.028500000000 - femur_l - - - 0.034300000000 -0.403000000000 -0.005500000000 - femur_l - knee_flex_l - 1.419999879423 2.617993877991 - - - 0.005800000000 0.048000000000 0.000600000000 - patella_l - - - - 1365.000000000000 - 0.087000000000 - 0.136000000000 - 0.052359877560 - 4 - - - - - - 0.004800000000 -0.185400000000 -0.034900000000 - femur_l - - - 0.026900000000 -0.259100000000 -0.040900000000 - femur_l - - - 0.036100000000 -0.403000000000 -0.020500000000 - femur_l - knee_flex_l - 1.209862237482 2.617993877991 - - - 0.025300000000 -0.424300000000 -0.018400000000 - femur_l - knee_flex_l - 1.919862177194 2.617993877991 - - - 0.010300000000 0.042300000000 -0.014100000000 - patella_l - - - - 1871.000000000000 - 0.084000000000 - 0.157000000000 - 0.087266462600 - 4 - - - - - - 0.039000000000 -0.082200000000 0.000000000000 - tibia_l - - - 0.002100000000 0.001500000000 -0.000100000000 - patella_l - - - - 100.000000000000 - 0.050000000000 - 0.005000000000 - 0.000000000000 - 4 - - - - - - -0.012700000000 -0.392900000000 0.023500000000 - femur_l - - - -0.023900000000 -0.402200000000 0.025800000000 - femur_l - knee_flex_l - -0.349065850399 0.770039265980 - - - -0.021700000000 -0.048700000000 0.029500000000 - tibia_l - - - 0.003360000000 0.032040000000 0.006340000000 - foot_l - - - - 1113.000000000000 - 0.045000000000 - 0.408000000000 - 0.296705972839 - 4 - - - - - - -0.015500000000 -0.394600000000 -0.027200000000 - femur_l - - - -0.025400000000 -0.401800000000 -0.027400000000 - femur_l - knee_flex_l - -0.349065850399 0.770039265980 - - - -0.024200000000 -0.048100000000 -0.023500000000 - tibia_l - - - 0.003360000000 0.032040000000 0.006340000000 - foot_l - - - - 488.000000000000 - 0.064000000000 - 0.385000000000 - 0.139626340160 - 4 - - - - - - -0.002400000000 -0.153300000000 -0.007100000000 - tibia_l - - - 0.003360000000 0.032040000000 0.006340000000 - foot_l - - - - 2839.000000000000 - 0.030000000000 - 0.268000000000 - 0.436332312999 - 4 - - - - - - -0.009400000000 -0.134800000000 -0.001900000000 - tibia_l - - - -0.014400000000 -0.405100000000 0.022900000000 - tibia_l - - - 0.041700000000 0.033400000000 0.028600000000 - foot_l - - - 0.077200000000 0.015900000000 0.028100000000 - foot_l - - - - 1270.000000000000 - 0.031000000000 - 0.310000000000 - 0.209439510239 - 4 - - - - - - 0.017900000000 -0.162400000000 -0.011500000000 - tibia_l - - - 0.032900000000 -0.395100000000 0.017700000000 - tibia_l - - - 0.116600000000 0.017800000000 0.030500000000 - foot_l - - - - 603.000000000000 - 0.098000000000 - 0.223000000000 - 0.087266462600 - 4 - - - - - - 0.003200000000 -0.138100000000 -0.027600000000 - tibia_l - - - 0.028900000000 -0.400700000000 -0.007200000000 - tibia_l - - - 0.092200000000 0.038800000000 0.000100000000 - foot_l - - - 0.161600000000 0.005500000000 -0.013000000000 - foot_l - - - 0.000300000000 0.004700000000 -0.015300000000 - toes_l - - - 0.044300000000 -0.000400000000 -0.025000000000 - toes_l - - - - 341.000000000000 - 0.102000000000 - 0.345000000000 - 0.139626340160 - 4 - - - - - - 0.001200000000 -0.176700000000 -0.022800000000 - tibia_l - - - 0.032600000000 -0.398500000000 0.008500000000 - tibia_l - - - 0.097000000000 0.038900000000 0.021100000000 - foot_l - - - 0.129300000000 0.030900000000 0.025700000000 - foot_l - - - 0.177780000000 0.011960000000 0.027300000000 - foot_l - - - 0.012170000000 0.010390000000 0.026330000000 - toes_l - - - 0.056280000000 -0.001310000000 0.018520000000 - toes_l - - - - 108.000000000000 - 0.111000000000 - 0.305000000000 - 0.104719755120 - 4 - - - - - - -0.008300000000 -0.204600000000 0.001800000000 - tibia_l - - - -0.015400000000 -0.405100000000 0.019600000000 - tibia_l - - - 0.043600000000 0.031500000000 0.028000000000 - foot_l - - - 0.070800000000 0.017600000000 0.026300000000 - foot_l - - - 0.163610000000 -0.007980000000 -0.010090000000 - foot_l - - - -0.001900000000 -0.007800000000 -0.014700000000 - toes_l - - - 0.028500000000 -0.007100000000 -0.021500000000 - toes_l - - - 0.044100000000 -0.006000000000 -0.024200000000 - toes_l - - - - 310.000000000000 - 0.034000000000 - 0.400000000000 - 0.122173047640 - 4 - - - - - - -0.007900000000 -0.233400000000 -0.024400000000 - tibia_l - - - -0.018600000000 -0.407900000000 0.017400000000 - tibia_l - - - 0.037400000000 0.027600000000 0.024100000000 - foot_l - - - 0.103800000000 0.006800000000 0.025600000000 - foot_l - - - 0.169560000000 -0.005010000000 0.027610000000 - foot_l - - - 0.008390000000 -0.007260000000 0.026900000000 - toes_l - - - 0.056070000000 -0.008390000000 0.018170000000 - toes_l - - - - 322.000000000000 - 0.043000000000 - 0.380000000000 - 0.174532925199 - 4 - - - - - - -0.007000000000 -0.264600000000 -0.032500000000 - tibia_l - - - -0.019800000000 -0.418400000000 -0.028300000000 - tibia_l - - - -0.014400000000 -0.429500000000 -0.028900000000 - tibia_l - - - 0.047100000000 0.027000000000 -0.023300000000 - foot_l - - - 0.075690000000 0.019340000000 -0.029420000000 - foot_l - - - - 348.000000000000 - 0.050000000000 - 0.161000000000 - 0.087266462600 - 4 - - - - - - 0.000500000000 -0.156800000000 -0.036200000000 - tibia_l - - - -0.020700000000 -0.420500000000 -0.028600000000 - tibia_l - - - -0.016200000000 -0.431900000000 -0.028900000000 - tibia_l - - - 0.043800000000 0.023000000000 -0.022100000000 - foot_l - - - 0.068100000000 0.010600000000 -0.028400000000 - foot_l - - - 0.085200000000 0.006900000000 -0.011800000000 - foot_l - - - 0.120300000000 0.008500000000 0.018400000000 - foot_l - - - - 754.000000000000 - 0.049000000000 - 0.345000000000 - 0.174532925199 - 4 - - - - - - 0.001000000000 -0.280400000000 -0.023100000000 - tibia_l - - - 0.022900000000 -0.406900000000 -0.015900000000 - tibia_l - - - 0.085700000000 0.022800000000 -0.029900000000 - foot_l - - - - 90.000000000000 - 0.079000000000 - 0.100000000000 - 0.226892802759 - 4 - - - - - - 0.0 0.0 -9.80665 - - - - pelvis - -0.170000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - pelvis - -0.170000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - pelvis - -0.170000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - pelvis - -0.170000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - pelvis - -0.170000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - pelvis - -0.170000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - pelvis - 0.014000000000 0.011500000000 0.128000000000 - 2.000000000000 - false - - - pelvis - 0.014000000000 0.011500000000 0.128000000000 - 2.000000000000 - false - - - pelvis - 0.014000000000 0.011500000000 0.128000000000 - 2.000000000000 - false - - - pelvis - -0.145000000000 0.040000000000 0.055000000000 - 2.000000000000 - false - - - pelvis - -0.145000000000 0.040000000000 0.055000000000 - 2.000000000000 - false - - - pelvis - -0.145000000000 0.040000000000 0.055000000000 - 2.000000000000 - false - - - pelvis - -0.010000000000 -0.020000000000 0.080000000000 - 2.000000000000 - false - - - pelvis - 0.014000000000 0.011500000000 -0.128000000000 - 2.000000000000 - false - - - pelvis - 0.014000000000 0.011500000000 -0.128000000000 - 2.000000000000 - false - - - pelvis - 0.014000000000 0.011500000000 -0.128000000000 - 2.000000000000 - false - - - pelvis - -0.145000000000 0.040000000000 -0.055000000000 - 2.000000000000 - false - - - pelvis - -0.145000000000 0.040000000000 -0.055000000000 - 2.000000000000 - false - - - pelvis - -0.145000000000 0.040000000000 -0.055000000000 - 2.000000000000 - false - - - pelvis - -0.010000000000 -0.020000000000 -0.080000000000 - 2.000000000000 - false - - - torso - -0.180000000000 0.220000000000 0.000000000000 - 1.000000000000 - false - - - torso - 0.035000000000 0.410000000000 0.000000000000 - 1.000000000000 - false - - - torso - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - torso - 0.180000000000 0.220000000000 0.000000000000 - 1.000000000000 - false - - - torso - 0.180000000000 0.220000000000 0.000000000000 - 1.000000000000 - false - - - torso - 0.180000000000 0.220000000000 0.000000000000 - 1.000000000000 - false - - - torso - -0.120000000000 -0.100000000000 -0.080000000000 - 1.000000000000 - false - - - torso - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - torso - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - torso - -0.085000000000 0.435000000000 0.184000000000 - 1.000000000000 - false - - - torso - -0.126000000000 0.400000000000 0.119000000000 - 1.000000000000 - false - - - torso - -0.135000000000 0.270000000000 0.135000000000 - 1.000000000000 - false - - - torso - -0.080000000000 0.455000000000 0.000000000000 - 1.000000000000 - false - - - torso - -0.120000000000 -0.100000000000 0.080000000000 - 1.000000000000 - false - - - torso - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - torso - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - torso - -0.085000000000 0.435000000000 -0.184000000000 - 1.000000000000 - false - - - torso - -0.126000000000 0.400000000000 -0.119000000000 - 1.000000000000 - false - - - torso - -0.135000000000 0.270000000000 -0.135000000000 - 1.000000000000 - false - - - neckhead - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - neckhead - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.190000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.190000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.190000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.190000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - -0.060000000000 0.095000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - -0.060000000000 0.095000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - -0.060000000000 0.095000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - -0.060000000000 0.095000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - OT_head - 0.000000000000 0.000000000000 0.000000000000 - 1.000000000000 - false - - - clavicle_marker_r - 0.000000000000 0.000000000000 0.165000000000 - 1.000000000000 - false - - - clavicle_marker_r - 0.000000000000 0.000000000000 0.165000000000 - 1.000000000000 - false - - - humerus_r - 0.040000000000 -0.150000000000 0.000000000000 - 2.000000000000 - false - - - humerus_r - -0.040000000000 -0.150000000000 0.000000000000 - 2.000000000000 - false - - - humerus_r - 0.030000000000 -0.150000000000 0.020000000000 - 2.000000000000 - false - - - elbow_marker_r - -0.010000000000 -0.281000000000 0.040000000000 - 2.000000000000 - false - - - elbow_marker_r - -0.010000000000 -0.281000000000 0.040000000000 - 2.000000000000 - false - - - elbow_marker_r - -0.010000000000 -0.281000000000 0.040000000000 - 2.000000000000 - false - - - elbow_marker_r - -0.010000000000 -0.281000000000 0.040000000000 - 2.000000000000 - false - - - elbow_marker_r - 0.000000000000 -0.281000000000 -0.040000000000 - 2.000000000000 - false - - - elbow_marker_r - 0.000000000000 -0.281000000000 -0.040000000000 - 2.000000000000 - false - - - ulna_r - 0.000000000000 -0.150000000000 0.000000000000 - 2.000000000000 - false - - - wrist_marker_r - -0.025000000000 -0.245000000000 0.006000000000 - 2.000000000000 - false - - - wrist_marker_r - -0.025000000000 -0.245000000000 0.006000000000 - 2.000000000000 - false - - - wrist_marker_r - -0.025000000000 -0.245000000000 0.006000000000 - 2.000000000000 - false - - - wrist_marker_r - 0.035000000000 -0.245000000000 0.000000000000 - 2.000000000000 - false - - - wrist_marker_r - 0.035000000000 -0.245000000000 0.000000000000 - 2.000000000000 - false - - - wrist_marker_r - 0.000000000000 -0.245000000000 -0.030000000000 - 2.000000000000 - false - - - wrist_marker_r - 0.000000000000 -0.245000000000 0.040000000000 - 2.000000000000 - false - - - wrist_marker_r - -0.025000000000 -0.245000000000 0.006000000000 - 2.000000000000 - false - - - wrist_marker_r - -0.025000000000 -0.245000000000 0.006000000000 - 2.000000000000 - false - - - wrist_marker_r - 0.035000000000 -0.245000000000 0.000000000000 - 2.000000000000 - false - - - hand_r - -0.030000000000 -0.050000000000 0.000000000000 - 1.000000000000 - false - - - clavicle_marker_l - 0.000000000000 0.000000000000 -0.165000000000 - 1.000000000000 - false - - - clavicle_marker_l - 0.000000000000 0.000000000000 -0.165000000000 - 1.000000000000 - false - - - humerus_l - 0.040000000000 -0.150000000000 0.000000000000 - 2.000000000000 - false - - - humerus_l - -0.040000000000 -0.150000000000 0.000000000000 - 2.000000000000 - false - - - humerus_l - 0.030000000000 -0.150000000000 -0.020000000000 - 2.000000000000 - false - - - elbow_marker_l - 0.010000000000 -0.281000000000 -0.040000000000 - 2.000000000000 - false - - - elbow_marker_l - 0.010000000000 -0.281000000000 -0.040000000000 - 2.000000000000 - false - - - elbow_marker_l - 0.010000000000 -0.281000000000 -0.040000000000 - 2.000000000000 - false - - - elbow_marker_l - 0.010000000000 -0.281000000000 -0.040000000000 - 2.000000000000 - false - - - elbow_marker_l - -0.020000000000 -0.281000000000 0.040000000000 - 2.000000000000 - false - - - elbow_marker_l - -0.020000000000 -0.281000000000 0.040000000000 - 2.000000000000 - false - - - ulna_l - 0.000000000000 -0.150000000000 0.000000000000 - 2.000000000000 - false - - - wrist_marker_l - -0.025000000000 -0.245000000000 -0.006000000000 - 2.000000000000 - false - - - wrist_marker_l - -0.025000000000 -0.245000000000 -0.006000000000 - 2.000000000000 - false - - - wrist_marker_l - -0.025000000000 -0.245000000000 -0.006000000000 - 2.000000000000 - false - - - wrist_marker_l - 0.035000000000 -0.245000000000 0.000000000000 - 2.000000000000 - false - - - wrist_marker_l - 0.035000000000 -0.245000000000 0.000000000000 - 2.000000000000 - false - - - wrist_marker_l - 0.000000000000 -0.245000000000 0.030000000000 - 2.000000000000 - false - - - wrist_marker_l - 0.000000000000 -0.245000000000 -0.040000000000 - 2.000000000000 - false - - - wrist_marker_l - -0.025000000000 -0.245000000000 -0.006000000000 - 2.000000000000 - false - - - wrist_marker_l - -0.025000000000 -0.245000000000 -0.006000000000 - 2.000000000000 - false - - - wrist_marker_l - 0.025000000000 -0.245000000000 0.000000000000 - 2.000000000000 - false - - - hand_l - -0.030000000000 -0.050000000000 0.000000000000 - 1.000000000000 - false - - - femur_r - 0.000000000000 -0.404000000000 0.050000000000 - 1.000000000000 - false - - - femur_r - 0.000000000000 -0.404000000000 0.050000000000 - 1.000000000000 - false - - - femur_r - 0.000000000000 -0.404000000000 0.050000000000 - 1.000000000000 - false - - - femur_r - 0.000000000000 -0.404000000000 0.050000000000 - 1.000000000000 - false - - - femur_r - 0.000000000000 -0.404000000000 -0.050000000000 - 1.000000000000 - false - - - femur_r - 0.000000000000 -0.404000000000 -0.050000000000 - 1.000000000000 - false - - - femur_r - 0.017900000000 -0.224000000000 0.114700000000 - 1.000000000000 - false - - - femur_r - 0.017900000000 -0.264000000000 0.064700000000 - 1.000000000000 - false - - - femur_r - 0.080000000000 -0.324000000000 0.004700000000 - 1.000000000000 - false - - - femur_r - -0.047900000000 -0.324000000000 0.004700000000 - 1.000000000000 - false - - - femur_r - 0.017900000000 -0.264000000000 0.064700000000 - 1.000000000000 - false - - - tibia_r - -0.005000000000 -0.410000000000 0.053000000000 - 2.000000000000 - false - - - tibia_r - -0.005000000000 -0.410000000000 0.053000000000 - 2.000000000000 - false - - - tibia_r - -0.005000000000 -0.410000000000 0.053000000000 - 2.000000000000 - false - - - tibia_r - -0.005000000000 -0.410000000000 0.053000000000 - 2.000000000000 - false - - - tibia_r - 0.006000000000 -0.388800000000 -0.038000000000 - 2.000000000000 - false - - - tibia_r - 0.006000000000 -0.388800000000 -0.038000000000 - 2.000000000000 - false - - - tibia_r - 0.010400000000 -0.232200000000 0.074800000000 - 1.000000000000 - false - - - tibia_r - 0.012500000000 -0.319600000000 0.060000000000 - 1.000000000000 - false - - - tibia_r - 0.042500000000 -0.359600000000 0.000000000000 - 1.000000000000 - false - - - tibia_r - -0.032500000000 -0.359600000000 0.000000000000 - 1.000000000000 - false - - - tibia_r - 0.012500000000 -0.319600000000 0.060000000000 - 1.000000000000 - false - - - foot_marker_r - 0.190000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - foot_marker_r - 0.190000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - foot_marker_r - 0.130000000000 0.000000000000 -0.030000000000 - 2.000000000000 - false - - - foot_marker_r - 0.130000000000 0.000000000000 0.050000000000 - 2.000000000000 - false - - - foot_marker_r - 0.000000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - foot_marker_r - 0.000000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - femur_l - 0.000000000000 -0.404000000000 -0.050000000000 - 1.000000000000 - false - - - femur_l - 0.000000000000 -0.404000000000 -0.050000000000 - 1.000000000000 - false - - - femur_l - 0.000000000000 -0.404000000000 -0.050000000000 - 1.000000000000 - false - - - femur_l - 0.000000000000 -0.404000000000 -0.050000000000 - 1.000000000000 - false - - - femur_l - 0.000000000000 -0.404000000000 0.050000000000 - 1.000000000000 - false - - - femur_l - 0.000000000000 -0.404000000000 0.050000000000 - 1.000000000000 - false - - - femur_l - 0.017900000000 -0.224000000000 -0.114700000000 - 1.000000000000 - false - - - femur_l - 0.017900000000 -0.264000000000 -0.064700000000 - 1.000000000000 - false - - - femur_l - 0.080000000000 -0.324000000000 -0.004700000000 - 1.000000000000 - false - - - femur_l - -0.047900000000 -0.324000000000 -0.004700000000 - 1.000000000000 - false - - - femur_l - 0.017900000000 -0.264000000000 -0.064700000000 - 1.000000000000 - false - - - tibia_l - -0.005000000000 -0.410000000000 -0.053000000000 - 2.000000000000 - false - - - tibia_l - -0.005000000000 -0.410000000000 -0.053000000000 - 2.000000000000 - false - - - tibia_l - -0.005000000000 -0.410000000000 -0.053000000000 - 2.000000000000 - false - - - tibia_l - -0.005000000000 -0.410000000000 -0.053000000000 - 2.000000000000 - false - - - tibia_l - 0.006000000000 -0.388800000000 0.038000000000 - 2.000000000000 - false - - - tibia_l - 0.006000000000 -0.388800000000 0.038000000000 - 2.000000000000 - false - - - tibia_l - 0.010400000000 -0.232200000000 -0.074800000000 - 1.000000000000 - false - - - tibia_l - 0.012500000000 -0.319600000000 -0.060000000000 - 1.000000000000 - false - - - tibia_l - 0.042500000000 -0.359600000000 0.000000000000 - 1.000000000000 - false - - - tibia_l - -0.032500000000 -0.359600000000 0.000000000000 - 1.000000000000 - false - - - tibia_l - 0.012500000000 -0.319600000000 -0.060000000000 - 1.000000000000 - false - - - foot_marker_l - 0.190000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - foot_marker_l - 0.190000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - foot_marker_l - 0.130000000000 0.000000000000 0.030000000000 - 2.000000000000 - false - - - foot_marker_l - 0.130000000000 0.000000000000 -0.050000000000 - 2.000000000000 - false - - - foot_marker_l - 0.000000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - foot_marker_l - 0.000000000000 0.000000000000 0.000000000000 - 2.000000000000 - false - - - - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - - 0.03000000 0.08000000 0.03000000 - true - 0.00000000 0.00000000 0.00000000 - -0.01730000 -0.08750000 0.78950000 - x - - - 0.03000000 - 0.12000000 - true - -1.64305296 -0.02426008 -0.02042035 - 0.00510000 -0.08030000 0.61640000 - x - - - 0.04000000 - true - -0.01902409 -0.09599311 -0.02722714 - 0.04270000 -0.08580000 0.70800000 - -x - - - - - - 8.377500000000 - -0.089300000000 -0.025796000000 0.000000000000 - 0.053993000000 0.000000000000 0.000000000000 0.000000000000 0.061279000000 0.000000000000 0.000000000000 0.000000000000 0.067263000000 - - pelvis_rv.vtp pelvis_lv.vtp sacrumv.vtp - - - - 24.217500000000 - 0.014500000000 0.256438000000 0.000000000000 - 0.440821000000 0.000000000000 0.000000000000 0.000000000000 0.276872000000 0.000000000000 0.000000000000 0.000000000000 0.596511000000 - - lumbar1sm.vtp lumbar2sm.vtp lumbar3sm.vtp lumbar4sm.vtp lumbar5.vtp ribcage_s.vtp thoracic1_s.vtp thoracic2_s.vtp thoracic3_s.vtp thoracic4_s.vtp thoracic5_s.vtp thoracic6_s.vtp thoracic7_s.vtp thoracic8_s.vtp thoracic9_s.vtp thoracic10_s.vtp thoracic11_s.vtp thoracic12_s.vtp clavicle_rvsm.vtp scapula_rvsm.vtp clavicle_lvsm.vtp scapula_lvsm.vtp - - - - 5.205000000000 - 0.028800000000 0.161501000000 0.000000000000 - 0.030471000000 0.000000000000 0.000000000000 0.000000000000 0.020920000000 0.000000000000 0.000000000000 0.000000000000 0.028195000000 - - cerv1sm.vtp cerv2sm.vtp cerv3sm.vtp cerv4sm.vtp cerv5sm.vtp cerv6sm.vtp cerv7.vtp skull_s.vtp jaw_s.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 2.032500000000 - 0.000000000000 -0.164502000000 0.000000000000 - 0.011946000000 0.000000000000 0.000000000000 0.000000000000 0.004121000000 0.000000000000 0.000000000000 0.000000000000 0.013409000000 - - humerus_rv.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 0.607500000000 - 0.000000000000 -0.120525000000 0.000000000000 - 0.002962000000 0.000000000000 0.000000000000 0.000000000000 0.000618000000 0.000000000000 0.000000000000 0.000000000000 0.003213000000 - - ulna_rv.vtp - - - - 0.607500000000 - 0.000000000000 -0.120525000000 0.000000000000 - 0.002962000000 0.000000000000 0.000000000000 0.000000000000 0.000618000000 0.000000000000 0.000000000000 0.000000000000 0.003213000000 - - radius_rv.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 0.457500000000 - 0.000000000000 -0.068095000000 0.000000000000 - 0.000892000000 0.000000000000 0.000000000000 0.000000000000 0.000547000000 0.000000000000 0.000000000000 0.000000000000 0.001340000000 - - pisiform_rvs.vtp lunate_rvs.vtp scaphoid_rvs.vtp triquetrum_rvs.vtp hamate_rvs.vtp capitate_rvs.vtp trapezoid_rvs.vtp trapezium_rvs.vtp metacarpal2_rvs.vtp index_proximal_rvs.vtp index_medial_rvs.vtp index_distal_rvs.vtp metacarpal3_rvs.vtp middle_proximal_rvs.vtp middle_medial_rvs.vtp middle_distal_rvs.vtp metacarpal4_rvs.vtp ring_proximal_rvs.vtp ring_medial_rvs.vtp ring_distal_rvs.vtp metacarpal5_rvs.vtp little_proximal_rvs.vtp little_medial_rvs.vtp little_distal_rvs.vtp metacarpal1_rvs.vtp thumb_proximal_rvs.vtp thumb_distal_rvs.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 2.032500000000 - 0.000000000000 -0.164502000000 0.000000000000 - 0.011946000000 0.000000000000 0.000000000000 0.000000000000 0.004121000000 0.000000000000 0.000000000000 0.000000000000 0.013409000000 - - humerus_lv.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 0.607500000000 - 0.000000000000 -0.120525000000 0.000000000000 - 0.002962000000 0.000000000000 0.000000000000 0.000000000000 0.000618000000 0.000000000000 0.000000000000 0.000000000000 0.003213000000 - - ulna_lv.vtp - - - - 0.607500000000 - 0.000000000000 -0.120525000000 0.000000000000 - 0.002962000000 0.000000000000 0.000000000000 0.000000000000 0.000618000000 0.000000000000 0.000000000000 0.000000000000 0.003213000000 - - radius_lv.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 0.457500000000 - 0.000000000000 -0.068095000000 0.000000000000 - 0.000892000000 0.000000000000 0.000000000000 0.000000000000 0.000547000000 0.000000000000 0.000000000000 0.000000000000 0.001340000000 - - pisiform_lvs.vtp lunate_lvs.vtp scaphoid_lvs.vtp triquetrum_lvs.vtp hamate_lvs.vtp capitate_lvs.vtp trapezoid_lvs.vtp trapezium_lvs.vtp metacarpal2_lvs.vtp index_proximal_lvs.vtp index_medial_lvs.vtp index_distal_lvs.vtp metacarpal3_lvs.vtp middle_proximal_lvs.vtp middle_medial_lvs.vtp middle_distal_lvs.vtp metacarpal4_lvs.vtp ring_proximal_lvs.vtp ring_medial_lvs.vtp ring_distal_lvs.vtp metacarpal5_lvs.vtp little_proximal_lvs.vtp little_medial_lvs.vtp little_distal_lvs.vtp metacarpal1_lvs.vtp thumb_proximal_lvs.vtp thumb_distal_lvs.vtp - - - - 10.522500000000 - 0.000000000000 -0.162162000000 0.000000000000 - 0.178608000000 0.000000000000 0.000000000000 0.000000000000 0.036634000000 0.000000000000 0.000000000000 0.000000000000 0.178608000000 - - femur_r.vtp - - - - 3.247500000000 - 0.000000000000 -0.171713000000 0.000000000000 - 0.029999000000 0.000000000000 0.000000000000 0.000000000000 0.005157000000 0.000000000000 0.000000000000 0.000000000000 0.031231000000 - - tibia_r.vtp fibula_r.vtp - - - - 0.097500000000 - 0.001800000000 0.026400000000 0.000000000000 - 0.000003000000 0.000000000000 0.000000000000 0.000000000000 0.000015000000 0.000000000000 0.000000000000 0.000000000000 0.000015000000 - - patella_r.vtp - - - - 0.045000000000 - 0.005500000000 0.002300000000 0.000000000000 - 0.000003000000 0.000000000000 0.000000000000 0.000000000000 0.000001000000 0.000000000000 0.000000000000 0.000000000000 0.000003000000 - - talus_rv.vtp - - - - 0.885000000000 - 0.101865000000 0.015600000000 0.000000000000 - 0.000909000000 0.000000000000 0.000000000000 0.000000000000 0.002314000000 0.000000000000 0.000000000000 0.000000000000 0.002683000000 - - calcaneous_rv.vtp navicular_rv.vtp cuboid_rv.vtp first_cuneiform_rv.vtp second_cuneiform_rv.vtp third_cuneiform_rv.vtp metatarsal1_rv.vtp metatarsal2_rv.vtp metatarsal3_rv.vtp metatarsal4_rv.vtp metatarsal5_rv.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 0.097500000000 - 0.030700000000 -0.002600000000 0.010500000000 - 0.000100000000 0.000000000000 0.000000000000 0.000000000000 0.000255000000 0.000000000000 0.000000000000 0.000000000000 0.000294000000 - - prox_phalanx1_rvs.vtp prox_phalanx2_rvs.vtp prox_phalanx3_rvs.vtp prox_phalanx4_rvs.vtp prox_phalanx5_rvs.vtp mid_phalanx2_rvs.vtp mid_phalanx3_rvs.vtp mid_phalanx4_rvs.vtp mid_phalanx5_rvs.vtp distal_phalanx1_rvs.vtp distal_phalanx2_rvs.vtp distal_phalanx3_rvs.vtp distal_phalanx4_rvs.vtp distal_phalanx5_rvs.vtp - - - - 10.522500000000 - 0.000000000000 -0.162162000000 0.000000000000 - 0.178608000000 0.000000000000 0.000000000000 0.000000000000 0.036634000000 0.000000000000 0.000000000000 0.000000000000 0.178608000000 - - femur_l.vtp - - - - 3.247500000000 - 0.000000000000 -0.171713000000 0.000000000000 - 0.029999000000 0.000000000000 0.000000000000 0.000000000000 0.005157000000 0.000000000000 0.000000000000 0.000000000000 0.031231000000 - - tibia_l.vtp fibula_l.vtp - - - - 0.097500000000 - 0.001800000000 0.026400000000 0.000000000000 - 0.000003000000 0.000000000000 0.000000000000 0.000000000000 0.000015000000 0.000000000000 0.000000000000 0.000000000000 0.000015000000 - - patella_l.vtp - - - - 0.045000000000 - 0.005500000000 0.002300000000 0.000000000000 - 0.000003000000 0.000000000000 0.000000000000 0.000000000000 0.000001000000 0.000000000000 0.000000000000 0.000000000000 0.000003000000 - - talus_lv.vtp - - - - 0.885000000000 - 0.101865000000 0.015600000000 0.000000000000 - 0.000909000000 0.000000000000 0.000000000000 0.000000000000 0.002314000000 0.000000000000 0.000000000000 0.000000000000 0.002683000000 - - calcaneous_lv.vtp navicular_lv.vtp cuboid_lv.vtp first_cuneiform_lv.vtp second_cuneiform_lv.vtp third_cuneiform_lv.vtp metatarsal1_lv.vtp metatarsal2_lv.vtp metatarsal3_lv.vtp metatarsal4_lv.vtp metatarsal5_lv.vtp - - - - 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 - 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 0.000000000000 - - - 0.097500000000 - 0.030700000000 -0.002600000000 -0.010500000000 - 0.000100000000 0.000000000000 0.000000000000 0.000000000000 0.000255000000 0.000000000000 0.000000000000 0.000000000000 0.000294000000 - - prox_phalanx1_lvs.vtp prox_phalanx2_lvs.vtp prox_phalanx3_lvs.vtp prox_phalanx4_lvs.vtp prox_phalanx5_lvs.vtp mid_phalanx2_lvs.vtp mid_phalanx3_lvs.vtp mid_phalanx4_lvs.vtp mid_phalanx5_lvs.vtp distal_phalanx1_lvs.vtp distal_phalanx2_lvs.vtp distal_phalanx3_lvs.vtp distal_phalanx4_lvs.vtp distal_phalanx5_lvs.vtp - - - - - - - - -10.000000000000 10.000000000000 - 0.000000000000 - 0.000000000000 - 0.100000000000 - false - false - false - - - -10.000000000000 10.000000000000 - 0.000000000000 - 0.000000000000 - 0.100000000000 - false - false - false - - - -10.000000000000 10.000000000000 - 0.969000000000 - 0.000000000000 - 0.100000000000 - false - false - false - - - -4.712388980385 4.712388980385 - 1.570796326795 - 0.000000000000 - 0.100000000000 - false - false - false - - - -4.712388980385 4.712388980385 - 0.000000000000 - 0.000000000000 - 0.100000000000 - false - false - false - - - -4.712388980385 4.712388980385 - 0.000000000000 - 0.000000000000 - 0.100000000000 - false - false - false - - - -0.785398163397 0.785398163397 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -1.047197551197 1.047197551197 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.785398163397 0.785398163397 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.698131700798 0.698131700798 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -1.151917306316 1.151917306316 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.872664625997 1.256637061436 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -2.094395102393 1.570796326795 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.174532925199 0.174532925199 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.785398163397 1.570796326795 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 2.268928027593 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.872664625997 1.221730476396 - 1.047197551197 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.436332312999 0.610865238198 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -1.221730476396 1.221730476396 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -2.094395102393 1.570796326795 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.174532925199 0.174532925199 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.785398163397 1.570796326795 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 2.268928027593 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.872664625997 1.221730476396 - 1.047197551197 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.436332312999 0.610865238198 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -1.221730476396 1.221730476396 - 0.000000000000 - 0.000000000000 - 0.010000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.872664625997 0.349065850399 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.698131700798 0.698131700798 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.191986217719 1.658062789395 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.174532925199 2.094395102393 - 0.000000000000 - 0.000000000000 - 0.200000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.523598775598 0.523598775598 - 0.000000000000 - 0.000000000000 - 2.000000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.349065850399 0.349065850399 - 0.000000000000 - 0.000000000000 - 2.000000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.523598775598 0.523598775598 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.872664625997 0.349065850399 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.698131700798 0.698131700798 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.191986217719 1.658062789395 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.174532925199 2.094395102393 - 0.000000000000 - 0.000000000000 - 0.200000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.523598775598 0.523598775598 - 0.000000000000 - 0.000000000000 - 2.000000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.349065850399 0.349065850399 - 0.000000000000 - 0.000000000000 - 2.000000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - -0.523598775598 0.523598775598 - 0.000000000000 - 0.000000000000 - 0.100000000000 - true - false - false - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - 0.000000000000 0.174532925199 0.523598775598 - 0.000000000000 20.000000000000 70.000000000000 - - - - - - - - - ground pelvis - - - - lower_torso_TX - - - -360.000000000000 360.000000000000 - -360.000000000000 360.000000000000 - - - - - lower_torso_TY - - - -360.000000000000 360.000000000000 - -360.000000000000 360.000000000000 - - - - - lower_torso_TZ - - - -360.000000000000 360.000000000000 - -360.000000000000 360.000000000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - lower_torso_RX - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - lower_torso_RY - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 0.000000000000 1.000000000000 - lower_torso_RZ - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - pelvis torso - - - - - - -0.087200000000 - - - - - - - 0.033100000000 - - - - - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - lumbar_pitch - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 1.000000000000 0.000000000000 0.000000000000 - lumbar_roll - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - lumbar_yaw - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - torso neckhead - - - - - - -0.025300000000 - - - - - - - 0.448950000000 - - - - - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - neck_pitch - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 1.000000000000 0.000000000000 0.000000000000 - neck_roll - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - neck_yaw - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - neckhead OT_head - - - - - - -0.071660000000 - - - - - - - 0.235900000000 - - - - - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - -1.570796326795 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - torso clavicle_marker_r - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - -0.048800000000 - - - - - - - 0.470650000000 - - - - - - - 0.000000000000 - - - - - - - - torso humerus_r - - - - - - -0.028100000000 - - - - - - - 0.416450000000 - - - - - - - 0.199000000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - arm_add_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 0.000000000000 1.000000000000 - arm_flex_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - arm_rot_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - humerus_r elbow_marker_r - - - - - - 0.000000000000 - - - - - - - 0.000000000000 - - - - - - - 0.000000000000 - - - - - 0.226046960485 0.022268996107 0.973861829762 - - - 0.052359877560 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.261799387799 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - humerus_r ulna_r - - - - - - 0.013144000000 - - - - - - - -0.286273000000 - - - - - - - -0.009595000000 - - - - - 0.226046960485 0.022268996107 0.973861829762 - elbow_flex_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - ulna_r radius_r - - - - - - -0.006727000000 - - - - - - - -0.013007000000 - - - - - - - 0.026083000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.056398025787 0.998406456499 0.001952000893 - pro_sup_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - radius_r wrist_marker_r - - - - - - 0.006727000000 - - - - - - - 0.013007000000 - - - - - - - -0.026083000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - -0.139626340160 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.174532925199 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - -0.069813170080 - - - - - - - - radius_r hand_r - - - - - - -0.008797000000 - - - - - - - -0.235841000000 - - - - - - - 0.013610000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - wrist_flex_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - wrist_dev_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - torso clavicle_marker_l - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - -0.048800000000 - - - - - - - 0.470650000000 - - - - - - - 0.000000000000 - - - - - - - - torso humerus_l - - - - - - -0.028100000000 - - - - - - - 0.416450000000 - - - - - - - -0.199000000000 - - - - - -1.000000000000 0.000000000000 0.000000000000 - arm_add_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 0.000000000000 1.000000000000 - arm_flex_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - arm_rot_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - humerus_l elbow_marker_l - - - - - - 0.000000000000 - - - - - - - 0.000000000000 - - - - - - - 0.000000000000 - - - - - -0.226046960485 -0.022268996107 0.973861829762 - - - 0.052359877560 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - - - -0.261799387799 - - - - - -1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - humerus_l ulna_l - - - - - - 0.013144000000 - - - - - - - -0.286273000000 - - - - - - - 0.009595000000 - - - - - -0.226046960485 -0.022268996107 0.973861829762 - elbow_flex_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - -1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - ulna_l radius_l - - - - - - -0.006727000000 - - - - - - - -0.013007000000 - - - - - - - -0.026083000000 - - - - - -1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - -0.056398025787 -0.998406456499 0.001952000893 - pro_sup_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - radius_l wrist_marker_l - - - - - - 0.006727000000 - - - - - - - 0.013007000000 - - - - - - - 0.026083000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.139626340160 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - -0.174532925199 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - -0.069813170080 - - - - - - - - radius_l hand_l - - - - - - -0.008797000000 - - - - - - - -0.235841000000 - - - - - - - -0.013610000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - wrist_flex_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - -1.000000000000 0.000000000000 0.000000000000 - wrist_dev_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - pelvis femur_r - - - - - - -0.080000000000 - - - - - - - -0.082400000000 - - - - - - - 0.078500000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - hip_flex_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 1.000000000000 0.000000000000 0.000000000000 - hip_add_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - hip_rot_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - femur_r tibia_r - - - - knee_flex_r - - - 0.000000000000 0.174532925199 0.349065850399 0.698131700798 1.047197551197 1.396263401595 1.745329251994 2.094395102393 - -0.005250000000 -0.003100000000 -0.001000000000 0.002120000000 0.004100000000 0.004110000000 0.001790000000 -0.003200000000 - - - - - knee_flex_r - - - 0.000000000000 0.174532925199 0.349065850399 0.523598775598 1.221730476396 2.094395102393 - -0.396000000000 -0.396600000000 -0.397600000000 -0.399000000000 -0.408200000000 -0.422600000000 - - - - - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 -1.000000000000 - knee_flex_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - tibia_r patella_r - - - - knee_flex_r - - - 0.000000000000 0.174532925199 0.349065850399 0.698131700798 1.047197551197 1.396263401595 2.094395102393 - 0.049600000000 0.048400000000 0.046900000000 0.043000000000 0.038100000000 0.032400000000 0.017300000000 - - - - - knee_flex_r - - - 0.004991641661 0.174532925199 0.349065850399 0.698131700798 1.047197551197 1.396263401595 1.570796326795 2.094395102393 - -0.022700000000 -0.022300000000 -0.021900000000 -0.021100000000 -0.020400000000 -0.020000000000 -0.020200000000 -0.021900000000 - - - - - - - 0.002400000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - knee_flex_r - - - -0.174532925199 -0.002617993878 0.354999969856 0.984889296900 1.999972789860 2.094395102393 - -0.161268422884 -0.042586033749 0.106290551446 0.241902634326 0.308050612977 0.308050612977 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - tibia_r talus_r - - - - - - 0.000000000000 - - - - - - - -0.426000000000 - - - - - - - 0.000000000000 - - - - - -0.104529047112 -0.173649078266 0.979244441356 - ankle_flex_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - talus_r foot_r - - - - - - -0.048770000000 - - - - - - - -0.041950000000 - - - - - - - 0.007920000000 - - - - - 0.787180020856 0.604747016023 -0.120949003205 - subt_angle_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - foot_r foot_marker_r - - - - - - -0.020000000000 - - - - - - - 0.018000000000 - - - - - - - -0.010000000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - foot_r toes_r - - - - - - 0.176800000000 - - - - - - - -0.002000000000 - - - - - - - 0.001080000000 - - - - - 0.541716972153 0.000000000000 -0.840560956791 - toe_angle_r - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - pelvis femur_l - - - - - - -0.080000000000 - - - - - - - -0.082400000000 - - - - - - - -0.078500000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - hip_flex_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - -1.000000000000 0.000000000000 0.000000000000 - hip_add_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - hip_rot_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - - - - femur_l tibia_l - - - - knee_flex_l - - - 0.000000000000 0.174532925199 0.349065850399 0.698131700798 1.047197551197 1.396263401595 1.745329251994 2.094395102393 - -0.005250000000 -0.003100000000 -0.001000000000 0.002120000000 0.004100000000 0.004110000000 0.001790000000 -0.003200000000 - - - - - knee_flex_l - - - 0.000000000000 0.174532925199 0.349065850399 0.523598775598 1.221730476396 2.094395102393 - -0.396000000000 -0.396600000000 -0.397600000000 -0.399000000000 -0.408200000000 -0.422600000000 - - - - - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 -1.000000000000 - knee_flex_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - tibia_l patella_l - - - - knee_flex_l - - - 0.000000000000 0.174532925199 0.349065850399 0.698131700798 1.047197551197 1.396263401595 2.094395102393 - 0.049600000000 0.048400000000 0.046900000000 0.043000000000 0.038100000000 0.032400000000 0.017300000000 - - - - - knee_flex_l - - - 0.004991641661 0.174532925199 0.349065850399 0.698131700798 1.047197551197 1.396263401595 1.570796326795 2.094395102393 - -0.022700000000 -0.022300000000 -0.021900000000 -0.021100000000 -0.020400000000 -0.020000000000 -0.020200000000 -0.021900000000 - - - - - - - -0.002400000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - knee_flex_l - - - -0.174532925199 -0.002617993878 0.354999969856 0.984889296900 1.999972789860 2.094395102393 - -0.161268422884 -0.042586033749 0.106290551446 0.241902634326 0.308050612977 0.308050612977 - - - - - -1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - tibia_l talus_l - - - - - - 0.000000000000 - - - - - - - -0.426000000000 - - - - - - - 0.000000000000 - - - - - 0.104529047112 0.173649078266 0.979244441356 - ankle_flex_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - -1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - - - - talus_l foot_l - - - - - - -0.048770000000 - - - - - - - -0.041950000000 - - - - - - - -0.007920000000 - - - - - -0.787180020856 -0.604747016023 -0.120949003205 - subt_angle_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 -1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - foot_l foot_marker_l - - - - - - -0.020000000000 - - - - - - - 0.018000000000 - - - - - - - 0.010000000000 - - - - - 1.000000000000 0.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - foot_l toes_l - - - - - - 0.176800000000 - - - - - - - -0.002000000000 - - - - - - - -0.001080000000 - - - - - -0.541716972153 0.000000000000 -0.840560956791 - toe_angle_l - - - -6.283185307179 6.283185307179 - -6.283185307179 6.283185307179 - - - - - 0.000000000000 1.000000000000 0.000000000000 - - - 0.000000000000 - - - - - 0.000000000000 0.000000000000 1.000000000000 - - - 0.000000000000 - - - - - - - - - - - diff --git a/OpenSim/Utilities/simmToOpenSim/test/mocap.jnt b/OpenSim/Utilities/simmToOpenSim/test/mocap.jnt deleted file mode 100644 index d5f0816d7a..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/mocap.jnt +++ /dev/null @@ -1,7260 +0,0 @@ -/******************************************************************************* - MOCAP MODEL - joint file - - (c) Copyright 2001-5, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This version of the Mocap Model is for use with Solver 1.2.10, - released September 14, 2005. - - This file contains the segments joints, gencoords, and constraint functions - for the SIMM model of an average adult male. It was developed by various - biomechanics researchers, including: - - Scott Delp, Ph.D., Stanford University - Wendy Murray, Ph.D., Stanford University - Silvia Salinas Blemker, M.S., Stanford University - Anita Vasavada, Ph.D., Washington State University - Srikanth Suryanarayanan, M.S., Rehabilitation Institute of Chicago - Frans van der Helm, Ph.D., Delft University - -*******************************************************************************/ - -#ifndef LOWER_EXTREMITY - #define LOWER_EXTREMITY 1 -#endif - -#ifndef UPPER_EXTREMITY - #define UPPER_EXTREMITY 1 -#endif - -#ifndef GROUND_PLANE_DEFINED - #define GROUND_PLANE_XZ 1 -#endif - -#ifndef RIGHT_HAND_FULL - #define RIGHT_HAND_FULL 1 -#endif - -#ifndef LEFT_HAND_FULL - #define LEFT_HAND_FULL 1 -#endif - -/* If subjects stand with their arms out, parallel to the - * ground, in the static pose, then SIMM should detect this - * situation and automatically set ARMS_UP to 1. Thus you - * should not have to change this constant. - */ -#ifndef ARMS_UP - #define ARMS_UP 1 -#endif - - -/* If this model is being used to analyze walking in a - * straight line, then set WALKING to 1, so that the - * ranges of motion for the ground -> body degrees of - * freedom are more restrictive. - */ -#ifndef WALKING - #define WALKING 1 -#endif - - -/* Because the shoulder and wrist models have many degrees - * of freedom, it is often difficult to control with only - * one marker at the shoulder, one at the elbow, and one at - * the wrist. To reduce this problem for gait analysis, some - * of the degrees of freedom in the arms are restricted to - * small ranges of motion. These ranges are suitable for most - * gait patterns, but may not be appropriate for motions - * involving more movement of the arms, such as dancing. - * To remove these restrictions and allow greater movement - * of the arms, set FULL_ARM_MOTION to 1. For best results - * when doing this, you should also add more arm markers to - * the model. - */ -#define FULL_ARM_MOTION 0 - - -/* For a shoulder model with more degrees of freedom, set - * COMPLEX_CLAVICLE to 1. This more complex shoulder model - * is much more difficult to control with the standard OrthoTrak - * marker set, so you will want to add more markers to the - * arms when using this shoulder model. - */ -#define COMPLEX_CLAVICLE 0 - - -/* To enhance the knee model to include varus/valgus motion, - * set VARUS_VALGUS to 1. - */ -#define VARUS_VALGUS 0 - - -/* The TWO_PELVIS_JOINTS parameter controls the transformation from the - * ground segment to the pelvis. If the parameter is set to 1, the - * transformation is controlled by two joints. The first joint goes - * from ground to the pelvic_frame, and has six degrees of freedom. - * The second joint goes from pelvic_frame to pelvis, and has only - * three degrees of freedom- pelvic_tilt, pelvic_obliquity, and pelvic_rotation. - * These DOFs are meant to contain more anatomically meaningful rotations. - * When loading the mocap model and solving the static pose, - * the rotations in the second joint are locked at zero, and the - * rotations in the first joint are set to appropriate values. - * Then when solving motions, the first joint's rotations are - * locked and the second joint's rotations are unlocked. Thus - * the pelvic tilt, obliquity, and rotation angles are measured - * with respect to the static pose. If you do not want this - * behavior, and instead want to remove those three DOFs and - * have only one joint between ground and the pelvis (the first - * joint with six DOFs), then set the parameter to 0. - */ -#define TWO_PELVIS_JOINTS 1 - - -/* These are the weights of the various markers in the - * mocap model. Solver uses them to determine which markers - * it should try harder to fit to the marker cloud data. - * 1.0 is the default, and numbers greater than 1.0 tell - * Solver to try harder for those markers. - */ -#define FOOT_WEIGHT 2.0 -#define PELVIS_WEIGHT 3.0 -#define KNEE_WEIGHT 2.0 -#define WRIST_WEIGHT 1.0 -#define ARM_WEIGHT 1.0 -#define OTHER_WEIGHT 1.0 - -#define TORSO_SIZE 0.5600 0.5600 0.3632 -#define HEAD_SIZE 0.1890 0.1890 0.1890 -#define UPPER_ARM_SIZE 0.2850 0.2850 0.2850 -#define LOWER_ARM_SIZE 0.2635 0.2635 0.2635 -#define HAND_SIZE 0.1879 0.1879 0.1879 -#define PELVIS_SIZE 0.1844 0.0922 0.256 -#define THIGH_SIZE 0.4050 0.4050 0.4050 -#define SHANK_SIZE 0.4000 0.4000 0.4000 -#define FOOT_SIZE 0.2670 0.2670 0.2670 - - -#if GROUND_PLANE_XY - #define SHADOW_PARAMS shadow Z 0.001 - gravity -Z -#elif GROUND_PLANE_XZ - #define SHADOW_PARAMS shadow Y 0.001 - gravity -Y -#else - #define SHADOW_PARAMS shadow X 0.001 - gravity -X -#endif - -#define DEFAULT_RIGHT_AXES \ -axis1 1.0 0.0 0.0 \ -axis2 0.0 1.0 0.0 \ -axis3 0.0 0.0 1.0 - -#define DEFAULT_LEFT_AXES \ -axis1 -1.0 0.0 0.0 \ -axis2 0.0 -1.0 0.0 \ -axis3 0.0 0.0 1.0 - - -name Mocap Model - -#if LOWER_EXTREMITY -muscle_file mocap.msl -#endif - -bone_path mocap_bones - -/* These are the units of the mocap model */ -length_units m -force_units N - -marker_radius 0.005 -marker_visibility off - -/* Defaults for solver 1.2.3 and later versions */ - -solver_accuracy 0.00010 -solver_method LM -solver_orient_body yes /* no longer used */ -solver_joint_limits yes -solver_max_iterations 100 - -/* If you want to make the ground-reaction force vector - * in the SIMM model window bigger/smaller/thinner/thicker, - * change the scaling factors shown below. The length of the - * vector (in meters) is the Y scale factor multiplied - * by the magnitude of the force in Newtons. - */ -beginmotionobject force -scale 1.0 0.002 1.0 -endmotionobject - - -/************************************* SEGMENTS *******************************/ -beginsegment ground -endsegment - -#if TWO_PELVIS_JOINTS -beginsegment pelvic_frame -begingroups spine torso right leg_r hip_r left leg_l hip_l endgroups -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.001 0.0 -endsegment -#endif - -beginsegment pelvis -bone pelvis_rvR.asc -bone pelvis_lvR.asc -bone sacrumvR.asc -material my_bone -begingroups spine torso right leg_r hip_r left leg_l hip_l endgroups -gait_scale PELVIS PELVIS_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 -1.0 0.0 -htr_y -1.0 0.0 0.0 -endsegment - -beginsegment pelvis_marker -marker V.Sacral -0.155 0.0 0.0 PELVIS_WEIGHT -marker Sacral -0.155 0.0 0.0 PELVIS_WEIGHT -marker V.Sacrum -0.155 0.0 0.0 PELVIS_WEIGHT -marker Sacrum -0.155 0.0 0.0 PELVIS_WEIGHT -marker SACR -0.155 0.0 0.0 PELVIS_WEIGHT -marker VSAC -0.155 0.0 0.0 PELVIS_WEIGHT -marker R.ASIS 0.0 0.0 0.128 PELVIS_WEIGHT -marker RASIS 0.0 0.0 0.128 PELVIS_WEIGHT -marker RASI 0.0 0.0 0.128 PELVIS_WEIGHT -marker R.PSIS -0.148 0.000 0.047 PELVIS_WEIGHT -marker RPSIS -0.148 0.000 0.047 PELVIS_WEIGHT -marker RPSI -0.148 0.000 0.047 PELVIS_WEIGHT -marker R.Trochanter -0.005 -0.042 0.093 PELVIS_WEIGHT -marker L.ASIS 0.0 0.0 -0.128 PELVIS_WEIGHT -marker LASIS 0.0 0.0 -0.128 PELVIS_WEIGHT -marker LASI 0.0 0.0 -0.128 PELVIS_WEIGHT -marker L.PSIS -0.148 0.000 -0.047 PELVIS_WEIGHT -marker LPSIS -0.148 0.000 -0.047 PELVIS_WEIGHT -marker LPSI -0.148 0.000 -0.047 PELVIS_WEIGHT -marker L.Trochanter -0.005 -0.042 -0.093 PELVIS_WEIGHT -gait_scale PELVIS PELVIS_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 -0.001 0.0 -htr_y -0.001 0.0 0.0 -endsegment - -#if UPPER_EXTREMITY -/****************** Lower Torso **********************/ -beginsegment lumbar1 -bone lumbar1.asc -material my_bone -begingroups lumbar torso spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment lumbar2 -bone lumbar2.asc -material my_bone -begingroups lumbar torso spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment lumbar3 -bone lumbar3.asc -material my_bone -begingroups lumbar torso spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment lumbar4 -bone lumbar4.asc -material my_bone -begingroups lumbar torso spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment lumbar5 -bone lumbar5.asc -material my_bone -begingroups lumbar torso spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -/********************** Thorax *********************/ -beginsegment thorax -bone ribcage.asc -bone thoracic1.asc -bone thoracic2.asc -bone thoracic3.asc -bone thoracic4.asc -bone thoracic5.asc -bone thoracic6.asc -bone thoracic7.asc -bone thoracic8.asc -bone thoracic9.asc -bone thoracic10.asc -bone thoracic11.asc -bone thoracic12.asc -material my_bone -begingroups torso spine endgroups -gait_scale TORSO TORSO_SIZE -marker Offset 0.06 0.26 0.0 OTHER_WEIGHT -marker T10 -0.12 0.09 0.0 OTHER_WEIGHT -marker CLAV 0.02 0.33 0.0 OTHER_WEIGHT -marker Sternum 0.07 0.24 0.0 OTHER_WEIGHT -marker STRN 0.07 0.24 0.0 OTHER_WEIGHT -marker RBAK -0.15 0.22 0.0 OTHER_WEIGHT -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -/******************** Neck and Head **********************/ -beginsegment cerv1 -bone cerv1.asc -material my_bone -begingroups neck spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment cerv2 -bone cerv2.asc -material my_bone -begingroups neck spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment cerv3 -bone cerv3.asc -material my_bone -begingroups neck spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment cerv4 -bone cerv4.asc -material my_bone -begingroups neck spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment cerv5 -bone cerv5.asc -material my_bone -begingroups neck spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment cerv6 -bone cerv6.asc -material my_bone -begingroups neck spine endgroups -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment cerv7 -bone cerv7.asc -material my_bone -begingroups neck spine endgroups -marker C7 0.0 0.0 0.0 OTHER_WEIGHT -marker C7 Spinous Process 0.0 0.0 0.0 OTHER_WEIGHT -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 1.0 0.0 -endsegment - -beginsegment skull -bone skull.asc -bone jaw.asc -material my_bone -begingroups head endgroups -gait_scale HEAD HEAD_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.2 0.0 -endsegment - -beginsegment OT_head -material my_bone -begingroups head endgroups -marker Rear.Head 0.0 0.0 0.0 OTHER_WEIGHT -marker Head.Rear 0.0 0.0 0.0 OTHER_WEIGHT -marker RearHead 0.0 0.0 0.0 OTHER_WEIGHT -marker HeadRear 0.0 0.0 0.0 OTHER_WEIGHT -marker Front.Head 0.0 0.19 0.0 OTHER_WEIGHT -marker Head.Front 0.0 0.19 0.0 OTHER_WEIGHT -marker FrontHead 0.0 0.19 0.0 OTHER_WEIGHT -marker HeadFront 0.0 0.19 0.0 OTHER_WEIGHT -marker Top.Head -0.06 0.095 0.0 OTHER_WEIGHT -marker Head.Top -0.06 0.095 0.0 OTHER_WEIGHT -marker TopHead -0.06 0.095 0.0 OTHER_WEIGHT -marker HeadTop -0.06 0.095 0.0 OTHER_WEIGHT -marker R.Ear 0.0 0.0 0.0 OTHER_WEIGHT -marker L.Ear 0.0 0.0 0.0 OTHER_WEIGHT -marker RBHD 0.0 0.0 0.0 OTHER_WEIGHT -marker RFHD 0.0 0.0 0.0 OTHER_WEIGHT -marker LBHD 0.0 0.0 0.0 OTHER_WEIGHT -marker LFHD 0.0 0.0 0.0 OTHER_WEIGHT -marker HEDO 0.0 0.0 0.0 OTHER_WEIGHT -marker HEDP 0.0 0.0 0.0 OTHER_WEIGHT -marker HEDA 0.0 0.0 0.0 OTHER_WEIGHT -marker HEDL 0.0 0.0 0.0 OTHER_WEIGHT -gait_scale HEAD HEAD_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.001 0.0 -endsegment - -/********************** Right Shoulder *****************/ -beginsegment clavicle_r -bone clavicle_rv.asc -material my_bone -begingroups right shoulder_r endgroups -marker R.Clavicle 0.03 0.0 0.0 OTHER_WEIGHT -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.0 1.0 -endsegment - -beginsegment clavicle_marker_r -material my_bone -begingroups right shoulder_r endgroups -marker R.Shoulder 0.0 0.0 0.165 OTHER_WEIGHT -marker RSHO 0.0 0.0 0.165 OTHER_WEIGHT -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.0 0.001 -endsegment - -beginsegment scapula_r -bone scapula_rv.asc -begingroups right shoulder_r endgroups -material my_bone -marker R.Scapula -0.12 -0.10 -0.08 OTHER_WEIGHT -marker R.Scapula.Top 0.0 0.0 0.0 OTHER_WEIGHT -marker R.Scapula.Bottom 0.0 0.0 0.0 OTHER_WEIGHT -marker R.Angulus Acromialis -0.055 -0.015 0.015 OTHER_WEIGHT -marker R.Trigonum Spinae -0.09 -0.045 -0.05 OTHER_WEIGHT -marker R.Angulus Inferior -0.10 -0.17 -0.04 OTHER_WEIGHT -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.0 0.001 -endsegment - -/************************* Right Arm **********************/ -beginsegment humerus_r -bone humerus_rv.asc -material my_bone -begingroups right shoulder_r arm_r elbow_r endgroups -marker R.Bicep 0.04 -0.15 0.0 ARM_WEIGHT -marker R.Tricep -0.04 -0.15 0.0 ARM_WEIGHT -marker R.Biceps.Lateral 0.03 -0.15 0.02 ARM_WEIGHT -marker RUPA 0.03 -0.15 0.02 ARM_WEIGHT -gait_scale R_UPPER_ARM UPPER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment elbow_marker_r -material my_bone -begingroups right arm_r elbow_r endgroups -marker R.Elbow -0.01 -0.281 0.04 ARM_WEIGHT -marker R.Elbow.Lateral -0.01 -0.281 0.04 ARM_WEIGHT -marker RELB -0.01 -0.281 0.04 ARM_WEIGHT -marker R.Elbow.Medial 0.0 -0.281 -0.04 ARM_WEIGHT -marker R.Elbow.Med 0.0 -0.281 -0.04 ARM_WEIGHT -gait_scale R_UPPER_ARM UPPER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment ulna_r -beginfiles ulna_rv.asc endfiles -material my_bone -begingroups right arm_r elbow_r endgroups -marker R.Forearm 0.0 -0.15 0.0 WRIST_WEIGHT -marker RFRA 0.0 -0.15 0.0 WRIST_WEIGHT -gait_scale R_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment radius_r -bone radius_rv.asc -material my_bone -begingroups right arm_r elbow_r endgroups -gait_scale R_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment wrist_marker_r -material my_bone -begingroups right arm_r elbow_r endgroups -marker R.Wrist -0.025 -0.245 0.006 WRIST_WEIGHT -marker R.Wrist.Lateral -0.025 -0.245 0.006 WRIST_WEIGHT -marker R.Wrist.Lat -0.025 -0.245 0.006 WRIST_WEIGHT -marker R.Wrist.Medial 0.035 -0.245 0.000 WRIST_WEIGHT -marker R.Wrist.Med 0.035 -0.245 0.000 WRIST_WEIGHT -marker R.Ulna 0.0 -0.245 -0.03 WRIST_WEIGHT -marker R.Radius 0.0 -0.245 0.04 WRIST_WEIGHT -marker RWRI -0.025 -0.245 0.006 WRIST_WEIGHT -marker RWRA -0.025 -0.245 0.006 WRIST_WEIGHT -marker RWRB 0.035 -0.245 0.000 WRIST_WEIGHT -gait_scale R_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -/*************************** Right Wrist and Hand ************/ -#if !(RIGHT_HAND_FULL) - -beginsegment wflex_r -material my_bone -begingroups right arm_r endgroups -gait_scale R_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment hand_r -bone pisiform_rvs.asc -bone lunate_rvs.asc -bone scaphoid_rvs.asc -bone triquetrum_rvs.asc -bone hamate_rvs.asc -bone capitate_rvs.asc -bone trapezoid_rvs.asc -bone trapezium_rvs.asc -bone metacarpal2_rvs.asc -bone index_proximal_rvs.asc -bone index_medial_rvs.asc -bone index_distal_rvs.asc -bone metacarpal3_rvs.asc -bone middle_proximal_rvs.asc -bone middle_medial_rvs.asc -bone middle_distal_rvs.asc -bone metacarpal4_rvs.asc -bone ring_proximal_rvs.asc -bone ring_medial_rvs.asc -bone ring_distal_rvs.asc -bone metacarpal5_rvs.asc -bone little_proximal_rvs.asc -bone little_medial_rvs.asc -bone little_distal_rvs.asc -bone metacarpal1_rvs.asc -bone thumb_proximal_rvs.asc -bone thumb_distal_rvs.asc -material my_bone -begingroups right arm_r endgroups -marker R.Hand -0.03 -0.05 0.0 OTHER_WEIGHT -marker RFIN -0.03 -0.05 0.0 OTHER_WEIGHT -gait_scale R_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.1 0.0 -endsegment - -#endif - -/************************ Left Shoulder *********************/ -beginsegment clavicle_l -bone clavicle_lv.asc -material my_bone -begingroups left shoulder_l endgroups -marker L.Clavicle 0.03 0.0 0.0 OTHER_WEIGHT -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.0 -1.0 -endsegment - -beginsegment clavicle_marker_l -material my_bone -begingroups left shoulder_l endgroups -marker L.Shoulder 0.0 0.0 -0.165 OTHER_WEIGHT -marker LSHO 0.0 0.0 -0.165 OTHER_WEIGHT -gait_scale TORSO TORSO_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.0 -0.001 -endsegment - -beginsegment scapula_l -bone scapula_lv.asc -material my_bone -begingroups left shoulder_l endgroups -gait_scale TORSO TORSO_SIZE -marker L.Scapula -0.12 -0.10 0.08 OTHER_WEIGHT -marker L.Scapula.Top 0.0 0.0 0.0 OTHER_WEIGHT -marker L.Scapula.Bottom 0.0 0.0 0.0 OTHER_WEIGHT -marker L.Angulus Acromialis -0.055 -0.015 -0.015 OTHER_WEIGHT -marker L.Trigonum Spinae -0.09 -0.045 0.05 OTHER_WEIGHT -marker L.Angulus Inferior -0.10 -0.17 0.04 OTHER_WEIGHT -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.0 -0.001 -endsegment - -/**************************** Left Arm ***************/ -beginsegment humerus_l -bone humerus_lv.asc -material my_bone -begingroups left shoulder_l arm_l elbow_l endgroups -marker L.Bicep 0.04 -0.15 0.0 ARM_WEIGHT -marker L.Tricep -0.04 -0.15 0.0 ARM_WEIGHT -marker L.Biceps.Lateral 0.03 -0.15 -0.02 ARM_WEIGHT -marker LUPA 0.03 -0.15 -0.02 ARM_WEIGHT -gait_scale L_UPPER_ARM UPPER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment elbow_marker_l -material my_bone -begingroups left arm_l elbow_l endgroups -marker L.Elbow 0.01 -0.281 -0.04 ARM_WEIGHT -marker L.Elbow.Lateral 0.01 -0.281 -0.04 ARM_WEIGHT -marker L.Elbow.Lat 0.01 -0.281 -0.04 ARM_WEIGHT -marker LELB 0.01 -0.281 -0.04 ARM_WEIGHT -marker L.Elbow.Medial -0.02 -0.281 0.04 ARM_WEIGHT -marker L.Elbow.Med -0.02 -0.281 0.04 ARM_WEIGHT -gait_scale L_UPPER_ARM UPPER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment ulna_l -bone ulna_lv.asc -material my_bone -begingroups left arm_l elbow_l endgroups -marker L.Forearm 0.0 -0.15 0.0 WRIST_WEIGHT -marker LFRA 0.0 -0.15 0.0 WRIST_WEIGHT -gait_scale L_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment radius_l -bone radius_lv.asc -material my_bone -begingroups left arm_l elbow_l endgroups -gait_scale L_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment wrist_marker_l -material my_bone -begingroups left arm_l elbow_l endgroups -marker L.Wrist -0.025 -0.245 -0.006 WRIST_WEIGHT -marker L.Wrist.Lateral -0.025 -0.245 -0.006 WRIST_WEIGHT -marker L.Wrist.Lat -0.025 -0.245 -0.006 WRIST_WEIGHT -marker L.Wrist.Medial 0.035 -0.245 0.000 WRIST_WEIGHT -marker L.Wrist.Med 0.035 -0.245 0.000 WRIST_WEIGHT -marker L.Ulna 0.0 -0.245 0.03 WRIST_WEIGHT -marker L.Radius 0.0 -0.245 -0.04 WRIST_WEIGHT -marker LWRI -0.025 -0.245 -0.006 WRIST_WEIGHT -marker LWRA -0.025 -0.245 -0.006 WRIST_WEIGHT -marker LWRB 0.025 -0.245 0.000 WRIST_WEIGHT -gait_scale L_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 0.001 0.0 -endsegment - -/*********************** Left Wrist and Hand ****************/ - -#if !(LEFT_HAND_FULL) - -beginsegment wflex_l -material my_bone -begingroups left arm_l endgroups -gait_scale L_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment hand_l -bone pisiform_lvs.asc -bone lunate_lvs.asc -bone scaphoid_lvs.asc -bone triquetrum_lvs.asc -bone hamate_lvs.asc -bone capitate_lvs.asc -bone trapezoid_lvs.asc -bone trapezium_lvs.asc -bone metacarpal2_lvs.asc -bone index_proximal_lvs.asc -bone index_medial_lvs.asc -bone index_distal_lvs.asc -bone metacarpal3_lvs.asc -bone middle_proximal_lvs.asc -bone middle_medial_lvs.asc -bone middle_distal_lvs.asc -bone metacarpal4_lvs.asc -bone ring_proximal_lvs.asc -bone ring_medial_lvs.asc -bone ring_distal_lvs.asc -bone metacarpal5_lvs.asc -bone little_proximal_lvs.asc -bone little_medial_lvs.asc -bone little_distal_lvs.asc -bone metacarpal1_lvs.asc -bone thumb_proximal_lvs.asc -bone thumb_distal_lvs.asc -material my_bone -begingroups left arm_l endgroups -marker L.Hand -0.03 -0.05 0.0 OTHER_WEIGHT -marker LFIN -0.03 -0.05 0.0 OTHER_WEIGHT -gait_scale L_LOWER_ARM LOWER_ARM_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.1 0.0 -endsegment - -#endif - -#endif /* UPPER_EXTREMITY */ - -#if LOWER_EXTREMITY - -/************************ Right Leg and Foot ***************/ -beginsegment femur_r -bone femur_r.asc -material my_bone -begingroups right leg_r hip_r knee_r endgroups -marker R.Knee 0.0 -0.404 0.05 KNEE_WEIGHT -marker R.Knee.Lateral 0.0 -0.404 0.05 KNEE_WEIGHT -marker R.Knee.Lat 0.0 -0.404 0.05 KNEE_WEIGHT -marker RKNE 0.0 -0.404 0.05 KNEE_WEIGHT -marker R.Knee.Medial 0.0 -0.404 -0.05 KNEE_WEIGHT -marker R.Knee.Med 0.0 -0.404 -0.05 KNEE_WEIGHT -marker R.Thigh 0.0179 -0.2240 0.1147 KNEE_WEIGHT -marker R.Thigh.Upper 0.0179 -0.2640 0.0647 KNEE_WEIGHT -marker R.Thigh.Front 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Thigh.Rear -0.0479 -0.3240 0.0047 KNEE_WEIGHT -marker RTHI 0.0179 -0.2640 0.0647 KNEE_WEIGHT -marker R.Thigh.M1 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Thigh.M2 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Thigh.M3 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Femur.Medial 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Femur.Lateral 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Femur.Anterior 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Femur.Posterior 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Femur.Proximal 0.0800 -0.3240 0.0047 KNEE_WEIGHT -marker R.Femur.Distal 0.0800 -0.3240 0.0047 KNEE_WEIGHT -gait_scale R_THIGH THIGH_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment tibia_r -bone tibia_r.asc -bone fibula_r.asc -material my_bone -begingroups right leg_r knee_r ankle_r endgroups -marker R.Ankle -0.005 -0.41 0.053 FOOT_WEIGHT -marker R.Ankle.Lateral -0.005 -0.41 0.053 FOOT_WEIGHT -marker R.Ankle.Lat -0.005 -0.41 0.053 FOOT_WEIGHT -marker RANK -0.005 -0.41 0.053 FOOT_WEIGHT -marker R.Ankle.Medial 0.006 -0.3888 -0.038 FOOT_WEIGHT -marker R.Ankle.Med 0.006 -0.3888 -0.038 FOOT_WEIGHT -marker R.Shank 0.0104 -0.2322 0.0748 OTHER_WEIGHT -marker R.Shank.Upper 0.0125 -0.3196 0.0600 OTHER_WEIGHT -marker R.Shank.Front 0.0425 -0.3596 0.0000 OTHER_WEIGHT -marker R.Shank.Rear -0.0325 -0.3596 0.0000 OTHER_WEIGHT -marker RTIB 0.0125 -0.3196 0.0600 OTHER_WEIGHT -marker R.Shank.M1 -0.008 -0.08 0.055 KNEE_WEIGHT fixed /* fibular head */ -marker R.Shank.M2 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker R.Shank.M3 0.0425 -0.3596 0.0000 KNEE_WEIGHT fixed /* medial tibia */ -marker R.Shank.M4 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker R.Shank.M5 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker R.Fibula.Head 0.0256 -0.04157 0.05976 KNEE_WEIGHT fixed -marker R.Tibia.medial 0.02398 -0.05473 -0.04540 KNEE_WEIGHT fixed -marker R.Tibia.tub 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker R.Tibia.prox 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker R.Tibia.distal 0.0425 -0.3596 0.0000 KNEE_WEIGHT -gait_scale R_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment patella_r -bone patella_r.asc -material my_bone -begingroups right leg_r knee_r endgroups -gait_scale R_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -#if VARUS_VALGUS -beginsegment tibia_vv_lat_r -material my_bone -begingroups right leg_r knee_r endgroups -gait_scale R_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment tibia_vv_med_r -material my_bone -begingroups right leg_r knee_r endgroups -gait_scale R_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment tibia_vv_norm_r -material my_bone -begingroups right leg_r knee_r endgroups -gait_scale R_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment patella_vv_tmp_r -material my_bone -begingroups right leg_r knee_r endgroups -gait_scale R_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment -#endif - -beginsegment talus_r -bone talus_rv.asc -material my_bone -begingroups right leg_r foot_r ankle_r endgroups -gait_scale R_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment foot_r -bone calcaneous_rv.asc -bone navicular_rv.asc -bone cuboid_rv.asc -bone first_cuneiform_rv.asc -bone second_cuneiform_rv.asc -bone third_cuneiform_rv.asc -bone metatarsal1_rv.asc -bone metatarsal2_rv.asc -bone metatarsal3_rv.asc -bone metatarsal4_rv.asc -bone metatarsal5_rv.asc -material my_bone -begingroups right leg_r ankle_r foot_r endgroups -gait_scale R_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 1.0 0.0 0.0 -endsegment - -beginsegment foot_marker_r -material my_bone -begingroups right leg_r ankle_r foot_r endgroups -marker R.Toe 0.190 0.0 0.0 FOOT_WEIGHT -marker RTOE 0.190 0.0 0.0 FOOT_WEIGHT -marker R.MedFoot 0.13 0.0 -0.03 FOOT_WEIGHT -marker R.LatFoot 0.13 0.0 0.05 FOOT_WEIGHT -marker R.Heel 0.0 0.0 0.0 FOOT_WEIGHT -marker RHEE 0.0 0.0 0.0 FOOT_WEIGHT -marker RMT5 0.0 0.0 0.0 FOOT_WEIGHT -gait_scale R_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.001 0.0 0.0 -endsegment - -beginsegment toes_r -bone prox_phalanx1_rvs.asc -bone prox_phalanx2_rvs.asc -bone prox_phalanx3_rvs.asc -bone prox_phalanx4_rvs.asc -bone prox_phalanx5_rvs.asc -bone mid_phalanx2_rvs.asc -bone mid_phalanx3_rvs.asc -bone mid_phalanx4_rvs.asc -bone mid_phalanx5_rvs.asc -bone distal_phalanx1_rvs.asc -bone distal_phalanx2_rvs.asc -bone distal_phalanx3_rvs.asc -bone distal_phalanx4_rvs.asc -bone distal_phalanx5_rvs.asc -material my_bone -begingroups right leg_r foot_r endgroups -gait_scale R_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.05 0.0 0.0 -endsegment - -/************************ Left Leg and Foot ********************/ -beginsegment femur_l -bone femur_l.asc -material my_bone -begingroups left leg_l hip_l knee_l endgroups -marker L.Knee 0.0 -0.404 -0.05 KNEE_WEIGHT -marker L.Knee.Lateral 0.0 -0.404 -0.05 KNEE_WEIGHT -marker L.Knee.Lat 0.0 -0.404 -0.05 KNEE_WEIGHT -marker LKNE 0.0 -0.404 -0.05 KNEE_WEIGHT -marker L.Knee.Medial 0.0 -0.404 0.05 KNEE_WEIGHT -marker L.Knee.Med 0.0 -0.404 0.05 KNEE_WEIGHT -marker L.Thigh 0.0179 -0.2240 -0.1147 KNEE_WEIGHT -marker L.Thigh.Upper 0.0179 -0.2640 -0.0647 KNEE_WEIGHT -marker L.Thigh.Front 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Thigh.Rear -0.0479 -0.3240 -0.0047 KNEE_WEIGHT -marker LTHI 0.0179 -0.2640 -0.0647 KNEE_WEIGHT -marker L.Thigh.M1 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Thigh.M2 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Thigh.M3 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Femur.Medial 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Femur.Lateral 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Femur.Anterior 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Femur.Posterior 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Femur.Proximal 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -marker L.Femur.Distal 0.0800 -0.3240 -0.0047 KNEE_WEIGHT -gait_scale L_THIGH THIGH_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment tibia_l -bone tibia_l.asc -bone fibula_l.asc -material my_bone -begingroups left leg_l knee_l ankle_l endgroups -marker L.Ankle -0.005 -0.41 -0.053 FOOT_WEIGHT -marker L.Ankle.Lateral -0.005 -0.41 -0.053 FOOT_WEIGHT -marker L.Ankle.Lat -0.005 -0.41 -0.053 FOOT_WEIGHT -marker LANK -0.005 -0.41 -0.053 FOOT_WEIGHT -marker L.Ankle.Medial 0.006 -0.3888 0.038 FOOT_WEIGHT -marker L.Ankle.Med 0.006 -0.3888 0.038 FOOT_WEIGHT -marker L.Shank 0.0104 -0.2322 -0.0748 OTHER_WEIGHT -marker L.Shank.Upper 0.0125 -0.3196 -0.0600 OTHER_WEIGHT -marker L.Shank.Front 0.0425 -0.3596 0.0000 OTHER_WEIGHT -marker L.Shank.Rear -0.0325 -0.3596 0.0000 OTHER_WEIGHT -marker LTIB 0.0125 -0.3196 -0.0600 OTHER_WEIGHT -marker L.Shank.M1 -0.008 -0.08 -0.055 KNEE_WEIGHT fixed /* fibular head */ -marker L.Shank.M2 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker L.Shank.M3 0.0425 -0.3596 0.0000 KNEE_WEIGHT fixed /* medial tibia */ -marker L.Shank.M4 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker L.Shank.M5 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker L.Fibula.Head 0.0256 -0.04157 -0.05976 KNEE_WEIGHT fixed -marker L.Tibia.medial 0.02398 -0.05473 0.04540 KNEE_WEIGHT fixed -marker L.Tibia.tub 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker L.Tibia.prox 0.0425 -0.3596 0.0000 KNEE_WEIGHT -marker L.Tibia.distal 0.0425 -0.3596 0.0000 KNEE_WEIGHT -gait_scale L_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment patella_l -bone patella_l.asc -material my_bone -begingroups left leg_l knee_l endgroups -gait_scale L_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -#if VARUS_VALGUS -beginsegment tibia_vv_lat_l -material my_bone -begingroups left leg_l knee_l endgroups -gait_scale L_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment tibia_vv_med_l -material my_bone -begingroups left leg_l knee_l endgroups -gait_scale L_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment tibia_vv_norm_l -material my_bone -begingroups left leg_l knee_l endgroups -gait_scale L_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment patella_vv_tmp_l -material my_bone -begingroups left leg_l knee_l endgroups -gait_scale L_SHANK SHANK_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment -#endif - -beginsegment talus_l -bone talus_lv.asc -material my_bone -begingroups left leg_l foot_l ankle_l endgroups -gait_scale L_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment foot_l -bone calcaneous_lv.asc -bone navicular_lv.asc -bone cuboid_lv.asc -bone first_cuneiform_lv.asc -bone second_cuneiform_lv.asc -bone third_cuneiform_lv.asc -bone metatarsal1_lv.asc -bone metatarsal2_lv.asc -bone metatarsal3_lv.asc -bone metatarsal4_lv.asc -bone metatarsal5_lv.asc -material my_bone -begingroups left leg_l ankle_l foot_l endgroups -gait_scale L_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 1.0 0.0 0.0 -endsegment - -beginsegment foot_marker_l -material my_bone -begingroups left leg_l ankle_l foot_l endgroups -marker L.Toe 0.190 0.0 0.0 FOOT_WEIGHT -marker LTOE 0.190 0.0 0.0 FOOT_WEIGHT -marker L.MedFoot 0.13 0.0 0.03 FOOT_WEIGHT -marker L.LatFoot 0.13 0.0 -0.05 FOOT_WEIGHT -marker L.Heel 0.0 0.0 0.0 FOOT_WEIGHT -marker LHEE 0.0 0.0 0.0 FOOT_WEIGHT -marker LMT5 0.0 0.0 0.0 FOOT_WEIGHT -gait_scale L_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.001 0.0 0.0 -endsegment - -beginsegment toes_l -bone prox_phalanx1_lvs.asc -bone prox_phalanx2_lvs.asc -bone prox_phalanx3_lvs.asc -bone prox_phalanx4_lvs.asc -bone prox_phalanx5_lvs.asc -bone mid_phalanx2_lvs.asc -bone mid_phalanx3_lvs.asc -bone mid_phalanx4_lvs.asc -bone mid_phalanx5_lvs.asc -bone distal_phalanx1_lvs.asc -bone distal_phalanx2_lvs.asc -bone distal_phalanx3_lvs.asc -bone distal_phalanx4_lvs.asc -bone distal_phalanx5_lvs.asc -material my_bone -begingroups left leg_l foot_l endgroups -gait_scale L_FOOT FOOT_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 0.0 0.0 1.0 -htr_y 0.05 0.0 0.0 -endsegment - -#endif /* LOWER_EXTREMITY */ - -/*********************************************/ -/* JOINTS */ -/*********************************************/ - -#if TWO_PELVIS_JOINTS - -beginjoint gnd_pelvic_frame -segments ground pelvic_frame -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx function f1(lower_torso_TX) -ty function f1(lower_torso_TY) -tz function f1(lower_torso_TZ) -r1 function f1(lower_torso_RX) -r2 function f1(lower_torso_RY) -r3 function f1(lower_torso_RZ) -endjoint - -beginjoint pelvic_frame_pelvis -segments pelvic_frame pelvis -order t r2 r3 r1 -axis1 -1.0 0.0 0.0 -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 -1.0 -tx constant 0.0 -ty constant 0.0 -tz constant 0.0 -r1 function f1(pelvic_lat_tilt) -r2 function f1(pelvic_rotation) -r3 function f1(pelvic_fwd_tilt) -endjoint - -#else - -beginjoint gnd_pelvis -segments ground pelvis -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx function f1(lower_torso_TX) -ty function f1(lower_torso_TY) -tz function f1(lower_torso_TZ) -r1 function f1(lower_torso_RX) -r2 function f1(lower_torso_RY) -r3 function f1(lower_torso_RZ) -endjoint - -#endif - -beginjoint pelvis_marker -segments pelvis pelvis_marker -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.0 -ty constant 0.0 -tz constant 0.0 -r1 constant 0.0 -r2 constant 0.0 -r3 constant 0.0 -endjoint - -#if UPPER_EXTREMITY - -/*************************** Lumbar Joints *****************/ -beginjoint pelvis_lum5 -segments pelvis lumbar5 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant -0.087200 -ty constant 0.033100 -tz constant 0.000000 -r1 function f151(lumbar_roll) -r2 function f152(lumbar_yaw) -r3 function f153(lumbar_pitch) -endjoint - -beginjoint lum5_lum4 -segments lumbar5 lumbar4 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.012300 -ty constant 0.032500 -tz constant 0.000000 -r1 function f141(lumbar_roll) -r2 function f142(lumbar_yaw) -r3 function f143(lumbar_pitch) -endjoint - -beginjoint lum4_lum3 -segments lumbar4 lumbar3 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.002000 -ty constant 0.034100 -tz constant 0.000000 -r1 function f131(lumbar_roll) -r2 function f132(lumbar_yaw) -r3 function f133(lumbar_pitch) -endjoint - -beginjoint lum3_lum2 -segments lumbar3 lumbar2 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant -0.005800 -ty constant 0.035400 -tz constant 0.000000 -r1 function f121(lumbar_roll) -r2 function f122(lumbar_yaw) -r3 function f123(lumbar_pitch) -endjoint - -beginjoint lum2_lum1 -segments lumbar2 lumbar1 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant -0.011200 -ty constant 0.029100 -tz constant 0.000000 -r1 function f111(lumbar_roll) -r2 function f112(lumbar_yaw) -r3 function f113(lumbar_pitch) -endjoint - -beginjoint thor12jnt -segments lumbar1 thorax -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.024900 -ty constant -0.014200 -tz constant 0.000000 -r3 constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -endjoint - -/*************************** Neck Joints **********************/ -beginjoint thor1_cerv7 -segments thorax cerv7 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant -0.047500 -ty constant 0.361300 -tz constant 0.000000 -r1 function f371(neck_roll) -r2 function f372(neck_yaw) -r3 function f373(neck_pitch) -endjoint - -beginjoint cerv7_cerv6 -segments cerv7 cerv6 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.005620 -ty constant 0.018030 -tz constant 0.000000 -r1 function f361(neck_roll) -r2 function f362(neck_yaw) -r3 function f363(neck_pitch) -endjoint - -beginjoint cerv6_cerv5 -segments cerv6 cerv5 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.004760 -ty constant 0.016180 -tz constant 0.000000 -r1 function f351(neck_roll) -r2 function f352(neck_yaw) -r3 function f353(neck_pitch) -endjoint - -beginjoint cerv5_cerv4 -segments cerv5 cerv4 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.002760 -ty constant 0.015600 -tz constant 0.000000 -r1 function f341(neck_roll) -r2 function f342(neck_yaw) -r3 function f343(neck_pitch) -endjoint - -beginjoint cerv4_cerv3 -segments cerv4 cerv3 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.002300 -ty constant 0.018100 -tz constant 0.000000 -r1 function f331(neck_roll) -r2 function f332(neck_yaw) -r3 function f333(neck_pitch) -endjoint - -beginjoint cerv3_cerv2 -segments cerv3 cerv2 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.000540 -ty constant 0.014080 -tz constant 0.000000 -r1 function f321(neck_roll) -r2 function f322(neck_yaw) -r3 function f323(neck_pitch) -endjoint - -beginjoint cerv2_cerv1 -segments cerv2 cerv1 -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.001460 -ty constant 0.032510 -tz constant 0.000000 -r1 function f311(neck_roll) -r2 function f312(neck_yaw) -r3 function f313(neck_pitch) -endjoint - -beginjoint cerv1_skull -segments cerv1 skull -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.000900 -ty constant 0.011400 -tz constant 0.000000 -r1 function f301(neck_roll) -r2 function f302(neck_yaw) -r3 function f303(neck_pitch) -endjoint - -beginjoint skull_OT_head -segments skull OT_head -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant -0.090000 -ty constant 0.110000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant -90.000000 -endjoint - -/***************************** Right Shoulder *****************/ -beginjoint sternoclavicular_r -segments thorax clavicle_r -order t r2 r3 r1 -DEFAULT_RIGHT_AXES -tx constant -0.011000 -ty constant 0.338000 -tz constant 0.022000 -#if COMPLEX_CLAVICLE -r1 function f1000(clav_add_r) -r2 function f1001(clav_rot_r) -r3 function f1(clav_flex_r) -#else -r1 constant -10.0 -r2 constant -20.0 -r3 constant 0.0 -#endif -endjoint - -beginjoint clavicularmarker_r -segments clavicle_r clavicle_marker_r -order r1 r3 r2 t -DEFAULT_RIGHT_AXES -tx constant -0.06 -ty constant 0.045 -tz constant -0.022 -r1 constant 10.0 -r2 constant 20.0 -r3 constant 0.0 -endjoint - -beginjoint st_cl_inv_r -segments clavicle_r scapula_r -order t r1 r3 r2 -DEFAULT_RIGHT_AXES -tx constant 0.008688 -ty constant -0.001300 -tz constant 0.155441 -#if COMPLEX_CLAVICLE -r1 function f1(scap_add_r) -r2 function f1(scap_rot_r) -r3 function f1(scap_flex_r) -#else -r1 constant 10.0 -r2 constant 20.0 -r3 constant 0.0 -#endif -endjoint - -beginjoint acromial_inv_r -segments scapula_r humerus_r -order t r1 r3 r2 -DEFAULT_RIGHT_AXES -tx constant 0.005000 -ty constant -0.035000 -tz constant 0.030000 -r1 function f1(arm_add_r) -r2 function f1(arm_rot_r) -r3 function f1(arm_flex_r) -endjoint - - -/**************************** Right Arm ************************/ -beginjoint elbowmarker_r -segments humerus_r elbow_marker_r -order t r3 r2 r1 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.226047 0.022269 0.973862 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 15.000000 -r3 constant 3.000000 -endjoint - -beginjoint elbow_r -segments humerus_r ulna_r -order t r3 r2 r1 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.226047 0.022269 0.973862 -tx constant 0.013144 -ty constant -0.286273 -tz constant -0.009595 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1(elbow_flex_r) -endjoint - -beginjoint radioulnar_r -segments ulna_r radius_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.056398 0.998406 0.001952 -axis3 0.000000 0.000000 1.000000 -tx constant -0.006727 -ty constant -0.013007 -tz constant 0.026083 -r1 constant 0.000000 -r2 function f1(pro_sup_r) -r3 constant 0.000000 -endjoint - -beginjoint wristmarker_r -segments radius_r wrist_marker_r -order t r1 r2 r3 -DEFAULT_RIGHT_AXES -tx constant 0.006727 -ty constant 0.013007 -tz constant -0.026083 -r1 constant -8.000000 -r2 constant 10.000000 -r3 constant -4.000000 -endjoint - -/************************* Right Wrist and Hand *******************/ - -#if !(RIGHT_HAND_FULL) - -beginjoint radius_wflex_r -segments radius_r wflex_r -order t r1 r2 r3 -DEFAULT_RIGHT_AXES -tx constant -0.008797 -ty constant -0.225841 -tz constant 0.013610 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1400(wrist_flex_r) -endjoint - -beginjoint wflex_hand_r -segments wflex_r hand_r -order t r1 r2 r3 -DEFAULT_RIGHT_AXES -tx constant 0.000000 -ty constant -0.030000 -tz constant 0.000000 -r1 function f1403(wrist_dev_r) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -#endif - -/***************************** Left Shoulder ********************/ -beginjoint sternoclavicular_l -segments thorax clavicle_l -order t r2 r3 r1 -DEFAULT_LEFT_AXES -tx constant -0.011000 -ty constant 0.338000 -tz constant -0.022000 -#if COMPLEX_CLAVICLE -r1 function f1300(clav_add_l) -r2 function f1301(clav_rot_l) -r3 function f1(clav_flex_l) -#else -r1 constant -10.0 -r2 constant -20.0 -r3 constant 0.0 -#endif -endjoint - -beginjoint clavicularmarker_l -segments clavicle_l clavicle_marker_l -order r1 r3 r2 t -DEFAULT_LEFT_AXES -tx constant -0.06 -ty constant 0.045 -tz constant 0.022 -r1 constant 10.0 -r2 constant 20.0 -r3 constant 0.0 -endjoint - -beginjoint st_cl_inv_l -segments clavicle_l scapula_l -order t r1 r3 r2 -DEFAULT_LEFT_AXES -tx constant 0.008688 -ty constant -0.001300 -tz constant -0.155441 -#if COMPLEX_CLAVICLE -r1 function f1(scap_add_l) -r2 function f1(scap_rot_l) -r3 function f1(scap_flex_l) -#else -r1 constant 10.0 -r2 constant 20.0 -r3 constant 0.0 -#endif -endjoint - -beginjoint acromial_inv_l -segments scapula_l humerus_l -order t r1 r3 r2 -DEFAULT_LEFT_AXES -tx constant 0.005000 -ty constant -0.035000 -tz constant -0.030000 -r1 function f1(arm_add_l) -r2 function f1(arm_rot_l) -r3 function f1(arm_flex_l) -endjoint - - -/****************************** Left Arm **********************/ -beginjoint elbowmarker_l -segments humerus_l elbow_marker_l -order t r3 r2 r1 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 -0.226047 -0.022269 0.973862 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant -15.000000 -r3 constant 3.000000 -endjoint - -beginjoint elbow_l -segments humerus_l ulna_l -order t r3 r2 r1 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 -0.226047 -0.022269 0.973862 -tx constant 0.013144 -ty constant -0.286273 -tz constant 0.009595 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1(elbow_flex_l) -endjoint - -beginjoint radioulnar_l -segments ulna_l radius_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 -0.056398 -0.998406 0.001952 -axis3 0.000000 0.000000 1.000000 -tx constant -0.006727 -ty constant -0.013007 -tz constant -0.026083 -r1 constant 0.000000 -r2 function f1(pro_sup_l) -r3 constant 0.000000 -endjoint - -beginjoint wristmarker_l -segments radius_l wrist_marker_l -order t r1 r2 r3 -DEFAULT_RIGHT_AXES -tx constant 0.006727 -ty constant 0.013007 -tz constant 0.026083 -r1 constant 8.000000 -r2 constant -10.000000 -r3 constant -4.000000 -endjoint - -/***************************** Left Wrist and Hand ****************/ - -#if !(LEFT_HAND_FULL) - -beginjoint radius_wflex_l -segments radius_l wflex_l -order t r1 r2 r3 -DEFAULT_LEFT_AXES -tx constant -0.008797 -ty constant -0.225841 -tz constant -0.013610 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f1400(wrist_flex_l) -endjoint - -beginjoint wflex_hand_l -segments wflex_l hand_l -order t r1 r2 r3 -DEFAULT_LEFT_AXES -tx constant 0.000000 -ty constant -0.030000 -tz constant 0.000000 -r1 function f1403(wrist_dev_l) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -#endif - -#endif /* UPPER_EXTREMITY */ - -#if LOWER_EXTREMITY - -/************************** Right Leg *********************/ -beginjoint hip_r -segments pelvis femur_r -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant -0.0421 -ty constant -0.0839 -tz constant 0.0785 -r3 function f1(hip_flex_r) -r1 function f1(hip_add_r) -r2 function f1(hip_rot_r) -endjoint - -#if VARUS_VALGUS - -beginjoint knee_flex_r -segments femur_r tibia_vv_lat_r -order t r3 r1 r2 -axis1 1.0 0.0 0.0 -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 -1.0 -tx function f1600(knee_flex_r) -ty function f1601(knee_flex_r) -tz constant 0.000 -r1 constant 0.0 -r2 constant 0.0 -r3 function f1(knee_flex_r) -endjoint - -beginjoint knee_lat_vv_r -segments tibia_vv_lat_r tibia_vv_med_r -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.0 -ty constant -0.030 -tz constant 0.025 -r1 function f1610(knee_vv_r) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -beginjoint knee_med_vv_r -segments tibia_vv_med_r tibia_vv_norm_r -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.0 -ty constant 0.0 -tz constant -0.05 -r1 function f1611(knee_vv_r) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -beginjoint knee_final_r -segments tibia_vv_norm_r tibia_r -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx constant 0.0 -ty constant 0.030 -tz constant 0.025 -r1 constant 0.0 -r2 constant 0.0 -r3 constant 0.0 -endjoint - -beginjoint tib_pat_r -segments tibia_r patella_vv_tmp_r -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx function f1602(knee_flex_r) -ty function f1603(knee_flex_r) -tz constant 0.0024 -r3 function f1604(knee_flex_r) -r1 constant 0.0 -r2 constant 0.0 -endjoint - -beginjoint patella_vv_corr_r -segments patella_vv_tmp_r patella_r -order t r1 r2 r3 -DEFAULT_RIGHT_AXES -tx constant 0.0 -ty constant 0.0 -tz constant 0.0 -r1 function f1612(knee_vv_r) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -#else - -beginjoint knee_r -segments femur_r tibia_r -order t r3 r1 r2 -axis1 1.0 0.0 0.0 -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 -1.0 -tx function f1600(knee_flex_r) -ty function f1601(knee_flex_r) -tz constant 0.000 -r1 constant 0.0 -r2 constant 0.0 -r3 function f1(knee_flex_r) -endjoint - -beginjoint tib_pat_r -segments tibia_r patella_r -order t r3 r1 r2 -DEFAULT_RIGHT_AXES -tx function f1602(knee_flex_r) -ty function f1603(knee_flex_r) -tz constant 0.0024 -r3 function f1604(knee_flex_r) -r1 constant 0.0 -r2 constant 0.0 -endjoint - -#endif - -beginjoint ankle_r -segments tibia_r talus_r -order t r3 r1 r2 -axis1 1.0 0.0 0.0 -axis2 0.0 1.0 0.0 -axis3 -0.10453 -0.17365 0.97925 /* ankle axis from Isman and Inman (BOLEGS.IN)*/ -tx constant 0.0 -ty constant -0.423 -tz constant 0.0 -r3 function f1(ankle_flex_r) -r1 constant 0.0 -r2 constant 0.0 -endjoint - -/* defines joint between talus and calcanus frames. Translation before - * rotation from ankle joint center to origin of calcaneal frame */ -beginjoint subtalar_r -segments talus_r foot_r -order t r1 r2 r3 -axis1 0.78718 0.604747 -0.120949 /* subtalar axis from Isman and Inman */ -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 1.0 -tx constant -0.04877 -ty constant -0.04195 -tz constant 0.00792 -r1 function f1(subt_angle_r) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -/* defines joint between the SIMM foot and the OrthoTrak foot */ -beginjoint foot_mark_r -segments foot_r foot_marker_r -order t r1 r2 r3 -axis1 1.0 0.0 0.0 -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 1.0 -tx constant -0.020 -ty constant 0.030 -tz constant -0.010 -r1 constant 0.0 -r2 constant 0.0 -r3 constant 0.0 -endjoint - -/* defines joint between calcaneous and toes_prox frames. Translation before - * rotation from calcaneal frame to base of second metatarsal */ -beginjoint toes_r -segments foot_r toes_r -order t r1 r2 r3 -axis1 0.542 0.0 -0.841 -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 1.0 -tx constant 0.1768 -ty constant -0.002 -tz constant 0.00108 -r1 function f1(toe_angle_r) -r2 constant 0.0 -r3 constant 0.0 -endjoint - - -/************************* Left Leg ***********************/ -beginjoint hip_l -segments pelvis femur_l -order t r3 r1 r2 -DEFAULT_LEFT_AXES -tx constant -0.0421 -ty constant -0.0839 -tz constant -0.0785 -r3 function f1(hip_flex_l) -r1 function f1(hip_add_l) -r2 function f1(hip_rot_l) -endjoint - -#if VARUS_VALGUS - -beginjoint knee_flex_l -segments femur_l tibia_vv_lat_l -order t r3 r1 r2 -axis1 -1.0 0.0 0.0 -axis2 0.0 -1.0 0.0 -axis3 0.0 0.0 -1.0 -tx function f1700(knee_flex_l) -ty function f1701(knee_flex_l) -tz constant 0.0 -r1 constant 0.0 -r2 constant 0.0 -r3 function f1(knee_flex_l) -endjoint - -beginjoint knee_lat_vv_l -segments tibia_vv_lat_l tibia_vv_med_l -order t r3 r1 r2 -DEFAULT_LEFT_AXES -tx constant 0.0 -ty constant -0.030 -tz constant -0.025 -r1 function f1710(knee_vv_l) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -beginjoint knee_med_vv_l -segments tibia_vv_med_l tibia_vv_norm_l -order t r3 r1 r2 -DEFAULT_LEFT_AXES -tx constant 0.0 -ty constant 0.0 -tz constant 0.05 -r1 function f1711(knee_vv_l) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -beginjoint knee_final_l -segments tibia_vv_norm_l tibia_l -order t r3 r1 r2 -DEFAULT_LEFT_AXES -tx constant 0.0 -ty constant 0.030 -tz constant -0.025 -r1 constant 0.0 -r2 constant 0.0 -r3 constant 0.0 -endjoint - -beginjoint tib_pat_l -segments tibia_l patella_vv_tmp_l -order t r3 r1 r2 -DEFAULT_LEFT_AXES -tx function f1702(knee_flex_l) -ty function f1703(knee_flex_l) -tz constant -0.0024 -r3 function f1704(knee_flex_l) -r1 constant 0.0 -r2 constant 0.0 -endjoint - -beginjoint patella_vv_corr_l -segments patella_vv_tmp_l patella_l -order t r1 r2 r3 -DEFAULT_LEFT_AXES -tx constant 0.0 -ty constant 0.0 -tz constant 0.0 -r1 function f1612(knee_vv_l) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -#else - -beginjoint knee_l -segments femur_l tibia_l -order t r3 r1 r2 -axis1 -1.0 0.0 0.0 -axis2 0.0 -1.0 0.0 -axis3 0.0 0.0 -1.0 -tx function f1700(knee_flex_l) -ty function f1701(knee_flex_l) -tz constant 0.0 -r1 constant 0.0 -r2 constant 0.0 -r3 function f1(knee_flex_l) -endjoint - -beginjoint tib_pat_l -segments tibia_l patella_l -order t r3 r1 r2 -DEFAULT_LEFT_AXES -tx function f1702(knee_flex_l) -ty function f1703(knee_flex_l) -tz constant -0.0024 -r3 function f1704(knee_flex_l) -r1 constant 0.0 -r2 constant 0.0 -endjoint - -#endif - -beginjoint ankle_l -segments tibia_l talus_l -order t r3 r1 r2 -axis1 -1.0000 0.00000 0.00000 -axis2 0.00000 -1.0000 0.00000 -axis3 0.10453 0.17365 0.97925 /* ankle axis from Isman and Inman (BOLEGS.IN)*/ -tx constant 0.0 -ty constant -0.423 -tz constant 0.0 -r3 function f1(ankle_flex_l) -r1 constant 0.0 -r2 constant 0.0 -endjoint - -/* defines joint between talus and calcanus frames. Translation before - * rotation from ankle joint center to origin of calcaneal frame */ -beginjoint subtalar_l -segments talus_l foot_l -order t r1 r2 r3 -axis1 -0.78718 -0.604747 -0.120949 /* subtalar axis from Isman and Inman */ -axis2 0.00000 -1.000000 0.0 -axis3 0.00000 0.000000 1.0 -tx constant -0.04877 -ty constant -0.04195 -tz constant -0.00792 -r1 function f1(subt_angle_l) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -/* defines joint between the SIMM foot and the OrthoTrak foot */ -beginjoint foot_mark_l -segments foot_l foot_marker_l -order t r1 r2 r3 -axis1 1.0 0.0 0.0 -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 1.0 -tx constant -0.020 -ty constant 0.030 -tz constant 0.010 -r1 constant 0.0 -r2 constant 0.0 -r3 constant 0.0 -endjoint - -/* defines joint between calcaneous and toes_prox frames. Translation before - * rotation from calcaneal frame to base of second metatarsal */ -beginjoint toes_l -segments foot_l toes_l -order t r1 r2 r3 -axis1 -0.542 0.0 -0.841 -axis2 0.0 1.0 0.0 -axis3 0.0 0.0 1.0 -tx constant 0.1768 -ty constant -0.002 -tz constant -0.00108 -r1 function f1(toe_angle_l) -r2 constant 0.0 -r3 constant 0.0 -endjoint - -#endif /* LOWER_EXTREMITY */ - -/*********************************************/ -/* GENCOORDS */ -/*********************************************/ -begingencoord lower_torso_TX -range -10.0 10.0 -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ - default_value 0.969 -#endif -begingroups ground_LT endgroups -clamped yes -visible no -endgencoord - -begingencoord lower_torso_TY -range -10.0 10.0 -#if GROUND_PLANE_XZ - default_value 0.969 -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ -#endif -begingroups ground_LT endgroups -clamped yes -visible no -endgencoord - -begingencoord lower_torso_TZ -range -10.0 10.0 -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY - default_value 0.969 -#elif GROUND_PLANE_YZ -#endif -begingroups ground_LT endgroups -clamped yes -visible no -endgencoord - -begingencoord lower_torso_RX -#if WALKING -range -270.0 270.0 -#else -range -1800.0 1800.0 -#endif -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY - default_value 90.0 -#elif GROUND_PLANE_YZ -#endif -begingroups ground_LT endgroups -clamped yes -#if TWO_PELVIS_JOINTS -locked yes -#endif -endgencoord - -begingencoord lower_torso_RY -#if WALKING -range -270.0 270.0 -#else -range -1800.0 1800.0 -#endif -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ - default_value -90.0 -#endif -begingroups ground_LT endgroups -clamped yes -#if TWO_PELVIS_JOINTS -locked yes -#endif -endgencoord - -begingencoord lower_torso_RZ -#if WALKING -range -270.0 270.0 -#else -range -1800.0 1800.0 -#endif -#if GROUND_PLANE_XZ -#elif GROUND_PLANE_XY -#elif GROUND_PLANE_YZ - default_value -90.0 -#endif -begingroups ground_LT endgroups -clamped yes -#if TWO_PELVIS_JOINTS -locked yes -#endif -endgencoord - -#if TWO_PELVIS_JOINTS - -begingencoord pelvic_rotation -range -90.0 90.0 -begingroups pelvis endgroups -clamped no -locked no -endgencoord - -begingencoord pelvic_fwd_tilt -range -90.0 90.0 -begingroups pelvis endgroups -clamped no -locked no -default_value 13.0 -endgencoord - -begingencoord pelvic_lat_tilt -range -90.0 90.0 -begingroups pelvis endgroups -clamped no -locked no -endgencoord - -#endif - -#if UPPER_EXTREMITY - -begingencoord lumbar_pitch -range -45.0 45.0 -begingroups lumbar spine endgroups -default_value 13.0 -clamped yes -visible no -endgencoord - -begingencoord lumbar_roll -range -45.0 45.0 -begingroups lumbar spine endgroups -clamped yes -visible no -endgencoord - -begingencoord lumbar_yaw -range -60.0 60.0 -begingroups lumbar spine endgroups -clamped yes -visible no -endgencoord - -begingencoord neck_pitch -range -50.0 72.0 -begingroups neck spine endgroups -clamped yes -visible no -endgencoord - -begingencoord neck_roll -range -40.0 40.0 -begingroups neck spine endgroups -clamped yes -visible no -endgencoord - -begingencoord neck_yaw -range -66.0 66.0 -begingroups neck spine endgroups -clamped yes -visible no -endgencoord - -/****************** Right Shoulder *****************/ - -#if COMPLEX_CLAVICLE - -begingencoord clav_add_r -range -15.0 5.0 -default_value -10.619275 -begingroups right shoulder_r clavicle_r endgroups -clamped yes -visible no -endgencoord - -begingencoord clav_rot_r -range -5.0 15.0 -default_value 5.754241 -begingroups right shoulder_r clavicle_r endgroups -clamped yes -visible no -endgencoord - -begingencoord clav_flex_r -range -20.0 20.0 -default_value 3.097808 -begingroups right shoulder_r clavicle_r endgroups -clamped yes -visible no -endgencoord - -begingencoord scap_add_r -range -30.0 30.0 -default_value 18.134767 -begingroups right shoulder_r scapula_r endgroups -clamped yes -visible no -endgencoord - -begingencoord scap_rot_r -range -30.0 30.0 -default_value 6.140150 -begingroups right shoulder_r scapula_r endgroups -clamped yes -visible no -endgencoord - -begingencoord scap_flex_r -range -30.0 30.0 -default_value 3.09789 -begingroups right shoulder_r scapula_r endgroups -clamped yes -visible no -endgencoord - -#endif - -begingencoord arm_add_r -range -120.0 90.0 -#if ARMS_UP -default_value -90.0 -#endif -begingroups right shoulder_r arm_r endgroups -clamped yes -visible no -endgencoord - -begingencoord arm_rot_r -#if FULL_ARM_MOTION -range -90.0 90.0 -#else -range -10.0 10.0 -#endif -begingroups right shoulder_r arm_r endgroups -clamped yes -visible no -endgencoord - -begingencoord arm_flex_r -#if FULL_ARM_MOTION -range -90.0 90.0 -#else -range -45.0 90.0 -#endif -begingroups right shoulder_r arm_r endgroups -clamped yes -visible no -endgencoord - - -/******************** Right Arm ************************/ -begingencoord elbow_flex_r -range 0.0 130.0 -begingroups right arm_r elbow_r endgroups -clamped yes -visible no -endgencoord - -begingencoord pro_sup_r -#if FULL_ARM_MOTION || RIGHT_HAND_FULL -range 0.000000 90.0 -#else -range 50.000000 70.000000 -#endif -default_value 60.0 -begingroups right arm_r elbow_r endgroups -clamped yes -visible no -endgencoord - -/******************** Right Wrist and Hand *****************/ -begingencoord wrist_flex_r -range -70.000000 70.000000 -begingroups right arm_r wrist_r endgroups -clamped yes -visible no -endgencoord - -begingencoord wrist_dev_r -range -25.000000 35.000000 -begingroups right arm_r wrist_r endgroups -clamped yes -visible no -endgencoord - - -/*********************** Left Shoulder ****************************/ - -#if COMPLEX_CLAVICLE - -begingencoord clav_add_l -range -15.000000 5.000000 -default_value -10.619275 -begingroups left shoulder_l clavicle_l endgroups -clamped yes -visible no -endgencoord - -begingencoord clav_rot_l -range -5.000000 15.000000 -default_value 5.754241 -begingroups left shoulder_l clavicle_l endgroups -clamped yes -visible no -endgencoord - -begingencoord clav_flex_l -range -20.000000 20.000000 -default_value 3.097808 -begingroups left shoulder_l clavicle_l endgroups -clamped yes -visible no -endgencoord - -begingencoord scap_add_l -range -30.000000 30.000000 -default_value 18.134767 -begingroups left shoulder_l scapula_l endgroups -clamped yes -visible no -endgencoord - -begingencoord scap_rot_l -range -30.000000 30.000000 -default_value 6.140150 -begingroups left shoulder_l scapula_l endgroups -clamped yes -visible no -endgencoord - -begingencoord scap_flex_l -range -30.000000 30.000000 -default_value 3.09789 -begingroups left shoulder_l scapula_l endgroups -clamped yes -visible no -endgencoord - -#endif - -begingencoord arm_add_l -range -120.000000 90.000000 -#if ARMS_UP -default_value -90.0 -#endif -begingroups left shoulder_l arm_l endgroups -clamped yes -visible no -endgencoord - -begingencoord arm_rot_l -#if FULL_ARM_MOTION -range -90.000000 90.000000 -#else -range -10.000000 10.000000 -#endif -begingroups left shoulder_l arm_l endgroups -clamped yes -visible no -endgencoord - -begingencoord arm_flex_l -#if FULL_ARM_MOTION -range -90.000000 90.000000 -#else -range -45.000000 90.000000 -#endif -begingroups left shoulder_l arm_l endgroups -clamped yes -visible no -endgencoord - -/*********************** Left Arm ***************************/ -begingencoord elbow_flex_l -range 0.000000 130.000000 -begingroups left arm_l elbow_l endgroups -clamped yes -visible no -endgencoord - -begingencoord pro_sup_l -#if FULL_ARM_MOTION || LEFT_HAND_FULL -range 0.000000 90.000000 -#else -range 50.000000 70.000000 -#endif -default_value 60.0 -begingroups left arm_l elbow_l endgroups -clamped yes -visible no -endgencoord - -/********************** Left Wrist and Hand **********************/ -begingencoord wrist_flex_l -range -70.000000 70.000000 -begingroups left arm_l wrist_l endgroups -clamped yes -visible no -endgencoord - -begingencoord wrist_dev_l -range -25.000000 35.000000 -begingroups left arm_l wrist_l endgroups -clamped yes -visible no -endgencoord - -#endif /* UPPER_EXTREMITY */ - -#if LOWER_EXTREMITY - -/************************** Right Leg ************************/ -begingencoord hip_flex_r -range -11.0 95.0 -begingroups right leg_r hip_r endgroups -default_value 13.0 -clamped yes -endgencoord - -begingencoord hip_add_r -range -50.0 20.0 -begingroups right leg_r hip_r endgroups -clamped yes -endgencoord - -begingencoord hip_rot_r -range -40.0 40.0 -begingroups right leg_r hip_r endgroups -clamped yes -endgencoord - -begingencoord knee_flex_r -range -10.0 120.0 -begingroups right leg_r knee_r endgroups -clamped yes -endgencoord - -#if VARUS_VALGUS -begingencoord knee_vv_r -range -25.0 25.0 -begingroups right leg_r knee_r endgroups -clamped yes -endgencoord -#endif - -begingencoord ankle_flex_r -range -30.0 30.0 -begingroups right leg_r ankle_r endgroups -clamped yes -endgencoord - -begingencoord subt_angle_r -range -20.0 20.0 -begingroups right leg_r ankle_r endgroups -clamped yes -endgencoord - -begingencoord toe_angle_r -range -30.0 30.0 -begingroups right leg_r toes_r endgroups -clamped yes -endgencoord - -/*************************** Left Leg ****************************/ -begingencoord hip_flex_l -range -11.0 95.0 -begingroups left leg_l hip_l endgroups -default_value 13.0 -clamped yes -endgencoord - -begingencoord hip_add_l -range -50.0 20.0 -begingroups left leg_l hip_l endgroups -clamped yes -endgencoord - -begingencoord hip_rot_l -range -40.0 40.0 -begingroups left leg_l hip_l endgroups -clamped yes -endgencoord - -begingencoord knee_flex_l -range -10.0 120.0 -begingroups left leg_l knee_l endgroups -clamped yes -endgencoord - -#if VARUS_VALGUS -begingencoord knee_vv_l -range -25.0 25.0 -begingroups left leg_l knee_l endgroups -clamped yes -endgencoord -#endif - -begingencoord ankle_flex_l -range -30.0 30.0 -begingroups left leg_l ankle_l endgroups -clamped yes -endgencoord - -begingencoord subt_angle_l -range -20.0 20.0 -begingroups left leg_l ankle_l endgroups -clamped yes -endgencoord - -begingencoord toe_angle_l -range -30.0 30.0 -begingroups left leg_l toes_l endgroups -clamped yes -endgencoord - -#endif /* LOWER_EXTREMITY */ - -/*********************************************/ -/* FUNCTIONS */ -/*********************************************/ -beginfunction f1 -(-360.000000,-360.000000) -( 360.000000, 360.000000) -endfunction - -beginfunction f2 -(-360.000000, 360.000000) -( 360.000000,-360.000000) -endfunction - -#if UPPER_EXTREMITY - -/********** Lumbar *************/ -/* LUM1 about LUM2 */ -beginfunction f111 -(-100.0, -20.7) -( 100.0, 20.7) -endfunction - -beginfunction f112 -(-100.0, -22.2) -( 100.0, 22.2) -endfunction - -beginfunction f113 -(-100.0, -16.2) -( 100.0, 16.2) -endfunction - -/* LUM2 about LUM3 */ -beginfunction f121 -(-100.0, -20.7) -( 100.0, 20.7) -endfunction - -beginfunction f122 -(-100.0, -22.2) -( 100.0, 22.2) -endfunction - -beginfunction f123 -(-100.0, -18.9) -( 100.0, 18.9) -endfunction - -/* LUM3 about LUM4 */ -beginfunction f131 -(-100.0, -27.6) -( 100.0, 27.6) -endfunction - -beginfunction f132 -(-100.0, -22.2) -( 100.0, 22.2) -endfunction - -beginfunction f133 -(-100.0, -20.3) -( 100.0, 20.3) -endfunction - -/* LUM4 about LUM5 */ -beginfunction f141 -(-100.0, -20.7) -( 100.0, 20.7) -endfunction - -beginfunction f142 -(-100.0, -22.2) -( 100.0, 22.2) -endfunction - -beginfunction f143 -(-100.0, -21.6) -( 100.0, 21.6) -endfunction - -/* LUM5 about pelvis */ -beginfunction f151 -(-100.0, -10.3) -( 100.0, 10.3) -endfunction - -beginfunction f152 -(-100.0, -11.1) -( 100.0, 11.1) -endfunction - -beginfunction f153 -(-100.0, -23.0) -( 100.0, 23.0) -endfunction - -/***************** Neck *******************/ -/* skull rel to C1 */ -beginfunction f301 -(-100.000000, -8.200000) -( 100.000000, 8.200000) -endfunction - -beginfunction f302 -(-100.000000, -6.500000) -( 100.000000, 6.500000) -endfunction - -beginfunction f303 -(-100.000000, -18.400000) -( 100.000000, 18.400000) -endfunction - -/* C1 rel to C2 */ -beginfunction f311 -(-100.000000, -8.200000) -( 100.000000, 8.200000) -endfunction - -beginfunction f312 -(-100.000000, -51.900000) -( 100.000000, 51.900000) -endfunction - -beginfunction f313 -(-100.000000, -14.700000) -( 100.000000, 14.700000) -endfunction - -/* C2 rel to C3 */ -beginfunction f321 -(-100.000000, -16.400000) -( 100.000000, 16.400000) -endfunction - -beginfunction f322 -(-100.000000, -3.900000) -( 100.000000, 3.900000) -endfunction - -beginfunction f323 -(-100.000000, -7.400000) -( 100.000000, 7.400000) -endfunction - -/* C3 rel to C4 */ -beginfunction f331 -(-100.000000, -18.000000) -( 100.000000, 18.000000) -endfunction - -beginfunction f332 -(-100.000000, -9.100000) -( 100.000000, 9.100000) -endfunction - -beginfunction f333 -(-100.000000, -11.000000) -( 100.000000, 11.000000) -endfunction - -/* C4 rel to C5 */ -beginfunction f341 -(-100.000000, -18.000000) -( 100.000000, 18.000000) -endfunction - -beginfunction f342 -(-100.000000, -9.100000) -( 100.000000, 9.100000) -endfunction - -beginfunction f343 -(-100.000000, -14.700000) -( 100.000000, 14.700000) -endfunction - -/* C5 rel to C6 */ -beginfunction f351 -(-100.000000, -13.100000) -( 100.000000, 13.100000) -endfunction - -beginfunction f352 -(-100.000000, -9.100000) -( 100.000000, 9.100000) -endfunction - -beginfunction f353 -(-100.000000, -14.700000) -( 100.000000, 14.700000) -endfunction - -/* C6 rel to C7 */ -beginfunction f361 -(-100.000000, -11.500000) -( 100.000000, 11.500000) -endfunction - -beginfunction f362 -(-100.000000, -7.800000) -( 100.000000, 7.800000) -endfunction - -beginfunction f363 -(-100.000000, -12.500000) -( 100.000000, 12.500000) -endfunction - -/* C7 rel to T1 */ -beginfunction f371 -(-100.000000, -6.600000) -( 100.000000, 6.600000) -endfunction - -beginfunction f372 -(-100.000000, -2.600000) -( 100.000000, 2.600000) -endfunction - -beginfunction f373 -(-100.000000, -6.600000) -( 100.000000, 6.600000) -endfunction - -/********************* Right Shoulder *************************/ -/* clavicle */ -beginfunction f1000 -(-180.0, -190.0) -(0.0, -10.0) -(180.0, 170.0) -endfunction - -beginfunction f1001 -(-180.0, -200.0) -(0.0, -20.0) -(180.0, 160.0) -endfunction - -beginfunction f1002 -(-180.0, 190.0) -(0.0, 10.0) -(180.0, -170.0) -endfunction - -beginfunction f1003 -(-180.0, 200.0) -(0.0, 20.0) -(180.0, -160.0) -endfunction - -/******************* Right Wrist and Hand ********************/ -/* radius-prox_flex RZ */ -beginfunction f1100 -( -90.000000, -45.000000) -( 90.000000, 45.000000) -endfunction - -/* prox_flex-prox_dev rX */ -beginfunction f1101 -( -25.000000, -9.375000) -( 0.000000, 0.000000) -( 35.000000, 17.500000) -endfunction - -/* prox_dev-dist_flex rZ */ -beginfunction f1102 -( -70.000000, -35.000000) -( 70.000000, 35.000000) -endfunction - -/* dist_flex-dist_dev rX */ -beginfunction f1103 -( -25.000000, -15.625000) -( 0.000000, 0.000000) -( 35.000000, 17.500000) -endfunction - -/* ty capitate-cap_aux1 */ -beginfunction f1209 -( -25.403461, -0.005296) -( -12.552980, -0.001860) -( 0.000000, 0.000000) -( 7.255741, -0.001401) -( 12.481216, -0.002375) -( 35.212013, -0.004810) -endfunction - -/* tz capitate-cap_aux1 */ -beginfunction f1210 -( -25.354568, 0.000723) -( -10.125652, 0.000708) -( -3.840702, 0.000317) -( 0.000000, 0.000000) -( 5.468287, -0.000859) -( 10.564981, -0.001663) -( 16.147070, -0.002381) -( 21.782553, -0.002774) -( 29.252764, -0.002942) -( 35.077644, -0.002942) -endfunction - -/* tz capitate-cap_aux2 */ -beginfunction f1211 -( -25.355537, 0.003199) -( 0.000000, 0.000000) -( 35.115795, -0.003638) -endfunction - -/* ty trapezium trpzm_aux1 */ -beginfunction f1204 -( -25.355537, -0.000969) -( 0.000000, 0.000000) -( 22.248272, 0.002119) -( 35.055393, 0.003237) -endfunction - -/* tz trapezium-trpzm_aux1 */ -beginfunction f1205 -( -34.559998, 0.000000) -( -26.475258, 0.000006) -( -18.111734, 0.000003) -( -8.075508, 0.000003) -( 0.000000, 0.000000) -( 35.136032, -0.000904) -endfunction - -/* tz scaphoid-scaph_aux1 */ -beginfunction f1201 -( -26.081165, -0.000932) -( -10.566132, -0.000266) -( 0.000000, 0.000000) -( 35.009277, 0.001910) -endfunction - -/* ty scaphoid-scaph_aux1 */ -beginfunction f1200 -( -25.457281, -0.000955) -( 0.000000, 0.000000) -( 35.313988, 0.001147) -endfunction - -/* tz scaphoid-scaph_aux2 */ -beginfunction f1203 -( -25.672873, 0.001403) -( -11.341747, 0.000201) -( -1.962238, -0.000004) -( 0.000000, 0.000000) -( 7.231682, 0.001196) -( 19.022863, 0.002061) -( 35.055382, 0.002131) -endfunction - -/* ty scaphoid-scaph_aux2*/ -beginfunction f1202 -( -25.427441, -0.000340) -( 0.000000, 0.000000) -( 35.265545, 0.000589) -endfunction - -/* ty hamate-ham_aux1 */ -beginfunction f1212 -( -25.387917, 0.004474) -( 0.022762, 0.000100) -( 49.934570, 0.010131) -endfunction - -/* tz trapezoid-trpzd_aux1 */ -beginfunction f1208 -( -25.387917, -0.004666) -( 0.022762, 0.000121) -( 35.457458, 0.001204) -endfunction - -/* tz trapezium-trpzm_aux2 */ -beginfunction f1207 -( -25.355537, 0.002558) -( 0.000000, 0.000000) -( 35.055393, -0.002218) -endfunction - -/* ty trapezium-trpzm_aux2 */ -beginfunction f1206 -( -25.355537, 0.000047) -( -18.831158, 0.000019) -( -12.065134, 0.000019) -( -6.990616, 0.000019) -( 0.000000, 0.000000) -( 35.055393, 0.006207) -endfunction - -/* tz metacarp1-mc1_aux1 */ -beginfunction f1213 -( -29.946768, 0.000249) -( -24.147320, 0.000174) -( -16.426317, 0.000101) -( -9.926102, 0.000039) -( 0.017050, 0.000000) -( 6.974461, 0.000096) -( 13.474678, 0.000191) -( 14.514713, 0.000241) -( 16.594780, 0.000274) -( 24.915058, 0.000244) -( 35.055397, 0.000223) -endfunction - -#endif /* UPPER_EXTREMITY */ - -#if LOWER_EXTREMITY - -/******************** Right Knee *********************/ - -/* TX for right knee */ -beginfunction f1600 -( 0.0, -0.00525) -( 10.0, -0.00310) -( 20.0, -0.00100) -( 40.0, 0.00212) -( 60.0, 0.00410) -( 80.0, 0.00411) -(100.0, 0.00179) -(120.0, -0.00320) -endfunction - -/* TY for right knee */ -beginfunction f1601 -( 0.0, -0.3960) -( 10.0, -0.3966) -( 20.0, -0.3976) -( 30.0, -0.3990) -( 70.0, -0.4082) -(120.0, -0.4226) -endfunction - -/* TX for right tib_pat joint */ -beginfunction f1602 -( 0.0, 0.0496) -( 10.0, 0.0484) -( 20.0, 0.0469) -( 40.0, 0.0430) -( 60.0, 0.0381) -( 80.0, 0.0324) -(120.0, 0.0173) -endfunction - -/* TY for right tib_pat */ -beginfunction f1603 -( 0.286,-0.0227) -( 10.0, -0.0223) -( 20.0, -0.0219) -( 40.0, -0.0211) -( 60.0, -0.0204) -( 80.0, -0.0200) -( 90.0, -0.0202) -(120.0, -0.0219) -endfunction - -/* RZ for right tib_pat joint */ -beginfunction f1604 -(-10.00, -9.24) -( -0.15, -2.44) -( 20.34, 6.09) -( 56.43, 13.86) -(114.59, 17.65) -(120.00, 17.65) -endfunction - -/* prox_toes function */ -beginfunction f1606 -(-90.0, -10.0) -(-45.0, -5.0) -( 0.0, 0.0) -( 30.0, 5.0) -( 90.0, 15.0) -endfunction - -beginfunction f1610 -(-15.0, -15.0 ) -(-10.0, -10.0 ) -( -5.0, -5.0 ) -( 0.0, 0.0 ) -( 2.5, 0.0 ) -( 5.0, 0.0 ) -( 10.0, 0.0 ) -( 15.0, 0.0 ) -endfunction - -beginfunction f1611 -(-15.0, 0.0 ) -(-10.0, 0.0 ) -( -5.0, 0.0 ) -( -2.5, 0.0 ) -( 0.0, 0.0 ) -( 5.0, 5.0 ) -( 10.0, 10.0 ) -( 15.0, 15.0 ) -endfunction - -beginfunction f1612 -(-15.0, 15.0 ) -(-10.0, 10.0 ) -( -5.0, 5.0 ) -( 0.0, 0.0 ) -( 5.0, -5.0 ) -( 10.0, -10.0 ) -( 15.0, -15.0 ) -endfunction - -#endif /* LOWER_EXTREMITY */ - -#if UPPER_EXTREMITY - -/********************* Left Shoulder *************************/ -/* clavicle */ -beginfunction f1300 -(-180.0, -190.0) -(0.0, -10.0) -(180.0, 170.0) -endfunction - -beginfunction f1301 -(-180.0, -200.0) -(0.0, -20.0) -(180.0, 160.0) -endfunction - -beginfunction f1302 -(-180.0, 190.0) -(0.0, 10.0) -(180.0, -170.0) -endfunction - -beginfunction f1303 -(-180.0, 200.0) -(0.0, 20.0) -(180.0, -160.0) -endfunction - -/******************* Left Wrist and Hand ********************/ -/* radius-prox_flex RZ */ -beginfunction f1400 -( -90.000000, -90.000000) -( 90.000000, 90.000000) -endfunction - -/* prox_flex-prox_dev rX */ -beginfunction f1401 -( -25.000000, -9.375000) -( 0.000000, 0.000000) -( 35.000000, 17.500000) -endfunction - -/* prox_dev-dist_flex rZ */ -beginfunction f1402 -( -70.000000, -35.000000) -( 70.000000, 35.000000) -endfunction - -/* dist_flex-dist_dev rX */ -beginfunction f1403 -( -90.000000, -90.000000) -( 90.000000, 90.000000) -endfunction - -/* ty capitate-cap_aux1 */ -beginfunction f1509 -( -25.403461, -0.005296) -( -12.552980, -0.001860) -( 0.000000, 0.000000) -( 7.255741, -0.001401) -( 12.481216, -0.002375) -( 35.212013, -0.004810) -endfunction - -/* tz capitate-cap_aux1 */ -beginfunction f1510 -( -25.354568, 0.000723) -( -10.125652, 0.000708) -( -3.840702, 0.000317) -( 0.000000, 0.000000) -( 5.468287, -0.000859) -( 10.564981, -0.001663) -( 16.147070, -0.002381) -( 21.782553, -0.002774) -( 29.252764, -0.002942) -( 35.077644, -0.002942) -endfunction - -/* tz capitate-cap_aux2 */ -beginfunction f1511 -( -25.355537, 0.003199) -( 0.000000, 0.000000) -( 35.115795, -0.003638) -endfunction - -/* ty trapezium trpzm_aux1 */ -beginfunction f1504 -( -25.355537, -0.000969) -( 0.000000, 0.000000) -( 22.248272, 0.002119) -( 35.055393, 0.003237) -endfunction - -/* tz trapezium-trpzm_aux1 */ -beginfunction f1505 -( -34.559998, 0.000000) -( -26.475258, 0.000006) -( -18.111734, 0.000003) -( -8.075508, 0.000003) -( 0.000000, 0.000000) -( 35.136032, -0.000904) -endfunction - -/* tz scaphoid-scaph_aux1 */ -beginfunction f1501 -( -26.081165, -0.000932) -( -10.566132, -0.000266) -( 0.000000, 0.000000) -( 35.009277, 0.001910) -endfunction - -/* ty scaphoid-scaph_aux1 */ -beginfunction f1500 -( -25.457281, -0.000955) -( 0.000000, 0.000000) -( 35.313988, 0.001147) -endfunction - -/* tz scaphoid-scaph_aux2 */ -beginfunction f1503 -( -25.672873, 0.001403) -( -11.341747, 0.000201) -( -1.962238, -0.000004) -( 0.000000, 0.000000) -( 7.231682, 0.001196) -( 19.022863, 0.002061) -( 35.055382, 0.002131) -endfunction - -/* ty scaphoid-scaph_aux2*/ -beginfunction f1502 -( -25.427441, -0.000340) -( 0.000000, 0.000000) -( 35.265545, 0.000589) -endfunction - -/* ty hamate-ham_aux1 */ -beginfunction f1512 -( -25.387917, 0.004474) -( 0.022762, 0.000100) -( 49.934570, 0.010131) -endfunction - -/* tz trapezoid-trpzd_aux1 */ -beginfunction f1508 -( -25.387917, -0.004666) -( 0.022762, 0.000121) -( 35.457458, 0.001204) -endfunction - -/* tz trapezium-trpzm_aux2 */ -beginfunction f1507 -( -25.355537, 0.002558) -( 0.000000, 0.000000) -( 35.055393, -0.002218) -endfunction - -/* ty trapezium-trpzm_aux2 */ -beginfunction f1506 -( -25.355537, 0.000047) -( -18.831158, 0.000019) -( -12.065134, 0.000019) -( -6.990616, 0.000019) -( 0.000000, 0.000000) -( 35.055393, 0.006207) -endfunction - -/* tz metacarp1-mc1_aux1 */ -beginfunction f1513 -( -29.946768, 0.000249) -( -24.147320, 0.000174) -( -16.426317, 0.000101) -( -9.926102, 0.000039) -( 0.017050, 0.000000) -( 6.974461, 0.000096) -( 13.474678, 0.000191) -( 14.514713, 0.000241) -( 16.594780, 0.000274) -( 24.915058, 0.000244) -( 35.055397, 0.000223) -endfunction - -#endif /* UPPER_EXTREMITY */ - -#if LOWER_EXTREMITY - -/******************** Left Knee *********************/ -/* TX for left knee */ -beginfunction f1700 -( 0.0, -0.00525) -( 10.0, -0.00310) -( 20.0, -0.00100) -( 40.0, 0.00212) -( 60.0, 0.00410) -( 80.0, 0.00411) -(100.0, 0.00179) -(120.0, -0.00320) -endfunction - -/* TY for left knee */ -beginfunction f1701 -( 0.0, -0.3960) -( 10.0, -0.3966) -( 20.0, -0.3976) -( 30.0, -0.3990) -( 70.0, -0.4082) -(120.0, -0.4226) -endfunction - -/* TX for left tib_pat joint */ -beginfunction f1702 -( 0.0, 0.0496) -( 10.0, 0.0484) -( 20.0, 0.0469) -( 40.0, 0.0430) -( 60.0, 0.0381) -( 80.0, 0.0324) -(120.0, 0.0173) -endfunction - -/* TY for left tib_pat joint */ -beginfunction f1703 -( 0.286,-0.0227) -( 10.0, -0.0223) -( 20.0, -0.0219) -( 40.0, -0.0211) -( 60.0, -0.0204) -( 80.0, -0.0200) -( 90.0, -0.0202) -(120.0, -0.0219) -endfunction - -/* RZ for left tib_pat joint */ -beginfunction f1704 -(-10.00, -9.24) -( -0.15, -2.44) -( 20.34, 6.09) -( 56.43, 13.86) -(114.59, 17.65) -(120.00, 17.65) -endfunction - -/* prox_toes function */ -beginfunction f1706 -(-90.0, -10.0) -(-45.0, -5.0) -( 0.0, 0.0) -( 30.0, 5.0) -( 90.0, 15.0) -endfunction - -beginfunction f1710 -(-15.0, -15.0 ) -(-10.0, -10.0 ) -( -5.0, -5.0 ) -( 0.0, 0.0 ) -( 2.5, 0.0 ) -( 5.0, 0.0 ) -( 10.0, 0.0 ) -( 15.0, 0.0 ) -endfunction - -beginfunction f1711 -(-15.0, 0.0 ) -(-10.0, 0.0 ) -( -5.0, 0.0 ) -( -2.5, 0.0 ) -( 0.0, 0.0 ) -( 5.0, 5.0 ) -( 10.0, 10.0 ) -( 15.0, 15.0 ) -endfunction - -beginfunction f1712 -(-15.0, 15.0 ) -(-10.0, 10.0 ) -( -5.0, 5.0 ) -( 0.0, 0.0 ) -( 5.0, -5.0 ) -( 10.0, -10.0 ) -( 15.0, -15.0 ) -endfunction - -#endif /* LOWER_EXTREMITY */ - -/************************************************/ -/* CONSTRAINT OBJECTS */ -/************************************************/ - -#if COMPLEX_CLAVICLE - -beginconstraintobject right_shoulder -constrainttype ellipsoid -segment thorax -xyz_body_rotation -3.28 -1.47 13.70 -translation -0.0172 0.1829 0.0698 -radius 0.1000 0.1800 0.0900 -beginpoints -r_sh_1 -0.0815 -0.0579 -0.0910 10.0000 scapula_r -r_sh_2 -0.0888 -0.1839 -0.0485 10.0000 scapula_r -r_sh_3 -0.0481 -0.0909 -0.0401 10.0000 scapula_r -endpoints -visible no -active yes -endconstraintobject - -beginconstraintobject left_shoulder -constrainttype ellipsoid -segment thorax -xyz_body_rotation 3.28 1.47 13.70 -translation -0.0172 0.1829 -0.0698 -radius 0.1000 0.1800 0.0900 -beginpoints -l_sh_1 -0.0815 -0.0579 0.0910 10.0000 scapula_l -l_sh_2 -0.0888 -0.1839 0.0485 10.0000 scapula_l -l_sh_3 -0.0481 -0.0909 0.0401 10.0000 scapula_l -endpoints -visible no -active yes -endconstraintobject - -#endif - -/*********************************************/ -/* MODEL VIEWS */ -/*********************************************/ - -#if GROUND_PLANE_XZ - beginview default - 0.0 0.0 1.0 0.0 - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview motion - -0.7046 -0.0060 0.7096 0.0000 - 0.0366 0.9983 0.0448 0.0000 - -0.7087 0.0575 -0.7032 0.0000 - 0.3181 -0.8978 -4.5020 1.0000 - endview - - beginview front - 0.0 0.0 1.0 0.0 - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview side - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview top - 1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -1.0 0.0 0.0 - 0.0 0.0 -5.0 1.0 - endview - -#elif GROUND_PLANE_XY - beginview default - 0.0 0.0 1.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview motion - -0.3906 -0.0806 0.9170 0.0000 - 0.9205 -0.0301 0.3895 0.0000 - -0.0038 0.9963 0.0860 0.0000 - 0.5573 -1.0410 -3.6453 1.0000 - endview - - beginview front - 0.0 0.0 1.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview side - 1.0 0.0 0.0 0.0 - 0.0 0.0 -1.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 -1.0 -5.0 1.0 - endview - - beginview top - 1.0 0.0 0.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 0.0 -5.0 1.0 - endview - -#elif GROUND_PLANE_YZ - beginview default - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -0.5 -5.0 1.0 - endview - - beginview motion - -0.0016 0.9994 -0.0347 0.0000 - -0.7450 -0.0243 -0.6666 0.0000 - -0.6670 0.0248 0.7446 0.0000 - 0.5645 -0.8547 -6.0538 1.0000 - endview - - beginview front - 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -0.5 -5.0 1.0 - endview - - beginview side - 0.0 1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 -0.5 -5.0 1.0 - endview - - beginview top - 0.0 0.0 1.0 0.0 - 0.0 -1.0 0.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 0.0 -5.0 1.0 - endview -#endif - -/*********************************************/ -/* MATERIALS */ -/*********************************************/ - -beginmaterial cyan -ambient 0.1000 0.8000 1.0000 -diffuse 0.1000 0.3000 0.5000 -specular 0.1000 0.3000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial green -ambient 0.0100 0.9000 0.0100 -diffuse 0.1000 0.9000 0.1000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial orange -ambient 1.0000 0.3000 0.0100 -diffuse 0.5000 0.1000 0.1000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial yellow -ambient 1.0000 0.8000 0.1000 -diffuse 0.5000 0.4000 0.1000 -specular 0.5000 0.4000 0.1000 -shininess 10.0000 -endmaterial - -beginmaterial purple -ambient 0.5000 0.0100 0.5000 -diffuse 0.5000 0.0100 0.5000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial blue -ambient 0.1000 0.1000 1.0000 -diffuse 0.1000 0.1000 1.0000 -specular 0.5000 0.5000 0.5000 -shininess 10.0000 -endmaterial - -beginmaterial pink -ambient 0.9 0.3 0.2 -specular 0.14 0.0 0.2 -diffuse 0.2 0.3 0.2 -shininess 25.0 -endmaterial - -beginmaterial tan -ambient 0.7 0.4 0.1 -specular 0.14 0.0 0.2 -diffuse 0.2 0.3 0.2 -shininess 10.0 -endmaterial - -beginmaterial red -ambient 1.0000 0.1000 0.1000 -diffuse 0.5000 0.1000 0.1000 -specular 0.5000 0.1000 0.1000 -shininess 10.0000 -endmaterial - -beginmaterial my_bone -ambient 0.6000 0.6000 0.6000 -diffuse 0.6000 0.4500 0.4000 -specular 0.7000 0.5500 0.4000 -shininess 10.0000 -endmaterial - -beginmaterial arrow_mat -ambient 0.4000 0.4000 0.7000 -diffuse 0.3000 0.3000 0.5000 -specular 0.3000 0.3000 0.5000 -shininess 0.0000 -endmaterial - -beginmaterial ligament_mat -ambient 0.8000 0.3000 0.3000 -diffuse 0.7000 0.2000 0.2000 -specular 1.0000 0.5000 0.5000 -shininess 20.0000 -endmaterial - -beginmaterial bone_mat -ambient 0.6000 0.6000 0.6000 -diffuse 0.6000 0.4500 0.4000 -specular 0.7000 0.5500 0.4000 -shininess 10.0000 -endmaterial - -beginmaterial goldmetal -ambient 0.4000 0.2000 0.0000 -diffuse 0.2200 0.1800 0.0500 -specular 0.8000 0.8000 0.4000 -shininess 93.0000 -endmaterial - -beginmaterial graymetal -ambient 0.6000 0.6000 0.6000 -diffuse 0.1500 0.1500 0.1500 -specular 1.0000 1.0000 1.0000 -shininess 95.0000 -endmaterial - -beginmaterial newfloor -ambient 0.2400 0.5400 0.7300 -diffuse 0.0500 0.1000 0.1500 -specular 1.0000 1.0000 1.0000 -shininess 25.0000 -endmaterial - -beginmaterial rad_bone -ambient 0.6000 0.6000 0.8000 -diffuse 0.6000 0.4500 0.7000 -specular 0.9000 0.4000 0.4000 -shininess 20.0000 -endmaterial - -beginmaterial dist_bone -ambient 0.6000 0.6000 0.8000 -diffuse 0.6000 0.4500 0.7000 -specular 0.9000 0.4000 0.4000 -shininess 20.0000 -endmaterial - -beginmaterial muscle_min -ambient 0.2000 0.0000 0.0000 -diffuse 0.4000 0.0000 0.0000 -specular 0.6000 0.0000 0.0000 -shininess 20.0000 -endmaterial - -beginmaterial muscle_max -ambient 0.5000 0.1000 0.1000 -diffuse 0.7000 0.1000 0.1000 -specular 0.9000 0.2000 0.2000 -shininess 80.0000 -endmaterial - -/****************************************************/ -/* MOTION OBJECTS */ -/****************************************************/ -beginmotionobject ball -material blue -scale 0.25 0.25 0.25 -endmotionobject - -#if GROUND_PLANE_XZ - #define FLOOR floor_xz_plane.asc - #define FLOOR_ORIGIN 0.0 -0.002 0.0 -#elif GROUND_PLANE_XY - #define FLOOR floor_xy_plane.asc - #define FLOOR_ORIGIN 0.0 0.0 -0.002 -#elif GROUND_PLANE_YZ - #define FLOOR floor_yz_plane.asc - #define FLOOR_ORIGIN -0.002 0.0 0.0 -#endif - -beginmaterial floor_material -ambient 0.5 0.4 0.3 -diffuse 0.5 0.4 0.3 -specular 0.5 0.4 0.3 -endmaterial - -beginworldobject floor -filename FLOOR -material newfloor -origin FLOOR_ORIGIN -drawmode solid_fill -endworldobject - -#if RIGHT_HAND_FULL - -/******************************************************************************* - RIGHT HAND MODEL - joint file - - (c) Copyright 2002, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the segments joints, gencoords, and constraint functions - for the right hand model of an average adult male. It was developed by Silvia - Salinas Blemker and Scott Delp. - - This model is described in the paper: - Delp, S. L., Grierson, A. E., Buchanan, T. S. Moments generated by the wrist - muscles in flexion-extension and radial-ulnar deviation, Journal of - Biomechanics, vol. 29, pp. 1371-1376, 1996. - -*******************************************************************************/ -#ifndef RF1M1 - #define RF1M1 1 -#endif -#ifndef RF1M2 - #define RF1M2 1 -#endif -#ifndef RF1M3 - #define RF1M3 1 -#endif - -#ifndef RF2M1 - #define RF2M1 1 -#endif -#ifndef RF2M2 - #define RF2M2 1 -#endif -#ifndef RF2M3 - #define RF2M3 1 -#endif - -#ifndef RF3M1 - #define RF3M1 1 -#endif -#ifndef RF3M2 - #define RF3M2 1 -#endif -#ifndef RF3M3 - #define RF3M3 1 -#endif - -#ifndef RF4M1 - #define RF4M1 1 -#endif -#ifndef RF4M2 - #define RF4M2 1 -#endif -#ifndef RF4M3 - #define RF4M3 1 -#endif - -#ifndef RF5M1 - #define RF5M1 1 -#endif -#ifndef RF5M2 - #define RF5M2 1 -#endif -#ifndef RF5M3 - #define RF5M3 1 -#endif - -/*************************** Right Wrist and Hand ************/ -beginsegment prox_carpals_r -beginfiles -pisiform_rv.asc -lunate_rv.asc -scaphoid_rv.asc -triquetrum_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - wrist_r - hand_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment dist_carpals_r -beginfiles -hamate_rv.asc -capitate_rv.asc -trapezoid_rv.asc -trapezium_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - wrist_r - hand_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment rotaxis_r -begingroups - aux_axes_r - wrist_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment prox_flex_CR_r -begingroups - aux_axes_r - wrist_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment prox_dev_CR_r -begingroups - aux_axes_r - wrist_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment dist_flex_CR_r -begingroups - aux_axes_r - wrist_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment dist_dev_CR_r -begingroups - aux_axes_r - wrist_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment scaph_aux1_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment scaph_aux2_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment cap_aux1_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment cap_aux2_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment ham_aux1_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment trpzd_aux1_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment trpzm_aux1_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment trpzm_aux2_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment mc1_aux1_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment carp_metacarp_r -begingroups - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment carp_thumb_r -begingroups - right - aux_axes_r - musc_aux_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment metacarpal2_r -beginfiles -metacarpal2_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment index_1_r -beginfiles -index_proximal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger2.M1 -0.012 -0.030 0.0 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment index_2_r -beginfiles -index_medial_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger2.M2 -0.008 -0.020 0.0 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment index_3_r -beginfiles -index_distal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger2.M3 -0.007 -0.010 0.0 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal3_r -beginfiles -metacarpal3_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Hand -0.020 -0.030 0.000 OTHER_WEIGHT -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment middle_1_r -beginfiles -middle_proximal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger3.M1 -0.012 -0.032 -0.003 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment middle_2_r -beginfiles -middle_medial_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger3.M2 -0.008 -0.020 -0.003 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment middle_3_r -beginfiles -middle_distal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger3.M3 -0.007 -0.016 -0.003 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal4_r -beginfiles -metacarpal4_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment ring_1_r -beginfiles -ring_proximal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger4.M1 -0.012 -0.027 -0.004 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment ring_2_r -beginfiles -ring_medial_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger4.M2 -0.008 -0.016 -0.002 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment ring_3_r -beginfiles -ring_distal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger4.M3 -0.008 -0.010 -0.002 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal5_r -beginfiles -metacarpal5_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment little_1_r -beginfiles -little_proximal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger5.M1 -0.010 -0.025 -0.005 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment little_2_r -beginfiles -little_medial_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger5.M2 -0.008 -0.015 -0.003 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment little_3_r -beginfiles -little_distal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Finger5.M3 -0.008 -0.010 -0.002 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal1_r -beginfiles -metacarpal1_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Thumb.M1 -0.012 -0.020 0.006 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment thumb_2_r -beginfiles -thumb_proximal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Thumb.M2 -0.008 -0.020 0.000 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment thumb_3_r -beginfiles -thumb_distal_rv.asc -endfiles -material my_bone -begingroups - right - arm_r - hand_r -endgroups -marker R.Thumb.M3 -0.007 -0.015 0.000 OTHER_WEIGHT fixed -gait_scale R_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - - -/*********************************************/ -/* JOINTS */ -/*********************************************/ -beginjoint rad-rotaxis_r -segments radius_r rotaxis_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.008797 -ty constant -0.245841 -tz constant 0.015610 -r1 constant -5.000000 -r2 constant 22.500000 -r3 constant 0.000000 -endjoint - -beginjoint rotaxis-prox_flex_CR_r -segments rotaxis_r prox_flex_CR_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.000832 -ty constant 0.008099 -tz constant -0.000364 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f816(wrist_flex_r) -endjoint - -beginjoint prox_flex_dev_r -segments prox_flex_CR_r prox_dev_CR_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.004500 -ty constant 0.000000 -tz constant 0.000000 -r1 function f824(wrist_dev_r) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint prox_dev-dist_flex_r -segments prox_dev_CR_r dist_flex_CR_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000900 -ty constant -0.018000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f836(wrist_flex_r) -endjoint - -beginjoint dist_flex-dev_r -segments dist_flex_CR_r dist_dev_CR_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 function f844(wrist_dev_r) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint prox_carpal_jnt_r -segments prox_dev_CR_r prox_carpals_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint dist_carpal_jnt_r -segments dist_dev_CR_r dist_carpals_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint scap-aux1_r -segments prox_carpals_r scaph_aux1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f8312(wrist_dev_r) -tz function f8313(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint scap-aux2_r -segments prox_carpals_r scaph_aux2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f8322(wrist_dev_r) -tz function f8323(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint trpzm-aux1_r -segments dist_carpals_r trpzm_aux1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f8712(wrist_dev_r) -tz function f8713(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint trpzm-aux2_r -segments dist_carpals_r trpzm_aux2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f8722(wrist_dev_r) -tz function f8723(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint trpzd-aux1_r -segments dist_carpals_r trpzd_aux1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz function f8813(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint cap-aux1_r -segments dist_carpals_r cap_aux1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f8512(wrist_dev_r) -tz function f8513(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint cap-aux2_r -segments dist_carpals_r cap_aux2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz function f8523(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint ham-aux1_r -segments dist_carpals_r ham_aux1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f8612(wrist_dev_r) -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint auxCMC_r -segments dist_dev_CR_r carp_metacarp_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint auxThumb_r -segments dist_dev_CR_r carp_thumb_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.006000 -ty constant -0.010000 -tz constant 0.032000 -r1 constant -30.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint CMC2_r -segments carp_metacarp_r metacarpal2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.004230 -ty constant -0.008550 -tz constant -0.004500 -r1 constant 10.000000 -r2 constant 15.000000 -r3 constant 0.000000 -endjoint - -beginjoint MCP2_r -segments metacarpal2_r index_1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.002162 -ty constant -0.061650 -tz constant 0.040500 -#if RF2M1 || (RF2M2 || RF2M3) -r1 function f1(MCP2_lateral_r) -r2 constant 0.000000 -r3 function f1(MCP2_flex_r) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP2_r -segments index_1_r index_2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000450 -ty constant -0.043434 -tz constant 0.007191 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF2M2 || RF2M3 - #if RF2M1 - r3 function f1(PIP2_flex_r) - #else - r3 function f1(MCP2_flex_r) - #endif -#else - #if RF2M1 - r3 function f1(MCP2_flex_r) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP2_r -segments index_2_r index_3_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.001737 -ty constant -0.029493 -tz constant 0.004248 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF2M3 - #if RF2M2 - r3 function f1(DIP2_flex_r) - #elif RF2M1 - r3 function f1(PIP2_flex_r) - #else - r3 function f1(MCP2_flex_r) - #endif -#else - #if RF2M2 - #if RF2M1 - r3 function f1(PIP2_flex_r) - #else - r3 function f1(MCP2_flex_r) - #endif - #else - #if RF2M1 - r3 function f1(MCP2_flex_r) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC3_r -segments carp_metacarp_r metacarpal3_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.004230 -ty constant -0.007650 -tz constant -0.004500 -r1 constant 5.000000 -r2 constant 10.000000 -r3 constant 5.000000 -endjoint - -beginjoint MCP3_r -segments metacarpal3_r middle_1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.002430 -ty constant -0.068670 -tz constant 0.014580 -#if RF3M1 || (RF3M2 || RF3M3) -r1 function f1(MCP3_lateral_r) -r2 constant 0.000000 -r3 function f1(MCP3_flex_r) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP3_r -segments middle_1_r middle_2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.003150 -ty constant -0.048924 -tz constant 0.002781 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF3M2 || RF3M3 - #if RF3M1 - r3 function f1(PIP3_flex_r) - #else - r3 function f1(MCP3_flex_r) - #endif -#else - #if RF3M1 - r3 function f1(MCP3_flex_r) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP3_r -segments middle_2_r middle_3_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000837 -ty constant -0.031653 -tz constant 0.001368 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF3M3 - #if RF3M2 - r3 function f1(DIP3_flex_r) - #elif RF3M1 - r3 function f1(PIP3_flex_r) - #else - r3 function f1(MCP3_flex_r) - #endif -#else - #if RF3M2 - #if RF3M1 - r3 function f1(PIP3_flex_r) - #else - r3 function f1(MCP3_flex_r) - #endif - #else - #if RF3M1 - r3 function f1(MCP3_flex_r) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC4_r -segments carp_metacarp_r metacarpal4_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.004230 -ty constant -0.007200 -tz constant -0.002970 -r1 constant 5.000000 -r2 constant 10.000000 -r3 constant 5.000000 -endjoint - -beginjoint MCP4_r -segments metacarpal4_r ring_1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000810 -ty constant -0.069300 -tz constant -0.006660 -#if RF4M1 || (RF4M2 || RF4M3) -r1 function f1(MCP4_lateral_r) -r2 constant 0.000000 -r3 function f1(MCP4_flex_r) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP4_r -segments ring_1_r ring_2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.002160 -ty constant -0.049644 -tz constant 0.000261 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF4M2 || RF4M3 - #if RF4M1 - r3 function f1(PIP4_flex_r) - #else - r3 function f1(MCP4_flex_r) - #endif -#else - #if RF4M1 - r3 function f1(MCP4_flex_r) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP4_r -segments ring_2_r ring_3_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.004347 -ty constant -0.026883 -tz constant 0.004338 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF4M3 - #if RF4M2 - r3 function f1(DIP4_flex_r) - #elif RF4M1 - r3 function f1(PIP4_flex_r) - #else - r3 function f1(MCP4_flex_r) - #endif -#else - #if RF4M2 - #if RF4M1 - r3 function f1(PIP4_flex_r) - #else - r3 function f1(MCP4_flex_r) - #endif - #else - #if RF4M1 - r3 function f1(MCP4_flex_r) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC5_r -segments carp_metacarp_r metacarpal5_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.001800 -ty constant -0.009000 -tz constant 0.003150 -r1 constant 5.000000 -r2 constant 10.000000 -r3 constant 5.000000 -endjoint - -beginjoint MCP5_r -segments metacarpal5_r little_1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.000360 -ty constant -0.056880 -tz constant -0.032940 -#if RF5M1 || (RF5M2 || RF5M3) -r1 function f1(MCP5_lateral_r) -r2 constant 0.000000 -r3 function f1(MCP5_flex_r) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP5_r -segments little_1_r little_2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.002340 -ty constant -0.039114 -tz constant -0.002439 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF5M2 || RF5M3 - #if RF5M1 - r3 function f1(PIP5_flex_r) - #else - r3 function f1(MCP5_flex_r) - #endif -#else - #if RF5M1 - r3 function f1(MCP5_flex_r) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP5_r -segments little_2_r little_3_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.003177 -ty constant -0.023013 -tz constant 0.002052 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF5M3 - #if RF5M2 - r3 function f1(DIP5_flex_r) - #elif RF5M1 - r3 function f1(PIP5_flex_r) - #else - r3 function f1(MCP5_flex_r) - #endif -#else - #if RF5M2 - #if RF5M1 - r3 function f1(PIP5_flex_r) - #else - r3 function f1(MCP5_flex_r) - #endif - #else - #if RF5M1 - r3 function f1(MCP5_flex_r) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC1_r -segments carp_thumb_r metacarpal1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.866000 0.500000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -#if RF1M1 || (RF1M2 || RF1M3) -r1 function f1(thumb_abd_r) -r2 constant 0.000000 -r3 function f1(thumb_flex_r) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PP1_r -segments metacarpal1_r thumb_2_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant -0.038000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 30.000000 -#if RF1M2 || RF1M3 - #if RF1M1 - r3 function f1(PP1_flex_r) - #else - r3 function f1(thumb_abd_r) - #endif -#else - #if RF1M1 - r3 function f1(thumb_abd_r) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DP1_r -segments thumb_2_r thumb_3_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.003000 -ty constant -0.032000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -#if RF1M3 - #if RF1M2 - r3 function f1(DP1_flex_r) - #elif RF1M1 - r3 function f1(PP1_flex_r) - #else - r3 function f1(thumb_abd_r) - #endif -#else - #if RF1M2 - #if RF1M1 - r3 function f1(PP1_flex_r) - #else - r3 function f1(thumb_abd_r) - #endif - #else - #if RF1M1 - r3 function f1(thumb_abd_r) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint thumb-aux1_r -segments metacarpal1_r mc1_aux1_r -order t r1 r2 r3 -axis1 1.000000 0.000000 0.000000 -axis2 0.000000 1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz function f8912(wrist_dev_r) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -/*********************************************/ -/* GENCOORDS */ -/*********************************************/ - -#if RF2M1 || (RF2M2 || RF2M3) -begingencoord MCP2_lateral_r -range -20.000000 20.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord - -begingencoord MCP2_flex_r -range -10.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF2M1 && (RF2M2 || RF2M3) -begingencoord PIP2_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF2M2 && RF2M3 -begingencoord DIP2_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF3M1 || (RF3M2 || RF3M3) -begingencoord MCP3_lateral_r -range -20.000000 20.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord - -begingencoord MCP3_flex_r -range -10.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF3M1 && (RF3M2 || RF3M3) -begingencoord PIP3_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF3M2 && RF3M3 -begingencoord DIP3_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF4M1 || (RF4M2 || RF4M3) -begingencoord MCP4_lateral_r -range -20.000000 20.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord - -begingencoord MCP4_flex_r -range -10.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF4M1 && (RF4M2 || RF4M3) -begingencoord PIP4_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF4M2 && RF4M3 -begingencoord DIP4_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF5M1 || (RF5M2 || RF5M3) -begingencoord MCP5_lateral_r -range -20.000000 20.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord - -begingencoord MCP5_flex_r -range -10.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF5M1 && (RF5M2 || RF5M3) -begingencoord PIP5_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF5M2 && RF5M3 -begingencoord DIP5_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF1M1 || (RF1M2 || RF1M3) -begingencoord thumb_abd_r -range -35.000000 25.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord - -begingencoord thumb_flex_r -range -15.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF1M1 && (RF1M2 || RF1M3) -begingencoord PP1_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -#if RF1M2 && RF1M3 -begingencoord DP1_flex_r -range -5.000000 90.000000 -begingroups - right - arm_r - hand_r -endgroups -clamped yes -endgencoord -#endif - -/*********************************************/ -/* FUNCTIONS */ -/*********************************************/ -/* radius-prox_flex RZ */ -beginfunction f816 -( -90.000000, -45.000000) -( 90.000000, 45.000000) -endfunction - -/* prox_flex-prox_dev rX */ -beginfunction f824 -( -25.000000, -9.375000) -( 0.000000, 0.000000) -( 35.000000, 17.500000) -endfunction - -/* prox_dev-dist_flex rZ */ -beginfunction f836 -( -70.000000, -35.000000) -( 70.000000, 35.000000) -endfunction - -/* dist_flex-dist_dev rX */ -beginfunction f844 -( -25.000000, -15.625000) -( 0.000000, 0.000000) -( 35.000000, 17.500000) -endfunction - -/* ty capitate-cap_aux1 */ -beginfunction f8512 -( -25.403461, -0.005296) -( -12.552980, -0.001860) -( 0.000000, 0.000000) -( 7.255741, -0.001401) -( 12.481216, -0.002375) -( 35.212013, -0.004810) -endfunction - -/* tz capitate-cap_aux1 */ -beginfunction f8513 -( -25.354568, 0.000723) -( -10.125652, 0.000708) -( -3.840702, 0.000317) -( 0.000000, 0.000000) -( 5.468287, -0.000859) -( 10.564981, -0.001663) -( 16.147070, -0.002381) -( 21.782553, -0.002774) -( 29.252764, -0.002942) -( 35.077644, -0.002942) -endfunction - -/* tz capitate-cap_aux2 */ -beginfunction f8523 -( -25.355537, 0.003199) -( 0.000000, 0.000000) -( 35.115795, -0.003638) -endfunction - -/* ty trapezium trpzm_aux1 */ -beginfunction f8712 -( -25.355537, -0.000969) -( 0.000000, 0.000000) -( 22.248272, 0.002119) -( 35.055393, 0.003237) -endfunction - -/* tz trapezium-trpzm_aux1 */ -beginfunction f8713 -( -34.559998, 0.000000) -( -26.475258, 0.000006) -( -18.111734, 0.000003) -( -8.075508, 0.000003) -( 0.000000, 0.000000) -( 35.136032, -0.000904) -endfunction - -/* tz scaphoid-scaph_aux1 */ -beginfunction f8313 -( -26.081165, -0.000932) -( -10.566132, -0.000266) -( 0.000000, 0.000000) -( 35.009277, 0.001910) -endfunction - -/* ty scaphoid-scaph_aux1 */ -beginfunction f8312 -( -25.457281, -0.000955) -( 0.000000, 0.000000) -( 35.313988, 0.001147) -endfunction - -/* tz scaphoid-scaph_aux2 */ -beginfunction f8323 -( -25.672873, 0.001403) -( -11.341747, 0.000201) -( -1.962238, -0.000004) -( 0.000000, 0.000000) -( 7.231682, 0.001196) -( 19.022863, 0.002061) -( 35.055382, 0.002131) -endfunction - -/* ty scaphoid-scaph_aux2*/ -beginfunction f8322 -( -25.427441, -0.000340) -( 0.000000, 0.000000) -( 35.265545, 0.000589) -endfunction - -/* ty hamate-ham_aux1 */ -beginfunction f8612 -( -25.387917, 0.004474) -( 0.022762, 0.000100) -( 49.934570, 0.010131) -endfunction - -/* tz trapezoid-trpzd_aux1 */ -beginfunction f8813 -( -25.387917, -0.004666) -( 0.022762, 0.000121) -( 35.457458, 0.001204) -endfunction - -/* tz trapezium-trpzm_aux2 */ -beginfunction f8723 -( -25.355537, 0.002558) -( 0.000000, 0.000000) -( 35.055393, -0.002218) -endfunction - -/* ty trapezium-trpzm_aux2 */ -beginfunction f8722 -( -25.355537, 0.000047) -( -18.831158, 0.000019) -( -12.065134, 0.000019) -( -6.990616, 0.000019) -( 0.000000, 0.000000) -( 35.055393, 0.006207) -endfunction - -/* tz metacarp1-mc1_aux1 */ -beginfunction f8912 -( -29.946768, 0.000249) -( -24.147320, 0.000174) -( -16.426317, 0.000101) -( -9.926102, 0.000039) -( 0.017050, 0.000000) -( 6.974461, 0.000096) -( 13.474678, 0.000191) -( 14.514713, 0.000241) -( 16.594780, 0.000274) -( 24.915058, 0.000244) -( 35.055397, 0.000223) -endfunction - -#endif /* RIGHT_HAND_FULL */ - - -#if LEFT_HAND_FULL - -/******************************************************************************* - LEFT HAND MODEL - joint file - - (c) Copyright 2002, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the segments joints, gencoords, and constraint functions - for the left hand model of an average adult male. It was developed by Silvia - Salinas Blemker and Scott Delp. - - This model is described in the paper: - Delp, S. L., Grierson, A. E., Buchanan, T. S. Moments generated by the wrist - muscles in flexion-extension and radial-ulnar deviation, Journal of - Biomechanics, vol. 29, pp. 1371-1376, 1996. - -*******************************************************************************/ -#ifndef LF1M1 - #define LF1M1 1 -#endif -#ifndef LF1M2 - #define LF1M2 1 -#endif -#ifndef LF1M3 - #define LF1M3 1 -#endif - -#ifndef LF2M1 - #define LF2M1 1 -#endif -#ifndef LF2M2 - #define LF2M2 1 -#endif -#ifndef LF2M3 - #define LF2M3 1 -#endif - -#ifndef LF3M1 - #define LF3M1 1 -#endif -#ifndef LF3M2 - #define LF3M2 1 -#endif -#ifndef LF3M3 - #define LF3M3 1 -#endif - -#ifndef LF4M1 - #define LF4M1 1 -#endif -#ifndef LF4M2 - #define LF4M2 1 -#endif -#ifndef LF4M3 - #define LF4M3 1 -#endif - -#ifndef LF5M1 - #define LF5M1 1 -#endif -#ifndef LF5M2 - #define LF5M2 1 -#endif -#ifndef LF5M3 - #define LF5M3 1 -#endif - -/*********************** Left Wrist and Hand ****************/ -beginsegment prox_carpals_l -beginfiles -pisiform_lv.asc -lunate_lv.asc -scaphoid_lv.asc -triquetrum_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - wrist_l - hand_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment dist_carpals_l -beginfiles -hamate_lv.asc -capitate_lv.asc -trapezoid_lv.asc -trapezium_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - wrist_l - hand_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment rotaxis_l -begingroups - aux_axes_l - wrist_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment prox_flex_CR_l -begingroups - aux_axes_l - wrist_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment prox_dev_CR_l -begingroups - aux_axes_l - wrist_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment dist_flex_CR_l -begingroups - aux_axes_l - wrist_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment dist_dev_CR_l -begingroups - aux_axes_l - wrist_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment scaph_aux1_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment scaph_aux2_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment cap_aux1_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment cap_aux2_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment ham_aux1_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment trpzd_aux1_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment trpzm_aux1_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment trpzm_aux2_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment mc1_aux1_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment carp_metacarp_l -begingroups - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment carp_thumb_l -begingroups - left - aux_axes_l - musc_aux_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.001 0.0 -endsegment - -beginsegment metacarpal2_l -beginfiles -metacarpal2_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment index_1_l -beginfiles -index_proximal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger2.M1 -0.012 -0.030 0.0 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment index_2_l -beginfiles -index_medial_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger2.M2 -0.008 -0.020 0.0 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment index_3_l -beginfiles -index_distal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger2.M3 -0.007 -0.010 0.0 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal3_l -beginfiles -metacarpal3_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Hand -0.020 -0.030 0.000 OTHER_WEIGHT -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment middle_1_l -beginfiles -middle_proximal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger3.M1 -0.012 -0.032 0.003 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment middle_2_l -beginfiles -middle_medial_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger3.M2 -0.008 -0.020 0.003 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment middle_3_l -beginfiles -middle_distal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger3.M3 -0.007 -0.016 0.003 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal4_l -beginfiles -metacarpal4_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment ring_1_l -beginfiles -ring_proximal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger4.M1 -0.012 -0.027 0.004 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment ring_2_l -beginfiles -ring_medial_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger4.M2 -0.008 -0.016 0.002 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment ring_3_l -beginfiles -ring_distal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger4.M3 -0.008 -0.010 0.002 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal5_l -beginfiles -metacarpal5_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment little_1_l -beginfiles -little_proximal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger5.M1 -0.010 -0.025 0.005 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment little_2_l -beginfiles -little_medial_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger5.M2 -0.008 -0.015 0.003 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment little_3_l -beginfiles -little_distal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Finger5.M3 -0.008 -0.010 0.002 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x 1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - -beginsegment metacarpal1_l -beginfiles -metacarpal1_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Thumb.M1 -0.012 -0.020 -0.006 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x -1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment thumb_2_l -beginfiles -thumb_proximal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Thumb.M2 -0.008 -0.020 0.000 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x -1.0 0.0 0.0 -htr_y 0.0 -1.0 0.0 -endsegment - -beginsegment thumb_3_l -beginfiles -thumb_distal_lv.asc -endfiles -material my_bone -begingroups - left - arm_l - hand_l -endgroups -marker L.Thumb.M3 -0.007 -0.015 0.000 OTHER_WEIGHT fixed -gait_scale L_HAND HAND_SIZE -SHADOW_PARAMS -htr_o 0.0 0.0 0.0 -htr_x -1.0 0.0 0.0 -htr_y 0.0 -0.02 0.0 -endsegment - - - -/*********************************************/ -/* JOINTS */ -/*********************************************/ -beginjoint rad-rotaxis_l -segments radius_l rotaxis_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.008797 -ty constant -0.245841 -tz constant -0.015610 -r1 constant -5.000000 -r2 constant 22.500000 -r3 constant 0.000000 -endjoint - -beginjoint rotaxis-prox_flex_CR_l -segments rotaxis_l prox_flex_CR_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.000832 -ty constant 0.008099 -tz constant 0.000364 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f716(wrist_flex_l) -endjoint - -beginjoint prox_flex_dev_l -segments prox_flex_CR_l prox_dev_CR_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.004500 -ty constant 0.000000 -tz constant 0.000000 -r1 function f724(wrist_dev_l) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint prox_dev-dist_flex_l -segments prox_dev_CR_l dist_flex_CR_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000900 -ty constant -0.018000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 function f736(wrist_flex_l) -endjoint - -beginjoint dist_flex_dev_l -segments dist_flex_CR_l dist_dev_CR_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 function f744(wrist_dev_l) -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint prox_carpal_jnt_l -segments prox_dev_CR_l prox_carpals_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint dist_carpal_jnt_l -segments dist_dev_CR_l dist_carpals_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint scap-aux1_l -segments prox_carpals_l scaph_aux1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f9312(wrist_dev_l) -tz function f9313(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint scap-aux2_l -segments prox_carpals_l scaph_aux2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f9322(wrist_dev_l) -tz function f9323(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint trpzm-aux1_l -segments dist_carpals_l trpzm_aux1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f9712(wrist_dev_l) -tz function f9713(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint trpzm-aux2_l -segments dist_carpals_l trpzm_aux2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f9722(wrist_dev_l) -tz function f9723(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint trpzd-aux1_l -segments dist_carpals_l trpzd_aux1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz function f9813(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint cap-aux1_l -segments dist_carpals_l cap_aux1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f9512(wrist_dev_l) -tz function f9513(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint cap-aux2_l -segments dist_carpals_l cap_aux2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz function f9523(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint ham-aux1_l -segments dist_carpals_l ham_aux1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty function f9612(wrist_dev_l) -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint auxCMC_l -segments dist_dev_CR_l carp_metacarp_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint auxThumb_l -segments dist_dev_CR_l carp_thumb_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.006000 -ty constant -0.010000 -tz constant -0.032000 -r1 constant -30.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - -beginjoint CMC2_l -segments carp_metacarp_l metacarpal2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.004230 -ty constant -0.008550 -tz constant 0.004500 -r1 constant 10.000000 -r2 constant 15.000000 -r3 constant 0.000000 -endjoint - -beginjoint MCP2_l -segments metacarpal2_l index_1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.002162 -ty constant -0.061650 -tz constant -0.040500 -#if LF2M1 || (LF2M2 || LF2M3) -r1 function f1(MCP2_lateral_l) -r2 constant 0.000000 -r3 function f1(MCP2_flex_l) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP_l -segments index_1_l index_2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000450 -ty constant -0.043434 -tz constant -0.007191 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF2M2 || LF2M3 - #if LF2M1 - r3 function f1(PIP2_flex_l) - #else - r3 function f1(MCP2_flex_l) - #endif -#else - #if LF2M1 - r3 function f1(MCP2_flex_l) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP_l -segments index_2_l index_3_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.001737 -ty constant -0.029493 -tz constant -0.004248 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF2M3 - #if LF2M2 - r3 function f1(DIP2_flex_l) - #elif LF2M1 - r3 function f1(PIP2_flex_l) - #else - r3 function f1(MCP2_flex_l) - #endif -#else - #if LF2M2 - #if LF2M1 - r3 function f1(PIP2_flex_l) - #else - r3 function f1(MCP2_flex_l) - #endif - #else - #if LF2M1 - r3 function f1(MCP2_flex_l) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC3_l -segments carp_metacarp_l metacarpal3_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.004230 -ty constant -0.007650 -tz constant 0.004500 -r1 constant 5.000000 -r2 constant 10.000000 -r3 constant 5.000000 -endjoint - -beginjoint MCP3_l -segments metacarpal3_l middle_1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.002430 -ty constant -0.068670 -tz constant -0.014580 -#if LF3M1 || (LF3M2 || LF3M3) -r1 function f1(MCP3_lateral_l) -r2 constant 0.000000 -r3 function f1(MCP3_flex_l) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP3_l -segments middle_1_l middle_2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.003150 -ty constant -0.048924 -tz constant -0.002781 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF3M2 || LF3M3 - #if LF3M1 - r3 function f1(PIP3_flex_l) - #else - r3 function f1(MCP3_flex_l) - #endif -#else - #if LF3M1 - r3 function f1(MCP3_flex_l) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP3_l -segments middle_2_l middle_3_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000837 -ty constant -0.031653 -tz constant -0.001368 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF3M3 - #if LF3M2 - r3 function f1(DIP3_flex_l) - #elif LF3M1 - r3 function f1(PIP3_flex_l) - #else - r3 function f1(MCP3_flex_l) - #endif -#else - #if LF3M2 - #if LF3M1 - r3 function f1(PIP3_flex_l) - #else - r3 function f1(MCP3_flex_l) - #endif - #else - #if LF3M1 - r3 function f1(MCP3_flex_l) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC4_l -segments carp_metacarp_l metacarpal4_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.004230 -ty constant -0.007200 -tz constant 0.002970 -r1 constant 5.000000 -r2 constant 10.000000 -r3 constant 5.000000 -endjoint - -beginjoint MCP4_l -segments metacarpal4_l ring_1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000810 -ty constant -0.069300 -tz constant 0.006660 -#if LF4M1 || (LF4M2 || LF4M3) -r1 function f1(MCP4_lateral_l) -r2 constant 0.000000 -r3 function f1(MCP4_flex_l) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP4_l -segments ring_1_l ring_2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.002160 -ty constant -0.049644 -tz constant -0.000261 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF4M2 || LF4M3 - #if LF4M1 - r3 function f1(PIP4_flex_l) - #else - r3 function f1(MCP4_flex_l) - #endif -#else - #if LF4M1 - r3 function f1(MCP4_flex_l) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP4_l -segments ring_2_l ring_3_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.004347 -ty constant -0.026883 -tz constant -0.004338 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF4M3 - #if LF4M2 - r3 function f1(DIP4_flex_l) - #elif LF4M1 - r3 function f1(PIP4_flex_l) - #else - r3 function f1(MCP4_flex_l) - #endif -#else - #if LF4M2 - #if LF4M1 - r3 function f1(PIP4_flex_l) - #else - r3 function f1(MCP4_flex_l) - #endif - #else - #if LF4M1 - r3 function f1(MCP4_flex_l) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC5_l -segments carp_metacarp_l metacarpal5_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.001800 -ty constant -0.009000 -tz constant -0.003150 -r1 constant 5.000000 -r2 constant 10.000000 -r3 constant 5.000000 -endjoint - -beginjoint MCP5_l -segments metacarpal5_l little_1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant -0.000360 -ty constant -0.056880 -tz constant 0.032940 -#if LF5M1 || (LF5M2 || LF5M3) -r1 function f1(MCP5_lateral_l) -r2 constant 0.000000 -r3 function f1(MCP5_flex_l) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PIP5_l -segments little_1_l little_2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.002340 -ty constant -0.039114 -tz constant 0.002439 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF5M2 || LF5M3 - #if LF5M1 - r3 function f1(PIP5_flex_l) - #else - r3 function f1(MCP5_flex_l) - #endif -#else - #if LF5M1 - r3 function f1(MCP5_flex_l) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DIP5_l -segments little_2_l little_3_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.003177 -ty constant -0.023013 -tz constant -0.002052 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF5M3 - #if LF5M2 - r3 function f1(DIP5_flex_l) - #elif LF5M1 - r3 function f1(PIP5_flex_l) - #else - r3 function f1(MCP5_flex_l) - #endif -#else - #if LF5M2 - #if LF5M1 - r3 function f1(PIP5_flex_l) - #else - r3 function f1(MCP5_flex_l) - #endif - #else - #if LF5M1 - r3 function f1(MCP5_flex_l) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint CMC1_l -segments carp_thumb_l metacarpal1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 -0.866000 0.500000 -tx constant 0.000000 -ty constant 0.000000 -tz constant 0.000000 -#if LF1M1 || (LF1M2 || LF1M3) -r1 function f1(thumb_abd_l) -r2 constant 0.000000 -r3 function f1(thumb_flex_l) -#else -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -#endif -endjoint - -beginjoint PP1_l -segments metacarpal1_l thumb_2_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant -0.038000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 30.000000 -#if LF1M2 || LF1M3 - #if LF1M1 - r3 function f1(PP1_flex_l) - #else - r3 function f1(thumb_abd_l) - #endif -#else - #if LF1M1 - r3 function f1(thumb_abd_l) - #else - r3 constant 0.000000 - #endif -#endif -endjoint - -beginjoint DP1_l -segments thumb_2_l thumb_3_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.003000 -ty constant -0.032000 -tz constant 0.000000 -r1 constant 0.000000 -r2 constant 0.000000 -#if LF1M3 - #if LF1M2 - r3 function f1(DP1_flex_l) - #elif LF1M1 - r3 function f1(PP1_flex_l) - #else - r3 function f1(thumb_abd_l) - #endif -#else - #if LF1M2 - #if LF1M1 - r3 function f1(PP1_flex_l) - #else - r3 function f1(thumb_abd_l) - #endif - #else - #if LF1M1 - r3 function f1(thumb_abd_l) - #else - r3 constant 0.000000 - #endif - #endif -#endif -endjoint - -beginjoint thumb-aux1_l -segments metacarpal1_l mc1_aux1_l -order t r1 r2 r3 -axis1 -1.000000 0.000000 0.000000 -axis2 0.000000 -1.000000 0.000000 -axis3 0.000000 0.000000 1.000000 -tx constant 0.000000 -ty constant 0.000000 -tz function f91012(wrist_dev_l) -r1 constant 0.000000 -r2 constant 0.000000 -r3 constant 0.000000 -endjoint - - -/*********************************************/ -/* GENCOORDS */ -/*********************************************/ - -#if LF2M1 || (LF2M2 || LF2M3) -begingencoord MCP2_lateral_l -range -20.000000 20.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord - -begingencoord MCP2_flex_l -range -10.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF2M1 && (LF2M2 || LF2M3) -begingencoord PIP2_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF2M2 && LF2M3 -begingencoord DIP2_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF3M1 || (LF3M2 || LF3M3) -begingencoord MCP3_lateral_l -range -20.000000 20.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord - -begingencoord MCP3_flex_l -range -10.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF3M1 && (LF3M2 || LF3M3) -begingencoord PIP3_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF3M2 && LF3M3 -begingencoord DIP3_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF4M1 || (LF4M2 || LF4M3) -begingencoord MCP4_lateral_l -range -20.000000 20.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord - -begingencoord MCP4_flex_l -range -10.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF4M1 && (LF4M2 || LF4M3) -begingencoord PIP4_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF4M2 && LF4M3 -begingencoord DIP4_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF5M1 || (LF5M2 || LF5M3) -begingencoord MCP5_lateral_l -range -20.000000 20.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord - -begingencoord MCP5_flex_l -range -10.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF5M1 && (LF5M2 || LF5M3) -begingencoord PIP5_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF5M2 && LF5M3 -begingencoord DIP5_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF1M1 || (LF1M2 || LF1M3) -begingencoord thumb_abd_l -range -45.000000 15.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord - -begingencoord thumb_flex_l -range -45.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF1M1 && (LF1M2 || LF1M3) -begingencoord PP1_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -#if LF1M2 && LF1M3 -begingencoord DP1_flex_l -range -5.000000 90.000000 -begingroups - left - arm_l - hand_l -endgroups -clamped yes -endgencoord -#endif - -/*********************************************/ -/* FUNCTIONS */ -/*********************************************/ -/* radius-prox_flex RZ */ -beginfunction f716 -( -90.000000, -45.000000) -( 90.000000, 45.000000) -endfunction - -/* prox_flex-prox_dev rX */ -beginfunction f724 -( -25.000000, -9.375000) -( 0.000000, 0.000000) -( 35.000000, 17.500000) -endfunction - -/* prox_dev-dist_flex rZ */ -beginfunction f736 -( -70.000000, -35.000000) -( 70.000000, 35.000000) -endfunction - -/* dist_flex-dist_dev rX */ -beginfunction f744 -( -25.000000, -15.625000) -( 0.000000, 0.000000) -( 35.000000, 17.500000) -endfunction - -/* ty capitate-cap_aux1 */ -beginfunction f9512 -( -25.403461, -0.005296) -( -12.552980, -0.001860) -( 0.000000, 0.000000) -( 7.255741, -0.001401) -( 12.481216, -0.002375) -( 35.212013, -0.004810) -endfunction - -/* tz capitate-cap_aux1 */ -beginfunction f9513 -( -25.354568, 0.000723) -( -10.125652, 0.000708) -( -3.840702, 0.000317) -( 0.000000, 0.000000) -( 5.468287, -0.000859) -( 10.564981, -0.001663) -( 16.147070, -0.002381) -( 21.782553, -0.002774) -( 29.252764, -0.002942) -( 35.077644, -0.002942) -endfunction - -/* tz capitate-cap_aux2 */ -beginfunction f9523 -( -25.355537, 0.003199) -( 0.000000, 0.000000) -( 35.115795, -0.003638) -endfunction - -/* ty trapezium trpzm_aux1 */ -beginfunction f9712 -( -25.355537, -0.000969) -( 0.000000, 0.000000) -( 22.248272, 0.002119) -( 35.055393, 0.003237) -endfunction - -/* tz trapezium-trpzm_aux1 */ -beginfunction f9713 -( -34.559998, 0.000000) -( -26.475258, 0.000006) -( -18.111734, 0.000003) -( -8.075508, 0.000003) -( 0.000000, 0.000000) -( 35.136032, -0.000904) -endfunction - -/* tz scaphoid-scaph_aux1 */ -beginfunction f9313 -( -26.081165, -0.000932) -( -10.566132, -0.000266) -( 0.000000, 0.000000) -( 35.009277, 0.001910) -endfunction - -/* ty scaphoid-scaph_aux1 */ -beginfunction f9312 -( -25.457281, -0.000955) -( 0.000000, 0.000000) -( 35.313988, 0.001147) -endfunction - -/* tz scaphoid-scaph_aux2 */ -beginfunction f9323 -( -25.672873, 0.001403) -( -11.341747, 0.000201) -( -1.962238, -0.000004) -( 0.000000, 0.000000) -( 7.231682, 0.001196) -( 19.022863, 0.002061) -( 35.055382, 0.002131) -endfunction - -/* ty scaphoid-scaph_aux2*/ -beginfunction f9322 -( -25.427441, -0.000340) -( 0.000000, 0.000000) -( 35.265545, 0.000589) -endfunction - -/* ty hamate-ham_aux1 */ -beginfunction f9612 -( -25.387917, 0.004474) -( 0.022762, 0.000100) -( 49.934570, 0.010131) -endfunction - -/* tz trapezoid-trpzd_aux1 */ -beginfunction f9813 -( -25.387917, -0.004666) -( 0.022762, 0.000121) -( 35.457458, 0.001204) -endfunction - -/* tz trapezium-trpzm_aux2 */ -beginfunction f9723 -( -25.355537, 0.002558) -( 0.000000, 0.000000) -( 35.055393, -0.002218) -endfunction - -/* ty trapezium-trpzm_aux2 */ -beginfunction f9722 -( -25.355537, 0.000047) -( -18.831158, 0.000019) -( -12.065134, 0.000019) -( -6.990616, 0.000019) -( 0.000000, 0.000000) -( 35.055393, 0.006207) -endfunction - -/* tz metacarp1-mc1_aux1 */ -beginfunction f91012 -( -29.946768, 0.000249) -( -24.147320, 0.000174) -( -16.426317, 0.000101) -( -9.926102, 0.000039) -( 0.017050, 0.000000) -( 6.974461, 0.000096) -( 13.474678, 0.000191) -( 14.514713, 0.000241) -( 16.594780, 0.000274) -( 24.915058, 0.000244) -( 35.055397, 0.000223) -endfunction - -#endif /* LEFT_HAND_FULL */ diff --git a/OpenSim/Utilities/simmToOpenSim/test/mocap.msl b/OpenSim/Utilities/simmToOpenSim/test/mocap.msl deleted file mode 100644 index 1187d280fd..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/test/mocap.msl +++ /dev/null @@ -1,1734 +0,0 @@ -/******************************************************************************* - MOCAP MODEL - muscle file - - (c) Copyright 2001-5, MusculoGraphics, a division of - Motion Analysis Corporation. - All rights reserved. - - This file contains the muscle definitions for the SIMM model of an average - adult male. It was developed by various biomechanics researchers, including: - - Scott Delp, Ph.D., Stanford University - Wendy Murray, Ph.D., Stanford University - Silvia Salinas Blemker, M.S., Stanford University - Anita Vasavada, Ph.D., Washington State University - Srikanth Suryanarayanan, M.S., Rehabilitation Institute of Chicago - Frans van der Helm, Ph.D., Delft University - -*******************************************************************************/ - -/* If MUSCLE_FORCES is set to 0, then the muscles will not - * contain force-generating properties, so you will not be - * able to plot muscle forces for this model. If MUSCLE_FORCES - * is set to 1, then the force-generating properties for - * an average adult male (which the mocap model represents) are - * included. However, since this mocap model is scaled to fit - * the motion capture subject, it is important to understand - * how force-generating properties are scaled before you plot - * muscle forces for a scaled model. Scaling of force-generating - * properties is currently an open research topic, so there is - * no generally accepted method for scaling them. The scalemodel - * utility in SIMM ASSUMES THAT FORCE-GENERATING PROPERTIES - * SCALE WITH TOTAL MUSCLE-TENDON LENGTH. For each muscle, SIMM - * computes the muscle-tendon length in the unscaled model (Lu) - * and in the scaled model (Ls). The force-generating properties - * of max_isometric_force, optimal_fiber_length, and - * resting_tendon_length are then scaled by the factor (Ls/Lu). - * If you do not accept this method of scaling, then you should - * not include force-generating muscle properties in your mocap - * model. - */ - -#define MUSCLE_FORCES 1 - -beginmuscle defaultmuscle -#if MUSCLE_FORCES -begintendonforcelengthcurve -/* (tendon strain, normalized force) */ -(-1.000,0.0000) -(-0.0020,0.0000) -(-0.0010,0.0000) -(0.00000,0.0000) -(0.00131,0.0108) -(0.00281,0.0257) -(0.00431,0.0435) -(0.00581,0.0652) -(0.00731,0.0915) -(0.00881,0.123) -(0.01030,0.161) -(0.01180,0.208) -(0.01230,0.227) -(9.20000,345.0) -(9.20100,345.0) -(9.20200,345.0) -(12.0000,345.0) -endtendonforcelengthcurve - -beginactiveforcelengthcurve -/* (norm length, norm force) */ -(0.000000,0.000000) -(0.401000,0.000000) -(0.402000,0.000000) -(0.403500,0.000000) -(0.527250,0.226667) -(0.628750,0.636667) -(0.718750,0.856667) -(0.861250,0.950000) -(1.045000,0.993333) -(1.217500,0.770000) -(1.438750,0.246667) -(1.618750,0.000000) -(1.620000,0.000000) -(1.621000,0.000000) -(2.000000,0.000000) -endactiveforcelengthcurve - -beginpassiveforcelengthcurve -/* (norm length, norm force) */ -(0.80000,0.000000) -(0.998000,0.000000) -(0.999000,0.000000) -(1.000000,0.000000) -(1.100000,0.035) -(1.200000,0.120) -(1.300000,0.260) -(1.400000,0.550) -(1.500000,1.170) -(1.600000,2.000000) -(1.601000,2.000000) -(1.602000,2.000000) -(2.000000,2.000000) -endpassiveforcelengthcurve - -beginforcevelocitycurve -/* velocity, normalized force */ -(-1.001000,0.000000) -(-1.000000,0.000000) -(-0.950000,0.010417) -(-0.900000,0.021739) -(-0.850000,0.034091) -(-0.800000,0.047619) -(-0.750000,0.062500) -(-0.700000,0.078947) -(-0.650000,0.097222) -(-0.600000,0.117647) -(-0.550000,0.140625) -(-0.500000,0.166667) -(-0.450000,0.196429) -(-0.400000,0.230769) -(-0.350000,0.270833) -(-0.300000,0.318182) -(-0.250000,0.375000) -(-0.200000,0.444444) -(-0.150000,0.531250) -(-0.100000,0.642857) -(-0.050000,0.791667) -(0.000000,1.000000) -(0.050000,1.482014) -(0.100000,1.601571) -(0.150000,1.655791) -(0.200000,1.686739) -(0.250000,1.706751) -(0.300000,1.720753) -(0.350000,1.731099) -(0.400000,1.739055) -(0.450000,1.745365) -(0.500000,1.750490) -(0.550000,1.754736) -(0.600000,1.758312) -(0.650000,1.761364) -(0.700000,1.763999) -(0.750000,1.766298) -(0.800000,1.768321) -(0.850000,1.770115) -(0.900000,1.771717) -(0.950000,1.773155) -(1.000000,1.774455) -endforcevelocitycurve -max_contraction_velocity 10.0 /* fiberlengths/second */ -#endif -min_thickness 0.003 /* 0.003 */ -max_thickness 0.012 /* 0.012 */ -min_material muscle_min -max_material muscle_max -endmuscle - - -/*********** Right Leg ***********/ - -/* GLUTEUS MAXIMUS */ -beginmuscle glut_max1_r -beginpoints --0.11441 0.02747 0.06060 segment pelvis --0.11422 -0.03160 0.08360 segment pelvis --0.0457 -0.0248 0.0392 segment femur_r --0.0277 -0.0566 0.0470 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 382.0 /* source: Brand */ -optimal_fiber_length 0.1420 /* source: Friederich */ -tendon_slack_length 0.1250 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_max2_r -beginpoints --0.12356 -0.01693 0.05130 segment pelvis --0.11053 -0.08535 0.08640 segment pelvis --0.0426 -0.0530 0.0293 segment femur_r --0.0156 -0.1016 0.0419 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 546.0 /* source: Brand */ -optimal_fiber_length 0.1470 /* source: Friederich */ -tendon_slack_length 0.1270 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_max3_r -beginpoints --0.12494 -0.09986 0.00740 segment pelvis --0.11347 -0.14063 0.03530 segment pelvis --0.0299 -0.1041 0.0135 segment femur_r --0.0060 -0.1419 0.0411 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 368.0 /* source: Brand */ -optimal_fiber_length 0.1440 /* source: Friederich */ -tendon_slack_length 0.1140 /* source: Delp/Loan */ -pennation_angle 5.0 /* source: Friederich */ -#endif -endmuscle - -/* GLUTEUS MEDIUS */ -beginmuscle glut_med1_r -beginpoints --0.03475 0.01671 0.11590 segment pelvis --0.0218 -0.0117 0.0555 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 546.0 /* units: Newtons, source: Brand */ -optimal_fiber_length 0.0535 /* units: meters, source: Friederich */ -tendon_slack_length 0.0780 /* units: meters, source: Delp */ -pennation_angle 8.0 /* units: degrees, source: Friederich */ -#endif -endmuscle - -beginmuscle glut_med2_r -beginpoints --0.07977 0.00591 0.07920 segment pelvis --0.0258 -0.0058 0.0527 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 382.0 /* source: Brand */ -optimal_fiber_length 0.0845 /* source: Friederich */ -tendon_slack_length 0.0370 /* source: Delp/Loan */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_med3_r -beginpoints --0.10968 -0.02101 0.05980 segment pelvis --0.0309 -0.0047 0.0518 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 435.0 /* source: Brand */ -optimal_fiber_length 0.0646 /* source: Friederich */ -tendon_slack_length 0.0530 /* source: Delp */ -pennation_angle 19.0 /* source: Friederich*/ -#endif -endmuscle - -/* GLUTEUS MINIMUS */ -beginmuscle glut_min1_r -beginpoints --0.03186 -0.02203 0.10060 segment pelvis --0.0072 -0.0104 0.0560 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 180.0 /* source: Brand */ -optimal_fiber_length 0.0680 /* source: Friederich */ -tendon_slack_length 0.0160 /* source: Delp */ -pennation_angle 10.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_min2_r -beginpoints --0.04837 -0.02430 0.09410 segment pelvis --0.0096 -0.0104 0.0560 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 190.0 /* source: Brand */ -optimal_fiber_length 0.0560 /* source: Friederich */ -tendon_slack_length 0.0260 /* source: LOAN (was 0.10) */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle glut_min3_r -beginpoints --0.06800 -0.02863 0.08060 segment pelvis --0.0135 -0.0083 0.0550 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 215.0 /* source: Brand */ -optimal_fiber_length 0.0380 /* source: Friederich */ -tendon_slack_length 0.0510 /* source: Delp */ -pennation_angle 21.0 /* source: Friederich */ -#endif -endmuscle - -/* ADDUCTOR LONGUS */ -beginmuscle add_long_r -beginpoints --0.00125 -0.09861 0.00980 segment pelvis - 0.0050 -0.2111 0.0234 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 418.0 /* source: Wick */ -optimal_fiber_length 0.1380 /* source: Wick */ -tendon_slack_length 0.1105 /* source: Delp/Loan */ -pennation_angle 6.0 /* source: Wick */ -#endif -endmuscle - -/* ADDUCTOR BREVIS */ -beginmuscle add_brev_r -beginpoints --0.02422 -0.11069 0.01640 segment pelvis - 0.0009 -0.1196 0.0294 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 286.0 /* source: Wick */ -optimal_fiber_length 0.1330 /* source: Wick */ -tendon_slack_length 0.0130 /* source: Delp/Loan */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* ADDUCTOR MAGNUS */ -beginmuscle add_mag1_r -beginpoints --0.03307 -0.13459 0.02050 segment pelvis --0.0045 -0.1211 0.0339 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 346.0 /* source: Brand */ -optimal_fiber_length 0.0870 /* source: Friederich */ -tendon_slack_length 0.0600 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle add_mag2_r -beginpoints --0.04231 -0.13857 0.02580 segment pelvis - 0.0054 -0.2285 0.0227 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 312.0 /* source: Brand */ -optimal_fiber_length 0.1210 /* source: Friederich */ -tendon_slack_length 0.1300 /* source: Delp */ -pennation_angle 3.0 /* source: Friederich */ -#endif -endmuscle - -beginmuscle add_mag3_r -beginpoints --0.03671 -0.13615 0.02260 segment pelvis - 0.0070 -0.3837 -0.0266 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 444.0 /* source: Brand */ -optimal_fiber_length 0.1310 /* source: Friederich */ -tendon_slack_length 0.2600 /* source: Delp */ -pennation_angle 5.0 /* source: Friederich */ -#endif -endmuscle - -/* PECTINEUS */ -beginmuscle pectineus_r -beginpoints --0.01288 -0.08826 0.04010 segment pelvis --0.0122 -0.0822 0.0253 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 177.0 /* source: Wick */ -optimal_fiber_length 0.1330 /* source: Wick */ -tendon_slack_length 0.0010 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* ILIACUS */ -beginmuscle iliacus_r -beginpoints --0.06352 0.01551 0.08280 segment pelvis - 0.00297 -0.06223 0.08010 segment pelvis - 0.0017 -0.0543 0.0057 segment femur_r --0.0193 -0.0621 0.0129 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 429.0 /* source: Brand */ -optimal_fiber_length 0.1000 /* source: Friederich */ -tendon_slack_length 0.0900 /* source: Delp */ -pennation_angle 7.0 /* source: Friederich */ -#endif -endmuscle - -/* PSOAS */ -beginmuscle psoas_r -beginpoints --0.07047 0.04663 0.02070 segment pelvis - 0.00148 -0.06462 0.07090 segment pelvis - 0.00160 -0.05070 0.00380 segment femur_r --0.01880 -0.05970 0.01040 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 371.0 /* source: Brand */ -optimal_fiber_length 0.1040 /* source: Friederich */ -tendon_slack_length 0.1130 /* source: Delp/Loan */ -pennation_angle 8.0 /* source: Friederich */ -#endif -endmuscle - -/* QUADRATUS FEMORIS */ -beginmuscle quad_fem_r -beginpoints --0.07364 -0.14159 0.04700 segment pelvis --0.0381 -0.0359 0.0366 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 254.0 /* source: Brand */ -optimal_fiber_length 0.0540 /* source: Friederich */ -tendon_slack_length 0.0240 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -/* GEMELLUS */ -beginmuscle gemellus_r -beginpoints --0.08011 -0.10912 0.06640 segment pelvis --0.0142 -0.0033 0.0443 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 109.0 /* source: Brand */ -optimal_fiber_length 0.0240 /* source: Friederich */ -tendon_slack_length 0.0390 /* source: Delp */ -pennation_angle 0.0 /* source: Friederich */ -#endif -endmuscle - -/* PIRIFORMIS */ -beginmuscle piri_r -beginpoints --0.13606 -0.06815 0.02910 segment pelvis --0.10070 -0.06748 0.06050 segment pelvis --0.0148 -0.0036 0.0437 segment femur_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 296.0 /* source: Brand */ -optimal_fiber_length 0.0260 /* source: Friederich */ -tendon_slack_length 0.1060 /* source: Delp/Loan */ -pennation_angle 10.0 /* source: Friederich */ -#endif -endmuscle - -/* TENSOR FASCIAE LATAE */ -beginmuscle TFL_r -beginpoints --0.02327 0.01012 0.11910 segment pelvis - 0.0294 -0.0995 0.0597 segment femur_r - 0.0054 -0.4049 0.0357 segment femur_r - 0.0060 -0.0487 0.0297 segment tibia_r -endpoints -begingroups - right leg_r hip_r -endgroups -#if MUSCLE_FORCES -max_force 155.0 /* source: Brand */ -optimal_fiber_length 0.0950 /* source: Friederich */ -tendon_slack_length 0.4250 /* source: Delp */ -pennation_angle 3.0 /* source: Friederich */ -#endif -endmuscle - -/* GRACILIS */ -beginmuscle gracilis_r -beginpoints --0.01966 -0.11754 0.00290 segment pelvis --0.0154 -0.0475 -0.0358 segment tibia_r - 0.0060 -0.0836 -0.0228 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 108.0 /* source: Wick */ -optimal_fiber_length 0.3520 /* source: Wick */ -tendon_slack_length 0.1400 /* source: Delp */ -pennation_angle 3.0 /* source: Wick */ -#endif -endmuscle - -/* SEMIMEMBRANOSUS */ -beginmuscle semimem_r -beginpoints --0.08147 -0.12944 0.06450 segment pelvis --0.0243 -0.0536 -0.0194 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1030.0 /* source: Wick */ -optimal_fiber_length 0.0800 /* source: Wick */ -tendon_slack_length 0.3590 /* source: Delp */ -pennation_angle 15.0 /* source: Wick */ -#endif -endmuscle - -/* SEMITENDINOSUS */ -beginmuscle semiten_r -beginpoints --0.08522 -0.13318 0.05530 segment pelvis --0.0314 -0.0545 -0.0146 segment tibia_r --0.0113 -0.0746 -0.0245 segment tibia_r - 0.0027 -0.0956 -0.0193 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 328.0 /* source: Wick */ -optimal_fiber_length 0.2010 /* source: Wick */ -tendon_slack_length 0.2620 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* BICEPS FEMORIS long head */ -beginmuscle bi_fem_lh_r -beginpoints --0.08685 -0.12925 0.06160 segment pelvis --0.0081 -0.0729 0.0423 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 717.0 /* source: Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.1090 /* source: Wick */ -tendon_slack_length 0.3410 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* BICEPS FEMORIS short head */ -beginmuscle bi_fem_sh_r -beginpoints - 0.0050 -0.2111 0.0234 segment femur_r --0.0101 -0.0725 0.0406 segment tibia_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 402.0 /* source:Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.1730 /* source: Wick */ -tendon_slack_length 0.1000 /* source: Delp */ -pennation_angle 23.0 /* source: Wick */ -#endif -endmuscle - -/* SARTORIUS */ -beginmuscle sartorius_r -beginpoints --0.00755 -0.00995 0.11980 segment pelvis --0.0030 -0.3568 -0.0421 segment femur_r --0.0056 -0.0419 -0.0399 segment tibia_r - 0.0060 -0.0589 -0.0383 segment tibia_r - 0.0243 -0.0840 -0.0252 segment tibia_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 104.0 /* source: Wick */ -optimal_fiber_length 0.5790 /* source: Wick */ -tendon_slack_length 0.0400 /* source: Delp */ -pennation_angle 0.0 /* source: Wick */ -#endif -endmuscle - -/* RECTUS FEMORIS */ -beginmuscle rectus_fem_r -beginpoints --0.00990 -0.04067 0.09180 segment pelvis - 0.0334 -0.4030 0.0019 segment femur_r ranges 1 knee_flex_r (83.65, 150.0) - 0.0121 0.0437 -0.0010 segment patella_r -endpoints -begingroups - right leg_r hip_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 779.0 /* source: Wick */ -optimal_fiber_length 0.0840 /* source: Wick */ -tendon_slack_length 0.3460 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* VASTUS MEDIALIS */ -beginmuscle vas_med_r -beginpoints -0.0140 -0.2099 0.0188 segment femur_r -0.0356 -0.2769 0.0009 segment femur_r -0.0370 -0.4048 -0.0125 segment femur_r ranges 1 knee_flex_r (69.32, 150.0) -0.0274 -0.4255 -0.0131 segment femur_r ranges 1 knee_flex_r (101.99, 150.0) -0.0063 0.0445 -0.0170 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1294.0 /* source: Wick */ -optimal_fiber_length 0.0890 /* source: Wick */ -tendon_slack_length 0.1260 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* VASTUS INTERMEDIUS */ -beginmuscle vas_int_r -beginpoints -0.0290 -0.1924 0.0310 segment femur_r -0.0335 -0.2084 0.0285 segment femur_r -0.0343 -0.4030 0.0055 segment femur_r ranges 1 knee_flex_r (81.36, 150.0) -0.0058 0.0480 -0.0006 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1365.0 /* source: Wick */ -optimal_fiber_length 0.0870 /* source: Wick */ -tendon_slack_length 0.1360 /* source: Delp */ -pennation_angle 3.0 /* source: Wick */ -#endif -endmuscle - -/* VASTUS LATERALIS */ -beginmuscle vas_lat_r -beginpoints -0.0048 -0.1854 0.0349 segment femur_r -0.0269 -0.2591 0.0409 segment femur_r -0.0361 -0.4030 0.0205 segment femur_r ranges 1 knee_flex_r (69.32, 150.0) -0.0253 -0.4243 0.0184 segment femur_r ranges 1 knee_flex_r (110.0, 150.0) -0.0103 0.0423 0.0141 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 1871.0 /* source: Wick */ -optimal_fiber_length 0.0840 /* source: */ -tendon_slack_length 0.1570 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* PATELLAR LIGAMENT */ -beginmuscle pat_lig_r -beginpoints -0.0390 -0.0822 0.0000 segment tibia_r -0.0021 0.0015 0.0001 segment patella_r -endpoints -begingroups - right leg_r knee_r -endgroups -#if MUSCLE_FORCES -max_force 100.0 /* source: Bogus */ -optimal_fiber_length 0.0500 /* source: Bogus */ -tendon_slack_length 0.0050 /* source: Bogus */ -pennation_angle 0.0 /* source: Bogus */ -#endif -endmuscle - -/* GASTROC medial head */ -beginmuscle gas_med_r -beginpoints --0.01270 -0.39290 -0.02350 segment femur_r --0.02390 -0.40220 -0.02580 segment femur_r ranges 1 knee_flex_r (-20.00, 44.12) --0.02170 -0.04870 -0.02950 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r knee_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 1113.0 /* source: Brand (Wick lumps the two heads) */ -optimal_fiber_length 0.0450 /* source: Wick */ -tendon_slack_length 0.4080 /* source: Delp */ -pennation_angle 17.0 /* source: Wick */ -#endif -endmuscle - -/* GASTROC lateral head */ -beginmuscle gas_lat_r -beginpoints --0.01550 -0.39460 0.02720 segment femur_r --0.02540 -0.40180 0.02740 segment femur_r ranges 1 knee_flex_r (-20.00, 44.12) --0.02420 -0.04810 0.02350 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r knee_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 488.0 /* source:Brand(Wick lumps the two heads) */ -optimal_fiber_length 0.0640 /* source: Wick */ -tendon_slack_length 0.3850 /* source: Delp */ -pennation_angle 8.0 /* source: Wick */ -#endif -endmuscle - -/* SOLEUS */ -beginmuscle soleus_r -beginpoints --0.00240 -0.15330 0.00710 segment tibia_r - 0.00336 0.03204 -0.00634 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 2839.0 /* 4176.0 source: Wick 3340 = 0.8 * 4176 */ -optimal_fiber_length 0.0300 /* Wick val=.025, Brand val= 0.03 */ -tendon_slack_length 0.2680 /* source: Delp */ -pennation_angle 25.0 /* source: Wick */ -#endif -endmuscle - -/* TIBIALIS POSTERIOR */ -beginmuscle tib_post_r -beginpoints --0.0094 -0.1348 0.0019 segment tibia_r --0.0144 -0.4051 -0.0229 segment tibia_r - 0.0417 0.0334 -0.0286 segment foot_r - 0.0772 0.0159 -0.0281 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 1270.0 /* source: Wick */ -optimal_fiber_length 0.0310 /* source: Wick */ -tendon_slack_length 0.3100 /* source: Delp */ -pennation_angle 12.0 /* source: Wick */ -#endif -endmuscle - -/* TIBIALIS ANTERIOR */ -beginmuscle tib_ant_r -beginpoints -0.0179 -0.1624 0.0115 segment tibia_r -0.0329 -0.3951 -0.0177 segment tibia_r -0.1166 0.0178 -0.0305 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 603.0 /* source: Wick */ -optimal_fiber_length 0.0980 /* source: Wick */ -tendon_slack_length 0.2230 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* EXTENSOR DIGITORUM */ -beginmuscle ext_dig_r -beginpoints - 0.00320 -0.13810 0.02760 segment tibia_r - 0.02890 -0.40070 0.00720 segment tibia_r - 0.09220 0.03880 -0.00010 segment foot_r - 0.16160 0.00550 0.01300 segment foot_r - 0.00030 0.00470 0.01530 segment toes_r - 0.04430 -0.00040 0.02500 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 341.0 /* source: Wick */ -optimal_fiber_length 0.1020 /* source: Wick */ -tendon_slack_length 0.3450 /* source: LOAN (was 0.30) */ -pennation_angle 8.0 /* source: Wick */ -#endif -endmuscle - -/* EXTENSOR HALLUCIS */ -beginmuscle ext_hal_r -beginpoints - 0.00120 -0.17670 0.02280 segment tibia_r - 0.03260 -0.39850 -0.00850 segment tibia_r - 0.09700 0.03890 -0.02110 segment foot_r - 0.12930 0.03090 -0.02570 segment foot_r - 0.17778 0.01196 -0.02730 segment foot_r - 0.01217 0.01039 -0.02633 segment toes_r - 0.05628 -0.00131 -0.01852 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 108.0 /* source: Wick */ -optimal_fiber_length 0.1110 /* source: Wick */ -tendon_slack_length 0.3050 /* source: Delp */ -pennation_angle 6.0 /* source: Wick */ -#endif -endmuscle - -/* FLEXOR DIGITORUM */ -beginmuscle flex_dig_r -beginpoints --0.00830 -0.20460 -0.00180 segment tibia_r --0.01540 -0.40510 -0.01960 segment tibia_r - 0.04360 0.03150 -0.02800 segment foot_r - 0.07080 0.01760 -0.02630 segment foot_r - 0.16361 -0.00798 0.01009 segment foot_r --0.00190 -0.00780 0.01470 segment toes_r - 0.02850 -0.00710 0.02150 segment toes_r - 0.04410 -0.00600 0.02420 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 310.0 /* source: Wick */ -optimal_fiber_length 0.0340 /* source: Wick */ -tendon_slack_length 0.4000 /* source: LOAN (was 0.39) */ -pennation_angle 7.0 /* source: Wick */ -#endif -endmuscle - -/* FLEXOR HALLUCIS */ -beginmuscle flex_hal_r -beginpoints --0.00790 -0.23340 0.02440 segment tibia_r --0.01860 -0.40790 -0.01740 segment tibia_r - 0.03740 0.02760 -0.02410 segment foot_r - 0.10380 0.00680 -0.02560 segment foot_r - 0.16956 -0.00501 -0.02761 segment foot_r - 0.00839 -0.00726 -0.02690 segment toes_r - 0.05607 -0.00839 -0.01817 segment toes_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 322.0 /* source: Wick */ -optimal_fiber_length 0.0430 /* source: Wick */ -tendon_slack_length 0.3800 /* source: LOAN (was 0.40) */ -pennation_angle 10.0 /* source: Wick */ -#endif -endmuscle - -/* PERONEUS BREVIS */ -beginmuscle per_brev_r -beginpoints --0.00700 -0.26460 0.03250 segment tibia_r --0.01980 -0.41840 0.02830 segment tibia_r --0.01440 -0.42950 0.02890 segment tibia_r - 0.04710 0.02700 0.02330 segment foot_r - 0.07569 0.01934 0.02942 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 348.0 /* source: Wick */ -optimal_fiber_length 0.0500 /* source: Wick */ -tendon_slack_length 0.1610 /* source: Delp */ -pennation_angle 5.0 /* source: Wick */ -#endif -endmuscle - -/* PERONEUS LONGUS */ -beginmuscle per_long_r -beginpoints - 0.0005 -0.1568 0.0362 segment tibia_r --0.0207 -0.4205 0.0286 segment tibia_r --0.0162 -0.4319 0.0289 segment tibia_r - 0.0438 0.0230 0.0221 segment foot_r - 0.0681 0.0106 0.0284 segment foot_r - 0.0852 0.0069 0.0118 segment foot_r - 0.1203 0.0085 -0.0184 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 754.0 /* source: Wick */ -optimal_fiber_length 0.0490 /* source: Wick */ -tendon_slack_length 0.3450 /* source: Delp */ -pennation_angle 10.0 /* source: Wick */ -#endif -endmuscle - -/* PERONEUS TERTIUS */ -beginmuscle per_tert_r -beginpoints -0.0010 -0.2804 0.0231 segment tibia_r -0.0229 -0.4069 0.0159 segment tibia_r -0.0857 0.0228 0.0299 segment foot_r -endpoints -begingroups - right leg_r ankle_r -endgroups -#if MUSCLE_FORCES -max_force 90.0 /* source: Brand (not reported by wick) */ -optimal_fiber_length 0.0790 /* source: Friederich */ -tendon_slack_length 0.1000 /* source: Delp */ -pennation_angle 13.0 /* source: Friederich */ -#endif -endmuscle - - -/*********** Left Leg ***********/ - -beginmuscle glut_max1_l -beginpoints --0.11441 0.02747 -0.06060 segment pelvis --0.11422 -0.03160 -0.08360 segment pelvis --0.0457 -0.0248 -0.0392 segment femur_l --0.0277 -0.0566 -0.0470 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 382.000 -optimal_fiber_length 0.14200 -tendon_slack_length 0.12500 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle glut_max2_l -beginpoints --0.12356 -0.01693 -0.05130 segment pelvis --0.11053 -0.08535 -0.08640 segment pelvis --0.0426 -0.0530 -0.0293 segment femur_l --0.0156 -0.1016 -0.0419 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 546.000 -optimal_fiber_length 0.14700 -tendon_slack_length 0.12700 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle glut_max3_l -beginpoints --0.12494 -0.09986 -0.00740 segment pelvis --0.11347 -0.14063 -0.03530 segment pelvis --0.0299 -0.1041 -0.0135 segment femur_l --0.0060 -0.1419 -0.0411 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 368.000 -optimal_fiber_length 0.14400 -tendon_slack_length 0.11400 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle glut_med1_l -beginpoints --0.03475 0.01671 -0.11590 segment pelvis --0.0218 -0.0117 -0.0555 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 546.000 -optimal_fiber_length 0.05350 -tendon_slack_length 0.07800 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle glut_med2_l -beginpoints --0.07977 0.00591 -0.07920 segment pelvis --0.0258 -0.0058 -0.0527 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 382.000 -optimal_fiber_length 0.08450 -tendon_slack_length 0.03700 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle glut_med3_l -beginpoints --0.10968 -0.02101 -0.05980 segment pelvis --0.0309 -0.0047 -0.0518 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 435.000 -optimal_fiber_length 0.06460 -tendon_slack_length 0.05300 -pennation_angle 19.000 -#endif -endmuscle - -beginmuscle glut_min1_l -beginpoints --0.03186 -0.02203 -0.10060 segment pelvis --0.0072 -0.0104 -0.0560 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 180.000 -optimal_fiber_length 0.06800 -tendon_slack_length 0.01600 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle glut_min2_l -beginpoints --0.04837 -0.02430 -0.09410 segment pelvis --0.0096 -0.0104 -0.0560 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 190.000 -optimal_fiber_length 0.05600 -tendon_slack_length 0.02600 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle glut_min3_l -beginpoints --0.06800 -0.02863 -0.08060 segment pelvis --0.0135 -0.0083 -0.0550 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 215.000 -optimal_fiber_length 0.03800 -tendon_slack_length 0.05100 -pennation_angle 21.000 -#endif -endmuscle - -beginmuscle add_long_l -beginpoints --0.00125 -0.09861 -0.00980 segment pelvis - 0.0050 -0.2111 -0.0234 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 418.000 -optimal_fiber_length 0.13800 -tendon_slack_length 0.11050 -pennation_angle 6.000 -#endif -endmuscle - -beginmuscle add_brev_l -beginpoints --0.02422 -0.11069 -0.01640 segment pelvis - 0.0009 -0.1196 -0.0294 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 286.000 -optimal_fiber_length 0.13300 -tendon_slack_length 0.01300 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle add_mag1_l -beginpoints --0.03307 -0.13459 -0.02050 segment pelvis --0.0045 -0.1211 -0.0339 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 346.000 -optimal_fiber_length 0.08700 -tendon_slack_length 0.06000 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle add_mag2_l -beginpoints --0.04231 -0.13857 -0.02580 segment pelvis - 0.0054 -0.2285 -0.0227 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 312.000 -optimal_fiber_length 0.12100 -tendon_slack_length 0.13000 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle add_mag3_l -beginpoints --0.03671 -0.13615 -0.02260 segment pelvis - 0.0070 -0.3837 -0.0266 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 444.000 -optimal_fiber_length 0.13100 -tendon_slack_length 0.26000 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle pectineus_l -beginpoints --0.01288 -0.08826 -0.04010 segment pelvis --0.0122 -0.0822 -0.0253 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 177.000 -optimal_fiber_length 0.13300 -tendon_slack_length 0.00100 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle iliacus_l -beginpoints --0.06352 0.01551 -0.08280 segment pelvis - 0.00297 -0.06223 -0.08010 segment pelvis - 0.0017 -0.0543 -0.0057 segment femur_l --0.0193 -0.0621 -0.0129 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 429.000 -optimal_fiber_length 0.10000 -tendon_slack_length 0.09000 -pennation_angle 7.000 -#endif -endmuscle - -beginmuscle psoas_l -beginpoints --0.07047 0.04663 -0.02070 segment pelvis - 0.00148 -0.06462 -0.07090 segment pelvis - 0.00160 -0.05070 -0.00380 segment femur_l --0.01880 -0.05970 -0.01040 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 371.000 -optimal_fiber_length 0.10400 -tendon_slack_length 0.11300 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle quad_fem_l -beginpoints --0.07364 -0.14159 -0.04700 segment pelvis --0.0381 -0.0359 -0.0366 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 254.000 -optimal_fiber_length 0.05400 -tendon_slack_length 0.02400 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle gemellus_l -beginpoints --0.08011 -0.10912 -0.06640 segment pelvis --0.0142 -0.0033 -0.0443 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 109.000 -optimal_fiber_length 0.02400 -tendon_slack_length 0.03900 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle piri_l -beginpoints --0.13606 -0.06815 -0.02910 segment pelvis --0.10070 -0.06748 -0.06050 segment pelvis --0.0148 -0.0036 -0.0437 segment femur_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 296.000 -optimal_fiber_length 0.02600 -tendon_slack_length 0.10600 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle TFL_l -beginpoints --0.02327 0.01012 -0.11910 segment pelvis - 0.0294 -0.0995 -0.0597 segment femur_l - 0.0054 -0.4049 -0.0357 segment femur_l - 0.0060 -0.0487 -0.0297 segment tibia_l -endpoints -begingroups - left leg_l hip_l -endgroups -#if MUSCLE_FORCES -max_force 155.000 -optimal_fiber_length 0.09500 -tendon_slack_length 0.42500 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle gracilis_l -beginpoints --0.01966 -0.11754 -0.00290 segment pelvis --0.0154 -0.0475 0.0358 segment tibia_l - 0.0060 -0.0836 0.0228 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 108.000 -optimal_fiber_length 0.35200 -tendon_slack_length 0.14000 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle semimem_l -beginpoints --0.08147 -0.12944 -0.06450 segment pelvis --0.0243 -0.0536 0.0194 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1030.000 -optimal_fiber_length 0.08000 -tendon_slack_length 0.35900 -pennation_angle 15.000 -#endif -endmuscle - -beginmuscle semiten_l -beginpoints --0.08522 -0.13318 -0.05530 segment pelvis --0.0314 -0.0545 0.0146 segment tibia_l --0.0113 -0.0746 0.0245 segment tibia_l - 0.0027 -0.0956 0.0193 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 328.000 -optimal_fiber_length 0.20100 -tendon_slack_length 0.26200 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle bi_fem_lh_l -beginpoints --0.08685 -0.12925 -0.06160 segment pelvis --0.0081 -0.0729 -0.0423 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 717.000 -optimal_fiber_length 0.10900 -tendon_slack_length 0.34100 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle bi_fem_sh_l -beginpoints - 0.0050 -0.2111 -0.0234 segment femur_l --0.0101 -0.0725 -0.0406 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 402.000 -optimal_fiber_length 0.17300 -tendon_slack_length 0.10000 -pennation_angle 23.000 -#endif -endmuscle - -beginmuscle sartorius_l -beginpoints --0.00755 -0.00995 -0.11980 segment pelvis --0.0030 -0.3568 0.0421 segment femur_l --0.0056 -0.0419 0.0399 segment tibia_l - 0.0060 -0.0589 0.0383 segment tibia_l - 0.0243 -0.0840 0.0252 segment tibia_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 104.000 -optimal_fiber_length 0.57900 -tendon_slack_length 0.04000 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle rectus_fem_l -beginpoints --0.00990 -0.04067 -0.09180 segment pelvis - 0.0334 -0.4030 -0.0019 segment femur_l ranges 1 knee_flex_l (83.65, 150.00) - 0.0121 0.0437 0.0010 segment patella_l -endpoints -begingroups - left leg_l hip_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 779.000 -optimal_fiber_length 0.08400 -tendon_slack_length 0.34600 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle vas_med_l -beginpoints - 0.01400 -0.20990 -0.01880 segment femur_l - 0.03560 -0.27690 -0.00090 segment femur_l - 0.03700 -0.40480 0.01250 segment femur_l ranges 1 knee_flex_l (69.32, 150.00) - 0.02740 -0.42550 0.01310 segment femur_l ranges 1 knee_flex_l (101.99, 150.00) - 0.00630 0.04450 0.01700 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1294.000 -optimal_fiber_length 0.08900 -tendon_slack_length 0.12600 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle vas_int_l -beginpoints - 0.02900 -0.19240 -0.03100 segment femur_l - 0.03350 -0.20840 -0.02850 segment femur_l - 0.03430 -0.40300 -0.00550 segment femur_l ranges 1 knee_flex_l (81.36, 150.00) - 0.00580 0.04800 0.00060 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1365.000 -optimal_fiber_length 0.08700 -tendon_slack_length 0.13600 -pennation_angle 3.000 -#endif -endmuscle - -beginmuscle vas_lat_l -beginpoints - 0.00480 -0.18540 -0.03490 segment femur_l - 0.02690 -0.25910 -0.04090 segment femur_l - 0.03610 -0.40300 -0.02050 segment femur_l ranges 1 knee_flex_l (69.32, 150.00) - 0.02530 -0.42430 -0.01840 segment femur_l ranges 1 knee_flex_l (110.00, 150.00) - 0.01030 0.04230 -0.01410 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 1871.000 -optimal_fiber_length 0.08400 -tendon_slack_length 0.15700 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle pat_lig_l -beginpoints - 0.03900 -0.08220 0.00000 segment tibia_l - 0.00210 0.00150 -0.00010 segment patella_l -endpoints -begingroups - left leg_l knee_l -endgroups -#if MUSCLE_FORCES -max_force 100.000 -optimal_fiber_length 0.05000 -tendon_slack_length 0.00500 -pennation_angle 0.000 -#endif -endmuscle - -beginmuscle gas_med_l -beginpoints --0.01270 -0.39290 0.02350 segment femur_l --0.02390 -0.40220 0.02580 segment femur_l ranges 1 knee_flex_l (-20.00, 44.12) --0.02170 -0.04870 0.02950 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l knee_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 1113.000 -optimal_fiber_length 0.04500 -tendon_slack_length 0.40800 -pennation_angle 17.000 -#endif -endmuscle - -beginmuscle gas_lat_l -beginpoints --0.01550 -0.39460 -0.02720 segment femur_l --0.02540 -0.40180 -0.02740 segment femur_l ranges 1 knee_flex_l (-20.00, 44.12) --0.02420 -0.04810 -0.02350 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l knee_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 488.000 -optimal_fiber_length 0.06400 -tendon_slack_length 0.38500 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle soleus_l -beginpoints --0.00240 -0.15330 -0.00710 segment tibia_l - 0.00336 0.03204 0.00634 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 2839.000 -optimal_fiber_length 0.03000 -tendon_slack_length 0.26800 -pennation_angle 25.000 -#endif -endmuscle - -beginmuscle tib_post_l -beginpoints --0.00940 -0.13480 -0.00190 segment tibia_l --0.01440 -0.40510 0.02290 segment tibia_l - 0.04170 0.03340 0.02860 segment foot_l - 0.07720 0.01590 0.02810 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 1270.000 -optimal_fiber_length 0.03100 -tendon_slack_length 0.31000 -pennation_angle 12.000 -#endif -endmuscle - -beginmuscle tib_ant_l -beginpoints - 0.01790 -0.16240 -0.01150 segment tibia_l - 0.03290 -0.39510 0.01770 segment tibia_l - 0.11660 0.01780 0.03050 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 603.000 -optimal_fiber_length 0.09800 -tendon_slack_length 0.22300 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle ext_dig_l -beginpoints - 0.00320 -0.13810 -0.02760 segment tibia_l - 0.02890 -0.40070 -0.00720 segment tibia_l - 0.09220 0.03880 0.00010 segment foot_l - 0.16160 0.00550 -0.01300 segment foot_l - 0.00030 0.00470 -0.01530 segment toes_l - 0.04430 -0.00040 -0.02500 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 341.000 -optimal_fiber_length 0.10200 -tendon_slack_length 0.34500 -pennation_angle 8.000 -#endif -endmuscle - -beginmuscle ext_hal_l -beginpoints - 0.00120 -0.17670 -0.02280 segment tibia_l - 0.03260 -0.39850 0.00850 segment tibia_l - 0.09700 0.03890 0.02110 segment foot_l - 0.12930 0.03090 0.02570 segment foot_l - 0.17778 0.01196 0.02730 segment foot_l - 0.01217 0.01039 0.02633 segment toes_l - 0.05628 -0.00131 0.01852 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 108.000 -optimal_fiber_length 0.11100 -tendon_slack_length 0.30500 -pennation_angle 6.000 -#endif -endmuscle - -beginmuscle flex_dig_l -beginpoints --0.00830 -0.20460 0.00180 segment tibia_l --0.01540 -0.40510 0.01960 segment tibia_l - 0.04360 0.03150 0.02800 segment foot_l - 0.07080 0.01760 0.02630 segment foot_l - 0.16361 -0.00798 -0.01009 segment foot_l --0.00190 -0.00780 -0.01470 segment toes_l - 0.02850 -0.00710 -0.02150 segment toes_l - 0.04410 -0.00600 -0.02420 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 310.000 -optimal_fiber_length 0.03400 -tendon_slack_length 0.40000 -pennation_angle 7.000 -#endif -endmuscle - -beginmuscle flex_hal_l -beginpoints --0.00790 -0.23340 -0.02440 segment tibia_l --0.01860 -0.40790 0.01740 segment tibia_l - 0.03740 0.02760 0.02410 segment foot_l - 0.10380 0.00680 0.02560 segment foot_l - 0.16956 -0.00501 0.02761 segment foot_l - 0.00839 -0.00726 0.02690 segment toes_l - 0.05607 -0.00839 0.01817 segment toes_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 322.000 -optimal_fiber_length 0.04300 -tendon_slack_length 0.38000 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle per_brev_l -beginpoints --0.00700 -0.26460 -0.03250 segment tibia_l --0.01980 -0.41840 -0.02830 segment tibia_l --0.01440 -0.42950 -0.02890 segment tibia_l - 0.04710 0.02700 -0.02330 segment foot_l - 0.07569 0.01934 -0.02942 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 348.000 -optimal_fiber_length 0.05000 -tendon_slack_length 0.16100 -pennation_angle 5.000 -#endif -endmuscle - -beginmuscle per_long_l -beginpoints - 0.00050 -0.15680 -0.03620 segment tibia_l --0.02070 -0.42050 -0.02860 segment tibia_l --0.01620 -0.43190 -0.02890 segment tibia_l - 0.04380 0.02300 -0.02210 segment foot_l - 0.06810 0.01060 -0.02840 segment foot_l - 0.08520 0.00690 -0.01180 segment foot_l - 0.12030 0.00850 0.01840 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 754.000 -optimal_fiber_length 0.04900 -tendon_slack_length 0.34500 -pennation_angle 10.000 -#endif -endmuscle - -beginmuscle per_tert_l -beginpoints - 0.00100 -0.28040 -0.02310 segment tibia_l - 0.02290 -0.40690 -0.01590 segment tibia_l - 0.08570 0.02280 -0.02990 segment foot_l -endpoints -begingroups - left leg_l ankle_l -endgroups -#if MUSCLE_FORCES -max_force 90.000 -optimal_fiber_length 0.07900 -tendon_slack_length 0.10000 -pennation_angle 13.000 -#endif -endmuscle - diff --git a/OpenSim/Utilities/simmToOpenSim/tools.c b/OpenSim/Utilities/simmToOpenSim/tools.c deleted file mode 100644 index b2d68bff63..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/tools.c +++ /dev/null @@ -1,2452 +0,0 @@ -/******************************************************************************* - - TOOLS.C - - Author: Peter Loan - - Date: 8-DEC-88 - - Copyright (c) 1992-5 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: This file contains a bunch of generic tool - routines that are used in a wide range of circumstances. - - Routines: - finddistance : computes distance between two points, spans ref frames - get_path_between_frames : finds the path betwen any two ref frames - find_joint_between_frames : finds the joint spanning two ref frames - findvelocity : finds the velocity of a musculotendon unit - evaluate : finds the current value of a reference equation (dof) - error : handles error messages - setgencoord : sets value of a gencoord, marks joint matrices dirty - member : checks an int array for a specified member - change_filename_suffix : puts a new suffix on a filename - message : prints a message to stdout - gencoord_in_path : checks if a gencoord is used in a chain of joints - print_4x4matrix : prints a 4x4 matrix to stdout - register_tool : records tool info in array when tool is created - strcat3 : concatenates three strings together - mstrcpy : like strcpy, but mallocs space for the copy first - draw_title_area : draws the title area at the top of each tool window - check_title_area : checks if user hit a button in the title area - print_time : prints the current time to stdio (for benchmarking) - make_time_string : converts current time into readable string - convert_string : converts special chars in a string to underscores - set_viewport : sets the viewport and resets MODELVIEW and PROJECTION mats - -*******************************************************************************/ - -#include "universal.h" - -#include -#include -#include -#include - -#define _POSIX_ 1 - -#include -#include - -#include "globals.h" -#include "functions.h" -#include "normio.h" - - -/*************** DEFINES (for this file only) *********************************/ -#define BETA_VERSION 0 - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static int last_index = -1; -char* keys[] = { -"null_key","null_key","null_key","null_key","null_key","null_key","null_key","null_key", -"backspace_key","tab_key","null_key","null_key","null_key","return_key","enter_key", -"null_key","null_key","null_key","null_key","null_key","null_key","null_key","null_key", -"null_key","null_key","null_key","null_key","escape_key","null_key","null_key", -"null_key","null_key","space_key","bang_key","double_quote_key","pound_sign_key", -"dollar_key","percent_key","ampersand_key","single_quote_key", -"left_paren_key","right_paren_key","asterisk_key","plus_key", -"comma_key","dash_key","period_key","slash_key","zero_key","one_key","two_key","three_key", -"four_key","five_key","six_key","seven_key","eight_key","nine_key","colon_key","semicolon_key", -"less_than_key","equals_key","greater_than_key","question_mark_key","at_sign_key", -"A_key","B_key","C_key","D_key","E_key","F_key","G_key","H_key","I_key","J_key","K_key", -"L_key","M_key","N_key","O_key","P_key","Q_key","R_key","S_key","T_key","U_key","V_key", -"W_key","X_key","Y_key","Z_key","left_bracket_key","backslash_key", -"right_bracket_key","carat_key","underscore_key","back_quote_key", -"a_key","b_key","c_key","d_key","e_key","f_key","g_key","h_key","i_key", -"j_key","k_key","l_key","m_key","n_key","o_key","p_key","q_key","r_key","s_key", -"t_key","u_key","v_key","w_key","x_key","y_key","z_key","left_brace_key", -"vertical_bar_key","right_brace_key","tilde_key","delete_key", -"leftmouse_button","middlemouse_button","rightmouse_button", -"mouse_motion","window_shut","window_quit","input_change","depth_change","window_thaw", -"window_freeze","f1_key","f2_key","f3_key","f4_key","f5_key","f6_key","f7_key", -"f8_key","f9_key","f10_key","f11_key","f12_key","left_arrow_key","up_arrow_key", -"right_arrow_key","down_arrow_key","page_up_key","page_down_key","home_key", -"end_key","insert_key","shift_key","control_key","alt_ket","caps_lock_key"}; - - -/*************** GLOBAL VARIABLES (used in only a few files) ******************/ - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ -#if ! ENGINE -extern HWND __mdiClientHwnd; // Windows handle to main SIMM window -#endif - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static SBoolean verify_date(int day, int month, int year); -unsigned sysid(unsigned char id[16]); - -#if OPENSMAC -#undef ENGINE -#define ENGINE 1 -#endif - -/* MAKE_STRING_LOWER_CASE: */ - -void make_string_lower_case(char str_buffer[]) -{ - char c; - int curpos = 0; - - while ((c=str_buffer[curpos++]) != STRING_TERMINATOR) - if (c >= 65 && c <= 91) - str_buffer[curpos-1] += 32; -} - - -/* CALC_VECTOR_LENGTH: This routine calculates the length of a vector. The - * vector is defined by two points, which can be in different body segment - * reference frames. - */ -double calc_vector_length(ModelStruct* ms, double p1[], int frame1, double p2[], int frame2) -{ - double ans, x, p3[3]; - - p3[0] = p1[0]; - p3[1] = p1[1]; - p3[2] = p1[2]; - - // Convert the start point into the end point's frame. - if (frame1 != frame2) - convert(ms, p3, frame1, frame2); - - x = ((p2[0]-p3[0])*(p2[0]-p3[0]) + (p2[1]-p3[1])*(p2[1]-p3[1]) + - (p2[2]-p3[2])*(p2[2]-p3[2])); - - ans = sqrt(x); - - return ans; -} - - -/* GET_PATH_BETWEEN_FRAMES: This routine returns the path between two body - * segment reference frames. The path consists of the list of body segments - * in between the two specified frames. - */ -int* get_path_between_frames(ModelStruct* ms, int frame1, int frame2) -{ - return (ms->pathptrs[ms->numsegments*frame1+frame2]); -} - - - -/* FIND_JOINT_BETWEEN_FRAMES: This routine finds the joint which spans the - * two specified body segment reference frames. It also indicates whether - * the joint is traversed forwards or backwards to get from the from_frame - * to the to_frame. If the reference frames are not adjacent, it returns NULL. - */ -int find_joint_between_frames(ModelStruct* ms, int from_frame, int to_frame, Direction* dir) -{ - int i; - - for (i=0; inumjoints; i++) - { - if (ms->joint[i].from == from_frame && ms->joint[i].to == to_frame) - { - *dir = FORWARD; - return i; - } - - if (ms->joint[i].from == to_frame && ms->joint[i].to == from_frame) - { - *dir = INVERSE; - return i; - } - } - - return ZERO; -} - -#if ! CORTEX_PLUGIN && ! OPENSMAC && ! SIMMTOOPENSIM - -#endif - -/* EVALUATE_DOF: This routine calculates the current value of a dof. It stores - * the value inside the dof structure, and also returns the value. If the dof - * is a constant, then the value is already stored in var->value. - */ -double evaluate_dof(ModelStruct* ms, DofStruct* var) -{ - if (var->type == constant_dof) - return var->value; - else - { - var->value = interpolate_function(var->gencoord->value, var->function, zeroth, 1.0, 1.0); - return var->value; - } -} - -#if ! ENGINE - -void deiconify_message_window () -{ -#if ! NO_GUI - int i, savedID = glutGetWindow(); - - for (i = 0; i < root.numwindows; i++) - if (root.window[i].win_parameters->id == root.messages.window_id) - break; - - if (i == root.numwindows) - return; - - glutSetWindow(root.messages.window_id); - - if (i < root.numwindows && root.window[i].state == WINDOW_ICONIC) - glutShowWindow(); - else - glutPopWindow(); - - glutSetWindow(savedID); -#endif -} - -#endif /* ENGINE */ - -/* ERROR: this routine prints an error message depending on a string - * and error status that are passed in. - */ -void error(ErrorAction action, char str_buffer[]) -{ - -#if ! ENGINE && ! CORTEX_PLUGIN - deiconify_message_window(); -#endif - - if (str_buffer != NULL) - message(str_buffer,HIGHLIGHT_TEXT,DEFAULT_MESSAGE_X_OFFSET); - - if (action == recover) - message("Attempting to recover.",0,DEFAULT_MESSAGE_X_OFFSET); - else if (action == abort_action) - message("Action cancelled.",0,DEFAULT_MESSAGE_X_OFFSET); - else if (action == exit_program) - { - putchar('\a'); /* was ringbell() */ - fflush(stdout); - - message("Program terminated.",HIGHLIGHT_TEXT,DEFAULT_MESSAGE_X_OFFSET); - -#if ! NO_GUI - fprintf(stderr,"Fatal Error.\nProgram terminated."); -#endif - - exit(0); - } -} - - -int find_next_active_field(Form* form, int current_field, TextFieldAction tfa) -{ - int field, increment; - - if (tfa == goto_next_field) - increment = 1; - else if (tfa == goto_previous_field) - increment = form->numoptions - 1; - else - return current_field; - - field = (current_field+increment) % form->numoptions; - - while (field != current_field) - { - if ((form->option[field].active == yes) && (form->option[field].editable == yes)) - break; - field = (field+increment) % form->numoptions; - } - - return field; -} - - -/* SET_GENCOORD_VALUE: this is an important little routine. It should be the - * ONLY way that the value of a generalized coordinate is changed. - * It sets the value, and then marks some conversion matrices invalid - * so that they will not be used again without being recalculated. - * The matrices which use the dof in question, as stored in the jointnum[] - * array for that dof, are marked. - */ -#if OPENSMAC -int set_gencoord_value(ModelStruct* ms, GeneralizedCoord* gencoord, double value, SBoolean solveLoopsAndConstraints) -{ - int i; - SBoolean solveLoops, solveConstraints, sol; - - /* check whether the gencoord value has changed. If not, don't bother - * updating anything. */ - if ((DABS(value - gencoord->value) <= gencoord->tolerance)) - return 0; - - if (gencoord->type == rotation_gencoord) - checkGencoordRange(gencoord, &value); - - if (gencoord->clamped == yes) - { - if (value < gencoord->range.start) - value = gencoord->range.start; - else if (value > gencoord->range.end) - value = gencoord->range.end; - } - - /* Resolve any closed loops in the model, then update the gencoord value - * (which may have been changed to close the loops). If any other - * gencoord values are changed to close loops, resolveClosedLoops takes - * care of changing their values. If the solver could not find a valid - * solution, the gencoord is not updated and the configuration does not - * change. - */ - if (solveLoopsAndConstraints == yes) - { - sol = solveLCAffectedByGC(ms, gencoord, &value); - ms->constraintsOK = sol;// && ms->constraintsOK; - ms->loopsOK = sol;// && ms->loopsOK; - - gencoord->value = value; - if (sol == no) - return 0; - } - else - { - /* loops and constraints are not solved, copy new value into gencoord */ - gencoord->value = value; - } - - for (i=0; inumjoints; i++) - invalidate_joint_matrix(ms, &ms->joint[gencoord->jointnum[i]]); - - /* if the gencoord being changed is a translational dof, then we need to - * invalidate the current bounds of the scene to prevent the model from - * sliding behind the far clipping plane. -- added KMS 10/7/99 - */ - if (gencoord->type == translation_gencoord) - ms->max_diagonal_needs_recalc = yes; - - return 1; -} -#else -int set_gencoord_value(ModelStruct* ms, GeneralizedCoord* gencoord, double value, SBoolean solveLoopsAndConstraints) -{ - int i; - SBoolean sol; - int genc = getGencoordIndex(ms, gencoord); - -#if ! OPENSMAC - /* check whether the gencoord value has changed. If not, don't bother - * updating anything. Also check the value in the model viewer window - * to see whether that needs updating */ - if ((DABS(value - gencoord->value) <= gencoord->tolerance) -#if ! CORTEX_PLUGIN - && (DABS(value - ms->gencslider.sl[genc].value) <= gencoord->tolerance) -#endif - ) - { - return 0; - } -#endif - -#if ! ENGINE && ! CORTEX_PLUGIN - if (gencoord->type == rotation_gencoord) - checkGencoordRange(gencoord, &value); - - ms->gencform.option[genc].use_alternate_colors = no; - ms->gc_chpanel.checkbox[genc].use_alternate_colors = no; -#endif - - if (gencoord->clamped == yes) -// if ((gencoord->clamped == yes) && (solveLoopsAndConstraints == yes)) //added dkb apr 16 2003 - { - // if the value in the motion file for a clamped gencoord is outside the gencoord range, - // set the value to the closest range point - // DKB TODO: set some kind of flag, colour etc? to let user know what is happening - if (value < gencoord->range.start) - { -#if ! CORTEX_PLUGIN - //ms->gencform.option[genc].use_alternate_colors = yes; ///dkb jul 2008 - ms->gc_chpanel.checkbox[genc].use_alternate_colors = yes; ///dkb jul 2008 -#endif - value = gencoord->range.start; - } - else if (value > gencoord->range.end) - { -#if ! CORTEX_PLUGIN - // ms->gencform.option[genc].use_alternate_colors = yes; ///dkb jul 2008 - ms->gc_chpanel.checkbox[genc].use_alternate_colors = yes; -#endif - value = gencoord->range.end; - } - } - else - { -#if ! ENGINE && ! CORTEX_PLUGIN - if (value < gencoord->range.start || value > gencoord->range.end) - ms->gencform.option[genc].use_alternate_colors = yes; -#endif - } - -#if ! ENGINE - /* Resolve any closed loops in the model, then update the gencoord value - * (which may have been changed to close the loops). If any other - * gencoord values are changed to close loops, resolveClosedLoops takes - * care of changing their values. If the solver could not find a valid - * solution, the gencoord is not updated and the configuration does not - * change. - */ - - if (solveLoopsAndConstraints == yes) - { - sol = solveLCAffectedByGC(ms, gencoord, &value); - ms->constraintsOK = sol;// && ms->constraintsOK; - ms->loopsOK = sol;// && ms->loopsOK; - - gencoord->value = value; - if (sol == no) - return 0; - } - else - { - /* loops and constraints are not solved, copy new value into gencoord */ - gencoord->value = value; - } - -#if ! CORTEX_PLUGIN - if (value < gencoord->range.start || value > gencoord->range.end) - ms->gencform.option[genc].use_alternate_colors = yes; - - ms->gencslider.sl[genc].value = value; - - storeDoubleInForm(&ms->gencform.option[genc], gencoord->value, 3); -#endif - - for (i=0; inumjoints; i++) - invalidate_joint_matrix(ms, &ms->joint[gencoord->jointnum[i]]); - - /* hack so that ground-reaction forces are shown only during a motion */ - ms->dis.applied_motion = NULL; - - /* if the gencoord being changed is a translational dof, then we need to - * invalidate the current bounds of the scene to prevent the model from - * sliding behind the far clipping plane. -- added KMS 10/7/99 - */ - if (gencoord->type == translation_gencoord) - ms->max_diagonal_needs_recalc = yes; - -#endif - - return 1; -} -#endif /* OPENSMAC */ - - -void set_gencoord_velocity(ModelStruct* ms, GeneralizedCoord* gencoord, double value) -{ - gencoord->velocity = value; -} - - -char* get_suffix(char str[]) -{ - int cp = 0; - - cp = strlen(str) - 1; - - while (cp >= 0 && str[cp] != '.') - cp--; - - if (cp == 0) - return NULL; - - return &str[cp+1]; -} - - -/* CHANGE_FILENAME_SUFFIX: this routine changes the suffix of a file name. - * It scans the name for a "." (starting from the end) and assumes that - * everything after the "." is the suffix which is to be changed. Examples: - * input = "foo.bar", suffix = "tree" --------> output = "foo.tree" - * input = "foo.foo", suffix = "bar" ---------> output = "foo.bar" - * input = "foo", suffix = "bar" -------------> output = "foo.bar" - * input = "foo.bar.tree", suffix = "rock" ---> output = "foo.bar.rock" - */ -void change_filename_suffix(const char input[], char output[], const char suffix[], int outputSize) -{ - int cp; - - cp = strlen(input) - 1; - - while (input[cp] != '.' && cp > 0) - cp--; - - if (cp == 0) - { - if (suffix) - sprintf(output, "%s.%s", input, suffix); - else - strcpy(output, input); - } - else - { - if (suffix) - { - strncpy(output, input, cp + 1); - output[cp + 1] = STRING_TERMINATOR; - strcat(output, suffix); - } - else - { - strncpy(output, input, cp); - output[cp] = STRING_TERMINATOR; - } - } -} - -#if ENGINE || CORTEX_PLUGIN -#define NO_GUI 1 -#endif - -void message(char message_str[], int format, int xoffset) -{ -#if OPENSMAC - static int overwritable = 0; - if (overwritable || (format & OVERWRITE_LAST_LINE)) - { - add_line_to_converter_dialog(message_str, 1); - } - else - { - add_line_to_converter_dialog(message_str, 0); - } - if (format & OVERWRITABLE) - overwritable = 1; - else - overwritable = 0; -#elif NO_GUI - printf("%s\n", message_str); - fflush(stdout); - -#else - - int i, nl, winid; - HelpStruct* hp; - -#if 0 - printf("%s\n", message_str); - fflush(stdout); -#endif - - /* If there are no lines malloced in the message structure, just print - * the message to stdout and return. - */ - - if (root.messages.line == NULL) - { - printf("%s\n", message_str); - return; - } - - winid = glutGetWindow(); - - hp = &root.messages; - - /* If the last line in the list is overwritable, or if the incoming - * line is OVERWRITE_LAST_LINE, overwrite the last line with the - * incoming one. - */ - if (hp->num_lines > 0 && - ((hp->line[hp->num_lines - 1].format & OVERWRITABLE) || (format & OVERWRITE_LAST_LINE))) - { - hp->num_lines--; - FREE_IFNOTNULL(hp->line[hp->num_lines].text); - } - - if (hp->num_lines < hp->num_lines_malloced) - { - nl = hp->num_lines; - mstrcpy(&hp->line[nl].text,message_str); - hp->line[nl].format = format; - hp->line[nl].xoffset = xoffset; - hp->num_lines++; - if (hp->num_lines <= hp->lines_per_page) - hp->sl.thumb_thickness = -1; - else - hp->sl.thumb_thickness = hp->lines_per_page* - (hp->sl.shaft.y2-hp->sl.shaft.y1)/hp->num_lines; - } - else - { - FREE_IFNOTNULL(hp->line[0].text); - for (i=0; inum_lines_malloced; i++) - { - hp->line[i].text = hp->line[i+1].text; - hp->line[i].format = hp->line[i+1].format; - hp->line[i].xoffset = hp->line[i+1].xoffset; - } - nl = hp->num_lines_malloced - 1; - mstrcpy(&hp->line[nl].text,message_str); - hp->line[nl].format = format; - hp->line[nl].xoffset = xoffset; - } - - hp->sl.max_value = hp->num_lines*20.0; - hp->sl.value = hp->sl.min_value; - hp->starting_line = HELP_WINDOW_TEXT_Y_SPACING * _MAX(0,hp->num_lines-hp->lines_per_page); - - draw_message_window(NULL,NULL); - - glutSetWindow(winid); - -#endif /* ! NO_GUI */ -} - - - -/* ------------------------------------------------------------------------- - simm_printf - this routine provides printf-style output to the simm - message window. The 'hilite_text' parameter specifies whether the - text is displayed normal or hilited. - - NOTE: this routine will buffer text until an end-of-line character is - detected. This allows you to build a single line message via multiple - calls to this routine. However, this means that you MUST TERMINATE - EACH MESSAGE LINE WITH A '\n' CHARACTER FOR THE LINE TO BE SENT TO THE - MESSAGE WINDOW. ----------------------------------------------------------------------------- */ -public int simm_printf (SBoolean hilite_text, const char* format, ...) -{ - static char sMessageBuf[CHARBUFFER]; - - va_list ap; - int n, simmMsgFormat = 0; - - va_start(ap, format); - n = vsprintf(msg, format, ap); - va_end(ap); - -#if ! ENGINE && ! CORTEX_PLUGIN - if (hilite_text) - { - simmMsgFormat += HIGHLIGHT_TEXT; - - deiconify_message_window(); - } -#endif - - if (strchr(msg, '\n')) - { - char* p = strtok(msg, "\n"); - - if (strlen(sMessageBuf) > 0) - { - if (p) - strcat(sMessageBuf, p); - - message(sMessageBuf, simmMsgFormat, DEFAULT_MESSAGE_X_OFFSET); - - sMessageBuf[0] = '\0'; - } - else if (p) - message(p, simmMsgFormat, DEFAULT_MESSAGE_X_OFFSET); - - if (p) - for (p = strtok(NULL, "\n"); p; p = strtok(NULL, "\n")) - message(p, simmMsgFormat, DEFAULT_MESSAGE_X_OFFSET); - } - else - strcat(sMessageBuf, msg); - - return n; -} - - -SBoolean gencoord_in_path(ModelStruct* ms, int frame1, int frame2, GeneralizedCoord* gencoord) -{ - int i, j, joint; - int* path; - - path = GET_PATH(ms, frame1, frame2); - - for (i=0; path[i] != ms->numjoints + 1; i++) - { - joint = ABS(path[i]) - 1; - for (j=0; j<6; j++) - if (ms->joint[joint].dofs[j].type == function_dof) - if (ms->joint[joint].dofs[j].gencoord == gencoord) - return yes; - } - - return no; -} - - -void print_4x4matrix(double matrix[][4]) -{ - int i, j; - - for (i=0; i<4; i++) - { - for (j=0; j<4; j++) - printf("%8.5lf ", matrix[i][j]); - printf("\n"); - } -} - - -#if ! ENGINE -ToolStruct* register_tool(int struct_size, unsigned int event_mask, - void (*event_handler)(SimmEvent), - void (*command_handler)(char*), - SBoolean (*query_handler)(QueryType, void*), - char name[], int* ref_number) -{ - - int i; - - for (i=0; ivp.x1, win_params->vp.y2-TITLE_AREA_HEIGHT, - win_params->vp.x2+1, win_params->vp.y2+1); - - simm_color(TOOL_TITLE_AREA_BORDER); - glBegin(GL_LINE_STRIP); - glVertex2i(win_params->vp.x1-1, win_params->vp.y2-TITLE_AREA_HEIGHT); - glVertex2i(win_params->vp.x2+1, win_params->vp.y2-TITLE_AREA_HEIGHT); - glEnd(); - - simm_color(TOOL_TITLE_AREA_TEXT); - - glueSetFont(root.gfont.largefont); - - if (title_mask & SHOW_MODEL) - { - glRasterPos2i(win_params->vp.x1+15, win_params->vp.y2-23); - glueDrawString("Model: "); - if (ms == NULL) - glueDrawString("none"); - else - glueDrawString(ms->name); - } - - if (title_mask & SHOW_PLOT) - { - glRasterPos2i((win_params->vp.x1+win_params->vp.x2)/2-35, win_params->vp.y2-23); - glueDrawString("Plot: "); - if (ps == NULL) - glueDrawString("none"); - else - glueDrawString(ps->title); - } - - if (title_mask & SHOW_MODEL) - { - root.model_selector.origin.x = win_params->vp.x1+15; - root.model_selector.origin.y = win_params->vp.y2-TITLE_AREA_HEIGHT+5; - draw_menu(&root.model_selector); - } - - if (title_mask & SHOW_PLOT) - { - root.plot_selector.origin.x = (win_params->vp.x1+win_params->vp.x2)/2 - 35; - root.plot_selector.origin.y = win_params->vp.y2-TITLE_AREA_HEIGHT+5; - draw_menu(&root.plot_selector); - } - - if (title_mask & SHOW_HELP) - { - root.help_selector.origin.x = win_params->vp.x2-85; - root.help_selector.origin.y = win_params->vp.y2-TITLE_AREA_HEIGHT+5; - draw_menu(&root.help_selector); - } -} - - -typedef struct { - TitleAreaCBParams params; - ModelStruct* model; - PlotStruct* plot; - int entry; - title_area_cb titleAreaCB; -} TitleAreaMenuCBData; - -static void model_menu_cb(int item, void* userData) -{ - TitleAreaMenuCBData* data = (TitleAreaMenuCBData*) userData; - - int i, numstructs = 0; - - if (data->model && data->entry > 0) - glueCheckMenuItem(root.modelmenu,data->entry,GLUE_UNCHECK); - - //highlight_menu_item(&root.model_selector,0,off,yes); - - if (item <= 0 || root.nummodels == 0) - return; - - for (i=0; iparams.struct_ptr = (void*)(gModel[i]); - - if (data->titleAreaCB) - data->titleAreaCB(MODEL_SELECTED, &data->params); - - free(data); - -} - -static void plot_menu_cb(int item, void* userData) -{ - TitleAreaMenuCBData* data = (TitleAreaMenuCBData*) userData; - int i, numstructs = 0; - - if (data->plot != NULL && data->entry > 0) - glueCheckMenuItem(root.plotmenu,data->entry+1,GLUE_UNCHECK); - //highlight_menu_item(&root.plot_selector,0,off,yes); - - if (item <= 0) - return; - - if (item == 1) - { - data->params.struct_ptr = (void*) NULL; - } - else - { - item -= 1; - if (item <= 0 || root.numplots == 0) - return; - for (i=0; iparams.struct_ptr = (void*)(gPlot[i]); - } - - if (data->titleAreaCB) - data->titleAreaCB(PLOT_SELECTED, &data->params); - - free(data); - -} - -static TitleAreaMenuCBData* alloc_title_area_data(WindowParams* win_params, - void* struct_ptr, - int entry, - ModelStruct* ms, - PlotStruct* ps, - title_area_cb titleAreaCB) -{ - TitleAreaMenuCBData* data = NULL; - - if (titleAreaCB) - { - data = (TitleAreaMenuCBData*) simm_malloc(sizeof(TitleAreaMenuCBData)); - - data->params.win_params = win_params; - data->params.struct_ptr = struct_ptr; - data->entry = entry; - data->model = ms; - data->plot = ps; - data->titleAreaCB= titleAreaCB; - } - return data; -} - - -int check_title_area(int title_mask, int mx, int my, WindowParams* win_params, - void** struct_ptr, ModelStruct* ms, PlotStruct* ps, - title_area_cb titleAreaCB) -{ - - int entry = 0; - - root.model_selector.origin.x = win_params->vp.x1+15; - root.model_selector.origin.y = win_params->vp.y2-TITLE_AREA_HEIGHT+5; - root.plot_selector.origin.x = (win_params->vp.x1+win_params->vp.x2)/2 - 35; - root.plot_selector.origin.y = win_params->vp.y2-TITLE_AREA_HEIGHT+5; - root.help_selector.origin.x = win_params->vp.x2-85; - root.help_selector.origin.y = win_params->vp.y2-TITLE_AREA_HEIGHT+5; - - if ((title_mask & SHOW_MODEL) && (check_menu(&root.model_selector,mx,my) == 0)) - { - highlight_menu_item(&root.model_selector,0,on,yes); - if (ms != NULL) - { - if ((entry = find_model_ordinal(ms->modelnum)) > 0) - { - glueCheckMenuItem(root.modelmenu,entry,GLUE_CHECK); - } - } - gluePopMenu(root.modelmenu, model_menu_cb, - alloc_title_area_data(win_params, *struct_ptr, entry, ms, ps, - titleAreaCB)); - return MODEL_SELECTED; - } - - if ((title_mask & SHOW_PLOT) && (check_menu(&root.plot_selector,mx,my) == 0)) - { - highlight_menu_item(&root.plot_selector,0,on,yes); - if (ps != NULL) - { - if ((entry = find_plot_ordinal(ps->plotnum)) > 0) - { - glueCheckMenuItem(root.plotmenu,entry+1,GLUE_CHECK); - } - } - gluePopMenu(root.plotmenu, plot_menu_cb, - alloc_title_area_data(win_params, *struct_ptr, entry, ms, ps, - titleAreaCB)); - return PLOT_SELECTED; - } - - if ((title_mask & SHOW_HELP) && (check_menu(&root.help_selector,mx,my) == 0)) - { - if (titleAreaCB) - { - TitleAreaMenuCBData* data = alloc_title_area_data(win_params, - *struct_ptr, - entry, ms, ps, - titleAreaCB); - titleAreaCB(HELP_SELECTED, &data->params); - - free(data); - } - return HELP_SELECTED; - } - - return NULL_SELECTED; - -} - -#endif /* ! NO_GUI */ -#endif /* ENGINE */ - - -void make_time_string(char** time_string) -{ - time_t t = time(NULL); - - strftime(buffer, CHARBUFFER, "%m/%d/%Y %I:%M:%S %p", localtime(&t)); - - mstrcpy(time_string, buffer); -} - -#if ! SIMMTOOPENSIM && ! OPENSMAC - -/* CONVERT_STRING: this routine scans a string and converts all special - * characters into underscores. A special character is any character - * other than a letter or number. If prependUnderscore is 'yes,' this - * function will prepend an underscore character if the string starts with - * a number. It is assumed that the string already has space for this extra - * character. The resulting string is one token, and can therefore be used - * as a variable name in SIMM-written C code. - */ -void convert_string(char str[], SBoolean prependUnderscore) -{ - int i, len; - - len = strlen(str); - - for (i = 0; i < len; i++) - { - if (str[i] >= 97 && str[i] <= 122) /* lowercase letters */ - continue; - if (str[i] >= 65 && str[i] <= 90) /* uppercase letters */ - continue; - if (str[i] >= 48 && str[i] <= 57) /* numbers */ - continue; - str[i] = '_'; - } - - /* If the first character is a number, prepend an underscore. */ - if (str[0] >= 48 && str[0] <= 57) - { - for (i = len + 1; i > 0; i--) - str[i] = str[i-1]; - str[0] = '_'; - } -} - -#endif - -/* convertSpacesInString: this routine scans a string and converts all spaces - * into underscores. - */ -void convertSpacesInString(char str[]) -{ - - unsigned int i; - - for (i = 0; i < strlen(str); i++) - { - if (str[i] == 32) - str[i] = '_'; - } - -} - -#if ! ENGINE - -void simm_color(int index) -{ - glColor3fv(root.color.cmap[index].rgb); -} - - -void set_hourglass_cursor(double percent) -{ - int index, num_cursors = 11, new_cursor = 20; - GLshort junk = 0xfff; - - if (percent > 99.99) - { - index = -1; - } - else - { - index = percent*num_cursors/100.0; - if (index < 0) - index = 0; - if (index >= num_cursors) - index = num_cursors - 1; - } - - if (index == last_index) - return; - - if (index == -1) - glutSetCursor(GLUT_CURSOR_INHERIT); - else - glutSetCursor(GLUT_CURSOR_WAIT); - - last_index = index; -} - -#endif /* ENGINE */ - -/* ------------------------------------------------------------------------- - simm_fopen - CodeWarrior's fopen() appears to have a bug in which the "w" - and "w+" modes do not discard the previous contents of the file being - opened. Therefore you should always call this routine instead of fopen() - to workaround the bug. ----------------------------------------------------------------------------- */ -FILE* simm_fopen (const char* name, const char* mode) -{ -#ifdef WIN32 - if (mode && mode[0] == 'w') - remove(name); -#endif - - errno = 0; - - return fopen(name, mode); -} - -/* ------------------------------------------------------------------------- - simm_open - CodeWarrior's open() appears to have a bug in which the - O_CREAT mode does not discard the previous contents of the file being - opened. Therefore you should always call this routine instead of open() - to workaround the bug. ----------------------------------------------------------------------------- */ -int simm_open (const char *name, int oflag, ...) -{ -#ifdef WIN32 - if ((oflag & O_CREAT) && ((oflag & O_WRONLY) || (oflag & O_TRUNC))) - remove(name); -#endif - - errno = 0; - - return open(name, oflag); -} - -/* --------------------------------------------------------------------------- - simm_lookup_file - this routine looks for the specified file in a list - of directory paths starting with the first path and moving forward - through the list until the file is found or the list is exhausted. If - the file is found, it is opened using the specified 'mode', and the - path+name of the file is returned in the buffer pointed to by 'pathList'. - - NOTE: this routine uses strtok() to parse the specified 'pathList'. - strtok() will modify the pathList by inserting NUL characters. ------------------------------------------------------------------------------- */ -FILE* simm_lookup_file (char* pathList, const char* fileName, const char* mode) -{ -#define PATH_SEPERATORS ",;" - - char* p; - - if (pathList == NULL || fileName == NULL || mode == NULL) - return NULL; - - for (p = strtok(pathList, PATH_SEPERATORS); p; p = strtok(NULL, PATH_SEPERATORS)) - { - FILE* f; - - char buf[CHARBUFFER] = ""; - - strcpy(buf, p); - - if (buf[strlen(buf) - 1] != DIR_SEP_CHAR) - strcat(buf, DIR_SEP_STRING); - - strcat(buf, fileName); - - f = simm_fopen(buf, mode); - - if (f) - { - strcpy(pathList, buf); /* return path+file in 'pathList' */ - - return f; - } - } - return NULL; -} - -#if ! MEMORY_LEAK -void* simm_malloc(unsigned mem_size) -{ - void* ptr; - - // Temporary hack so that you don't have to check mem_size before calling - // simm_malloc()-- Make sure you don't try to malloc 0 bytes because malloc - // will [appropriately] return NULL. The long-term solution is to check all - // places where simm_malloc() is called and be smart enough not to call - // the routine if mem_size is 0. - - if (mem_size <= 0) - mem_size = sizeof(int); - - ptr = malloc(mem_size); - - if (ptr == NULL) - { - // error() may need to malloc, so don't call it. - //sprintf(errorbuffer,"Ran out of memory. Unable to malloc %d bytes.", - //(int)mem_size); - //error(none,errorbuffer); - } - - return ptr; -} - - -void* simm_calloc(unsigned num_elements, unsigned elem_size) -{ - void* ptr; - - // Temporary hack so that you don't have to check mem_size before calling - // simm_calloc()-- Make sure you don't try to calloc 0 bytes because calloc - // will [appropriately] return NULL. The long-term solution is to check all - // places where simm_calloc() is called and be smart enough not to call - // the routine if mem_size is 0. - - if (num_elements*elem_size <= 0) - { - num_elements = 1; - elem_size = sizeof(int); - } - - ptr = calloc(num_elements, elem_size); - - if (ptr == NULL) - { - sprintf(errorbuffer, "Ran out of memory. Unable to calloc %d bytes.", - (int)(num_elements*elem_size)); - error(none, errorbuffer); - } - - return ptr; -} - - -void* simm_realloc(void* ptr, unsigned mem_size, ReturnCode* rc) -{ - void* new_ptr; - - // Temporary hack so that you don't have to check mem_size before calling - // simm_realloc()-- Make sure you don't try to realloc 0 bytes because realloc - // will [appropriately] return NULL. The long-term solution is to check all - // places where simm_realloc() is called and be smart enough not to call - // the routine if mem_size is 0. - - if (mem_size <= 0) - mem_size = sizeof(int); - - new_ptr = realloc(ptr, mem_size); - - if (new_ptr == NULL) - { - sprintf(errorbuffer, "Ran out of memory. Unable to realloc %d bytes.", - (int)mem_size); - *rc = code_bad; - return (ptr); - } - - *rc = code_fine; - return new_ptr; -} -#endif - - -char* get_drawmode_name(DrawingMode drawmode) -{ - if (drawmode == wireframe) - return "wireframe"; - if (drawmode == solid_fill) - return "solid_fill"; - if (drawmode == flat_shading) - return "flat_shading"; - if (drawmode == gouraud_shading) - return "gouraud_shading"; - if (drawmode == outlined_polygons) - return "outlined_polygons"; - if (drawmode == no_surface) - return "none"; - if (drawmode == bounding_box) - return "bounding_box"; - - /* this should really be an error, but return gouraud_shading - * as if nothing is wrong. - */ - return "gouraud_shading"; -} - - -char* get_simmkey_name(int keynum) -{ - return keys[keynum]; -} - - -#if ! ENGINE - -/* SET_VIEWPORT: the MODELVIEW and PROJECTION matrices should - * always be reset when you change the viewport. That's why - * this utility routine exists. - */ -void set_viewport(int x1, int y1, int xsize, int ysize) -{ - glViewport(x1, y1, xsize, ysize); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); -} - - -/* SET_ORTHO2: this utility function sets a 2D ortho, - * with or without the infamous 0.5 offset. - * - * NOTE: I replaced the "-0.5 +0.5" approach with one I read about in - * a document titled "OpenGL Correctness Tips" (from Microsoft's MSDN - * online docs). This new approach involves a glTranslate by 0.375 - * in the x and y direction. -- KMS 11/19/98 - */ -#define NEW_ORTHO_APPROACH 1 - -void set_ortho2o(Ortho box) -{ -#if NEW_ORTHO_APPROACH - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - gluOrtho2D((int) box.x1, (int) box.x2, (int) box.y1, (int) box.y2); - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - glTranslatef(0.375, 0.375, 0.0); -#else - glOrtho(box.x1-0.5, box.x2+0.5, box.y1-0.5, box.y2+0.5, -1.0, 1.0); -#endif -} - -void set_ortho2i(GLint ortho[]) -{ - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - gluOrtho2D(ortho[0], ortho[2], ortho[1], ortho[3]); - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - glTranslatef(0.375, 0.375, 0.0); -} - -void set_ortho2(double x1, double x2, double y1, double y2) -{ -#if NEW_ORTHO_APPROACH - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - gluOrtho2D((int) x1, (int) x2, (int) y1, (int) y2); - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - glTranslatef(0.375, 0.375, 0.0); -#else - glOrtho(x1-0.5, x2+0.5, y1-0.5, y2+0.5, -1.0, 1.0); -#endif - -} - - -/* ------------------------------------------------------------------------- - get_associated_model - return the ModelStruct currently associated with - the specified window. ----------------------------------------------------------------------------- */ -ModelStruct* get_associated_model (int i) -{ - ModelStruct* ms = NULL; - - if (i < 0) - return NULL; - - if (root.window[i].type == SCENE) - { - // Assume that there is always at least one model in the scene - // and return the first one. - ms = root.window[i].win_struct->scene->model[0]; - } - else if (root.window[i].type == TOOL) - { - ToolStruct* tool = root.window[i].win_struct->tool; - - if (tool && tool->query_handler) - tool->query_handler(GET_TOOL_MODEL, &ms); - } - else // plot window or other - { - /* If a plot is the topmost window there is only one model, - * go ahead and return it as the 'selected' model. - */ - if (root.nummodels == 1) - { - int j; - - for (j=0; jplot; - } - else if (root.window[i].type == TOOL) - { - ToolStruct* tool = root.window[i].win_struct->tool; - - if (tool && tool->query_handler) - tool->query_handler(GET_TOOL_PLOT, &plot); - } - else if (root.window[i].type == PLOTKEY) - { - plot = root.window[i].win_struct->plotkey->plot; - } - return plot; -} - -#endif /* ENGINE */ - -/* ------------------------------------------------------------------------- - lookup_polyhedron - this routine tries to read the specified polyhedron - by first checking the current directory, and then checking the - standard SIMM directory for bones. - TODO5.0: If one of the paths to search starts with a drive letter that - does not exist, Vista will display an annoying error dialog (other versions - of Windows quietly move on). Find some way to check for a valid drive - letter before performing the action that causes the error. ----------------------------------------------------------------------------- */ -FileReturnCode lookup_polyhedron(PolyhedronStruct* ph, char filename[], ModelStruct* ms) -{ - int i; - char* jointpath = NULL; - char fullpath[CHARBUFFER], tmppath[CHARBUFFER]; - FileReturnCode rc; - - /* (0) strip the joint file name from ms->jointfilename to get just - * the path to the joint file. - */ - if (ms && ms->jointfilename) - { - get_pure_path_from_path(ms->jointfilename, &jointpath); - } - - /* (1) First check the bone folder specified in the joint file. - * If this is an absolute path, use it as is. If it is a relative - * path, it is assumed to be relative to the joint file's folder. - */ - if (ms && ms->bonepathname) - { - if (ms->jointfilename) - build_full_path(jointpath, ms->bonepathname, tmppath); - else - build_full_path(NULL, ms->bonepathname, tmppath); - build_full_path(tmppath, filename, fullpath); - rc = read_polyhedron(ph, fullpath, yes); - if (rc == file_good || rc == file_bad) - { - FREE_IFNOTNULL(jointpath); - return rc; - } - } - - /* (2) Next check the folder "bones" under the joint file's - * folder (PC only). - */ - if (ms->jointfilename) - strcat3(tmppath, jointpath, DIR_SEP_STRING, "bones", CHARBUFFER); - else - strcpy(tmppath, "bones"); - strcat3(fullpath, tmppath, DIR_SEP_STRING, filename, CHARBUFFER); - rc = read_polyhedron(ph, fullpath, yes); - if (rc == file_good || rc == file_bad) - { - FREE_IFNOTNULL(jointpath); - return rc; - } - - /* (3) Next check the joint file's folder itself (PC only). */ - if (ms->jointfilename) - strcpy(tmppath, jointpath); - else - strcpy(tmppath, "."); - FREE_IFNOTNULL(jointpath); - strcat3(fullpath, tmppath, DIR_SEP_STRING, filename, CHARBUFFER); - rc = read_polyhedron(ph, fullpath, yes); - if (rc == file_good || rc == file_bad) - return rc; - -#if ! ENGINE - /* (4) check the global bones folder. */ - build_full_path(get_bones_folder(), filename, fullpath); - rc = read_polyhedron(ph, fullpath, yes); - if (rc == file_good || rc == file_bad) - return rc; - - /* (5) check the mocap bones folder. */ - sprintf(fullpath, "%s%s%s%s", get_mocap_folder(), "bones", DIR_SEP_STRING, filename); - rc = read_polyhedron(ph, fullpath, yes); - if (rc == file_good || rc == file_bad) - return rc; -#endif - - /* You only make it to here if the file was not found in - * any of the folders. - */ - return file_missing; -} - - -/* ------------------------------------------------------------------------- - is_absolute_path - return yes if the specified path is an absolute path, - otherwise return no. ----------------------------------------------------------------------------- */ -public SBoolean is_absolute_path (const char* path) -{ - if (path == NULL) - return no; - - while (path && *path && isspace(*path)) - path++; - -#ifdef WIN32 - if (*path == '/' || - *path == DIR_SEP_CHAR || - strlen(path) >= 3 && path[1] == ':' && (path[2] == DIR_SEP_CHAR || path[2] == '/')) - { - return yes; - } - else - return no; -#else - return (SBoolean) (*path == DIR_SEP_CHAR); -#endif -} - -/* ------------------------------------------------------------------------- - append_if_necessary - if the specified character 'c' is not the currently - the last character in the specified string 'str', then append 'c' to 'str'. ----------------------------------------------------------------------------- */ -void append_if_necessary (char* str, char c) -{ - int n = strlen(str); - - if (n == 0) - return; - - if (str[n-1] != c) - { - str[n] = c; - str[n+1] = '\0'; - } -} - -/* ------------------------------------------------------------------------- - build_full_path - this routine combines a "preference path" with a - "file path" that may contain absolute or relative path specifications - into a full path. ----------------------------------------------------------------------------- */ -void build_full_path(const char* prefPath, const char* filePath, char* fullPath) -{ - if (fullPath == NULL) - return; - - // Ignore any leading spaces in 'prefPath' and 'filePath'. - while (prefPath && *prefPath && isspace(*prefPath)) - prefPath++; - - while (filePath && *filePath && isspace(*filePath)) - filePath++; - - // Copy the preference path if necessary. - if (prefPath && ! is_absolute_path(filePath)) - { - strcpy(fullPath, prefPath); - strip_trailing_white_space(fullPath); - append_if_necessary(fullPath, DIR_SEP_CHAR); - } - else - *fullPath = '\0'; - - // Now append the file path. - if (filePath) - { - strcat(fullPath, filePath); - strip_trailing_white_space(fullPath); - } -} - -/* ------------------------------------------------------------------------- - get_filename_from_path - ----------------------------------------------------------------------------- */ -const char* get_filename_from_path (const char* pathname) -{ - char *p = NULL, *p1 = NULL, *p2 = NULL; - - p1 = strrchr(pathname, DIR_SEP_CHAR); - p2 = strrchr(pathname, '/'); - - p = _MAX(p1,p2); - - return p ? (p+1) : pathname; -} - -/* ------------------------------------------------------------------------- - get_pure_path_from_path - this function assumes that there is a filename - on the end of the path! It has to because there is no way to distinguish - between a file and a folder at the end of the path. ----------------------------------------------------------------------------- */ -void get_pure_path_from_path (const char* fullPath, char** purePath) -{ - int len; - char *p = NULL, *p1 = NULL, *p2 = NULL; - - p1 = strrchr(fullPath, DIR_SEP_CHAR); - p2 = strrchr(fullPath, '/'); - p = _MAX(p1,p2); - - if (p) - { - len = p - fullPath; - *purePath = (char*)simm_malloc((len+1) * sizeof(char)); - strncpy(*purePath, fullPath, len); - (*purePath)[len] = STRING_TERMINATOR; - } - else // fullPath is just a filename, so purePath is empty. - { - mstrcpy(purePath, "."); - } -} - -/* ------------------------------------------------------------------------- - upperstr - ----------------------------------------------------------------------------- */ -public void upperstr (char* s) -{ - for ( ; *s; s++) - *s = toupper(*s); -} - -/* ------------------------------------------------------------------------- - lowerstr - ----------------------------------------------------------------------------- */ -public void lowerstr (char* s) -{ - for ( ; *s; s++) - *s = tolower(*s); -} - -/* --------------------------------------------------------------------------- - read_double - I've found that some implementations of scanf("%lf") do - not read "NaN" correctly. This routine reads any floating-point number - including "NaN". -- KMS 4/21/00 - - NOTE: this routine was originally static to analog.c. In moving it here - I had to replace the call to _read_token() with fscanf("%s"). I don't - think this will have any side-effects. ------------------------------------------------------------------------------- */ -public ReturnCode read_double (FILE* f, double* value) -{ - SBoolean eof = (SBoolean) (fscanf(f, "%s", buffer) != 1); - - if (eof) - return code_bad; - - lowerstr(buffer); - - if (STRINGS_ARE_EQUAL(buffer, "nan")) - { - *value = 0.0; /* ACK! can't seem to find a way to assign NAN in VC++!! */ - } - else if (isdigit(buffer[0]) || buffer[0] == '.' || buffer[0] == '-' || buffer[0] == '+') - *value = atof(buffer); - else - return code_bad; - - return code_fine; -} - - -/* --------------------------------------------------------------------------- - read_double_tab - I've found that some implementations of scanf("%lf") do - not read "NaN" correctly. This routine reads any floating-point number - including "NaN". -- KMS 4/21/00 - - This function is like read_double(), but is designed for reading tab-delimited - numbers (e.g., from XLS files). In these files, two tabs in a row means that - a number field is empty-- detect this, read just the first tab from the file, - and return 0.0 for the number. ------------------------------------------------------------------------------- */ -public ReturnCode read_double_tab(FILE* f, double* value) -{ - SBoolean eof; - char c; - long position = ftell(f); - - /* Two tabs in a row is the only allowable way to specify an empty number field, - * so read two characters from the file and see if they are both tabs. - */ - c = fgetc(f); - if (c == '\t') - { - position = ftell(f); - c = fgetc(f); - if (c == '\t') - { - /* Put the second tab back and return 0.0 for the empty field. */ - fseek(f, position, SEEK_SET); - *value = 0.0; - return code_fine; - } - } - - /* Go back to the saved position to read the number. */ - fseek(f, position, SEEK_SET); - - eof = (SBoolean) (fscanf(f, "%s", buffer) != 1); - - if (eof) - return code_bad; - - lowerstr(buffer); - - if (STRINGS_ARE_EQUAL(buffer, "nan")) - { - *value = 0.0; /* ACK! can't seem to find a way to assign NAN in VC++!! */ - } - else if (isdigit(buffer[0]) || buffer[0] == '.' || buffer[0] == '-' || buffer[0] == '+') - *value = atof(buffer); - else - return code_bad; - - return code_fine; -} - - -int strings_equal_case_insensitive(const char str1[], const char str2[]) -{ - char buf1[1024]; - - /* make the strings upper case and compare them */ - strcpy(buffer, str1); - _strupr(buffer); - - strcpy(buf1, str2); - _strupr(buf1); - - return !strcmp(buffer, buf1); -} - -int strings_equal_n_case_insensitive(const char str1[], const char str2[], int n) -{ - char buf1[1024]; - - if ((int)strlen(str1) < n || (int)strlen(str2) < n) - return 0; - - /* make the strings upper case and compare them */ - strncpy(buffer, str1, n); - buffer[n] = STRING_TERMINATOR; - _strupr(buffer); - - strncpy(buf1, str2, n); - buf1[n] = STRING_TERMINATOR; - _strupr(buf1); - - return !strcmp(buffer, buf1); -} - - -void addNameToString(char name[], char string[], int maxStringSize) -{ - int newLen = strlen(name) + 2; // size of name + ", " - int curLen = strlen(string); - - /* Add the name to the string as long as there is room - * for it plus ", " plus "..." - */ - if (curLen + newLen + 5 < maxStringSize) - { - /* if curLen > 1, assume there's already a name in the string */ - if (curLen > 1) - strcat(string, ", "); - strcat(string, name); - } - else if (curLen + 5 < maxStringSize) - { - /* if there is room for "..." and the string doesn't already end with "...", add it. */ - if (strcmp(&string[curLen-3], "...")) - strcat(string, ", ..."); - } -} - - -void simmPrintMultiLines(char string[], SBoolean hilite, int lineSize, int pixelIndent) -{ - int simmMsgFormat = 0; - int len, start = 0, end = 0; - - if (!string || lineSize <= 0 || pixelIndent < 0) - return; - -#if ! ENGINE - if (hilite) - { - simmMsgFormat += HIGHLIGHT_TEXT; - deiconify_message_window(); - } -#endif - - len = strlen(string); - - while (end < len) - { - end = start + lineSize - 1; // -1 because you include the char at 'end' - if (end >= len) - { - end = len; - } - else - { - for (; end > start; end--) - { - if (CHAR_IS_WHITE_SPACE(string[end])) - break; - } - /* If end == start, there is no white space in the line, - * so set end back to what it was and split a word. - */ - if (end == start) - end = start + lineSize - 1; - } - strncpy(buffer, &string[start], end - start + 1); - buffer[end - start + 1] = STRING_TERMINATOR; - message(buffer, simmMsgFormat, pixelIndent); - start = end + 1; - } -} - -#if ! OPENSIM_BUILD && ! CORTEX_PLUGIN - -const char* get_preference(const char name[]) -{ - int i; - - for (i=0; i 1 && value[0] == '\"' && value[len-1] == '\"') - { - memmove(value, &value[1], len-2); - value[len-2] = STRING_TERMINATOR; - } - - return yes; -} - -void save_preferences_file(SBoolean verbose) -{ - int i; - FILE* fp; - char filename[CHARBUFFER], mess[CHARBUFFER]; - - strcpy(filename, get_preference("RESOURCES_FOLDER")); - append_if_necessary(filename, DIR_SEP_CHAR); - strcat(filename, "preferences.txt"); - - fp = simm_fopen(filename, "w"); - - if (fp == NULL) - { - if (verbose == yes) - { - (void)sprintf(mess, "Unable to save preferences file: %s", filename); - error(none, mess); - } - } - else - { - for (i=0; imodelLock) - glutAcquireMutex(gModel[i]->modelLock); - } - } - else - { - glutAcquireMutex(ms->modelLock); - } -} - - -/* ------------------------------------------------------------------------- - unlock_model - release the realtime mutex, but only if the model is currently - receiving realtime motion data from EVaRT or from its simulation dll. ----------------------------------------------------------------------------- */ -void unlock_model(ModelStruct* ms) -{ - if (is_model_realtime(ms) == rtNotConnected) - return; - - /* If model is NULL, release the locks for all models. */ - if (ms == NULL) - { - int i; - - for (i = 0; i < MODELBUFFER; i++) - { - if (gModel[i] && gModel[i]->modelLock) - glutReleaseMutex(gModel[i]->modelLock); - } - } - else - { - glutReleaseMutex(ms->modelLock); - } -} - -PlotStruct* get_plot_by_name(const char name[]) -{ - int i; - - for (i=0; ititle, name)) - return gPlot[i]; - - return NULL; -} - -#endif /* ! ENGINE */ - -/* ------------------------------------------------------------------------- - is_model_realtime - returns the current state of the model's realtime - connection: - rtMocap: connected to EVaRT - rtSimulation: connected to a simulation dll - rtNotConnected: not connected to either - If 'ms' is NULL this function scans all of the models and returns the - first realtime state that is not rtNotConnected. ----------------------------------------------------------------------------- */ -RTConnection is_model_realtime(ModelStruct* ms) -{ - if (ms == NULL) - { - int i; - - for (i = 0; i < MODELBUFFER; i++) - { - if (gModel[i] && gModel[i]->realtimeState != rtNotConnected) - return gModel[i]->realtimeState; - } - return rtNotConnected; - } - else - { - return ms->realtimeState; - } -} - -int getMusclePointSegment(dpMuscleStruct *muscle, int pointIndex) -{ - if (muscle == NULL || muscle->path == NULL) - return -1; - if (pointIndex < 0 || pointIndex >= muscle->path->num_orig_points) - return -1; - if (muscle->path->mp_orig == NULL) - return -1; - return muscle->path->mp_orig[pointIndex].segment; -} - -void setMusclePointSegment(dpMuscleStruct *muscle, int pointIndex, int newSeg) -{ - if (muscle == NULL || muscle->path == NULL) - return; - if (pointIndex < 0 || pointIndex >= muscle->path->num_orig_points) - return; - if (muscle->path->mp_orig == NULL) - return; - muscle->path->mp_orig[pointIndex].segment = newSeg; -} - -int makeDir(const char aDirName[]) -{ -#ifdef __linux__ - return mkdir(aDirName,S_IRWXU); -#else - return _mkdir(aDirName); -#endif -} - -int find_string_in_list(const char name[], const char* string_list[], int n) -{ - int i; - - for (i=0; inumgencoords; i++) - if (model->gencoord[i] == gencoord) - return i; - - return -1; -} - - -int getJointIndex(ModelStruct* model, JointStruct* joint) -{ - int i; - - for (i=0; inumjoints; i++) - if (&model->joint[i] == joint) - return i; - - return -1; -} - - -int getLigamentIndex(ModelStruct* model, char lig_name[]) -{ - int i; - - for (i=0; inumligaments; i++) - if (STRINGS_ARE_EQUAL(lig_name, model->ligament[i].name)) - return i; - - return -1; -} diff --git a/OpenSim/Utilities/simmToOpenSim/universal.h b/OpenSim/Utilities/simmToOpenSim/universal.h deleted file mode 100644 index f62f8eec58..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/universal.h +++ /dev/null @@ -1,91 +0,0 @@ -/******************************************************************************* - - UNIVERSAL.H - - Author: Peter Loan - - Date: 16-JUL-90 - - Copyright (c) 1992, 1993 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - This file is included into EVERY ".c" file, and thus should only - contain "#includes" that every file needs. - -*******************************************************************************/ - -#ifndef UNIVERSAL_H -#define UNIVERSAL_H - -#if MEMORY_LEAK - #define _CRTDBG_MAP_ALLOC - #include - #include - #define simm_malloc malloc - #define simm_calloc calloc - #define simm_realloc(ptr, size, rc) realloc(ptr, size) - #define mstrcpy(dest, src) {int len=strlen(src)+1; *(dest) = (char*)malloc(len); strcpy(*(dest), src);} -#else - #include -#endif - -#include -#include -#include -#include -#include -#include -#include -#include - -#if defined(_MSC_VER) - #define MS_VISUAL_C 1 -#endif - -#define POLYGON_DISPLAY_HACK 1 -#define STEADYCAM 1 - -#ifndef M_PI -#define M_PI 3.1415926535897932384626433832795 -#define M_PI_2 (M_PI / 2.0) -#endif - -#ifdef SIMM_RENDERMATIC -#define _3D_MATRIX_ -#include "3d.h" -#endif - -#if OPENSIM_BUILD -typedef float GLfloat; -typedef unsigned int GLuint; -typedef int GLint; -typedef double GLdouble; -typedef unsigned int GLenum; -typedef unsigned char GLubyte; -#else -#include /* GLUT includes GL/gl.h and GL/glu.h for us */ -#include -#endif - -#include "sm.h" -#include "dp.h" -#include "dp420.h" - -#include "macros.h" -#include "basic.h" -#include "events.h" -#include "color.h" -#include "simmkeys.h" - -#include "modelplot.h" -#include "windowroot.h" -#include "mathtools.h" -#if ! OPENSIM_BUILD -#include "glutglue.h" -#endif -#include "mocap.h" - -#endif /*UNIVERSAL_H*/ - - diff --git a/OpenSim/Utilities/simmToOpenSim/wefunctions.h b/OpenSim/Utilities/simmToOpenSim/wefunctions.h deleted file mode 100644 index d92165b496..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/wefunctions.h +++ /dev/null @@ -1,113 +0,0 @@ -/******************************************************************************* - - WEFUNCTIONS.H - this header declares the Wrap Editor's public functions - as well as the 3 wrapping algorithm public functions: - - calc_line_intersect_ellipsoid - calc_line_intersect_cylinder - calc_line_intersect_sphere - - Author: Kenny Smith (based on pmfunctions.h by Peter Loan) - - Date: 22-OCT-98 - - Copyright (c) 1998 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef WEFUNCTIONS_H -#define WEFUNCTIONS_H - -void makewrapeditormenus(void); -void we_entervalue(SimmEvent se); -void update_we_forms(void); -void move_we_help_text(int dummy_int, double slider_value, double delta); - -void we_track_cb(void* data, SimmEvent se); - -void inval_model_wrapping(ModelStruct* model, dpWrapObject* wrap_object); - -int get_wrap_object_index(ModelStruct* model, dpWrapObject* wrap_object); -int is_current_wrap_object(ModelStruct*, dpWrapObject*); -void select_wrapping_object(int wrapobj, SBoolean redisplay); - -void convert_to_wrap_object_frame(dpWrapObject*, double* pt); -void convert_from_wrap_object_frame(dpWrapObject*, double* pt); - -void reset_wrapobj_xform(); -void recalc_xforms(dpWrapObject*); - -void apply_xform_to_wrapobj(double factor); -void clear_we_xform_form(); - -void save_all_wrap_objects(ModelStruct* ms); - -SBoolean query_muscle_wrap_association(dpMuscleStruct* muscle, dpWrapObject* wrap_object); -void add_muscle_wrap_association(dpMuscleStruct* muscle, MuscWrapAssoc* mwa, dpWrapObject* wrap_object); -void remove_muscle_wrap_association(dpMuscleStruct* muscle, dpWrapObject* wrap_object); - -void update_we_win_status(); - -#define VISUAL_WRAPPING_DEBUG 0 -#define USER_SPECIFIED_WRAP_METHOD 1 - -/* wrapping algorithms - */ -#define WE_HYBRID_ALGORITHM 0 /* Frans + fan algorithm */ -#define WE_MIDPOINT_ALGORITHM 1 /* midpoint algorithm */ -#define WE_AXIAL_ALGORITHM 2 /* Frans only algorithm */ -#define WE_NUM_WRAP_ALGORITHMS 3 - -#define WE_FAN_ALGORITHM 4 /* fan only algorithm */ - -const char* get_wrap_type_name (int i); -const char* get_wrap_algorithm_name(int wrap_algorithm); - -int calc_line_intersect_ellipsoid(double p1[], double p2[], - double m[], double a[], - double *rlen, double r1[], double r2[], - double** wrap_pts, int *num_wrap_pts, - int wrap_axis, int wrap_sign, - dpMuscleWrapStruct*, int* p_flag, dpWrapObject*); - -int calc_line_intersect_cylinder(double p1[], double p2[], - double p0[], double dn[], double r, double len, - double *rlen, double r1[], double r2[], - double** wrap_pts, int *num_wrap_pts, - int wrap_axis, int wrap_sign, - dpMuscleWrapStruct*, int* p_flag, dpWrapObject*); - -int calc_line_intersect_sphere(double p1[], double p2[], - double m[], double r, - double *rlen, double r1[], double r2[], - double** wrap_pts, int *num_wrap_pts, - int wrap_axis, int wrap_sign, - dpMuscleWrapStruct*, int* p_flag, dpWrapObject*); - -int calc_line_intersect_torus(double p1[], double p2[], - double m[], double a[], - double *rlen, double r1[], double r2[], - double** wrap_pts, int *num_wrap_pts, - int wrap_axis, int wrap_sign, - dpMuscleWrapStruct*, int* p_flag, dpWrapObject*); - -void enable_debug_shapes(SBoolean); - -#if VISUAL_WRAPPING_DEBUG - -void add_debug_point( - dpWrapObject* wo, double factor, - double pt[], float radius, const char* name, const float* color); - -void add_debug_line( - dpWrapObject* wo, double factor, - double pt1[], double pt2[], float lineWidth, - const char* name1, const char* name2, const float* color); - -float* lerp_clr(const float start[3], const float end[3], double t, float color[3]); - -#endif /* VISUAL_WRAPPING_DEBUG */ - -#endif /* WEFUNCTIONS_H */ diff --git a/OpenSim/Utilities/simmToOpenSim/wetools.c b/OpenSim/Utilities/simmToOpenSim/wetools.c deleted file mode 100644 index 818ad45395..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/wetools.c +++ /dev/null @@ -1,1434 +0,0 @@ -/******************************************************************************* - - WETOOLS.C - - Author: Kenny Smith (based on pmtools.c by Peter Loan) - - Date: 22-OCT-98 - - Copyright (c) 1992-8 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: - - Routines: - makewrapeditormenus : makes all the WrapEditor menus - setwemenus : sets the positions of the menus - update_we_forms : updates the WrapEditor form fields - we_entervalue : handles the editing of form fields - draw_we_help_window : draws the help window - we_help_input : handles user events in the help window - move_we_help_text : scrolls the help text up and down - -*******************************************************************************/ - -#include "universal.h" -#include "wrapeditor.h" - -#include "globals.h" -#include "functions.h" -#include "wefunctions.h" - -#if OPENSMAC || SIMM_VIEWER -#undef ENGINE -#define ENGINE 1 -#endif - -#if ! ENGINE - -/*************** DEFINES (for this file only) *********************************/ -#define CHECK_MIN_VALUE 0 -#define CHECK_MAX_VALUE 1 - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ -static char* weoptions[] = { - "wrap object>", - "segment>", - "muscles>", - "save all", - "restore all", - "delete", - "+","-", - "clear", - "reset" -}; - -static char* weform[] = { - "object name", - "radius x","y","z", - "x","y","z", - "x","y","z" -}; -static char* wrap_type_panelstr[] = { "sphere", "cylinder", "ellipsoid", "torus" }; -static char* wrap_method_panelstr[] = { "hybrid", "midpoint", "axial" }; -static char* quadrant_panelstr[] = { "x","y","z" }; -static char* quadrant_checkstr[] = { "positive","negative" }; -static char* active_visible_panelstr[] = { "active", "visible", "show pts", "trackball" }; -static char* transform_panelstr[] = { "local frame", "parent frame" }; - - -/*************** EXTERNED VARIABLES (declared in another .c file) *************/ -extern WrapEditorStruct* we; -extern WindowParams* we_win_params; -extern WinUnion* we_win_union; - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void setwemenus(void); - - -int is_current_wrap_object(ModelStruct* ms, dpWrapObject* wo) -{ - if (we) - { - WEModelOptions* weop = &we->weop[ms->modelnum]; - - if (wo == weop->wrap_object) - { - if (ms == we->model) - return 2; - else - return 1; - } - } - return 0; -} - - -void makewrapeditormenus(void) -{ - int i, thumb_thickness; - IntBox bbox; - Menu* ms; - Form* form; - CheckBoxPanel* check; - - /* make the plot popup menu */ - //TODO5.0: why is this here???????? - root.plotmenu = glueCreateMenu("Plots"); - - glutAddMenuEntry("new", 1); - - /* make the main wrapeditor menu */ - ms = &we->optionsmenu; - ms->title = NULL; - ms->type = normal_menu; - ms->numoptions = sizeof(weoptions)/sizeof(char*); - ms->option = (MenuItem*)simm_malloc(ms->numoptions*sizeof(MenuItem)); - if (ms->option == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - ms->option[i].active = (SBoolean) (i == 0); -// ms->option[i].visible = (SBoolean) (i == 0); - ms->option[i].visible = yes; - mstrcpy(&ms->option[i].name,weoptions[i]); - } - - /* make the options form */ - form = &we->optionsform; - form->title = NULL; - form->selected_item = -1; - form->cursor_position = 0; - form->highlight_start = 0; - form->numoptions = sizeof(weform)/sizeof(char*); - form->option = (FormItem*)simm_malloc(form->numoptions*sizeof(FormItem)); - if (form->option == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - form->option[i].active = yes; - form->option[i].editable = no; - form->option[i].visible = no; - form->option[i].use_alternate_colors = no; - form->option[i].justify = no; - if (i == 4 || i == 5 || i == 6) - form->option[i].decimal_places = 2; - else - form->option[i].decimal_places = 3; - form->option[i].data = NULL; - mstrcpy(&form->option[i].name,weform[i]); - } - - /* make the wrap type radio-button panel */ - check = &we->wrap_type_radiopanel; - check->title = ""; - check->type = radio_checkbox; - check->numoptions = sizeof(wrap_type_panelstr)/sizeof(char*); - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - mstrcpy(&check->checkbox[i].name,wrap_type_panelstr[i]); - } - -#if USER_SPECIFIED_WRAP_METHOD - /* make the wrap plane radio-button panel */ - check = &we->wrap_method_radiopanel; - check->title = ""; - check->type = radio_checkbox; - check->numoptions = sizeof(wrap_method_panelstr) / sizeof(char*); - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - mstrcpy(&check->checkbox[i].name, wrap_method_panelstr[i]); - } -#endif - - /* make the wrap quadrant radio-button panel */ - check = &we->quadrant_radiopanel; - check->title = "constrain to quadrant:"; - check->type = radio_checkbox; - check->numoptions = sizeof(quadrant_panelstr)/sizeof(char*); - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - mstrcpy(&check->checkbox[i].name,quadrant_panelstr[i]); - } - - /* make the wrap quadrant check-button panel */ - check = &we->quadrant_checkpanel; - check->title = ""; - check->type = normal_checkbox; - check->numoptions = sizeof(quadrant_checkstr)/sizeof(char*); - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - mstrcpy(&check->checkbox[i].name,quadrant_checkstr[i]); - } - - /* make the active/visible checkbox panel */ - check = &we->active_visible_checkpanel; - check->title = ""; - check->type = normal_checkbox; - check->numoptions = sizeof(active_visible_panelstr)/sizeof(char*); - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - mstrcpy(&check->checkbox[i].name,active_visible_panelstr[i]); - } - - /* make the transform radio-button panel */ - check = &we->transform_radiopanel; - check->title = "transform in:"; - check->type = radio_checkbox; - check->numoptions = sizeof(transform_panelstr)/sizeof(char*); - check->checkbox = (CheckBox*)simm_malloc(check->numoptions*sizeof(CheckBox)); - if (check->checkbox == NULL) - error(exit_program,tool_message); - - for (i=0; inumoptions; i++) - { - check->checkbox[i].just = right; - check->checkbox[i].active = yes; - check->checkbox[i].visible = yes; - mstrcpy(&check->checkbox[i].name,transform_panelstr[i]); - } - - setwemenus(); - - /* Make the scroll bar for the tool window itself */ - bbox.x1 = 0; - bbox.x2 = WE_SLIDER_WIDTH; - bbox.y1 = 0; - bbox.y2 = 100; /* is changed during every redraw */ - thumb_thickness = 50; /* is changed during every redraw */ - - make_slider(&we->win_slider,vertical_slider,bbox,thumb_thickness, - (double)we->canvas_height,0.0,(double)we->canvas_height,10.0,NULL,NULL); - -} - - - -static void setwemenus(void) -{ - - Menu* ms; - Form* form; - CheckBoxPanel* check; - - /* Set the positions of the main menu boxes */ - ms = &we->optionsmenu; - - SET_BOX1221(ms->option[WE_WRAP_OBJECT].box, - -85,ms->option[WE_WRAP_OBJECT].box.x1+110, - 5,ms->option[WE_WRAP_OBJECT].box.y2-MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_CHOOSE_SEGMENT].box,ms->option[WE_WRAP_OBJECT].box.x1, - ms->option[WE_WRAP_OBJECT].box.x2,ms->option[WE_WRAP_OBJECT].box.y1, - ms->option[WE_CHOOSE_SEGMENT].box.y2-MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_MUSCLEGROUPS].box,ms->option[WE_WRAP_OBJECT].box.x1, - ms->option[WE_WRAP_OBJECT].box.x2,ms->option[WE_CHOOSE_SEGMENT].box.y1, - ms->option[WE_MUSCLEGROUPS].box.y2-MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_SAVE_ALL].box,ms->option[WE_WRAP_OBJECT].box.x1, - ms->option[WE_WRAP_OBJECT].box.x2, - ms->option[WE_MUSCLEGROUPS].box.y1 - 10, - ms->option[WE_SAVE_ALL].box.y2-MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_RESTORE_ALL].box,ms->option[WE_WRAP_OBJECT].box.x1, - ms->option[WE_WRAP_OBJECT].box.x2, - ms->option[WE_SAVE_ALL].box.y1 - 5, - ms->option[WE_RESTORE_ALL].box.y2-MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_DELETE_OBJECT].box,ms->option[WE_WRAP_OBJECT].box.x1, - ms->option[WE_WRAP_OBJECT].box.x2, - ms->option[WE_RESTORE_ALL].box.y1 - 5, - ms->option[WE_DELETE_OBJECT].box.y2-MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_APPLY_POS_XFORM].box, - ms->option[WE_WRAP_OBJECT].box.x1 + 38, - ms->option[WE_APPLY_POS_XFORM].box.x1 + 30, - ms->option[WE_DELETE_OBJECT].box.y1 - 143, - ms->option[WE_APPLY_POS_XFORM].box.y2 - MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_APPLY_NEG_XFORM].box, - ms->option[WE_APPLY_POS_XFORM].box.x2, - ms->option[WE_APPLY_NEG_XFORM].box.x1 + 30, - ms->option[WE_APPLY_POS_XFORM].box.y2, - ms->option[WE_APPLY_NEG_XFORM].box.y2 - MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_CLEAR_XFORM].box, - ms->option[WE_APPLY_NEG_XFORM].box.x2 + 5, - ms->option[WE_CLEAR_XFORM].box.x1 + 60, - ms->option[WE_APPLY_NEG_XFORM].box.y2, - ms->option[WE_CLEAR_XFORM].box.y2 - MENU_ITEM_HEIGHT); - - SET_BOX1221(ms->option[WE_IDENTITY_XFORM].box, - ms->option[WE_CLEAR_XFORM].box.x2 + 230, - ms->option[WE_IDENTITY_XFORM].box.x1 + 75, - ms->option[WE_CLEAR_XFORM].box.y1 - 10, - ms->option[WE_IDENTITY_XFORM].box.y2 - MENU_ITEM_HEIGHT); - - /* Set the positions of the options form boxes */ - form = &we->optionsform; - - form->option[WE_OBJECT_NAME].box.x1 = 0; - form->option[WE_OBJECT_NAME].box.x2 = form->option[WE_OBJECT_NAME].box.x1 + 275; - form->option[WE_OBJECT_NAME].box.y2 = 0; - form->option[WE_OBJECT_NAME].box.y1 = form->option[WE_OBJECT_NAME].box.y2 - FORM_FIELD_HEIGHT; - - form->option[WE_RADIUS_X].box.x1 = form->option[WE_OBJECT_NAME].box.x1; - form->option[WE_RADIUS_X].box.x2 = form->option[WE_RADIUS_X].box.x1 + 70; - form->option[WE_RADIUS_X].box.y2 = form->option[WE_OBJECT_NAME].box.y2 - 2 * FORM_FIELD_YSPACING; - form->option[WE_RADIUS_X].box.y1 = form->option[WE_RADIUS_X].box.y2 - FORM_FIELD_HEIGHT; - - form->option[WE_RADIUS_Y].box.x1 = form->option[WE_RADIUS_X].box.x1; - form->option[WE_RADIUS_Y].box.x2 = form->option[WE_RADIUS_X].box.x2; - form->option[WE_RADIUS_Y].box.y1 = form->option[WE_RADIUS_X].box.y1 - FORM_FIELD_YSPACING; - form->option[WE_RADIUS_Y].box.y2 = form->option[WE_RADIUS_Y].box.y1 + FORM_FIELD_HEIGHT; - - form->option[WE_RADIUS_Z].box.x1 = form->option[WE_RADIUS_Y].box.x1; - form->option[WE_RADIUS_Z].box.x2 = form->option[WE_RADIUS_Y].box.x2; - form->option[WE_RADIUS_Z].box.y1 = form->option[WE_RADIUS_Y].box.y1 - FORM_FIELD_YSPACING; - form->option[WE_RADIUS_Z].box.y2 = form->option[WE_RADIUS_Z].box.y1 + FORM_FIELD_HEIGHT; - - form->option[WE_TRANSLATE_X].box.x1 = form->option[WE_OBJECT_NAME].box.x1 - 215; - form->option[WE_TRANSLATE_X].box.x2 = form->option[WE_TRANSLATE_X].box.x1 + 60; - form->option[WE_TRANSLATE_X].box.y1 = form->option[WE_OBJECT_NAME].box.y1 - 236; - form->option[WE_TRANSLATE_X].box.y2 = form->option[WE_TRANSLATE_X].box.y1 + FORM_FIELD_HEIGHT; - - form->option[WE_TRANSLATE_Y].box.x1 = form->option[WE_TRANSLATE_X].box.x1; - form->option[WE_TRANSLATE_Y].box.x2 = form->option[WE_TRANSLATE_X].box.x2; - form->option[WE_TRANSLATE_Y].box.y1 = form->option[WE_TRANSLATE_X].box.y1 - FORM_FIELD_YSPACING + 7; - form->option[WE_TRANSLATE_Y].box.y2 = form->option[WE_TRANSLATE_Y].box.y1 + FORM_FIELD_HEIGHT; - - form->option[WE_TRANSLATE_Z].box.x1 = form->option[WE_TRANSLATE_Y].box.x1; - form->option[WE_TRANSLATE_Z].box.x2 = form->option[WE_TRANSLATE_Y].box.x2; - form->option[WE_TRANSLATE_Z].box.y1 = form->option[WE_TRANSLATE_Y].box.y1 - FORM_FIELD_YSPACING + 7; - form->option[WE_TRANSLATE_Z].box.y2 = form->option[WE_TRANSLATE_Z].box.y1 + FORM_FIELD_HEIGHT; - - form->option[WE_ROTATE_X].box.x1 = form->option[WE_TRANSLATE_X].box.x2 + 30; - form->option[WE_ROTATE_X].box.x2 = form->option[WE_ROTATE_X].box.x1 + 60; - form->option[WE_ROTATE_X].box.y1 = form->option[WE_TRANSLATE_X].box.y1; - form->option[WE_ROTATE_X].box.y2 = form->option[WE_ROTATE_X].box.y1 + FORM_FIELD_HEIGHT; - - form->option[WE_ROTATE_Y].box.x1 = form->option[WE_ROTATE_X].box.x1; - form->option[WE_ROTATE_Y].box.x2 = form->option[WE_ROTATE_X].box.x2; - form->option[WE_ROTATE_Y].box.y1 = form->option[WE_ROTATE_X].box.y1 - FORM_FIELD_YSPACING + 7; - form->option[WE_ROTATE_Y].box.y2 = form->option[WE_ROTATE_Y].box.y1 + FORM_FIELD_HEIGHT; - - form->option[WE_ROTATE_Z].box.x1 = form->option[WE_ROTATE_Y].box.x1; - form->option[WE_ROTATE_Z].box.x2 = form->option[WE_ROTATE_Y].box.x2; - form->option[WE_ROTATE_Z].box.y1 = form->option[WE_ROTATE_Y].box.y1 - FORM_FIELD_YSPACING + 7; - form->option[WE_ROTATE_Z].box.y2 = form->option[WE_ROTATE_Z].box.y1 + FORM_FIELD_HEIGHT; - - /* set the positions of the wrap type radio buttons */ - check = &we->wrap_type_radiopanel; - - check->checkbox[WE_SPHERE].box.x1 = 0; - check->checkbox[WE_SPHERE].box.x2 = check->checkbox[WE_SPHERE].box.x1 + CHECKBOX_XSIZE; - check->checkbox[WE_SPHERE].box.y2 = 0; - check->checkbox[WE_SPHERE].box.y1 = check->checkbox[WE_SPHERE].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_CYLINDER].box.x1 = check->checkbox[WE_SPHERE].box.x1; - check->checkbox[WE_CYLINDER].box.x2 = check->checkbox[WE_SPHERE].box.x2; - check->checkbox[WE_CYLINDER].box.y2 = check->checkbox[WE_SPHERE].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_CYLINDER].box.y1 = check->checkbox[WE_CYLINDER].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_ELLIPSOID].box.x1 = check->checkbox[WE_CYLINDER].box.x1; - check->checkbox[WE_ELLIPSOID].box.x2 = check->checkbox[WE_CYLINDER].box.x2; - check->checkbox[WE_ELLIPSOID].box.y2 = check->checkbox[WE_CYLINDER].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_ELLIPSOID].box.y1 = check->checkbox[WE_ELLIPSOID].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_TORUS].box.x1 = check->checkbox[WE_ELLIPSOID].box.x1; - check->checkbox[WE_TORUS].box.x2 = check->checkbox[WE_ELLIPSOID].box.x2; - check->checkbox[WE_TORUS].box.y2 = check->checkbox[WE_ELLIPSOID].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_TORUS].box.y1 = check->checkbox[WE_TORUS].box.y2 - CHECKBOX_YSIZE; - -#if USER_SPECIFIED_WRAP_METHOD - /* set the positions of the wrap plane radio buttons */ - check = &we->wrap_method_radiopanel; - - check->checkbox[0].box.x1 = 0; - check->checkbox[0].box.x2 = check->checkbox[0].box.x1 + CHECKBOX_XSIZE; - check->checkbox[0].box.y2 = 0; - check->checkbox[0].box.y1 = check->checkbox[0].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[1].box.x1 = check->checkbox[0].box.x1; - check->checkbox[1].box.x2 = check->checkbox[0].box.x2; - check->checkbox[1].box.y2 = check->checkbox[0].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[1].box.y1 = check->checkbox[1].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[2].box.x1 = check->checkbox[1].box.x1; - check->checkbox[2].box.x2 = check->checkbox[1].box.x2; - check->checkbox[2].box.y2 = check->checkbox[1].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[2].box.y1 = check->checkbox[2].box.y2 - CHECKBOX_YSIZE; -#endif - - /* set the positions of the wrap quadrant radio buttons */ - check = &we->quadrant_radiopanel; - - check->checkbox[WE_X_QUADRANT].box.x1 = 10; - check->checkbox[WE_X_QUADRANT].box.x2 = check->checkbox[WE_X_QUADRANT].box.x1 + CHECKBOX_XSIZE; - check->checkbox[WE_X_QUADRANT].box.y2 = -10; - check->checkbox[WE_X_QUADRANT].box.y1 = check->checkbox[WE_X_QUADRANT].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_Y_QUADRANT].box.x1 = check->checkbox[WE_X_QUADRANT].box.x1; - check->checkbox[WE_Y_QUADRANT].box.x2 = check->checkbox[WE_X_QUADRANT].box.x2; - check->checkbox[WE_Y_QUADRANT].box.y2 = check->checkbox[WE_X_QUADRANT].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_Y_QUADRANT].box.y1 = check->checkbox[WE_Y_QUADRANT].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_Z_QUADRANT].box.x1 = check->checkbox[WE_Y_QUADRANT].box.x1; - check->checkbox[WE_Z_QUADRANT].box.x2 = check->checkbox[WE_Y_QUADRANT].box.x2; - check->checkbox[WE_Z_QUADRANT].box.y2 = check->checkbox[WE_Y_QUADRANT].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_Z_QUADRANT].box.y1 = check->checkbox[WE_Z_QUADRANT].box.y2 - CHECKBOX_YSIZE; - - /* set the positions of the wrap quadrant check buttons */ - check = &we->quadrant_checkpanel; - - check->checkbox[WE_POSITIVE_QUADRANT].box.x1 = we->quadrant_radiopanel.checkbox[WE_X_QUADRANT].box.x2 + 40; - check->checkbox[WE_POSITIVE_QUADRANT].box.x2 = check->checkbox[WE_POSITIVE_QUADRANT].box.x1 + CHECKBOX_XSIZE; - check->checkbox[WE_POSITIVE_QUADRANT].box.y2 = we->quadrant_radiopanel.checkbox[WE_X_QUADRANT].box.y2; - check->checkbox[WE_POSITIVE_QUADRANT].box.y1 = check->checkbox[WE_POSITIVE_QUADRANT].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_NEGATIVE_QUADRANT].box.x1 = check->checkbox[WE_POSITIVE_QUADRANT].box.x1; - check->checkbox[WE_NEGATIVE_QUADRANT].box.x2 = check->checkbox[WE_NEGATIVE_QUADRANT].box.x1 + CHECKBOX_XSIZE; - check->checkbox[WE_NEGATIVE_QUADRANT].box.y2 = check->checkbox[WE_POSITIVE_QUADRANT].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_NEGATIVE_QUADRANT].box.y1 = check->checkbox[WE_NEGATIVE_QUADRANT].box.y2 - CHECKBOX_YSIZE; - - /* set the positions of the checkboxes */ - check = &we->active_visible_checkpanel; - - check->checkbox[WE_ACTIVE_CHBOX].box.x1 = 0; - check->checkbox[WE_ACTIVE_CHBOX].box.x2 = check->checkbox[WE_ACTIVE_CHBOX].box.x1 + CHECKBOX_XSIZE; - check->checkbox[WE_ACTIVE_CHBOX].box.y2 = 13; - check->checkbox[WE_ACTIVE_CHBOX].box.y1 = check->checkbox[WE_ACTIVE_CHBOX].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_VISIBLE_CHBOX].box.x1 = check->checkbox[WE_ACTIVE_CHBOX].box.x1; - check->checkbox[WE_VISIBLE_CHBOX].box.x2 = check->checkbox[WE_ACTIVE_CHBOX].box.x2; - check->checkbox[WE_VISIBLE_CHBOX].box.y2 = check->checkbox[WE_ACTIVE_CHBOX].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_VISIBLE_CHBOX].box.y1 = check->checkbox[WE_VISIBLE_CHBOX].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_SHOW_PTS_CHBOX].box.x1 = check->checkbox[WE_VISIBLE_CHBOX].box.x1; - check->checkbox[WE_SHOW_PTS_CHBOX].box.x2 = check->checkbox[WE_VISIBLE_CHBOX].box.x2; - check->checkbox[WE_SHOW_PTS_CHBOX].box.y2 = check->checkbox[WE_VISIBLE_CHBOX].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_SHOW_PTS_CHBOX].box.y1 = check->checkbox[WE_SHOW_PTS_CHBOX].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_TRACKBALL_CHBOX].box.x1 = -230; - check->checkbox[WE_TRACKBALL_CHBOX].box.x2 = check->checkbox[WE_TRACKBALL_CHBOX].box.x1 + CHECKBOX_XSIZE; - check->checkbox[WE_TRACKBALL_CHBOX].box.y2 = -162; - check->checkbox[WE_TRACKBALL_CHBOX].box.y1 = check->checkbox[WE_TRACKBALL_CHBOX].box.y2 - CHECKBOX_YSIZE; - - /* set the positions of the checkboxes */ - check = &we->transform_radiopanel; - - check->checkbox[WE_LOCAL_FRAME_RBTN].box.x1 = 10; - check->checkbox[WE_LOCAL_FRAME_RBTN].box.x2 = check->checkbox[WE_LOCAL_FRAME_RBTN].box.x1 + CHECKBOX_XSIZE; - check->checkbox[WE_LOCAL_FRAME_RBTN].box.y2 = -10; - check->checkbox[WE_LOCAL_FRAME_RBTN].box.y1 = check->checkbox[WE_LOCAL_FRAME_RBTN].box.y2 - CHECKBOX_YSIZE; - - check->checkbox[WE_PARENT_FRAME_RBTN].box.x1 = check->checkbox[WE_LOCAL_FRAME_RBTN].box.x1; - check->checkbox[WE_PARENT_FRAME_RBTN].box.x2 = check->checkbox[WE_LOCAL_FRAME_RBTN].box.x2; - check->checkbox[WE_PARENT_FRAME_RBTN].box.y2 = check->checkbox[WE_LOCAL_FRAME_RBTN].box.y2 - FORM_FIELD_HEIGHT; - check->checkbox[WE_PARENT_FRAME_RBTN].box.y1 = check->checkbox[WE_PARENT_FRAME_RBTN].box.y2 - CHECKBOX_YSIZE; -} - - - -void update_we_forms(void) -{ - Form* form = &we->optionsform; - WEModelOptions* weop = &we->weop[we->model->modelnum]; - - if (weop->wrap_object) - { - dpWrapObject* wo = weop->wrap_object; - - storeStringInForm(&form->option[WE_OBJECT_NAME], wo->name); - storeDoubleInForm(&form->option[WE_RADIUS_X], wo->radius[0], 4); - - if (wo->wrap_type == dpWrapCylinder) - storeDoubleInForm(&form->option[WE_RADIUS_Y], wo->height, 4); - else - storeDoubleInForm(&form->option[WE_RADIUS_Y], wo->radius[1], 4); - - storeDoubleInForm(&form->option[WE_RADIUS_Z], wo->radius[2], 4); - } - else { - storeStringInForm(&form->option[WE_OBJECT_NAME], NULL); - storeStringInForm(&form->option[WE_RADIUS_X], NULL); - storeStringInForm(&form->option[WE_RADIUS_Y], NULL); - storeStringInForm(&form->option[WE_RADIUS_Z], NULL); - } - - if (weop->translate.xyz[0] != 0.0) - storeDoubleInForm(&form->option[WE_TRANSLATE_X], weop->translate.xyz[0], 4); - else - storeStringInForm(&form->option[WE_TRANSLATE_X], NULL); - - if (weop->translate.xyz[1] != 0.0) - storeDoubleInForm(&form->option[WE_TRANSLATE_Y], weop->translate.xyz[1], 4); - else - storeStringInForm(&form->option[WE_TRANSLATE_Y], NULL); - - if (weop->translate.xyz[2] != 0.0) - storeDoubleInForm(&form->option[WE_TRANSLATE_Z], weop->translate.xyz[2], 4); - else - storeStringInForm(&form->option[WE_TRANSLATE_Z], NULL); - - - if (weop->rotate.xyz[0] != 0.0) - storeDoubleInForm(&form->option[WE_ROTATE_X], weop->rotate.xyz[0], 3); - else - storeStringInForm(&form->option[WE_ROTATE_X], NULL); - - if (weop->rotate.xyz[1] != 0.0) - storeDoubleInForm(&form->option[WE_ROTATE_Y], weop->rotate.xyz[1], 3); - else - storeStringInForm(&form->option[WE_ROTATE_Y], NULL); - - if (weop->rotate.xyz[2] != 0.0) - storeDoubleInForm(&form->option[WE_ROTATE_Z], weop->rotate.xyz[2], 3); - else - storeStringInForm(&form->option[WE_ROTATE_Z], NULL); -} - - - -void we_entervalue(SimmEvent se) -{ - int i, j, rc; - double rd; - Form* form = &we->optionsform; - WEModelOptions* weop = &we->weop[we->model->modelnum]; - dpWrapObject* wo = weop->wrap_object; - TextFieldAction tfa; - - switch (we->selected_item) - { - case WE_OBJECT_NAME: - rc = get_string(form, se, &tfa, no); - if (rc == STRING_NOT_DONE) - return; - if (rc != KEEP_OLD_STRING) - { - // Make sure the wrap object has a unique name. - for (i=0; imodel->num_wrap_objects; i++) - { - if (we->model->wrapobj[i] != wo && STRINGS_ARE_EQUAL(we->model->wrapobj[i]->name, form->option[form->selected_item].valuestr)) - { - (void)sprintf(errorbuffer, "The name %s is already being used by another wrap object.", form->option[form->selected_item].valuestr); - error(none, errorbuffer); - break; - } - } - if (i < we->model->num_wrap_objects) - break; - - free(wo->name); - mstrcpy(&wo->name, form->option[form->selected_item].valuestr); - - /* Send simm event to tell Muscle Editor about the name change. */ - //make_and_queue_simm_event(WRAP_OBJECT_CHANGED, (void*)we->model, weop->wrap_object, ZERO); - } - break; - - default: - rd = get_double(form, se, &tfa); - if (rd == DOUBLE_NOT_DONE) - return; - - if (rd != KEEPOLDDOUBLE && wo) - { - switch (we->selected_item) - { - case WE_RADIUS_X: - if (rd > 0.0) - { - wo->radius[0] = rd; - wo->display_list_is_stale = dpYes; - inval_model_wrapping(we->model, weop->wrap_object); - } - else - { - error(none, "Value must be greater than zero."); - } - break; - - case WE_RADIUS_Y: - if (rd > 0.0) - { - wo->radius[1] = rd; - if (wo->wrap_type == dpWrapCylinder) - wo->height = rd; - wo->display_list_is_stale = dpYes; - inval_model_wrapping(we->model, weop->wrap_object); - } - else - { - error(none, "Value must be greater than zero."); - } - break; - - case WE_RADIUS_Z: - if (rd > 0.0) - { - wo->radius[2] = rd; - wo->display_list_is_stale = dpYes; - inval_model_wrapping(we->model, weop->wrap_object); - } - else - { - error(none, "Value must be greater than zero."); - } - break; - - case WE_TRANSLATE_X: weop->translate.xyz[0] = rd; break; - case WE_TRANSLATE_Y: weop->translate.xyz[1] = rd; break; - case WE_TRANSLATE_Z: weop->translate.xyz[2] = rd; break; - - case WE_ROTATE_X: weop->rotate.xyz[0] = rd; break; - case WE_ROTATE_Y: weop->rotate.xyz[1] = rd; break; - case WE_ROTATE_Z: weop->rotate.xyz[2] = rd; break; - } - - } - break; - } - - if (tfa == goto_previous_field || tfa == goto_next_field) - { - we->selected_item = find_next_active_field(form,we->selected_item,tfa); - form->selected_item = we->selected_item; - form->cursor_position = strlen(form->option[form->selected_item].valuestr); - form->highlight_start = 0; - } - else - { - we->current_mode = WE_TOP_LEVEL; - we->selected_item = -1; - form->selected_item = -1; - form->cursor_position = 0; - form->highlight_start = form->cursor_position; - } - - /* send WRAP_OBJECT_CHANGED event to tell the Muscle Editor about the change. */ - make_and_queue_simm_event(WRAP_OBJECT_CHANGED, we->model->modelnum, wo, NULL, ZERO, ZERO); - update_we_forms(); - display_wrapeditor(we_win_params, we_win_union); -} - - -public void draw_we_help_window(WindowParams* win_parameters, WinUnion* win_struct) -{ - draw_help_window(&we->help); -} - - - -public void we_help_input(WindowParams* win_parameters, WinUnion* win_struct, - SimmEvent se) -{ - if (se.field1 == window_shut) - { - delete_window(we->help.window_id); - we->help.window_id = NO_WINDOW; - return; - } - - if (se.field1 != select_button || se.field2 != key_pressed) - return; - - if (check_slider(&we->help.sl,se, - move_we_help_text,DUMMY_INT) == yes) - draw_help_window(&we->help); -} - - - -public void move_we_help_text(int dummy_int, double slider_value, double delta) -{ - we->help.starting_line = we->help.sl.max_value - slider_value; - - draw_help_window(&we->help); -} - - - -void apply_xform_to_wrapobj (double factor) -{ - if (we->model) - { - WEModelOptions* weop = &we->weop[we->model->modelnum]; - - if (weop->wrap_object) - { - dpCoord3D translate = { 0.0, 0.0, 0.0 }; - dpCoord3D rotate = { 0.0, 0.0, 0.0 }; - DMatrix m; - - dpWrapObject* wo = weop->wrap_object; - - /* collect transform input from form fields: - */ - if (*we->optionsform.option[WE_TRANSLATE_X].valuestr) - sscanf(we->optionsform.option[WE_TRANSLATE_X].valuestr, "%lf", &translate.xyz[0]); - - if (*we->optionsform.option[WE_TRANSLATE_Y].valuestr) - sscanf(we->optionsform.option[WE_TRANSLATE_Y].valuestr, "%lf", &translate.xyz[1]); - - if (*we->optionsform.option[WE_TRANSLATE_Z].valuestr) - sscanf(we->optionsform.option[WE_TRANSLATE_Z].valuestr, "%lf", &translate.xyz[2]); - - if (*we->optionsform.option[WE_ROTATE_X].valuestr) - sscanf(we->optionsform.option[WE_ROTATE_X].valuestr, "%lf", &rotate.xyz[0]); - - if (*we->optionsform.option[WE_ROTATE_Y].valuestr) - sscanf(we->optionsform.option[WE_ROTATE_Y].valuestr, "%lf", &rotate.xyz[1]); - - if (*we->optionsform.option[WE_ROTATE_Z].valuestr) - sscanf(we->optionsform.option[WE_ROTATE_Z].valuestr, "%lf", &rotate.xyz[2]); - - /* build transform matrix: - */ - identity_matrix(m); - rotate_matrix_axis_angle(m, wo->rotation_axis.xyz, wo->rotation_angle); - - if (weop->xform_frame == WE_LOCAL_FRAME) - { - x_rotate_matrix_bodyfixed(m, factor * rotate.xyz[0] * DTOR); - y_rotate_matrix_bodyfixed(m, factor * rotate.xyz[1] * DTOR); - z_rotate_matrix_bodyfixed(m, factor * rotate.xyz[2] * DTOR); - } else { - x_rotate_matrix_spacefixed(m, factor * rotate.xyz[0] * DTOR); - y_rotate_matrix_spacefixed(m, factor * rotate.xyz[1] * DTOR); - z_rotate_matrix_spacefixed(m, factor * rotate.xyz[2] * DTOR); - } - extract_rotation(m, &wo->rotation_axis, &wo->rotation_angle); - - if (weop->xform_frame == WE_LOCAL_FRAME) - { - dpCoord3D v; - - v.xyz[0] = factor * translate.xyz[0]; - v.xyz[1] = factor * translate.xyz[1]; - v.xyz[2] = factor * translate.xyz[2]; - - transform_vec(m, v.xyz); - - wo->translation.xyz[0] += v.xyz[0]; - wo->translation.xyz[1] += v.xyz[1]; - wo->translation.xyz[2] += v.xyz[2]; - - wo->undeformed_translation.xyz[0] += v.xyz[0]; - wo->undeformed_translation.xyz[1] += v.xyz[1]; - wo->undeformed_translation.xyz[2] += v.xyz[2]; - } else { - wo->translation.xyz[0] += factor * translate.xyz[0]; - wo->translation.xyz[1] += factor * translate.xyz[1]; - wo->translation.xyz[2] += factor * translate.xyz[2]; - - wo->undeformed_translation.xyz[0] += factor * translate.xyz[0]; - wo->undeformed_translation.xyz[1] += factor * translate.xyz[1]; - wo->undeformed_translation.xyz[2] += factor * translate.xyz[2]; - } - - /* apply transform to wrap object: - */ - wo->xforms_valid = no; - recalc_xforms(wo); - inval_model_wrapping(we->model, weop->wrap_object); - /* send WRAP_OBJECT_CHANGED event to tell the Muscle Editor about the change. */ - make_and_queue_simm_event(WRAP_OBJECT_CHANGED, we->model->modelnum, wo, NULL, ZERO, ZERO); - queue_model_redraw(we->model); - } - } -} - -void clear_we_xform_form () -{ - storeStringInForm(&we->optionsform.option[WE_TRANSLATE_X], NULL); - storeStringInForm(&we->optionsform.option[WE_TRANSLATE_Y], NULL); - storeStringInForm(&we->optionsform.option[WE_TRANSLATE_Z], NULL); - - storeStringInForm(&we->optionsform.option[WE_ROTATE_X], NULL); - storeStringInForm(&we->optionsform.option[WE_ROTATE_Y], NULL); - storeStringInForm(&we->optionsform.option[WE_ROTATE_Z], NULL); - - if (we->model) - { - WEModelOptions* weop = &we->weop[we->model->modelnum]; - - weop->translate.xyz[0] = 0.0; - weop->translate.xyz[1] = 0.0; - weop->translate.xyz[2] = 0.0; - - weop->rotate.xyz[0] = 0.0; - weop->rotate.xyz[1] = 0.0; - weop->rotate.xyz[2] = 0.0; - } - display_wrapeditor(we_win_params, we_win_union); -} - - - -void reset_wrapobj_xform () -{ - if (we->model) - { - WEModelOptions* weop = &we->weop[we->model->modelnum]; - - if (weop->wrap_object) - { - dpWrapObject* wo = weop->wrap_object; - - wo->undeformed_translation.xyz[0] = wo->translation.xyz[0] = 0.0; - wo->undeformed_translation.xyz[1] = wo->translation.xyz[1] = 0.0; - wo->undeformed_translation.xyz[2] = wo->translation.xyz[2] = 0.0; - - wo->rotation_axis.xyz[0] = 1.0; - wo->rotation_axis.xyz[1] = 0.0; - wo->rotation_axis.xyz[2] = 0.0; - - wo->rotation_angle = 0.0; - - wo->xforms_valid = no; - - recalc_xforms(wo); - inval_model_wrapping(we->model, weop->wrap_object); - /* send WRAP_OBJECT_CHANGED event to tell the Muscle Editor about the change. */ - make_and_queue_simm_event(WRAP_OBJECT_CHANGED, we->model->modelnum, wo, NULL, ZERO, ZERO); - queue_model_redraw(we->model); - display_wrapeditor(we_win_params, we_win_union); - } - } -} - -/* save all the wrap objects */ -void save_all_wrap_objects (ModelStruct* ms) -{ - if (ms && ms->num_wrap_objects > 0) - { - int i, j, k; - size_t nBytes = ms->num_wrap_objects * sizeof(dpWrapObject); - - /* dealloc previous saved wrap object arrays */ - if (ms->save.wrap_object) - { - for (i = 0; i < ms->save.num_wrap_objects; i++) - FREE_IFNOTNULL(ms->save.wrap_object[i].name); - FREE_IFNOTNULL(ms->save.wrap_object); - } - - /* alloc new saved wrap object arrays */ - ms->save.wrap_object = (dpWrapObject*) simm_malloc(nBytes); - - if (ms->save.wrap_object) - { - ms->save.num_wrap_objects = ms->num_wrap_objects; - - /* copy wrap objects and their names */ - memcpy(ms->save.wrap_object, ms->wrapobj, nBytes); - - for (i = 0; i < ms->num_wrap_objects; i++) - mstrcpy(&ms->save.wrap_object[i].name, ms->wrapobj[i]->name); - - /* save muscle -> wrap object associations */ - if (ms->save.muscwrap_associations) - { - free(ms->save.muscwrap_associations); - ms->save.muscwrap_associations = NULL; - } - - ms->save.num_muscwrap_associations = 0; - - for (i = 0; i < ms->nummuscles; i++) - ms->save.num_muscwrap_associations += ms->muscle[i]->numWrapStructs; - - if (ms->save.num_muscwrap_associations > 0) - { - ms->save.muscwrap_associations = (MuscWrapAssoc*) simm_malloc( - ms->save.num_muscwrap_associations * sizeof(MuscWrapAssoc)); - - if (ms->save.muscwrap_associations) - { - for (i = 0, k = 0; i < ms->nummuscles; i++) - { - for (j = 0; j < ms->muscle[i]->numWrapStructs; j++) - { - ms->save.muscwrap_associations[k].muscle = i; - ms->save.muscwrap_associations[k].musc_wrap_index = j; - ms->save.muscwrap_associations[k].start_pt = ms->muscle[i]->wrapStruct[j]->startPoint; - ms->save.muscwrap_associations[k].end_pt = ms->muscle[i]->wrapStruct[j]->endPoint; - ms->save.muscwrap_associations[k].wrap_algorithm = ms->muscle[i]->wrapStruct[j]->wrap_algorithm; - ms->save.muscwrap_associations[k].wrap_object = ms->muscle[i]->wrapStruct[j]->wrap_object; - k++; - } - } - } - } - } - } -} - - -void we_track_cb(void* data, SimmEvent se) -{ - int i, count, we_vals[WE_MAX_DEVS]; - int model_windex; - - WrapEditorTracker* tracker = (WrapEditorTracker*) data; - Scene* scene = tracker->scene; - ModelStruct* ms = tracker->model; - ModelDisplayStruct* dis = &ms->dis; - WEModelOptions* weop = &we->weop[ms->modelnum]; - dpWrapObject* wo; - - SBoolean redraw = no; - double z_dist, tmp_matrix[4][4], inv_view_matrix[4][4]; - double wpt[4], wpt2[4], owpt[4], owpt2[4]; - int bpan_mx_new, bpan_my_new; - double bpan_wx_new, bpan_wy_new, bpan_wz_new; - int mx_new, my_new; - double wx_new, wy_new, wz_new; - double angle, origin[4], new_origin[4], naxis[4], axis[4], x_percent, cursor_movement; - - getDev(we->numdevs, we->devs, we_vals); - - for (count = 0, i = 0; i < we->numdevs; i++) - { - if (we_vals[i] == 1) - count++; - } - if (count == 0) - { - REMOVE_TRACKER(); - return; - } - - wo = weop->wrap_object; - - if (wo->visible == no) - return; -#if 1 - if (wo->wrap_type == dpWrapCylinder && se.field1 == space_key && se.field2 == key_released) - { - wo->wrap_algorithm = ! wo->wrap_algorithm; - inval_model_wrapping(we->model, weop->wrap_object); - queue_model_redraw(ms); - return; - } -#endif - - if (we_vals[WE_MOVE_WRAP_OBJECT_KEY]) - { - mx_new = se.mouse_x; - my_new = se.mouse_y; - - if (weop->trackball_rotation) - { - if (we_vals[WE_TRACKBALL_KEY]) - { - z_dist = scene->tz; - - find_world_coords(scene, mx_new, my_new, - z_dist, &wx_new, &wy_new, &wz_new); - - if (tracker->mx_old == -1 || tracker->my_old == -1) - { - tracker->mx_old = mx_new; - tracker->my_old = my_new; - tracker->wx_old = wx_new; - tracker->wy_old = wy_new; - tracker->wz_old = wz_new; - } - else if (tracker->mx_old != mx_new || tracker->my_old != my_new) - { - origin[0] = origin[1] = origin[2] = 0.0; - origin[3] = 1.0; - - axis[0] = tracker->wy_old - wy_new; - axis[1] = wx_new - tracker->wx_old; - axis[2] = 0.0; - - normalize_vector(axis, naxis); - naxis[3] = 1.0; - - invert_4x4transform(scene->transform_matrix, tmp_matrix); - mult_4x4matrix_by_vector(tmp_matrix, naxis, axis); - mult_4x4matrix_by_vector(tmp_matrix, origin, new_origin); - - axis[0] -= new_origin[0]; - axis[1] -= new_origin[1]; - axis[2] -= new_origin[2]; - - normalize_vector(axis, naxis); - - naxis[0] -= origin[0]; - naxis[1] -= origin[1]; - naxis[2] -= origin[2]; - - normalize_vector(naxis, naxis); - convert_vector(ms, naxis, ms->ground_segment, wo->segment); - normalize_vector(naxis, naxis); - - /* if cursor moves a full screen width, rotate by 90 degrees */ - cursor_movement = sqrt((mx_new-tracker->mx_old)*(mx_new-tracker->mx_old) + - (my_new-tracker->my_old)*(my_new-tracker->my_old)); - - x_percent = cursor_movement / (double)(scene->viewport[2]); - - angle = x_percent * 90.0; - - if (angle >= 0.0 && angle <= 180.0) - { - DMatrix m; - - identity_matrix(m); - rotate_matrix_axis_angle(m, wo->rotation_axis.xyz, - wo->rotation_angle); - rotate_matrix_axis_angle(m, naxis, angle * DTOR); - - extract_rotation(m, &wo->rotation_axis, &wo->rotation_angle); - - redraw = yes; - - tracker->mx_old = mx_new; - tracker->my_old = my_new; - tracker->wx_old = wx_new; - tracker->wy_old = wy_new; - tracker->wz_old = wz_new; - } - } - } - else - { - tracker->wx_old = tracker->wy_old = tracker->wz_old = -1.0; - tracker->mx_old = tracker->my_old = -1; - } - - if (we_vals[WE_PAN_WRAP_OBJECT_KEY]) - { - wpt[0] = wpt[1] = wpt[2] = 0.0; - convert_from_wrap_object_frame(wo, wpt); - wpt[3] = 1.0; - - convert(ms, wpt, wo->segment, ms->ground_segment); - mult_4x4matrix_by_vector(scene->transform_matrix, wpt, wpt2); - - z_dist = wpt2[2]; - bpan_mx_new = se.mouse_x; - bpan_my_new = se.mouse_y; - - find_world_coords(scene, bpan_mx_new, bpan_my_new, - z_dist, &bpan_wx_new, &bpan_wy_new, &bpan_wz_new); - - if (tracker->bpan_mx_old == -1 || tracker->bpan_my_old == -1) - { - tracker->bpan_mx_old = bpan_mx_new; - tracker->bpan_my_old = bpan_my_new; - tracker->bpan_wx_old = bpan_wx_new; - tracker->bpan_wy_old = bpan_wy_new; - tracker->bpan_wz_old = bpan_wz_new; - } - else if (tracker->bpan_mx_old != bpan_mx_new || - tracker->bpan_my_old != bpan_my_new) - { - wpt[0] = bpan_wx_new; - wpt[1] = bpan_wy_new; - wpt[2] = bpan_wz_new; - wpt[3] = 1.0; - - invert_4x4transform(scene->transform_matrix, inv_view_matrix); - -#if !STEADYCAM - if (dis->camera_segment >= 0) - copy_4x4matrix(*get_ground_conversion(ms->modelnum, - dis->camera_segment, to_ground), tmp_matrix); - else - reset_4x4matrix(tmp_matrix); - append_4x4matrix(inv_view_matrix,tmp_matrix); -#endif - mult_4x4matrix_by_vector(inv_view_matrix,wpt,wpt2); - convert(ms, wpt2, ms->ground_segment, wo->segment); - - owpt[0] = tracker->bpan_wx_old; - owpt[1] = tracker->bpan_wy_old; - owpt[2] = tracker->bpan_wz_old; - owpt[3] = 1.0; - - mult_4x4matrix_by_vector(inv_view_matrix,owpt,owpt2); - convert(ms, owpt2, ms->ground_segment, wo->segment); - - wo->translation.xyz[0] += (wpt2[XX] - owpt2[XX]); - wo->translation.xyz[1] += (wpt2[YY] - owpt2[YY]); - wo->translation.xyz[2] += (wpt2[ZZ] - owpt2[ZZ]); - - wo->undeformed_translation.xyz[0] += (wpt2[XX] - owpt2[XX]); - wo->undeformed_translation.xyz[1] += (wpt2[YY] - owpt2[YY]); - wo->undeformed_translation.xyz[2] += (wpt2[ZZ] - owpt2[ZZ]); - - tracker->bpan_mx_old = bpan_mx_new; - tracker->bpan_my_old = bpan_my_new; - tracker->bpan_wx_old = bpan_wx_new; - tracker->bpan_wy_old = bpan_wy_new; - tracker->bpan_wz_old = bpan_wz_new; - redraw = yes; - } - } - else - { - tracker->bpan_wx_old = tracker->bpan_wy_old = tracker->bpan_wz_old = -1.0; - tracker->bpan_mx_old = tracker->bpan_my_old = -1; - } - - if (we_vals[WE_ZOOM_WRAP_OBJECT_KEY]) - { - if (tracker->zoom_mx_old == -1 || tracker->zoom_my_old == -1) - { - tracker->zoom_mx_old = mx_new; - tracker->zoom_my_old = my_new; - - wpt[0] = wpt[1] = 0.0; - wpt[2] = wpt[3] = 1.0; - - invert_4x4transform(scene->transform_matrix, tmp_matrix); - - mult_4x4matrix_by_vector(tmp_matrix, wpt, wpt2); - convert(ms, wpt2, ms->ground_segment, wo->segment); - - tracker->zoom_vec.xyz[0] = wpt2[0]; - tracker->zoom_vec.xyz[1] = wpt2[1]; - tracker->zoom_vec.xyz[2] = wpt2[2]; - - wpt[0] = wpt[1] = wpt[2] = 0.0; - wpt[3] = 1.0; - - mult_4x4matrix_by_vector(tmp_matrix, wpt, wpt2); - convert(ms, wpt2, ms->ground_segment, wo->segment); - - tracker->zoom_vec.xyz[0] -= wpt2[0]; - tracker->zoom_vec.xyz[1] -= wpt2[1]; - tracker->zoom_vec.xyz[2] -= wpt2[2]; - } - else if (tracker->zoom_mx_old != mx_new || tracker->zoom_my_old != my_new) - { - double netmove = (mx_new - tracker->zoom_mx_old) * 0.002; - - wo->translation.xyz[0] += netmove * tracker->zoom_vec.xyz[0]; - wo->translation.xyz[1] += netmove * tracker->zoom_vec.xyz[1]; - wo->translation.xyz[2] += netmove * tracker->zoom_vec.xyz[2]; - - wo->undeformed_translation.xyz[0] += netmove * tracker->zoom_vec.xyz[0]; - wo->undeformed_translation.xyz[1] += netmove * tracker->zoom_vec.xyz[1]; - wo->undeformed_translation.xyz[2] += netmove * tracker->zoom_vec.xyz[2]; - - redraw = yes; - - tracker->zoom_mx_old = mx_new; - tracker->zoom_my_old = my_new; - } - } - else - tracker->zoom_mx_old = tracker->zoom_my_old = -1; - } - else /* trackball turned off, do x,y,z rotation */ - { - double new_rot_angle; - DMatrix m; - - identity_matrix(m); - rotate_matrix_axis_angle(m, wo->rotation_axis.xyz, wo->rotation_angle); - - if (we_vals[WE_ROTATE_X_KEY]) - { - if (CURSOR_IN_VIEWPORT(mx_new, my_new, scene->viewport)) - { - new_rot_angle = DISTANCE_FROM_MIDPOINT(mx_new, scene->viewport) * 0.1; - - if (weop->xform_frame == WE_LOCAL_FRAME) - x_rotate_matrix_bodyfixed(m, new_rot_angle * DTOR); - else - x_rotate_matrix_spacefixed(m, new_rot_angle * DTOR); - - redraw = yes; - } - } - if (we_vals[WE_ROTATE_Y_KEY]) - { - if (CURSOR_IN_VIEWPORT(mx_new, my_new, scene->viewport)) - { - new_rot_angle = DISTANCE_FROM_MIDPOINT(mx_new, scene->viewport) * 0.1; - - if (weop->xform_frame == WE_LOCAL_FRAME) - y_rotate_matrix_bodyfixed(m, new_rot_angle * DTOR); - else - y_rotate_matrix_spacefixed(m, new_rot_angle * DTOR); - - redraw = yes; - } - } - if (we_vals[WE_ROTATE_Z_KEY]) - { - if (CURSOR_IN_VIEWPORT(mx_new, my_new, scene->viewport)) - { - new_rot_angle = DISTANCE_FROM_MIDPOINT(mx_new, scene->viewport) * 0.1; - - if (weop->xform_frame == WE_LOCAL_FRAME) - z_rotate_matrix_bodyfixed(m, new_rot_angle * DTOR); - else - z_rotate_matrix_spacefixed(m, new_rot_angle * DTOR); - - redraw = yes; - } - } - if (redraw) - extract_rotation(m, &wo->rotation_axis, &wo->rotation_angle); - } - } - - if (redraw == yes) - { - wo->xforms_valid = no; - recalc_xforms(wo); - inval_model_wrapping(ms, weop->wrap_object); - queue_model_redraw(ms); - if (ms == we->model) - display_wrapeditor(we_win_params, we_win_union); - } -} - - -static SBoolean sEnableDebugShapes = no; - -/* ------------------------------------------------------------------------- - enable_debug_shapes ----------------------------------------------------------------------------- */ -void enable_debug_shapes (SBoolean state) -{ - sEnableDebugShapes = state; -} - -#if VISUAL_WRAPPING_DEBUG - -/* ------------------------------------------------------------------------- - add_debug_point - add a point for debugging display. ----------------------------------------------------------------------------- */ -void add_debug_point ( - dpWrapObject* wo, - double factor, - double pt[], - float radius, - const char* name, - const float* color) -{ - int i = wo->num_debug_glyphs; - - if ( ! sEnableDebugShapes) - return; - - if (i >= MAX_DEBUG_GLYPHS) - return; - - wo->glyph[i].isLine = no; - wo->glyph[i].p1[0] = pt[0] / factor; - wo->glyph[i].p1[1] = pt[1] / factor; - wo->glyph[i].p1[2] = pt[2] / factor; - wo->glyph[i].radius = radius / factor; - wo->glyph[i].name1 = name; - wo->glyph[i].name2 = NULL; - wo->glyph[i].color = color; - - wo->num_debug_glyphs++; -} - -/* ------------------------------------------------------------------------- - add_debug_line - add a line segment for debugging display. ----------------------------------------------------------------------------- */ -void add_debug_line ( - dpWrapObject* wo, - double factor, - double pt1[], - double pt2[], - float lineWidth, - const char* name1, - const char* name2, - const float* color) -{ - int i = wo->num_debug_glyphs; - - if ( ! sEnableDebugShapes) - return; - - if (i >= MAX_DEBUG_GLYPHS) - return; - - wo->glyph[i].isLine = yes; - wo->glyph[i].p1[0] = pt1[0] / factor; - wo->glyph[i].p1[1] = pt1[1] / factor; - wo->glyph[i].p1[2] = pt1[2] / factor; - wo->glyph[i].p2[0] = pt2[0] / factor; - wo->glyph[i].p2[1] = pt2[1] / factor; - wo->glyph[i].p2[2] = pt2[2] / factor; - wo->glyph[i].radius = lineWidth; - wo->glyph[i].name1 = name1; - wo->glyph[i].name2 = name2; - wo->glyph[i].color = color; - - wo->num_debug_glyphs++; -} - -/* ------------------------------------------------------------------------- - lerp_clr - interpolate between two rgb colors. ----------------------------------------------------------------------------- */ -float* lerp_clr (const float start[3], const float end[3], double t, float color[3]) -{ - if (t < 0.0) - { - color[0] = start[0]; - color[1] = start[1]; - color[2] = start[2]; - } - else if (t > 1.0) - { - color[0] = end[0]; - color[1] = end[1]; - color[2] = end[2]; - } - else - { - color[0] = start[0] + t * (end[0] - start[0]); - color[1] = start[1] + t * (end[1] - start[1]); - color[2] = start[2] + t * (end[2] - start[2]); - } - return color; -} - -#endif /* VISUAL_WRAPPING_DEBUG */ - -#endif /* ! ENGINE */ - -void recalc_xforms(dpWrapObject* wo) -{ - if ( ! wo->xforms_valid) - { - identity_matrix(wo->from_local_xform); - - if (wo->rotation_angle != 0.0) - rotate_matrix_axis_angle(wo->from_local_xform, wo->rotation_axis.xyz, wo->rotation_angle); - - translate_matrix(wo->from_local_xform, wo->translation.xyz); - - invert_4x4transform(wo->from_local_xform, wo->to_local_xform); - - wo->xforms_valid = yes; - } -} - -const char* get_wrap_type_name (int i) -{ - switch (i) { - case dpWrapSphere: - return "sphere"; - case dpWrapCylinder: - return "cylinder"; - case dpWrapEllipsoid: - return "ellipsoid"; - case dpWrapTorus: - return "torus"; - } - - return "none"; -} - -const char* get_wrap_algorithm_name (int i) -{ - static char* names[WE_NUM_WRAP_ALGORITHMS] = { "hybrid", "midpoint", "axial" }; - - if (i >= 0 && i < WE_NUM_WRAP_ALGORITHMS) - return names[i]; - - return NULL; -} - -SBoolean query_muscle_wrap_association (dpMuscleStruct* muscle, dpWrapObject* wrap_object) -{ - /* See if the given wrap object is used in the given muscle. - * You don't care which slot it's in, just that it's there. - */ - if (muscle && wrap_object) - { - int i; - - for (i = 0; i < muscle->numWrapStructs; i++) - if (muscle->wrapStruct[i]->wrap_object == wrap_object) - return yes; - } - - return no; -} diff --git a/OpenSim/Utilities/simmToOpenSim/windowroot.h b/OpenSim/Utilities/simmToOpenSim/windowroot.h deleted file mode 100644 index 1eb52356e1..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/windowroot.h +++ /dev/null @@ -1,186 +0,0 @@ - /******************************************************************************* - - WINDOWROOT.H - - Author: Peter Loan - - Date: 13-APR-89 - - Copyright (c) 1992 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef WINDOWROOT_H -#define WINDOWROOT_H - -#define NO_WINDOW_CHANGE 0 -#define CHANGED_ORIGIN 1 -#define CHANGED_SIZE 2 -#define CHANGED_SIZE_AND_ORIGIN 3 - -#define WINDOW_ICONIC 0 -#define WINDOW_OPEN 1 -#define WINDOW_KILLED 2 - -#define POP_WINDOW 0 -#define PUSH_WINDOW 1 - -#ifdef WIN32 - #define TITLE_BAR_HEIGHT 20 -#else - #define TITLE_BAR_HEIGHT 30 -#endif - -ENUM { - PLOT, /* */ - PLOTKEY, /* */ - TOOL, /* */ - SCENE, /* */ - NOTYPE /* */ -} WindowType; /* */ - - -STRUCT { - int id; /* identifier returned by glueOpenWindow() */ - IntBox vp; /* viewport */ - Ortho ortho; /* values for ortho() call */ - int minwidth; /* minimum allowable width */ - int minheight; /* minimum allowable height */ - int xo; /* x coord of window origin */ - int yo; /* y coord of window origin */ - int xsize; /* width of window in pixels */ - int ysize; /* height of window in pixels */ - char* name; /* name of the window */ -} WindowParams; /* window properties */ - - -STRUCT { - unsigned int event_code; /* type of event */ - int model_index; /* index of model associated with the event */ - void* struct_ptr1; /* user data */ - void* struct_ptr2; /* user data */ - int field1; /* user data */ - int field2; /* user data */ - int mouse_x; /* mouse X position */ - int mouse_y; /* mouse Y position */ - int key_modifiers; /* shift, ctrl, alt key states */ - int window_id; /* id of window the event happened in */ - PickIndex object; /* for identifying any object in a model */ -} SimmEvent; - -STRUCT { - SBoolean used; - char* name; - unsigned int simm_event_mask; - void* tool_struct; - void (*simm_event_handler)(SimmEvent); - void (*command_handler)(char*); - SBoolean (*query_handler)(QueryType, void*); -} ToolStruct; - - -UNION { - ToolStruct* tool; /* */ - Scene* scene; /* */ - PlotStruct* plot; /* */ - PlotKeyStruct* plotkey; /* */ -} WinUnion; - - -STRUCT { - WindowType type; /* */ - int reference_number; /* */ - int state; /* whether window is iconic or not */ - WinUnion* win_struct; /* */ - WindowParams* win_parameters; /* */ - void (*input_handler)(WindowParams*, WinUnion*, SimmEvent); - void (*update_function)(WindowParams*, WinUnion*); - void (*display_function)(WindowParams*, WinUnion*); -} WindowList; - -STRUCT { - char* name; - char* value; -} SimmPreference; - -STRUCT { - int num_primaries; - int num_secondaries; - int num_misc; - int max_tool_colors; - int num_tool_colors; - int first_tool_color_index; - ColorRGB cmap[COLORBUFFER]; -} ColorDatabase; - -STRUCT { - char* name; - SBoolean defined_yet; -} ElementStruct; - -STRUCT { - long max_screen_x; - long max_screen_y; -} GLStruct; - -/* data for handling selecting an object from a popup menu */ -typedef struct { - WindowParams* win_parameters; - int menu; - int* submenu; - int num_submenus; -} ObjectSelectMenuData; - -STRUCT { - WindowParams* win_params; - void* struct_ptr; - ModelStruct* ms; - PlotStruct* ps; -} TitleAreaCBParams; - -typedef void (*title_area_cb)(int selector, TitleAreaCBParams* params); - -#if ! OPENSIM_BUILD -STRUCT { - IntBox surgwin; /* */ - int basewindow; /* */ - int nummodels; /* */ - int numplots; /* */ - int numtools; /* */ - int numwindows; /* */ - int toptool; /* */ - int currentmodel; /* */ - int currentplot; /* */ - int currentwindow; /* */ - int modelcount; /* total number of models added so far */ - int numbitplanes; /* */ - int numworldobjects; /* */ - long modelmenu; /* */ - long plotmenu; /* */ - long gravityMenu; /* menu for choosing direction of gravity */ - WindowList window[200]; /* */ - GlobalFonts gfont; /* */ - Menu confirm_menu; /* */ - int events_in_queue; /* */ - int event_queue_length; /* */ - SimmEvent* simm_event_queue; /* */ - Menu model_selector; /* */ - Menu plot_selector; /* */ - Menu help_selector; /* */ - SBoolean confirm_window_open; /* */ - void (*confirm_callback)(SBoolean); /* */ - ColorDatabase color; /* */ - GLStruct gldesc; /* */ - const glutSysInfo* ginfo; /* */ - int num_commands; /* number of commands in command list */ - char* command[COMMAND_BUFFER]; /* list of commands to be parsed */ - HelpStruct messages; /* */ - int num_preferences; - int preference_array_size; - SimmPreference* preference; -} RootStruct; /* */ -#endif - -#endif /*WINDOWROOT_H*/ diff --git a/OpenSim/Utilities/simmToOpenSim/worldobjects.c b/OpenSim/Utilities/simmToOpenSim/worldobjects.c deleted file mode 100644 index f57059e350..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/worldobjects.c +++ /dev/null @@ -1,316 +0,0 @@ -/******************************************************************************* - - WORLDOBJECTS.C - - Author: Peter Loan - - Date: 21-MAR-91 - - Copyright (c) 1992-4 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - - Description: - - Routines: - readworldobjects : reads a world object from input file - compute_polygon_normals : computes normals for world object polygons - -*******************************************************************************/ - -#include "universal.h" -#include "globals.h" -#include "functions.h" -#include "normio.h" -#include "normtools.h" - -#if OPENSMAC -#undef ENGINE -#define ENGINE 1 -#endif - -/*************** DEFINES (for this file only) *********************************/ -#define ARRAY_INCREMENT 5 - - -/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/ - - - -/*************** EXTERNED VARIABLES (declared in another file) ****************/ - - - -/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/ -static void init_world_object(ModelStruct* ms, WorldObject* obj); - - -#if ! ENGINE -PickIndex pack_world_value(int world_obj) -{ - if (world_obj >= MAX_OBJECTS) - return 0; - else - return (WORLD_OBJECT << OBJECT_BITS) + (PickIndex)world_obj; -} - - -void unpack_world_value(PickIndex value, int* world_obj) -{ - *world_obj = value; -} -#endif - -ReturnCode read_world_object(ModelStruct* ms, FILE* fp) -{ - int i, j; - ReturnCode rc = code_fine; - WorldObject* obj; - char fname[CHARBUFFER], name[CHARBUFFER], mess[CHARBUFFER]; - double scale[3], translation[3], rotation[3]; - - if (ms->numworldobjects >= ms->world_array_size) - { - ms->world_array_size += ARRAY_INCREMENT; - ms->worldobj = (WorldObject*)simm_realloc(ms->worldobj, - ms->world_array_size*sizeof(WorldObject),&rc); - if (rc == code_bad) - { - ms->world_array_size -= ARRAY_INCREMENT; - return code_bad; - } - } - - obj = &ms->worldobj[ms->numworldobjects]; - - init_world_object(ms, obj); - scale[0] = scale[1] = scale[2] = 1.0; - translation[0] = translation[1] = translation[2] = 0.0; - rotation[0] = rotation[1] = rotation[2] = 0.0; - - if (fscanf(fp, "%s", name) != 1) - { - error(abort_action, "Error reading name in object definition"); - return code_bad; - } - else - mstrcpy(&obj->name,name); - - while (1) - { - if (read_string(fp,buffer) == EOF) - { - (void)sprintf(errorbuffer, "Unexpected EOF found reading definition of object %s", obj->name); - error(abort_action,errorbuffer); - return code_bad; - } - else if (STRINGS_ARE_EQUAL(buffer, "endworldobject")) - { - break; - } - else if (STRINGS_ARE_EQUAL(buffer, "filename")) - { - FileReturnCode frc; - if (fscanf(fp, "%s", fname) != 1) - { - (void)sprintf(errorbuffer, "Error reading filename in definition of worldobject %s", obj->name); - error(abort_action, errorbuffer); - return code_bad; - } -#if ! ENGINE - frc = lookup_polyhedron(obj->wobj, fname, ms); - - if (frc == file_missing) - { - (void)sprintf(mess, "Unable to locate object file %s", fname); - error(none, mess); - } - else if (frc == file_bad) - { - (void)sprintf(mess, "Unable to read object from file %s", fname); - error(none, mess); - } - else - { - /* simm_printf(no, "Read object file %s\n", fname); */ - //TODO5.0 shouldn't the filename be copied anyway, even if the file was not found? - } - mstrcpy(&obj->filename, fname); -#else - mstrcpy(&obj->filename, fname); -#endif - } - else if (STRINGS_ARE_EQUAL(buffer, "origin")) - { - if (fscanf(fp, "%lg %lg %lg", &translation[0], &translation[1], &translation[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading origin in definition of worldobject %s", obj->name); - error(none, errorbuffer); - error(none, "Using default value of 0.0 0.0 0.0"); - translation[0] = translation[1] = translation[2] = 0.0; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "scale")) - { - if (fscanf(fp, "%lg %lg %lg", &scale[0], &scale[1], &scale[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading scale factor in definition of worldobject %s", obj->name); - error(none, errorbuffer); - error(none, "Using default values of 1.0 1.0 1.0"); - scale[0] = scale[1] = scale[2] = 1.0; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "rotation")) - { - if (fscanf(fp, "%lg %lg %lg", &rotation[0], &rotation[1], &rotation[2]) != 3) - { - (void)sprintf(errorbuffer, "Error reading XYZ Euler rotations in definition of worldobject %s", obj->name); - error(none, errorbuffer); - error(none, "Using default values of 0.0 0.0 0.0"); - rotation[0] = rotation[1] = rotation[2] = 0.0; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "drawmode")) - { - if (fscanf(fp, "%s", buffer) != 1) - { - (void)sprintf(errorbuffer, "Error reading drawmode in definition of worldobject %s", obj->name); - error(none, errorbuffer); - error(none, "Using default value of solid_fill"); - } - if (STRINGS_ARE_EQUAL(buffer, "wireframe")) - obj->drawmode = wireframe; - else if (STRINGS_ARE_EQUAL(buffer, "solid_fill")) - obj->drawmode = solid_fill; - else if (STRINGS_ARE_EQUAL(buffer, "flat_shading")) - obj->drawmode = flat_shading; - else if (STRINGS_ARE_EQUAL(buffer, "gouraud_shading")) - obj->drawmode = gouraud_shading; - else if (STRINGS_ARE_EQUAL(buffer, "outlined_polygons")) - obj->drawmode = outlined_polygons; - else if (STRINGS_ARE_EQUAL(buffer, "bounding_box")) - { - (void)sprintf(errorbuffer, "Warning: bounding_box drawmode not supported for worldobjects (%s).", obj->name); - error(none, errorbuffer); - error(none, "Using solid_fill instead."); - obj->drawmode = solid_fill; - } - else if (STRINGS_ARE_EQUAL(buffer, "none")) - obj->drawmode = no_surface; - else - { - (void)sprintf(errorbuffer, "Unknown drawmode found for worldobject (%s).", obj->name); - error(none, errorbuffer); - error(none, "Using solid_fill instead."); - obj->drawmode = solid_fill; - } - } - else if (STRINGS_ARE_EQUAL(buffer, "material")) - { - if (fscanf(fp,"%s", fname) != 1) - { - (void)sprintf(errorbuffer, "Error reading material in definition of worldobject %s", obj->name); - error(none, errorbuffer); - error(none, "Using default value of def_bone"); - obj->material = enter_material(ms, "def_bone", just_checking_element); - } - else - { - obj->material = enter_material(ms,fname,declaring_element); - } - } - else if (STRINGS_ARE_EQUAL(buffer, "texture_file")) - { - FileReturnCode frc; - if (fscanf(fp, "%s", fname) != 1) - { - (void)sprintf(errorbuffer, "Error reading name of texture file in definition of worldobject %s", obj->name); - error(abort_action, errorbuffer); - return code_bad; - } - - mstrcpy(&obj->texture_filename, fname); - } - else if (STRINGS_ARE_EQUAL(buffer, "texture_coords_file")) - { - FileReturnCode frc; - if (fscanf(fp, "%s", fname) != 1) - { - (void)sprintf(errorbuffer, "Error reading name of texture coordinates file in definition of worldobject %s", obj->name); - error(abort_action, errorbuffer); - return code_bad; - } - - mstrcpy(&obj->texture_coords_filename, fname); - } - else - { - (void)sprintf(errorbuffer, "Unknown string (%s) in definition of object %s", buffer, obj->name); - error(recover, errorbuffer); - } - } - - identity_matrix(obj->transform); - - x_rotate_matrix_bodyfixed(obj->transform, rotation[0] * DTOR); - y_rotate_matrix_bodyfixed(obj->transform, rotation[1] * DTOR); - z_rotate_matrix_bodyfixed(obj->transform, rotation[2] * DTOR); - - for (i=0; i<3; i++) - for (j=0; j<3; j++) - obj->transform[i][j] *= scale[j]; - - obj->transform[3][0] = translation[0]; - obj->transform[3][1] = translation[1]; - obj->transform[3][2] = translation[2]; - -#if ! ENGINE - // If a texture coords file was specified, load it now. - // This must be done after the world object's polyhedron has been read in, because - // it stores the texture coordinates in the vertex structures. - if (obj->texture_coords_filename) - { - FileReturnCode frc = lookup_texture_coord_file(obj->wobj, obj->texture_coords_filename, ms); - - if (frc == file_missing) - { - (void)sprintf(mess, "Unable to locate texture coordinate file %s", obj->texture_coords_filename); - error(none, mess); - } - else if (frc == file_bad) - { - (void)sprintf(mess, "Unable to read texture coordinates from file %s", obj->texture_coords_filename); - error(none, mess); - } - else - { - //TODO5.0 how to indicate an error??? - } - } -#endif - - ms->numworldobjects++; - - return code_fine; -} - - -static void init_world_object(ModelStruct* ms, WorldObject* obj) -{ - obj->filename = NULL; - obj->texture_filename = NULL; - obj->texture_coords_filename = NULL; - identity_matrix(obj->transform); - obj->drawmode = solid_fill; - obj->selected = no; - obj->material = ms->dis.mat.default_world_object_material; - obj->drawmodemenu = 0; - -#if ENGINE - obj->wobj = NULL; -#else - obj->wobj = (PolyhedronStruct*)simm_malloc(sizeof(PolyhedronStruct)); - preread_init_polyhedron(obj->wobj); -#endif -} - diff --git a/OpenSim/Utilities/simmToOpenSim/wrapeditor.h b/OpenSim/Utilities/simmToOpenSim/wrapeditor.h deleted file mode 100644 index 53696e3b88..0000000000 --- a/OpenSim/Utilities/simmToOpenSim/wrapeditor.h +++ /dev/null @@ -1,134 +0,0 @@ -/******************************************************************************* - - WRAPEDITOR.H - - Author: Kenny Smith (based on plotmaker.h by Peter Loan) - - Date: 22-OCT-98 - - Copyright (c) 1992-8 MusculoGraphics, Inc. - All rights reserved. - Portions of this source code are copyrighted by MusculoGraphics, Inc. - -*******************************************************************************/ - -#ifndef WRAPEDITOR_H -#define WRAPEDITOR_H - -#define WE_WRAP_OBJECT 0 -#define WE_CHOOSE_SEGMENT 1 -#define WE_MUSCLEGROUPS 2 -#define WE_SAVE_ALL 3 -#define WE_RESTORE_ALL 4 -#define WE_DELETE_OBJECT 5 -#define WE_APPLY_POS_XFORM 6 -#define WE_APPLY_NEG_XFORM 7 -#define WE_CLEAR_XFORM 8 -#define WE_IDENTITY_XFORM 9 - -#define WE_OBJECT_NAME 0 -#define WE_RADIUS_X 1 -#define WE_RADIUS_Y 2 -#define WE_RADIUS_Z 3 -#define WE_TRANSLATE_X 4 -#define WE_TRANSLATE_Y 5 -#define WE_TRANSLATE_Z 6 -#define WE_ROTATE_X 7 -#define WE_ROTATE_Y 8 -#define WE_ROTATE_Z 9 - -#define WE_SPHERE 0 /* see note below! */ -#define WE_CYLINDER 1 -#define WE_ELLIPSOID 2 -#define WE_TORUS 3 -#define WE_NUM_WRAP_TYPE_RADIO_BTNS 4 - -/* NOTE: the radio-button enum above *must* match the dpWrapObjectType - * enum in modelplot.h!! - */ - -#define WE_X_QUADRANT 0 -#define WE_Y_QUADRANT 1 -#define WE_Z_QUADRANT 2 - -#define WE_POSITIVE_QUADRANT 0 -#define WE_NEGATIVE_QUADRANT 1 - -#define WE_ACTIVE_CHBOX 0 -#define WE_VISIBLE_CHBOX 1 -#define WE_SHOW_PTS_CHBOX 2 -#define WE_TRACKBALL_CHBOX 3 - -#define WE_LOCAL_FRAME_RBTN 0 -#define WE_PARENT_FRAME_RBTN 1 - -#define WE_SLIDER_WIDTH 16 -#define WE_MGROUP_YOFFSET 410 - -#define WE_PICK_WRAP_OBJECT_KEY 0 -#define WE_MOVE_WRAP_OBJECT_KEY 1 -#define WE_PAN_WRAP_OBJECT_KEY 2 -#define WE_ZOOM_WRAP_OBJECT_KEY 3 -#define WE_TRACKBALL_KEY 4 -#define WE_ROTATE_X_KEY 5 -#define WE_ROTATE_Y_KEY 6 -#define WE_ROTATE_Z_KEY 7 -#define WE_MAX_DEVS 8 - -enum { WE_LOCAL_FRAME, WE_PARENT_FRAME }; - -STRUCT { - int segment; /* parent segment of current wrapping object */ - dpWrapObject* wrap_object; /* index of current wrapping object */ - dpCoord3D translate; - dpCoord3D rotate; - int xform_frame; - SBoolean trackball_rotation; - MuscleMenu mgroup[GROUPBUFFER]; /* list of muscle groups */ - int menucolumns[COLUMNBUFFER]; /* used for placing menus in the window */ - int* musc; /* list of muscle numbers */ - int nummuscles; /* number of muscles currently selected */ -} WEModelOptions; /* user-selectable plotting options */ - -ENUM { - WE_TOP_LEVEL, /* */ - WE_ENTER_VALUE /* */ -} WEMODE; /* */ - -STRUCT { - ModelStruct* model; /* */ - PlotStruct* plot; /* */ - WEMODE current_mode; /* */ - int selected_item; /* */ - Menu optionsmenu; /* */ - Form optionsform; /* */ - CheckBoxPanel wrap_type_radiopanel; - CheckBoxPanel wrap_method_radiopanel; - CheckBoxPanel quadrant_radiopanel; - CheckBoxPanel quadrant_checkpanel; - CheckBoxPanel active_visible_checkpanel; - CheckBoxPanel transform_radiopanel; - WEModelOptions weop[MODELBUFFER]; /* */ - HelpStruct help; /* */ - int reference_number; /* */ - Slider win_slider; /* */ - int canvas_height; /* */ - int wrap_object_menu; - int numdevs; /* number of device numbers in devs[] */ - int devs[WE_MAX_DEVS]; /* button numbers to move bone vertices */ -} WrapEditorStruct; /* */ - -STRUCT { - Scene* scene; - ModelStruct* model; - double wx_old, wy_old, wz_old; - int mx_old, my_old; - double bpan_wx_old, bpan_wy_old, bpan_wz_old; - int bpan_mx_old, bpan_my_old; - double span_wx_old, span_wy_old, span_wz_old; - int span_mx_old, span_my_old; - int zoom_mx_old, zoom_my_old; - dpCoord3D zoom_vec; -} WrapEditorTracker; - -#endif /*WRAPEDITOR_H*/ diff --git a/Vendors/tropter/cmake/CMakeLists.txt b/Vendors/tropter/cmake/CMakeLists.txt index d494b33489..355093dcc3 100644 --- a/Vendors/tropter/cmake/CMakeLists.txt +++ b/Vendors/tropter/cmake/CMakeLists.txt @@ -21,9 +21,7 @@ if(TROPTER_COPY_DEPENDENCIES AND APPLE) ${IPOPT_LIBDIR}/libipopt.dylib ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.3.dylib ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.dylib - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.1.3.10.dylib - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.1.dylib - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.dylib + ${IPOPT_LIBDIR}/../../metis/lib/libmetis.dylib DESTINATION ${CMAKE_INSTALL_LIBDIR}) @@ -38,7 +36,7 @@ if(TROPTER_COPY_DEPENDENCIES AND APPLE) install(SCRIPT "${script}") elseif(TROPTER_COPY_DEPENDENCIES AND UNIX) - message(STATUS "Finding dependencies") + message(STATUS "Finding tropter dependencies") message(STATUS "getting dir for ${pkgcfg_lib_IPOPT_gfortran}" ) message(STATUS "ADOLC_DIR dir ${ADOLC_DIR}" ) message(STATUS "ColPack_ROOT_DIR dir ${ColPack_ROOT_DIR}" ) @@ -64,9 +62,7 @@ elseif(TROPTER_COPY_DEPENDENCIES AND UNIX) ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so.3.0.5 ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so.3 ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.so.1.3.10 - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.so.1 - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.so + ${IPOPT_LIBDIR}/../../metis/lib/libmetis.so ${gfortran} ${quadmath} diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index b96e8d128e..7ecea1ffa0 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -320,14 +320,15 @@ else() # operators like && is not defined." # Patch the scripts that download Metis and MUMPS to use our # Sourceforge mirror. The original links are not reliable. + message(STATUS "Building Metis...") + ExternalProject_Add(metis - GIT_REPOSITORY https://github.com/coin-or-tools/ThirdParty-Metis.git - GIT_TAG releases/1.3.10 - PATCH_COMMAND cd && ./get.Metis - CONFIGURE_COMMAND /configure --prefix= ADD_FFLAGS=-fallow-argument-mismatch ADD_CFLAGS=-Wno-error=implicit-function-declaration - BUILD_COMMAND ${CMAKE_MAKE_PROGRAM} ${BUILD_FLAGS} + # URL http://glaros.dtc.umn.edu/gkhome/fetch/sw/metis/metis-5.1.0.tar.gz + URL https://sourceforge.net/projects/myosin/files/metis-5.1.0.tar.gz/download + CONFIGURE_COMMAND cd && ${CMAKE_MAKE_PROGRAM} shared=1 config "prefix=${CMAKE_INSTALL_PREFIX}/metis" BUILDDIR=bdir + BUILD_COMMAND cd /bdir && ${CMAKE_MAKE_PROGRAM} INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/metis" - INSTALL_COMMAND ${CMAKE_MAKE_PROGRAM} install) + INSTALL_COMMAND cd /bdir && ${CMAKE_MAKE_PROGRAM} install) # GCC 10 generates a warning when compling MUMPS that we must suppress using # -fallow-argument-mismatch. diff --git a/dependencies/get.Metis.patch b/dependencies/get.Metis.patch deleted file mode 100644 index bfcfa5498e..0000000000 --- a/dependencies/get.Metis.patch +++ /dev/null @@ -1,13 +0,0 @@ ---- get.Metis 2012-05-29 03:55:14.000000000 -0700 -+++ get.Metis 2019-11-19 09:55:59.000000000 -0800 -@@ -22,8 +22,8 @@ - echo " " - - rm -f metis-4.0.3.tar.gz --echo "Downloading the source code from glaros.dtc.umn.edu..." --$wgetcmd http://glaros.dtc.umn.edu/gkhome/fetch/sw/metis/OLD/metis-4.0.3.tar.gz -+echo "Downloading the source code..." -+$wgetcmd https://sourceforge.net/projects/myosin/files/ipopt/metis-4.0.3.tar.gz - - rm -rf metis-4.0 - diff --git a/doc/Moco/MocoStudy.dox b/doc/Moco/MocoStudy.dox index 4fd8ab2c81..4524406fad 100644 --- a/doc/Moco/MocoStudy.dox +++ b/doc/Moco/MocoStudy.dox @@ -159,6 +159,32 @@ A MocoSolver attempts to use an optimal control method to solve the MocoProblem, and exposes settings you can use to control the transcription to a generic optimization problem and the optimization process. +@subsection transcriptionschemes Transcription schemes + +Moco supports multiple transcription schemes which can be specified +via the `transcription_scheme` property when configuring a MocoSolver. +For example, here's how to specify the trapezoidal transcription scheme: + +@code{.cpp} +MocoCasADiSolver& solver = moco.initCasADiSolver(); +solver.set_transcription_scheme("trapezoidal"); +@endcode + +The transcription schemes supported by Moco are summarized in the table below: + +| Scheme | `transcription_scheme` setting | Theory | +|----------------------|--------------------------------|-----------------------------------| +| trapezoidal | "trapezoidal" | @ref mocotraptheory | +| Hermite-Simpson | "hermite-simpson" | @ref mocohermitesimpsontheory | +| Legendre-Gauss | "legendre-gauss-#" | @ref mocolegendregausstheory | +| Legendre-Gauss-Radau | "legendre-gauss-radau-#" | @ref mocolegendregaussradautheory | + +For the Legendre-Gauss and Legendre-Gauss-Radau transcription schemes, the `#` +in the setting string should be replaced by a number between [1, 9], which +represents the number of collocation points per mesh interval (this is also +equal to the degree of the interpolating Lagrange polynomials used in these +schemes). + @subsection implicitmultibody Implicit multibody dynamics mode MocoSolvers support two modes for expressing differential equations for @@ -189,40 +215,79 @@ solver.set_enforce_constraint_derivatives(true); solver.set_lagrange_multiplier_weight(10); @endcode -If constraint derivatives are enforced, velocity correction variables are added -to the problem according to the method described in Posa, Kuindersma, and -Tedrake, 2016, "Optimization and stabilization of trajectories for constrained -dynamical systems." - -@note Enforcing kinematic constraints is only supported with Hermite-Simpson -transcription, since this scheme is necessary for implementing the method -described in Posa et al. 2016. See @ref mocohermitesimpsontheory for a -mathematical description. - -It is possible to modify the default bounds on these velocity correction -variables: +Moco supports two methods for enforcing kinematic constraints based on the +following publications: +- Posa, Kuindersma, and Tedrake, 2016, "Optimization and stabilization of + trajectories for constrained dynamical systems," and the +- Bordalba, Schoels, Ros, Porta, and Diehl, 2023, "Direct Collocation Methods + for Trajectory Optimization in Constraint Robotic Systems." + +Hermite-Simpson is the only transcription scheme supported when enforcing +kinematic constraints with the method by Posa et al. 2016. See @ref +mocohermitesimpsontheory for a mathematical description. The kinematic +constraint method by Bordalba et al. 2023 is only supported when using +MocoCasADiSolver and requires that constraint derivatives are enforced. +See @ref mocolegendregausstheory for a mathematical description. Note that +the Bordalba et al. 2023 method is valid for all transcription schemes, +not just Legendre-Gauss collocation. The implementation for each method +has been slighly modified from the original publications to be compatible +with Simbody; we refer to the mathematical descriptions linked above which +describe the implementations used by Moco. + +When constraint derivatives are enforced, slack variables are added to the +problem for projecting the model coordinates back onto the constraint manifold. +These variables also prevent the problem from being overconstrained. The Posa et +al. 2016 method adds a set of "velocity correction" variables that are applied +to coordinate state derivatives at the midpoint collocation point in the +Hermite-Simpson method. The Bordalba et al. 2023 method uses slack variables in +a set of constraints that enforce the coordinate states to be an orthogonal +projection of the states trajectory obtained from enforcing the defect constraints. + +The default method is the method by Posa et al. 2016 since this was originally +the only kinematic constraint method supported in Moco. However, we recommend +using the method by Bordalba et al. 2023 since it is agnostic to the transcription +scheme and does not require modifying the state derivative values as in the +Posa et al. 2016 method. + +It is possible to modify the default bounds on the slack variables used by both +methods. For the velocity correction variables in the Posa et al. 2016 method: @code{.cpp} // Default bounds: [-0.1, 0.1] solver.set_velocity_correction_bounds({-0.25, 0.25}); @endcode +For the slack variables in the Bordalba et al. 2023 method: + +@code{.cpp} +const MocoCasADiSolver& solver = moco.initCasADiSolver(); +// Default bounds: [-1e-3, 1e-3] +solver.set_projection_slack_variable_bounds({-0.05, 0.05}); +@endcode + +The slack variables in both methods are multiplied by model's kinematic +constraint Jacobian to provide a small projection back onto the constraint +manifold. Therefore, the slack values should be very small in a converged +solution, so in most cases the default bounds should be sufficient. + If constraint derivatives are not enforced or if the multibody constraint Jacobian is rank-deficient, it is recommended that the Lagrange multiplier minimization term is added to the problem to impose uniqueness for these variables. -Even though OpenSim Models can contain non-holonomic constraints (that is, -\f$ \nu(q, u, p) = 0 \f$), Moco solvers support only holonomic constraints; that -is, \f$ \phi(q) = 0 \f$. +When using the method by Posa et al. 2016, we only support enforcing holonomic +constraints; that is \f$ \phi(q, p) = 0 \f$. The method by Bordalba et al. 2023 +also supports both non-holonomic constraints (that is, \f$ \nu(q, u, p) = 0 \f$) +and acceleration-only constraints (that is, \f$ \alpha(q, u, \dot u, p) = 0 \f$), +but these constraints are less commonly used in OpenSim models. -By default, the controls at mesh interval midpoints are constrained by linear -interpolation of control mesh endpoint values and is the recommended setting. +By default, the controls within the mesh interval interior are constrained by +linear interpolation of control mesh endpoint values and is the recommended setting. However, you may disable this behavior in the solver: \code{.cpp} MocoCasADiSolver& solver = moco.initCasADiSolver(); -solver.set_interpolate_control_midpoints(false); +solver.set_interpolate_control_mesh_interior_points(false); \endcode @section casadivstropter MocoCasADiSolver versus MocoTropterSolver diff --git a/doc/Moco/MocoTheoryGuide.dox b/doc/Moco/MocoTheoryGuide.dox index c171f70fbd..8d7ca34c28 100644 --- a/doc/Moco/MocoTheoryGuide.dox +++ b/doc/Moco/MocoTheoryGuide.dox @@ -152,6 +152,15 @@ see the following excellent material: https://doi.org/10.1137/16M1062569 - Textbook: Betts, J. T. (2010). Practical methods for optimal control and estimation using nonlinear programming (Vol. 19). Siam. + - Journal article: Bordalba, R., Schoels, T., Ros, L., Porta J. M., and + Diehl, M. (2023). Direct collocation methods for trajectory optimization + in constrained robotic systems. IEEE Transations on Robotics, 39(1), 183-202. + https://doi.org/10.1109/TRO.2022.3193776 + - Thesis: Benson, A. (2005). A Gauss pseudospectral transcription for optimal + control. PhD thesis, Massachusetts Institute of Technology. + - Thesis: Huntington, G.T. (2007). Advancement and analysis of a Gauss + pseudospectral transcription for optimal control. PhD thesis, Massachusetts + Institute of Technology. @subsection mocotraptheory Trapezoidal transcription @@ -170,7 +179,7 @@ We use the trapezoidal rule to remove integrals and derivatives from the original continuous time problem, \f[ - \textrm{trap}_i(F(\eta, p)) = \frac{1}{2} h_i (F(\eta_{i-1}, p) + F(\eta_i, p)) + \textrm{trap}_i(F(t, \eta, p)) = \frac{1}{2} h_i (F(t_{i-1}, \eta_{i-1}, p) + F(t_i, \eta_i, p)) \f] where \f$ \eta \f$ is any continuous variable in the optimal control problem @@ -197,29 +206,32 @@ finite-dimensional nonlinear optimization problem (or nonlinear program, abbrevi & \quad\quad S_{b,k} = \sum_{i=1}^{n} \textrm{trap}_i(s_{b,k}(t, y, x, \lambda, p)) && k = 1, \ldots, K \\ & g_{L} \leq g(t_i, y_i, x_{i}, \lambda_i, p) \leq g_{U} && i = 0, \ldots, n\\ \mbox{with respect to} \quad - & t_0 \in [t_{0,L}, t_{0,U}] && t_n \in [t_{f,L}, t_{f,U}] \\ - & y_0 \in [y_{0,L}, y_{0,U}] && y_n \in [y_{f,L}, y_{f,U}] \\ - & y_i \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1\\ + & t_0 \in [t_{0,L}, t_{0,U}] \\ + & t_n \in [t_{f,L}, t_{f,U}] \\ + & y_0 \in [y_{0,L}, y_{0,U}] \\ + & y_n \in [y_{f,L}, y_{f,U}] \\ + & y_i \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1 \\ & \zeta_i \in [\zeta_{L}, \zeta_{U}] && i = 0, \ldots, n \\ - & x_0 \in [x_{0,L}, x_{0,U}] && x_n \in [x_{f,L}, x_{f,U}] \\ - & x_i \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1\\ + & x_0 \in [x_{0,L}, x_{0,U}] \\ + & x_n \in [x_{f,L}, x_{f,U}] \\ + & x_i \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1 \\ & \lambda_i \in [\lambda_L, \lambda_U] && i = 0, \ldots, n \\ & p \in [p_{L}, p_{U}]. \end{aligned} \end{align} \f] -Above, the multibody dynamics \f$ f_{\textrm{mb}} \f$ are expressed as explicit +Above, the multibody dynamics \f$ f_{\dot{u}} \f$ are expressed as explicit differential equations: \f[ - f_{\textrm{mb}}(t, y, x, \lambda, p) = + f_{\dot{u}}(t, y, x, \lambda, p) = M(q, p)^{-1}\big[(f_{\textrm{app}}(t, y, x, p) - f_{\textrm{inertial}}(q, u, p) - G(q, p)^T \lambda\big] \f] If the multibody dynamics are enforced as implicit differential equations, then -the constraint involving \f$ f_\textrm{mb} \f$ is replaced with the following: +the constraint involving \f$ f_\dot{u} \f$ is replaced with the following: \f[ \begin{alignat*}{2} @@ -265,8 +277,8 @@ with the Hermite interpolant and Simpson integration rule: \f[ \begin{align} - \textrm{hermite}_i(\eta, F(\eta, p)) &= \frac{1}{2} (\eta_{i-1} + \eta_i) + \frac{h_i}{8} (F(\eta_{i-1}, p) - F(\eta_i, p)) \\ - \textrm{simpson}_i(F(\eta, p)) &= \frac{h_i}{6} (F(\eta_{i-1}, p) + 4 F(\bar{\eta}_i, p) + F(\eta_i, p)) + \textrm{hermite}_i(\eta, F(t, \eta, p)) &= \frac{1}{2} (\eta_{i-1} + \eta_i) + \frac{h_i}{8} (F(t_{i-1}, \eta_{i-1}, p) - F(t_i, \eta_i, p)) \\ + \textrm{simpson}_i(F(t, \eta, p)) &= \frac{h_i}{6} (F(t_{i-1}, \eta_{i-1}, p) + 4 F(\bar{t}_i, \bar{\eta}_i, p) + F(t_i, \eta_i, p)) \end{align} \f] @@ -284,7 +296,7 @@ The resulting finite-dimensional NLP is as follows: + w_{\lambda} \sum_{i=1}^{n} \textrm{simpson}_i(\|\lambda\|_2^2) \\ & \quad\quad S_{c,j} = \sum_{i=1}^{n} \textrm{simpson}_i(s_{c,j}(t, y, x, \lambda, p)) \\ \mbox{subject to} \quad - & \bar{q}_i = \textrm{hermite}_i(q, u) + G(\bar{q}_i, p)^T \bar{\gamma}_i && i = 1, \ldots, n \\ + & \bar{q}_i = \textrm{hermite}_i(q, u) + P(\bar{q}_i, p)^T \bar{\gamma}_i && i = 1, \ldots, n \\ & q_i = q_{i-1} + \textrm{simpson}_i(u) && i = 1, \ldots, n \\ & \bar{u}_i = \textrm{hermite}_i(u, f_{\dot{u}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ & u_i = u_{i-1} + \textrm{simpson}_i(f_{\dot{u}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ @@ -296,18 +308,21 @@ The resulting finite-dimensional NLP is as follows: & \bar{x}_i = (x_{i-1} + x_i)/2 && i = 1, \ldots, n \\ & 0 = \phi(q_i, p) && i = 0, \ldots, n\\ & 0 = \dot{\phi}(q_i, u_i, p) && i = 0, \ldots, n\\ - & 0 = \ddot{\phi}(t_i, y_i, x_i, \lambda_i, p) && i = 0, \ldots, n\\ + & 0 = \ddot{\phi}(q_i, u_i, x_i, \lambda_i, p) && i = 0, \ldots, n\\ & V_{L,k} \leq V_k(t_0, t_f, y_0, y_f, x_{0}, x_{f}, \lambda_0, \lambda_f, p, S_{b,k}) \leq V_{U,k} \\ & \quad\quad S_{b,k} = \sum_{i=1}^{n} \textrm{simpson}_i(s_{b,k}(t, y, x, \lambda, p)) && k = 1, \ldots, K \\ & g_{L} \leq g(t_i, y_i, x_{i}, \lambda_i, p) \leq g_{U} && i = 0, \ldots, n\\ \mbox{with respect to} \quad - & t_0 \in [t_{0,L}, t_{0,U}] && t_n \in [t_{f,L}, t_{f,U}] \\ - & y_0 \in [y_{0,L}, y_{0,U}] && y_n \in [y_{f,L}, y_{f,U}] \\ + & t_0 \in [t_{0,L}, t_{0,U}] \\ + & t_n \in [t_{f,L}, t_{f,U}] \\ + & y_0 \in [y_{0,L}, y_{0,U}] \\ + & y_n \in [y_{f,L}, y_{f,U}] \\ & y_i \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1 \\ & \bar{y}_i \in [y_{L}, y_{U}] && i = 1, \ldots, n \\ & \zeta_i \in [\zeta_{L}, \zeta_{U}] && i = 0, \ldots, n \\ & \bar{\zeta}_i \in [\zeta_{L}, \zeta_{U}] && i = 1, \ldots, n \\ - & x_0 \in [x_{0,L}, x_{0,U}] && x_n \in [x_{f,L}, x_{f,U}] \\ + & x_0 \in [x_{0,L}, x_{0,U}] \\ + & x_n \in [x_{f,L}, x_{f,U}] \\ & x_i \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1 \\ & \bar{x}_i \in [x_{L}, x_{U}] && i = 1, \ldots, n \\ & \lambda_i \in [\lambda_L, \lambda_U] && i = 0, \ldots, n \\ @@ -318,34 +333,34 @@ The resulting finite-dimensional NLP is as follows: \end{align} \f] -The explicit multibody dynamics function \f$ f_{\textrm{mb}} \f$ is the same as +The explicit multibody dynamics function \f$ f_{\dot{u}} \f$ is the same as in Trapezoidal transcription. -The \f$ G(\bar{q}_i, p)^T \bar{\gamma}_i \f$ term -represents a velocity correction that is necessary to impose when enforcing -derivatives of kinematic constraints in the NLP. The additional variables -\f$ \bar{\gamma}_i \f$ help avoid problems becoming overconstrained, and the -kinematic Jacobian transpose operator, \f$ G(\bar{q}_i, p)^T \f$, restricts the -velocity correction to the tangent plane of the constraint manifold (see Posa et -al. 2016 described in @ref kincon for more details). Currently, we only support -enforcing derivatives of position-level, or holonomic, constraints, represented -by the equations: +This formulation includes the equations needed to enforce kinematic constraints based +on the method from Posa et al. (2016) (see @ref kincon for more details). The +\f$ P(\bar{q}_i, p)^T \bar{\gamma}_i \f$ term represents a velocity correction that +is necessary to impose when enforcing derivatives of kinematic constraints in the NLP. +The additional variables \f$ \bar{\gamma}_i \f$ help avoid problems becoming +overconstrained, and the kinematic Jacobian transpose operator, \f$ P(\bar{q}_i, p)^T \f$, +restricts the velocity correction to the tangent plane of the constraint manifold. +When using the Posa et al. (2016) method, we only support enforcing derivatives of +position-level, or holonomic, constraints, represented by the equations: \f[ \begin{alignat*}{2} - & 0 = \dot{\phi}(q, u, p) = G(q, p) u\\ - & 0 = \ddot{\phi}(t, y, x, \lambda, p) = G(q, p) f_{\textrm{mb}}(t, y, x, \lambda, p) + \dot{G}(q, p) u \\ + & 0 = \dot{\phi}(q, u, p) = P(q, p) u - c(t, q, p)\\ + & 0 = \ddot{\phi}(q, u, x, \lambda, p) = P(q, p) f_{\dot{u}}(t, y, x, \lambda, p) - b_p(t, q, u, p) \\ \end{alignat*} \f] +Where \f$ P(q, p) \f$ is the Jacobian of the holonomic constraints, which is a submatrix +of the kinematic constraint Jacobian \f$ G(q, p) \f$. When only holonomic constraints are +present in the multibody system, \f$ G = P \f$. + The explicit multibody dynamics function is used here where \f$ \dot{u} \f$ would be if it were a continuous variable in the problem (as is in implicit mode, see below). -Even though OpenSim supports non-holonomic constraints (that is, -\f$ \nu(q, u, p) = 0 \f$), we only support holonomic constraints; that is, -\f$ \phi(q) = 0 \f$. - The implicit dynamics formulation is as follows: \f[ @@ -365,5 +380,282 @@ The implicit dynamics formulation is as follows: \end{alignat*} \f] +@subsection mocolegendregausstheory Legendre-Gauss transcription + +Legendre-Gauss transcription works by approximating the states, \f$ y(t) \f$, within a +mesh interval range \f$ t \in [t_i, t_{i+1}] \f$ using a set of Lagrange interpolating +polynomials of degree \f$ d \f$: + +\f[ + \begin{alignat*}{1} + \mathcal{L}(t, c_i) = y_{i,0} \cdot l_0(t - t_i) + \cdots + y_{i,d} \cdot l_d(t - t_i) + \end{alignat*} +\f] + +where \f$ l_k \f$ are the Lagrange polynomial basis functions and \f$ c_i \f$ is +the vector of interpolated state values which represent the polynomial coefficients: + +\f[ + \begin{alignat*}{1} + c_i = (y_{i,0}, \ldots, y_{i,d}) + \end{alignat*} +\f] + +These polynomials only depend on the time points \f$ t_{i,0}, \ldots, t_{i,d} \f$ and +take on the following values: + +\f[ + \begin{alignat*}{1} + l_j(t_{i,k} - t_i) = \left\{ + \begin{array}{ll} + 1, & \quad \textrm{if } j = k \\ + 0, & \quad \textrm{otherwise} + \end{array} + \right. + \end{alignat*} +\f] + +Therefore, the interpolating polynomial is equal to the discrete state values when +evaluated at the collocation points: + +\f[ + \begin{alignat*}{1} + \mathcal{L}(t_{i,k}, c_i) = y_{i,k} \quad \textrm{for } k = 0, \ldots, d + \end{alignat*} +\f] + +The polynomial at the initial time point of the mesh interval is simply the state variable +at the initial mesh point, \f$ y_{i,0} = y_i \f$. The remaining polynomial coefficients can +be determined by enforcing the \f$ d \f$ collocation constraints at the collocation points +\f$ t_{i,1}, \ldots, t_{i,d} \f$: + +\f[ + \begin{alignat*}{1} + \mathcal{\dot L}_i(t_{i,k}, c_i) = f(t_{i,k}, y_{i,k}, x_{i,k}, \lambda_{i,k}, p) \quad \textrm{for } k = 1, \ldots, d + \end{alignat*} +\f] + +The left-hand side can be formulated using the constant differentiation matrix, \f$ D \f$: + +\f[ + \begin{alignat*}{1} + \mathcal{\dot L}_i(t_{i,k}, c_i) = D(t_{i,0},\ldots,t_{i,d}) \cdot y_i + \end{alignat*} +\f] + +Finally, the end state of the mesh interval, \f$ y_{i+1} \f$, is given by: + +\f[ + \begin{alignat*}{1} + y_{i+1} = \mathcal{L}(t_{i+1}, c_i) + \end{alignat*} +\f] + +As in the previous transcription methods, we have \f$ n + 1 \f$ mesh points and +\f$ n \f$ mesh intervals. Legendre-Gauss transcription introduces \f$ d \f$ +collocation points in the mesh interval interior located at points based on the +roots of the \f$ d \f$-th order Legendre polynomial, \f$ \textrm{legendre_roots}(k) \f$. + +\f[ + \begin{alignat*}{1} + 0 &= \tau_0 < \tau_1 < \tau_2 < \ldots < \tau_i < \ldots < \tau_{n - 1} < \tau_n = 1 \\ + \tau_{i,k} &= \tau_i + (\tau_{i+1} - \tau_i) * \textrm{legendre_roots}(k) \quad \textrm{for } k = 1, \ldots, d \\ + t_i &= (t_f - t_0) \tau_i + t_0 \\ + t_{i,k} &= (t_f - t_0) \tau_{i,k} + t_0 \quad \textrm{for } k = 1, \ldots, d \\ + h_i &= (t_f - t_0)(\tau_i - \tau_{i-1}) \\ + \end{alignat*} +\f] + +The collocation constraint equations and endpoint state equation above can be described using the following defect constraint function: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss}_i(\eta_i, F(t_i, \eta_i, p)) = \begin{bmatrix} h_i * F(t_{i,1:d}, \eta_{i,1:d}, p) - D_i \cdot \eta_{i,0:d} \\ \eta_{i, d+1} - C_i \cdot \eta_{i, 0:d} \end{bmatrix} + \end{alignat*} +\f] + +where \f$ \eta \f$ is any continuous variable in the optimal control problem +(e.g., \f$ y \f$), \f$ F \f$ is any function, \f$ D_i \f$ is the differentiation matrix +described above, and \f$ C_i \f$ a matrix containing values to perform the polynomial +interpolation for \f$ \eta_{i, d+1} \f$. Note that the end state is coincident with time +point \f$ t_{i+1} \f$, but we use the the notation \f$ \eta_{i, d+1} \f$ to denote the end +state of the mesh interval. + +\f$ \eta_{i,0:d} \f$ and \f$ F(t_{i,1:d}, \eta_{i,1:d}, p) \f$ are matrix quantities +with row size based on the polynomial degree \f$ d \f$: + +\f[ + \begin{alignat*}{1} + \eta_{i,0:d} &= \begin{bmatrix} \eta_{i,0} \\ \vdots \\ \eta_{i,d} \end{bmatrix} \quad + F(t_{i,1:d}, \eta_{i,1:d}, p) &= \begin{bmatrix} F(t_{i,1}, \eta_{i,1}, p) \\ \vdots \\ F(t_{i,d}, \eta_{i,d}, p) \end{bmatrix} \quad + \end{alignat*} +\f] + +These are matrix quantities since \f$ \eta_{i,k} \f$ and \f$ F(t_{i,k}, \eta_{i,k}, p) \f$ are +row vectors containing the state variable values and their derivatives, respectively. + +Similarly, integrals are replaced using Gauss quadrature at the Legendre polynomial roots +with coefficients \f$ \omega_k \f$: + +\f[ + \begin{alignat*}{1} + \textrm{quadrature}_i(Q(\eta, p)) = \sum^d_{k=1} \omega_k * Q(\eta_{i,k}, p) + \end{alignat*} +\f] + +For a full description of how the Gauss quadrature coefficients are determined, we refer +the reader to the theses of Benson (2005) and Huntington (2007) mentioned above. + +The resulting finite-dimensional NLP is as follows: + +\f[ +\begin{align} + \begin{aligned} + \mbox{minimize} \quad + & \sum_j w_j J_{j}(t_0, t_f, y_0, y_n, x_{0}, x_{n}, \lambda_0, \lambda_n, p, S_{c,j}) + + w_{\lambda} \sum_{i=1}^{n} \textrm{quadrature}_i(\|\lambda\|_2^2) + + w_{proj} \sum_{i=1}^{n} (\|q_i^{\prime} - q_i\|_2^2 + \|u_i^{\prime} - u_i\|_2^2) \\ + & \quad\quad S_{c,j} = \sum_{i=1}^{n} \textrm{quadrature}_i(s_{c,j}(t, y, x, \lambda, p)) \\ + \mbox{subject to} \quad + & 0 = \textrm{legendre_gauss}^{\prime}_i(q, u) && i = 1, \ldots, n \\ + & 0 = \textrm{legendre_gauss}^{\prime}_i(u, f_{\dot{u}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ + & 0 = \textrm{legendre_gauss}_i(z_\textrm{ex}, f_{\dot{z},\textrm{ex}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\ + & 0 = \textrm{legendre_gauss}_i(z_\textrm{im}, \zeta) && i = 1, \ldots, n \\ + & 0 = f_{\dot{z},\textrm{im}}(t_i, y_i, \zeta_i, x_i, \lambda_i, p) && i = 0, \ldots, n \\ + & 0 = \phi(q_i, p) && i = 0, \ldots, n \\ + & 0 = \dot{\phi}(q_i, u_i, p) && i = 0, \ldots, n \\ + & 0 = \nu(q_i, u_i, p) && i = 0, \ldots, n \\ + & 0 = \ddot{\phi}(q_{i,k}, u_{i,k}, x_{i,k}, \lambda_{i,k}, p) && i = 0, \ldots, n && k = 0, \ldots, d \\ + & 0 = \dot{\nu}(q_{i,k}, u_{i,k}, x_{i,k}, \lambda_{i,k}, p) && i = 0, \ldots, n && k = 0, \ldots, d \\ + & 0 = \alpha(q_{i,k}, u_{i,k}, x_{i,k}, \lambda_{i,k}, p) && i = 0, \ldots, n && k = 0, \ldots, d \\ + & q_{i+1} = q^{\prime}_{i+1} + P_q(q_{i+1},p)^T \mu^{q}_i && i = 0, \ldots, n-1 \\ + & u_{i+1} = u^{\prime}_{i+1} + G_{pv}(q_{i+1},p)^T \mu^{u}_i && i = 0, \ldots, n-1 \\ + & V_{L,k} \leq V_k(t_0, t_f, y_0, y_f, x_{0}, x_{f}, \lambda_0, \lambda_f, p, S_{b,k}) \leq V_{U,k} \\ + & \quad\quad S_{b,k} = \sum_{i=1}^{n} \textrm{quadrature}_i(s_{b,k}(t, y, x, \lambda, p)) && k = 1, \ldots, K \\ + & g_{L} \leq g(t_i, y_i, x_{i}, \lambda_i, p) \leq g_{U} && i = 0, \ldots, n\\ + \mbox{with respect to} \quad + & t_0 \in [t_{0,L}, t_{0,U}] && \\ + & t_n \in [t_{f,L}, t_{f,U}] && \\ + & y_0 \in [y_{0,L}, y_{0,U}] && \\ + & y_n \in [y_{f,L}, y_{f,U}] && \\ + & y_{i,k} \in [y_{L}, y_{U}] && i = 1, \ldots, n - 1 && k = 0, \ldots, d \\ + & \zeta_{i,k} \in [\zeta_{L}, \zeta_{U}] && i = 1, \ldots, n - 1 && k = 0, \ldots, d \\ + & x_0 \in [x_{0,L}, x_{0,U}] && \\ + & x_n \in [x_{f,L}, x_{f,U}] && \\ + & x_{i,k} \in [x_{L}, x_{U}] && i = 1, \ldots, n - 1 && k = 0, \ldots, d \\ + & \lambda_{i,k} \in [\lambda_L, \lambda_U] && i = 0, \ldots, n && k = 0, \ldots, d \\ + & q^{\prime}_{i} \in [q^{\prime}_{L}, q^{\prime}_{U}] && i = 1, \ldots, n \\ + & u^{\prime}_{i} \in [u^{\prime}_{L}, u^{\prime}_{U}] && i = 1, \ldots, n \\ + & \mu^{q}_{i} \in [\mu^{q}_{L}, \mu^{q}_{U}] && i = 1, \ldots, n \\ + & \mu^{u}_{i} \in [\mu^{u}_{L}, \mu^{u}_{U}] && i = 1, \ldots, n \\ + & p \in [p_{L}, p_{U}]. + \end{aligned} +\end{align} +\f] + +This formulation includes the equations needed to enforce kinematic constraints based +on the method from Bordalba et al. (2023) (see @ref kincon for more details). Our +implementation of this method contains one major difference from the original method: +the projection equation (Eq. 35 in the original publication) is separated into the +equations \f$ q_{i+1} = q^{\prime}_{i+1} + P_q(q_{i+1},p)^T \mu^{q}_i \f$ and +\f$ u_{i+1} = u^{\prime}_{i+1} + G_{pv}(q_{i+1},p)^T \mu^{u}_i \f$. This separation is +necessary because Simbody does not (yet) provide submatrices of the matrix \f$ F_x \f$ +from the original method corresponding to the change in velocity-level constraints +with respect to the generalized coordinates (i.e., \f$ \frac{\partial \dot{\phi}}{\partial q} \f$ +and \f$ \frac{\partial \nu}{\partial u} \f$). However, the projection equations +implemented do restrict \f$ q_{i+1} \f$ to be the orthogonal projection of \f$ q^{\prime}_{i+1} \f$ +onto the manifold defined by \f$ \phi = 0 \f$, and restrict \f$ u_{i+1} \f$ to be the +orthogonal projection of \f$ u^{\prime}_{i+1} \f$ onto the manifold defined by +\f$ \dot{\phi} = 0 \f$ and \f$ \nu = 0 \f$. Furthermore, these projections are consistent +with how Simbody enforces constraints via coordinate projection in time-stepping simulations. +If the kinematic constraint equations are enforced and the slack variables \f$ \mu^{q}_i \f$ +and \f$ \mu^{u}_i \f$ are close to zero, then the difference between the true and projection +state variables will be small. + +The Bordalba et al. (2023) method supports holonomic, non-holonomic, and acceleration-only +constraints, but non-holonomic and acceleration-only constraints currently have limited +testing and support in Moco. All acceleration-level constraints (including the derivatives +of holonomic and non-holonomic constraints) are enforced at all collocation points, not +just the mesh points. The cost term preceded by \f$ w_{proj} \f$ is used to penalize the +difference between the true state variables and the projection state variables at the +end of each mesh interval to encourage the closest possible projection to the constraint +manifold. + +The function \f$ \textrm{legendre_gauss}^{\prime}(\eta, F(t, \eta, p)) \f$ used for the +multibody dynamics defect constraints is a slight modification of the function +\f$ \textrm{legendre_gauss} \f$: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss}^{\prime}_i(\eta, F(t, \eta, p)) = \begin{bmatrix} h_i * F(t_i, \eta_i, p) - D_i \cdot \eta_i \\ \eta^{\prime}_{i+1} - C_i \cdot \eta_i \end{bmatrix} + \end{alignat*} +\f] + +where \f$ \eta^{\prime}_{i+1} \f$ represents the "projection" state variables used in the +method from Bordalba et al. (2023). The true state variables \f$ \eta_{i+1} \f$ are the +projection of \f$ \eta^{\prime}_{i+1} \f$ onto the constraint manifold. The matrices +\f$ P_q(q_{i+1},p) \f$ and \f$ G_{pv}(q_{i+1},p) \f$ are used to perform the projection for +the coordinate values, \f$ q \f$, and coordinate speeds, \f$ u \f$, respectively. +\f$ G_{pv} = [P; V] \f$ contains submatrices of the full kinematic constraint Jacobian +(\f$ G = [P; V; A] \f$) for the holonomic, \f$ P \f$, and non-holonomic, \f$ V \f$, +constraints. \f$ Pq = P N^{-1} \f$, where \f$ N \f$ is a matrix relating the generalized +speeds to the generalized coordinate derivatives \f$ \dot q = N u \f$. Currently, Moco +requires that \f$ N = I \f$, but the Bordalba et al. (2023) method will be valid when +Moco supports joints where \f$ \dot q \neq u \f$, in the future. Finally, the vectors +\f$ \mu^{q}_i \f$ and \f$ \mu^{u}_i \f$ are arbitrary slack variables used by the +optimizer to scale the projection magnitude to reach the constraint manifold. + +The implicit dynamics formulation is as follows: + +\f[ + \begin{alignat*}{2} + \mbox{subject to} \quad + & 0 = \textrm{legendre_gauss}^{\prime}_i(u, \upsilon) & i = 1, \ldots, n \\ + & M(q_i, p)\upsilon_i + G(q_i, p)^T \lambda_i = + f_{\textrm{app}}(t_i, y_i, x_{i}, p) - + f_{\textrm{inertial}}(q_i, u_{i}, p) & \quad i = 0, \ldots, n \\ + \mbox{with respect to} \quad + & \upsilon_i \in [-\upsilon_{B}, \upsilon_{B}] & i = 0, \ldots, n \\ + \end{alignat*} +\f] + +@subsection mocolegendregaussradautheory Legendre-Gauss-Radau transcription + +Legendre-Gauss-Radau transcription is very similar to Legendre-Gauss transcription, +with the main difference being that the final collocation point lies at the mesh +interval endpoint rather than the interior of the mesh interval. Both methods have +have \f$ d \f$ collocation points corresponding to the degree of the interpolating +polynomial, but Legendre-Gauss-Radau transcription has one fewer grid point per mesh +interval compared to Legendre-Gauss transcription since the final collocation point +is at the mesh interval endpoint. + +As a result, the defect constraint function no longer needs to explictly include the +interpolation constraint for the final mesh interval state: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss_radau}_i(\eta_i, F(t_i, \eta_i, p)) = h_i * F(t_{i,1:d}, \eta_{i,1:d}, p) - D_i \cdot \eta_{i,0:d} + \end{alignat*} +\f] + +Replacing \f$ \textrm{legendre_gauss}_i \f$ in @ref mocolegendregausstheory above with +\f$ \textrm{legendre_gauss_radau}_i \f$ will yield the equations for Legendre-Gauss-Radau +transcription. The modified equation needed to enforce the kinematic constraints, +\f$ \textrm{legendre_gauss_radau}^{\prime}_i \f$, has the same form, but uses a modified +\f$ \eta^{\prime}_i \f$ matrix where the final row contains the projection state variables: + +\f[ + \begin{alignat*}{1} + \textrm{legendre_gauss_radau}^{\prime}_i(\eta^{\prime}_i, F(t_i, \eta^{\prime}_i, p)) = h_i * F(t_{i,1:d}, \eta^{\prime}_{i,1:d}, p) - D_i \cdot \eta^{\prime}_{i,0:d} + \end{alignat*} +\f] + +\f[ + \begin{alignat*}{1} + \eta^{\prime}_{i,0:d} &= \begin{bmatrix} \eta_{i,0} \\ \vdots \\ \eta^{\prime}_{i,d} \end{bmatrix} \quad + F(t_{i,1:d}, \eta^{\prime}_{i,1:d}, p) &= \begin{bmatrix} F(t_{i,1}, \eta_{i,1}, p) \\ \vdots \\ F(t_{i,d}, \eta^{\prime}_{i,d}, p) \end{bmatrix} \quad + \end{alignat*} +\f] + */ } // namespace OpenSim