I want to apply SLERP function (Quaternion interpolateToSelf(Quaternion target, float t)).
How to implement it with Quaternion alignment and rotation matrix ??


void display() {
    noStroke();
   
    // Rotation vectors
    // use to perform orientation to velocity vector
    Vec3D new_dir = new Vec3D(velocity);
    new_dir.normalize();
   
    Vec3D new_up = new Vec3D(0.0, 1.0, 0.0);
    new_up.normalize();
   
    //axis of rotation
    Vec3D new_side = new_dir.cross(new_up);
    new_side.normalize();
   
    float dotP  = new_dir.dot(new_up);
    float angle = new_dir.angleBetween(new_up, true);
    //println(angle);
    //println(acos(dotP));
   
    //quaternion that rotates the vector given by the "forward" param into the direction given by the "dir" param.
    //public static Quaternion getAlignmentQuat(ReadonlyVec3D dir, ReadonlyVec3D forward)
    Quaternion alignment = Quaternion.getAlignmentQuat( new_dir,Vec3D.Y_AXIS);
   
    //public Matrix4x4 translateSelf(ReadonlyVec3D trans)
    Matrix4x4 mat = new Matrix4x4().translateSelf( mouseX, mouseY, 0);
    mat.multiplySelf(alignment. toMatrix4x4());
      
    pushMatrix();
    // update location
    translate(location.x, location.y, location.z);
   
    float[] axis = Quaternion.getAlignmentQuat( new_dir,Vec3D.Y_AXIS). toAxisAngle();
       
    rotate(axis[0],axis[1],axis[2] ,axis[3]);
     
    agent.display();  //draw stuff
    popMatrix();

}