示例#1
0
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();
}
示例#2
0
void RotJoint::preStabilization(double h) {
    timeval t1, t2;
    gettimeofday(&t1, 0);
    Vector3d pPos = parent->rotation.rotMatrix() * this->pPos;
    Vector3d cPos = child->rotation.rotMatrix() * this->cPos;
    Vector3d j = solvej(h);
    Matrix3d J1 = Matrix3d::createScale(0, 0, 0);
    Matrix3d J2 = J1;
    if (!(parent->nailed)) {
        Matrix3d rot1 = parent->rotation.rotMatrix();
        J1 = rot1 * parent->J * rot1.transpose();
    }
    if (!(child->nailed)) {
        Matrix3d rot2 = child->rotation.rotMatrix();
        J2 = rot2 * child->J * rot2.transpose();
    }
    double term1 = parent->nailed ? 0 : 1 / parent->mass;
    double term2 = child->nailed ? 0 : 1 / child->mass;
    if (!violated()) {
        if (!(parent->nailed)) {
            parent->v += j * term1;
            parent->w += J1 * pPos.crossProduct(j);
        }
        if (!(child->nailed)) {
            child->v -= j * term2;
            child->w -= J2 * cPos.crossProduct(j);
        }
    } else {
        Vector3d jt = solvejt(h);
        if (!(parent->nailed)) {
            parent->v += j * term1;
            parent->w += J1 * jt;
        }
        if (!(child->nailed)) {
            child->v -= j * term2;
            child->w -= J2 * jt;
        }
    }
    gettimeofday(&t2, 0);
    g_jointTime += (t2.tv_sec - t1.tv_sec) + 1e-6 * (t2.tv_usec - t1.tv_usec);
}
示例#3
0
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;
}
示例#4
0
void Physics::check_in_bounds(){
  if (all_.size() > 0) {
      double mu = .2;
      std::list<Physical *>::iterator it = all_.begin();
      double impulse, r;
      Vector3d tang_v, fric_dir, f_fric_max, dw_fric_max, dv_fric_max;
      
      while (it != all_.end()) {
        Physical *p = (*it);
        r = p->intersection_distance();
        if (p->has_collisions()){
          // Wall Normal Vector    
          Vector3d n;
          bool collides = false;
          //Left Wall
          if (p->pos_.x - p->intersection_distance() < x_min_ && p->vel_.x < 0){
            p->pos_.x =  x_min_ + p->intersection_distance();
            p->vel_.x = -p->vel_.x;
            if(p->acc_.x < 0) p->acc_.x = 0;

            n = Vector3d(1, 0, 0); // Normal vector for wall
            collides = true;
          }

          //Right Wall
          else if (p->pos_.x + p->intersection_distance() > x_max_ && p->vel_.x > 0){
            p->pos_.x =  x_max_ - p->intersection_distance();
            p->vel_.x = -p->vel_.x;
            if(p->acc_.x > 0) p->acc_.x = 0;

            n = Vector3d(-1, 0, 0); // Normal vector for wall
            collides = true;
            
          }

          //Bottom Wall
          if (p->pos_.y - p->intersection_distance() < y_min_ && p->vel_.y < 0){
            p->pos_.y =  y_min_ + p->intersection_distance();
            p->vel_.y = -p->vel_.y;
            if(p->acc_.y < 0) p->acc_.y = 0;
            n = Vector3d(0, 1, 0); // Normal vector for wall
            collides = true;
          }

          //Top Wall
          else if (p->pos_.y + p->intersection_distance() > y_max_ && p->vel_.y > 0){
            p->pos_.y =  y_max_ - p->intersection_distance();
            p->vel_.y = -p->vel_.y;
            if(p->acc_.y > 0) p->acc_.y = 0;
            n = Vector3d(0, -1, 0); // Normal vector for wall
            collides = true;
          }
          
          if (collides && p->vel_.dotProduct(n) != p->vel_.length()){// Handles divide by zero cases
            // The impulse in the direction normal to the wall
            impulse = p->vel_.dotProduct(n) * 2 * p->m_ ;
            // The direction of the friction
            tang_v = -(p->vel_ + n.crossProduct(p->ang_vel_) * r);
            fric_dir = tang_v - n.projectOnto(tang_v);
            fric_dir.normalize();
            // Amount of rotation and velocity change to add
            f_fric_max = fric_dir * impulse * mu;
            dw_fric_max = -n.crossProduct(f_fric_max) * r / p->I_;
            dv_fric_max = fric_dir * dw_fric_max.length() * r;
            // Limit friction              
            dv_fric_max = fric_dir * fmin(fabs(dv_fric_max.length()), fabs(p->vel_.dotProduct(fric_dir)));
  
            p->ang_vel_ +=  dw_fric_max;
            p->vel_ += dv_fric_max; 
          }
        } 
        ++it;
      }
    }
}
示例#5
0
// 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; 
      
  }
}
示例#6
0
bool RotJoint::postStabilization() {
    timeval t1, t2;
    gettimeofday(&t1, 0);
    Vector3d wrel;
    Vector3d tAxis;
    bool is_violated = false;
    if (!(parent->nailed && child->nailed)) {
        is_violated = true;
        Vector3d d1 = parent->Transform() * dp;
        Vector3d d2 = child->Transform() * dc;
        d1.normalize();
        d2.normalize();
        tAxis = d1;
        wp = d1 * d1.dotProduct(parent->w);
        wc = d2 * d2.dotProduct(child->w);
        if (!violated()) {
            wrel = wp - wc + child->w - parent->w;
            is_violated = false;
        } else {
            wrel = (wp - wc) * -kr + (child->w - parent->w);
        }
    } else {
        gettimeofday(&t2, 0);
        g_jointTime += (t2.tv_sec - t1.tv_sec) + 1e-6 * (t2.tv_usec - t1.tv_usec);
        return false;
    }
    Vector3d pPos = parent->rotation.rotMatrix() * this->pPos;
    Vector3d cPos = child->rotation.rotMatrix() * this->cPos;
    Vector3d v1 = parent->v + parent->w.crossProduct(pPos);
    Vector3d v2 = child->v + child->w.crossProduct(cPos);
    Vector3d vrel = v2 - v1;
    if ((vrel.length() < 1e-4 && wrel.length() < 1e-4) || (parent->nailed && child->nailed)) {
        gettimeofday(&t2, 0);
        g_jointTime += (t2.tv_sec - t1.tv_sec) + 1e-6 * (t2.tv_usec - t1.tv_usec);
        return false;
    }
    double term1 = parent->nailed ? 0 : 1 / parent->mass;
    double term2 = child->nailed ? 0 : 1 / child->mass;
    Matrix3d rot1 = parent->rotation.rotMatrix();
    Matrix3d rot2 = child->rotation.rotMatrix();
    Matrix3d J1 = Matrix3d::createScale(0, 0, 0);
    Matrix3d J2 = J1;
    if (!(parent->nailed)) {
        J1 = rot1 * parent->J * rot1.transpose();
    }
    if (!(child->nailed)) {
        J2 = rot2 * child->J * rot2.transpose();
    }
    Matrix3d rp = Matrix3d::createCrossProductMatrix(pPos);
    Matrix3d rc = Matrix3d::createCrossProductMatrix(cPos);
    Matrix3d rpt = rp.transpose();
    Matrix3d rct = rc.transpose();
    Matrix3d vrel_A1 = Matrix3d() * (term1 + term2) + rpt * J1 * rp + rct * J2 * rc;
    Matrix3d vrel_A2 = rpt * J1 + rct * J2;
    Matrix3d wrel_A1 = J1 * rp + J2 * rc;
    Matrix3d wrel_A2 = J1 + J2;
    ARRAY<2, double> a(6,6);
    ARRAY<1, double> b(6);
    ARRAY<1, double> x(6);
    for (int i = 1; i <= 3; ++i) {
        b(i) = vrel[i-1];
        b(i + 3) = wrel[i-1];
        for (int j = 1; j <= 3; ++j) {
            a(i,j) = vrel_A1.at(j-1,i-1);
            a(i,j+3) = vrel_A2.at(j-1,i-1);
            a(i+3,j) = wrel_A1.at(j-1,i-1);
            a(i+3,j+3) = wrel_A2.at(j-1,i-1);
        }
    }
    Solver::LinearSolve(a, b, x);
    gettimeofday(&t2, 0);
    g_jointTime += (t2.tv_sec - t1.tv_sec) + 1e-6 * (t2.tv_usec - t1.tv_usec);
    Vector3d j(x(1), x(2), x(3));
    Vector3d jt(x(4), x(5), x(6));
    if (!is_violated) {
        Vector3d diff;
        Vector3d axis = wrel_A2.inverse() * tAxis;
        axis.normalize();
        while (true) {
            diff += axis * jt.dotProduct(axis);
            if (diff.length() < 1e-13)
                break;
            jt -= diff;
            diff = vrel_A1.inverse() * (vrel_A2 * diff);
            j += diff;
            diff = wrel_A2.inverse() * (wrel_A1 * diff);
        }
    }
    if (!(parent->nailed)) {
        parent->v += j * term1;
        parent->w += J1 * (pPos.crossProduct(j) + jt);
    }
    if (!(child->nailed)) {
        child->v -= j * term2;
        child->w -= J2 * (cPos.crossProduct(j) + jt);
    }
    return true;
}