/**
* Compute and return the velocity impulsion
* given the contact point (in global frame)
* and collision normal direction
*/
inline Vector2D computeImpulsion
(const Vector2D& point, const Vector2D& dir) const
{
if (dir.norm() < 0.999 || dir.norm() > 1.001) {
throw std::logic_error(
"UnaryConstraint not normalized dir");
}
//Compute velocity of collisioning point
Vector2D centerPos = _system->evalPosition(*_body);
Vector2D centerVel = _system->evalPositionVel(*_body);
scalar centerAngleVel = _system->evalAngleVel(*_body);
Vector2D vel = centerVel
+ centerAngleVel*Vector2D::normal(point - centerPos);
//FIXME: between moving bodies?
//Build impulse
Vector2D impulse(0.0, 0.0);
//Declare normal and tangent unit vector
Vector2D n = dir;
Vector2D t = Vector2D::normal(dir);
//Tangent (friction) component
if (Constraint::isFriction()) {
impulse += -Vector2D::dot(vel, t)*t;
}
//Normal (response) component
scalar e = Constraint::getRestitutionCoef();
impulse += -(e+1.0)*Vector2D::dot(vel, n)*n;
return impulse;
}
Delaunay::Line::Line(const Point2D & p, const Point2D & q)
{
Vector2D t = q - p;
Real len = t.norm();
_a = t.y / len;
_b = - t.x / len;
_c = -(_a*p.x + _b*p.y);
}
double PathToTarget(Vector2D * targ)
{
bool isLeft = false;
double ang = AngleToTarget(targ);
if (ang > 0) isLeft = true; // else false
double theta;
if (isLeft)
{
theta = position->getZ() + PI / 2.0;
}
else
{
theta = position->getZ() - PI / 2.0;
}
Vector2D * P = new Vector2D(position->getX(), position->getY());
Vector2D * n = new Vector2D(r * cos(theta), r * sin(theta));
// R = targ - (P + n)
Vector2D * G = P->add(n);
Vector2D * R = targ->extract(G);
Vector2D * ni = n->inv();
double beta = R->angleTo(n->inv());
double beta1 = ni->anticlockAngle(R);
if (!isLeft) beta1 *= -1;
double e = acos(r / R->norm());
if (beta1 < 0)
beta = 2 * PI - beta;
/*
if (beta1 > 0 && !isLeft)
beta = 2 * PI - beta;
*/
double eps = beta - e;
double L = r * eps;
double Rn = R->norm();
double T = sqrt(Rn * Rn + r * r);
return L + T;
}
double Vector2D::scaledCross(const Vector2D &v) const
{
double norm = this->norm();
//if the vectors are too small, scale them to approximately 2
// this is done because the cross product between
if (norm < 1.0)
{
Vector2D v2(this);
v2.multiply(2.0/norm);
norm = v.norm();
if (norm < 1.0)
{
Vector2D v3(v);
v3.multiply(2.0/norm);
return v2.cross(v3);
}
return v2.cross(v);
}
norm = v.norm();
if (norm < 1.0)
{
Vector2D v3(v);
v3.multiply(2.0/norm);
return this->cross(v3);
}
return this->cross(v);
}
//Draw lines between all particles that are within a certain distance
void drawLines(FrameWriter *writer, std::vector<DrawablePolygonPtrType> p, const double distance, const double width) {
int nHere = _poly.size();
int nOther = p.size();
for (int i=0; i<nHere; i++) {
for(int j=0; j<nOther; j++) {
Vector2D r =_poly[i]->_pos-(p[j]->_pos);
//cout << r.norm() << " " << distance << std::endl;
double norm = r.norm();
if ((norm < distance) && (norm > 1e-5))
writer->line(_poly[i]->_pos.getX(), _poly[i]->_pos.getY(), p[j]->_pos.getX(), p[j]->_pos.getY(), width, 0,0,0);
}
}
};
void Cluster::predictRelativePosition(){
if(estimatedVelocity.x == 0.0f && estimatedVelocity.y == 0.0f) //virgin state
estimatedVelocity = velocity;
else{ // else estimate new velocity from acceleration...
if(velocity.normalizedDot(estimatedVelocity) <= -20.7){
acceleration.reset();
}
else{
// calculate new relative velocity
float angle = 0; //calc rotation angle to align velocity vector to x-axis
Vector2D xAxis(1,0);
if(velocity.norm() > 0)
angle = acos(velocity.normalizedDot(xAxis));
if(velocity.y > 0)
angle = 2*PI - angle;
Vector2D forward = estimatedVelocity.rotate(angle);
Vector2D target = velocity.rotate(angle);
float m = target.norm()-forward.norm();
//filter and set acceleration magnitude...
float a = 0;
if(target.norm() > 0)
a = 1-target.normalizedDot(xAxis);
if(target.y < 0)
a = -a;
//filter and set acceleration angle...
}
//estimate new position
float angle = 0;
Vector2D xAxis(1,0);
if(velocity.norm() > 0)
angle = acos(velocity.normalizedDot(xAxis));
if(velocity.y > 0)
angle = 2*PI - angle;
Vector2D forward = estimatedVelocity.rotate(angle);
float m = forward.norm() + acceleration.m * delta;
float a = acceleration.a * delta;
Vector2D newVelocity;
newVelocity.x = m * cos(abs(a));
int sgn = 0;
if(a != 0.0f)
sgn = a < 0 ? -1 : 1;
newVelocity.y = m * sin(abs(a)) * sgn;
newVelocity = newVelocity.rotate(-angle);
// filter - synthetic velocity
Vector2D measured;
Vector2D predicted;
if(measured.normalizedDot(predicted) < 0.5){
acceleration.reset();
estimatedVelocity = velocity;
}
}
// compute new forward velocity... => jaer code: SyntheticVelocityPredictor
}
