bool VertexPair::overlapsRect(const FloatRect& rect) const
{
bool boundsOverlap = (minX() < rect.maxX()) && (maxX() > rect.x()) && (minY() < rect.maxY()) && (maxY() > rect.y());
if (!boundsOverlap)
return false;
float leftSideValues[4] = {
leftSide(vertex1(), vertex2(), rect.minXMinYCorner()),
leftSide(vertex1(), vertex2(), rect.maxXMinYCorner()),
leftSide(vertex1(), vertex2(), rect.minXMaxYCorner()),
leftSide(vertex1(), vertex2(), rect.maxXMaxYCorner())
};
int currentLeftSideSign = 0;
for (unsigned i = 0; i < 4; ++i) {
if (!leftSideValues[i])
continue;
int leftSideSign = leftSideValues[i] > 0 ? 1 : -1;
if (!currentLeftSideSign)
currentLeftSideSign = leftSideSign;
else if (currentLeftSideSign != leftSideSign)
return true;
}
return false;
}
bool FloatPolygon::containsNonZero(const FloatPoint& point) const
{
int windingNumber = 0;
for (unsigned i = 0; i < numberOfEdges(); ++i) {
const FloatPoint& vertex1 = edgeAt(i).vertex1();
const FloatPoint& vertex2 = edgeAt(i).vertex2();
if (isPointOnLineSegment(vertex1, vertex2, point))
return true;
if (vertex2.y() < point.y()) {
if ((vertex1.y() > point.y()) && (leftSide(vertex1, vertex2, point) > 0))
++windingNumber;
} else if (vertex2.y() > point.y()) {
if ((vertex1.y() <= point.y()) && (leftSide(vertex1, vertex2, point) < 0))
--windingNumber;
}
}
return windingNumber;
}
// How to take this strategy
void sideimpactstrategy_action(sensors_t sens){
Serial.println("SIDE IMPACT!");
leftSide(100);
rightSide(100);
return;
}
// How to take this strategy
void centersit_action(sensors_t sens){
Serial.println("Center Sit!");
leftSide(0);
rightSide(0);
return;
}
static inline bool isReflexVertex(const FloatPoint& prevVertex, const FloatPoint& vertex, const FloatPoint& nextVertex)
{
return leftSide(prevVertex, nextVertex, vertex) < 0;
}
Intersection* Plane3D::testRayIntersection(Ray r)
{
/*std::cout << "ray start pos " << r.startPosition.x << " " << r.startPosition.y << " " << r.startPosition.z << std::endl;
std::cout << "ray direction " << r.direction.x << " " << r.direction.y << " " << r.direction.z << std::endl;*/
//The ray is described by Point on ray = start(O) + direction(D) * t
//If a point B on the ray hits a surface it must be inside the plane of that surface
//We can check if B is inside the surface by making a vector between a point on the surface(A) and B
//and dot multiply it with the plan's normal and see if it becomes zero
//(A-B)*n = 0
//Then the point B is on the plane
//We want to know at what t-value we get a point B on the ray that is inside the plane
//After some calculations this formula was found
//t = (O - A) dot n / D dot n
//translate the plane to it's local coordinate system rotate it and tranlate it back
glm::mat4 translation = glm::translate(glm::mat4(1.f), -position);
glm::mat4 translationBack = glm::translate(glm::mat4(1.f), position);
glm::mat4 rotat = glm::rotate(glm::rotate(glm::rotate(glm::mat4(1.f), -rotation.x, glm::vec3(1, 0, 0)), -rotation.y, glm::vec3(0, 1, 0)), -rotation.z, glm::vec3(0, 0, 1));
glm::mat4 toLocal = rotat * translation;
glm::vec4 temp(translationBack * toLocal * glm::vec4(position + glm::vec3(-dimensions.x / 2, -dimensions.y / 2, 0), 1));
glm::vec3 lowerLeftCorner(temp.x, temp.y, temp.z);
temp = glm::vec4(translationBack * toLocal * glm::vec4(position + glm::vec3(dimensions.x / 2, -dimensions.y / 2, 0), 1));
glm::vec3 lowerRightCorner(temp.x, temp.y, temp.z);
temp = glm::vec4(translationBack * toLocal * glm::vec4(position + glm::vec3(-dimensions.x / 2, dimensions.y / 2, 0), 1));
glm::vec3 upperLeftCorner(temp.x, temp.y, temp.z);
//Find plane normal
glm::vec3 planeNormal(glm::normalize(glm::cross(lowerRightCorner - lowerLeftCorner, upperLeftCorner - lowerLeftCorner)));
/*std::cout << "lower left " << lowerLeftCorner.x << " " << lowerLeftCorner.y << " " << lowerLeftCorner.z << std::endl;
std::cout << "lower right " << lowerRightCorner.x << " " << lowerRightCorner.y << " " << lowerRightCorner.z << std::endl;
std::cout << "upper left " << upperLeftCorner.x << " " << upperLeftCorner.y << " " << upperLeftCorner.z << std::endl;
std::cout << "ray start pos " << r.startPosition.x << " " << r.startPosition.y << " " << r.startPosition.z << std::endl;
std::cout << "ray direction " << r.direction.x << " " << r.direction.y << " " << r.direction.z << std::endl;*/
//If D dot n == 0 the direction goes along the plane and the ray won't hit the plane
//If D dot n < 0 the surface can't be seen from the ray's starting point
//But if the ray is a shadow ray it doesn't matter if we can't see the object, just that it is in the way
float DdotN = glm::dot(-r.direction, planeNormal);
if (r.shadowRay)
{
if (DdotN < EPSILON && DdotN > -EPSILON)
return nullptr;
}
else
if (DdotN < EPSILON){
//std::cout << "DdotN <= 0 " << std::endl;
return nullptr;
}
//std::cout << "ray direction " << r.direction.x << " " << r.direction.y << " " << r.direction.z << std::endl;
//std::cout << "planeNormal " << planeNormal.x << " " << planeNormal.y << " " << planeNormal.z << std::endl;
//Find point inside plane -> position, which is the center of the plane
//Find intersection
float t;
if (r.startPosition - position != planeNormal)
{
t = glm::dot(r.startPosition - position, planeNormal) / DdotN;
//std::cout << "1t = " << t << std::endl;
}
else
{
glm::vec3 temp = r.startPosition - lowerLeftCorner;
//std::cout << "vector on plane? " << temp.x << " " << temp.y << " " << temp.z << std::endl;
t = glm::dot(r.startPosition - lowerLeftCorner, planeNormal) / DdotN;
//std::cout << "2t = " << t << " dot " << glm::dot(r.startPosition - lowerLeftCorner, planeNormal) << std::endl;
}
//If t < 0 the plane is behind or inside the ray origin and we aren't interested in it
if (t < EPSILON){
//std::cout << "t <= 0 " << std::endl;
return nullptr;
}
//Is B inside the surface bounds?
//Create vectors that decribe the bounds
//"Inside" is on the right side of the vector
temp = glm::vec4(translationBack * toLocal * glm::vec4(position + glm::vec3(dimensions.x / 2, dimensions.y / 2, 0), 1));
glm::vec3 upperRightCorner(temp.x, temp.y, temp.z);
//std::cout << "upper right " << upperRightCorner.x << " " << upperRightCorner.y << " " << upperRightCorner.z << std::endl;
glm::vec3 leftSide(upperLeftCorner - lowerLeftCorner);
glm::vec3 topSide(upperRightCorner - upperLeftCorner);
glm::vec3 rightSide(lowerRightCorner - upperRightCorner);
glm::vec3 bottomSide(lowerLeftCorner - lowerRightCorner);
//To know if B is on the correct side of the vector we take the cross product of
//the bounding vector and a vector from the same starting point as the bounding vector and ends in B
//If the result is a vector along the plane's normal, the point B is on the correct side of the bounding vector
//If the result is a vector against the plane's normal the point is on the wrong side
//To know if the vector is going along the normal we take the dot product between them
//If it is larger than zero they go with each other
//leftSide
glm::vec3 B(r.startPosition + t * r.direction);
glm::vec3 Bvector(B - lowerLeftCorner);
if (glm::dot(glm::cross(Bvector, leftSide), planeNormal) > 0)
{
//topSide
Bvector = glm::vec3(B - upperLeftCorner);
if (glm::dot(glm::cross(Bvector, topSide), planeNormal) > 0)
{
//rightSide
Bvector = glm::vec3(B - upperRightCorner);
if (glm::dot(glm::cross(Bvector, rightSide), planeNormal) > 0)
{
//bottomSide
Bvector = glm::vec3(B - lowerRightCorner);
if (glm::dot(glm::cross(Bvector, bottomSide), planeNormal) > 0)
{
// If the object is blocked
if (t > r.tMax){
//std::cout << "t > r.tMax" << std::endl;
return nullptr;
}
//else
r.tMax = t;
glm::vec3 intersectionPoint(r.startPosition + t * r.direction);
//std::cout << "intersected object" << std::endl;
return new Intersection(intersectionPoint, planeNormal, color, reflectionCoef, t);
}
}
}
}
//std::cout << "not inside bounds" << std::endl;
return nullptr;
}
void FPSTask(void* ignore)
{
/** x coordinate of the center of the goal */
int netx=72;
/** y coordinate of the center of the goal */
int nety=72;
/** checks what side we are starting on */
bool lside=leftSide();
/** sets the starting position of the robot*/
position=(Location){0,0};//determineXInit(lside), determineYInit()};
/** sets the starting position of the net*/
Location net=netPos(lside, netx, nety);
/** variable that increments whenever we move a linear distance */
int movement=0;
/** variable that keeps track of what the encoder value was a certain number of iterations prior */
int previousEncoder=0;
/** variable that sets the minimum amount the encoder value needs to change for us to count it as a definite movement */
int thresEncoder=5;
/** variable that sets the minimum amount the gyroscope value needs to change for us to count it as a definite turn */
int thresGyro=5;
/** variable that keeps track of what the gyroscope value was a certain number of iterations prior */
int previousGyro=0;
/** variable that keeps track of how many times the main code has looped */
int iteration=0;
/** variable for the angle between the robot and the net */
int angle=0;
/** variable that sets the number of iterations we let pass before checking for movement */
int refreshrate=10;
while(1)
{
//printf("In FPSTask\n\r");
//FPSBase.correction.localAngle = getLocalAngle(gyroscope);
/** increments the iteration variable */
iteration++;
/** checks if a certain number of iterations has passed to check for a change */
if (iteration%refreshrate==0){
printf("FPS 1\n\r");
printf("1[chX:%4d, chY:%4d]\n\r", FPSBase.direction.chX, FPSBase.direction.chY);
//printf("axisTurn: %d\n\r", (int)axisTurn);
/** checks if we have not turned and/or are not turning since the last check for movement */
//if (abs(baseAngle(FPSBase)-previousGyro)<=thresGyro){
if (FPSBase.axis != axisTurn){
/** checks if we have moved a linear distance since the last check for movement */
//if (abs(encoderValue()-previousEncoder)>thresEncoder){
//if (FPSBase.axis != none){
if (true){
/** increments the movement value by the difference between the current encoder value and the encoder value from the previous check for movement */
movement+=encoderValue()-previousEncoder;
/** changes the x coordinate of the robot by the movement in the x direction */
position.x+=disMoved(movement)*cos(baseAngle(FPSBase));
/** changes the y coordinate of the robot by the movement in the y direction */
position.y+=disMoved(movement)*sin(baseAngle(FPSBase));
/** sets the current value of the encoder to the previous encoder value*/
previousEncoder=encoderValue();
printf("encoders:[left:%6d, right:%6d]\n\r", encoderGet(encoderBaseLeft), encoderGet(encoderBaseRight));
printf("2[chX:%4d, chY:%4d]\n\r", FPSBase.direction.chX, FPSBase.direction.chY);
printf("movement:%6d, X:%4d, Y:%4d\n\r", movement, position.x, position.y);
}
}
else{
/** sets the change in linear movement to 9 */
movement=0;
previousEncoder=encoderValue();
}
/** sets the current value of the gyroscope to the previous gyroscope value*/
previousGyro=baseAngle(FPSBase);
}
/** sets the angle variable to the angle between the current position of the robot and the net */
angle=shooterAngle_Xaxis(position, distance(position, net), net);
/** checks if the 7L or 7R buttons on the controller have been pressed; one or the other, not both */
if ((joystickGetDigital(1,7,JOY_LEFT)) ^ (joystickGetDigital(1,7,JOY_RIGHT))){
/** if the 7L button has been pressed, the position is reset by the ultrasonic sensors (left and back) to minimize error */
if ((joystickGetDigital(1,7,JOY_LEFT))){
position.x=getSonarLeft();
position.y=getSonarBack();
}
/** if the 7R button has been pressed, the position is reset by the ultrasonic sensors (right and back) to minimize error */
else{
position.x=getSonarRight();
position.y=getSonarBack();
}
}
printf("\n\r");
wait(TIME_DELAY);
}
}
void linefollow_actionTurnLeft(sensors_t sens){
Serial.println("LF left!");
leftSide(-TURN_SPEED_X);
rightSide(TURN_SPEED_X);
}