function solve_one_step(
  StructArray body,
  StructArray contact,
  RealArray mass,
  MatrixArray inertia,
  Real normal_epsilon,   
  Real normal_compliance
  Real tan_epsilon
  Real tan_compliance,
  Real damping,
  Real time_step,
  Real friction_coefficient
  VectorArray epsilon,
  VectorArray D,
  VectorArray lambda,
  VectorArray f_free;
)
{

  for (int i = contact.first; i <= contact.last; ++i) {

    /*
     * Contact constrain data
     */
    epsilon[i] = Vecter3(normal_epsilon, tan_epsilon, tan_epsilon);
    Real gamma = 1.0 / ( 1.0 + 4.0 * damping / time_step );
    lambda[i] = Vector3(0,0,0);
    
    Vector3 pos1 = contact[i].body_1.position;
    Vector3 pos2 = contact[i].body_2.position;
    Vector3 contact_point = contact_point[i];
    Vector3 localPoint1 = contact[i].contact_point - pos1;
    Vector3 localPoint2 = contact[i].contact_point - pos2;

    G = contact[i].Jacobian;

    //First row
    G.normal = Vector4(normal, 
	  crossproduct(-normal ^ localPoint1),
	  -normal, 
	  crossproduct(normal ^ localPoint2));

    //Second row
    G.tang_u = Vector4(u, 
	  crossproduct(-u, localPoint1),
	  -u, 
	  crossproduct(u, localPoint1));

    //Third row
    G.tang_v = Vector4(v, 
	  crossproduct(-v, localPoint1),
	  -v, 
	  crossproduct(v, localPoint1));

    //Penetration depth (nonlinear)
    contact[i].g_n = math::pow(contact[i].g, 4/5);
    G_n = G*math::pow(contact[i].g, 1/5)*4/5;
    contact[i].G_n = G_n;
    
    GMInv = contact[i].GMInv;
    GMInv.normal[0]= G_n.normal[0]/contact[i].body_1.mass;
    GMInv.normal[2]= G_n.normal[0]/contact[i].body_2.mass;
    GMInv.tang_u[0]= G_n.normal[0]/contact[i].body_1.mass;
    GMInv.tang_u[2]= G_n.normal[0]/contact[i].body_2.mass;
    GMInv.tang_v[0]= G_n.normal[0]/contact[i].body_1.mass;
    GMInv.tang_v[2]= G_n.normal[0]/contact[i].body_2.mass;
    GMInv.normal[1]= G_n.normal[0]/contact[i].body_1.inertia;
    GMInv.normal[3]= G_n.normal[0]/contact[i].body_2.inertia;
    GMInv.tang_u[1]= G_n.normal[0]/contact[i].body_1.inertia;
    GMInv.tang_u[3]= G_n.normal[0]/contact[i].body_2.inertia;
    GMInv.tang_v[1]= G_n.normal[0]/contact[i].body_1.inertia;
    GMInv.tang_v[3]= G_n.normal[0]/contact[i].body_2.inertia;

    
    D[i] = Vector3(normal_epsilon, tan_epsilon, tan_epsilon);
    D[i] +=  Vector3(G_n.normal[0] * GMInv.normal[0] + G_n.normal[2] * GMInv.normal[2], 
		  G_n.tang_u[0] * GMInv.tang_u[0] + G_n.tang_u[2] * GMInv.tang_u[2], 
		  G_n.tang_v[0] * GMInv.tang_v[0] + G_n.tang_v[2] * GMInv.tang_v[2],);

    /* Compute RHS (q) */
    contact.rhs = Vector3(4.0/time_step*gamma*(contact[i].g_n), 0, 0);
    contact.rhs[0] += gamma*(G.normal[0]*contact[i].body_1.velocity;
    contact.rhs[0] += gamma*(G.normal[1]*contact[i].body_1.angularVelocity;
    contact.rhs[0] += gamma*(G.normal[2]*contact[i].body_2.velocity;
    contact.rhs[0] += gamma*(G.normal[3]*contact[i].body_2.angularVelocity;

    }
    
  /* Compute p */
  for (int i = body.first; i <= body.last; ++i) {
    p[i] = mass[i]*body[i].velocity + time_step*f_free;
  }    
    
  /*
  * Solve impacts
  */
  for (int i = contact.first; i <= contact.last; ++i) {
    /*
     * Solve impacts
     */
    // Calculate residual - normal.
    rn = 0 - epsilon[i][0] * lambda[i][0];

    rn -= G.normal[0]*contact[i].body_1.velocity;
    rn -= G.normal[1]*contact[i].body_1.angularVelocity;
    rn -= G.normal[2]*contact[i].body_1.velocity;
    rn -= G.normal[3]*contact[i].body_1.angularVelocity;

    // Update lambda - normal.
    newLambdaN = rn/D[i][0] + lambda[i][0];
    oldLambdaN = lambda[i][0];
    
    lambda[i][0] = math::max(newLambdaN, 0);
    dln = lambda[i][0] - oldLambdaN;

    // Calculate residual - tangents.
    ru = 0 - epsilon[i][1] * lambda[i][1];
    ru -= G.tang_u[0]*contact[i].body_1.velocity;
    ru -= G.tang_u[1]*contact[i].body_1.angularVelocity;
    ru -= G.tang_u[2]*contact[i].body_2.velocity;
    ru -= G.tang_u[3]*contact[i].body_2.angularVelocity;
    
    rv = 0 - epsilon[i][2] * lambda[i][2];
    rv -= G.tang_v[0]*contact[i].body_1.velocity;
    rv -= G.tang_v[1]*contact[i].body_1.angularVelocity;
    rv -= G.tang_v[2]*contact[i].body_2.velocity;
    rv -= G.tang_v[3]*contact[i].body_2.angularVelocity;
    
    oldLambdaU = lambda[i][1];
    oldLambdaV = lambda[i][2];

    lambda[i][1] += ru/D[i][1];
    lambda[i][2] += rv/D[i][2];

    if (lambda[i][0]>0){
	f = math::sqrt(math::pow(math::2norm(G.tang_u[0])*lambda[i][1],2)
		      +math::pow(math::2norm(G.tang_v[0])*lambda[i][1],2));

	if ( f~=0 && f >= friction_coefficient * math::2norm(J.normal)*lambda_n ) {
	    lambda[i][1] = lambda[i][1]*(friction_coefficient*math::2norm(G.normal[0])*lambda[i][0]/f);
	    lambda[i][2] = lambda[i][2]*(friction_coefficient*math::2norm(G.normal[0])*lambda[i][0]/f);
	}
    }else{
	lambda[i][1] = 0;
	lambda[i][2] = 0;
    }

    dlu = lambda[i][1] - oldLambdaU;
    dlv = lambda[i][2] - oldLambdaV;

    contact[i].body_1.velocity += GMInv.normal[0]*dn;
    contact[i].body_1.angularVelocity += GMInv.normal[1]*dn;
    contact[i].body_2.velocity += GMInv.normal[2]*dn;
    contact[i].body_2.angularVelocity += GMInv.normal[3]*dn;

    contact[i].body_1.velocity += GMInv.tang_u[0]*dlu-GMInv.tang_v[0]*dlv;
    contact[i].body_1.angularVelocity += GMInv.tang_u[1]*dlU+GMInv.tang_v[1]*dlV;

    contact[i].body_2.velocity += GMInv.tang_u[2]*dlu+GMInv.tang_v[2]*dlv;
    contact[i].body_2.angularVelocity += GMInv.tang_u[3]*dlU+GMInv.tang_v[3]*dlV;

  }

  /*
   * Pre-solve
   */
  for (int i = body.first; i <= body.last; ++i) {
    body[i].velocity = body[i].velocity + time_step*f_free;
  } 

  /*
   * Continuous contacts
   */
  for (int i = contact.first; i <= contact.last; ++i) {
    /*
     * Solve impacts
     */
    // Calculate residual - normal.
    rn = contact.rhs[0] - epsilon[i][0] * lambda[i][0];

    rn -= G.normal[0]*contact[i].body_1.velocity;
    rn -= G.normal[1]*contact[i].body_1.angularVelocity;
    rn -= G.normal[2]*contact[i].body_1.velocity;
    rn -= G.normal[3]*contact[i].body_1.angularVelocity;

    // Update lambda - normal.
    newLambdaN = rn/D[i][0] + lambda[i][0];
    oldLambdaN = lambda[i][0];
    
    lambda[i][0] = math::max(newLambdaN, 0);
    dln = lambda[i][0] - oldLambdaN;

    // Calculate residual - tangents.
    ru = contact.rhs[1] - epsilon[i][1] * lambda[i][1];
    ru -= G.tang_u[0]*contact[i].body_1.velocity;
    ru -= G.tang_u[1]*contact[i].body_1.angularVelocity;
    ru -= G.tang_u[2]*contact[i].body_2.velocity;
    ru -= G.tang_u[3]*contact[i].body_2.angularVelocity;
    
    rv = contact.rhs[2] - epsilon[i][2] * lambda[i][2];
    rv -= G.tang_v[0]*contact[i].body_1.velocity;
    rv -= G.tang_v[1]*contact[i].body_1.angularVelocity;
    rv -= G.tang_v[2]*contact[i].body_2.velocity;
    rv -= G.tang_v[3]*contact[i].body_2.angularVelocity;
    
    oldLambdaU = lambda[i][1];
    oldLambdaV = lambda[i][2];

    lambda[i][1] += ru/D[i][1];
    lambda[i][2] += rv/D[i][2];

    if (lambda[i][0]>0){
	f = math::sqrt(math::pow(math::2norm(G.tang_u[0])*lambda[i][1],2)
		      +math::pow(math::2norm(G.tang_v[0])*lambda[i][1],2));

	if ( f~=0 && f >= friction_coefficient * math::2norm(J.normal)*lambda_n ) {
	    lambda[i][1] = lambda[i][1]*(friction_coefficient*math::2norm(G.normal[0])*lambda[i][0]/f);
	    lambda[i][2] = lambda[i][2]*(friction_coefficient*math::2norm(G.normal[0])*lambda[i][0]/f);
	}
    }else{
	lambda[i][1] = 0;
	lambda[i][2] = 0;
    }

    dlu = lambda[i][1] - oldLambdaU;
    dlv = lambda[i][2] - oldLambdaV;

    contact[i].body_1.velocity += GMInv.normal[0]*dn;
    contact[i].body_1.angularVelocity += GMInv.normal[1]*dn;
    contact[i].body_2.velocity += GMInv.normal[2]*dn;
    contact[i].body_2.angularVelocity += GMInv.normal[3]*dn;

    contact[i].body_1.velocity += GMInv.tang_u[0]*dlu-GMInv.tang_v[0]*dlv;
    contact[i].body_1.angularVelocity += GMInv.tang_u[1]*dlU+GMInv.tang_v[1]*dlV;

    contact[i].body_2.velocity += GMInv.tang_u[2]*dlu+GMInv.tang_v[2]*dlv;
    contact[i].body_2.angularVelocity += GMInv.tang_u[3]*dlU+GMInv.tang_v[3]*dlV;
  }

  /*
  * Update position and orientation
  */
  for (int i = body.first; i <= body.last; ++i) {
    body[i].position = body[i].position + time_step*body[i].velocity;
    body[i].quaternion = math::update_quaternion(body[i].quaternion, body[i].angularVelocity, time_step);
  } 
  
}