28 lines
1.1 KiB
C
28 lines
1.1 KiB
C
#include "kernels.h"
|
|
#include <math.h>
|
|
|
|
double native_contact(double penetration, double velocity, double stiffness,
|
|
double damping, double pdis, int signed_force) {
|
|
if (penetration <= 0) return 0;
|
|
double fraction = pdis > 0 ? -expm1(-penetration / pdis) : 1;
|
|
double force = stiffness * penetration + fraction * damping * velocity;
|
|
return signed_force == 1 ? force : fmax(force, 0);
|
|
}
|
|
|
|
void native_stop_motion(double x, double v, double lower, double upper,
|
|
double *acceleration, double *velocity) {
|
|
double vt = 1e-12 * fmax(fabs(v), 1);
|
|
if ((x <= lower + 1e-12*fmax(fabs(lower),1) && v <= vt && *acceleration <= 0) ||
|
|
(x >= upper - 1e-12*fmax(fabs(upper),1) && v >= -vt && *acceleration >= 0)) {
|
|
*acceleration = 0;
|
|
*velocity = 0;
|
|
}
|
|
}
|
|
|
|
double native_limit_force(double penetration, double velocity, double stiffness,
|
|
double damping, double depth, int signed_force) {
|
|
if(penetration<=0) return 0;
|
|
double force=stiffness*penetration+(depth>0?fmin(penetration/depth,1):1)*damping*velocity;
|
|
return signed_force==1?force:fmax(force,0);
|
|
}
|