/************************************************************************
Does not implement:
in_air_not_allowed

/************************************************************************/
/*
dynamics-impact.c: 
This is where the numerical integration and SDFAST stuff is done.
Use multiple models (in_air, single_support, double_support).
Use impact equations to analytically compute effect of impacts
and model transitions.
*/
/************************************************************************/
/*
In this version we are going to use the following conventions:
x (right: the direction of roll)
y forward
z up

roll is the torso angle relative to vertical
+ leans the torso to positive X (right) 

hip is the joint angle between the torso and the thigh
hip +
moving the torso in postive X (right) for a fixed vertical thigh, 
moving the thigh in positve X (right) for a fixed vertical torso.

knee is the length between the thigh and the calf
knee +
lengthens leg.

ankle is the angle between the calf and vertical
ankle +
moving the calf in positive X for a fixed horizontal foot,
moving the right side of the foot upward for a fixed vertical calf.
*/
/************************************************************************/
/* BUGS
We could try to change step size to find the exact time of impact.
*/
/************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "main.h"
#include "main2.h"
#include "sdfast/sra.h"

/************************************************************************/
/* DEFINES */

#define FEET_TOGETHER (1e-3) // meters. Feet too close together to measure tilt

#define EPSILON1 -1e-6 /* Fudge factor for ground level */

#define MAX_N_A_B 20 // For allocating a, b below.

#define SS_IMPACT1_N (5+2)
#define SS_IMPACT1_A1D 0
#define SS_IMPACT1_A2D 1
#define SS_IMPACT1_A3D 2
#define SS_IMPACT1_A4D 3
#define SS_IMPACT1_A5D 4
#define SS_IMPACT1_IMPULSE_X 5
#define SS_IMPACT1_IMPULSE_Z 6

#define SS_IMPACT2_N (7+2+2+2)
#define SS_IMPACT2_XD 0
#define SS_IMPACT2_YD 1
#define SS_IMPACT2_ROLLD 2
#define SS_IMPACT2_L_HIPD 3
#define SS_IMPACT2_L_KNEED 4
#define SS_IMPACT2_R_HIPD 5
#define SS_IMPACT2_R_KNEED 6
#define SS_IMPACT2_IMPULSE_X 7
#define SS_IMPACT2_IMPULSE_Z 8
#define SS_IMPACT2_FOOTD_X 9
#define SS_IMPACT2_FOOTD_Z 10
#define SS_IMPACT2_FOOTD2_X 11
#define SS_IMPACT2_FOOTD2_Z 12

#define SS_IMPACT3_N (17)
#define SS_IMPACT3_A1D 0
#define SS_IMPACT3_A2D 1
#define SS_IMPACT3_A3D 2
#define SS_IMPACT3_A4D 3
#define SS_IMPACT3_A5D 4
#define SS_IMPACT3_IMPULSE0_X 5
#define SS_IMPACT3_IMPULSE0_Z 6
#define SS_IMPACT3_IMPULSE1_X 7
#define SS_IMPACT3_IMPULSE1_Z 8
#define SS_IMPACT3_IMPULSE2_X 9
#define SS_IMPACT3_IMPULSE2_Z 10
#define SS_IMPACT3_IMPULSE3_X 11
#define SS_IMPACT3_IMPULSE3_Z 12
#define SS_IMPACT3_IMPULSE4_X 13
#define SS_IMPACT3_IMPULSE4_Z 14
#define SS_IMPACT3_IMPULSE_X 15
#define SS_IMPACT3_IMPULSE_Z 16


/*
vars = { j1xdf, j1zdf, a1df, a2df, a3df, a4df, a5df,
	 R12x, R12z, RLK, TLK, R36x, R36z, R14x, R14z, RRK, TRK }
*/
#define SS_IMPACT4_N (17)
#define SS_IMPACT4_XD 0
#define SS_IMPACT4_YD 1
#define SS_IMPACT4_ROLLD 2
#define SS_IMPACT4_L_HIPD 3
#define SS_IMPACT4_L_KNEED 4
#define SS_IMPACT4_R_HIPD 5
#define SS_IMPACT4_R_KNEED 6
#define SS_IMPACT4_IMPULSE0_X 7
#define SS_IMPACT4_IMPULSE0_Z 8
#define SS_IMPACT4_IMPULSE1_X 9
#define SS_IMPACT4_IMPULSE1_Z 10
#define SS_IMPACT4_IMPULSE_X 11
#define SS_IMPACT4_IMPULSE_Z 12
#define SS_IMPACT4_IMPULSE3_X 13
#define SS_IMPACT4_IMPULSE3_Z 14
#define SS_IMPACT4_IMPULSE4_X 15
#define SS_IMPACT4_IMPULSE4_Z 16

/************************************************************************/
/* GLOBAL VARIABLES */

SIM sim; /* The simulation structure */

int interactive = 0;
int impact1_print = 0;
int impact2_print = 0;

/* Not thread safe */
float **a = NULL;
float *b = NULL;

int ds_debug = 0;

/************************************************************************/
/************************************************************************/
/************************************************************************/

int other_side( int value )
{
  if ( value == LEFT )
    return RIGHT;
  else
    return LEFT;
}

/************************************************************************/
/* Make up for the fact that the ss and ds models have a foot location */

offset_x( SIM *s, double v[3] )
{
  v[XX] += s->x_offset;
}

/************************************************************************/

void debug_positions( SIM *s )
{

  printf( "head: %g %g %g\n", s->head[XX], s->head[YY], s->head[ZZ] );
  printf( "torso: %g %g %g\n", s->torso[XX], s->torso[YY], s->torso[ZZ] );
  printf( "lhip: %g %g %g\n", s->hip[LEFT][XX], s->hip[LEFT][YY],
	  s->hip[LEFT][ZZ] );
  printf( "lthigh: %g %g %g\n", s->thigh[LEFT][XX], s->thigh[LEFT][YY],
	  s->thigh[LEFT][ZZ] );
  printf( "lknee: %g %g %g\n", s->knee[LEFT][XX], s->knee[LEFT][YY],
	  s->knee[LEFT][ZZ] );
  printf( "lcalf: %g %g %g\n", s->calf[LEFT][XX], s->calf[LEFT][YY],
	  s->calf[LEFT][ZZ] );
  printf( "lfoot: %g %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][YY],
	  s->foot[LEFT][ZZ] );
  printf( "rhip: %g %g %g\n", s->hip[RIGHT][XX], s->hip[RIGHT][YY],
	  s->hip[RIGHT][ZZ] );
  printf( "rthigh: %g %g %g\n", s->thigh[RIGHT][XX], s->thigh[RIGHT][YY],
	  s->thigh[RIGHT][ZZ] );
  printf( "rknee: %g %g %g\n", s->knee[RIGHT][XX], s->knee[RIGHT][YY],
	  s->knee[RIGHT][ZZ] );
  printf( "rcalf: %g %g %g\n", s->calf[RIGHT][XX], s->calf[RIGHT][YY],
	  s->calf[RIGHT][ZZ] );
  printf( "rfoot: %g %g %g\n", s->foot[RIGHT][XX], s->foot[RIGHT][YY],
	  s->foot[RIGHT][ZZ] );

  printf( "roll: %g\n", s->roll );
  printf( "hips: %g %g\n", s->hip_angle[LEFT], s->hip_angle[RIGHT] );
  printf( "knees: %g %g\n", s->knee_length[LEFT], s->knee_length[RIGHT] );
  printf( "ankles: %g %g\n", s->ankle_angle[LEFT], s->ankle_angle[RIGHT] );
}

/************************************************************************/

init_in_air_model( SIM *s )
{
  int i, j;
  double vector[3];
  double inertia[3][3];

  /* Model already initialized

  sramass( AIR_TORSO_BODY, s->torso_mass );
  sramass( AIR_L_THIGH_BODY, s->thigh_mass );
  sramass( AIR_R_THIGH_BODY, s->thigh_mass );
  sramass( AIR_L_CALF_BODY, s->calf_mass );
  sramass( AIR_R_CALF_BODY, s->calf_mass );

  for ( i = 0; i < 3; i++ )
    {
      vector[i] = 0;
      for ( j = 0; j < 3; j++ )
	{
	  if ( i == j )
	    inertia[i][i] = 1.0;
	  else
	    inertia[i][j] = 0;
	}
    }
  inertia[YY][YY] = s->torso_I;
  srainer( AIR_TORSO_BODY, inertia );
  inertia[YY][YY] = s->thigh_I;
  srainer( AIR_L_THIGH_BODY, inertia );
  srainer( AIR_R_THIGH_BODY, inertia );
  inertia[YY][YY] = s->calf_I;
  srainer( AIR_L_CALF_BODY, inertia );
  srainer( AIR_R_CALF_BODY, inertia );

  vector[ZZ] = -s->torso_cm;
  srabtj( AIR_TORSO_BODY, vector );
  vector[ZZ] = s->thigh_cm;
  srabtj( AIR_L_THIGH_BODY, vector );
  srabtj( AIR_R_THIGH_BODY, vector );
  vector[ZZ] = s->calf_cm;
  srabtj( AIR_L_CALF_BODY, vector );
  srabtj( AIR_R_CALF_BODY, vector );

  vector[ZZ] = -s->torso_cm;
  sraitj( AIR_L_THIGH_BODY, vector );
  sraitj( AIR_R_THIGH_BODY, vector );

  vector[ZZ] = -(s->thigh_length - s->thigh_cm);
  sraitj( AIR_L_CALF_BODY, vector );
  sraitj( AIR_R_CALF_BODY, vector );
  */

  srainit(); /* initialize SDFAST model */
  srastab( 2.0*s->sdfast_baumgarte, s->sdfast_baumgarte*s->sdfast_baumgarte ); 
  sraprinterr(stderr);
  sraclearerr();
}

/************************************************************************/

void forward_kinematics( SIM *s )
{
  int i, j;
  double total_z_force = 0;
  double w_left, w_right;

  for ( i = 0; i < 3; i++ )
    {
      s->groin[i] = (s->hip[LEFT][i] + s->hip[RIGHT][i])/2;
      s->groind[i] = (s->hipd[LEFT][i] + s->hipd[RIGHT][i])/2;
      s->com[i] = s->torso[i]*s->torso_mass
	+ s->thigh[LEFT][i]*s->thigh_mass
	+ s->thigh[RIGHT][i]*s->thigh_mass
	+ s->calf[LEFT][i]*s->calf_mass
	+ s->calf[RIGHT][i]*s->calf_mass;
      s->com[i] /= s->total_mass;
      s->comd[i] = s->torsod[i]*s->torso_mass
	+ s->thighd[LEFT][i]*s->thigh_mass
	+ s->thighd[RIGHT][i]*s->thigh_mass
	+ s->calfd[LEFT][i]*s->calf_mass
	+ s->calfd[RIGHT][i]*s->calf_mass;
      s->comd[i] /= s->total_mass;
      s->cop_foot[LEFT][i] = s->foot[LEFT][i];
      s->cop_foot[RIGHT][i] = s->foot[RIGHT][i];
    }
  for ( j = LEFT; j <= RIGHT; j++ )
    {
      if ( s->ground_force[j][ZZ] > 0.01*s->total_weight_in_newtons )
	s->cop_foot[j][XX] += s->ankle_torque[j]/s->ground_force[j][ZZ];
    }
  total_z_force = s->ground_force[LEFT][ZZ] + s->ground_force[RIGHT][ZZ];
  for ( i = 0; i < 3; i++ )
    {
      if ( total_z_force > 0.01*s->total_weight_in_newtons )
	{
	  s->cof[i] = (s->ground_force[LEFT][ZZ] * s->foot[LEFT][i]
		       + s->ground_force[RIGHT][ZZ] * s->foot[RIGHT][i])/
	    total_z_force;
	  s->cop[i] = (s->ground_force[LEFT][ZZ] * s->cop_foot[LEFT][i]
		       + s->ground_force[RIGHT][ZZ] * s->cop_foot[RIGHT][i])/
	    total_z_force;
	}
      else
	{
	  s->cof[i] = 0.5*(s->foot[LEFT][i] + s->foot[RIGHT][i]);
	  s->cop[i] = 0.5*(s->foot[LEFT][i] + s->foot[RIGHT][i]);
	}
      s->com_to_cof[i] = s->com[i] - s->cof[i];
      s->com_to_cop[i] = s->com[i] - s->cop[i];
    }

  // since the ankle angles don't exist in the sdfast model, we have to
  // compute them.
  s->ankle_angle[LEFT] = s->roll - s->hip_angle[LEFT];
  s->ankle_angle[RIGHT] = s->roll - s->hip_angle[RIGHT];
  s->ankle_angled[LEFT] = s->rolld - s->hip_angled[LEFT];
  s->ankle_angled[RIGHT] = s->rolld - s->hip_angled[RIGHT];

  s->torso_abs_angle = 
    -atan2( s->head[XX] - s->groin[XX], s->head[ZZ] - s->groin[ZZ] );
  for( i = 0; i < 2; i++ )
    {
      s->thigh_abs_angle[i] = 
	-atan2( s->hip[i][XX] - s->knee[i][XX],
		s->hip[i][ZZ] - s->knee[i][ZZ] );
      s->calf_abs_angle[i] = 
	-atan2( s->knee[i][XX] - s->foot[i][XX],
		s->knee[i][ZZ] - s->foot[i][ZZ] );
    }
  
  s->dynamics_type = s->sdfast_model;
}

/************************************************************************/

void in_air_forward_kinematics( SIM *s )
{
  int i;
  double zero_offset[3] = { 0.0, 0.0, 0.0 };

  srapos( AIR_TORSO_BODY, s->head_offset, s->head );    
  srapos( AIR_TORSO_BODY, s->hip_offset[LEFT], s->hip[LEFT] );    
  srapos( AIR_TORSO_BODY, s->hip_offset[RIGHT], s->hip[RIGHT] );    
  srapos( AIR_L_CALF_BODY, s->knee_offset, &(s->knee[LEFT][0]) );    
  srapos( AIR_R_CALF_BODY, s->knee_offset, &(s->knee[RIGHT][0]) );    
  /*
  srapos( AIR_L_THIGH_BODY, s->knee_offset, &(s->knee[LEFT][0]) );    
  srapos( AIR_R_THIGH_BODY, s->knee_offset, &(s->knee[RIGHT][0]) );    
  */
  srapos( AIR_L_CALF_BODY, s->foot_offset, &(s->foot[LEFT][0]) );    
  srapos( AIR_R_CALF_BODY, s->foot_offset, &(s->foot[RIGHT][0]) );    

  srapos( AIR_TORSO_BODY, zero_offset, s->torso );
  srapos( AIR_L_THIGH_BODY, zero_offset, &(s->thigh[LEFT][0]) );
  srapos( AIR_R_THIGH_BODY, zero_offset, &(s->thigh[RIGHT][0]) );
  srapos( AIR_L_CALF_BODY, zero_offset, &(s->calf[LEFT][0]) );
  srapos( AIR_R_CALF_BODY, zero_offset, &(s->calf[RIGHT][0]) );

  sravel( AIR_TORSO_BODY, s->head_offset, s->headd );    
  sravel( AIR_TORSO_BODY, s->hip_offset[LEFT], s->hipd[LEFT] );    
  sravel( AIR_TORSO_BODY, s->hip_offset[RIGHT], s->hipd[RIGHT] );    
  sravel( AIR_L_CALF_BODY, s->knee_offset, &(s->kneed[LEFT][0]) );    
  sravel( AIR_R_CALF_BODY, s->knee_offset, &(s->kneed[RIGHT][0]) );    
  /*
  sravel( AIR_L_THIGH_BODY, s->knee_offset, &(s->kneed[LEFT][0]) );    
  sravel( AIR_R_THIGH_BODY, s->knee_offset, &(s->kneed[RIGHT][0]) );    
  */
  sravel( AIR_L_CALF_BODY, s->foot_offset, &(s->footd[LEFT][0]) );    
  sravel( AIR_R_CALF_BODY, s->foot_offset, &(s->footd[RIGHT][0]) );    

  sravel( AIR_TORSO_BODY, zero_offset, s->torsod );
  sravel( AIR_L_THIGH_BODY, zero_offset, &(s->thighd[LEFT][0]) );
  sravel( AIR_R_THIGH_BODY, zero_offset, &(s->thighd[RIGHT][0]) );
  sravel( AIR_L_CALF_BODY, zero_offset, &(s->calfd[LEFT][0]) );
  sravel( AIR_R_CALF_BODY, zero_offset, &(s->calfd[RIGHT][0]) );

  sraprinterr(stderr);
  sraclearerr();

  s->roll = -s->sdfast_state[AIR_ROLL];
  s->hip_angle[LEFT] = s->sdfast_state[AIR_L_HIP];
  s->hip_angle[RIGHT] = s->sdfast_state[AIR_R_HIP];
  s->knee_length[LEFT] = s->sdfast_state[AIR_L_KNEE];
  s->knee_length[RIGHT] = s->sdfast_state[AIR_R_KNEE];

  s->rolld = -s->sdfast_state[AIR_ROLLD];
  s->hip_angled[LEFT] = s->sdfast_state[AIR_L_HIPD];
  s->hip_angled[RIGHT] = s->sdfast_state[AIR_R_HIPD];
  s->knee_lengthd[LEFT] = s->sdfast_state[AIR_L_KNEED];
  s->knee_lengthd[RIGHT] = s->sdfast_state[AIR_R_KNEED];

  forward_kinematics( s );
}

/************************************************************************/
/* Initialize the state vector */

void init_state_in_air( SIM *s )
{
  int i, j;

  s->status = STATUS_OK;

  s->sdfast_state[AIR_X] = s->torso[XX];
  s->sdfast_state[AIR_Z] = s->torso[ZZ];
  s->sdfast_state[AIR_ROLL] = -s->roll;
  s->sdfast_state[AIR_L_HIP] = s->hip_angle[LEFT];
  s->sdfast_state[AIR_R_HIP] = s->hip_angle[RIGHT];
  s->sdfast_state[AIR_L_KNEE] = s->knee_length[LEFT];
  s->sdfast_state[AIR_R_KNEE] = s->knee_length[RIGHT];

  s->sdfast_state[AIR_XD] = s->torsod[XX];
  s->sdfast_state[AIR_ZD] = s->torsod[ZZ];
  s->sdfast_state[AIR_ROLLD] = -s->rolld;
  s->sdfast_state[AIR_L_HIPD] = s->hip_angled[LEFT];
  s->sdfast_state[AIR_R_HIPD] = s->hip_angled[RIGHT];
  s->sdfast_state[AIR_L_KNEED] = s->knee_lengthd[LEFT];
  s->sdfast_state[AIR_R_KNEED] = s->knee_lengthd[RIGHT];

  for( i = 0; i < AIR_N_STATES; i++ )
    {
      s->sdfast_stated[i] = 0;
    }

  for( i = 0, j = AIR_N_Q; i < AIR_N_U; i++, j++ )
    { 
      s->sdfast_stated[i] = s->sdfast_state[j];
    }

  srastate( 0.0, s->sdfast_state, s->sdfast_stated );
  s->status = STATUS_OK;

  sraprinterr(stderr);
  sraclearerr();

  in_air_forward_kinematics( s );

  s->foot_status[LEFT] = FOOT_IN_AIR;
  s->foot_status[RIGHT] = FOOT_IN_AIR;
}

/************************************************************************/

init_single_support_models( SIM *s )
{
  int i, j;
  double vector[3];
  double inertia[3][3];
  static int firsttime = 0;

  /* Model already initialized

  b1ssmass( SS_TORSO_BODY, s->torso_mass );
  b1ssmass( SS_THIGH1_BODY, s->thigh_mass );
  b1ssmass( SS_THIGH2_BODY, s->thigh_mass );
  b1ssmass( SS_CALF1_BODY, s->calf_mass );
  b1ssmass( SS_CALF2_BODY, s->calf_mass );

  for ( i = 0; i < 3; i++ )
    {
      vector[i] = 0;
      for ( j = 0; j < 3; j++ )
	{
	  if ( i == j )
	    inertia[i][i] = 1.0;
	  else
	    inertia[i][j] = 0;
	}
    }
  inertia[ZZ][ZZ] = s->torso_I;
  b1ssiner( SS_TORSO_BODY, inertia );
  inertia[ZZ][ZZ] = s->thigh_I;
  b1ssiner( SS_THIGH1_BODY, inertia );
  b1ssiner( SS_THIGH2_BODY, inertia );
  inertia[ZZ][ZZ] = s->calf_I;
  b1ssiner( SS_CALF1_BODY, inertia );
  b1ssiner( SS_CALF2_BODY, inertia );


  if ( firsttime )
    {
      for( i = 0; i < 5; i++ )
	{
	  b1ssgetbtj( i, vector );
	  printf( "ss r btj %d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
    }

  vector[YY] = -(s->calf_length - s->calf_cm);
  b1ssbtj( SS_CALF1_BODY, vector );
  vector[YY] = -(s->thigh_length - s->thigh_cm);
  b1ssbtj( SS_THIGH1_BODY, vector );
  vector[YY] = -s->torso_cm;
  b1ssbtj( SS_TORSO_BODY, vector );
  vector[YY] = s->thigh_cm;
  b1ssbtj( SS_THIGH2_BODY, vector );
  vector[YY] = s->calf_cm;
  b1ssbtj( SS_CALF2_BODY, vector );

  if ( firsttime )
    {
      for( i = 0; i < 5; i++ )
	{
	  b1ssgetbtj( i, vector );
	  printf( "ss r btj %d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
    }

  if ( firsttime )
    {
      for( i = 0; i < 5; i++ )
	{
	  b1ssgetitj( i, vector );
	  printf( "ss r itj %d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
    }

  vector[YY] = s->calf_cm;
  b1ssitj( SS_THIGH1_BODY, vector );
  vector[YY] = s->thigh_cm;
  b1ssitj( SS_TORSO_BODY, vector );
  vector[YY] = -s->torso_cm;
  b1ssitj( SS_THIGH2_BODY, vector );
  vector[YY] = -(s->thigh_length - s->thigh_cm);
  b1ssitj( SS_CALF2_BODY, vector );

  if ( firsttime )
    {
      for( i = 0; i < 5; i++ )
	{
	  b1ssgetitj( i, vector );
	  printf( "ss itj %d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
      printf( "press return to continue. 2 %g\n", s->time );
      getchar();
    }

  firsttime = 0;
  */

  srssinit(); /* initialize SDFAST model */
  srssstab( 2.0*s->sdfast_baumgarte, s->sdfast_baumgarte*s->sdfast_baumgarte ); 
  srssprinterr(stderr);
  srssclearerr();

  srssrinit(); /* initialize SDFAST model */
  srssrstab( 2.0*s->sdfast_baumgarte, s->sdfast_baumgarte*s->sdfast_baumgarte ); 
  srssrprinterr(stderr);
  srssrclearerr();
}

/************************************************************************/
/* Left foot down single support: convert sdfast to sim coordinates. */

void ssl_forward_kinematics( SIM *s )
{
  double offset[3] = { 0, 0, 0 };
  int i;

  if ( s->ss_foot_down != LEFT )
    {
      fprintf( stderr, "ssl_forward_kinematics: wrong foot down: %d\n", s->ss_foot_down );
      exit( -1 );
    }

  srsspos( SS_TORSO_BODY, offset, s->torso );
  offset_x( s, s->torso );
  srssvel( SS_TORSO_BODY, offset, s->torsod );
  srsspos( SS_TORSO_BODY, s->head_offset, s->head );
  offset_x( s, s->head );
  srssvel( SS_TORSO_BODY, s->head_offset, s->headd );

  s->roll = -(s->sdfast_state[SS_ANKLE1] + s->sdfast_state[SS_HIP1]);
  s->rolld = -(s->sdfast_state[SS_ANKLE1D] + s->sdfast_state[SS_HIP1D]);

  if ( s->ss_foot_down == LEFT )
    {
      for( i = 0; i < 3; i++ )
	{
	  s->foot[LEFT][i] = 0;
	  s->footd[LEFT][i] = 0;
	}
      offset_x( s, s->foot[LEFT] );
      srsspos( SS_CALF1_BODY, offset, s->calf[LEFT] );
      offset_x( s, s->calf[LEFT] );
      srssvel( SS_CALF1_BODY, offset, s->calfd[LEFT] );
      srsspos( SS_THIGH1_BODY, offset, s->thigh[LEFT] );
      offset_x( s, s->thigh[LEFT] );
      srssvel( SS_THIGH1_BODY, offset, s->thighd[LEFT] );
      srsspos( SS_THIGH2_BODY, offset, s->thigh[RIGHT] );
      offset_x( s, s->thigh[RIGHT] );
      srssvel( SS_THIGH2_BODY, offset, s->thighd[RIGHT] );
      srsspos( SS_CALF2_BODY, offset, s->calf[RIGHT] );
      offset_x( s, s->calf[RIGHT] );
      srssvel( SS_CALF2_BODY, offset, s->calfd[RIGHT] );
      /*
      printf( "offset: %g %g %g\n", offset[0], offset[1], offset[2] );
      printf( "rcalf: %g %g %g\n", s->calf[RIGHT][XX], s->calf[RIGHT][YY], s->calf[RIGHT][ZZ] ); 
      */

      offset[ZZ] = s->calf_cm;
      srsspos( SS_CALF1_BODY, offset, s->knee[LEFT] );
      offset_x( s, s->knee[LEFT] );
      srssvel( SS_CALF1_BODY, offset, s->kneed[LEFT] );
      srsspos( SS_CALF2_BODY, offset, s->knee[RIGHT] );
      offset_x( s, s->knee[RIGHT] );
      srssvel( SS_CALF2_BODY, offset, s->kneed[RIGHT] );

      srsspos( SS_TORSO_BODY, s->hip_offset[LEFT], s->hip[LEFT] );
      offset_x( s, s->hip[LEFT] );
      srssvel( SS_TORSO_BODY, s->hip_offset[LEFT], s->hipd[LEFT] );
      srsspos( SS_TORSO_BODY, s->hip_offset[RIGHT], s->hip[RIGHT] );
      offset_x( s, s->hip[RIGHT] );
      srssvel( SS_TORSO_BODY, s->hip_offset[RIGHT], s->hipd[RIGHT] );

      srsspos( SS_CALF2_BODY, s->foot_offset, s->foot[RIGHT] );
      offset_x( s, s->foot[RIGHT] );
      srssvel( SS_CALF2_BODY, s->foot_offset, s->footd[RIGHT] ); 

      s->knee_length[LEFT] = s->sdfast_state[SS_KNEE1];
      s->hip_angle[LEFT] = -s->sdfast_state[SS_HIP1];
      s->hip_angle[RIGHT] = s->sdfast_state[SS_HIP2];
      s->knee_length[RIGHT] = s->sdfast_state[SS_KNEE2];
      s->knee_lengthd[LEFT] = s->sdfast_state[SS_KNEE1D];
      s->hip_angled[LEFT] = -s->sdfast_state[SS_HIP1D];
      s->hip_angled[RIGHT] = s->sdfast_state[SS_HIP2D];
      s->knee_lengthd[RIGHT] = s->sdfast_state[SS_KNEE2D];
    }
  else
    {
      for( i = 0; i < 3; i++ )
	{
	  s->foot[RIGHT][i] = 0;
	  s->footd[RIGHT][i] = 0;
	}
      offset_x( s, s->foot[RIGHT] );
      srsspos( SS_CALF1_BODY, offset, s->calf[RIGHT] );
      offset_x( s, s->calf[RIGHT] );
      srssvel( SS_CALF1_BODY, offset, s->calfd[RIGHT] );
      srsspos( SS_THIGH1_BODY, offset, s->thigh[RIGHT] );
      offset_x( s, s->thigh[RIGHT] );
      srssvel( SS_THIGH1_BODY, offset, s->thighd[RIGHT] );
      srsspos( SS_THIGH2_BODY, offset, s->thigh[LEFT] );
      offset_x( s, s->thigh[LEFT] );
      srssvel( SS_THIGH2_BODY, offset, s->thighd[LEFT] );
      srsspos( SS_CALF2_BODY, offset, s->calf[LEFT] );
      offset_x( s, s->calf[LEFT] );
      srssvel( SS_CALF2_BODY, offset, s->calfd[LEFT] );

      offset[ZZ] = s->calf_cm;
      srsspos( SS_CALF1_BODY, offset, s->knee[RIGHT] );
      offset_x( s, s->knee[RIGHT] );
      srssvel( SS_CALF1_BODY, offset, s->kneed[RIGHT] );
      srsspos( SS_CALF2_BODY, offset, s->knee[LEFT] );
      offset_x( s, s->knee[LEFT] );
      srssvel( SS_CALF2_BODY, offset, s->kneed[LEFT] );

      srsspos( SS_TORSO_BODY, s->hip_offset[RIGHT], s->hip[RIGHT] );
      offset_x( s, s->hip[RIGHT] );
      srssvel( SS_TORSO_BODY, s->hip_offset[RIGHT], s->hipd[RIGHT] );
      srsspos( SS_TORSO_BODY, s->hip_offset[LEFT], s->hip[LEFT] );
      offset_x( s, s->hip[LEFT] );
      srssvel( SS_TORSO_BODY, s->hip_offset[LEFT], s->hipd[LEFT] );

      srsspos( SS_CALF2_BODY, s->foot_offset, s->foot[LEFT] );
      offset_x( s, s->foot[LEFT] );
      srssvel( SS_CALF2_BODY, s->foot_offset, s->footd[LEFT] ); 

      s->knee_length[RIGHT] = s->sdfast_state[SS_KNEE1];
      s->hip_angle[RIGHT] = -s->sdfast_state[SS_HIP1];
      s->hip_angle[LEFT] = s->sdfast_state[SS_HIP2];
      s->knee_length[LEFT] = s->sdfast_state[SS_KNEE2];
      s->knee_lengthd[RIGHT] = s->sdfast_state[SS_KNEE1D];
      s->hip_angled[RIGHT] = -s->sdfast_state[SS_HIP1D];
      s->hip_angled[LEFT] = s->sdfast_state[SS_HIP2D];
      s->knee_lengthd[LEFT] = s->sdfast_state[SS_KNEE2D];
    }

  srssprinterr(stderr);
  srssclearerr();

  forward_kinematics( s );
}

/************************************************************************/
/* Right foot down single support: convert sdfast to sim coordinates. */

void ssr_forward_kinematics( SIM *s )
{
  double offset[3] = { 0, 0, 0 };
  int i;

  if ( s->ss_foot_down != RIGHT )
    {
      fprintf( stderr, "ssr_forward_kinematics: wrong foot down: %d\n", s->ss_foot_down );
      exit( -1 );
    }

  srssrpos( SS_TORSO_BODY, offset, s->torso );
  offset_x( s, s->torso );
  srssrvel( SS_TORSO_BODY, offset, s->torsod );
  srssrpos( SS_TORSO_BODY, s->head_offset, s->head );
  offset_x( s, s->head );
  srssrvel( SS_TORSO_BODY, s->head_offset, s->headd );

  s->roll = -(s->sdfast_state[SS_ANKLE1] + s->sdfast_state[SS_HIP1]);
  s->rolld = -(s->sdfast_state[SS_ANKLE1D] + s->sdfast_state[SS_HIP1D]);

  for( i = 0; i < 3; i++ )
    {
      s->foot[RIGHT][i] = 0;
      s->footd[RIGHT][i] = 0;
    }
  offset_x( s, s->foot[RIGHT] );
  srssrpos( SS_CALF1_BODY, offset, s->calf[RIGHT] );
  offset_x( s, s->calf[RIGHT] );
  srssrvel( SS_CALF1_BODY, offset, s->calfd[RIGHT] );
  srssrpos( SS_THIGH1_BODY, offset, s->thigh[RIGHT] );
  offset_x( s, s->thigh[RIGHT] );
  srssrvel( SS_THIGH1_BODY, offset, s->thighd[RIGHT] );
  srssrpos( SS_THIGH2_BODY, offset, s->thigh[LEFT] );
  offset_x( s, s->thigh[LEFT] );
  srssrvel( SS_THIGH2_BODY, offset, s->thighd[LEFT] );
  srssrpos( SS_CALF2_BODY, offset, s->calf[LEFT] );
  offset_x( s, s->calf[LEFT] );
  srssrvel( SS_CALF2_BODY, offset, s->calfd[LEFT] );

  offset[ZZ] = s->calf_cm;
  srssrpos( SS_CALF1_BODY, offset, s->knee[RIGHT] );
  offset_x( s, s->knee[RIGHT] );
  srssrvel( SS_CALF1_BODY, offset, s->kneed[RIGHT] );
  srssrpos( SS_CALF2_BODY, offset, s->knee[LEFT] );
  offset_x( s, s->knee[LEFT] );
  srssrvel( SS_CALF2_BODY, offset, s->kneed[LEFT] );

  srssrpos( SS_TORSO_BODY, s->hip_offset[RIGHT], s->hip[RIGHT] );
  offset_x( s, s->hip[RIGHT] );
  srssrvel( SS_TORSO_BODY, s->hip_offset[RIGHT], s->hipd[RIGHT] );
  srssrpos( SS_TORSO_BODY, s->hip_offset[LEFT], s->hip[LEFT] );
  offset_x( s, s->hip[LEFT] );
  srssrvel( SS_TORSO_BODY, s->hip_offset[LEFT], s->hipd[LEFT] );
  
  srssrpos( SS_CALF2_BODY, s->foot_offset, s->foot[LEFT] );
  offset_x( s, s->foot[LEFT] );
  srssrvel( SS_CALF2_BODY, s->foot_offset, s->footd[LEFT] ); 
  
  s->knee_length[RIGHT] = s->sdfast_state[SS_KNEE1];
  s->hip_angle[RIGHT] = -s->sdfast_state[SS_HIP1];
  s->hip_angle[LEFT] = s->sdfast_state[SS_HIP2];
  s->knee_length[LEFT] = s->sdfast_state[SS_KNEE2];
  s->knee_lengthd[RIGHT] = s->sdfast_state[SS_KNEE1D];
  s->hip_angled[RIGHT] = -s->sdfast_state[SS_HIP1D];
  s->hip_angled[LEFT] = s->sdfast_state[SS_HIP2D];
  s->knee_lengthd[LEFT] = s->sdfast_state[SS_KNEE2D];

  srssrprinterr(stderr);
  srssrclearerr();

  forward_kinematics( s );
}

/************************************************************************/

set_ssl_state( SIM *s )
{
  int i, j;

  for( i = 0; i < SS_N_STATES; i++ )
    {
      s->sdfast_stated[i] = 0;
    }

  for( i = 0, j = SS_N_Q; i < SS_N_U; i++, j++ )
    { 
      s->sdfast_stated[i] = s->sdfast_state[j];
    }
  /*
  printf( "set_ssl_state state: %g %g %g %g %g\n",
	  s->sdfast_state[5], s->sdfast_state[6], s->sdfast_state[7], 
	  s->sdfast_state[8], s->sdfast_state[9] );
  printf( "set_ssl_state stated: %g %g %g %g %g\n",
	  s->sdfast_stated[0], s->sdfast_stated[1], s->sdfast_stated[2], 
	  s->sdfast_stated[3], s->sdfast_stated[4] );
  */
  srssstate( s->time, s->sdfast_state, s->sdfast_stated );
  s->status = STATUS_OK;
  srssprinterr(stderr);
  srssclearerr();

  ssl_forward_kinematics( s );

  s->foot_status[LEFT] = FOOT_ON_GROUND;
  s->foot_status[RIGHT] = FOOT_IN_AIR;
}

/************************************************************************/

set_ssr_state( SIM *s )
{
  int i, j;

  for( i = 0; i < SS_N_STATES; i++ )
    {
      s->sdfast_stated[i] = 0;
    }

  for( i = 0, j = SS_N_Q; i < SS_N_U; i++, j++ )
    { 
      s->sdfast_stated[i] = s->sdfast_state[j];
    }
  srssrstate( s->time, s->sdfast_state, s->sdfast_stated );
  s->status = STATUS_OK;
  srssrprinterr(stderr);
  srssrclearerr();

  ssr_forward_kinematics( s );

  s->foot_status[LEFT] = FOOT_IN_AIR;
  s->foot_status[RIGHT] = FOOT_ON_GROUND;
}

/************************************************************************/
/* Begin single support, right stance; from ds or in air */
/* Called from integrate_step: in air and ds */
// which_foot_down = -1 to let this subroutine figure it out. 

void init_state_one_foot_on_ground( SIM *s, int which_foot_down )
{
  int i, j;

  // debug_positions( s );

  /* User did not specify which foot is down */
  if ( which_foot_down < 0 )
    {
      /* Which foot is on the ground? */
      init_state_in_air( s );
      if (s->foot[LEFT][ZZ] <= s->foot[RIGHT][ZZ] )
	which_foot_down = LEFT;
      else
	which_foot_down = RIGHT;
    }

  if ( interactive )
    printf( "single support: %g; new foot down: %d\n", s->time,
	    which_foot_down );

  s->sdfast_model = SINGLE_SUPPORT;
  s->ss_foot_down = which_foot_down;

  /*
  printf( "%g %g %g %g %g %g %g %g %g %g\n",
	  s->sdfast_state[SS_ANKLE1],
	  s->sdfast_state[SS_KNEE1],
	  s->sdfast_state[SS_HIP1],
	  s->sdfast_state[SS_HIP2],
	  s->sdfast_state[SS_KNEE2],
	  s->sdfast_state[SS_ANKLE1D],
	  s->sdfast_state[SS_KNEE1D],
	  s->sdfast_state[SS_HIP1D],
	  s->sdfast_state[SS_HIP2D],
	  s->sdfast_state[SS_KNEE2D] );
  */
  /*
  printf( "%g %g %g %g %g %g %g %g %g %g\n",
	  s->sdfast_state[DS_ANKLE1],
	  s->sdfast_state[DS_KNEE1],
	  s->sdfast_state[DS_HIP1],
	  s->sdfast_state[DS_HIP2],
	  s->sdfast_state[DS_KNEE2],
	  s->sdfast_state[DS_ANKLE1D],
	  s->sdfast_state[DS_KNEE1D],
	  s->sdfast_state[DS_HIP1D],
	  s->sdfast_state[DS_HIP2D],
	  s->sdfast_state[DS_KNEE2D] );
  */

  for ( i = 0; i < SS_N_STATES; i++ )
    {
      s->sdfast_state[i] = 0;
      s->sdfast_stated[i] = 0;
    }

  /* Only use these variables, since impact2 only updates these */
  s->sdfast_state[SS_ANKLE1] = -s->roll + s->hip_angle[which_foot_down];
  s->sdfast_state[SS_KNEE1] = s->knee_length[which_foot_down];
  s->sdfast_state[SS_HIP1] = -s->hip_angle[which_foot_down];
  s->sdfast_state[SS_HIP2] = s->hip_angle[other_side(which_foot_down)];
  s->sdfast_state[SS_KNEE2] = s->knee_length[other_side(which_foot_down)];

  s->sdfast_state[SS_ANKLE1D] = -s->rolld + s->hip_angled[which_foot_down];
  s->sdfast_state[SS_KNEE1D] = s->knee_lengthd[which_foot_down];
  s->sdfast_state[SS_HIP1D] = -s->hip_angled[which_foot_down];
  s->sdfast_state[SS_HIP2D] = s->hip_angled[other_side(which_foot_down)];
  s->sdfast_state[SS_KNEE2D] = s->knee_lengthd[other_side(which_foot_down)];

  /*
  printf( "SS: %g %g %g %g %g %g %g %g %g %g\n",
	  s->sdfast_state[SS_ANKLE1],
	  s->sdfast_state[SS_KNEE1],
	  s->sdfast_state[SS_HIP1],
	  s->sdfast_state[SS_HIP2],
	  s->sdfast_state[SS_KNEE2],
	  s->sdfast_state[SS_ANKLE1D],
	  s->sdfast_state[SS_KNEE1D],
	  s->sdfast_state[SS_HIP1D],
	  s->sdfast_state[SS_HIP2D],
	  s->sdfast_state[SS_KNEE2D] );
  */

  s->x_offset = s->foot[which_foot_down][XX];

  if ( which_foot_down == LEFT )
    set_ssl_state( s );
  else
    set_ssr_state( s );

  // debug_positions( s );

  /*
  printf( "Press return to continue. 3 %g\n", s->time );
  getchar();
  */
}

/************************************************************************/

init_double_support_model( SIM *s )
{
  int i, j;
  double vector[3];
  double inertia[3][3];
  static int firsttime = 0;
  double inbtojoint[3];

  /* This is not needed

  b1dsmass( DS_TORSO_BODY, s->torso_mass );
  b1dsmass( DS_THIGH1_BODY, s->thigh_mass );
  b1dsmass( DS_THIGH2_BODY, s->thigh_mass );
  b1dsmass( DS_CALF1_BODY, s->calf_mass );
  b1dsmass( DS_CALF2_BODY, s->calf_mass );

  for ( i = 0; i < 3; i++ )
    {
      vector[i] = 0;
      for ( j = 0; j < 3; j++ )
	{
	  if ( i == j )
	    inertia[i][i] = 1.0;
	  else
	    inertia[i][j] = 0;
	}
    }
  inertia[ZZ][ZZ] = s->torso_I;
  b1dsiner( DS_TORSO_BODY, inertia );
  inertia[ZZ][ZZ] = s->thigh_I;
  b1dsiner( DS_THIGH1_BODY, inertia );
  b1dsiner( DS_THIGH2_BODY, inertia );
  inertia[ZZ][ZZ] = s->calf_I;
  b1dsiner( DS_CALF1_BODY, inertia );
  b1dsiner( DS_CALF2_BODY, inertia );

  if ( firsttime )
    {
      for( i = 0; i <= 5; i++ )
	{
	  b1dsgetbtj( i, vector );
	  printf( "ds btj %d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
    }

  vector[YY] = -(s->calf_length - s->calf_cm);
  b1dsbtj( DS_CALF1_BODY, vector );
  vector[YY] = -(s->thigh_length - s->thigh_cm);
  b1dsbtj( DS_THIGH1_BODY, vector );
  vector[YY] = -s->torso_cm;
  b1dsbtj( DS_TORSO_BODY, vector );
  vector[YY] = s->thigh_cm;
  b1dsbtj( DS_THIGH2_BODY, vector );
  vector[YY] = s->calf_cm;
  b1dsbtj( DS_CALF2_BODY, vector );
  vector[YY] = -(s->calf_length - s->calf_cm);
  b1dsbtj( DS_LOOP_BODY, vector );

  if ( firsttime )
    {
      for( i = 0; i <=5; i++ )
	{
	  b1dsgetbtj( i, vector );
	  printf( "ds btj %d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
    }

  if ( firsttime )
    {
      for( i = 0; i <=5; i++ )
	{
	  b1dsgetitj( i, vector );
	  printf( "ds itj %d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
    }

  vector[YY] = s->calf_cm;
  b1dsitj( DS_THIGH1_BODY, vector );
  vector[YY] = s->thigh_cm;
  b1dsitj( DS_TORSO_BODY, vector );
  vector[YY] = -s->torso_cm;
  b1dsitj( DS_THIGH2_BODY, vector );
  vector[YY] = -(s->thigh_length - s->thigh_cm);
  b1dsitj( DS_CALF2_BODY, vector );

  if ( firsttime )
    {
      for( i = 0; i <=5; i++ )
	{
	  b1dsgetitj( i, vector );
	  printf( "%d: %g %g %g\n", i, vector[0],
		  vector[1], vector[2] );
	}
      printf( "press return to continue.\n" );
      getchar();
    }

  firsttime = 0;
  */

  /* We have to initialize this model again when we know where
     the 2nd foot is. */

  inbtojoint[0] = 0;
  inbtojoint[1] = 0;
  inbtojoint[2] = 0;
  srdsitj( DS_LOOP_BODY, inbtojoint );
  srdsprinterr(stderr); 
  srdsclearerr(stderr);
  srdsstab( 2.0*s->sdfast_baumgarte, s->sdfast_baumgarte*s->sdfast_baumgarte ); 
  srdsinit(); 
  srdsprinterr(stderr);
  srdsclearerr(stderr);
}

/************************************************************************/
/* Double support: convert sdfast to sim coordinates. */

void ds_forward_kinematics( SIM *s )
{
  double offset[3] = { 0, 0, 0 };
  int i;

  srdspos( DS_TORSO_BODY, offset, s->torso );
  offset_x( s, s->torso );
  srdsvel( DS_TORSO_BODY, offset, s->torsod );
  srdspos( DS_TORSO_BODY, s->head_offset, s->head );
  offset_x( s, s->head );
  srdsvel( DS_TORSO_BODY, s->head_offset, s->headd );

  for( i = 0; i < 3; i++ )
    {
      s->foot[LEFT][i] = 0;
      s->footd[LEFT][i] = 0;
    }
  offset_x( s, s->foot[LEFT] );
  srdspos( DS_CALF1_BODY, offset, s->calf[LEFT] );
  offset_x( s, s->calf[LEFT] );
  srdsvel( DS_CALF1_BODY, offset, s->calfd[LEFT] );
  srdspos( DS_THIGH1_BODY, offset, s->thigh[LEFT] );
  offset_x( s, s->thigh[LEFT] );
  srdsvel( DS_THIGH1_BODY, offset, s->thighd[LEFT] );
  srdspos( DS_THIGH2_BODY, offset, s->thigh[RIGHT] );
  offset_x( s, s->thigh[RIGHT] );
  srdsvel( DS_THIGH2_BODY, offset, s->thighd[RIGHT] );
  srdspos( DS_CALF2_BODY, offset, s->calf[RIGHT] );
  offset_x( s, s->calf[RIGHT] );
  srdsvel( DS_CALF2_BODY, offset, s->calfd[RIGHT] );
  
  offset[ZZ] = s->calf_cm;
  srdspos( DS_CALF1_BODY, offset, s->knee[LEFT] );
  offset_x( s, s->knee[LEFT] );
  srdsvel( DS_CALF1_BODY, offset, s->kneed[LEFT] );
  srdspos( DS_CALF2_BODY, offset, s->knee[RIGHT] );
  offset_x( s, s->knee[RIGHT] );
  srdsvel( DS_CALF2_BODY, offset, s->kneed[RIGHT] );

  srdspos( DS_TORSO_BODY, s->hip_offset[LEFT], s->hip[LEFT] );
  offset_x( s, s->hip[LEFT] );
  srdsvel( DS_TORSO_BODY, s->hip_offset[LEFT], s->hipd[LEFT] );
  srdspos( DS_TORSO_BODY, s->hip_offset[RIGHT], s->hip[RIGHT] );
  offset_x( s, s->hip[RIGHT] );
  srdsvel( DS_TORSO_BODY, s->hip_offset[RIGHT], s->hipd[RIGHT] );

  srdspos( DS_CALF2_BODY, s->foot_offset, s->foot[RIGHT] );
  offset_x( s, s->foot[RIGHT] );
  srdsvel( DS_CALF2_BODY, s->foot_offset, s->footd[RIGHT] ); 
  /* Hack to keep foot2 above ground. */
  if ( s->foot[RIGHT][ZZ] < 0 )
    s->foot[RIGHT][ZZ] = 0;

  s->roll = -(s->sdfast_state[DS_ANKLE1] + s->sdfast_state[DS_HIP1]);
  s->knee_length[LEFT] = s->sdfast_state[DS_KNEE1];
  s->hip_angle[LEFT] = -s->sdfast_state[DS_HIP1];
  s->hip_angle[RIGHT] = s->sdfast_state[DS_HIP2];
  s->knee_length[RIGHT] = s->sdfast_state[DS_KNEE2];
  s->rolld = -(s->sdfast_state[DS_ANKLE1D] + s->sdfast_state[DS_HIP1D]);
  s->knee_lengthd[LEFT] = s->sdfast_state[DS_KNEE1D];
  s->hip_angled[LEFT] = -s->sdfast_state[DS_HIP1D];
  s->hip_angled[RIGHT] = s->sdfast_state[DS_HIP2D];
  s->knee_lengthd[RIGHT] = s->sdfast_state[DS_KNEE2D];

  /*
  printf( "%g %g %g %g %g\n", s->time, s->sdfast_state[DS_KNEE1], s->sdfast_state[DS_KNEE2],
	  s->hip[LEFT][ZZ], s->hip[RIGHT][ZZ] ); 
  */

  forward_kinematics( s );
}

/************************************************************************/
/* Need to handle xd, zd, and rolld requests correctly */
/* called from integrate_step: air */

// This subroutine expects sdfast_state to be set.

void ds_init_double_support_state_aux( SIM *s )
{
  double r_foot[3];
  double inbtojoint[3];
  double distance;
  double tt = 0.0;
  double tol = 1e-5;
  double angle_offset;
  int lock[DS_N_U];
  int maxevals = 10;
  int fcnt;
  int err;
  int i, j;

  /*
  printf( "set_state: %g; %g %g %g %g %g; %g %g %g %g %g\n",
	  s->time, 
	  s->sdfast_state[0],
	  s->sdfast_state[1],
	  s->sdfast_state[2],
	  s->sdfast_state[3],
	  s->sdfast_state[4],
	  s->sdfast_state[5],
	  s->sdfast_state[6],
	  s->sdfast_state[7],
	  s->sdfast_state[8],
	  s->sdfast_state[9] );
  */

  /*
    1) Figure out distance between feet.
    2) Figure out x for left foot.
  */

  r_foot[XX] = 
    -(s->calf_length + s->thigh_length + s->sdfast_state[DS_KNEE1])
    *sin( s->sdfast_state[DS_ANKLE1] ) 
    + s->pelvis_width*cos( s->sdfast_state[DS_ANKLE1]
			   + s->sdfast_state[DS_HIP1] ) 
    + (s->calf_length + s->thigh_length + s->sdfast_state[DS_KNEE2])
    *sin( s->sdfast_state[DS_ANKLE1] + s->sdfast_state[DS_HIP1]
	  + s->sdfast_state[DS_HIP2] );
  r_foot[YY] = 0;
  r_foot[ZZ] = 
    (s->calf_length + s->thigh_length + s->sdfast_state[DS_KNEE1])
    *cos( s->sdfast_state[DS_ANKLE1] ) 
    + s->pelvis_width*sin( s->sdfast_state[DS_ANKLE1]
			   + s->sdfast_state[DS_HIP1] ) 
    - (s->calf_length + s->thigh_length + s->sdfast_state[DS_KNEE2])
    *cos( s->sdfast_state[DS_ANKLE1] + s->sdfast_state[DS_HIP1]
	  + s->sdfast_state[DS_HIP2] );

  distance = sqrt(r_foot[XX]*r_foot[XX] + r_foot[ZZ]*r_foot[ZZ]);

  /* Need a signed distance */
  if ( r_foot[XX] < 0 )
    distance = -distance;

  /*
  printf( "r_foot: %g %g %g\n", r_foot[XX], r_foot[YY], r_foot[ZZ] );
  printf( "%g %g %g %g\n", s->calf_length, s->sdfast_state[DS_ANKLE1],
	  sin( s->sdfast_state[DS_ANKLE1] ), cos( s->sdfast_state[DS_ANKLE1] ) ); 
  printf( "Distance: %g\n", distance );
  */

  // CGA **** 
  /*
  printf( "OVERRIDING DISTANCE.\n" );
  distance = 0;
  */

  /* initialize SDFAST model two legs on the ground */
  inbtojoint[0] = distance;
  inbtojoint[1] = 0;
  inbtojoint[2] = 0;
  srdsitj( DS_LOOP_BODY, inbtojoint );
  srdsprinterr(stderr);
  srdsclearerr();
  srdsinit();
  srdsprinterr(stderr);
  srdsclearerr();

  /*
  printf( "init okay.\n" );
  */

  /*
    second_leg_down_transition();
  */

  // printf( "r_foot: %g %g\n", r_foot[XX], r_foot[ZZ] );

  /* Need to handle case where two feet together */
  if ( fabs( r_foot[XX] ) > FEET_TOGETHER )
    {
      if ( r_foot[XX] > 0 )
	angle_offset = atan2( r_foot[ZZ], r_foot[XX] );
      else
	angle_offset = atan2( -r_foot[ZZ], -r_foot[XX] );
      s->sdfast_state[DS_ANKLE1] -= angle_offset;
    }
  else
    {
      if ( fabs( r_foot[ZZ] ) > FEET_TOGETHER/10 )
	{
	  fprintf( stderr, "ds: rfoot problem: %g %g %g\n", 
		   r_foot[XX], r_foot[ZZ], FEET_TOGETHER );
	}
    };

  for( i = 0; i < DS_N_U; i++ )
    lock[i] = 0;

  /*
  printf( "Before assemble: %g %g %g %g %g\n", 
	  s->sdfast_state[0], s->sdfast_state[1], s->sdfast_state[2], s->sdfast_state[3], s->sdfast_state[4] );
  */

  srdsassemble( tt, s->sdfast_state, lock, tol, maxevals, &fcnt, &err );
  srdsprinterr(stderr);
  srdsclearerr();

  /*
  printf( "After assemble: %d %d: %g %g %g %g %g %g %g %g %g %g\n", err, fcnt,
	  s->sdfast_state[0], s->sdfast_state[1], s->sdfast_state[2], s->sdfast_state[3], s->sdfast_state[4],
	  s->sdfast_state[5], s->sdfast_state[6], s->sdfast_state[7], s->sdfast_state[8], s->sdfast_state[9] );
  */

  srdsinitvel( tt, s->sdfast_state, lock, tol, maxevals, &fcnt, &err );
  srdsprinterr(stderr);
  srdsclearerr();

  /*
  printf( "After initvel: %d %d: %g %g %g %g %g %g %g %g %g %g\n", err, fcnt,
	  s->sdfast_state[0], s->sdfast_state[1], s->sdfast_state[2], s->sdfast_state[3], s->sdfast_state[4],
	  s->sdfast_state[5], s->sdfast_state[6], s->sdfast_state[7], s->sdfast_state[8], s->sdfast_state[9] );
  */

  for( i = 0; i < DS_N_STATES; i++ )
    {
      s->sdfast_stated[i] = 0;
    }
  for( i = 0, j = DS_N_Q; i < DS_N_U; i++, j++ )
    { 
      s->sdfast_stated[i] = s->sdfast_state[j];
    }

  s->sdfast_model = DOUBLE_SUPPORT;
  s->x_offset = s->foot[LEFT][XX];
  s->status = STATUS_OK;
  s->foot_status[LEFT] = FOOT_ON_GROUND;
  s->foot_status[RIGHT] = FOOT_ON_GROUND;

  ds_forward_kinematics( s );

  /*
  printf( "Press return to continue. 1 %g\n", s->time );
  getchar();
  */
}

/************************************************************************/
/* Need to handle xd, zd, and rolld requests correctly */
/* called from integrate_step: air */

void ds_init_double_support_state( SIM *s )
{
  s->sdfast_state[DS_ANKLE1] = -s->roll + s->hip_angle[LEFT];
  s->sdfast_state[DS_KNEE1] = s->knee_length[LEFT];
  s->sdfast_state[DS_HIP1] = -s->hip_angle[LEFT];
  s->sdfast_state[DS_HIP2] = s->hip_angle[RIGHT];
  s->sdfast_state[DS_KNEE2] = s->knee_length[RIGHT];

  s->sdfast_state[DS_ANKLE1D] = -s->rolld + s->hip_angled[LEFT];
  s->sdfast_state[DS_KNEE1D] = s->knee_lengthd[LEFT];
  s->sdfast_state[DS_HIP1D] = -s->hip_angled[LEFT];
  s->sdfast_state[DS_HIP2D] = s->hip_angled[RIGHT];
  s->sdfast_state[DS_KNEE2D] = s->knee_lengthd[RIGHT];

  ds_init_double_support_state_aux( s );
}

/************************************************************************/

void init_state_two_feet_on_ground( SIM *s )
{

  ds_init_double_support_state( s );
  s->status = STATUS_OK;
}

/************************************************************************/
/* Initialize sdfast  */

void init_dynamics( SIM *s )
{
  int i, j;

  if ( AIR_N_STATES > MAX_N_SDFAST_STATE )
    {
      fprintf( stderr, 
	       "Need to increast MAX_N_SDFAST_STATE (%d) to be at least %d\n",
	       MAX_N_SDFAST_STATE, AIR_N_STATES );
      exit( -1 );
    }

  s->sdfast_model = INITIALIZE;

  init_in_air_model( s );
  init_single_support_models( s );
  init_double_support_model( s );

  for( i = 0; i < MAX_N_SDFAST_STATE; i++ )
    s->sdfast_state[i] = 0;

  // Initialize straight up */
  /* Not sure what x reference is going to be */
  s->torso[XX] = 0.0;
  s->groin[XX] = 0.0;
  s->foot[LEFT][XX] = 0;
  s->foot[RIGHT][XX] = 0;

  s->roll = 0.0;
  s->hip_angle[LEFT] = 0.0;
  s->hip_angle[RIGHT] = 0.0;
  s->knee_length[LEFT] = 0.0;
  s->knee_length[RIGHT] = 0.0;
  s->ankle_angle[LEFT] = 0.0;
  s->ankle_angle[RIGHT] = 0.0;

  // init_state_in_air( s, s->sdfast_state );
  // init_state_two_feet_on_ground( s );
  // init_state_one_foot_on_ground( s );

  // debug_positions( s );

  s->simulation_type = SDFAST_IMPACTS;
}

/************************************************************************/
/* Could be called from DS or SS */

void transition_to_air( SIM *s )
{

  if ( interactive )
    printf( "transition_to_air: %g\n", s->time );

  if ( s->in_air_not_allowed )
    {
      fprintf( stderr, "In air not allowed: %g\n", s->time );
      s->status = CRASHED;
      return;
    }

  init_state_in_air( s );
}

/************************************************************************/
/*************************************************************************/

double Power( double value, int n )
{
  if ( n != 2 )
    {
      fprintf( stderr, "Bad power: %g %d\n", value, n );
      exit( -1 );
    }
  return value*value;
}

/*************************************************************************/
/* Foot 1 is on the ground, and foot 2 just touched down.
Use TMT (JMJ) approach
~/research/impact-model/db2-ss-ds-lateral/srss.math
 */

ss_impact1( SIM *s, double *result )
{
  float **matrix();
  float *vector();
  float d;
  int n, indx[SS_IMPACT1_N+1];
  int i, j;

  // debug_jacobian1( s );

  if ( interactive )
    printf( "ss_impact1\n" );

  double a1, a2, a3, a4, a5;
  double a1d, a2d, a3d, a4d, a5d;
  double l1, l2, l3, l4, l5;
  double l1cm, l2cm, l3cm, l4cm, l5cm;
  double m[MAX_N_A_B][MAX_N_A_B];
  double v[MAX_N_A_B];
  double jacobian[MAX_N_A_B][MAX_N_A_B];
  double I1, I2, I3, I4, I5;
  double m1, m2, m3, m4, m5;

  double c1, s1;
  double c12, s12;
  double c123, s123;
  double c1234, s1234;
  double c12345, s12345;

  /*
  c1 = cos( a1 );
  s1 = sin( a1 );
  c12 = cos( a1 + a2 );
  s12 = sin( a1 + a2 );
  c123 = cos( a1 + a2 + a3 );
  s123 = sin( a1 + a2 + a3 );
  c1234 = cos( a1 + a2 + a3 + a4 );
  s1234 = sin( a1 + a2 + a3 + a4 );
  c12345 = cos( a1 + a2 + a3 + a4 + a5 );
  s12345 = sin( a1 + a2 + a3 + a4 + a5 );
  */

  a1 = s->sdfast_state[0];
  a2 = s->sdfast_state[1];
  a3 = s->sdfast_state[2];
  a4 = s->sdfast_state[3];
  a5 = s->sdfast_state[4];

  a1d = s->sdfast_state[5];
  a2d = s->sdfast_state[6];
  a3d = s->sdfast_state[7];
  a4d = s->sdfast_state[8];
  a5d = s->sdfast_state[9];

  l1 = s->calf_length;
  l2 = s->thigh_length;
  if ( s->ss_foot_down == LEFT )
    l3 = s->pelvis_width;
  else
    l3 = -s->pelvis_width;
  l4 = s->thigh_length;
  l5 = s->calf_length;

  l1cm = s->calf_length - s->calf_cm;
  l2cm = s->thigh_length - s->thigh_cm;
  l3cm = s->torso_cm;
  l4cm = s->thigh_cm;
  l5cm = s->calf_cm;

  m1 = s->calf_mass;
  m2 = s->thigh_mass;
  m3 = s->torso_mass;
  m4 = s->thigh_mass;
  m5 = s->calf_mass;

  I1 = s->calf_I;
  I2 = s->thigh_I;
  I3 = s->torso_I;
  I4 = s->thigh_I;
  I5 = s->calf_I;

  m[0][0] = 
   I1 + I2 + I3 + I4 + I5 + Power(l1cm,2)*m1 + Power(a2,2)*m2 + 2*a2*l1*m2 + 
    Power(l1,2)*m2 + 2*a2*l2cm*m2 + 2*l1*l2cm*m2 + Power(l2cm,2)*m2 + 
    Power(a2,2)*m3 + 2*a2*l1*m3 + Power(l1,2)*m3 + 2*a2*l2*m3 + 2*l1*l2*m3 + 
    Power(l2,2)*m3 + (Power(l3,2)*m3)/4. + Power(l3cm,2)*m3 + 
    Power(a2,2)*m4 + 2*a2*l1*m4 + Power(l1,2)*m4 + 2*a2*l2*m4 + 2*l1*l2*m4 + 
    Power(l2,2)*m4 + Power(l3,2)*m4 + Power(l4cm,2)*m4 + Power(a2,2)*m5 + 
    Power(a5,2)*m5 + 2*a2*l1*m5 + Power(l1,2)*m5 + 2*a2*l2*m5 + 2*l1*l2*m5 + 
    Power(l2,2)*m5 + Power(l3,2)*m5 + 2*a5*l4*m5 + Power(l4,2)*m5 + 
    2*a5*l5cm*m5 + 2*l4*l5cm*m5 + Power(l5cm,2)*m5 + 
    2*(a2 + l1 + l2)*l3cm*m3*cos(a3) - 
    2*(a2 + l1 + l2)*(l4cm*m4 + (a5 + l4 + l5cm)*m5)*cos(a3 + a4) + 
    a2*l3*m3*sin(a3) + l1*l3*m3*sin(a3) + l2*l3*m3*sin(a3) + 
    2*a2*l3*m4*sin(a3) + 2*l1*l3*m4*sin(a3) + 2*l2*l3*m4*sin(a3) + 
    2*a2*l3*m5*sin(a3) + 2*l1*l3*m5*sin(a3) + 2*l2*l3*m5*sin(a3) + 
    2*l3*l4cm*m4*sin(a4) + 2*a5*l3*m5*sin(a4) + 2*l3*l4*m5*sin(a4) + 
    2*l3*l5cm*m5*sin(a4);

  m[0][1] = 
   (l3*(m3 + 2*(m4 + m5))*cos(a3))/2. - l3cm*m3*sin(a3) + 
    (l4cm*m4 + (a5 + l4 + l5cm)*m5)*sin(a3 + a4);

  m[0][2] = 
   I3 + I4 + I5 + (Power(l3,2)*m3)/4. + Power(l3cm,2)*m3 + Power(l3,2)*m4 + 
    Power(l4cm,2)*m4 + Power(a5,2)*m5 + Power(l3,2)*m5 + 2*a5*l4*m5 + 
    Power(l4,2)*m5 + 2*a5*l5cm*m5 + 2*l4*l5cm*m5 + Power(l5cm,2)*m5 + 
    (a2 + l1 + l2)*l3cm*m3*cos(a3) - 
    (a2 + l1 + l2)*(l4cm*m4 + (a5 + l4 + l5cm)*m5)*cos(a3 + a4) + 
    (a2*l3*m3*sin(a3))/2. + (l1*l3*m3*sin(a3))/2. + (l2*l3*m3*sin(a3))/2. + 
    a2*l3*m4*sin(a3) + l1*l3*m4*sin(a3) + l2*l3*m4*sin(a3) + 
    a2*l3*m5*sin(a3) + l1*l3*m5*sin(a3) + l2*l3*m5*sin(a3) + 
    2*l3*l4cm*m4*sin(a4) + 2*a5*l3*m5*sin(a4) + 2*l3*l4*m5*sin(a4) + 
    2*l3*l5cm*m5*sin(a4);

  m[0][3] = 
   I4 + I5 + Power(l4cm,2)*m4 + Power(a5,2)*m5 + 2*a5*l4*m5 + 
    Power(l4,2)*m5 + 2*a5*l5cm*m5 + 2*l4*l5cm*m5 + Power(l5cm,2)*m5 - 
    (a2 + l1 + l2)*(l4cm*m4 + (a5 + l4 + l5cm)*m5)*cos(a3 + a4) + 
    l3*(l4cm*m4 + (a5 + l4 + l5cm)*m5)*sin(a4);

  m[0][4] =
    -(m5*(l3*cos(a4) + (a2 + l1 + l2)*sin(a3 + a4)));

  m[1][0] = m[0][1];

  m[1][1] = m2 + m3 + m4 + m5;

  m[1][2] = (l3*(m3 + 2*(m4 + m5))*cos(a3))/2. - l3cm*m3*sin(a3) + 
    (l4cm*m4 + (a5 + l4 + l5cm)*m5)*sin(a3 + a4);

  m[1][3] = (l4cm*m4 + (a5 + l4 + l5cm)*m5)*sin(a3 + a4);

  m[1][4] = -(m5*cos(a3 + a4));

  m[2][0] = m[0][2];

  m[2][1] = m[1][2];

  m[2][2] =    I3 + I4 + I5 + (Power(l3,2)*m3)/4. + Power(l3cm,2)*m3 + Power(l3,2)*m4 + 
    Power(l4cm,2)*m4 + Power(a5,2)*m5 + Power(l3,2)*m5 + 2*a5*l4*m5 + 
    Power(l4,2)*m5 + 2*a5*l5cm*m5 + 2*l4*l5cm*m5 + Power(l5cm,2)*m5 + 
    2*l3*(l4cm*m4 + (a5 + l4 + l5cm)*m5)*sin(a4);

  m[2][3] =    I4 + I5 + Power(l4cm,2)*m4 + Power(a5,2)*m5 + 2*a5*l4*m5 + 
    Power(l4,2)*m5 + 2*a5*l5cm*m5 + 2*l4*l5cm*m5 + Power(l5cm,2)*m5 + 
    l3*(l4cm*m4 + (a5 + l4 + l5cm)*m5)*sin(a4);

  m[2][4] = -(l3*m5*cos(a4));

  m[3][0] = m[0][3];

  m[3][1] = m[1][3];

  m[3][2] = m[2][3];

  m[3][3] =    I4 + I5 + Power(l4cm,2)*m4 + Power(a5,2)*m5 + 2*a5*l4*m5 + 
    Power(l4,2)*m5 + 2*a5*l5cm*m5 + 2*l4*l5cm*m5 + Power(l5cm,2)*m5;

  m[3][4] = 0;

  m[4][0] = m[0][4];

  m[4][1] = m[1][4];

  m[4][2] = m[2][4];

  m[4][3] = m[3][4];

  m[4][4] = m5;

  jacobian[0][0] = -((a2 + l1 + l2)*cos(a1)) 
    + (a5 + l4 + l5)*cos(a1 + a3 + a4) - l3*sin(a1 + a3);
  jacobian[0][1] = -sin(a1);
  jacobian[0][2] = (a5 + l4 + l5)*cos(a1 + a3 + a4) - l3*sin(a1 + a3);
  jacobian[0][3] = (a5 + l4 + l5)*cos(a1 + a3 + a4);
  jacobian[0][4] = sin(a1 + a3 + a4);

  jacobian[1][0] = l3*cos(a1 + a3) - (a2 + l1 + l2)*sin(a1) + 
     (a5 + l4 + l5)*sin(a1 + a3 + a4);
  jacobian[1][1] = cos(a1);
  jacobian[1][2] = l3*cos(a1 + a3) + (a5 + l4 + l5)*sin(a1 + a3 + a4);
  jacobian[1][3] = (a5 + l4 + l5)*sin(a1 + a3 + a4);
  jacobian[1][4] = -cos(a1 + a3 + a4);

  if ( a == NULL )
    a = matrix( 1, MAX_N_A_B, 1, MAX_N_A_B );
  if ( b == NULL )
    b = vector( 1, MAX_N_A_B );

  for( i = 1; i <= MAX_N_A_B; i++ )
    {
      b[i] = 0;
      for( j = 1; j <= MAX_N_A_B; j++ )
        {
          a[i][j] = 0;
        }
    }

  /* Multiply M and current velocities */
  for( i = 0; i < 5; i++ )
    {
      v[i] = m[i][0]*a1d + m[i][1]*a2d + m[i][2]*a3d + m[i][3]*a4d + m[i][4]*a5d;
    }

  /* Load M into A and M*vel into B */
  for( i = 1; i <= 5; i++ )
    {
      b[i] = v[i-1];
      for( j = 1; j <= 5; j++ )
        {
          a[i][j] = m[i-1][j-1];
        }
    }

  /* Load J and J^T into A */
  for( i = 0; i < 5; i++ )
    {
      for( j = 0; j < 2; j++ )
        {
	  a[6+j][1+i] = jacobian[j][i];
	  a[1+i][6+j] = jacobian[j][i];
        }
    }

  /*
  for( i = 1; i <= SS_IMPACT1_N; i++ )
    {
      for( j = 1; j <= SS_IMPACT1_N; j++ )
	printf( "%7.3f ", a[i][j] );
      printf( "; %7.3f\n", b[i] );
    }
  */

  n = SS_IMPACT1_N;

  ludcmp(a,n,indx,&d);
  lubksb(a,n,indx,b);

  for( i = 1; i <= n; i++ )
    {
      if ( impact1_print )
	printf( "JMJ1: %d: %g\n", i, b[i] );
      result[i-1] = b[i];
    }
}

/*************************************************************************/
/* Foot 1 is on the ground, and foot 2 just touched down
Use Wilhelm approach
~/research/impact-model/db2-ss-ds-lateral/srss-wilhelms.math
 */

ss_impact3( SIM *s, double *result )
{
  float **matrix();
  float *vector();
  float d;
  int n, indx[SS_IMPACT3_N+1];
  int i, j;

  printf( "ss_impact3\n" );

  double a1, a2, a3, a4, a5;
  double a1di, a2di, a3di, a4di, a5di;
  double a1d, a2d, a3d, a4d, a5d;
  double l1, l2, l3, l4, l5;
  double r1, r2, r3, r4, r5;
  double I1, I2, I3, I4, I5;
  double m1, m2, m3, m4, m5;
  double j1x, j1z, j2x, j2z, j3x, j3z, j4x, j4z, j5x, j5z, j6x, j6z;
  double p1x, p1z, p2x, p2z, p3x, p3z, p4x, p4z, p5x, p5z, p6x, p6z;

  a1 = s->sdfast_state[0];
  a2 = s->sdfast_state[1];
  a3 = s->sdfast_state[2];
  a4 = s->sdfast_state[3];
  a5 = s->sdfast_state[4];

  a1d = a1di = s->sdfast_state[5];
  a2d = a2di = s->sdfast_state[6];
  a3d = a3di = s->sdfast_state[7];
  a4d = a4di = s->sdfast_state[8];
  a5d = a5di = s->sdfast_state[9];

  l1 = s->calf_length;
  l2 = s->thigh_length;
  if ( s->ss_foot_down == LEFT )
    l3 = s->pelvis_width;
  else
    l3 = -s->pelvis_width;
  l4 = s->thigh_length;
  l5 = s->calf_length;

  r1 = s->calf_length - s->calf_cm;
  r2 = s->thigh_length - s->thigh_cm;
  r3 = s->torso_cm;
  r4 = s->thigh_cm;
  r5 = s->calf_cm;

  m1 = s->calf_mass;
  m2 = s->thigh_mass;
  m3 = s->torso_mass;
  m4 = s->thigh_mass;
  m5 = s->calf_mass;

  I1 = s->calf_I;
  I2 = s->thigh_I;
  I3 = s->torso_I;
  I4 = s->thigh_I;
  I5 = s->calf_I;

  if ( a == NULL )
    a = matrix( 1, MAX_N_A_B, 1, MAX_N_A_B );
  if ( b == NULL )
    b = vector( 1, MAX_N_A_B );

  for( i = 1; i <= MAX_N_A_B; i++ )
    {
      b[i] = 0;
      for( j = 1; j <= MAX_N_A_B; j++ )
        {
          a[i][j] = 0;
        }
    }

  j1x = s->x_offset;
  j1z = 0;
  j2x = j1x + -l1*sin(a1);
  j2z = j1z + l1*cos(a1);
  j3x = j2x + -(l2+a2)*sin(a1); 
  j3z = j2z + (l2+a2)*cos(a1);
  j4x = j3x + l3*cos(a1+a3);
  j4z = j3z + l3*sin(a1+a3);
  j5x = j4x + (l4+a5)*sin(a1+a3+a4);
  j5z = j4z + -(l4+a5)*cos(a1+a3+a4);
  j6x = j5x + l5*sin(a1+a3+a4);
  j6z = j5z + -l5*cos(a1+a3+a4);

  p1x = j1x + -r1*sin(a1);
  p1z = j1z + r1*cos(a1);
  p2x = j2x + -(r2+a2)*sin(a1);
  p2z = j2z + (r2+a2)*cos(a1);
  p3x = j3x + -r3*sin(a1+a3) + l3*cos(a1+a3)/2;
  p3z = j3z + r3*cos(a1+a3) + l3*sin(a1+a3)/2;
  p4x = j4x + r4*sin(a1+a3+a4);
  p4z = j4z + -r4*cos(a1+a3+a4);
  p5x = j5x + r5*sin(a1+a3+a4);
  p5z = j5z + -r5*cos(a1+a3+a4);

  printf( "DJ3: torso: %g %g\n",
	  p3x, p3z );
  printf( "DJ3: hips: %g %g; %g %g\n",
	  j3x, j3z, j4x, j4z );
  printf( "DJ3: thighs: %g %g; %g %g\n",
	  p2x, p2z, p4x, p4z );
  printf( "DJ3: knees: %g %g; %g %g\n",
	  j2x, j2z, j5x, j5z );
  printf( "DJ3: calves: %g %g; %g %g\n",
	  p1x, p1z, p5x, p5z );
  printf( "DJ3: feet: %g %g; %g %g\n",
	  j1x, j1z, j6x, j6z );
  printf( "DJ3: torsod: %g %g\n",
	  (-2*a1d*(a2 + l1 + l2)*cos(a1) - 2*(a1d + a3d)*r3*cos(a1 + a3) -
	   2*a2d*sin(a1) - a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3))/2.,
	  (2*a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) -
	   2*(a1d*(a2 + l1 + l2)*sin(a1) + (a1d + a3d)*r3*sin(a1 + a3)))/2. );
  printf( "DJ3: hipsd: %g %g; %g %g\n",
	  -(a1d*(a2 + l1 + l2)*cos(a1)) - a2d*sin(a1), 
	  a2d*cos(a1) - a1d*(a2 + l1 + l2)*sin(a1),
	  -(a1d*(a2 + l1 + l2)*cos(a1)) - a2d*sin(a1) 
	  - (a1d + a3d)*l3*sin(a1 + a3),
	  a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) 
	  - a1d*(a2 + l1 + l2)*sin(a1) );
  printf( "DJ3: thighds: %g %g; %g %g\n",
	  -(a1d*(a2 + l1 + r2)*cos(a1)) - a2d*sin(a1),
	  a2d*cos(a1) - a1d*(a2 + l1 + r2)*sin(a1),
	  -(a1d*(a2 + l1 + l2)*cos(a1)) 
	  + (a1d + a3d + a4d)*r4*cos(a1 + a3 + a4) -
	  a2d*sin(a1) - a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3),
	  a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) - a1d*a2*sin(a1) -
	  a1d*l1*sin(a1) - a1d*l2*sin(a1) + a1d*r4*sin(a1 + a3 + a4) +
	  a3d*r4*sin(a1 + a3 + a4) + a4d*r4*sin(a1 + a3 + a4) );
  printf( "DJ3: kneesd: %g %g; %g %g\n",
	  -(a1d*l1*cos(a1)), -(a1d*l1*sin(a1)),
	  -(a1d*(a2 + l1 + l2)*cos(a1)) +
	  (a1d + a3d + a4d)*(a5 + l4)*cos(a1 + a3 + a4) - a2d*sin(a1) -
	  a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3) + a5d*sin(a1 + a3 + a4),
	  a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) - a5d*cos(a1 + a3 + a4) -
	  a1d*a2*sin(a1) - a1d*l1*sin(a1) - a1d*l2*sin(a1) +
	  a1d*a5*sin(a1 + a3 + a4) + a3d*a5*sin(a1 + a3 + a4) +
	  a4d*a5*sin(a1 + a3 + a4) + a1d*l4*sin(a1 + a3 + a4) +
	  a3d*l4*sin(a1 + a3 + a4) + a4d*l4*sin(a1 + a3 + a4) );
  printf( "DJ3: calfds: %g %g; %g %g\n",
	  -(a1d*r1*cos(a1)), -(a1d*r1*sin(a1)),
	  -(a1d*(a2 + l1 + l2)*cos(a1)) +
	  (a1d + a3d + a4d)*(a5 + l4 + r5)*cos(a1 + a3 + a4) - a2d*sin(a1) -
	  a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3) + a5d*sin(a1 + a3 + a4),
	  a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) - a5d*cos(a1 + a3 + a4) -
	  a1d*a2*sin(a1) - a1d*l1*sin(a1) - a1d*l2*sin(a1) +
	  a1d*a5*sin(a1 + a3 + a4) + a3d*a5*sin(a1 + a3 + a4) +
	  a4d*a5*sin(a1 + a3 + a4) + a1d*l4*sin(a1 + a3 + a4) +
	  a3d*l4*sin(a1 + a3 + a4) + a4d*l4*sin(a1 + a3 + a4) +
	  a1d*r5*sin(a1 + a3 + a4) + a3d*r5*sin(a1 + a3 + a4) +
	  a4d*r5*sin(a1 + a3 + a4) );
  printf( "DJ3: feetd: %g %g; %g %g\n",
	  0.0, 0.0, -(a1d*(a2 + l1 + l2)*cos(a1)) +
	  (a1d + a3d + a4d)*(a5 + l4 + l5)*cos(a1 + a3 + a4) - a2d*sin(a1) -
	  a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3) + a5d*sin(a1 + a3 + a4),
	  a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) - a5d*cos(a1 + a3 + a4) -
	  a1d*a2*sin(a1) - a1d*l1*sin(a1) - a1d*l2*sin(a1) +
	  a1d*a5*sin(a1 + a3 + a4) + a3d*a5*sin(a1 + a3 + a4) +
	  a4d*a5*sin(a1 + a3 + a4) + a1d*l4*sin(a1 + a3 + a4) +
	  a3d*l4*sin(a1 + a3 + a4) + a4d*l4*sin(a1 + a3 + a4) +
	  a1d*l5*sin(a1 + a3 + a4) + a3d*l5*sin(a1 + a3 + a4) +
	  a4d*l5*sin(a1 + a3 + a4) );

  a[1][1] = -(m1*r1*cos(a1));
  a[1][6] = 1;
  a[1][8] = cos(a1);

  a[2][1] = -(m1*r1*sin(a1));
  a[2][7] = 1;
  a[2][8] = sin(a1);

  a[3][1] = -(a2*m2*cos(a1)) - l1*m2*cos(a1) - m2*r2*cos(a1);
  a[3][2] = -(m2*sin(a1));
  a[3][8] = -cos(a1);
  a[3][10] = -1;

  a[4][1] = -(a2*m2*sin(a1)) - l1*m2*sin(a1) - m2*r2*sin(a1);
  a[4][2] = m2*cos(a1);
  a[4][8]= -sin(a1);
  a[4][11] = -1;

  a[5][1] = -(a2*m3*cos(a1)) - l1*m3*cos(a1) - l2*m3*cos(a1)
    - m3*r3*cos(a1 + a3) - (l3*m3*sin(a1 + a3))/2.;
  a[5][2] = -(m3*sin(a1));
  a[5][3] = -(m3*r3*cos(a1 + a3)) - (l3*m3*sin(a1 + a3))/2.;
  a[5][10] = 1;
  a[5][12] = -1;

  a[6][1] = (l3*m3*cos(a1 + a3))/2. - a2*m3*sin(a1) - l1*m3*sin(a1)
    - l2*m3*sin(a1) - m3*r3*sin(a1 + a3);
  a[6][2] = m3*cos(a1);
  a[6][3] = (l3*m3*cos(a1 + a3))/2. - m3*r3*sin(a1 + a3);
  a[6][11] = 1;
  a[6][13] = -1;

  a[7][1] = -(a2*m4*cos(a1)) - l1*m4*cos(a1) - l2*m4*cos(a1) +
    m4*r4*cos(a1 + a3 + a4) - l3*m4*sin(a1 + a3);
  a[7][2] = -(m4*sin(a1));
  a[7][3] = m4*r4*cos(a1 + a3 + a4) - l3*m4*sin(a1 + a3);
  a[7][4] = m4*r4*cos(a1 + a3 + a4);
  a[7][12] = 1;
  a[7][14] = -cos(a1 + a3 + a4);

  a[8][1] = l3*m4*cos(a1 + a3) - a2*m4*sin(a1) - l1*m4*sin(a1)
    - l2*m4*sin(a1) + m4*r4*sin(a1 + a3 + a4);
  a[8][2] = m4*cos(a1);
  a[8][3] = l3*m4*cos(a1 + a3) + m4*r4*sin(a1 + a3 + a4);
  a[8][4] = m4*r4*sin(a1 + a3 + a4);
  a[8][13] = 1;
  a[8][14] = -sin(a1 + a3 + a4);

  a[9][1] = -(a2*m5*cos(a1)) - l1*m5*cos(a1) - l2*m5*cos(a1) +
    a5*m5*cos(a1 + a3 + a4) + l4*m5*cos(a1 + a3 + a4) +
    m5*r5*cos(a1 + a3 + a4) - l3*m5*sin(a1 + a3);
  a[9][2] = -(m5*sin(a1));
  a[9][3] = a5*m5*cos(a1 + a3 + a4) + l4*m5*cos(a1 + a3 + a4) +
    m5*r5*cos(a1 + a3 + a4) - l3*m5*sin(a1 + a3);
  a[9][4] = a5*m5*cos(a1 + a3 + a4) + l4*m5*cos(a1 + a3 + a4)
    + m5*r5*cos(a1 + a3 + a4);
  a[9][5] = m5*sin(a1 + a3 + a4);
  a[9][14] = cos(a1 + a3 + a4);
  a[9][16] = -1;

  a[10][1] = l3*m5*cos(a1 + a3) - a2*m5*sin(a1) - l1*m5*sin(a1)
    - l2*m5*sin(a1) + a5*m5*sin(a1 + a3 + a4) + l4*m5*sin(a1 + a3 + a4) +
    m5*r5*sin(a1 + a3 + a4);
  a[10][2] = m5*cos(a1);
  a[10][3] = l3*m5*cos(a1 + a3) + a5*m5*sin(a1 + a3 + a4)
    + l4*m5*sin(a1 + a3 + a4) + m5*r5*sin(a1 + a3 + a4);
  a[10][4] = a5*m5*sin(a1 + a3 + a4) + l4*m5*sin(a1 + a3 + a4)
    + m5*r5*sin(a1 + a3 + a4);
  a[10][5] = -(m5*cos(a1 + a3 + a4));
  a[10][14] = sin(a1 + a3 + a4);
  a[10][17] = -1;

  a[11][1] = I1;
  a[11][6] = r1*cos(a1);
  a[11][7] = r1*sin(a1);
  a[11][8] = -(l1*Power(cos(a1),2)) + r1*Power(cos(a1),2)
    - l1*Power(sin(a1),2) + r1*Power(sin(a1),2);
  a[11][9] = -1;

  a[12][1] = I2;
  a[12][8] = -(a2*Power(cos(a1),2)) - r2*Power(cos(a1),2)
    - a2*Power(sin(a1),2) - r2*Power(sin(a1),2);
  a[12][9] = 1;
  a[12][10] = l2*cos(a1) - r2*cos(a1);
  a[12][11] = l2*sin(a1) - r2*sin(a1);

  a[13][1] = I3;
  a[13][3] = I3;
  a[13][10] = r3*cos(a1 + a3) + (l3*sin(a1 + a3))/2.;
  a[13][11] = -(l3*cos(a1 + a3))/2. + r3*sin(a1 + a3);
  a[13][12] = -(r3*cos(a1 + a3)) + (l3*sin(a1 + a3))/2.;
  a[13][13] = -(l3*cos(a1 + a3))/2. - r3*sin(a1 + a3);

  a[14][1] = I4;
  a[14][3] = I4;
  a[14][4] = I4;
  a[14][12] = -(r4*cos(a1 + a3 + a4));
  a[14][13] = -(r4*sin(a1 + a3 + a4));
  a[14][14] = -(a5*Power(cos(a1 + a3 + a4),2)) 
    - l4*Power(cos(a1 + a3 + a4),2) +
    r4*Power(cos(a1 + a3 + a4),2) - a5*Power(sin(a1 + a3 + a4),2) -
    l4*Power(sin(a1 + a3 + a4),2) + r4*Power(sin(a1 + a3 + a4),2);
  a[14][15] = -1;

  a[15][1] = I5;
  a[15][3] = I5;
  a[15][4] = I5;
  a[15][14] = -(r5*Power(cos(a1 + a3 + a4),2)) - r5*Power(sin(a1 + a3 + a4),2);
  a[15][15] = 1;
  a[15][16] = -(l5*cos(a1 + a3 + a4)) + r5*cos(a1 + a3 + a4);
  a[15][17] = -(l5*sin(a1 + a3 + a4)) + r5*sin(a1 + a3 + a4);

  a[16][1] =   -(a2*cos(a1)) - l1*cos(a1) - l2*cos(a1)
    + a5*cos(a1 + a3 + a4) +
    l4*cos(a1 + a3 + a4) + l5*cos(a1 + a3 + a4) - l3*sin(a1 + a3);
  a[16][2] = -sin(a1);
  a[16][3] = a5*cos(a1 + a3 + a4) + l4*cos(a1 + a3 + a4)
    + l5*cos(a1 + a3 + a4) - l3*sin(a1 + a3);
  a[16][4] = a5*cos(a1 + a3 + a4) + l4*cos(a1 + a3 + a4)
    + l5*cos(a1 + a3 + a4);
  a[16][5] = sin(a1 + a3 + a4);

  a[17][1] = l3*cos(a1 + a3) - a2*sin(a1) - l1*sin(a1) - l2*sin(a1) +
    a5*sin(a1 + a3 + a4) + l4*sin(a1 + a3 + a4) + l5*sin(a1 + a3 + a4);
  a[17][2] = cos(a1);
  a[17][3] = l3*cos(a1 + a3) + a5*sin(a1 + a3 + a4) + l4*sin(a1 + a3 + a4) +
    l5*sin(a1 + a3 + a4);
  a[17][4] = a5*sin(a1 + a3 + a4) + l4*sin(a1 + a3 + a4)
    + l5*sin(a1 + a3 + a4);
  a[17][5] = -cos(a1 + a3 + a4);

  b[1] = -(a1di*m1*r1*cos(a1));
  b[2] = -(a1di*m1*r1*sin(a1));
  b[3] = -(a1di*a2*m2*cos(a1)) - a1di*l1*m2*cos(a1) - a1di*m2*r2*cos(a1) -
    a2di*m2*sin(a1);
  b[4] = a2di*m2*cos(a1) - a1di*a2*m2*sin(a1) - a1di*l1*m2*sin(a1) -
    a1di*m2*r2*sin(a1);
  b[5] = -(a1di*a2*m3*cos(a1)) - a1di*l1*m3*cos(a1) - a1di*l2*m3*cos(a1) -
    a1di*m3*r3*cos(a1 + a3) - a3di*m3*r3*cos(a1 + a3) - a2di*m3*sin(a1) -
    (a1di*l3*m3*sin(a1 + a3))/2. - (a3di*l3*m3*sin(a1 + a3))/2.;
  b[6] = a2di*m3*cos(a1) + (a1di*l3*m3*cos(a1 + a3))/2. +
    (a3di*l3*m3*cos(a1 + a3))/2. - a1di*a2*m3*sin(a1) - a1di*l1*m3*sin(a1) -
    a1di*l2*m3*sin(a1) - a1di*m3*r3*sin(a1 + a3) - a3di*m3*r3*sin(a1 + a3);
  b[7] = -(a1di*a2*m4*cos(a1)) - a1di*l1*m4*cos(a1) - a1di*l2*m4*cos(a1) +
    a1di*m4*r4*cos(a1 + a3 + a4) + a3di*m4*r4*cos(a1 + a3 + a4) +
    a4di*m4*r4*cos(a1 + a3 + a4) - a2di*m4*sin(a1) -
    a1di*l3*m4*sin(a1 + a3) - a3di*l3*m4*sin(a1 + a3);
  b[8] = a2di*m4*cos(a1) + a1di*l3*m4*cos(a1 + a3) + a3di*l3*m4*cos(a1 + a3) -
    a1di*a2*m4*sin(a1) - a1di*l1*m4*sin(a1) - a1di*l2*m4*sin(a1) +
    a1di*m4*r4*sin(a1 + a3 + a4) + a3di*m4*r4*sin(a1 + a3 + a4) +
    a4di*m4*r4*sin(a1 + a3 + a4);
  b[9] = -(a1di*a2*m5*cos(a1)) - a1di*l1*m5*cos(a1) - a1di*l2*m5*cos(a1) +
    a1di*a5*m5*cos(a1 + a3 + a4) + a3di*a5*m5*cos(a1 + a3 + a4) +
    a4di*a5*m5*cos(a1 + a3 + a4) + a1di*l4*m5*cos(a1 + a3 + a4) +
    a3di*l4*m5*cos(a1 + a3 + a4) + a4di*l4*m5*cos(a1 + a3 + a4) +
    a1di*m5*r5*cos(a1 + a3 + a4) + a3di*m5*r5*cos(a1 + a3 + a4) +
    a4di*m5*r5*cos(a1 + a3 + a4) - a2di*m5*sin(a1) -
    a1di*l3*m5*sin(a1 + a3) - a3di*l3*m5*sin(a1 + a3) +
    a5di*m5*sin(a1 + a3 + a4);
  b[10] = a2di*m5*cos(a1) + a1di*l3*m5*cos(a1 + a3) + a3di*l3*m5*cos(a1 + a3) -
    a5di*m5*cos(a1 + a3 + a4) - a1di*a2*m5*sin(a1) - a1di*l1*m5*sin(a1) -
    a1di*l2*m5*sin(a1) + a1di*a5*m5*sin(a1 + a3 + a4) +
    a3di*a5*m5*sin(a1 + a3 + a4) + a4di*a5*m5*sin(a1 + a3 + a4) +
    a1di*l4*m5*sin(a1 + a3 + a4) + a3di*l4*m5*sin(a1 + a3 + a4) +
    a4di*l4*m5*sin(a1 + a3 + a4) + a1di*m5*r5*sin(a1 + a3 + a4) +
    a3di*m5*r5*sin(a1 + a3 + a4) + a4di*m5*r5*sin(a1 + a3 + a4);
  b[11] = a1di*I1;
  b[12] = a1di*I2;
  b[13] = a1di*I3 + a3di*I3;
  b[14] = a1di*I4 + a3di*I4 + a4di*I4;
  b[15] = a1di*I5 + a3di*I5 + a4di*I5;
  b[16] = 0;
  b[17] = 0;

  n = SS_IMPACT3_N;

  ludcmp(a,n,indx,&d);
  lubksb(a,n,indx,b);

  for( i = 1; i <= n; i++ )
    {
      printf( "MATH1: %d: %g\n", i, b[i] );
      result[i-1] = b[i];
    }
}

/************************************************************************/
/* This impact analysis assumes robot in air and one foot hits */
/* Assumes that the foot != s->ss_foot_down is the impact foot
~/research/impact-model/db2-air-lateral/sra.math
 */

ss_impact2( SIM *s, double *result )
{
  float **matrix();
  float *vector();
  float d;
  int n, indx[SS_IMPACT2_N+1];
  int i, j;

  if ( interactive )
    printf( "ss_impact2\n" );

  double m[MAX_N_A_B][MAX_N_A_B];
  double v[MAX_N_A_B];
  double jacobian[MAX_N_A_B][MAX_N_A_B];
  double I1, I2, I3, I4, I5;
  double m1, m2, m3, m4, m5;
  double x, y, a1, a2, a3, a4, a5;
  double xd, yd, a1d, a2d, a3d, a4d, a5d;
  double l1, l2, l3, l4, l5;
  double l1cm, l2cm, l3cm, l4cm, l5cm;

  x = s->torso[XX];
  y = s->torso[ZZ];
  a1 = -s->roll;
  a2 = s->hip_angle[LEFT];
  a3 = s->knee_length[LEFT];
  a4 = s->hip_angle[RIGHT];
  a5 = s->knee_length[RIGHT];

  xd = s->torsod[XX];
  yd = s->torsod[ZZ];
  a1d = -s->rolld;
  a2d = s->hip_angled[LEFT];
  a3d = s->knee_lengthd[LEFT];
  a4d = s->hip_angled[RIGHT];
  a5d = s->knee_lengthd[RIGHT];

  /*
  if ( s->ss_foot_down == LEFT )
    l1 = s->pelvis_width;
  else
    l1 = -s->pelvis_width;
  */
  l1 = s->pelvis_width;
  l2 = s->thigh_length;
  l3 = s->thigh_length; // yes, for some reason I changed the order
  l4 = s->calf_length;
  l5 = s->calf_length;

  l1cm = s->torso_cm;
  l2cm = s->thigh_cm;
  l3cm = s->calf_cm;
  l4cm = s->thigh_cm;
  l5cm = s->calf_cm;

  m1 = s->torso_mass;
  m2 = s->thigh_mass;
  m3 = s->calf_mass;
  m4 = s->thigh_mass;
  m5 = s->calf_mass;

  I1 = s->torso_I;
  I2 = s->thigh_I;
  I3 = s->calf_I;
  I4 = s->thigh_I;
  I5 = s->calf_I;

  m[0][0] = m1 + m2 + m3 + m4 + m5;

  m[0][1] = 0;

  m[0][2] = l1cm*(m2 + m3 + m4 + m5)*cos(a1) + 
    (l2cm*m2 + (a3 + l2 + l3cm)*m3)*cos(a1 + a2) + 
    (2*(l4cm*m4 + (a5 + l3 + l5cm)*m5)*cos(a1 + a4) + 
       l1*(m2 + m3 - m4 - m5)*sin(a1))/2.;

  m[0][3] = (l2cm*m2 + (a3 + l2 + l3cm)*m3)*cos(a1 + a2);

  m[0][4] = m3*sin(a1 + a2);

  m[0][5] = (l4cm*m4 + (a5 + l3 + l5cm)*m5)*cos(a1 + a4);

  m[0][6] = m5*sin(a1 + a4);

  m[1][0] = m[0][1];

  m[1][1] = m1 + m2 + m3 + m4 + m5;

  m[1][2] = -(l1*(m2 + m3 - m4 - m5)*cos(a1))/2. + l1cm*(m2 + m3 + m4 + m5)*sin(a1) + 
    l2cm*m2*sin(a1 + a2) + a3*m3*sin(a1 + a2) + l2*m3*sin(a1 + a2) + 
    l3cm*m3*sin(a1 + a2) + l4cm*m4*sin(a1 + a4) + a5*m5*sin(a1 + a4) + 
    l3*m5*sin(a1 + a4) + l5cm*m5*sin(a1 + a4);

  m[1][3] = (l2cm*m2 + (a3 + l2 + l3cm)*m3)*sin(a1 + a2);

  m[1][4] = -(m3*cos(a1 + a2));

  m[1][5] = (l4cm*m4 + (a5 + l3 + l5cm)*m5)*sin(a1 + a4);

  m[1][6] = -(m5*cos(a1 + a4));

  m[2][0] = m[0][2];

  m[2][1] = m[1][2];

  m[2][2] =  I1 + I2 + I3 + I4 + I5 + (Power(l1,2)*m2)/4. + Power(l1cm,2)*m2 + 
    Power(l2cm,2)*m2 + Power(a3,2)*m3 + (Power(l1,2)*m3)/4. + 
    Power(l1cm,2)*m3 + 2*a3*l2*m3 + Power(l2,2)*m3 + 2*a3*l3cm*m3 + 
    2*l2*l3cm*m3 + Power(l3cm,2)*m3 + (Power(l1,2)*m4)/4. + 
    Power(l1cm,2)*m4 + Power(l4cm,2)*m4 + Power(a5,2)*m5 + 
    (Power(l1,2)*m5)/4. + Power(l1cm,2)*m5 + 2*a5*l3*m5 + Power(l3,2)*m5 + 
    2*a5*l5cm*m5 + 2*l3*l5cm*m5 + Power(l5cm,2)*m5 + 
    2*l1cm*(l2cm*m2 + (a3 + l2 + l3cm)*m3)*cos(a2) + 
    2*l1cm*(l4cm*m4 + (a5 + l3 + l5cm)*m5)*cos(a4) - l1*l2cm*m2*sin(a2) - 
    a3*l1*m3*sin(a2) - l1*l2*m3*sin(a2) - l1*l3cm*m3*sin(a2) + 
    l1*l4cm*m4*sin(a4) + a5*l1*m5*sin(a4) + l1*l3*m5*sin(a4) + 
    l1*l5cm*m5*sin(a4);

  m[2][3] =  I2 + I3 + Power(l2cm,2)*m2 + Power(a3,2)*m3 + 2*a3*l2*m3 + 
    Power(l2,2)*m3 + 2*a3*l3cm*m3 + 2*l2*l3cm*m3 + Power(l3cm,2)*m3 + 
    l1cm*(l2cm*m2 + (a3 + l2 + l3cm)*m3)*cos(a2) - 
    (l1*(l2cm*m2 + (a3 + l2 + l3cm)*m3)*sin(a2))/2.;

  m[2][4] = (l1*m3*cos(a2))/2. + l1cm*m3*sin(a2);

  m[2][5] =  l1cm*(l4cm*m4 + (a5 + l3 + l5cm)*m5)*cos(a4) + 
    (2*(I4 + I5 + Power(l4cm,2)*m4 + Power(a5,2)*m5 + 2*a5*l3*m5 + 
          Power(l3,2)*m5 + 2*a5*l5cm*m5 + 2*l3*l5cm*m5 + Power(l5cm,2)*m5) + 
     l1*(l4cm*m4 + (a5 + l3 + l5cm)*m5)*sin(a4))/2.;

  m[2][6] = -(l1*m5*cos(a4))/2. + l1cm*m5*sin(a4);

  m[3][0] = m[0][3];

  m[3][1] = m[1][3];

  m[3][2] = m[2][3];

  m[3][3] =  I2 + I3 + Power(l2cm,2)*m2 + Power(a3,2)*m3 + 2*a3*l2*m3 + 
    Power(l2,2)*m3 + 2*a3*l3cm*m3 + 2*l2*l3cm*m3 + Power(l3cm,2)*m3;

  m[3][4] = 0;

  m[3][5] = 0;

  m[3][6] = 0;

  m[4][0] = m[0][4];

  m[4][1] = m[1][4];

  m[4][2] = m[2][4];

  m[4][3] = m[3][4];

  m[4][4] = m3;

  m[4][5] = 0;

  m[4][6] = 0;

  m[5][0] = m[0][5];

  m[5][1] = m[1][5];

  m[5][2] = m[2][5];

  m[5][3] = m[3][5];

  m[5][4] = m[4][5];

  m[5][5] = I4 + I5 + Power(l4cm,2)*m4 + Power(a5,2)*m5 + 2*a5*l3*m5 + 
    Power(l3,2)*m5 + 2*a5*l5cm*m5 + 2*l3*l5cm*m5 + Power(l5cm,2)*m5;

  m[5][6] = 0;

  m[6][0] = m[0][6];

  m[6][1] = m[1][6];

  m[6][2] = m[2][6];

  m[6][3] = m[3][6];

  m[6][4] = m[4][6];

  m[6][5] = m[5][6];

  m[6][6] = m5;

  if ( s->ss_foot_down == RIGHT )
    { // left foot is impacting
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a3 + l2 + l4)*cos(a1 + a2) + (l1*sin(a1))/2.;
      jacobian[0][3] = (a3 + l2 + l4)*cos(a1 + a2);
      jacobian[0][4] = sin(a1 + a2);
      jacobian[0][5] = 0.0;
      jacobian[0][6] = 0.0;
      
      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = -(l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][3] = (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][4] = -cos(a1 + a2);
      jacobian[1][5] = 0.0;
      jacobian[1][6] = 0.0;
    }
  else
    {
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a5 + l3 + l5)*cos(a1 + a4) - (l1*sin(a1))/2.;
      jacobian[0][3] = 0.0;
      jacobian[0][4] = 0.0;
      jacobian[0][5] = (a5 + l3 + l5)*cos(a1 + a4);
      jacobian[0][6] = sin(a1 + a4);

      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = (l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][3] = 0.0;
      jacobian[1][4] = 0.0;
      jacobian[1][5] = (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][6] = -cos(a1 + a4);
    }

  if ( a == NULL )
    a = matrix( 1, MAX_N_A_B, 1, MAX_N_A_B );
  if ( b == NULL )
    b = vector( 1, MAX_N_A_B );

  for( i = 1; i <= MAX_N_A_B; i++ )
    {
      b[i] = 0;
      for( j = 1; j <= MAX_N_A_B; j++ )
        {
          a[i][j] = 0;
        }
    }

  /* Multiply M and current velocities */
  for( i = 0; i < 7; i++ )
    {
      v[i] = m[i][0]*xd + m[i][1]*yd + m[i][2]*a1d + m[i][3]*a2d
	+ m[i][4]*a3d + m[i][5]*a4d + m[i][6]*a5d;
    }

  /* Load M into A and M*vel into B */
  for( i = 1; i <= 7; i++ )
    {
      b[i] = v[i-1];
      for( j = 1; j <= 7; j++ )
        {
          a[i][j] = m[i-1][j-1];
        }
    }

  /* Load J and J^T into A */
  for( i = 0; i < 7; i++ )
    {
      for( j = 0; j < 2; j++ )
        {
	  a[8+j][1+i] = jacobian[j][i];
	  a[1+i][8+j] = jacobian[j][i];
        }
    }

  n = 7 + 2;

  /*
  for( i = 1; i <= n; i++ )
    {
      for( j = 1; j <= n; j++ )
	printf( "%7.3f ", a[i][j] );
      printf( "; %7.3f\n", b[i] );
    }
  */

  ludcmp(a,n,indx,&d);
  lubksb(a,n,indx,b);

  for( i = 1; i <= n; i++ )
    {
      result[i-1] = b[i];
    }

  /* Check velocity of foot on ground */
  for( i = 0; i < 2; i++ )
    {
      result[SS_IMPACT2_FOOTD2_X+i] = 0;
      for( j = 0; j < 7; j++ )
        {
	  result[SS_IMPACT2_FOOTD2_X+i] += jacobian[i][j]*result[j];
        }
    }

  if ( s->ss_foot_down == LEFT )
    { // left foot was on ground
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a3 + l2 + l4)*cos(a1 + a2) + (l1*sin(a1))/2.;
      jacobian[0][3] = (a3 + l2 + l4)*cos(a1 + a2);
      jacobian[0][4] = sin(a1 + a2);
      jacobian[0][5] = 0.0;
      jacobian[0][6] = 0.0;
      
      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = -(l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][3] = (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][4] = -cos(a1 + a2);
      jacobian[1][5] = 0.0;
      jacobian[1][6] = 0.0;
    }
  else
    {
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a5 + l3 + l5)*cos(a1 + a4) - (l1*sin(a1))/2.;
      jacobian[0][3] = 0.0;
      jacobian[0][4] = 0.0;
      jacobian[0][5] = (a5 + l3 + l5)*cos(a1 + a4);
      jacobian[0][6] = sin(a1 + a4);

      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = (l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][3] = 0.0;
      jacobian[1][4] = 0.0;
      jacobian[1][5] = (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][6] = -cos(a1 + a4);
    }

  for( i = 0; i < 2; i++ )
    {
      result[SS_IMPACT2_FOOTD_X+i] = 0;
      for( j = 0; j < 7; j++ )
        {
	  result[SS_IMPACT2_FOOTD_X+i] += jacobian[i][j]*result[j];
        }
    }

  /*
  footx = j1x + l4*s14 + l5*s145;
  footy = j1y - l4*c14 - l5*c145;
  footxd = b[1] + (b[3] + b[6])*l4*c14
    + (b[3] + b[6] + b[7])*l5*c145;
  footyd = b[2] + (b[3] + b[6])*l4*s14
    + (b[3] + b[6] + b[7])*l5*s145;

  result[17] = footx;
  result[18] = footy;
  result[19] = footxd;
  result[20] = footyd;
  result[21] = b[1];
  result[22] = b[2];
  result[23] = b[1] + (b[3] + b[6])*l4*c14;
  result[24] = b[2] + (b[3] + b[6])*l4*s14;
  */

  if ( impact2_print )
    for( i = 0; i < SS_IMPACT2_N; i++ )
      printf( "JMJ2: %d: %g\n", i, result[i] );
}

/************************************************************************/
/* This impact analysis assumes robot in air and one foot hits */
/* Assumes that the foot != s->ss_foot_down is the impact foot */
/* Uses Wilhelms approach 
~/research/impact-model/db2-air-lateral/sra-wilhelms.math
*/

ss_impact4( SIM *s, double *result )
{
  float **matrix();
  float *vector();
  float d;
  int n, indx[SS_IMPACT4_N+1];
  int i, j;

  printf( "ss_impact4\n" );

  double a1, a2, a3, a4, a5;
  double j1xd, j1zd, j1xdi, j1zdi;
  double a1d, a2d, a3d, a4d, a5d;
  double a1di, a2di, a3di, a4di, a5di;
  double l1, l2, l3, l4, l5;
  double l1cm, l2cm, l3cm, l4cm, l5cm;
  double I1, I2, I3, I4, I5;
  double m1, m2, m3, m4, m5;
  double j1x, j1z, j2x, j2z, j3x, j3z, j4x, j4z, j5x, j5z, j6x, j6z, j7x, j7z;
  double p1x, p1z, p2x, p2z, p3x, p3z, p4x, p4z, p5x, p5z, p6x, p6z;

  a1 = -s->roll;

  j1xdi = j1xd = s->torsod[XX];
  j1zdi = j1zd = s->torsod[ZZ];
  a1di = a1d = -s->rolld;

  l2 = s->thigh_length;
  l3 = s->calf_length;
  l4 = s->thigh_length;
  l5 = s->calf_length;

  l1cm = s->torso_cm;
  l2cm = s->thigh_cm;
  l3cm = s->calf_cm;
  l4cm = s->thigh_cm;
  l5cm = s->calf_cm;

  m1 = s->torso_mass;
  m2 = s->thigh_mass;
  m3 = s->calf_mass;
  m4 = s->thigh_mass;
  m5 = s->calf_mass;

  I1 = s->torso_I;
  I2 = s->thigh_I;
  I3 = s->calf_I;
  I4 = s->thigh_I;
  I5 = s->calf_I;
  
  if ( s->ss_foot_down == RIGHT )
    {
      // Left foot is impact foot.
      a2 = s->hip_angle[LEFT];
      a3 = s->knee_length[LEFT];
      a4 = s->hip_angle[RIGHT];
      a5 = s->knee_length[RIGHT];

      a2di = a2d = s->hip_angled[LEFT];
      a3di = a3d = s->knee_lengthd[LEFT];
      a4di = a4d = s->hip_angled[RIGHT];
      a5di = a5d = s->knee_lengthd[RIGHT];

      l1 = s->pelvis_width;
    }
  else
    {
      // Right foot is impact foot.

      a2 = s->hip_angle[RIGHT];
      a3 = s->knee_length[RIGHT];
      a4 = s->hip_angle[LEFT];
      a5 = s->knee_length[LEFT];

      a2di = a2d = s->hip_angled[RIGHT];
      a3di = a3d = s->knee_lengthd[RIGHT];
      a4di = a4d = s->hip_angled[LEFT];
      a5di = a5d = s->knee_lengthd[LEFT];

      l1 = -s->pelvis_width;
    }

  j1x = s->torso[XX];
  j1z = s->torso[ZZ];
  j2x = j1x + l1cm*sin(a1) - l1*cos(a1)/2;
  j2z = j1z + -l1cm*cos(a1) - l1*sin(a1)/2;
  j3x = j2x + (l2+a3)*sin(a1+a2);
  j3z = j2z + -(l2+a3)*cos(a1+a2);
  j6x = j3x + l3*sin(a1+a2);
  j6z = j3z + -l3*cos(a1+a2);
  j4x = j1x + l1cm*sin(a1) + l1*cos(a1)/2;
  j4z = j1z + -l1cm*cos(a1) + l1*sin(a1)/2;
  j5x = j4x + (l4+a5)*sin(a1+a4);
  j5z = j4z + -(l4+a5)*cos(a1+a4);
  j7x = j5x + l5*sin(a1+a4);
  j7z = j5z + -l5*cos(a1+a4);

  p1x = j1x;
  p1z = j1z;
  p2x = j2x + l2cm*sin(a1+a2);
  p2z = j2z + -l2cm*cos(a1+a2);
  p3x = j3x + l3cm*sin(a1+a2);
  p3z = j3z + -l3cm*cos(a1+a2);
  p4x = j4x + l4cm*sin(a1+a4);
  p4z = j4z + -l4cm*cos(a1+a4);
  p5x = j5x + l5cm*sin(a1+a4);
  p5z = j5z + -l5cm*cos(a1+a4);

  printf( "DJ4: torso: %g %g\n",
	  j1x, j1z );
  printf( "DJ4: hips: %g %g; %g %g\n",
	  j2x, j2z, j4x, j4z );
  printf( "DJ4: thighs: %g %g; %g %g\n",
	  p2x, p2z, p4x, p4z );
  printf( "DJ4: knees: %g %g; %g %g\n",
	  j3x, j3z, j5x, j5z );
  printf( "DJ4: calves: %g %g; %g %g\n",
	  p3x, p3z, p5x, p5z );
  printf( "DJ4: feet: %g %g; %g %g\n",
	  j6x, j6z, j7x, j7z );
  printf( "DJ4: torsod: %g %g\n",
	  j1xd, j1zd );
  printf( "DJ4: hipsd: %g %g; %g %g\n",
	  j1xd + a1d*l1cm*cos(a1) + (a1d*l1*sin(a1))/2.,
	  j1zd - (a1d*l1*cos(a1))/2. + a1d*l1cm*sin(a1),
	  j1xd + a1d*l1cm*cos(a1) - (a1d*l1*sin(a1))/2.,
	  j1zd + (a1d*l1*cos(a1))/2. + a1d*l1cm*sin(a1) );
  printf( "DJ4: thighds: %g %g; %g %g\n",
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a2d)*l2cm*cos(a1 + a2) +
	  (a1d*l1*sin(a1))/2.,
	  j1zd - (a1d*l1*cos(a1))/2. + a1d*l1cm*sin(a1)
	  + a1d*l2cm*sin(a1 + a2) + a2d*l2cm*sin(a1 + a2),
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a4d)*l4cm*cos(a1 + a4) -
	  (a1d*l1*sin(a1))/2.,
	  j1zd + (a1d*l1*cos(a1))/2. + a1d*l1cm*sin(a1)
	  + a1d*l4cm*sin(a1 + a4) + a4d*l4cm*sin(a1 + a4) );
  printf( "DJ4: kneesd: %g %g; %g %g\n",
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a2d)*(a3 + l2)*cos(a1 + a2) +
	  (a1d*l1*sin(a1))/2. + a3d*sin(a1 + a2),
	  j1zd - (a1d*l1*cos(a1))/2. - a3d*cos(a1 + a2) + a1d*l1cm*sin(a1) +
	  a1d*a3*sin(a1 + a2) + a2d*a3*sin(a1 + a2) + a1d*l2*sin(a1 + a2) +
	  a2d*l2*sin(a1 + a2),
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a4d)*(a5 + l4)*cos(a1 + a4) -
	  (a1d*l1*sin(a1))/2. + a5d*sin(a1 + a4),
	  j1zd + (a1d*l1*cos(a1))/2. - a5d*cos(a1 + a4) + a1d*l1cm*sin(a1) +
	  a1d*a5*sin(a1 + a4) + a4d*a5*sin(a1 + a4) + a1d*l4*sin(a1 + a4) +
	  a4d*l4*sin(a1 + a4) );
  printf( "DJ4: calfds: %g %g; %g %g\n",
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a2d)*(a3 + l2 + l3cm)*cos(a1 + a2) +
	  (a1d*l1*sin(a1))/2. + a3d*sin(a1 + a2),
	  j1zd - (a1d*l1*cos(a1))/2. - a3d*cos(a1 + a2) + a1d*l1cm*sin(a1) +
	  a1d*a3*sin(a1 + a2) + a2d*a3*sin(a1 + a2) + a1d*l2*sin(a1 + a2) +
	  a2d*l2*sin(a1 + a2) + a1d*l3cm*sin(a1 + a2) + a2d*l3cm*sin(a1 + a2),
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a4d)*(a5 + l4 + l5cm)*cos(a1 + a4) -
	  (a1d*l1*sin(a1))/2. + a5d*sin(a1 + a4),
	  j1zd + (a1d*l1*cos(a1))/2. - a5d*cos(a1 + a4) + a1d*l1cm*sin(a1) +
	  a1d*a5*sin(a1 + a4) + a4d*a5*sin(a1 + a4) + a1d*l4*sin(a1 + a4) +
	  a4d*l4*sin(a1 + a4) + a1d*l5cm*sin(a1 + a4)
	  + a4d*l5cm*sin(a1 + a4) );
  printf( "DJ4: feetd: %g %g; %g %g\n",
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a2d)*(a3 + l2 + l3)*cos(a1 + a2) +
	  (a1d*l1*sin(a1))/2. + a3d*sin(a1 + a2),
	  j1zd - (a1d*l1*cos(a1))/2. - a3d*cos(a1 + a2) + a1d*l1cm*sin(a1) +
	  a1d*a3*sin(a1 + a2) + a2d*a3*sin(a1 + a2) + a1d*l2*sin(a1 + a2) +
	  a2d*l2*sin(a1 + a2) + a1d*l3*sin(a1 + a2) + a2d*l3*sin(a1 + a2),
	  j1xd + a1d*l1cm*cos(a1) + (a1d + a4d)*(a5 + l4 + l5)*cos(a1 + a4) -
	  (a1d*l1*sin(a1))/2. + a5d*sin(a1 + a4),
	  j1zd + (a1d*l1*cos(a1))/2. - a5d*cos(a1 + a4) + a1d*l1cm*sin(a1) +
	  a1d*a5*sin(a1 + a4) + a4d*a5*sin(a1 + a4) + a1d*l4*sin(a1 + a4) +
	  a4d*l4*sin(a1 + a4) + a1d*l5*sin(a1 + a4) + a4d*l5*sin(a1 + a4) );


  if ( a == NULL )
    a = matrix( 1, MAX_N_A_B, 1, MAX_N_A_B );
  if ( b == NULL )
    b = vector( 1, MAX_N_A_B );

  for( i = 1; i <= MAX_N_A_B; i++ )
    {
      b[i] = 0;
      for( j = 1; j <= MAX_N_A_B; j++ )
        {
          a[i][j] = 0;
        }
    }

  a[1][1] = m1;
  a[1][8] = -1;
  a[1][14] = -1;

  a[2][2] = m1;
  a[2][9] = -1;
  a[2][15] = -1;

  a[3][1] = m2;
  a[3][3] = l1cm*m2*cos(a1) + l2cm*m2*cos(a1 + a2) + (l1*m2*sin(a1))/2.;
  a[3][4] = l2cm*m2*cos(a1 + a2);
  a[3][8] = 1;
  a[3][10] = -cos(a1 + a2);

  a[4][2] = m2;
  a[4][3] = -(l1*m2*cos(a1))/2. + l1cm*m2*sin(a1) + l2cm*m2*sin(a1 + a2);
  a[4][4] = l2cm*m2*sin(a1 + a2);
  a[4][9] = 1;
  a[4][10] = -sin(a1 + a2);

  a[5][1] = m3;
  a[5][3] = l1cm*m3*cos(a1) + a3*m3*cos(a1 + a2) + l2*m3*cos(a1 + a2) +
    l3cm*m3*cos(a1 + a2) + (l1*m3*sin(a1))/2.;
  a[5][4] = a3*m3*cos(a1 + a2) + l2*m3*cos(a1 + a2) + l3cm*m3*cos(a1 + a2);
  a[5][5] = m3*sin(a1 + a2);
  a[5][10] = cos(a1 + a2);
  a[5][12] = -1;

  a[6][2] = m3;
  a[6][3] = -(l1*m3*cos(a1))/2. + l1cm*m3*sin(a1) + a3*m3*sin(a1 + a2) +
    l2*m3*sin(a1 + a2) + l3cm*m3*sin(a1 + a2);
  a[6][4] = a3*m3*sin(a1 + a2) + l2*m3*sin(a1 + a2) + l3cm*m3*sin(a1 + a2);
  a[6][5] = -(m3*cos(a1 + a2));
  a[6][10] = sin(a1 + a2);
  a[6][13] = -1;

  a[7][1] = m4;
  a[7][3] = l1cm*m4*cos(a1) + l4cm*m4*cos(a1 + a4) - (l1*m4*sin(a1))/2.;
  a[7][6] = l4cm*m4*cos(a1 + a4);
  a[7][14] = 1;
  a[7][16] = -cos(a1 + a4);

  a[8][2] = m4;
  a[8][3] = (l1*m4*cos(a1))/2. + l1cm*m4*sin(a1) + l4cm*m4*sin(a1 + a4);
  a[8][6] = l4cm*m4*sin(a1 + a4);
  a[8][15] = 1;
  a[8][16] = -sin(a1 + a4);

  a[9][1] = m5;
  a[9][3] = l1cm*m5*cos(a1) + a5*m5*cos(a1 + a4) + l4*m5*cos(a1 + a4) +
    l5cm*m5*cos(a1 + a4) - (l1*m5*sin(a1))/2.;
  a[9][6] = a5*m5*cos(a1 + a4) + l4*m5*cos(a1 + a4) + l5cm*m5*cos(a1 + a4);
  a[9][7] = m5*sin(a1 + a4);
  a[9][16] = cos(a1 + a4);

  a[10][2] = m5;
  a[10][3] = (l1*m5*cos(a1))/2. + l1cm*m5*sin(a1) + a5*m5*sin(a1 + a4) +
    l4*m5*sin(a1 + a4) + l5cm*m5*sin(a1 + a4);
  a[10][6] = a5*m5*sin(a1 + a4) + l4*m5*sin(a1 + a4) + l5cm*m5*sin(a1 + a4);
  a[10][7] = -(m5*cos(a1 + a4));
  a[10][16] = sin(a1 + a4);

  a[11][3] = I1;
  a[11][8] = -(l1cm*cos(a1)) - (l1*sin(a1))/2.;
  a[11][9] = (l1*cos(a1))/2. - l1cm*sin(a1);
  a[11][14] = -(l1cm*cos(a1)) + (l1*sin(a1))/2.;
  a[11][15] = -(l1*cos(a1))/2. - l1cm*sin(a1);

  a[12][3] = I2;
  a[12][4] = I2;
  a[12][8] = -(l2cm*cos(a1 + a2));
  a[12][9] = -(l2cm*sin(a1 + a2));
  a[12][10] =   -(a3*Power(cos(a1 + a2),2)) - l2*Power(cos(a1 + a2),2) +
    l2cm*Power(cos(a1 + a2),2) - a3*Power(sin(a1 + a2),2) -
    l2*Power(sin(a1 + a2),2) + l2cm*Power(sin(a1 + a2),2);
  a[12][11] = -1;

  a[13][3] = I3;
  a[13][4] = I3;
  a[13][10] = -(l3cm*Power(cos(a1 + a2),2)) - l3cm*Power(sin(a1 + a2),2);
  a[13][11] = 1;
  a[13][12] = -(l3*cos(a1 + a2)) + l3cm*cos(a1 + a2);
  a[13][13] = -(l3*sin(a1 + a2)) + l3cm*sin(a1 + a2);

  a[14][3] = I4;
  a[14][6] = I4;
  a[14][14] = -(l4cm*cos(a1 + a4));
  a[14][15] = -(l4cm*sin(a1 + a4));
  a[14][16] = -(a5*Power(cos(a1 + a4),2)) - l4*Power(cos(a1 + a4),2) +
    l4cm*Power(cos(a1 + a4),2) - a5*Power(sin(a1 + a4),2) -
    l4*Power(sin(a1 + a4),2) + l4cm*Power(sin(a1 + a4),2);
  a[14][17] = -1;

  a[15][3] = I5;
  a[15][6] = I5;
  a[15][16] = -(l5cm*Power(cos(a1 + a4),2)) - l5cm*Power(sin(a1 + a4),2);
  a[15][17] = 1;

  a[16][1] = 1;
  a[16][3] = l1cm*cos(a1) + a3*cos(a1 + a2) + l2*cos(a1 + a2)
    + l3*cos(a1 + a2) + (l1*sin(a1))/2.;
  a[16][4] = a3*cos(a1 + a2) + l2*cos(a1 + a2) + l3*cos(a1 + a2);
  a[16][5] = sin(a1 + a2);

  a[17][2] = 1;
  a[17][3] = -(l1*cos(a1))/2. + l1cm*sin(a1) + a3*sin(a1 + a2)
    + l2*sin(a1 + a2) + l3*sin(a1 + a2);
  a[17][4] = a3*sin(a1 + a2) + l2*sin(a1 + a2) + l3*sin(a1 + a2);
  a[17][5] = -cos(a1 + a2);

  b[1] = j1xdi*m1;
  b[2] = j1zdi*m1;
  b[3] = j1xdi*m2 + a1di*l1cm*m2*cos(a1) + a1di*l2cm*m2*cos(a1 + a2) +
    a2di*l2cm*m2*cos(a1 + a2) + (a1di*l1*m2*sin(a1))/2.;
  b[4] = j1zdi*m2 - (a1di*l1*m2*cos(a1))/2. + a1di*l1cm*m2*sin(a1) +
    a1di*l2cm*m2*sin(a1 + a2) + a2di*l2cm*m2*sin(a1 + a2);
  b[5] = j1xdi*m3 + a1di*l1cm*m3*cos(a1) + a1di*a3*m3*cos(a1 + a2) +
    a2di*a3*m3*cos(a1 + a2) + a1di*l2*m3*cos(a1 + a2) +
    a2di*l2*m3*cos(a1 + a2) + a1di*l3cm*m3*cos(a1 + a2) +
    a2di*l3cm*m3*cos(a1 + a2) + (a1di*l1*m3*sin(a1))/2. + a3di*m3*sin(a1 + a2);
  b[6] = j1zdi*m3 - (a1di*l1*m3*cos(a1))/2. - a3di*m3*cos(a1 + a2) +
    a1di*l1cm*m3*sin(a1) + a1di*a3*m3*sin(a1 + a2) +
    a2di*a3*m3*sin(a1 + a2) + a1di*l2*m3*sin(a1 + a2) +
    a2di*l2*m3*sin(a1 + a2) + a1di*l3cm*m3*sin(a1 + a2) +
    a2di*l3cm*m3*sin(a1 + a2);
  b[7] = j1xdi*m4 + a1di*l1cm*m4*cos(a1) + a1di*l4cm*m4*cos(a1 + a4) +
    a4di*l4cm*m4*cos(a1 + a4) - (a1di*l1*m4*sin(a1))/2.;
  b[8] = j1zdi*m4 + (a1di*l1*m4*cos(a1))/2. + a1di*l1cm*m4*sin(a1) +
    a1di*l4cm*m4*sin(a1 + a4) + a4di*l4cm*m4*sin(a1 + a4);
  b[9] = j1xdi*m5 + a1di*l1cm*m5*cos(a1) + a1di*a5*m5*cos(a1 + a4) +
    a4di*a5*m5*cos(a1 + a4) + a1di*l4*m5*cos(a1 + a4) +
    a4di*l4*m5*cos(a1 + a4) + a1di*l5cm*m5*cos(a1 + a4) +
    a4di*l5cm*m5*cos(a1 + a4) - (a1di*l1*m5*sin(a1))/2. + a5di*m5*sin(a1 + a4);
  b[10] = j1zdi*m5 + (a1di*l1*m5*cos(a1))/2. - a5di*m5*cos(a1 + a4) +
    a1di*l1cm*m5*sin(a1) + a1di*a5*m5*sin(a1 + a4) +
    a4di*a5*m5*sin(a1 + a4) + a1di*l4*m5*sin(a1 + a4) +
    a4di*l4*m5*sin(a1 + a4) + a1di*l5cm*m5*sin(a1 + a4) +
    a4di*l5cm*m5*sin(a1 + a4);
  b[11] = a1di*I1;
  b[12] = a1di*I2 + a2di*I2;
  b[13] = a1di*I3 + a2di*I3;
  b[14] = a1di*I4 + a4di*I4;
  b[15] = a1di*I5 + a4di*I5;
  b[16] = 0;
  b[17] = 0;

  n = SS_IMPACT4_N;

  ludcmp(a,n,indx,&d);
  lubksb(a,n,indx,b);

  for( i = 1; i <= n; i++ )
    {
      printf( "MATH2: %d: %g\n", i, b[i] );
      result[i-1] = b[i];
    }
}

/************************************************************************/
/* Presumably this updates the state appropriately
Does the biped have the foot on the ground, or in the air?
 */

ss_impact2_update( SIM *s, double *impact2_analysis )
{

  s->torsod[XX] = impact2_analysis[SS_IMPACT2_XD];
  s->torsod[ZZ] = impact2_analysis[SS_IMPACT2_YD];
  s->rolld = -impact2_analysis[SS_IMPACT2_ROLLD];
  s->hip_angled[LEFT] = impact2_analysis[SS_IMPACT2_L_HIPD];
  s->knee_lengthd[LEFT] = impact2_analysis[SS_IMPACT2_L_KNEED];
  s->hip_angled[RIGHT] = impact2_analysis[SS_IMPACT2_R_HIPD];
  s->knee_lengthd[RIGHT] = impact2_analysis[SS_IMPACT2_R_KNEED];
  
  /* This does not seem right
  if ( s->ss_foot_down == LEFT )
    {
      s->hip_angled[RIGHT] = impact2_analysis[SS_IMPACT2_L_HIPD];
      s->knee_lengthd[RIGHT] = impact2_analysis[SS_IMPACT2_L_KNEED];
      s->hip_angled[LEFT] = impact2_analysis[SS_IMPACT2_R_HIPD];
      s->knee_lengthd[LEFT] = impact2_analysis[SS_IMPACT2_R_KNEED];
    }
  else
    {
      s->hip_angled[LEFT] = impact2_analysis[SS_IMPACT2_L_HIPD];
      s->knee_lengthd[LEFT] = impact2_analysis[SS_IMPACT2_L_KNEED];
      s->hip_angled[RIGHT] = impact2_analysis[SS_IMPACT2_R_HIPD];
      s->knee_lengthd[RIGHT] = impact2_analysis[SS_IMPACT2_R_KNEED];
    }
  */
}

/*************************************************************************/
/* Debug Jacobean for ss */

debug_jacobian1( SIM *s )
{
  int i, j;

  double a1, a2, a3, a4, a5;
  double a1d, a2d, a3d, a4d, a5d;
  double l1, l2, l3, l4, l5;
  double l1cm, l2cm, l3cm, l4cm, l5cm;
  double jacobian[MAX_N_A_B][MAX_N_A_B];

  a1 = s->sdfast_state[0];
  a2 = s->sdfast_state[1];
  a3 = s->sdfast_state[2];
  a4 = s->sdfast_state[3];
  a5 = s->sdfast_state[4];

  a1d = s->sdfast_state[5];
  a2d = s->sdfast_state[6];
  a3d = s->sdfast_state[7];
  a4d = s->sdfast_state[8];
  a5d = s->sdfast_state[9];

  l1 = s->calf_length;
  l2 = s->thigh_length;
  if ( s->ss_foot_down == LEFT )
    l3 = s->pelvis_width;
  else
    l3 = -s->pelvis_width;
  l4 = s->thigh_length;
  l5 = s->calf_length;

  l1cm = s->calf_length - s->calf_cm;
  l2cm = s->thigh_length - s->thigh_cm;
  l3cm = s->torso_cm;
  l4cm = s->thigh_cm;
  l5cm = s->calf_cm;

  jacobian[0][0] = -((a2 + l1 + l2)*cos(a1)) 
    + (a5 + l4 + l5)*cos(a1 + a3 + a4) - l3*sin(a1 + a3);
  jacobian[0][1] = -sin(a1);
  jacobian[0][2] = (a5 + l4 + l5)*cos(a1 + a3 + a4) - l3*sin(a1 + a3);
  jacobian[0][3] = (a5 + l4 + l5)*cos(a1 + a3 + a4);
  jacobian[0][4] = sin(a1 + a3 + a4);

  jacobian[1][0] = l3*cos(a1 + a3) - (a2 + l1 + l2)*sin(a1) + 
     (a5 + l4 + l5)*sin(a1 + a3 + a4);
  jacobian[1][1] = cos(a1);
  jacobian[1][2] = l3*cos(a1 + a3) + (a5 + l4 + l5)*sin(a1 + a3 + a4);
  jacobian[1][3] = (a5 + l4 + l5)*sin(a1 + a3 + a4);
  jacobian[1][4] = -cos(a1 + a3 + a4);

  printf( "DJ: %g %g\n",
	  jacobian[0][0] * a1d
	  + jacobian[0][1] * a2d
	  + jacobian[0][2] * a3d
	  + jacobian[0][3] * a4d
	  + jacobian[0][4] * a5d,
	  jacobian[1][0] * a1d
	  + jacobian[1][1] * a2d
	  + jacobian[1][2] * a3d
	  + jacobian[1][3] * a4d
	  + jacobian[1][4] * a5d );

  printf( "DJ1: a1, a1d: %g %g\n", a1, a1d );

  /*
  if ( s->ss_foot_down == LEFT )
    {
  */
      printf( "DJ1: torso: %g %g\n",
	      (l3*cos(a1 + a3))/2. - (a2 + l1 + l2)*sin(a1)
	      - l3cm*sin(a1 + a3) + s->x_offset,
	      (a2 + l1 + l2)*cos(a1) + l3cm*cos(a1 + a3)
	      + (l3*sin(a1 + a3))/2. );
      printf( "DJ1: hips: %g %g; %g %g\n",
	      -(l1*sin(a1)) - (a2 + l2)*sin(a1) + s->x_offset,
	      l1*cos(a1) + (a2 + l2)*cos(a1),
	      l3*cos(a1 + a3) - (a2 + l1 + l2)*sin(a1) + s->x_offset,
	      (a2 + l1 + l2)*cos(a1) + l3*sin(a1 + a3) );
      printf( "DJ1: thighs: %g %g; %g %g\n",
	      -(l1*sin(a1)) - (a2 + l2cm)*sin(a1) + s->x_offset,
	      l1*cos(a1) + (a2 + l2cm)*cos(a1),
	      l3*cos(a1 + a3) - (a2 + l1 + l2)*sin(a1)
	      + l4cm*sin(a1 + a3 + a4) + s->x_offset,
	      (a2 + l1 + l2)*cos(a1) - l4cm*cos(a1 + a3 + a4)
	      + l3*sin(a1 + a3) );
      printf( "DJ1: knees: %g %g; %g %g\n",
	      -l1*sin(a1) + s->x_offset,
	      l1*cos(a1),
	      l3*cos(a1 + a3) - (a2 + l1 + l2)*sin(a1) +
	      (a5 + l4)*sin(a1 + a3 + a4) + s->x_offset,
	      (a2 + l1 + l2)*cos(a1) - (a5 + l4)*cos(a1 + a3 + a4) 
	      + l3*sin(a1 + a3) );
      printf( "DJ1: calves: %g %g; %g %g\n",
	      -l1cm*sin(a1) + s->x_offset,
	      l1cm*cos(a1), 
	      l3*cos(a1 + a3) - (a2 + l1 + l2)*sin(a1) +
	      (a5 + l4 + l5cm)*sin(a1 + a3 + a4) + s->x_offset,
	      (a2 + l1 + l2)*cos(a1) - (a5 + l4 + l5cm)*cos(a1 + a3 + a4) +
	      l3*sin(a1 + a3) );
      printf( "DJ1: feet: %g %g; %g %g\n", 0.0 + s->x_offset, 0.0,
	      l3*cos(a1 + a3) - (a2 + l1 + l2)*sin(a1) +
	      (a5 + l4 + l5)*sin(a1 + a3 + a4) + s->x_offset,
	      (a2 + l1 + l2)*cos(a1) - (a5 + l4 + l5)*cos(a1 + a3 + a4) +
	      l3*sin(a1 + a3) );
      printf( "DJ1: torsod: %g %g\n",
	      -a1d*(a2 + l1 + l2)*cos(a1) 
	      - (a1d + a3d)*l3cm*cos(a1 + a3)
	      - a2d*sin(a1) - 0.5*a1d*l3*sin(a1 + a3)
	      - 0.5*a3d*l3*sin(a1 + a3),
	      a2d*cos(a1)
	      + 0.5*(a1d + a3d)*l3*cos(a1 + a3)
	      - a1d*(a2 + l1 + l2)*sin(a1) 
	      - (a1d + a3d)*l3cm*sin(a1 + a3) );
      printf( "DJ1: hipsd: %g %g; %g %g\n",
	      -(a1d*l1*cos(a1)) - a1d*(a2 + l2)*cos(a1) - a2d*sin(a1),
	      a2d*cos(a1) - a1d*l1*sin(a1) - a1d*(a2 + l2)*sin(a1),
	      -(a1d*(a2 + l1 + l2)*cos(a1)) - a2d*sin(a1) -
	      (a1d + a3d)*l3*sin(a1 + a3),
	      a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3)
	      - a1d*(a2 + l1 + l2)*sin(a1) );
      printf( "DJ1: thighds: %g %g; %g %g\n",
	      -(a1d*l1*cos(a1)) - a1d*(a2 + l2cm)*cos(a1) - a2d*sin(a1),
	      a2d*cos(a1) - a1d*l1*sin(a1) - a1d*(a2 + l2cm)*sin(a1),
	      -(a1d*(a2 + l1 + l2)*cos(a1)) +
	      (a1d + a3d + a4d)*l4cm*cos(a1 + a3 + a4) - a2d*sin(a1) -
	      a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3),
	      a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) - a1d*a2*sin(a1) -
	      a1d*l1*sin(a1) - a1d*l2*sin(a1) + a1d*l4cm*sin(a1 + a3 + a4) +
	      a3d*l4cm*sin(a1 + a3 + a4) + a4d*l4cm*sin(a1 + a3 + a4) );
      printf( "DJ1: kneesd: %g %g; %g %g\n",
	      -(a1d*l1*cos(a1)), -(a1d*l1*sin(a1)),
	      -(a1d*(a2 + l1 + l2)*cos(a1)) +
	      (a1d + a3d + a4d)*(a5 + l4)*cos(a1 + a3 + a4) - a2d*sin(a1) -
	      a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3) 
	      + a5d*sin(a1 + a3 + a4),
	      a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) 
	      - a5d*cos(a1 + a3 + a4) -
	      a1d*a2*sin(a1) - a1d*l1*sin(a1) - a1d*l2*sin(a1) +
	      a1d*a5*sin(a1 + a3 + a4) + a3d*a5*sin(a1 + a3 + a4) +
	      a4d*a5*sin(a1 + a3 + a4) + a1d*l4*sin(a1 + a3 + a4) +
	      a3d*l4*sin(a1 + a3 + a4) + a4d*l4*sin(a1 + a3 + a4) );
      printf( "DJ1: calfds: %g %g; %g %g\n",
	      -(a1d*l1cm*cos(a1)),
	      -(a1d*l1cm*sin(a1)),
	      -(a1d*(a2 + l1 + l2)*cos(a1)) +
	      (a1d + a3d + a4d)*(a5 + l4 + l5cm)*cos(a1 + a3 + a4) - a2d*sin(a1) -
	      a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3) + a5d*sin(a1 + a3 + a4),
	      a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3) - a5d*cos(a1 + a3 + a4) -
	      a1d*a2*sin(a1) - a1d*l1*sin(a1) - a1d*l2*sin(a1) +
	      a1d*a5*sin(a1 + a3 + a4) + a3d*a5*sin(a1 + a3 + a4) +
	      a4d*a5*sin(a1 + a3 + a4) + a1d*l4*sin(a1 + a3 + a4) +
	      a3d*l4*sin(a1 + a3 + a4) + a4d*l4*sin(a1 + a3 + a4) +
	      a1d*l5cm*sin(a1 + a3 + a4) + a3d*l5cm*sin(a1 + a3 + a4) +
	      a4d*l5cm*sin(a1 + a3 + a4) );
      printf( "DJ1: feetd: %g %g; %g %g\n", 0.0, 0.0,
	      -(a1d*(a2 + l1 + l2)*cos(a1)) +
	      (a1d + a3d + a4d)*(a5 + l4 + l5)*cos(a1 + a3 + a4)
	      - a2d*sin(a1) -
	      a1d*l3*sin(a1 + a3) - a3d*l3*sin(a1 + a3) 
	      + a5d*sin(a1 + a3 + a4),
	      a2d*cos(a1) + (a1d + a3d)*l3*cos(a1 + a3)
	      - a5d*cos(a1 + a3 + a4) -
	      a1d*a2*sin(a1) - a1d*l1*sin(a1) - a1d*l2*sin(a1) +
	      a1d*a5*sin(a1 + a3 + a4) + a3d*a5*sin(a1 + a3 + a4) +
	      a4d*a5*sin(a1 + a3 + a4) + a1d*l4*sin(a1 + a3 + a4) +
	      a3d*l4*sin(a1 + a3 + a4) + a4d*l4*sin(a1 + a3 + a4) +
	      a1d*l5*sin(a1 + a3 + a4) + a3d*l5*sin(a1 + a3 + a4) +
	      a4d*l5*sin(a1 + a3 + a4) );
      /*
    }
      */

  /*
  printf( "Press return to continue.\n" );
  getchar();
  */
}

/************************************************************************/
/* This impact analysis assumes robot in air and one foot hits */
/* Assumes that the foot != s->ss_foot_down is the impact foot */

debug_jacobian2( SIM *s )
{
  int i, j;

  double jacobian[MAX_N_A_B][MAX_N_A_B];
  double x, y, a1, a2, a3, a4, a5;
  double xd, yd, a1d, a2d, a3d, a4d, a5d;
  double l1, l2, l3, l4, l5;
  double l1cm, l2cm, l3cm, l4cm, l5cm;

  x = s->torso[XX];
  y = s->torso[ZZ];
  a1 = -s->roll;
  a2 = s->hip_angle[LEFT];
  a3 = s->knee_length[LEFT];
  a4 = s->hip_angle[RIGHT];
  a5 = s->knee_length[RIGHT];

  xd = s->torsod[XX];
  yd = s->torsod[ZZ];
  a1d = -s->rolld;
  a2d = s->hip_angled[LEFT];
  a3d = s->knee_lengthd[LEFT];
  a4d = s->hip_angled[RIGHT];
  a5d = s->knee_lengthd[RIGHT];

  /*
  if ( s->ss_foot_down == LEFT )
    l1 = s->pelvis_width;
  else
    l1 = -s->pelvis_width;
  */
  l1 = s->pelvis_width;
  l2 = s->thigh_length;
  l3 = s->thigh_length;
  l4 = s->calf_length;
  l5 = s->calf_length;

  l1cm = s->torso_cm;
  l2cm = s->thigh_cm;
  l3cm = s->calf_cm;
  l4cm = s->thigh_cm;
  l5cm = s->calf_cm;


  if ( s->ss_foot_down == RIGHT )
    { // left foot is impacting
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a3 + l2 + l4)*cos(a1 + a2) + (l1*sin(a1))/2.;
      jacobian[0][3] = (a3 + l2 + l4)*cos(a1 + a2);
      jacobian[0][4] = sin(a1 + a2);
      jacobian[0][5] = 0.0;
      jacobian[0][6] = 0.0;
      
      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = -(l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][3] = (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][4] = -cos(a1 + a2);
      jacobian[1][5] = 0.0;
      jacobian[1][6] = 0.0;
    }
  else
    {
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a5 + l3 + l5)*cos(a1 + a4) - (l1*sin(a1))/2.;
      jacobian[0][3] = 0.0;
      jacobian[0][4] = 0.0;
      jacobian[0][5] = (a5 + l3 + l5)*cos(a1 + a4);
      jacobian[0][6] = sin(a1 + a4);

      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = (l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][3] = 0.0;
      jacobian[1][4] = 0.0;
      jacobian[1][5] = (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][6] = -cos(a1 + a4);
    }


  if ( s->ss_foot_down == LEFT )
    { // left foot was on ground
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a3 + l2 + l4)*cos(a1 + a2) + (l1*sin(a1))/2.;
      jacobian[0][3] = (a3 + l2 + l4)*cos(a1 + a2);
      jacobian[0][4] = sin(a1 + a2);
      jacobian[0][5] = 0.0;
      jacobian[0][6] = 0.0;
      
      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = -(l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][3] = (a3 + l2 + l4)*sin(a1 + a2);
      jacobian[1][4] = -cos(a1 + a2);
      jacobian[1][5] = 0.0;
      jacobian[1][6] = 0.0;
    }
  else
    {
      jacobian[0][0] = 1.0;
      jacobian[0][1] = 0.0;
      jacobian[0][2] = l1cm*cos(a1) 
	+ (a5 + l3 + l5)*cos(a1 + a4) - (l1*sin(a1))/2.;
      jacobian[0][3] = 0.0;
      jacobian[0][4] = 0.0;
      jacobian[0][5] = (a5 + l3 + l5)*cos(a1 + a4);
      jacobian[0][6] = sin(a1 + a4);

      jacobian[1][0] = 0.0;
      jacobian[1][1] = 1.0;
      jacobian[1][2] = (l1*cos(a1))/2. + l1cm*sin(a1) 
	+ (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][3] = 0.0;
      jacobian[1][4] = 0.0;
      jacobian[1][5] = (a5 + l3 + l5)*sin(a1 + a4);
      jacobian[1][6] = -cos(a1 + a4);
    }

  printf( "DJ2: Torso: %g %g\n", x, y );
  printf( "DJ2: Hips: %g %g; %g %g\n", 
	  x - (l1*cos(a1))/2. + l1cm*sin(a1),
	  y - l1cm*cos(a1) - (l1*sin(a1))/2.,
	  x + (l1*cos(a1))/2. + l1cm*sin(a1),
	  y - l1cm*cos(a1) + (l1*sin(a1))/2. );
  printf( "DJ2: Thighs: %g %g; %g %g\n", 
	  x - (l1*cos(a1))/2. + l1cm*sin(a1) + l2cm*sin(a1 + a2),
	  y - l1cm*cos(a1) - l2cm*cos(a1 + a2) - (l1*sin(a1))/2.,
	  x + (l1*cos(a1))/2. + l1cm*sin(a1) + l4cm*sin(a1 + a4),
	  y - l1cm*cos(a1) - l4cm*cos(a1 + a4) + (l1*sin(a1))/2. );
  printf( "DJ2: Knees: %g %g; %g %g\n", 
	  x - (l1*cos(a1))/2. + l1cm*sin(a1) 
	  + a3*sin(a1 + a2) + l2*sin(a1 + a2),
	  y - l1cm*cos(a1) - (a3 + l2)*cos(a1 + a2) -
	  (l1*sin(a1))/2.,
	  x + (l1*cos(a1))/2. + l1cm*sin(a1) + a5*sin(a1 + a4) +
	  l3*sin(a1 + a4),
	  y - l1cm*cos(a1) - (a5 + l3)*cos(a1 + a4) + (l1*sin(a1))/2 );
  printf( "DJ2: Calves: %g %g; %g %g\n", 
	  x - (l1*cos(a1))/2. + l1cm*sin(a1) + a3*sin(a1 + a2) +
	  l2*sin(a1 + a2) + l3cm*sin(a1 + a2),
	  y - l1cm*cos(a1) - (a3 + l2 + l3cm)*cos(a1 + a2) - (l1*sin(a1))/2.,
	  x + (l1*cos(a1))/2. + l1cm*sin(a1) + a5*sin(a1 + a4) +
	  l3*sin(a1 + a4) + l5cm*sin(a1 + a4),
	  y - l1cm*cos(a1) - (a5 + l3 + l5cm)*cos(a1 + a4) + (l1*sin(a1))/2. );
  printf( "DJ2: Feet: %g %g; %g %g\n", 
	  x - (l1*cos(a1))/2. + l1cm*sin(a1) + a3*sin(a1 + a2) +
	  l2*sin(a1 + a2) + l4*sin(a1 + a2),
	  y - l1cm*cos(a1) - (a3 + l2 + l4)*cos(a1 + a2) - (l1*sin(a1))/2.,
	  x + (l1*cos(a1))/2. + l1cm*sin(a1) + a5*sin(a1 + a4) +
	  l3*sin(a1 + a4) + l5*sin(a1 + a4),
	  y - l1cm*cos(a1) - (a5 + l3 + l5)*cos(a1 + a4) + (l1*sin(a1))/2. );
  printf( "DJ2: Torsod: %g %g\n", xd, yd );
  printf( "DJ2: Hipsd: %g %g; %g %g\n", 
	  xd + a1d*(l1cm*cos(a1) + (l1*sin(a1))/2.),
	  yd + a1d*(-(l1*cos(a1))/2. + l1cm*sin(a1)),
	  xd + a1d*(l1cm*cos(a1) - (l1*sin(a1))/2.),
	  yd + a1d*((l1*cos(a1))/2. + l1cm*sin(a1)) );
  printf( "DJ2: Thighsd: %g %g; %g %g\n", 
	  xd + a1d*l1cm*cos(a1) + (a1d + a2d)*l2cm*cos(a1 + a2) +
	  (a1d*l1*sin(a1))/2.,
	  yd - (a1d*l1*cos(a1))/2. + a1d*l1cm*sin(a1) +
	  a1d*l2cm*sin(a1 + a2) + a2d*l2cm*sin(a1 + a2),
	  xd + a1d*l1cm*cos(a1) + (a1d + a4d)*l4cm*cos(a1 + a4) -
	  (a1d*l1*sin(a1))/2.,yd + (a1d*l1*cos(a1))/2. + a1d*l1cm*sin(a1) +
	  a1d*l4cm*sin(a1 + a4) + a4d*l4cm*sin(a1 + a4) );
  printf( "DJ2: Kneesd: %g %g; %g %g\n", 
	  xd + a1d*(l1cm*cos(a1) + (a3 + l2)*cos(a1 + a2) + (l1*sin(a1))/2.)
	  + a2d*((a3 + l2)*cos(a1 + a2)) + a3d*(sin(a1 + a2)),
	  yd + a1d*(-(l1*cos(a1))/2. + l1cm*sin(a1) + (a3 + l2)*sin(a1 + a2))
	  + a2d*((a3 + l2)*sin(a1 + a2)) + a3d*(-cos(a1 + a2)),
	  xd + a1d*(l1cm*cos(a1) + (a5 + l3)*cos(a1 + a4) - (l1*sin(a1))/2.)
	  + a4d*(a5 + l3)*cos(a1 + a4) + a5d*sin(a1 + a4),
	  yd + a1d*((l1*cos(a1))/2. + l1cm*sin(a1) + (a5 + l3)*sin(a1 + a4))
	  + a4d*(a5 + l3)*sin(a1 + a4) - a5d*cos(a1 + a4) );
  printf( "DJ2: Calfsd: %g %g; %g %g\n", 
	  xd + a1d*l1cm*cos(a1) + (a1d + a2d)*(a3 + l2 + l3cm)*cos(a1 + a2) +
	  (a1d*l1*sin(a1))/2. + a3d*sin(a1 + a2),
	  yd - (a1d*l1*cos(a1))/2. - a3d*cos(a1 + a2) + a1d*l1cm*sin(a1) +
	  a1d*a3*sin(a1 + a2) + a2d*a3*sin(a1 + a2) + a1d*l2*sin(a1 + a2) +
	  a2d*l2*sin(a1 + a2) + a1d*l3cm*sin(a1 + a2) + a2d*l3cm*sin(a1 + a2),
	  xd + a1d*l1cm*cos(a1) + (a1d + a4d)*(a5 + l3 + l5cm)*cos(a1 + a4) -
	  (a1d*l1*sin(a1))/2. + a5d*sin(a1 + a4),
	  yd + (a1d*l1*cos(a1))/2. - a5d*cos(a1 + a4) + a1d*l1cm*sin(a1) +
	  a1d*a5*sin(a1 + a4) + a4d*a5*sin(a1 + a4) + a1d*l3*sin(a1 + a4) +
	  a4d*l3*sin(a1 + a4) + a1d*l5cm*sin(a1 + a4)
	  + a4d*l5cm*sin(a1 + a4) );
  printf( "DJ2: Feetd: %g %g; %g %g\n",
	  xd + a1d*(l1cm*cos(a1) + (a3 + l2 + l4)*cos(a1 + a2)
		    + (l1*sin(a1))/2.)
	  + a2d*((a3 + l2 + l4)*cos(a1 + a2)) + a3d*(sin(a1 + a2)),
	  yd + a1d*(-(l1*cos(a1))/2. + l1cm*sin(a1)
		    + (a3 + l2 + l4)*sin(a1 + a2))
	  + a2d*((a3 + l2 + l4)*sin(a1 + a2)) + a3d*(-cos(a1 + a2)),
	  xd + a1d*(l1cm*cos(a1) + (a5 + l3 + l5)*cos(a1 + a4)
		    - (l1*sin(a1))/2.)
	  + a4d*((a5 + l3 + l5)*cos(a1 + a4)) + a5d*(sin(a1 + a4)),
	  yd + a1d*((l1*cos(a1))/2. + l1cm*sin(a1)
		    + (a5 + l3 + l5)*sin(a1 + a4))
	  + a4d*((a5 + l3 + l5)*sin(a1 + a4)) + a5d*(-cos(a1 + a4)) );
}

/************************************************************************/
/************************************************************************/
/* impact analysis with a foot free.*/
/* This can be called from air or single support */

touchdown_to_ss( SIM *s )
{
  double impact2_analysis[SS_IMPACT2_N+4+4];
  int i;

  if ( interactive )
    printf( "touchdown_to_ss: %g; old foot down: %d\n", s->time,
	    s->ss_foot_down );

  // for gen-v-data
  if ( s->sdfast_model == SINGLE_SUPPORT )
    for ( i = 0; i < SS_N_STATES; i++ )
      s->stashed_ss_state[i] = s->sdfast_state[i];

  ss_impact2( s, impact2_analysis );

  if ( -impact2_analysis[SS_IMPACT2_IMPULSE_Z] < 0 && interactive )
    {
      printf( "Negative impulse: ss_impact2???? %g %g\n", s->time,
	      -impact2_analysis[SS_IMPACT2_IMPULSE_Z] );
      impact2_print = 1;
    }

  if ( impact2_print )
    {
      printf( "x, z, p, lh, lk, rh, rk: %g %g %g %g %g %g %g\n",
	      s->torso[XX], s->torso[ZZ],
	      s->roll, s->hip_angle[LEFT],
	      s->knee_length[LEFT],
	      s->hip_angle[RIGHT],
	      s->knee_length[RIGHT] );
      printf( "xd, zd, pd, lhd, lkd, rhd, rkd: %g %g %g %g %g %g %g\n",
	      s->torsod[XX], s->torsod[ZZ],
	      s->rolld, s->hip_angled[LEFT],
	      s->knee_lengthd[LEFT],
	      s->hip_angled[RIGHT],
	      s->knee_lengthd[RIGHT] );
      printf( "torso: %g %g\n", s->torso[XX], s->torso[ZZ] );
      printf( "hips: %g %g; %g %g\n", s->hip[LEFT][XX], s->hip[LEFT][ZZ],
	      s->hip[RIGHT][XX], s->hip[RIGHT][ZZ] );
      printf( "knees: %g %g; %g %g\n", s->knee[LEFT][XX], s->knee[LEFT][ZZ],
	      s->knee[RIGHT][XX], s->knee[RIGHT][ZZ] );
      printf( "feet: %g %g; %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ],
	      s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );
      printf( "torsod: %g %g\n", s->torsod[XX], s->torsod[ZZ] );
      printf( "hipsd: %g %g; %g %g\n", s->hipd[LEFT][XX], s->hipd[LEFT][ZZ],
	      s->hipd[RIGHT][XX], s->hipd[RIGHT][ZZ] );
      printf( "kneesd: %g %g; %g %g\n", s->kneed[LEFT][XX], s->kneed[LEFT][ZZ],
	      s->kneed[RIGHT][XX], s->kneed[RIGHT][ZZ] );
      printf( "feetd: %g %g; %g %g\n", s->footd[LEFT][XX], s->footd[LEFT][ZZ],
	      s->footd[RIGHT][XX], s->footd[RIGHT][ZZ] );
      printf( "force: %g %g; %g %g\n", s->ground_force[LEFT][XX], s->ground_force[LEFT][ZZ],
	      s->ground_force[RIGHT][XX], s->ground_force[RIGHT][ZZ] );

      for( i = 0; i < SS_IMPACT2_N; i++ )
	{
	  printf( "%d: %g\n", i, impact2_analysis[i] );
	}
    }

  ss_impact2_update( s, impact2_analysis );

  if ( impact2_print )
    {
      /*
      printf( "x, z, p, lh, lk, rh, rk: %g %g %g %g %g %g %g\n",
	      s->torso[XX], s->torso[ZZ],
	      s->roll, s->hip_angle[LEFT],
	      s->knee_length[LEFT],
	      s->hip_angle[RIGHT],
	      s->knee_length[RIGHT] );
      */
      printf( "xd, zd, pd, lhd, lkd, rhd, rkd: %g %g %g %g %g %g %g\n",
	      s->torsod[XX], s->torsod[ZZ],
	      s->rolld, s->hip_angled[LEFT],
	      s->knee_lengthd[LEFT],
	      s->hip_angled[RIGHT],
	      s->knee_lengthd[RIGHT] );
    }


  // debug_jacobian2( s );

  s->last_impulse_side = other_side( s->ss_foot_down );
  s->last_impulse[XX] = -impact2_analysis[SS_IMPACT2_IMPULSE_X];
  s->last_impulse[YY] = 0.0;
  s->last_impulse[ZZ] = -impact2_analysis[SS_IMPACT2_IMPULSE_Z];
  s->last_impulse_time = s->time;

  if ( s->ss_foot_down == LEFT )
    init_state_one_foot_on_ground( s, RIGHT );
  else
    init_state_one_foot_on_ground( s, LEFT );

  if ( impact2_print )
    {
      /*
      printf( "x, z, p, lh, lk, rh, rk: %g %g %g %g %g %g %g\n",
	      s->torso[XX], s->torso[ZZ],
	      s->roll, s->hip_angle[LEFT],
	      s->knee_length[LEFT],
	      s->hip_angle[RIGHT],
	      s->knee_length[RIGHT] );
      */
      printf( "xd, zd, pd, lhd, lkd, rhd, rkd: %g %g %g %g %g %g %g\n",
	      s->torsod[XX], s->torsod[ZZ],
	      s->rolld, s->hip_angled[LEFT],
	      s->knee_lengthd[LEFT],
	      s->hip_angled[RIGHT],
	      s->knee_lengthd[RIGHT] );
      printf( "torso: %g %g\n", s->torso[XX], s->torso[ZZ] );
      printf( "hips: %g %g; %g %g\n", s->hip[LEFT][XX], s->hip[LEFT][ZZ],
	      s->hip[RIGHT][XX], s->hip[RIGHT][ZZ] );
      printf( "knees: %g %g; %g %g\n", s->knee[LEFT][XX], s->knee[LEFT][ZZ],
	      s->knee[RIGHT][XX], s->knee[RIGHT][ZZ] );
      printf( "feet: %g %g; %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ],
	      s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );
      printf( "torsod: %g %g\n", s->torsod[XX], s->torsod[ZZ] );
      printf( "hipsd: %g %g; %g %g\n", s->hipd[LEFT][XX], s->hipd[LEFT][ZZ],
	      s->hipd[RIGHT][XX], s->hipd[RIGHT][ZZ] );
      printf( "kneesd: %g %g; %g %g\n", s->kneed[LEFT][XX], s->kneed[LEFT][ZZ],
	      s->kneed[RIGHT][XX], s->kneed[RIGHT][ZZ] );
      printf( "feetd: %g %g; %g %g\n", s->footd[LEFT][XX], s->footd[LEFT][ZZ],
	      s->footd[RIGHT][XX], s->footd[RIGHT][ZZ] );
    }

  // debug_jacobian1( s );

  impact2_print = 0;
}

/************************************************************************/
/* Need to rerun impact analysis with foot1 free.*/

ss_touchdown_to_ds( SIM *s )
{
  double impact1_analysis[SS_IMPACT1_N];
  double impact3_analysis[SS_IMPACT3_N];
  int i;

  if ( interactive )
    printf( "ss_touchdown_to_ds: %g %d\n", s->time, s->ss_foot_down );

  // for gen-v-data
  if ( s->sdfast_model == SINGLE_SUPPORT )
    for ( i = 0; i < SS_N_STATES; i++ )
      s->stashed_ss_state[i] = s->sdfast_state[i];

  ss_impact1( s, impact1_analysis );
  // ss_impact3( s, impact3_analysis );

  /* Not sure why G plays a role here?
  if ( ( -impact1_analysis[SS_IMPACT1_IMPULSE_Z] < -s->time_step*9.81 )
       && interactive )
  */
  if ( ( -impact1_analysis[SS_IMPACT1_IMPULSE_Z] < 0 ) && interactive )
    {
      printf( "Negative impulse ss_impact1???? %g %g\n",
	      s->time, impact1_analysis[SS_IMPACT1_IMPULSE_Z] );
      impact1_print = 1;
    }

  /* We are allowing negative impulses here.
     Not sure what this is doing here: looks like checking for bad
     impulse on foot that was down.
  if ( impact1_analysis[6] > s->time_step*9.81 )
    {
      printf( "Negative impulse 2 ss_impact1????\n" );
      impact1_print = 1;
    }
  */

  if ( impact1_print )
    {
      printf( "p, lh, lk, rh, rk: %g %g %g %g %g\n",
	      s->roll, s->hip_angle[LEFT],
	      s->knee_length[LEFT],
	      s->hip_angle[RIGHT],
	      s->knee_length[RIGHT] );
      printf( "pd, lhd, lkd, rhd, rkd: %g %g %g %g %g\n",
	      s->rolld, s->hip_angled[LEFT],
	      s->knee_lengthd[LEFT],
	      s->hip_angled[RIGHT],
	      s->knee_lengthd[RIGHT] );
      
      printf( "hips: %g %g; %g %g\n", s->hip[LEFT][XX], s->hip[LEFT][ZZ],
	      s->hip[RIGHT][XX], s->hip[RIGHT][ZZ] );
      printf( "knees: %g %g; %g %g\n", s->knee[LEFT][XX], s->knee[LEFT][ZZ],
	      s->knee[RIGHT][XX], s->knee[RIGHT][ZZ] );
      printf( "feet: %g %g; %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ],
	      s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );
      printf( "hipsd: %g %g; %g %g\n", s->hipd[LEFT][XX], s->hipd[LEFT][ZZ],
	      s->hipd[RIGHT][XX], s->hipd[RIGHT][ZZ] );
      printf( "kneesd: %g %g; %g %g\n", s->kneed[LEFT][XX], s->kneed[LEFT][ZZ],
	      s->kneed[RIGHT][XX], s->kneed[RIGHT][ZZ] );
      printf( "feetd: %g %g; %g %g\n", s->footd[LEFT][XX], s->footd[LEFT][ZZ],
	      s->footd[RIGHT][XX], s->footd[RIGHT][ZZ] );
      printf( "force: %g %g; %g %g\n",
	      s->ground_force[LEFT][XX], s->ground_force[LEFT][ZZ],
	      s->ground_force[RIGHT][XX], s->ground_force[RIGHT][ZZ] );

      for( i = 0; i < SS_IMPACT1_N; i++ )
	{
	  printf( "%d: %g\n", i, impact1_analysis[i] );
	}
    }

  /*
  if ( s->ss_foot_down == LEFT )
    {
      ds_flag = (impact1_analysis[6] < s->time_step*s->ground_force[LEFT][ZZ]);
      printf( "ds? %d; %g %g; %g %g\n",
	      ds_flag, impact1_analysis[6], s->time_step*s->ground_force[LEFT][ZZ],
	      s->time_step, s->ground_force[LEFT][ZZ] );
    }
  else
    {
      ds_flag = (impact1_analysis[6] < s->time_step*s->ground_force[RIGHT][ZZ]);
      printf( "ds? %d; %g %g; %g %g\n",
	      ds_flag, impact1_analysis[6], s->time_step*s->ground_force[RIGHT][ZZ],
	      s->time_step, s->ground_force[RIGHT][ZZ] );
    }
  */


  for( i = 0; i < SS_N_Q; i++ )
    s->sdfast_state[i+SS_N_Q] = impact1_analysis[i];

  /*
    printf( "hip: %g %g\n", s->hip[XX], s->hip[ZZ] );
    printf( "feet: %g %g; %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ],
    s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );
    printf( "hipd: %g %g\n", s->hipd[XX], s->hipd[ZZ] );
  */
  if ( s->ss_foot_down == LEFT )
    ssl_forward_kinematics( s );
  else
    ssr_forward_kinematics( s );
  /*
    printf( "hip: %g %g\n", s->hip[XX], s->hip[ZZ] );
    printf( "feet: %g %g; %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ],
    s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );
    printf( "hipd: %g %g\n", s->hipd[XX], s->hipd[ZZ] );
  */

  s->last_impulse_side = other_side( s->ss_foot_down );
  s->last_impulse[XX] = -impact1_analysis[SS_IMPACT1_IMPULSE_X];
  s->last_impulse[YY] = 0.0;
  s->last_impulse[ZZ] = -impact1_analysis[SS_IMPACT1_IMPULSE_Z];
  s->last_impulse_time = s->time;

  ds_init_double_support_state( s );

  /*
    printf( "hip: %g %g\n", s->hip[XX], s->hip[ZZ] );
    printf( "feet: %g %g; %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ],
    s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );
    printf( "hipd: %g %g\n", s->hipd[XX], s->hipd[ZZ] );
  */

  impact1_print = 0;
}

/************************************************************************/
/* Assumes foot1 is on ground, and foot2 just touched down.
Two things can happen.
A) foot1 can be jerked off the ground during the impact.
B) Both feet can end up on the ground: double support.
We first assume case A is what will happen, and check the
velocity of foot1. If it is downwards, it must be case B.
A few things can go wrong.
In either case the impulse for foot2 can be downwards. Need
to understand whether this is a bug or is correct.
It may be the impulse for foot1 in the double support case
has the wrong sign. Need
to understand whether this is a bug or is correct.
*/

ss_touchdown( SIM *s )
{
  double impact1_analysis[SS_IMPACT2_N+4+4];
  double impact2_analysis[SS_IMPACT2_N+4+4];
  double impact4_analysis[SS_IMPACT4_N];
  int ds_flag;

  if ( interactive )
    printf( "\nss_touchdown: %g; old foot down: %d\n", s->time,
	    s->ss_foot_down );

  /*
  printf( "hip: %g %g\n", s->hip[XX], s->hip[ZZ] );
  printf( "knees: %g %g; %g %g\n", s->knee[LEFT][XX], s->knee[LEFT][ZZ],
	  s->knee[RIGHT][XX], s->knee[RIGHT][ZZ] );
  printf( "feet: %g %g; %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ],
	  s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );
  */

  /* Check whether the stance foot reaction force has right sign. */
  /*
  ss_impact1( s, impact1_analysis );
  */

  /* Check whether the stance foot lifts off. */
  ss_impact2( s, impact2_analysis );
  /*
  debug_jacobian2( s );
  ss_impact4( s, impact4_analysis );
  */

  /*
  if ( ( impact1_analysis[6] <= 0 ) != ( impact2_analysis[20] < 0 ) && interactive )
    {
      printf( "Conflict between predictions. %g %g\n", impact1_analysis[6], impact2_analysis[20] );
      printf( "Press return to continue.\n" );
      getchar();
    }
  */

  /* Here we implement a threshold for double support. */
  /*
  if ( s->ss_foot_down == LEFT )
    {
      ds_flag = (impact1_analysis[6] < s->time_step*s->ground_force[LEFT][ZZ]);
      if ( interactive )
	printf( "ds? %d; %g %g; %g %g\n",
		ds_flag, impact1_analysis[6], s->time_step*s->ground_force[LEFT][ZZ],
		s->time_step, s->ground_force[LEFT][ZZ] );
    }
  else
    {
      ds_flag = (impact1_analysis[6] < s->time_step*s->ground_force[RIGHT][ZZ]);
      if ( interactive )
	printf( "ds? %d; %g %g; %g %g\n",
		ds_flag, impact1_analysis[6], s->time_step*s->ground_force[RIGHT][ZZ],
		s->time_step, s->ground_force[RIGHT][ZZ] );
    }
  */

  if ( interactive )
    printf( "footd_z: %g\n", impact2_analysis[SS_IMPACT2_FOOTD_Z] );

  if ( impact2_analysis[SS_IMPACT2_FOOTD_Z] > 0 )
    {
      touchdown_to_ss( s );
      return;
    }

  /* looks like double support */
  ss_touchdown_to_ds( s );
}

/************************************************************************/

void integrate_one_time_step_in_air( SIM *s  )
{
  int i, step;
  int err; 
    /* { OK, DERIVATIVE_DISCONTINUITY, SYSTEM_LOCKED, CONSTRAINTS_ERR } */
  double errest;
  int flag = 1;
  int is_bad_number = 0;

  // clear outstanding error flags
  sraclearerr();

  for( step = 0;
       step < s->sdfast_integration_steps_per_control_step_impact;
       step++ )
    {
      srafmotion( &(s->time), s->sdfast_state, s->sdfast_stated,
	  s->time_step/s->sdfast_integration_steps_per_control_step_impact,
		  s->sdfast_ctol, &flag, &errest, &err );
      sraprinterr( stderr );
      sraclearerr();

      for( i = 0; i < AIR_N_STATES; i++ )
	{ // isfinite() should have worked.
	  if ( isnan( s->sdfast_state[i] ) || isinf( s->sdfast_state[i])
	       || isnan( s->sdfast_stated[i] )
	       || isinf( s->sdfast_stated[i]) ) 
	    {
	      is_bad_number = 1;
	      fprintf( stderr, "Nan detected.\n" );
	      break;
	    }
	}
	  
      if ( err > 0 )
	{
	  fprintf( stderr, "sdfast error %d\n", err );
	  break;
	}
    }

  if ( is_bad_number || (err > 0) )
    {
      s->status = CRASHED;
      return;
    }

  in_air_forward_kinematics( s );

  if ( s->disable_dynamics_transitions )
    return;

  if ( s->foot[LEFT][ZZ] < 0 && s->foot[RIGHT][ZZ] < 0 )
    // Need to do double impacts.
    {
      fprintf( stderr, "Need to handle double impact.\n" );
      s->status = CRASHED;
      return;
      // exit( -1 );
    }
  else if ( s->foot[LEFT][ZZ] < 0 )
    {
      s->ss_foot_down = RIGHT; // Fake out collision software
      touchdown_to_ss( s );
    }
  else if ( s->foot[RIGHT][ZZ] < 0 )
    {
      s->ss_foot_down = LEFT; // Fake out collision software
      touchdown_to_ss( s );
    }
}

/************************************************************************/

void integrate_one_time_step_single_support_left_down( SIM *s  )
{
  int i, step;
  int err; 
    /* { OK, DERIVATIVE_DISCONTINUITY, SYSTEM_LOCKED, CONSTRAINTS_ERR } */
  double errest;
  int flag = 1;
  int is_bad_number = 0;
  double ss_joint_forces[SS_N_JOINTS][3];
  double ss_joint_torques[SS_N_JOINTS][3];
  int foot_status_local[2];

  if ( s->ss_foot_down != LEFT )
    {
      fprintf( stderr,
	       "integrate_one_time_step_single_support_left_down: wrong foot down: %d\n",
	       s->ss_foot_down );
      exit( -1 );
    }

  // clear outstanding error flags
  srssclearerr();

  for( step = 0;
       step < s->sdfast_integration_steps_per_control_step_impact;
       step++ )
    {
      srssfmotion( &(s->time), s->sdfast_state, s->sdfast_stated,
	   s->time_step/s->sdfast_integration_steps_per_control_step_impact,
		   SS_CTOL, &flag, &errest, &err );
      srssprinterr(stderr);
      srssclearerr();

      for( i = 0; i < SS_N_STATES; i++ )
	{ // isfinite() should have worked.
	  if ( isnan( s->sdfast_state[i] ) || isinf( s->sdfast_state[i])
	       || isnan( s->sdfast_stated[i] )
	       || isinf( s->sdfast_stated[i]) ) 
	    {
	      is_bad_number = 1;
	      fprintf( stderr, "SS: Nan detected.\n" );
	      break;
	    }
	}
	  
      if ( err > 0 )
	{
	  fprintf( stderr, "SS: sdfast error %d\n", err );
	  break;
	}
    }

  if ( is_bad_number || (err > 0) )
    {
      s->status = CRASHED;
      return;
    }

  srssreac( ss_joint_forces, ss_joint_torques );
  
  if ( s->ss_foot_down == LEFT )
    {
      srsstrans( SS_CALF1_BODY, ss_joint_forces[SS_ANKLE1], SS_GND_BODY,
		 s->ground_force[LEFT] );
      for( i = 0; i < 3; i++ )
	s->ground_force[RIGHT][i] = 0;
    }
  else
    {
      srsstrans( SS_CALF1_BODY, ss_joint_forces[SS_ANKLE1], SS_GND_BODY,
		 s->ground_force[RIGHT] );
      for( i = 0; i < 3; i++ )
	s->ground_force[LEFT][i] = 0;
    }

  srssprinterr(stderr);
  srssclearerr();

  ssl_forward_kinematics( s );

  /* Should handle this better */
  if ( s->disable_dynamics_transitions )
       return;

  foot_status_local[LEFT] = FOOT_ON_GROUND;
  foot_status_local[RIGHT] = FOOT_IN_AIR;
  if ( s->ground_force[LEFT][ZZ] < 0 )
    foot_status_local[LEFT] = FOOT_IN_AIR;
  else if ( fabs( s->ground_force[LEFT][XX]/s->ground_force[LEFT][ZZ] )
	    > sim.friction_cone_limit )
    foot_status_local[LEFT] = FOOT_SLIDING;
  // impact changes everything, test for stance foot sliding 
  // after considering impact.
  
  if ( s->foot[RIGHT][ZZ] < EPSILON1 )
    {
      ss_touchdown( s );
      return;
    }
  if ( foot_status_local[LEFT] == FOOT_IN_AIR )
    {
      transition_to_air( s );
      return;
    }
  if ( foot_status_local[LEFT] == FOOT_SLIDING )
    { 
      fprintf( stderr, "SS1: Need to handle foot sliding.\n" );
      s->status = CRASHED;
      return;
    }
}

/************************************************************************/

void integrate_one_time_step_single_support_right_down( SIM *s  )
{
  int i, step;
  int err; 
    /* { OK, DERIVATIVE_DISCONTINUITY, SYSTEM_LOCKED, CONSTRAINTS_ERR } */
  double errest;
  int flag = 1;
  int is_bad_number = 0;
  double ss_joint_forces[SS_N_JOINTS][3];
  double ss_joint_torques[SS_N_JOINTS][3];
  int foot_status_local[2];

  if ( s->ss_foot_down != RIGHT )
    {
      fprintf( stderr,
	       "integrate_one_time_step_single_support_right_down: wrong foot down: %d\n",
	       s->ss_foot_down );
      exit( -1 );
    }

  // clear outstanding error flags
  srssrclearerr();

  for( step = 0;
       step < s->sdfast_integration_steps_per_control_step_impact;
       step++ )
    {
      srssrfmotion( &(s->time), s->sdfast_state, s->sdfast_stated,
	   s->time_step/s->sdfast_integration_steps_per_control_step_impact,
		   SS_CTOL, &flag, &errest, &err );
      srssrprinterr(stderr);
      srssrclearerr();

      for( i = 0; i < SS_N_STATES; i++ )
	{ // isfinite() should have worked.
	  if ( isnan( s->sdfast_state[i] ) || isinf( s->sdfast_state[i])
	       || isnan( s->sdfast_stated[i] )
	       || isinf( s->sdfast_stated[i]) ) 
	    {
	      is_bad_number = 1;
	      fprintf( stderr, "SS: Nan detected.\n" );
	      break;
	    }
	}
	  
      if ( err > 0 )
	{
	  fprintf( stderr, "SS: sdfast error %d\n", err );
	  break;
	}
    }

  if ( is_bad_number || (err > 0) )
    {
      s->status = CRASHED;
      return;
    }

  srssrreac( ss_joint_forces, ss_joint_torques );
  
  if ( s->ss_foot_down == LEFT )
    {
      srssrtrans( SS_CALF1_BODY, ss_joint_forces[SS_ANKLE1], SS_GND_BODY,
		 s->ground_force[LEFT] );
      for( i = 0; i < 3; i++ )
	s->ground_force[RIGHT][i] = 0;
    }
  else
    {
      srssrtrans( SS_CALF1_BODY, ss_joint_forces[SS_ANKLE1], SS_GND_BODY,
		 s->ground_force[RIGHT] );
      for( i = 0; i < 3; i++ )
	s->ground_force[LEFT][i] = 0;
    }

  srssrprinterr(stderr);
  srssrclearerr();

  ssr_forward_kinematics( s );

  /* Should handle this better */
  if ( s->disable_dynamics_transitions )
       return;

  foot_status_local[RIGHT] = FOOT_ON_GROUND;
  foot_status_local[LEFT] = FOOT_IN_AIR;
  if ( s->ground_force[RIGHT][ZZ] < 0 )
    foot_status_local[RIGHT] = FOOT_IN_AIR;
  else if ( fabs( s->ground_force[RIGHT][XX]
		  /s->ground_force[RIGHT][ZZ] )
	    > sim.friction_cone_limit )
    foot_status_local[RIGHT] = FOOT_SLIDING;
  // impact changes everything, test for stance foot sliding 
  // after considering impact.

  if ( s->foot[LEFT][ZZ] < EPSILON1 )
    {
      ss_touchdown( s );
      return;
    }
  if ( foot_status_local[RIGHT] == FOOT_IN_AIR )
    {
      transition_to_air( s );
      return;
    }
  if ( foot_status_local[RIGHT] == FOOT_SLIDING )
    { 
      fprintf( stderr, "SS1: Need to handle foot sliding.\n" );
      s->status = CRASHED;
      return;
    }
}

/************************************************************************/

void integrate_one_time_step_double_support( SIM *s  )
{
  int i, step;
  int err; 
    /* { OK, DERIVATIVE_DISCONTINUITY, SYSTEM_LOCKED, CONSTRAINTS_ERR } */
  double errest;
  int flag = 1;
  int is_bad_number = 0;
  double ds_joint_forces[DS_N_JOINTS][3];
  double ds_joint_torques[DS_N_JOINTS][3];
  int foot_status_local[2];

  // clear outstanding error flags
  srdsclearerr();

  for( step = 0;
       step < s->sdfast_integration_steps_per_control_step_impact;
       step++ )
    {
      if ( ds_debug )
	printf( "%d: %g; %g %g %g %g %g; %g %g %g %g %g; %g %g %d\n",
		step,
		s->time, 
		s->sdfast_state[0],
		s->sdfast_state[1],
		s->sdfast_state[2],
		s->sdfast_state[3],
		s->sdfast_state[4],
		s->sdfast_stated[0],
		s->sdfast_stated[1],
		s->sdfast_stated[2],
		s->sdfast_stated[3],
		s->sdfast_stated[4],
		s->time_step/s->sdfast_integration_steps_per_control_step_impact,
		DS_CTOL, 
		flag );
      if ( ds_debug )
	printf( "%g %g %g %g; %g %g\n",
		s->foot[LEFT][XX], s->foot[LEFT][ZZ],
		s->foot[RIGHT][XX], s->foot[RIGHT][ZZ],
		s->foot[RIGHT][XX] - s->foot[LEFT][XX],
		s->foot[RIGHT][ZZ] - s->foot[LEFT][ZZ] );

      srdsfmotion( &(s->time), s->sdfast_state, s->sdfast_stated,
	   s->time_step/s->sdfast_integration_steps_per_control_step_impact,
		   DS_CTOL, &flag, &errest, &err );
      srdsprinterr(stderr);

      if ( ds_debug )
	printf( "%d: %g; %g %g %g %g %g; %g %g %g %g %g; %g %g %d\n",
		step,
		s->time, 
		s->sdfast_state[0],
		s->sdfast_state[1],
		s->sdfast_state[2],
		s->sdfast_state[3],
		s->sdfast_state[4],
		s->sdfast_stated[0],
		s->sdfast_stated[1],
		s->sdfast_stated[2],
		s->sdfast_stated[3],
		s->sdfast_stated[4],
		s->time_step/s->sdfast_integration_steps_per_control_step_impact,
		DS_CTOL, 
		flag );

      for( i = 0; i < DS_N_STATES; i++ )
	{ // isfinite() should have worked.
	  if ( isnan( s->sdfast_state[i] ) || isinf( s->sdfast_state[i])
	       || isnan( s->sdfast_stated[i] )
	       || isinf( s->sdfast_stated[i]) ) 
	    {
	      is_bad_number = 1;
	      fprintf( stderr, "DS: Nan detected.\n" );
	      break;
	    }
	}
	  
      if ( err > 0 )
	{
	  fprintf( stderr, "DS: sdfast error %d\n", err );
	  break;
	}
    }

  if ( is_bad_number || (err > 0) )
    {
      s->status = CRASHED;
      return;
    }

  for( i = 0; i < DS_N_STATES; i++ )
    { // isfinite() should have worked.
      if ( isnan( s->sdfast_state[i] ) || isinf( s->sdfast_state[i])
	   || isnan( s->sdfast_stated[i] )
	   || isinf( s->sdfast_stated[i]) ) 
	{
	  is_bad_number = 1;
	  fprintf( stderr, "DS2: Nan detected.\n" );
	  s->status = CRASHED;
	  return;
	}
    }

  ds_forward_kinematics( s );

  srdsreac( ds_joint_forces, ds_joint_torques );
  srdstrans( DS_CALF1_BODY, ds_joint_forces[DS_ANKLE1], DS_GND_BODY,
	     s->ground_force[LEFT] );
  srdstrans( DS_CALF2_BODY, ds_joint_forces[DS_KNEE2+1], DS_GND_BODY,
	     s->ground_force[RIGHT] );

  for ( i = LEFT; i <= RIGHT; i++ )
    {
      foot_status_local[i] = FOOT_ON_GROUND;
      if ( s->ground_force[i][ZZ] < 0 )
	foot_status_local[i] = FOOT_IN_AIR;
      else if ( fabs( s->ground_force[i][XX]/s->ground_force[i][ZZ] )
		> sim.friction_cone_limit )
	foot_status_local[i] = FOOT_SLIDING;
    }

  // printf( "%g %d %d\n", s->time, foot_status_local[LEFT], foot_status_local[RIGHT] );

  /* Should handle this better */
  // neeed to correct negative forces?
  if ( s->disable_dynamics_transitions )
       return;

  if ( foot_status_local[LEFT] == FOOT_ON_GROUND &&
       foot_status_local[RIGHT] == FOOT_IN_AIR )
    {
      /* Fix ground forces */
      s->ground_force[LEFT][XX] += s->ground_force[RIGHT][XX];
      s->ground_force[LEFT][ZZ] += s->ground_force[RIGHT][ZZ];
      s->ground_force[RIGHT][XX] = 0;
      s->ground_force[RIGHT][ZZ] = 0;
      /* There doesn't seem to be a right answer to this, so I am going
	 to do a messy hack. */
      if ( s->ground_force[LEFT][ZZ] < 0 )
	s->ground_force[LEFT][ZZ] = 0;
      init_state_one_foot_on_ground( s, LEFT );
      return;
    }

  if ( foot_status_local[RIGHT] == FOOT_ON_GROUND &&
       foot_status_local[LEFT] == FOOT_IN_AIR )
    {
      /* Fix ground forces */
      s->ground_force[RIGHT][XX] += s->ground_force[LEFT][XX];
      s->ground_force[RIGHT][ZZ] += s->ground_force[LEFT][ZZ];
      s->ground_force[LEFT][XX] = 0;
      s->ground_force[LEFT][ZZ] = 0;
      /* There doesn't seem to be a right answer to this, so I am going
	 to do a messy hack. */
      if ( s->ground_force[RIGHT][ZZ] < 0 )
	s->ground_force[RIGHT][ZZ] = 0;
      init_state_one_foot_on_ground( s, RIGHT );
      return;
    }

  /* fix negative forces */
  if ( s->ground_force[LEFT][ZZ] < 0 )
    s->ground_force[LEFT][ZZ] = 0;
  if ( s->ground_force[RIGHT][ZZ] < 0 )
    s->ground_force[RIGHT][ZZ] = 0;

  /* Handle one foot slipping with this, which is not quite correct. */
  if ( foot_status_local[LEFT] == FOOT_ON_GROUND &&
       foot_status_local[RIGHT] != FOOT_ON_GROUND )
    {
      init_state_one_foot_on_ground( s, LEFT );
      return;
    }

  /* Handle one foot slipping with this, which is not quite correct. */
  if ( foot_status_local[RIGHT] == FOOT_ON_GROUND &&
       foot_status_local[LEFT] != FOOT_ON_GROUND )
    {
      init_state_one_foot_on_ground( s, RIGHT );
      return;
    }

  if ( foot_status_local[LEFT] == FOOT_IN_AIR &&
       foot_status_local[RIGHT] == FOOT_IN_AIR )
    {
      transition_to_air( s );
      return;
    }

  if ( foot_status_local[LEFT] != FOOT_ON_GROUND ||
       foot_status_local[RIGHT] != FOOT_ON_GROUND )
    {
      fprintf( stderr, "Need to handle two feet sliding in DS.\n" );
      s->status = CRASHED;
      return;
      // exit( -1 );
    }

  // All feet locked on ground
  // foot_status_local[LEFT] == FOOT_ON_GROUND &&
  // foot_status_local[RIGHT] == FOOT_ON_GROUND

#ifdef COMMENT
  /* Okay, if feet together, get weird force distribution */
  /* Fix it by averaging forces */
  if ( fabs( s->foot[RIGHT][XX] - s->foot[LEFT][XX] ) < FEET_TOGETHER
       || fabs( s->foot[RIGHT][ZZ] - s->foot[LEFT][ZZ] ) < FEET_TOGETHER )
    {
      s->ground_force[LEFT][ZZ] = s->ground_force[RIGHT][ZZ] =
	(s->ground_force[LEFT][ZZ] + s->ground_force[RIGHT][ZZ])/2;
      /*
	printf( "grf: %g %g %g %g %g %g\n", 
	s->ground_force[LEFT][XX],
	s->ground_force[LEFT][YY],
	s->ground_force[LEFT][ZZ],
	s->ground_force[RIGHT][XX],
	s->ground_force[RIGHT][YY],
	s->ground_force[RIGHT][ZZ] );
      */
    }
#endif
}

/************************************************************************/
/* This is what is called on each integration step */

void integrate_one_time_step( SIM *s  )
{ 
  
  /*
  printf( "%g\n", s->time );
  */

  if( s->status == CRASHED )
    {
      s->time += s->time_step;
      return;
    }

  /* Give up if hip is too high or too low */
  if ( s->groin[ZZ] < s->hip_min || s->groin[ZZ] > s->hip_max )
    {
      s->status = CRASHED;
      s->time += s->time_step;
      return;
    }

  if ( s->sdfast_model == IN_AIR )
    integrate_one_time_step_in_air( s );
  else if ( s->sdfast_model == SINGLE_SUPPORT )
    {
      if ( s->ss_foot_down == LEFT )
	integrate_one_time_step_single_support_left_down( s );
      else
	integrate_one_time_step_single_support_right_down( s );
    }
  else if ( s->sdfast_model == DOUBLE_SUPPORT )
    integrate_one_time_step_double_support( s );

  if ( !(s->disable_dynamics_transitions)
       && (s->ground_force[LEFT][ZZ] < 0 || s->ground_force[RIGHT][ZZ] < 0 ) )
    {
      printf( "Foot force error: %g %d %g %g\n",
	      s->time, s->sdfast_model, s->ground_force[LEFT][ZZ],
	      s->ground_force[RIGHT][ZZ] );
      // exit( -1 );
    }

  /*
  printf( "Press return to continue.\n" );
  getchar();
  */
}

/************************************************************************/
/* SDFAST stuff */
/************************************************************************/
/* This is where the control (hip_torque) is applied. May be called many
times per integration step at any time or state. */

void srauforce(double t, double *q, double *u)
{
  double force[3] = { 0, 0, 0 }; // force vector in world coordinates
  double force_b[3];  // force vector in body coordinates
  double offset[3] = { 0, 0, 0 };

  // Apply horizontal perturbation to torso
  force[XX] = sim.torso_perturbation;
  // transform the vector into body coordinates
  sratrans( AIR_WORLD_BODY, force, AIR_TORSO_BODY, force_b );
  // apply to the model
  srapointf( AIR_TORSO_BODY, offset, force_b );
  // printf( "force_b: %g %g %g\n", force_b[0], force_b[1], force_b[2] );

  /* Apply joint torques */
  srahinget( AIR_L_HIP_JOINT, 0, -sim.hip_torque[LEFT] );
  srahinget( AIR_L_KNEE_JOINT, 0, sim.knee_force[LEFT] );
  srahinget( AIR_R_HIP_JOINT, 0, -sim.hip_torque[RIGHT] );
  srahinget( AIR_R_KNEE_JOINT, 0, sim.knee_force[RIGHT] );

  /* There are no ankle torques in the air. */

  sraprinterr(stderr);
  sraclearerr();
}

/*************************************************************************/

void srssuforce(double t, double *q, double *u)
{
  double force[3] = { 0, 0, 0 }; // force vector in world coordinates
  double force_b[3];  // force vector in body coordinates
  double offset[3] = { 0, 0, 0 };

  // Apply horizontal perturbation to torso
  force[XX] = sim.torso_perturbation;
  // transform the vector into body coordinates
  srsstrans( SS_GND_BODY, force, SS_TORSO_BODY, force_b );
  // apply to the model
  srsspointf( SS_TORSO_BODY, offset, force_b );
  // printf( "force_b: %g %g %g\n", force_b[0], force_b[1], force_b[2] );

  if ( sim.ss_foot_down == LEFT )
    {
      srsshinget( SS_ANKLE1_JOINT, 0, -sim.ankle_torque[LEFT] );
      srsshinget( SS_KNEE1_JOINT, 0, sim.knee_force[LEFT] );
      srsshinget( SS_HIP1_JOINT, 0, -sim.hip_torque[LEFT] );
      srsshinget( SS_HIP2_JOINT, 0, sim.hip_torque[RIGHT] );
      srsshinget( SS_KNEE2_JOINT, 0, sim.knee_force[RIGHT] );
    }
  else
    {
      srsshinget( SS_ANKLE1_JOINT, 0, -sim.ankle_torque[RIGHT] );
      srsshinget( SS_KNEE1_JOINT, 0, sim.knee_force[RIGHT] );
      srsshinget( SS_HIP1_JOINT, 0, -sim.hip_torque[RIGHT] );
      srsshinget( SS_HIP2_JOINT, 0, sim.hip_torque[LEFT] );
      srsshinget( SS_KNEE2_JOINT, 0, sim.knee_force[LEFT] );
    }

  srssprinterr(stderr);
  srssclearerr();
}

/*************************************************************************/

void srssruforce(double t, double *q, double *u)
{
  double force[3] = { 0, 0, 0 }; // force vector in world coordinates
  double force_b[3];  // force vector in body coordinates
  double offset[3] = { 0, 0, 0 };

  // Apply horizontal perturbation to torso
  force[XX] = sim.torso_perturbation;
  // transform the vector into body coordinates
  srssrtrans( SS_GND_BODY, force, SS_TORSO_BODY, force_b );
  // apply to the model
  srssrpointf( SS_TORSO_BODY, offset, force_b );
  // printf( "force_b: %g %g %g\n", force_b[0], force_b[1], force_b[2] );

  srssrhinget( SS_ANKLE1_JOINT, 0, -sim.ankle_torque[RIGHT] );
  srssrhinget( SS_KNEE1_JOINT, 0, sim.knee_force[RIGHT] );
  srssrhinget( SS_HIP1_JOINT, 0, -sim.hip_torque[RIGHT] );
  srssrhinget( SS_HIP2_JOINT, 0, sim.hip_torque[LEFT] );
  srssrhinget( SS_KNEE2_JOINT, 0, sim.knee_force[LEFT] );

  srssrprinterr(stderr);
  srssrclearerr();
}

/*************************************************************************/

void srdsuforce( double t, double *q, double *u )
{
  double force[3] = { 0, 0, 0 }; // force vector in world coordinates
  double force_b[3];  // force vector in body coordinates
  double offset[3] = { 0, 0, 0 };
  double torque[3] = { 0, 0, 0 };

  // Apply horizontal perturbation to torso
  force[XX] = sim.torso_perturbation;
  // transform the vector into body coordinates
  srdstrans( DS_GND_BODY, force, DS_TORSO_BODY, force_b );
  // apply to the model
  srdspointf( DS_TORSO_BODY, offset, force_b );
  // printf( "force_b: %g %g %g\n", force_b[0], force_b[1], force_b[2] );

  /* Apply joint torques */
  srdshinget( DS_ANKLE1_JOINT, 0, -sim.ankle_torque[LEFT] );
  srdshinget( DS_KNEE1_JOINT, 0, sim.knee_force[LEFT] );
  srdshinget( DS_HIP1_JOINT, 0, -sim.hip_torque[LEFT] );
  srdshinget( DS_HIP2_JOINT, 0, sim.hip_torque[RIGHT] );
  srdshinget( DS_KNEE2_JOINT, 0, sim.knee_force[RIGHT] );
  srdshinget( DS_ANKLE2_JOINT, 0, -sim.ankle_torque[RIGHT] );

  if ( ds_debug )
    printf( "%g; %g %g %g %g %g %g\n",
	    sim.time,
	    sim.ankle_torque[LEFT],
	    sim.knee_force[LEFT],
	    sim.hip_torque[LEFT],
	    sim.hip_torque[RIGHT],
	    sim.knee_force[RIGHT],
	    sim.ankle_torque[RIGHT] );

  /* Apply ankle torques: Duplicates srdshinget above */
  /*
  torque[YY] = sim.ankle_torque[RIGHT];
  srdsbodyt( DS_CALF2_BODY, torque );
  */

  // printf( "%g %g\n", sim.time, sim.ankle_torque[RIGHT] );

  srdsprinterr(stderr);
  srdsclearerr();
}

/************************************************************************/
/************************************************************************/
