/*
Containing class vars:
handOrn = target orientation quaternion
hand pos = target position
... interpolated values to follow point in front player camera somewhat smoothly
*/

void AccumulateForce (float timestep)
   {
      //position... (code from older newton demos)

      if (!pickedBody) return;
      float MOUSE_PICK_DAMP = 20.0f;
      float MOUSE_PICK_STIFFNESS = 150.0f;
      float ANGULAR_STIFFNESS = 0.1f;

      float mass;
      float Ixx;
      float Iyy;
      float Izz;
      sVec3 com;
      sVec3 veloc;
      sMat4 matrix;
      BodyGetMatrix(pickedBody, matrix);

      BodyGetVelocity (pickedBody, veloc);
      BodyGetMassMatrix (pickedBody, mass, Ixx, Iyy, Izz);

                sVec3 pickedForce = handPos - matrix[3]; // body position
      float mag2 = pickedForce.SqL();
      if (mag2 > 20.0f * 20.0f) pickedForce *= 20.0f / sqrt (mag2);
      sVec3 force (pickedForce * mass * MOUSE_PICK_STIFFNESS);
      sVec3 dampForce (veloc * mass * MOUSE_PICK_DAMP);
      force -= dampForce;
      sVec3 grav (0.0f, mass * GRAVITY_Y, 0.0f);
      force -= grav;

            
      BodyData *data = (BodyData*) BodyGetUserData (pickedBody); // containing force and torque vectors, which are the applied in the callback
      
      data->force += force;

      // orientation...
      sQuat q;
      BodyGetRotation (pickedBody, q);

      sVec3 curAngVel;
      BodyGetAngVel (pickedBody, curAngVel); // == NewtonBodyGetOmega
      
      sVec3 targetAngVel = AngVelFromAToB (q, handOrn) * (1 / timestep * ANGULAR_STIFFNESS);
      sVec3 torque = ConvertAngVelToTorque (targetAngVel, curAngVel, matrix, timestep, mass, Ixx, Iyy, Izz);
      data->torque += torque;

      BodyActivate (pickedBody);
   }



// utilitys...


// convert rotational offset to angular velocity (divide by time afterwards)
inline sVec3 AngVelFromAToB (const sQuat &qA, const sQuat &qB)
{

   const float matchTolerance = 0.01f;
   const float faultTolerance = 0.05f;

   sQuat q = QuatFromAToB (qA, qB);
   sVec3 *omegaDir = (sVec3*)&q;
   float sql = omegaDir->SqL();
   if (sql   < (matchTolerance * matchTolerance))
      return sVec3 (0, 0, 0);

   float length = sqrt (sql);
   if (q[3] < -1) q[3] = -1;
   if (q[3] >  1) q[3] =  1;
   sVec3 angVel = (*omegaDir / length) * (2.0 * acos(q[3]));
   if (length   < faultTolerance) angVel *= (length - matchTolerance) / (faultTolerance - matchTolerance);
   return angVel;

}

// choose shortest rotation between 2 orientations
inline sQuat QuatFromAToB (const sQuat &qA, const sQuat &qB)
{
   sQuat q;
   if (qA.Dot(qB) < 0.0f)
   {
      q[0] = qA[0]; q[1] = qA[1]; q[2] = qA[2];
      q[3] = -qA[3];
   }
   else
   {
      q[0] = -qA[0]; q[1] = -qA[1]; q[2] = -qA[2];
      q[3] = qA[3];
   }
         
   return qB * q;
}

inline sVec3 ConvertAngVelToTorque (   sVec3 &targetAngVel, sVec3 &currentAngVel, sMat4 &matrix,
                        float timestep, float mass, float Ixx, float Iyy, float Izz)
{
   sVec3 torque = (targetAngVel - currentAngVel) / timestep;
   torque = matrix.Unrotate (torque);
   torque[0] *= Ixx;
   torque[1] *= Iyy;
   torque[2] *= Izz;
   torque = matrix.Rotate (torque);
   return torque;
}