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); }