get control from cmd

The public getConvexHull functions will setup a VertexDataSource object and call this

Get ellipsoid

Get polyhedron

Find intersection between two lines on the same plane, return false if they are not intersected

* return the minimum on the projective line */

unitize a d+1 dimensional point */

y1 = -x^4 + 2*x0 * x^3 y2 = 2x0^3 x - x0^4 static double penF(const double& x, double& grad) { static double x0 = 0.1; static double x02 = x0 * x0; st

GaaiLam

Inputs are desired RPM for the motors Rotor numbering is: 1* Front 3 4 2 with 1 and 2 clockwise and 3 and 4 counter-clockwise (looking from top

set initial state

update state from sensors

Set dimension of bounding box

Set obstacle points

This function solves Ax=b, then stores x in b The input b is required to be N*m, i.e., m vectors to be solved.

This function solves ATx=b, then stores x in b The input b is required to be N*m, i.e., m vectors to be solved.

Runs the actual dynamics simulation with a time step of dt

Calculate rotation matrix from a vector (aligned with x-axis)