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