void Object::accelerate(Point3 velocity)
{
Point3 temp = this->velocity + velocity;
float speed1 = this->velocity.len();
float speed2 = velocity.len();
float newSpeed = (speed1 + speed2)/(1 + speed1 * speed2/(MAX_SPEED * MAX_SPEED));
this->velocity = temp.normalize() * newSpeed;
}
/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix3 D_p_point;
Point3 normalized = point.normalize(H ? &D_p_point : 0);
Unit3 direction;
direction.p_ = normalized.vector();
if (H)
*H << direction.basis().transpose() * D_p_point;
return direction;
}
Plane(Point3 _AA, Point3 _BB, Point3 _CC)
{
A=_AA;
B=_BB;
C=_CC;
Point3 normal = (B-A)*(C-A);
N = normal.normalize();
a = N.x;
b = N.y;
c = N.z;
d = -(A^N);
O = Point3(-d*a, -d*b, -d*c);
if(A!=O) X = (A-O).normalize();
else X = (B-O).normalize();
Y = normal*X;
}
