void QuadTree::CheckWalls( Plane pl0, Plane pl1, Plane pl2, Plane pl3, Ball* b , Spinner* s )
{
//planes 0 1 2 3
//is behind bools!
bool p0 = false;
bool p1 = false;
bool p2 = false;
bool p3 = false;
Vector3 p0diff,p1diff,p2diff,p3diff;
p0diff = pl0.ClosestPointToPoint(b->Position()) - b->Position();
if(p0diff.getLength() <= b->radius)
{
p0 = true;
}
p1diff = pl1.ClosestPointToPoint(b->Position()) - b->Position();
if(p1diff.getLength() <= b->radius)
{
p1 = true;
}
p2diff = pl2.ClosestPointToPoint(b->Position()) - b->Position();
if(p2diff.getLength() <= b->radius)
{
p2 = true;
}
p3diff = pl3.ClosestPointToPoint(b->Position()) - b->Position();
if(p3diff.getLength() <= b->radius)
{
p3 = true;
}
// Check for which plane(s) it HAS collided with
if (p0)
{
HitSpinnerWall(s, pl0, b);
}
if (p1)
{
HitSpinnerWall(s, pl1, b);
}
if (p2)
{
HitSpinnerWall(s, pl2, b);
}
if (p3)
{
HitSpinnerWall(s, pl3, b);
}
}
void HitSpinnerWall( Spinner* sp, Plane &plane, Ball* b )
{
Vector3 wall = plane.points[2] - plane.points[0];
Vector3 ballFromWall = b->Position() - plane.points[0];
float relativePosition = plane.normal * ballFromWall;
float relativeDirection = plane.normal * b->Velocity();
if ((relativePosition > 0 && relativeDirection < 0) || (relativePosition < 0 && relativeDirection > 0))
{
float theta, c, s;
theta = acos((wall * Vector3(1,0,0))/wall.getLength());
if (b->position.x<0)
{
theta *= -1;
}
c = cos(theta), s = sin(theta);
Vector3 position2 = Vector3();
Vector3 velocityRot = Vector3();
position2 = Vector3(c * ballFromWall.x + s * ballFromWall.y, c * ballFromWall.y - s * ballFromWall.x, 0);
velocityRot = Vector3( c * b->Velocity().x + s * b->Velocity().y, c * b->Velocity().y - s * b->Velocity().x, 0);
// Set the ball outside the wall, reverse velocity toward wall
Vector3 scaledNormal = b->radius * Vector3(0,-1,0);
position2.y = scaledNormal.y;
velocityRot.y *= -1;
// Rotate back
ballFromWall = Vector3(c * position2.x - s * position2.y,c * position2.y + s * position2.x,0);
b->Velocity(Vector3(c * velocityRot.x - s * velocityRot.y,c * velocityRot.y + s * velocityRot.x,0));
Point3 newPos = plane.points[0] + ballFromWall;
b->position = newPos;
//float bEnergy = .5 * b->Mass() * b->Velocity() * b->Velocity();
Vector3 momenteum = b->Mass() * b->Velocity();
Point3 closest = plane.ClosestPointToPoint(b->Position());
//Vector3 tempV = diff;
//tempV.normalize();
//Vector3 proj = (closest*tempV)*tempV;
//float temp = proj.getLength();
//// check if in the bounding box
//if(diff.getLength() > temp + b->radius)
//{
// continue;
//}
//// then it needs to go, "Hey!".
////find the force applied to the bar
// ft = dp/dt = m*dv/dt
////find perpendicular momentum
//Vector3 totalForce = bEnergy * b->Velocity();
//// t = r X F
//Vector3 torque = diff.cross(totalForce);
Vector3 radius = Point3(closest.x,closest.y,closest.z) - sp->Position();
Vector3 torque = radius.cross(momenteum);
Vector3 dL = .05 * torque;
//// find the rotation matrix and the inverse of the rotation matrix
Matrix33 myRotation = Matrix33(sp->Rotation());
Matrix33 myRotationT = myRotation.Transpose();
sp->MyL(sp->MyL() + dL);
////w(i+1) = q(i+1) * I(-1) * qT(i+1) * L(i+1)
sp->AngularVel(myRotation * *sp->Tensor()->inverse() * myRotationT * sp->MyL());
////q(n+1) = q(n) + h(.5 * w(n)*q(n))
sp->Rotation(sp->Rotation() + .05f*(.5 * Quaternion(sp->AngularVel()) * sp->Rotation()));
Quaternion q = sp->Rotation();
q.normalize();
sp->Rotation(q);
}
}
