| Step 1. Initialization: |
| Initialize time interval , step length , configuration , velocity , and let be 0; |
| Step 2. Main loop: |
| Step 2.1. Compute the Gauss points in , corresponding differentiation matrix |
| and quadrature weights ; |
| Step 2.2. Compute velocities and at the Gauss points: |
| Step 2.2.1. Set the number of iterations, threshold of iterations deviation and let current step be 1; |
| Step 2.2.2. Let initial value of both and be , that of both and be ; |
| Step 2.2.3. Child loop: |
| Step 2.2.3.1. According to the following equations, obtain , |
| |
| Step 2.2.3.2. Update velocities at the Gauss point, |
| |
| Step 2.2.3.3. Compute 4th-order reduced truncated Magnus series expansion at the Gauss points, |
|
| Step 2.2.3.4. Using the Cayley map, update configurations at the Gauss points, |
|
| Step 2.2.3.5. Compute the configuration deviation and velocity deviation respectively between the adjacent steps, |
| and let the larger one between them be iterative deviation at the current step, |
| , |
| Step 2.2.3.6. Substitute and at and into and respectively, at the same time, let plus 1; |
| Step 2.2.3.7. Compute the difference between iterative deviation and specified tolerance, |
| so that determine whether or not to terminate the iterative process. |
| Step 2.2.4. End child loop; |
| Step 2.2.5. Use the final iterative results and in child loop to compute the velocity at the endpoint , |
|
| Step 2.2.6. Compute configuration at : |
| Step 2.2.6.1. Compute 4th-order reduced truncated Magnus series expansion at , |
|
| Step 2.2.6.2. Use the Cayley map to update configuration , |
| |
| Step 2.2.7. Compare with , so that determine whether or not to terminate the main loop; |
| Step 3. End loop. |
| |
