#include "__cf_CMFB.h"
#include <math.h>
#include <string.h>
#include "pm_std.h"
#include "sm_std.h"
#include "ne_std.h"
#include "ne_dae.h"
#include "sm_ssci_run_time_errors.h"
#include "sm_CTarget.h"
static boolean_T checkTargets_6 ( const double * rtdv , const double * state
) { ( void ) rtdv ; return fabs ( state [ 6 ] - 0.014 ) < 1.0e-9 ; } void
CMFB_cde73719_3_checkTargets ( const double * rtdv , const double * state ) {
const char * msgId = "sm:compiler:state:UnsatisfiedDesiredTarget" ; if ( !
checkTargets_6 ( rtdv , state ) ) { pmf_preformatted_warning ( msgId ,
 "CMFB/Muscle-Finger-Button-Event/Forceless  planar joint Px high priority position target not achieved"
) ; pmf_printf (
 "[manual]CMFB/Muscle-Finger-Button-Event/Forceless  planar joint Px high priority position target not achieved\n"
) ; } } void CMFB_cde73719_3_setTargets ( const double * rtdv , CTarget *
targets ) { ( void ) rtdv ; ( void ) targets ; } void
CMFB_cde73719_3_resetStateVector ( const void * mech , double * state ) {
double xx [ 1 ] ; ( void ) mech ; xx [ 0 ] = 0.0 ; state [ 0 ] = xx [ 0 ] ;
state [ 1 ] = xx [ 0 ] ; state [ 2 ] = xx [ 0 ] ; state [ 3 ] = xx [ 0 ] ;
state [ 4 ] = xx [ 0 ] ; state [ 5 ] = xx [ 0 ] ; state [ 6 ] = xx [ 0 ] ;
state [ 7 ] = xx [ 0 ] ; state [ 8 ] = xx [ 0 ] ; state [ 9 ] = xx [ 0 ] ;
state [ 10 ] = xx [ 0 ] ; state [ 11 ] = xx [ 0 ] ; } void
CMFB_cde73719_3_initializeTrackedAngleState ( const void * mech , const
double * rtdv , const double * motionData , double * state , void *
neDiagMgr0 ) { NeuDiagnosticManager * neDiagMgr = ( NeuDiagnosticManager * )
neDiagMgr0 ; ( void ) mech ; ( void ) rtdv ; ( void ) motionData ; ( void )
state ; ( void ) neDiagMgr ; } void CMFB_cde73719_3_computeDiscreteState (
const void * mech , const double * rtdv , double * state ) { ( void ) mech ;
( void ) rtdv ; ( void ) state ; } void CMFB_cde73719_3_adjustPosition (
const void * mech , const double * dofDeltas , double * state ) { ( void )
mech ; state [ 0 ] = state [ 0 ] + dofDeltas [ 0 ] ; state [ 1 ] = state [ 1
] + dofDeltas [ 1 ] ; state [ 2 ] = state [ 2 ] + dofDeltas [ 2 ] ; state [ 6
] = state [ 6 ] + dofDeltas [ 3 ] ; state [ 7 ] = state [ 7 ] + dofDeltas [ 4
] ; state [ 8 ] = state [ 8 ] + dofDeltas [ 5 ] ; } static void
perturbState_0_0 ( double mag , double * state ) { state [ 0 ] = state [ 0 ]
+ mag ; } static void perturbState_0_0v ( double mag , double * state ) {
state [ 0 ] = state [ 0 ] + mag ; state [ 3 ] = state [ 3 ] - 0.875 * mag ; }
static void perturbState_0_1 ( double mag , double * state ) { state [ 1 ] =
state [ 1 ] + mag ; } static void perturbState_0_1v ( double mag , double *
state ) { state [ 1 ] = state [ 1 ] + mag ; state [ 4 ] = state [ 4 ] - 0.875
* mag ; } static void perturbState_0_2 ( double mag , double * state ) {
state [ 2 ] = state [ 2 ] + mag ; } static void perturbState_0_2v ( double
mag , double * state ) { state [ 2 ] = state [ 2 ] + mag ; state [ 5 ] =
state [ 5 ] - 0.875 * mag ; } static void perturbState_1_0 ( double mag ,
double * state ) { state [ 6 ] = state [ 6 ] + mag ; } static void
perturbState_1_0v ( double mag , double * state ) { state [ 6 ] = state [ 6 ]
+ mag ; state [ 9 ] = state [ 9 ] - 0.875 * mag ; } static void
perturbState_1_1 ( double mag , double * state ) { state [ 7 ] = state [ 7 ]
+ mag ; } static void perturbState_1_1v ( double mag , double * state ) {
state [ 7 ] = state [ 7 ] + mag ; state [ 10 ] = state [ 10 ] - 0.875 * mag ;
} static void perturbState_1_2 ( double mag , double * state ) { state [ 8 ]
= state [ 8 ] + mag ; } static void perturbState_1_2v ( double mag , double *
state ) { state [ 8 ] = state [ 8 ] + mag ; state [ 11 ] = state [ 11 ] -
0.875 * mag ; } void CMFB_cde73719_3_perturbState ( const void * mech ,
size_t stageIdx , size_t primIdx , double mag , boolean_T doPerturbVelocity ,
double * state ) { ( void ) mech ; ( void ) stageIdx ; ( void ) primIdx ; (
void ) mag ; ( void ) doPerturbVelocity ; ( void ) state ; switch ( (
stageIdx * 6 + primIdx ) * 2 + ( doPerturbVelocity ? 1 : 0 ) ) { case 0 :
perturbState_0_0 ( mag , state ) ; break ; case 1 : perturbState_0_0v ( mag ,
state ) ; break ; case 2 : perturbState_0_1 ( mag , state ) ; break ; case 3
: perturbState_0_1v ( mag , state ) ; break ; case 4 : perturbState_0_2 ( mag
, state ) ; break ; case 5 : perturbState_0_2v ( mag , state ) ; break ; case
12 : perturbState_1_0 ( mag , state ) ; break ; case 13 : perturbState_1_0v (
mag , state ) ; break ; case 14 : perturbState_1_1 ( mag , state ) ; break ;
case 15 : perturbState_1_1v ( mag , state ) ; break ; case 16 :
perturbState_1_2 ( mag , state ) ; break ; case 17 : perturbState_1_2v ( mag
, state ) ; break ; } } void CMFB_cde73719_3_computeDofBlendMatrix ( const
void * mech , size_t stageIdx , size_t primIdx , const double * state , int
partialType , double * matrix ) { ( void ) mech ; ( void ) stageIdx ; ( void
) primIdx ; ( void ) state ; ( void ) partialType ; ( void ) matrix ; switch
( ( stageIdx * 6 + primIdx ) ) { } } void
CMFB_cde73719_3_projectPartiallyTargetedPos ( const void * mech , size_t
stageIdx , size_t primIdx , const double * origState , int partialType ,
double * state ) { ( void ) mech ; ( void ) stageIdx ; ( void ) primIdx ; (
void ) origState ; ( void ) partialType ; ( void ) state ; switch ( (
stageIdx * 6 + primIdx ) ) { } } void CMFB_cde73719_3_propagateMotion ( const
void * mech , const double * rtdv , const double * state , double *
motionData ) { double xx [ 24 ] ; ( void ) mech ; ( void ) rtdv ; xx [ 0 ] =
0.5 ; xx [ 1 ] = xx [ 0 ] * state [ 2 ] ; xx [ 2 ] = cos ( xx [ 1 ] ) ; xx [
3 ] = xx [ 0 ] * xx [ 2 ] ; xx [ 4 ] = sin ( xx [ 1 ] ) ; xx [ 1 ] = xx [ 0 ]
* xx [ 4 ] ; xx [ 5 ] = xx [ 3 ] - xx [ 1 ] ; xx [ 6 ] = xx [ 1 ] - xx [ 3 ]
; xx [ 7 ] = xx [ 3 ] + xx [ 1 ] ; xx [ 8 ] = - xx [ 7 ] ; xx [ 9 ] = xx [ 3
] + xx [ 1 ] ; xx [ 1 ] = - xx [ 9 ] ; xx [ 3 ] = 0.0 ; xx [ 10 ] = xx [ 0 ]
* state [ 8 ] ; xx [ 0 ] = cos ( xx [ 10 ] ) ; xx [ 11 ] = sin ( xx [ 10 ] )
; xx [ 10 ] = 2.0 ; xx [ 12 ] = xx [ 6 ] ; xx [ 13 ] = xx [ 8 ] ; xx [ 14 ] =
xx [ 1 ] ; xx [ 15 ] = xx [ 9 ] * state [ 7 ] ; xx [ 16 ] = xx [ 9 ] * state
[ 6 ] ; xx [ 17 ] = xx [ 6 ] * state [ 7 ] + state [ 6 ] * xx [ 7 ] ; xx [ 18
] = xx [ 15 ] ; xx [ 19 ] = - xx [ 16 ] ; xx [ 20 ] = xx [ 17 ] ;
pm_math_cross3 ( xx + 12 , xx + 18 , xx + 21 ) ; xx [ 12 ] = state [ 3 ] * xx
[ 4 ] ; xx [ 13 ] = state [ 4 ] * xx [ 4 ] ; xx [ 14 ] = state [ 4 ] - xx [
10 ] * ( xx [ 12 ] * xx [ 2 ] + xx [ 13 ] * xx [ 4 ] ) ; xx [ 18 ] = state [
3 ] + xx [ 10 ] * ( xx [ 13 ] * xx [ 2 ] - xx [ 12 ] * xx [ 4 ] ) ; xx [ 2 ]
= state [ 5 ] * state [ 7 ] + xx [ 14 ] ; xx [ 4 ] = xx [ 18 ] - state [ 5 ]
* state [ 6 ] ; xx [ 12 ] = xx [ 4 ] * xx [ 11 ] ; xx [ 13 ] = xx [ 2 ] * xx
[ 11 ] ; xx [ 19 ] = state [ 10 ] * xx [ 11 ] ; xx [ 20 ] = state [ 9 ] * xx
[ 11 ] ; motionData [ 0 ] = xx [ 5 ] ; motionData [ 1 ] = xx [ 6 ] ;
motionData [ 2 ] = xx [ 8 ] ; motionData [ 3 ] = xx [ 1 ] ; motionData [ 4 ]
= state [ 0 ] ; motionData [ 5 ] = xx [ 3 ] ; motionData [ 6 ] = state [ 1 ]
; motionData [ 7 ] = - xx [ 0 ] ; motionData [ 8 ] = xx [ 3 ] ; motionData [
9 ] = xx [ 3 ] ; motionData [ 10 ] = - xx [ 11 ] ; motionData [ 11 ] = state
[ 6 ] ; motionData [ 12 ] = state [ 7 ] ; motionData [ 13 ] = xx [ 3 ] ;
motionData [ 14 ] = - ( xx [ 9 ] * xx [ 11 ] + xx [ 5 ] * xx [ 0 ] ) ;
motionData [ 15 ] = - ( xx [ 6 ] * xx [ 0 ] - xx [ 7 ] * xx [ 11 ] ) ;
motionData [ 16 ] = xx [ 6 ] * xx [ 11 ] + xx [ 7 ] * xx [ 0 ] ; motionData [
17 ] = - ( xx [ 5 ] * xx [ 11 ] - xx [ 9 ] * xx [ 0 ] ) ; motionData [ 18 ] =
state [ 6 ] + xx [ 10 ] * ( xx [ 21 ] + xx [ 15 ] * xx [ 5 ] ) + state [ 0 ]
; motionData [ 19 ] = state [ 7 ] + xx [ 10 ] * ( xx [ 22 ] - xx [ 16 ] * xx
[ 5 ] ) ; motionData [ 20 ] = xx [ 10 ] * ( xx [ 17 ] * xx [ 5 ] + xx [ 23 ]
) + state [ 1 ] ; motionData [ 21 ] = xx [ 3 ] ; motionData [ 22 ] = xx [ 3 ]
; motionData [ 23 ] = - state [ 5 ] ; motionData [ 24 ] = xx [ 14 ] ;
motionData [ 25 ] = xx [ 18 ] ; motionData [ 26 ] = xx [ 3 ] ; motionData [
27 ] = xx [ 3 ] ; motionData [ 28 ] = xx [ 3 ] ; motionData [ 29 ] = state [
11 ] - state [ 5 ] ; motionData [ 30 ] = xx [ 2 ] + xx [ 10 ] * ( xx [ 12 ] *
xx [ 0 ] - xx [ 13 ] * xx [ 11 ] ) + state [ 9 ] + xx [ 10 ] * ( xx [ 19 ] *
xx [ 0 ] - xx [ 20 ] * xx [ 11 ] ) ; motionData [ 31 ] = xx [ 4 ] - xx [ 10 ]
* ( xx [ 13 ] * xx [ 0 ] + xx [ 12 ] * xx [ 11 ] ) + state [ 10 ] - xx [ 10 ]
* ( xx [ 20 ] * xx [ 0 ] + xx [ 19 ] * xx [ 11 ] ) ; motionData [ 32 ] = xx [
3 ] ; } size_t CMFB_cde73719_3_computeAssemblyError ( const void * mech ,
const double * rtdv , size_t constraintIdx , const double * state , const
double * motionData , double * error ) { ( void ) mech ; ( void ) rtdv ; (
void ) state ; ( void ) motionData ; ( void ) error ; ( void ) state ; switch
( constraintIdx ) { } return 0 ; } size_t
CMFB_cde73719_3_computeAssemblyJacobian ( const void * mech , const double *
rtdv , size_t constraintIdx , boolean_T forVelocitySatisfaction , const
double * state , const double * motionData , double * J ) { ( void ) mech ; (
void ) rtdv ; ( void ) state ; ( void ) forVelocitySatisfaction ; ( void )
motionData ; ( void ) J ; switch ( constraintIdx ) { } return 0 ; } size_t
CMFB_cde73719_3_computeFullAssemblyJacobian ( const void * mech , const
double * rtdv , const double * state , const double * motionData , double * J
) { ( void ) mech ; ( void ) rtdv ; ( void ) state ; ( void ) motionData ; (
void ) J ; return 0 ; } int CMFB_cde73719_3_isInKinematicSingularity ( const
void * mech , const double * rtdv , size_t constraintIdx , const double *
motionData ) { ( void ) mech ; ( void ) rtdv ; ( void ) motionData ; switch (
constraintIdx ) { } return 0 ; } PmfMessageId
CMFB_cde73719_3_convertStateVector ( const void * asmMech , const double *
rtdv , const void * simMech , const double * asmState , double * simState ,
void * neDiagMgr0 ) { NeuDiagnosticManager * neDiagMgr = (
NeuDiagnosticManager * ) neDiagMgr0 ; ( void ) asmMech ; ( void ) rtdv ; (
void ) simMech ; ( void ) neDiagMgr ; simState [ 0 ] = asmState [ 0 ] ;
simState [ 1 ] = asmState [ 1 ] ; simState [ 2 ] = asmState [ 2 ] ; simState
[ 3 ] = asmState [ 3 ] ; simState [ 4 ] = asmState [ 4 ] ; simState [ 5 ] =
asmState [ 5 ] ; simState [ 6 ] = asmState [ 6 ] ; simState [ 7 ] = asmState
[ 7 ] ; simState [ 8 ] = asmState [ 8 ] ; simState [ 9 ] = asmState [ 9 ] ;
simState [ 10 ] = asmState [ 10 ] ; simState [ 11 ] = asmState [ 11 ] ;
return NULL ; } void CMFB_cde73719_3_constructStateVector ( const void * mech
, const double * solverState , const double * u , const double * uDot ,
double * discreteState , double * fullState ) { ( void ) mech ; ( void ) u ;
( void ) uDot ; ( void ) discreteState ; fullState [ 0 ] = solverState [ 0 ]
; fullState [ 1 ] = solverState [ 1 ] ; fullState [ 2 ] = solverState [ 2 ] ;
fullState [ 3 ] = solverState [ 3 ] ; fullState [ 4 ] = solverState [ 4 ] ;
fullState [ 5 ] = solverState [ 5 ] ; fullState [ 6 ] = solverState [ 6 ] ;
fullState [ 7 ] = solverState [ 7 ] ; fullState [ 8 ] = solverState [ 8 ] ;
fullState [ 9 ] = solverState [ 9 ] ; fullState [ 10 ] = solverState [ 10 ] ;
fullState [ 11 ] = solverState [ 11 ] ; } void
CMFB_cde73719_3_extractSolverStateVector ( const void * mech , const double *
fullState , double * solverState ) { ( void ) mech ; solverState [ 0 ] =
fullState [ 0 ] ; solverState [ 1 ] = fullState [ 1 ] ; solverState [ 2 ] =
fullState [ 2 ] ; solverState [ 3 ] = fullState [ 3 ] ; solverState [ 4 ] =
fullState [ 4 ] ; solverState [ 5 ] = fullState [ 5 ] ; solverState [ 6 ] =
fullState [ 6 ] ; solverState [ 7 ] = fullState [ 7 ] ; solverState [ 8 ] =
fullState [ 8 ] ; solverState [ 9 ] = fullState [ 9 ] ; solverState [ 10 ] =
fullState [ 10 ] ; solverState [ 11 ] = fullState [ 11 ] ; } int
CMFB_cde73719_3_isPositionViolation ( const void * mech , const double * rtdv
, const int * eqnEnableFlags , const double * state ) { ( void ) mech ; (
void ) rtdv ; ( void ) eqnEnableFlags ; ( void ) state ; return 0 ; } int
CMFB_cde73719_3_isVelocityViolation ( const void * mech , const double * rtdv
, const int * eqnEnableFlags , const double * state ) { ( void ) mech ; (
void ) rtdv ; ( void ) eqnEnableFlags ; ( void ) state ; return 0 ; }
PmfMessageId CMFB_cde73719_3_projectStateSim ( const void * mech , const
double * rtdv , const int * eqnEnableFlags , const double * input , double *
state , void * neDiagMgr0 ) { NeuDiagnosticManager * neDiagMgr = (
NeuDiagnosticManager * ) neDiagMgr0 ; ( void ) mech ; ( void ) rtdv ; ( void
) eqnEnableFlags ; ( void ) input ; ( void ) state ; ( void ) neDiagMgr ;
return NULL ; }
