#include "__cf_CMFB.h"
#include "pm_std.h"
#include "ne_std.h"
#include "ne_dae.h"
#include "pm_default_allocator.h"
#include "sm_ssci_NeDaePrivateData.h"
#include "sm_CTarget.h"
PmfMessageId sm_ssci_recordRunTimeError ( const char * errorId , const char *
errorMsg , NeuDiagnosticManager * mgr ) ;
#define pm_allocator_alloc(_allocator, _m, _n) ((_allocator)->mCallocFcn((_allocator), (_m), (_n)))
#define PM_ALLOCATE_ARRAY(_name, _type, _size, _allocator)\
 _name = (_type *) pm_allocator_alloc(_allocator, sizeof(_type), _size)
#define pm_size_to_int(_size)          ((int32_T) (_size))
PmIntVector * pm_create_int_vector ( size_t , PmAllocator * ) ; int_T
pm_create_int_vector_fields ( PmIntVector * , size_t , PmAllocator * ) ;
int_T pm_create_real_vector_fields ( PmRealVector * , size_t , PmAllocator *
) ; int_T pm_create_char_vector_fields ( PmCharVector * , size_t ,
PmAllocator * ) ; int_T pm_create_bool_vector_fields ( PmBoolVector * ,
size_t , PmAllocator * ) ; void pm_rv_equals_rv ( const PmRealVector * ,
const PmRealVector * ) ; void sm_ssci_setupLoggerFcn_codeGen ( const NeDae *
dae , NeLoggerBuilder * neLoggerBuilder ) ; int32_T sm_ssci_logFcn_codeGen (
const NeDae * dae , const NeSystemInput * systemInput , PmRealVector * output
) ; extern const NeAssertData CMFB_cde73719_3_assertData [ ] ; void
CMFB_cde73719_3_computeRuntimeParameters ( const double *
runtimeRootVariables , double * runtimeParameters ) ; void
CMFB_cde73719_3_validateRuntimeParameters ( const double * runtimeParameters
, int32_T * assertSatisfactionFlags ) ; void
CMFB_cde73719_3_computeAsmRuntimeDerivedValues ( const double *
runtimeParameters , double * runtimeDerivedValues ) ; void
CMFB_cde73719_3_computeSimRuntimeDerivedValues ( const double *
runtimeParameters , double * runtimeDerivedValues ) ; PmfMessageId
CMFB_cde73719_3_deriv ( const double * , const int * , const double * , const
double * , const double * , const double * , const double * , double * ,
double * , NeuDiagnosticManager * neDiagMgr ) ; PmfMessageId
CMFB_cde73719_3_checkDynamics ( const double * , const double * , const
double * , const double * , const double * , const double * , double * ,
NeuDiagnosticManager * neDiagMgr ) ; PmfMessageId CMFB_cde73719_3_outputDyn (
const double * , const int * , const double * , const double * , const double
* , const double * , const double * , double * , double * , int * , double *
, NeuDiagnosticManager * neDiagMgr ) ; PmfMessageId CMFB_cde73719_3_outputKin
( const double * , const double * , const double * , const double * , const
double * , const double * , double * , NeuDiagnosticManager * neDiagMgr ) ;
PmfMessageId CMFB_cde73719_3_output ( const double * , const int * , const
double * , const double * , const double * , const double * , const double *
, double * , double * , int * , double * , NeuDiagnosticManager * neDiagMgr )
; void CMFB_cde73719_3_checkTargets ( const double * runtimeDerivedValues ,
const double * stateVector ) ; void CMFB_cde73719_3_setTargets ( const double
* runtimeDerivedValues , CTarget * targets ) ; void
CMFB_cde73719_3_resetStateVector ( const void * mech , double * stateVector )
; void CMFB_cde73719_3_initializeTrackedAngleState ( const void * mech ,
const double * runtimeDerivedValues , const double * motionData , double *
stateVector , void * neDiagMgr ) ; void CMFB_cde73719_3_computeDiscreteState
( const void * mech , const double * runtimeDerivedValues , double *
stateVector ) ; void CMFB_cde73719_3_adjustPosition ( const void * mech ,
const double * dofDeltas , double * stateVector ) ; void
CMFB_cde73719_3_perturbState ( const void * mech , size_t stageIdx , size_t
primitiveIdx , double magnitude , boolean_T doPerturbVelocity , double *
stateVector ) ; void CMFB_cde73719_3_computeDofBlendMatrix ( const void *
mech , size_t stageIdx , size_t primitiveIdx , const double * stateVector ,
int partialType , double * matrix ) ; void
CMFB_cde73719_3_projectPartiallyTargetedPos ( const void * mech , size_t
stageIdx , size_t primitiveIdx , const double * origStateVector , int
partialType , double * stateVector ) ; void CMFB_cde73719_3_propagateMotion (
const void * mech , const double * runtimeDerivedValues , const double *
stateVector , double * motionData ) ; size_t
CMFB_cde73719_3_computeAssemblyError ( const void * mech , const double *
runtimeDerivedValues , size_t constraintIdx , const double * stateVector ,
const double * motionData , double * error ) ; size_t
CMFB_cde73719_3_computeAssemblyJacobian ( const void * mech , const double *
runtimeDerivedValues , size_t constraintIdx , boolean_T
forVelocitySatisfaction , const double * stateVector , const double *
motionData , double * J ) ; size_t
CMFB_cde73719_3_computeFullAssemblyJacobian ( const void * mech , const
double * runtimeDerivedValues , const double * stateVector , const double *
motionData , double * J ) ; int CMFB_cde73719_3_isInKinematicSingularity (
const void * mech , const double * runtimeDerivedValues , size_t
constraintIdx , const double * motionData ) ; PmfMessageId
CMFB_cde73719_3_convertStateVector ( const void * asmMech , const double *
asmRuntimeDerivedValues , const void * simMech , const double *
asmStateVector , double * simStateVector , void * neDiagMgr ) ; void
CMFB_cde73719_3_constructStateVector ( const void * mech , const double *
solverStateVector , const double * u , const double * uDot , const double *
discreteStateVector , double * fullStateVector ) ; void
CMFB_cde73719_3_extractSolverStateVector ( const void * mech , const double *
fullStateVector , double * solverStateVector ) ; int
CMFB_cde73719_3_isPositionViolation ( const void * mech , const double *
runtimeDerivedValues , const int * constraintEqnEnableFlags , const double *
stateVector ) ; int CMFB_cde73719_3_isVelocityViolation ( const void * mech ,
const double * runtimeDerivedValues , const int * constraintEqnEnableFlags ,
const double * stateVector ) ; PmfMessageId CMFB_cde73719_3_projectStateSim (
const void * mech , const double * runtimeDerivedValues , const int *
constraintEqnEnableFlags , const double * inputVector , double * stateVector
, void * neDiagMgr ) ; void CMFB_cde73719_3_computeConstraintError ( const
void * mech , const double * runtimeDerivedValues , const double *
stateVector , double * error ) ; PmfMessageId CMFB_cde73719_3_assemble (
const double * u , double * udot , double * x , NeuDiagnosticManager *
neDiagMgr ) { ( void ) x ; ( void ) u ; ( void ) udot ; ( void ) neDiagMgr ;
return NULL ; } static void dae_cg_setParameters_function ( const NeDae * dae
, const NeParameterBundle * paramBundle ) { const NeDaePrivateData * smData =
dae -> mPrivateData ; const double * runtimeRootVariables = paramBundle ->
mRealParameters . mX ; if ( smData -> mRuntimeParameterScalars . mN == 0 )
return ; CMFB_cde73719_3_computeRuntimeParameters ( runtimeRootVariables ,
smData -> mRuntimeParameterScalars . mX ) ;
CMFB_cde73719_3_computeAsmRuntimeDerivedValues ( smData ->
mRuntimeParameterScalars . mX , smData -> mAsmRuntimeDerivedValueScalars . mX
) ; CMFB_cde73719_3_computeSimRuntimeDerivedValues ( smData ->
mRuntimeParameterScalars . mX , smData -> mSimRuntimeDerivedValueScalars . mX
) ; sm_core_computeRedundantConstraintEquations ( & dae -> mPrivateData ->
mSimulationDelegate , smData -> mSimRuntimeDerivedValueScalars . mX ) ;
#if 0
{ size_t i ; const size_t n = smData -> mSimulationDelegate .
mRunTimeEnabledEquations . mSize ; pmf_printf (
"\nRuntime Enabled Equations (%lu)\n" , n ) ; for ( i = 0 ; i < n ; ++ i )
pmf_printf ( "  %2lu:  %d\n" , i , smData -> mSimulationDelegate .
mRunTimeEnabledEquations . mValues [ i ] ) ; }
#endif
} static PmfMessageId dae_cg_pAssert_method ( const NeDae * dae , const
NeSystemInput * systemInput , NeDaeMethodOutput * daeMethodOutput ,
NeuDiagnosticManager * neDiagMgr ) { const NeDaePrivateData * smData = dae ->
mPrivateData ; const double * runtimeParams = smData ->
mRuntimeParameterScalars . mX ; int32_T * assertSatisfactionFlags =
daeMethodOutput -> mPASSERT . mX ; ( void ) systemInput ; ( void ) neDiagMgr
; CMFB_cde73719_3_validateRuntimeParameters ( runtimeParams ,
assertSatisfactionFlags ) ; return NULL ; } static PmfMessageId
dae_cg_deriv_method ( const NeDae * dae , const NeSystemInput * systemInput ,
NeDaeMethodOutput * daeMethodOutput , NeuDiagnosticManager * neDiagMgr ) {
const NeDaePrivateData * smData = dae -> mPrivateData ; PmfMessageId errorId
= NULL ; double errorResult = 0.0 ; if ( smData ->
mCachedDerivativesAvailable ) memcpy ( daeMethodOutput -> mXP0 . mX , smData
-> mCachedDerivatives . mX , 12 * sizeof ( real_T ) ) ; else errorId =
CMFB_cde73719_3_deriv ( smData -> mSimRuntimeDerivedValueScalars . mX ,
smData -> mSimulationDelegate . mRunTimeEnabledEquations . mValues ,
systemInput -> mX . mX , systemInput -> mU . mX , systemInput -> mU . mX + 9
, systemInput -> mV . mX + 9 , systemInput -> mD . mX , daeMethodOutput ->
mXP0 . mX , & errorResult , neDiagMgr ) ; return errorId ; } static
PmfMessageId dae_cg_output_method ( const NeDae * dae , const NeSystemInput *
systemInput , NeDaeMethodOutput * daeMethodOutput , NeuDiagnosticManager *
neDiagMgr ) { PmfMessageId errorId = NULL ; NeDaePrivateData * smData = dae
-> mPrivateData ; if ( smData -> mDoComputeDynamicOutputs ) { int derivErr =
0 ; double errorResult = 0.0 ; errorId = CMFB_cde73719_3_outputDyn ( smData
-> mSimRuntimeDerivedValueScalars . mX , smData -> mSimulationDelegate .
mRunTimeEnabledEquations . mValues , systemInput -> mX . mX , systemInput ->
mU . mX , systemInput -> mU . mX + 9 , systemInput -> mV . mX + 9 ,
systemInput -> mD . mX , smData -> mCachedDerivatives . mX , daeMethodOutput
-> mY . mX , & derivErr , & errorResult , neDiagMgr ) ; smData ->
mCachedDerivativesAvailable = ( derivErr == 0 ) ; } else errorId =
CMFB_cde73719_3_outputKin ( smData -> mSimRuntimeDerivedValueScalars . mX ,
systemInput -> mX . mX , systemInput -> mU . mX , systemInput -> mU . mX + 9
, systemInput -> mV . mX + 9 , systemInput -> mD . mX , daeMethodOutput -> mY
. mX , neDiagMgr ) ; return errorId ; } static PmfMessageId
dae_cg_project_solve ( const NeDae * dae , const NeSystemInput * systemInput
, NeuDiagnosticManager * neDiagMgr ) { NeDaePrivateData * smData = dae ->
mPrivateData ; return sm_core_projectState ( false , & smData ->
mSimulationDelegate , smData -> mSimRuntimeDerivedValueScalars . mX ,
systemInput -> mU . mX , systemInput -> mU . mX + 9 , systemInput -> mD . mX
, systemInput -> mX . mX , neDiagMgr ) ; } static PmfMessageId
dae_cg_check_solve ( const NeDae * dae , const NeSystemInput * systemInput ,
NeuDiagnosticManager * neDiagMgr ) { NeDaePrivateData * smData = dae ->
mPrivateData ; PmfMessageId errorId = NULL ; if ( smData ->
mNumConstraintEqns > 0 ) errorId = sm_core_projectState ( false , & smData ->
mSimulationDelegate , smData -> mSimRuntimeDerivedValueScalars . mX ,
systemInput -> mU . mX , systemInput -> mU . mX + 9 , systemInput -> mD . mX
, systemInput -> mX . mX , neDiagMgr ) ; if ( errorId == NULL && smData ->
mDoCheckDynamics ) { double result = 0.0 ; errorId =
CMFB_cde73719_3_checkDynamics ( smData -> mSimRuntimeDerivedValueScalars . mX
, systemInput -> mX . mX , systemInput -> mU . mX , systemInput -> mU . mX +
9 , systemInput -> mV . mX + 9 , systemInput -> mD . mX , & result ,
neDiagMgr ) ; } return errorId ; } static PmfMessageId
dae_cg_projectMaybe_solve ( const NeDae * dae , const NeSystemInput *
systemInput , NeuDiagnosticManager * neDiagMgr ) { NeDaePrivateData * smData
= dae -> mPrivateData ; return sm_core_projectState ( true , & smData ->
mSimulationDelegate , smData -> mSimRuntimeDerivedValueScalars . mX ,
systemInput -> mU . mX , systemInput -> mU . mX + 9 , systemInput -> mD . mX
, systemInput -> mX . mX , neDiagMgr ) ; } static PmfMessageId
dae_cg_assemble_solve ( const NeDae * dae , const NeSystemInput * systemInput
, NeuDiagnosticManager * neDiagMgr ) { NeDaePrivateData * smData = dae ->
mPrivateData ; const SmMechanismDelegate * delegate = & smData ->
mAssemblyDelegate ; const double * runtimeDerivedValues = smData ->
mAsmRuntimeDerivedValueScalars . mX ; PmfMessageId errorId = NULL ; size_t i
; const size_t numTargets = 12 ; unsigned int asmStatus = 0 ; double *
assemblyFullStateVector = smData -> mAssemblyFullStateVector . mX ; double *
simulationFullStateVector = smData -> mSimulationFullStateVector . mX ; ( *
delegate -> mSetTargets ) ( runtimeDerivedValues , smData -> mTargets ) ; {
const double * u = systemInput -> mU . mX ; const double * uDot = u + smData
-> mInputVectorSize ; CTarget * target = smData -> mTargets + smData ->
mNumInternalTargets ; for ( i = 0 ; i < smData -> mNumInputMotionPrimitives ;
++ i ) { const size_t inputOffset = smData -> mMotionInputOffsets . mX [ i ]
; ( target ++ ) -> mValue [ 0 ] = u [ inputOffset ] ; ( target ++ ) -> mValue
[ 0 ] = uDot [ inputOffset ] ; } } errorId = sm_core_computeStateVector (
delegate , runtimeDerivedValues , numTargets , smData -> mTargets ,
assemblyFullStateVector , neDiagMgr ) ; if ( errorId != NULL ) return errorId
; asmStatus = sm_core_checkAssembly ( delegate , runtimeDerivedValues ,
numTargets , smData -> mTargets , assemblyFullStateVector , NULL , NULL ,
NULL ) ; if ( asmStatus != 1 ) { return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:AssemblyFailure" , asmStatus == 2 ?
 "Model not assembled due to a position violation. The failure occurred during the attempt to assemble all joints in the system and satisfy any motion inputs. If an Update Diagram operation completes successfully, the failure is likely caused by motion inputs. Consider adjusting the motion inputs to specify a different starting configuration. Also consider adjusting or adding joint targets to better guide the assembly."
: ( asmStatus == 3 ?
 "Model not assembled due to a velocity violation. The failure occurred during the attempt to assemble all joints in the system and satisfy any motion inputs. If an Update Diagram operation completes successfully, the failure is likely caused by motion inputs. Consider adjusting the motion inputs to specify a different starting configuration. Also consider adjusting or adding joint targets to better guide the assembly."
:
 "Model not assembled due to a singularity violation. The failure occurred during the attempt to assemble all joints in the system and satisfy any motion inputs. If an Update Diagram operation completes successfully, the failure is likely caused by motion inputs. Consider adjusting the motion inputs to specify a different starting configuration. Also consider adjusting or adding joint targets to better guide the assembly."
) , neDiagMgr ) ; }
#if 0
CMFB_cde73719_3_checkTargets ( smData -> mSimRuntimeDerivedValueScalars . mX
, assemblyFullStateVector ) ;
#endif
errorId = ( * delegate -> mConvertStateVector ) ( NULL , runtimeDerivedValues
, NULL , assemblyFullStateVector , simulationFullStateVector , neDiagMgr ) ;
for ( i = 0 ; i < smData -> mStateVectorSize ; ++ i ) systemInput -> mX . mX
[ i ] = simulationFullStateVector [ smData -> mStateVectorMap . mX [ i ] ] ;
memcpy ( systemInput -> mD . mX , simulationFullStateVector + smData ->
mFullStateVectorSize - smData -> mDiscreteStateSize , smData ->
mDiscreteStateSize * sizeof ( double ) ) ; return errorId ; } typedef struct
{ size_t first ; size_t second ; } SizePair ; static void checkMemAllocStatus
( int_T status ) { ( void ) status ; } static PmCharVector
cStringToCharVector ( const char * src ) { const size_t n = strlen ( src ) ;
PmCharVector charVect ; const int_T status = pm_create_char_vector_fields ( &
charVect , n + 1 , pm_default_allocator ( ) ) ; checkMemAllocStatus ( status
) ; strcpy ( charVect . mX , src ) ; return charVect ; } static void
initBasicAttributes ( NeDaePrivateData * smData ) { size_t i ; smData ->
mStateVectorSize = 12 ; smData -> mFullStateVectorSize = 12 ; smData ->
mDiscreteStateSize = 0 ; smData -> mInputVectorSize = 9 ; smData ->
mOutputVectorSize = 23 ; smData -> mNumConstraintEqns = 0 ; smData ->
mDoCheckDynamics = true ; for ( i = 0 ; i < 4 ; ++ i ) smData -> mChecksum [
i ] = 0 ; } static void initStateVector ( NeDaePrivateData * smData ) {
PmAllocator * alloc = pm_default_allocator ( ) ; const int32_T stateVectorMap
[ 12 ] = { 0 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 , 9 , 10 , 11 } ; const CTarget
targets [ 12 ] = { { 0 , 16 , 0 , false , 0 , 0 , "1" , false , false , +
1.000000000000000000e+00 , true , 1 , { + 0.000000000000000000e+00 , +
0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 } , { + 0.000000000000000000e+00 } } , { 0 , 16 , 0
, false , 0 , 0 , "1" , true , false , + 1.000000000000000000e+00 , true , 1
, { + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 , + 0.000000000000000000e+00 } , { +
0.000000000000000000e+00 } } , { 0 , 16 , 1 , false , 0 , 0 , "1" , false ,
false , + 1.000000000000000000e+00 , true , 1 , { + 0.000000000000000000e+00
, + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 } , { + 0.000000000000000000e+00 } } , { 0 , 16 , 1
, false , 0 , 0 , "1" , true , false , + 1.000000000000000000e+00 , true , 1
, { + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 , + 0.000000000000000000e+00 } , { +
0.000000000000000000e+00 } } , { 0 , 16 , 2 , false , 0 , 0 , "1" , false ,
true , + 1.000000000000000000e+00 , true , 1 , { + 0.000000000000000000e+00 ,
+ 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 } , { + 0.000000000000000000e+00 } } , { 0 , 16 , 2
, false , 0 , 0 , "1" , true , true , + 1.000000000000000000e+00 , true , 1 ,
{ + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 , + 0.000000000000000000e+00 } , { +
0.000000000000000000e+00 } } , { 0 , 43 , 0 , false , 0 , 2 , "m" , false ,
false , + 1.000000000000000000e+00 , true , 1 , { + 1.400000000000000029e-02
, + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 } , { + 0.000000000000000000e+00 } } , { 0 , 43 , 0
, false , 0 , 0 , "1" , true , false , + 1.000000000000000000e+00 , true , 1
, { + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 , + 0.000000000000000000e+00 } , { +
0.000000000000000000e+00 } } , { 0 , 43 , 1 , false , 0 , 0 , "1" , false ,
false , + 1.000000000000000000e+00 , true , 1 , { + 0.000000000000000000e+00
, + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 } , { + 0.000000000000000000e+00 } } , { 0 , 43 , 1
, false , 0 , 0 , "1" , true , false , + 1.000000000000000000e+00 , true , 1
, { + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 , + 0.000000000000000000e+00 } , { +
0.000000000000000000e+00 } } , { 0 , 43 , 2 , false , 0 , 0 , "1" , false ,
true , + 1.000000000000000000e+00 , true , 1 , { + 0.000000000000000000e+00 ,
+ 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 } , { + 0.000000000000000000e+00 } } , { 0 , 43 , 2
, false , 0 , 0 , "1" , true , true , + 1.000000000000000000e+00 , true , 1 ,
{ + 0.000000000000000000e+00 , + 0.000000000000000000e+00 , +
0.000000000000000000e+00 , + 0.000000000000000000e+00 } , { +
0.000000000000000000e+00 } } } ; const size_t numTargets = 12 ; int_T status
; size_t i ; status = pm_create_real_vector_fields ( & smData ->
mAssemblyFullStateVector , 12 , alloc ) ; checkMemAllocStatus ( status ) ;
status = pm_create_real_vector_fields ( & smData ->
mSimulationFullStateVector , 12 , alloc ) ; checkMemAllocStatus ( status ) ;
status = pm_create_int_vector_fields ( & smData -> mStateVectorMap , smData
-> mStateVectorSize , alloc ) ; checkMemAllocStatus ( status ) ; memcpy (
smData -> mStateVectorMap . mX , stateVectorMap , smData -> mStateVectorSize
* sizeof ( int32_T ) ) ; smData -> mNumInternalTargets = 12 ; smData ->
mNumInputMotionPrimitives = 0 ; PM_ALLOCATE_ARRAY ( smData -> mTargets ,
CTarget , numTargets , alloc ) ; for ( i = 0 ; i < numTargets ; ++ i )
sm_compiler_CTarget_copy ( targets + i , smData -> mTargets + i ) ; } static
void initAsserts ( NeDaePrivateData * smData ) { PmAllocator * alloc =
pm_default_allocator ( ) ; int_T status = 0 ; smData -> mNumParamAsserts = 0
; smData -> mParamAssertObjects = NULL ; smData -> mParamAssertPaths = NULL ;
smData -> mParamAssertDescriptors = NULL ; smData -> mParamAssertMessages =
NULL ; smData -> mParamAssertMessageIds = NULL ; status =
pm_create_bool_vector_fields ( & smData -> mParamAssertIsWarnings , smData ->
mNumParamAsserts , alloc ) ; checkMemAllocStatus ( status ) ; if ( smData ->
mNumParamAsserts > 0 ) { const NeAssertData * ad = CMFB_cde73719_3_assertData
; size_t i ; PM_ALLOCATE_ARRAY ( smData -> mParamAssertObjects , PmCharVector
, 0 , alloc ) ; PM_ALLOCATE_ARRAY ( smData -> mParamAssertPaths ,
PmCharVector , 0 , alloc ) ; PM_ALLOCATE_ARRAY ( smData ->
mParamAssertDescriptors , PmCharVector , 0 , alloc ) ; PM_ALLOCATE_ARRAY (
smData -> mParamAssertMessages , PmCharVector , 0 , alloc ) ;
PM_ALLOCATE_ARRAY ( smData -> mParamAssertMessageIds , PmCharVector , 0 ,
alloc ) ; for ( i = 0 ; i < smData -> mNumParamAsserts ; ++ i , ++ ad ) {
smData -> mParamAssertObjects [ i ] = cStringToCharVector ( ad -> mObject ) ;
smData -> mParamAssertPaths [ i ] = cStringToCharVector ( ad -> mPath ) ;
smData -> mParamAssertDescriptors [ i ] = cStringToCharVector ( ad ->
mDescriptor ) ; smData -> mParamAssertMessages [ i ] = cStringToCharVector (
ad -> mMessage ) ; smData -> mParamAssertMessageIds [ i ] =
cStringToCharVector ( ad -> mMessageID ) ; smData -> mParamAssertIsWarnings .
mX [ i ] = ad -> mIsWarn ; } } } static void initVariables ( NeDaePrivateData
* smData ) { const char * varFullPaths [ 12 ] = {
"Muscle_Finger_Button_Event.Button_spring_and_damper.Px.p" ,
"Muscle_Finger_Button_Event.Button_spring_and_damper.Py.p" ,
"Muscle_Finger_Button_Event.Button_spring_and_damper.Rz.q" ,
"Muscle_Finger_Button_Event.Button_spring_and_damper.Px.v" ,
"Muscle_Finger_Button_Event.Button_spring_and_damper.Py.v" ,
"Muscle_Finger_Button_Event.Button_spring_and_damper.Rz.w" ,
"Muscle_Finger_Button_Event.Forceless_planar_joint.Px.p" ,
"Muscle_Finger_Button_Event.Forceless_planar_joint.Py.p" ,
"Muscle_Finger_Button_Event.Forceless_planar_joint.Rz.q" ,
"Muscle_Finger_Button_Event.Forceless_planar_joint.Px.v" ,
"Muscle_Finger_Button_Event.Forceless_planar_joint.Py.v" ,
"Muscle_Finger_Button_Event.Forceless_planar_joint.Rz.w" } ; const char *
varObjects [ 12 ] = {
"CMFB/Muscle-Finger-Button-Event/Button  spring and damper" ,
"CMFB/Muscle-Finger-Button-Event/Button  spring and damper" ,
"CMFB/Muscle-Finger-Button-Event/Button  spring and damper" ,
"CMFB/Muscle-Finger-Button-Event/Button  spring and damper" ,
"CMFB/Muscle-Finger-Button-Event/Button  spring and damper" ,
"CMFB/Muscle-Finger-Button-Event/Button  spring and damper" ,
"CMFB/Muscle-Finger-Button-Event/Forceless  planar joint" ,
"CMFB/Muscle-Finger-Button-Event/Forceless  planar joint" ,
"CMFB/Muscle-Finger-Button-Event/Forceless  planar joint" ,
"CMFB/Muscle-Finger-Button-Event/Forceless  planar joint" ,
"CMFB/Muscle-Finger-Button-Event/Forceless  planar joint" ,
"CMFB/Muscle-Finger-Button-Event/Forceless  planar joint" } ; smData ->
mNumVarScalars = 12 ; smData -> mVarFullPaths = NULL ; smData -> mVarObjects
= NULL ; if ( smData -> mNumVarScalars > 0 ) { size_t s ; PmAllocator * alloc
= pm_default_allocator ( ) ; PM_ALLOCATE_ARRAY ( smData -> mVarFullPaths ,
PmCharVector , 12 , alloc ) ; PM_ALLOCATE_ARRAY ( smData -> mVarObjects ,
PmCharVector , 12 , alloc ) ; for ( s = 0 ; s < smData -> mNumVarScalars ; ++
s ) { smData -> mVarFullPaths [ s ] = cStringToCharVector ( varFullPaths [ s
] ) ; smData -> mVarObjects [ s ] = cStringToCharVector ( varObjects [ s ] )
; } } } static void initRuntimeParameters ( NeDaePrivateData * smData ) {
PmAllocator * alloc = pm_default_allocator ( ) ; int_T status = 0 ; size_t i
= 0 ; const int32_T * rtpRootVarRows = NULL ; const int32_T * rtpRootVarCols
= NULL ; const char * * rtpFullPaths = NULL ; smData -> mNumRtpRootVars = 0 ;
status = pm_create_int_vector_fields ( & smData -> mRtpRootVarRows , smData
-> mNumRtpRootVars , alloc ) ; checkMemAllocStatus ( status ) ; memcpy (
smData -> mRtpRootVarRows . mX , rtpRootVarRows , smData -> mNumRtpRootVars *
sizeof ( int32_T ) ) ; status = pm_create_int_vector_fields ( & smData ->
mRtpRootVarCols , smData -> mNumRtpRootVars , alloc ) ; checkMemAllocStatus (
status ) ; memcpy ( smData -> mRtpRootVarCols . mX , rtpRootVarCols , smData
-> mNumRtpRootVars * sizeof ( int32_T ) ) ; smData -> mRtpFullPaths = NULL ;
if ( smData -> mNumRtpRootVars > 0 ) { size_t v ; PM_ALLOCATE_ARRAY ( smData
-> mRtpFullPaths , PmCharVector , 0 , alloc ) ; for ( v = 0 ; v < smData ->
mNumRtpRootVars ; ++ v ) { smData -> mRtpFullPaths [ v ] =
cStringToCharVector ( rtpFullPaths [ v ] ) ; } } smData ->
mNumRuntimeRootVarScalars = 0 ; status = pm_create_real_vector_fields ( &
smData -> mRuntimeParameterScalars , 0 , alloc ) ; checkMemAllocStatus (
status ) ; for ( i = 0 ; i < smData -> mRuntimeParameterScalars . mN ; ++ i )
smData -> mRuntimeParameterScalars . mX [ i ] = 0.0 ; status =
pm_create_real_vector_fields ( & smData -> mAsmRuntimeDerivedValueScalars , 0
, alloc ) ; checkMemAllocStatus ( status ) ; for ( i = 0 ; i < smData ->
mAsmRuntimeDerivedValueScalars . mN ; ++ i ) smData ->
mAsmRuntimeDerivedValueScalars . mX [ i ] = 0.0 ; status =
pm_create_real_vector_fields ( & smData -> mSimRuntimeDerivedValueScalars , 0
, alloc ) ; checkMemAllocStatus ( status ) ; for ( i = 0 ; i < smData ->
mSimRuntimeDerivedValueScalars . mN ; ++ i ) smData ->
mSimRuntimeDerivedValueScalars . mX [ i ] = 0.0 ; } static void
initIoInfoHelper ( size_t n , const char * portPathsSource [ ] , const char *
unitsSource [ ] , const SizePair dimensions [ ] , boolean_T doInputs ,
NeDaePrivateData * smData ) { PmCharVector * portPaths = NULL ; PmCharVector
* units = NULL ; NeDsIoInfo * infos = NULL ; if ( n > 0 ) { size_t s ;
PmAllocator * alloc = pm_default_allocator ( ) ; PM_ALLOCATE_ARRAY (
portPaths , PmCharVector , n , alloc ) ; PM_ALLOCATE_ARRAY ( units ,
PmCharVector , n , alloc ) ; PM_ALLOCATE_ARRAY ( infos , NeDsIoInfo , n ,
alloc ) ; for ( s = 0 ; s < n ; ++ s ) { portPaths [ s ] =
cStringToCharVector ( portPathsSource [ s ] ) ; units [ s ] =
cStringToCharVector ( unitsSource [ s ] ) ; { NeDsIoInfo * info = infos + s ;
info -> mName = info -> mIdentifier = portPaths [ s ] . mX ; info -> mM =
dimensions [ s ] . first ; info -> mN = dimensions [ s ] . second ; info ->
mUnit = units [ s ] . mX ; } } } if ( doInputs ) { smData -> mNumInputs = n ;
smData -> mInputPortPaths = portPaths ; smData -> mInputUnits = units ;
smData -> mInputInfos = infos ; } else { smData -> mNumOutputs = n ; smData
-> mOutputPortPaths = portPaths ; smData -> mOutputUnits = units ; smData ->
mOutputInfos = infos ; } } static void initIoInfo ( NeDaePrivateData * smData
) { const char * inputPortPaths [ 9 ] = {
"Muscle_Finger_Button_Event.External_Force_and_Torque.fz" ,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Cir.fx"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Cir.fy"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Cir.fz"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Cir.tz"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Lin.fx"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Lin.fy"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Lin.fz"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.External_Force_and_Torque_Lin.tz"
} ; const char * inputUnits [ 9 ] = { "m*kg/s^2" , "m*kg/s^2" , "m*kg/s^2" ,
"m*kg/s^2" , "m^2*kg/s^2" , "m*kg/s^2" , "m*kg/s^2" , "m*kg/s^2" ,
"m^2*kg/s^2" } ; const SizePair inputDimensions [ 9 ] = { { 1 , 1 } , { 1 , 1
} , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , {
1 , 1 } } ; const char * outputPortPaths [ 15 ] = {
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.Sensing.Transform_Sensor.wz"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.Sensing.Transform_Sensor.x"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.Sensing.Transform_Sensor.y"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.Sensing.Transform_Sensor.vx"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.Sensing.Transform_Sensor.vy"
,
 "Muscle_Finger_Button_Event.Finger_pulp_spring_and_damper.Sensing.Transform_Sensor1.R"
, "Muscle_Finger_Button_Event.PC_Delay.Transform_Sensor.z" ,
"Muscle_Finger_Button_Event.Sensing.Transform_Sensor.x" ,
"Muscle_Finger_Button_Event.Sensing.Transform_Sensor.y" ,
"Muscle_Finger_Button_Event.Sensing.Transform_Sensor.z" ,
"Muscle_Finger_Button_Event.Sensing.Transform_Sensor.vz" ,
"Muscle_Finger_Button_Event.Sensing.Transform_Sensor.az" ,
"Muscle_Finger_Button_Event.Sensing1.Transform_Sensor.x" ,
"Muscle_Finger_Button_Event.Sensing1.Transform_Sensor.y" ,
"Muscle_Finger_Button_Event.Sensing1.Transform_Sensor.z" } ; const char *
outputUnits [ 15 ] = { "rad/s" , "m" , "m" , "m/s" , "m/s" , "1" , "m" , "m"
, "m" , "m" , "m/s" , "m/s^2" , "m" , "m" , "m" } ; const SizePair
outputDimensions [ 15 ] = { { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , {
1 , 1 } , { 3 , 3 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1
} , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } , { 1 , 1 } } ; initIoInfoHelper ( 9 ,
inputPortPaths , inputUnits , inputDimensions , true , smData ) ;
initIoInfoHelper ( 15 , outputPortPaths , outputUnits , outputDimensions ,
false , smData ) ; } static void initInputDerivs ( NeDaePrivateData * smData
) { const int32_T numInputDerivs [ 9 ] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0
} ; PmAllocator * alloc = pm_default_allocator ( ) ; const int_T status =
pm_create_int_vector_fields ( & smData -> mNumInputDerivs , smData ->
mInputVectorSize , alloc ) ; checkMemAllocStatus ( status ) ; memcpy ( smData
-> mNumInputDerivs . mX , numInputDerivs , 9 * sizeof ( int32_T ) ) ; smData
-> mInputOrder = 1 ; } static void initDirectFeedthrough ( NeDaePrivateData *
smData ) { const boolean_T directFeedthroughVector [ 9 ] = { false , false ,
false , false , false , false , false , false , false } ; const boolean_T
directFeedthroughMatrix [ 207 ] = { false , false , false , false , false ,
false , false , false , false , false , false , false , false , false , false
, false , false , false , false , true , false , false , false , false ,
false , false , false , false , false , false , false , false , false , false
, false , false , false , false , false , false , false , false , true ,
false , false , false , false , false , false , false , false , false , false
, false , false , false , false , false , false , false , false , false ,
false , false , false , true , false , false , false , false , false , false
, false , false , false , false , false , false , false , false , false ,
false , false , false , false , false , false , false , true , false , false
, false , false , false , false , false , false , false , false , false ,
false , false , false , false , false , false , false , false , false , false
, false , true , false , false , false , false , false , false , false ,
false , false , false , false , false , false , false , false , false , false
, false , false , false , false , false , true , false , false , false ,
false , false , false , false , false , false , false , false , false , false
, false , false , false , false , false , false , false , false , false ,
true , false , false , false , false , false , false , false , false , false
, false , false , false , false , false , false , false , false , false ,
false , false , false , false , true , false , false , false , false , false
, false , false , false , false , false , false , false , false , false ,
false , false , false , false , false , false , false , false , true , false
, false , false } ; PmAllocator * alloc = pm_default_allocator ( ) ; { const
int_T status = pm_create_bool_vector_fields ( & smData ->
mDirectFeedthroughVector , 9 , alloc ) ; checkMemAllocStatus ( status ) ;
memcpy ( smData -> mDirectFeedthroughVector . mX , directFeedthroughVector ,
9 * sizeof ( boolean_T ) ) ; } { const int_T status =
pm_create_bool_vector_fields ( & smData -> mDirectFeedthroughMatrix , 207 ,
alloc ) ; checkMemAllocStatus ( status ) ; memcpy ( smData ->
mDirectFeedthroughMatrix . mX , directFeedthroughMatrix , 207 * sizeof (
boolean_T ) ) ; } } static void initOutputDerivProc ( NeDaePrivateData *
smData ) { PmAllocator * alloc = pm_default_allocator ( ) ; const int32_T
outputFunctionMap [ 23 ] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 } ; smData -> mOutputFunctionMap =
pm_create_int_vector ( 23 , alloc ) ; memcpy ( smData -> mOutputFunctionMap
-> mX , outputFunctionMap , 23 * sizeof ( int32_T ) ) ; smData ->
mNumOutputClasses = 2 ; smData -> mHasKinematicOutputs = true ; smData ->
mHasDynamicOutputs = true ; smData -> mIsOutputClass0Dynamic = false ; smData
-> mDoComputeDynamicOutputs = false ; smData -> mCachedDerivativesAvailable =
false ; { size_t i = 0 ; const int_T status = pm_create_real_vector_fields (
& smData -> mCachedDerivatives , 12 , pm_default_allocator ( ) ) ;
checkMemAllocStatus ( status ) ; for ( i = 0 ; i < smData ->
mCachedDerivatives . mN ; ++ i ) smData -> mCachedDerivatives . mX [ i ] =
0.0 ; } }
#if 0
static void initializeSizePairVector ( const SmSizePair * data ,
SmSizePairVector * vector ) { const size_t n = sm_core_SmSizePairVector_size
( vector ) ; size_t i ; for ( i = 0 ; i < n ; ++ i , ++ data )
sm_core_SmSizePairVector_setValue ( vector , i , data ++ ) ; }
#endif
static void initAssemblyDelegate ( SmMechanismDelegate * delegate ) {
SmMechanismDelegateScratchpad * scratchpad = NULL ; const SmSizePair
jointToStageIdx [ 2 ] = { { 16 , 0 } , { 43 , 1 } } ; const size_t
primitiveIndices [ 2 + 1 ] = { 0 , 3 , 6 } ; const SmSizePair stateOffsets [
6 ] = { { 0 , 3 } , { 1 , 4 } , { 2 , 5 } , { 6 , 9 } , { 7 , 10 } , { 8 , 11
} } ; const SmSizePair dofOffsets [ 6 ] = { { 0 , 1 } , { 1 , 2 } , { 2 , 3 }
, { 3 , 4 } , { 4 , 5 } , { 5 , 6 } } ; const size_t remodIndices [ 2 ] = { 2
, 8 } ; const size_t * equationsPerConstraint = NULL ; const size_t
dofToVelSlot [ 6 ] = { 3 , 4 , 5 , 9 , 10 , 11 } ; const size_t *
constraintDofs = NULL ; const size_t constraintDofOffsets [ 0 + 1 ] = { 0 } ;
const size_t Jm = 0 ; const size_t Jn = 6 ; SmSizePair zeroSizePair ;
zeroSizePair . mFirst = zeroSizePair . mSecond = 0 ;
sm_core_MechanismDelegate_allocScratchpad ( delegate ) ; scratchpad =
delegate -> mScratchpad ; delegate -> mTargetStrengthFree = 0 ; delegate ->
mTargetStrengthSuggested = 1 ; delegate -> mTargetStrengthDesired = 2 ;
delegate -> mTargetStrengthRequired = 3 ; delegate -> mConsistencyTol = +
1.000000000000000062e-09 ; delegate -> mDof = 6 ; delegate -> mStateSize = 12
; delegate -> mContinuousStateSize = 12 ; delegate -> mNumStages = 2 ;
delegate -> mNumConstraints = 0 ; delegate -> mNumAllConstraintEquations = 0
; sm_core_SmSizePairVector_create ( & delegate -> mJointToStageIdx , delegate
-> mNumStages , & zeroSizePair ) ; memcpy (
sm_core_SmSizePairVector_nonConstValues ( & delegate -> mJointToStageIdx ) ,
jointToStageIdx , delegate -> mNumStages * sizeof ( SmSizePair ) ) ;
sm_core_SmSizeTVector_create ( & delegate -> mPrimitiveIndices , delegate ->
mNumStages + 1 , 0 ) ; memcpy ( sm_core_SmSizeTVector_nonConstValues ( &
delegate -> mPrimitiveIndices ) , primitiveIndices , ( delegate -> mNumStages
+ 1 ) * sizeof ( size_t ) ) ; sm_core_SmSizePairVector_create ( & delegate ->
mStateOffsets , 6 , & zeroSizePair ) ; memcpy (
sm_core_SmSizePairVector_nonConstValues ( & delegate -> mStateOffsets ) ,
stateOffsets , 6 * sizeof ( SmSizePair ) ) ; sm_core_SmSizePairVector_create
( & delegate -> mDofOffsets , 6 , & zeroSizePair ) ; memcpy (
sm_core_SmSizePairVector_nonConstValues ( & delegate -> mDofOffsets ) ,
dofOffsets , 6 * sizeof ( SmSizePair ) ) ; sm_core_SmSizeTVector_create ( &
delegate -> mRemodIndices , 2 , 0 ) ; memcpy (
sm_core_SmSizeTVector_nonConstValues ( & delegate -> mRemodIndices ) ,
remodIndices , 2 * sizeof ( size_t ) ) ; sm_core_SmSizeTVector_create ( &
delegate -> mEquationsPerConstraint , delegate -> mNumConstraints , 0 ) ;
memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate ->
mEquationsPerConstraint ) , equationsPerConstraint , delegate ->
mNumConstraints * sizeof ( size_t ) ) ; sm_core_SmIntVector_create ( &
delegate -> mRunTimeEnabledEquations , delegate -> mNumAllConstraintEquations
, 1 ) ; sm_core_SmSizeTVector_create ( & delegate -> mDofToVelSlot , delegate
-> mDof , 0 ) ; memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate ->
mDofToVelSlot ) , dofToVelSlot , delegate -> mDof * sizeof ( size_t ) ) ;
sm_core_SmSizeTVector_create ( & delegate -> mConstraintDofs , 0 , 0 ) ;
memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate -> mConstraintDofs
) , constraintDofs , 0 * sizeof ( size_t ) ) ; sm_core_SmSizeTVector_create (
& delegate -> mConstraintDofOffsets , delegate -> mNumConstraints + 1 , 0 ) ;
memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate ->
mConstraintDofOffsets ) , constraintDofOffsets , ( delegate ->
mNumConstraints + 1 ) * sizeof ( size_t ) ) ; sm_core_SmBoundedSet_create ( &
scratchpad -> mPosRequired , 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad
-> mPosDesired , 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad ->
mPosSuggested , 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad -> mPosFree
, 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad -> mPosNonRequired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mPosSuggAndFree , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelRequired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelDesired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelSuggested , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelFree , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelNonRequired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelSuggAndFree , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mConstraintFilter , 0 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mActiveConstraints , 0 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mActiveDofs , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mActiveDofs0 , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mNewConstraints , 0 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mNewDofs , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mUnsatisfiedConstraints , 0 ) ;
sm_core_SmSizeTVector_create ( & scratchpad -> mActiveConstraintsVect , 0 , 0
) ; sm_core_SmSizeTVector_create ( & scratchpad -> mActiveDofsVect , 6 , 0 )
; sm_core_SmSizeTVector_create ( & scratchpad -> mFullDofToActiveDof , 6 , 0
) ; sm_core_SmSizePairVector_create ( & scratchpad ->
mPartiallyPosTargetedPrims , 6 , & zeroSizePair ) ;
sm_core_SmSizePairVector_create ( & scratchpad -> mPartiallyVelTargetedPrims
, 6 , & zeroSizePair ) ; sm_core_SmSizeTVector_create ( & scratchpad ->
mPosPartialTypes , 6 , 0 ) ; sm_core_SmSizeTVector_create ( & scratchpad ->
mVelPartialTypes , 6 , 0 ) ; sm_core_SmSizeTVector_create ( & scratchpad ->
mPartiallyActivePrims , 6 , 0 ) ; sm_core_SmSizePairVector_create ( &
scratchpad -> mBaseFrameVelOffsets , 0 , & zeroSizePair ) ;
sm_core_SmSizePairVector_create ( & scratchpad -> mCvVelOffsets , 6 , &
zeroSizePair ) ; sm_core_SmRealVector_create ( & scratchpad ->
mCvAzimuthValues , 6 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mInitialState , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mStartState , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mTestState , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mFullStateVector , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mJacobianRowMaj , Jm * Jn , 0.0 ) ; sm_core_SmRealVector_create ( &
scratchpad -> mJacobian , Jm * Jn , 0.0 ) ; sm_core_SmRealVector_create ( &
scratchpad -> mJacobianPrimSubmatrix , Jm * 6 , 0.0 ) ;
sm_core_SmRealVector_create ( & scratchpad -> mConstraintNonhomoTerms , Jm ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mConstraintError , Jm ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mBestConstraintError ,
Jm , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mDeltas , Jn * (
Jm <= Jn ? Jm : Jn ) , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mSvdWork , 49 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mLineSearchScaledDeltaVect , 6 , 0.0 ) ; sm_core_SmRealVector_create ( &
scratchpad -> mLineSearchTestStateVect , 12 , 0.0 ) ;
sm_core_SmRealVector_create ( & scratchpad -> mLineSearchErrorVect , Jm , 0.0
) ; sm_core_SmRealVector_create ( & scratchpad -> mActiveDofVelsVect , 6 ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mVelSystemRhs , Jm ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mMotionData , 33 , 0.0
) ; delegate -> mSetTargets = CMFB_cde73719_3_setTargets ; delegate ->
mResetStateVector = CMFB_cde73719_3_resetStateVector ; delegate ->
mInitializeTrackedAngleState = CMFB_cde73719_3_initializeTrackedAngleState ;
delegate -> mComputeDiscreteState = CMFB_cde73719_3_computeDiscreteState ;
delegate -> mAdjustPosition = CMFB_cde73719_3_adjustPosition ; delegate ->
mPerturbState = CMFB_cde73719_3_perturbState ; delegate ->
mComputeDofBlendMatrix = CMFB_cde73719_3_computeDofBlendMatrix ; delegate ->
mProjectPartiallyTargetedPos = CMFB_cde73719_3_projectPartiallyTargetedPos ;
delegate -> mPropagateMotion = CMFB_cde73719_3_propagateMotion ; delegate ->
mComputeAssemblyError = CMFB_cde73719_3_computeAssemblyError ; delegate ->
mComputeAssemblyJacobian = CMFB_cde73719_3_computeAssemblyJacobian ; delegate
-> mComputeFullAssemblyJacobian = CMFB_cde73719_3_computeFullAssemblyJacobian
; delegate -> mIsInKinematicSingularity =
CMFB_cde73719_3_isInKinematicSingularity ; delegate -> mConvertStateVector =
CMFB_cde73719_3_convertStateVector ; delegate -> mConstructStateVector =
CMFB_cde73719_3_constructStateVector ; delegate -> mExtractSolverStateVector
= CMFB_cde73719_3_extractSolverStateVector ; delegate -> mIsPositionViolation
= CMFB_cde73719_3_isPositionViolation ; delegate -> mIsVelocityViolation =
CMFB_cde73719_3_isVelocityViolation ; delegate -> mProjectStateSim =
CMFB_cde73719_3_projectStateSim ; delegate -> mComputeConstraintError =
CMFB_cde73719_3_computeConstraintError ; delegate -> mMech = NULL ; } static
void initSimulationDelegate ( SmMechanismDelegate * delegate ) {
SmMechanismDelegateScratchpad * scratchpad = NULL ; const SmSizePair
jointToStageIdx [ 2 ] = { { 16 , 0 } , { 43 , 1 } } ; const size_t
primitiveIndices [ 2 + 1 ] = { 0 , 3 , 6 } ; const SmSizePair stateOffsets [
6 ] = { { 0 , 3 } , { 1 , 4 } , { 2 , 5 } , { 6 , 9 } , { 7 , 10 } , { 8 , 11
} } ; const SmSizePair dofOffsets [ 6 ] = { { 0 , 1 } , { 1 , 2 } , { 2 , 3 }
, { 3 , 4 } , { 4 , 5 } , { 5 , 6 } } ; const size_t remodIndices [ 2 ] = { 2
, 8 } ; const size_t * equationsPerConstraint = NULL ; const size_t
dofToVelSlot [ 6 ] = { 3 , 4 , 5 , 9 , 10 , 11 } ; const size_t *
constraintDofs = NULL ; const size_t constraintDofOffsets [ 0 + 1 ] = { 0 } ;
const size_t Jm = 0 ; const size_t Jn = 6 ; SmSizePair zeroSizePair ;
zeroSizePair . mFirst = zeroSizePair . mSecond = 0 ;
sm_core_MechanismDelegate_allocScratchpad ( delegate ) ; scratchpad =
delegate -> mScratchpad ; delegate -> mTargetStrengthFree = 0 ; delegate ->
mTargetStrengthSuggested = 1 ; delegate -> mTargetStrengthDesired = 2 ;
delegate -> mTargetStrengthRequired = 3 ; delegate -> mConsistencyTol = +
1.000000000000000062e-09 ; delegate -> mDof = 6 ; delegate -> mStateSize = 12
; delegate -> mContinuousStateSize = 12 ; delegate -> mNumStages = 2 ;
delegate -> mNumConstraints = 0 ; delegate -> mNumAllConstraintEquations = 0
; sm_core_SmSizePairVector_create ( & delegate -> mJointToStageIdx , delegate
-> mNumStages , & zeroSizePair ) ; memcpy (
sm_core_SmSizePairVector_nonConstValues ( & delegate -> mJointToStageIdx ) ,
jointToStageIdx , delegate -> mNumStages * sizeof ( SmSizePair ) ) ;
sm_core_SmSizeTVector_create ( & delegate -> mPrimitiveIndices , delegate ->
mNumStages + 1 , 0 ) ; memcpy ( sm_core_SmSizeTVector_nonConstValues ( &
delegate -> mPrimitiveIndices ) , primitiveIndices , ( delegate -> mNumStages
+ 1 ) * sizeof ( size_t ) ) ; sm_core_SmSizePairVector_create ( & delegate ->
mStateOffsets , 6 , & zeroSizePair ) ; memcpy (
sm_core_SmSizePairVector_nonConstValues ( & delegate -> mStateOffsets ) ,
stateOffsets , 6 * sizeof ( SmSizePair ) ) ; sm_core_SmSizePairVector_create
( & delegate -> mDofOffsets , 6 , & zeroSizePair ) ; memcpy (
sm_core_SmSizePairVector_nonConstValues ( & delegate -> mDofOffsets ) ,
dofOffsets , 6 * sizeof ( SmSizePair ) ) ; sm_core_SmSizeTVector_create ( &
delegate -> mRemodIndices , 2 , 0 ) ; memcpy (
sm_core_SmSizeTVector_nonConstValues ( & delegate -> mRemodIndices ) ,
remodIndices , 2 * sizeof ( size_t ) ) ; sm_core_SmSizeTVector_create ( &
delegate -> mEquationsPerConstraint , delegate -> mNumConstraints , 0 ) ;
memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate ->
mEquationsPerConstraint ) , equationsPerConstraint , delegate ->
mNumConstraints * sizeof ( size_t ) ) ; sm_core_SmIntVector_create ( &
delegate -> mRunTimeEnabledEquations , delegate -> mNumAllConstraintEquations
, 1 ) ; sm_core_SmSizeTVector_create ( & delegate -> mDofToVelSlot , delegate
-> mDof , 0 ) ; memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate ->
mDofToVelSlot ) , dofToVelSlot , delegate -> mDof * sizeof ( size_t ) ) ;
sm_core_SmSizeTVector_create ( & delegate -> mConstraintDofs , 0 , 0 ) ;
memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate -> mConstraintDofs
) , constraintDofs , 0 * sizeof ( size_t ) ) ; sm_core_SmSizeTVector_create (
& delegate -> mConstraintDofOffsets , delegate -> mNumConstraints + 1 , 0 ) ;
memcpy ( sm_core_SmSizeTVector_nonConstValues ( & delegate ->
mConstraintDofOffsets ) , constraintDofOffsets , ( delegate ->
mNumConstraints + 1 ) * sizeof ( size_t ) ) ; sm_core_SmBoundedSet_create ( &
scratchpad -> mPosRequired , 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad
-> mPosDesired , 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad ->
mPosSuggested , 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad -> mPosFree
, 6 ) ; sm_core_SmBoundedSet_create ( & scratchpad -> mPosNonRequired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mPosSuggAndFree , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelRequired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelDesired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelSuggested , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelFree , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelNonRequired , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mVelSuggAndFree , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mConstraintFilter , 0 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mActiveConstraints , 0 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mActiveDofs , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mActiveDofs0 , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mNewConstraints , 0 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mNewDofs , 6 ) ;
sm_core_SmBoundedSet_create ( & scratchpad -> mUnsatisfiedConstraints , 0 ) ;
sm_core_SmSizeTVector_create ( & scratchpad -> mActiveConstraintsVect , 0 , 0
) ; sm_core_SmSizeTVector_create ( & scratchpad -> mActiveDofsVect , 6 , 0 )
; sm_core_SmSizeTVector_create ( & scratchpad -> mFullDofToActiveDof , 6 , 0
) ; sm_core_SmSizePairVector_create ( & scratchpad ->
mPartiallyPosTargetedPrims , 6 , & zeroSizePair ) ;
sm_core_SmSizePairVector_create ( & scratchpad -> mPartiallyVelTargetedPrims
, 6 , & zeroSizePair ) ; sm_core_SmSizeTVector_create ( & scratchpad ->
mPosPartialTypes , 6 , 0 ) ; sm_core_SmSizeTVector_create ( & scratchpad ->
mVelPartialTypes , 6 , 0 ) ; sm_core_SmSizeTVector_create ( & scratchpad ->
mPartiallyActivePrims , 6 , 0 ) ; sm_core_SmSizePairVector_create ( &
scratchpad -> mBaseFrameVelOffsets , 0 , & zeroSizePair ) ;
sm_core_SmSizePairVector_create ( & scratchpad -> mCvVelOffsets , 6 , &
zeroSizePair ) ; sm_core_SmRealVector_create ( & scratchpad ->
mCvAzimuthValues , 6 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mInitialState , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mStartState , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mTestState , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mFullStateVector , 12 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mJacobianRowMaj , Jm * Jn , 0.0 ) ; sm_core_SmRealVector_create ( &
scratchpad -> mJacobian , Jm * Jn , 0.0 ) ; sm_core_SmRealVector_create ( &
scratchpad -> mJacobianPrimSubmatrix , Jm * 6 , 0.0 ) ;
sm_core_SmRealVector_create ( & scratchpad -> mConstraintNonhomoTerms , Jm ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mConstraintError , Jm ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mBestConstraintError ,
Jm , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mDeltas , Jn * (
Jm <= Jn ? Jm : Jn ) , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mSvdWork , 49 , 0.0 ) ; sm_core_SmRealVector_create ( & scratchpad ->
mLineSearchScaledDeltaVect , 6 , 0.0 ) ; sm_core_SmRealVector_create ( &
scratchpad -> mLineSearchTestStateVect , 12 , 0.0 ) ;
sm_core_SmRealVector_create ( & scratchpad -> mLineSearchErrorVect , Jm , 0.0
) ; sm_core_SmRealVector_create ( & scratchpad -> mActiveDofVelsVect , 6 ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mVelSystemRhs , Jm ,
0.0 ) ; sm_core_SmRealVector_create ( & scratchpad -> mMotionData , 33 , 0.0
) ; delegate -> mSetTargets = NULL ; delegate -> mResetStateVector =
CMFB_cde73719_3_resetStateVector ; delegate -> mInitializeTrackedAngleState =
CMFB_cde73719_3_initializeTrackedAngleState ; delegate ->
mComputeDiscreteState = CMFB_cde73719_3_computeDiscreteState ; delegate ->
mAdjustPosition = CMFB_cde73719_3_adjustPosition ; delegate -> mPerturbState
= CMFB_cde73719_3_perturbState ; delegate -> mComputeDofBlendMatrix = NULL ;
delegate -> mProjectPartiallyTargetedPos = NULL ; delegate ->
mPropagateMotion = CMFB_cde73719_3_propagateMotion ; delegate ->
mComputeAssemblyError = CMFB_cde73719_3_computeAssemblyError ; delegate ->
mComputeAssemblyJacobian = CMFB_cde73719_3_computeAssemblyJacobian ; delegate
-> mComputeFullAssemblyJacobian = CMFB_cde73719_3_computeFullAssemblyJacobian
; delegate -> mIsInKinematicSingularity =
CMFB_cde73719_3_isInKinematicSingularity ; delegate -> mConvertStateVector =
CMFB_cde73719_3_convertStateVector ; delegate -> mConstructStateVector =
CMFB_cde73719_3_constructStateVector ; delegate -> mExtractSolverStateVector
= CMFB_cde73719_3_extractSolverStateVector ; delegate -> mIsPositionViolation
= CMFB_cde73719_3_isPositionViolation ; delegate -> mIsVelocityViolation =
CMFB_cde73719_3_isVelocityViolation ; delegate -> mProjectStateSim =
CMFB_cde73719_3_projectStateSim ; delegate -> mComputeConstraintError =
CMFB_cde73719_3_computeConstraintError ; delegate -> mMech = NULL ; } static
void initMechanismDelegates ( NeDaePrivateData * smData ) { PmAllocator *
alloc = pm_default_allocator ( ) ; const int32_T * motionInputOffsets = NULL
; int_T status = 0 ; initAssemblyDelegate ( & smData -> mAssemblyDelegate ) ;
initSimulationDelegate ( & smData -> mSimulationDelegate ) ; status =
pm_create_int_vector_fields ( & smData -> mMotionInputOffsets , smData ->
mNumInputMotionPrimitives , alloc ) ; checkMemAllocStatus ( status ) ; memcpy
( smData -> mMotionInputOffsets . mX , motionInputOffsets , 0 * sizeof (
int32_T ) ) ; } static void initComputationFcnPtrs ( NeDaePrivateData *
smData ) { smData -> mSetParametersFcn = dae_cg_setParameters_function ;
smData -> mPAssertFcn = dae_cg_pAssert_method ; smData -> mDerivativeFcn =
dae_cg_deriv_method ; smData -> mOutputFcn = dae_cg_output_method ; smData ->
mProjectionFcn = dae_cg_project_solve ; smData -> mProjectionMaybeFcn =
dae_cg_projectMaybe_solve ; smData -> mCheckFcn = ( smData ->
mStateVectorSize == 0 ) ? dae_cg_check_solve : NULL ; smData -> mAssemblyFcn
= dae_cg_assemble_solve ; smData -> mSetupLoggerFcn =
sm_ssci_setupLoggerFcn_codeGen ; smData -> mLogFcn = sm_ssci_logFcn_codeGen ;
smData -> mResidualsFcn = NULL ; smData -> mLinearizeFcn = NULL ; smData ->
mGenerateFcn = NULL ; } static void initLiveLinkToSm ( NeDaePrivateData *
smData ) { smData -> mLiveSmLink = NULL ; smData -> mLiveSmLink_destroy =
NULL ; smData -> mLiveSmLink_copy = NULL ; } void
CMFB_cde73719_3_NeDaePrivateData_create ( NeDaePrivateData * smData ) {
initBasicAttributes ( smData ) ; initStateVector ( smData ) ; initAsserts (
smData ) ; initVariables ( smData ) ; initRuntimeParameters ( smData ) ;
initIoInfo ( smData ) ; initInputDerivs ( smData ) ; initDirectFeedthrough (
smData ) ; initOutputDerivProc ( smData ) ; initMechanismDelegates ( smData )
; initComputationFcnPtrs ( smData ) ; initLiveLinkToSm ( smData ) ; }
