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