double Vector3d::distance( Vector3d a, Vector3d b, Vector3d c ) {
// calculate 2 displacement vectors in plane
b.subtract( a );
c.subtract( a );
// calculate normal vector
Vector3d n = b.crossProduct( c );
Vector3d p = *this;
p.subtract( a );
return n.dotProduct( p ) / n.absoluteValue();
}
double RotJoint::getAngle() {
Vector3d a(0.7536, 0.4844, 0.3428);
Vector3d pa = parent->rotation.rotMatrix() * a;
Vector3d pc = child->rotation.rotMatrix() * a;
Vector3d d1 = parent->Transform() * dp;
Vector3d d2 = child->Transform() * dc;
d1.normalize();
d2.normalize();
pa = pa - d1 * d1.dotProduct(pa);
pc = pc - d2 * d2.dotProduct(pc);
pa.normalize();
pc.normalize();
return atan2(pa.dotProduct(pc), d1.dotProduct(pa.crossProduct(pc))) / 3.141592654 * 180;
}
// Handles a collision using conservation of linear momentum;
void Physics::collide(Physical* a, Physical* b){
Vector3d between = b->pos_ - a->pos_;
double impulse, ra, rb, j, mu = .2;
Vector3d tang_v, fric_dir, f_fric_max, dw_fric_max, dv_fric_max;
ra = a->intersection_distance();
rb = b->intersection_distance();
// They are intersecting and one is moving towards the other
if (between.length() < rb + ra &&
(between.dotProduct(a->vel_)>0 || between.dotProduct(b->vel_)<0)){
// Separate the discs from each other
double move_dist = between.length() - rb - ra;
a->pos_ += between * move_dist/2.0;
b->pos_ += -between * move_dist/2.0;
between.normalize();
// Project vectors onto vector connecting radii
Vector3d vb_norm = between * between.dotProduct(b->vel_);
Vector3d va_norm = between * between.dotProduct(a->vel_);
// Perfectly inelastic collison. Momentum is conserved.
double term1_1 = (a->m_-b->m_) / (a->m_ + b->m_);
double term1_2 = (2*b->m_) / (a->m_ + b->m_);
double term2_1 = (2*a->m_) / (a->m_ + b->m_);
double term2_2 = (b->m_-a->m_) / (a->m_ + b->m_);
// Handles motion normal to collision
a->vel_ = a->vel_ - va_norm + va_norm * term1_1 + vb_norm * term1_2;
b->vel_ = b->vel_ - vb_norm + va_norm * term2_1 + vb_norm * term2_2;
Vector3d na = -between, nb = between;
j = 2 / ( 1/a->m_ + 1/b->m_ + ra*ra/a->I_ + rb*rb/b->I_ );
// The impulse in the direction normal to the wall
impulse = a->vel_.dotProduct(na) * j * a->m_ ;
// The direction of the friction
tang_v = -(a->vel_ + na.crossProduct(a->ang_vel_)*ra - b->vel_ - nb.crossProduct(b->ang_vel_)*rb);
fric_dir = tang_v - na.projectOnto(tang_v);
// Will cause divide by zero. This is a head on collision
if (fric_dir.length()==0) return;
fric_dir.normalize();
// Amount of rotation and velocity change to add
f_fric_max = fric_dir * impulse * mu;
dw_fric_max = -na.crossProduct(f_fric_max) * ra / a->I_;
dv_fric_max = fric_dir * dw_fric_max.length() * ra;
// Limit friction
dv_fric_max = fric_dir * fmin(fabs(dv_fric_max.length()), fabs(a->vel_.dotProduct(fric_dir)));
a->ang_vel_ += dw_fric_max;
a->vel_ += dv_fric_max;
// The impulse in the direction normal to the wall
impulse = b->vel_.dotProduct(nb) * j * b->m_ ;
// The direction of the friction
fric_dir = -fric_dir;
// Amount of rotation and velocity change to add
f_fric_max = fric_dir * impulse * mu;
dw_fric_max = -nb.crossProduct(f_fric_max) * rb / b->I_;
dv_fric_max = fric_dir * dw_fric_max.length() * rb;
// Limit friction
dv_fric_max = fric_dir * fmin(fabs(dv_fric_max.length()), fabs(b->vel_.dotProduct(fric_dir)));
b->ang_vel_ += dw_fric_max;
b->vel_ += dv_fric_max;
}
}
