preview

Vector Rk4 ( Const Vector

Decent Essays
Open Document

Vector RK4(const Vector &X, const Vector &Xdot, double t, double h, int resting, int stopped){ Vector K1(4), K2(4), K3(4), K4(4);

K1 = h * Xdot; K2 = h * dynamics(X + 0.5 * K1, t + 0.5 * h, resting, stopped); K3 = h * dynamics(X + 0.5 * K2, t + 0.5 * h, resting, stopped); K4 = h * dynamics(X + K3, t + h, resting, stopped);

return X + (K1 + 2 * K2 + 2 * K3 + K4) / 6.0;
}

Vector Euler(const Vector &X, const Vector &Xdot, double h){

return X + h * Xdot;
}

void XToxv(Vector3d &x, Vector3d &p, Vector3d &l, Quaternion &q, const Vector &X){ x.x = X[0]; x.y = X[1]; x.z = X[2]; p.x = X[3]; p.y = X[4]; p.x = X[5]; l.x = X[6]; l.y = X[7]; l.z = X[8]; q.w = X[9]; q.x = X[10]; q.y = X[11]; q.z = X[12];
} …show more content…

Given current state and time, return rate of change of state
//
Vector dynamics(const Vector &X, double t, int resting, int stopped){ Vector3d x, p, l; Quaternion q; Vector newXdot(13); Vector3d F; double fn, ft, vt; double friction;

// compute total spring and gravity force XToxv(x, p, l, q, X); F = pl.m * g - sp.k * ((x - x0).norm() - sp.l0) * (x - x0).normalize();

// if in resting contact compute the friction force and support force fn = F * pl.n; // force component normal to the surface if(resting && fn < 0){

if(stopped){ // static friction ft = F * pl.tangent; // force component tangent to the surface friction = -Min(Abs(pl.mus * fn), Abs(ft)) * Sign(ft); } else{ // dynamic friction vt = v * pl.tangent; // velocity component tangent to the surface friction = pl.mud * fn * Sign(vt); }

// add the friction force F = F + friction * pl.tangent;

// subtract support force normal to the resting surface F = F - fn * pl.n; }

// build the state rate

Get Access
Related
Get Access