35 lines
1.5 KiB
C
35 lines
1.5 KiB
C
#include "kernels.h"
|
|
#include <math.h>
|
|
|
|
double native_dry_friction(double velocity, double static_share, double coulomb,
|
|
double stiction, double stribeck_velocity, int stribeck,
|
|
double mode) {
|
|
if (fabs(mode)<.5) return -static_share;
|
|
double resistance=coulomb;
|
|
if (stribeck) resistance+=(stiction-coulomb)*exp(-3*fabs(velocity)/stribeck_velocity);
|
|
return mode>0 ? -resistance : resistance;
|
|
}
|
|
|
|
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) {
|
|
return native_contact(penetration, velocity, stiffness, damping, depth, signed_force);
|
|
}
|