void palLiquidDrag::Apply() {
palVector3 vout,V;
//linear drag
m_pBody->GetLinearVelocity(V);
Float drag = Float(0.5) * m_density * m_CD * m_area * vec_mag(&V) * vec_mag(&V);
vec_const_mul(&vout,&V,drag);
m_pBody->ApplyImpulse(-vout.x,-vout.y,-vout.z);
//angular drag
m_pBody->GetAngularVelocity(V);
drag = Float(0.5) * m_density * m_CD * m_area * vec_mag(&V) * vec_mag(&V);
vec_const_mul(&vout,&V,drag);
m_pBody->ApplyAngularImpulse(-vout.x,-vout.y,-vout.z);
}
/** Constant tide force, does not vary with distance. */
inline Vector
tide_force(const Vector& a, const Vector& b, double power)
{
static const double G = 0.0000000000667;
const Vector vec = vec_sub(a, b);
const double mag = vec_mag(vec);
return vec_mult(vec, G * power / mag);
}
/** Hooke's law */
inline Vector
spring_force(const Vector& a, const Vector& b, double length, double k)
{
const Vector vec = vec_sub(b, a);
const double mag = vec_mag(vec);
const double displacement = length - mag;
return vec_mult(vec, k * displacement * 0.5 / mag);
}
/** Repelling charge force, ala Coulomb's law. */
inline Vector
repel_force(const Region& a, const Region& b)
{
static const double MIN_DIST = 1.0;
Vector vec;
double dist = rect_distance(
&vec,
a.pos.x - (a.area.x / 2.0), a.pos.y - (a.area.y / 2.0),
a.pos.x + (a.area.x / 2.0), a.pos.y + (a.area.y / 2.0),
b.pos.x - (b.area.x / 2.0), b.pos.y - (b.area.y / 2.0),
b.pos.x + (b.area.x / 2.0), b.pos.y + (b.area.y / 2.0));
if (dist <= MIN_DIST) {
dist = MIN_DIST;
vec = vec_sub(a.pos, b.pos);
}
return vec_mult(vec, (CHARGE_KE * 0.5 / (vec_mag(vec) * dist * dist)));
}
void palSpring::Apply() {
palVector3 p1,p2;
m_pBody1->GetPosition(p1);
m_pBody2->GetPosition(p2);
palVector3 deltaP,deltaV;
Float dist;
vec_sub(&deltaP,&p1,&p2);
dist = vec_mag(&deltaP); //get the distance between the positions
palVector3 p1v,p2v;
Float HTerm,DTerm;
HTerm = (dist - mRestLen) * mKs; //calc: Ks * (dist - R )
m_pBody1->GetLinearVelocity(p1v);
m_pBody2->GetLinearVelocity(p2v);
//deltaV = p1->mVelocity - p2->mVelocity;
vec_sub(&deltaV,&p1v,&p2v); //get the velocitiy delta
DTerm = vec_dot(&deltaP,&deltaV) * mKd / dist;
//hey! check this code! is it right? -- kd term not correct?
palVector3 springForce;
vec_const_mul(&springForce,&deltaP,1.0f/dist);
//springForce = deltaP * 1.0f/dist;
//springForce = springForce * -(HTerm + DTerm);
vec_const_mul(&springForce,&springForce,-(HTerm + DTerm));
m_pBody1->ApplyImpulse(springForce.x,springForce.y,springForce.z);
m_pBody2->ApplyImpulse(-springForce.x,-springForce.y,-springForce.z);
/*
vec_sub(&springForce,&springForce,&last_force);
m_pBody1->AddForce(springForce.x,springForce.y,springForce.z);
m_pBody2->AddForce(-springForce.x,-springForce.y,-springForce.z);
//p1->mForce = p1->mForce + springForce;
//p2->mForce = p2->mForce - springForce;
last_force = springForce;
*/
}
static void do_camera_physics(vector *cam_pos, quat *cam_orient)
{
vector move;
quat qturn;
/* Rotation */
camera.turn.x = CAP(turn + mouseturn_x + turn_right-turn_left, 1.0f);
camera.turn.y = CAP(pitch + mouseturn_y + pitch_up-pitch_down, 1.0f);
camera.turn.z = CAP(roll + roll_left-roll_right, 1.0f);
cam_turn_vel.x += camera.turn.x * time_diff * turnaccel;
cam_turn_vel.y += camera.turn.y * time_diff * turnaccel;
cam_turn_vel.z += camera.turn.z * time_diff * turnaccel;
cam_turn_vel.x *= powf(turndrag, -time_diff);
cam_turn_vel.y *= powf(turndrag, -time_diff);
cam_turn_vel.z *= powf(turndrag, -time_diff);
quat_make_euler(&qturn, cam_turn_vel.x*time_diff, cam_turn_vel.y*time_diff*0.75f, cam_turn_vel.z*time_diff*0.75f);
quat_mul(cam_orient, cam_orient, &qturn);
quat_norm(cam_orient, cam_orient);
/* Movement */
camera.move.x = CAP(move_x + slide_right-slide_left, 1.0f);
camera.move.y = CAP(move_y + slide_up-slide_down, 1.0f);
camera.move.z = CAP(move_z + move_forward-move_backward, 1.0f);
move = camera.move;
if (vec_mag(&move))
vec_rotate(&move, cam_orient);
cam_vel.x += move.x * time_diff * accel;
cam_vel.y += move.y * time_diff * accel;
cam_vel.z += move.z * time_diff * accel;
cam_vel.x *= powf(drag, -time_diff);
cam_vel.y *= powf(drag, -time_diff);
cam_vel.z *= powf(drag, -time_diff);
cam_pos->x += cam_vel.x * time_diff;
cam_pos->y += cam_vel.y * time_diff;
cam_pos->z += cam_vel.z * time_diff;
}
Float palRevoluteLink::GetAngle() const {
if (m_pParent==NULL) return 0.0f;
if (m_pChild ==NULL) return 0.0f;
//palMatrix4x4 a_PAL,b_PAL;
palMatrix4x4 a,b;
a=m_pParent->GetLocationMatrix();
b=m_pChild->GetLocationMatrix();
palVector3 fac0;
mat_get_row(&m_frameA,&fac0,0);
palVector3 refAxis0;
vec_mat_mul(&refAxis0,&a,&fac0);
palVector3 fac1;
mat_get_row(&m_frameA,&fac1,1);
palVector3 refAxis1;
vec_mat_mul(&refAxis1,&a,&fac1);
palVector3 fbc1;
mat_get_row(&m_frameB,&fbc1,1);
palVector3 swingAxis;
vec_mat_mul(&swingAxis,&b,&fbc1);
Float d0 = vec_dot(&swingAxis,&refAxis0);
Float d1 = vec_dot(&swingAxis,&refAxis1);
return std::atan2(d0,d1);
#if 0 //this method does not do +/-, just positive :(
palVector3 pp,cp;
m_pParent->GetPosition(pp);
m_pChild->GetPosition(cp);
// printf("pp:");
// printvector(&pp);
// printf("cp:");
// printvector(&cp);
palVector3 linkpos;
linkpos.x=m_fRelativePosX;
linkpos.y=m_fRelativePosY;
linkpos.z=m_fRelativePosZ;
// printf("lp:");
// printvector(&linkpos);
palVector3 newlp;
vec_mat_mul(&newlp,&a,&linkpos);
vec_add(&newlp,&newlp,&pp);
// printf("nlp:");
// printvector(&newlp);
palVector3 la,lb;
vec_sub(&la,&pp,&newlp);
vec_sub(&lb,&cp,&newlp);
// la = pp;
// lb = cp;
vec_norm(&la);
vec_norm(&lb);
// printvector(&la);
// printvector(&lb);
Float dot=vec_dot(&la,&lb);
Float mag=vec_mag(&la)*vec_mag(&lb);
return Float(acos(dot/mag));
#endif
}
void
vec_norm(Vec *v)
{
vec_mul(v, 1.0f / vec_mag(v), v);
}
vec vec_norm(vec v)
{
double m = vec_mag(v);
return fabs(m)<0.0001?v:vec_scale(1.0/m,v);
}
