void findTangPoint(float result[3], float point[3],float targ[3], float r) {
float DNorms[3];
float D = mathVecMagnitude(point,3);
mathVecNormalize(point,3);
mathVecNormalize(targ,3);
mathVecSubtract(DNorms,targ,point,3);
mathVecNormalize(DNorms,3);
float alpha = acosf(r/D);
float beta = angleBetween(point,DNorms);
float theta = beta - alpha;
float dist = sinf(alpha)/sinf(theta);
DNorms[0] = DNorms[0]*dist;
DNorms[1] = DNorms[1]*dist;
DNorms[2] = DNorms[2]*dist;
mathVecAdd(result,point,DNorms,3);
mathVecNormalize(result,3);
result[0] = result[0] * r;
result[1] = result[1] * r;
result[2] = result[2] * r;
}
// return A halfway rotated around center in direction of B
Vector2d angle_bipartition(const Vector2d ¢er, const Vector2d &A, const Vector2d &B)
{
double angle = angleBetween(center-A, B-center) / 2;
return rotated(A, center, angle);
}
int main(int argc, char **argv)
{
double xpoints[3];
double ypoints[3];
double s1, s2, s3, a1, a2, a3;
bool scal, iso, equi, acute, obtuse, right;
bool collinear;
// get command line arguments
if(argc < 7)
{
printf("invalid number of coordinates\n");
}
else
{
xpoints[0] = atoi(argv[1]);
ypoints[0] = atoi(argv[2]);
xpoints[1] = atoi(argv[3]);
ypoints[1] = atoi(argv[4]);
xpoints[2] = atoi(argv[5]);
ypoints[2] = atoi(argv[6]);
}
// check for nat
if(isEqual(xpoints[0], xpoints[1]) && isEqual(ypoints[0], ypoints[1]))
{
printf("not a triangle\n");
return 0;
}
if(isEqual(xpoints[0], xpoints[2]) && isEqual(ypoints[0], ypoints[2]))
{
printf("not a triangle\n");
return 0;
}
if(isEqual(xpoints[2], xpoints[1]) && isEqual(ypoints[2], ypoints[1]))
{
printf("not a triangle\n");
return 0;
}
// check for nat: all 3 points lie on the same line
collinear = sameSlope(xpoints[0], ypoints[0], xpoints[1], ypoints[1], xpoints[2], ypoints[2]);
if(collinear)
{
printf("not a triangle\n");
return 0;
}
//scal, iso, equi, acute, obtuse, right
s1 = lengthOfSide(xpoints[0], ypoints[0], xpoints[1], ypoints[1]);
s2 = lengthOfSide(xpoints[2], ypoints[2], xpoints[1], ypoints[1]);
s3 = lengthOfSide(xpoints[2], ypoints[2], xpoints[0], ypoints[0]);
scal = isScalene(s1, s2, s3);
iso = isIsosceles(s1, s2, s3);
equi = isEquilateral(s1, s2, s3);
a3 = angleBetween(s1, s2, s3);
a2 = angleBetween(s1, s3, s2);
a1 = angleBetween(s2, s3, s1);
//right = isRight(a1, a2, a3);
right = isRightPy(s1, s2, s3);
acute = isAcute(a1, a2, a3);
obtuse = isObtuse(a1, a2, a3);
// (((scalene|isosceles|equilateral) (acute|obtuse|right))|not a triangle)
if(scal && right)
{
printf("scalene right\n");
}
else if(scal && obtuse)
{
printf("scalene obtuse\n");
}
else if(scal && acute)
{
printf("scalene acute\n");
}
else if(iso && right)
{
printf("isosceles right\n");
}
else if(iso && obtuse)
{
printf("isosceles obtuse\n");
}
else if(iso && acute)
{
printf("isosceles acute\n");
}
else
{
printf("not a triangle\n");
}
return 0;
}
void GameScene::update()
{
ColeScene::update();
p->update();
std::string result = p->clientNetworkUpdate();
while (result != "")
{
if(result == "ERROR") std::cout << "TODO!!!" << std::endl;
if (result != "")
{
result = result.substr(0, result.length()-1); // Remove the ;
std::string command = result.substr(0, result.find("§"));
std::vector<std::string> arguments;
std::string argumentsString = result.substr(result.find("§")+2);
while (argumentsString.length()>0)
{
arguments.push_back(argumentsString.substr(0, argumentsString.find("§")));
argumentsString = argumentsString.substr(argumentsString.find("§")+2); //it appears that § takes up 2 characters, so +2
}
if (command=="join" && arguments[0] != p->name)
{
std::cout << "New player joining" << std::endl;
std::shared_ptr<OtherPlayer> newPlayer = std::make_shared<OtherPlayer>(arguments[0], strToInt(arguments[3]));
newPlayer->x = strToInt(arguments[1]);
newPlayer->y = strToInt(arguments[2]);
otherPlayers.push_back(newPlayer);
w->addChild(newPlayer);
}
if (command == "move" && getOtherPlayerByName(arguments[0]) != NULL)
{
std::shared_ptr<OtherPlayer> thePlayer = getOtherPlayerByName(arguments[0]);
thePlayer->x = strToInt(arguments[1]);
thePlayer->y = strToInt(arguments[2]);
}
if (command == "leave" && getOtherPlayerByName(arguments[0]) != NULL)
{
std::shared_ptr<OtherPlayer> thePlayer = getOtherPlayerByName(arguments[0]);
otherPlayers.remove(thePlayer);
w->removeChild(thePlayer);
}
if (command == "world")
{
int worldSize = strToInt(arguments[0]);
int tilex = 0;
int tiley = 0;
for (int i=1; i<arguments.size(); i++) {
w->setTile(tilex, tiley, strToInt(arguments[i]));
tiley++;
if (tiley == worldSize)
{
tiley = 0;
tilex++;
}
}
}
if (command == "newbullet" && arguments[0] != p->name)
{
std::shared_ptr<BulletClient> b = std::make_shared<BulletClient>();
//s.broadcast("newbullet§"+p->name+"§"+doubleToStr(b->x)+"§"+doubleToStr(b->y)+"§"+doubleToStr(b->angle)+"§"+doubleToStr(b->speed)+"§"+doubleToStr(b->ttl)+"§"+doubleToStr(b->bulletType)+";");
b->owner = arguments[0];
b->setPos(strToDouble(arguments[1]), strToDouble(arguments[2]));
b->angle = strToDouble(arguments[3]);
b->speed = strToDouble(arguments[4]);
b->ttl = strToDouble(arguments[5]);
b->bulletType = strToInt(arguments[6]);
b->tag = 1;
w->addChild(b);
}
}
result = p->clientNetworkUpdate();
}
int futurex = 0, futurey = 0;
const Uint8 *state = SDL_GetKeyboardState( NULL );
if (state[SDL_SCANCODE_W]) futurey--;
if (state[SDL_SCANCODE_S]) futurey++;
if (state[SDL_SCANCODE_A]) futurex--;
if (state[SDL_SCANCODE_D]) futurex++;
if (futurex != 0 || futurey != 0)
p->move(futurex, futurey);
int mousex, mousey;
Uint32 mouseState = SDL_GetMouseState(&mousex, &mousey);
if (mouseState&SDL_BUTTON(1))
{
int altMouseX = mousex+p->x-(RenderManager::getInstance()->getScreenX()/2);
int altMouseY = mousey+p->y-(RenderManager::getInstance()->getScreenY()/2);
if (p->shootTowards(altMouseX, altMouseY))
{
std::shared_ptr<BulletClient> b = std::make_shared<BulletClient>();
b->setPos(p->x, p->y);
b->speed = 5;
b->ttl = 100;
b->angle = angleBetween(p->x, p->y, altMouseX, altMouseY);
b->owner = p->name;
b->tag = 1;
w->addChild(b);
}
}
}
double PLine::calcangle(const PLine rhs) const
{
return angleBetween((rhs.to-rhs.from), (to-from));
}
double PLine::calcangle() const
{
return angleBetween(from, to);
}
//=====================================================
///Execute the skill. This is the main part of the skill, where you tell the
///robot how to perform the skill.
void CutGoalSkill::execute()
{
///If not active, dont do anything!
if(!initialized)
{
return;
}
else
{
//grab the ball location
if(!presetBall){
ball = strategy->getCurrentRoboCupFrame()->getDefensiveBallLocation();
}
///Calculate the angle bisector of the area we want to cover
float theta;
float theta1=angleBetween(sp->field.OUR_GOAL_LINE,point1,ball.getX(),ball.getY());
float theta2=angleBetween(sp->field.OUR_GOAL_LINE,point2,ball.getX(),ball.getY());
theta=(theta1+theta2)/2.0f;
theta=normalizeAngle(theta+PI);
float halfAngle=ABS(angleDifference(theta1,theta2)/2.0f);
//calculate midpoint to extend from
Pair midpoint;
midpoint.setY((sp->field.OUR_GOAL_LINE-ball.getX()) * TAN(theta) + ball.getY());
midpoint.setX(sp->field.OUR_GOAL_LINE);
/*debugging helpful stuff
GUI_Record.debuggingInfo.setDebugPoint(robotID,6,midpoint);
GUI_Record.debuggingInfo.setDebugPoint(robotID,7,sp->field.OUR_GOAL_LINE,point1);
GUI_Record.debuggingInfo.setDebugPoint(robotID,8,sp->field.OUR_GOAL_LINE,point2);
Pair ang1(ball.getX()+.2f,ball.getY());
Pair ang2(ball.getX()+.2f,ball.getY());
rotateAboutPoint(ang1,ball,theta1,ang1);
rotateAboutPoint(ang2,ball,theta2,ang2);
GUI_Record.debuggingInfo.setDebugPoint(robotID,3,ang1);
GUI_Record.debuggingInfo.setDebugPoint(robotID,4,ang2);
Pair t(ball.getX()+.2f,ball.getY());
rotateAboutPoint(t,ball,theta,t);
GUI_Record.debuggingInfo.setDebugPoint(robotID,5,t);
*/
// The ideal point we want the robot to be in this circumstances
Pair dest;
float distance = sp->general.PLAYER_RADIUS / SIN(ABS(halfAngle)) ;
//char msg[80]; sprintf(msg,"dist: %5.2f",distance);GUI_Record.debuggingInfo.addDebugMessage(msg);
extendPoint(midpoint,ball,-distance,dest);
// We have to check if the destination point is between the Upper and lower limit
// float slope = (midpoint.getY() - ball.getY()) / (midpoint.getX() - ball.getY()) ;
// If it is above the limit
if(dest.getX() > UPPER_X){
extrapolateForY(midpoint,ball,UPPER_X,dest);
}
// If it is below the limit
if(dest.getX() < LOWER_X){
extrapolateForY(midpoint,ball,LOWER_X,dest);
}
command->setControl(OMNI_NORMAL);
command->setPos(dest);
command->setRotation(angleBetween(getLocation(robotID, *currentVisionData, *sp),
ball));
strategy->getCurrentFrame()->setMessage(robotID,"Covering Goal");
}
}
//===============================================================================
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void SupplementThreeManSkill::execute()
{
///If not initialized, dont do anything!
if(!initialized)
{
return;
}
//if the ball is close enough to defenisve players, move next to
//the defensive player that is closer to the ball
//since he'll become the aggresor and move upfield.
//this way we can slide in and form the defensive wall immediately.
Pair ballLoc = getBallLocation(*currentVisionData);
Pair robotLoc = getLocation(robotID, *currentVisionData, *sp);
RobotIndex closeDefensiveID = NO_ROBOT;
float closestDistance;
RobotIndex tempID;
float tempDistance;
tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER);
if(tempID != NO_ROBOT)
{
closestDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
closeDefensiveID = tempID;
}
tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER);
if(tempID != NO_ROBOT)
{
tempDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
if(tempDistance < closestDistance)
{
closestDistance = tempDistance;
closeDefensiveID = tempID;
}
if(closeDefensiveID == strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER))
closeDefensiveID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER);
}
tempID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(SPECIAL_OP_DEFENDER);
if(tempID != NO_ROBOT)
{
tempDistance = getLocation(tempID, *currentVisionData, *sp).distanceTo(ballLoc);
if(tempDistance < closestDistance)
{
closestDistance = tempDistance;
closeDefensiveID = tempID;
}
if(closeDefensiveID == strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER))
closeDefensiveID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(SPECIAL_OP_DEFENDER);
}
//if closest defensive player
//a.) exists
//b.) within tolerance, then go to side of him
if(closestDistance < 3.0f*sp->general.PLAYER_RADIUS)
{
Pair defensivePlayer = getLocation(closeDefensiveID, *currentVisionData, *sp);
if(defensivePlayer.getY() > sp->field.SPLIT_LINE)
{
command->setYPos(defensivePlayer.getY() + sp->general.PLAYER_RADIUS + 0.02f);
}
else
{
command->setYPos(defensivePlayer.getY() - sp->general.PLAYER_RADIUS - 0.02f);
}
command->setXPos(defensivePlayer.getX());
command->setRotation(angleBetween(robotLoc, ballLoc));
}
//else acquire the ball
else
{
Skill* skillHandle = skillSet->getSkill(AcquirePossessionSkill::skillNum);
if(!skillHandle->isInitialized())
skillHandle->initialize();
skillHandle->run();
}
}
double Poly::angleAtVertex(uint i) const
{
return angleBetween(getVertexCircular(i)-getVertexCircular(i-1),
getVertexCircular(i+1)-getVertexCircular(i));
}
TrackGenerator::TrackGenerator() {
/*
* Phase 1 -
* Generate the track tree and place control points that wrap
* closely around the tree segments.
*/
// Start by generating the tree
TrackTreeNode* rootTrackTreeNode = new TrackTreeNode(0.0, 0.0, NULL, 0);
// Place the control points
TrackTreeNode* current_start = rootTrackTreeNode;
TrackTreeNode* previous_start = rootTrackTreeNode->neighbors[0];
TrackTreeNode* current = current_start;
TrackTreeNode* previous = previous_start;
vector<vec3> controlPts;
do {
TrackTreeNode* next = current->getNextNode(previous);
if (previous == next) { // happens if current node only has one segment
vec2 direction = current->xz - previous->xz;
direction.normalize();
direction *= current->radius;
vec2 offset(-direction[1], direction[0]);
vec2 temp = current->xz + direction + offset;
vec3 v(temp[0], RAND * 20 + 10, temp[1]);
controlPts.push_back(v);
temp = current->xz + direction - offset;
v = vec3(temp[0], RAND * 20 + 10, temp[1]);
controlPts.push_back(v);
}
else { // Otherwise, do the bisector thing
vec2 direction1 = current->xz - previous->xz;
direction1.normalize();
direction1 = vec2(-direction1[1], direction1[0]);
vec2 direction2 = next->xz - current->xz;
direction2.normalize();
direction2 = vec2(-direction2[1], direction2[0]);
vec2 newDir = direction1 + direction2;
double angle = acos(direction1 * direction2);
newDir.normalize();
newDir *= current->radius / cos(angle / 2);
newDir += current->xz;
// First, randomly assign heights
vec3 v(newDir[0], RAND * 20 + 10, newDir[1]);
controlPts.push_back(v);
}
// Advance
previous = current;
current = next;
} while (current != current_start || previous != previous_start);
bool majorChange = true;
while (majorChange) {
majorChange = false;
/*
* Phase 2 -
* Merge nearby control points and prune control points with angles
* that are too acute. Repeat until the track stops changing.
*/
bool changed = true;
while (changed) {
changed = false;
vector<vec3> tempControlPts;
// Merge close control points first
double threshold = 20.0;
int size = controlPts.size();
for (int i = 0; i < size; i++) {
vec3 current = controlPts[i];
vec3 previous = controlPts[(i + size - 1) % size];
vec3 next = controlPts[(i + 1) % size];
vec3 distvec1 = current - previous;
vec3 distvec2 = next - current;
distvec1[1] = 0.0;
distvec2[1] = 0.0;
double dist1 = distvec1.length();
double dist2 = distvec2.length();
if (dist1 > threshold && dist2 > threshold) {
tempControlPts.push_back(current);
}
else if (dist2 <= threshold) {
changed = true;
vec3 newPoint = (current + next) / 2;
tempControlPts.push_back(newPoint);
}
majorChange |= changed;
}
controlPts = tempControlPts;
// Next, prune sharply angled control points
tempControlPts.clear();
size = controlPts.size();
for (int i = 0; i < size; i++) {
vec3 current = controlPts[i];
vec3 previous = controlPts[(i + size - 1) % size];
vec3 next = controlPts[(i + 1) % size];
vec3 dir1 = current - previous;
vec3 dir2 = current - next;
dir1.normalize();
dir2.normalize();
if (acos(dir1 * dir2) < M_PI / 4) {
changed = true;
}
else {
tempControlPts.push_back(current);
}
majorChange |= changed;
}
controlPts = tempControlPts;
}
/*
* Phase 3 -
* Add height information and then adjust to make sure that
* the track will not overlap too closely.
*/
// TODO - compare segment distances to make sure that they aren't overalapping too closely
changed = true;
while (changed) {
changed = false;
for (unsigned int i = 0; i < controlPts.size() - 1; i++) {
for (unsigned int j = i + 1; j < controlPts.size(); j++) {
unsigned int jn = (j+1)%controlPts.size();
pair<vec3, vec3> seg1(controlPts[i], controlPts[i+1]);
pair<vec3, vec3> seg2(controlPts[j], controlPts[jn]);
vec3 check = checkSegments(seg1, seg2);
if (check.length() > 0.01) {
changed = true;
controlPts[i] += check / 2;
controlPts[i+1] += check / 2;
controlPts[j] -= check / 2;
controlPts[jn] -= check / 2;
controlPts[i][1] = max(controlPts[i][1], 4.0);
controlPts[i+1][1] = max(controlPts[i+1][1], 4.0);
controlPts[j][1] = max(controlPts[j][1], 4.0);
controlPts[jn][1] = max(controlPts[jn][1], 4.0);
}
}
}
majorChange |= changed;
}
}
/*
* Recenter the track
*/
vector<vec3>::iterator it = controlPts.begin();
double xMin = (*it)[0];
double xMax = (*it)[0];
double zMin = (*it)[2];
double zMax = (*it)[2];
it++;
while (it != controlPts.end()) {
xMin = min(xMin, (*it)[0]);
xMax = max(xMax, (*it)[0]);
zMin = min(zMin, (*it)[2]);
zMax = max(zMax, (*it)[2]);
it++;
}
double xMid = (xMax + xMin) / 2;
double zMid = (zMax + zMin) / 2;
for (unsigned int i = 0; i < controlPts.size(); i++) {
controlPts[i][0] -= xMid;
controlPts[i][2] -= zMid;
}
xWidth = xMax - xMin;
zWidth = zMax - zMin;
/*
* Add banking to make better turns, and add to pathPts
*/
for (unsigned int i = 0; i < controlPts.size(); i++) {
vec2 current = vec2(controlPts[i], VY);
vec2 prev = vec2(controlPts[(i - 1 + controlPts.size()) % controlPts.size()], VY);
vec2 next = vec2(controlPts[(i + 1) % controlPts.size()], VY);
double angle = angleBetween(current, prev, next);
vec2 dir = current - prev;
vec2 turn = next - current;
vec3 dir2(dir[0], 0, dir[1]);
vec3 turn2(turn[0], 0, turn[1]);
double bankAmount = 45 * pow(0.999, angle * dir.length() * turn.length() / 100);
if ((dir2 ^ turn2)[1] > 0) {
bankAmount *= -1;
}
pathPts.push_back(PathPoint(controlPts[i], bankAmount));
}
}
//===============================================================================
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void RotateAroundBallSkill::execute()
{
///If not active, dont do anything!
if(!initialized) return;
command->setDribble(SLOW_DRIBBLE);
command->setVerticalDribble(SLOW_V_DRIBBLE);
if(USE_BALL_FOR_PIVOT)
{
pivotPoint.set(strategy->getCurrentRoboCupFrame()->getOffensiveBallLocation());
}
else
{
pivotPoint.set(frontOfRobot(robotID, *currentVisionData, *sp));
}
ultimateAngle = angleBetween(pivotPoint,ultimateDestination);
//get robots current location and angle
robotLoc.set(getLocation(robotID,*currentVisionData,*sp));
robotAngle = angleBetween(robotLoc, pivotPoint);
//if finished, then keep position and rotation
if(isFinished())
{
command->setPos(robotLoc);
command->setRotation(angleBetween(robotLoc, ultimateDestination));
strategy->getCurrentFrame()->setMessage(robotID, "rotate done");
return;
}
strategy->getCurrentFrame()->setMessage(robotID, "rotate rotating");
//determine next angle robot should be at
nextAngle = ultimateAngle;
//determine angle diff
angleDiff = angleDifference(ultimateAngle, robotAngle);
if( angleDiff > ROTATION_STEP )
{
nextAngle = robotAngle + ROTATION_STEP;
angleDiff = ROTATION_STEP;
}
else if (angleDiff < -ROTATION_STEP)
{
nextAngle = robotAngle - ROTATION_STEP;
}
if(ABS(angleDiff) < FINISHED_ROTATION_THRESHOLD)
{
finished = true;
}
Pair currLoc;
extendPoint(robotLoc,
pivotPoint,
-(sp->general.PLAYER_RADIUS - PUSH_FACTOR),
currLoc);
rotateAboutPoint(currLoc,
pivotPoint,
nextAngle - robotAngle,
rotatedDestination);
destRot = angleBetween(rotatedDestination, pivotPoint);
command->setPos(rotatedDestination);
command->setRotation(destRot);
GUI_Record.debuggingInfo.setDebugPoint(robotID,1,ultimateDestination);
GUI_Record.debuggingInfo.setDebugPoint(robotID,2,robotLoc);
GUI_Record.debuggingInfo.setDebugPoint(robotID,3,rotatedDestination);
}
//==============================================
void PenaltyShotUs::executePlay(VisionData* vision, RobocupStrategyData* rsd)
{
//==============================================
BALL_X = rsd->getSystemParams().field.THEIR_GOAL_LINE -
rsd->getSystemParams().field.PENALTY_KICK_X_OFFSET;
BALL_Y = rsd->getSystemParams().field.SPLIT_LINE;
//==============================================
//BLOCKER
index = rsd->getRobotByPosition(BLOCKER);
if (index != NO_ROBOT)
{
rsd->setMessage(index, "Penalty Shot Us Formation");
rsd->getDestination(index)->setPos(rsd->getSystemParams().field.HALF_LINE + BLOCKER_X,
rsd->getSystemParams().field.SPLIT_LINE + BLOCKER_Y);
rsd->getDestination(index)->setRotation(angleBetween
(getLocation(index, *vision, rsd->getSystemParams()),
Pair(BALL_X,
BALL_Y)));
rsd->getDestination(index)->setControl(OMNI_NORMAL_ENTERBOX);
rsd->getDestination(index)->setSpeed(FORMATION_SPEED);
}
//==============================================
//DEFENDER
index = rsd->getRobotByPosition(DEFENDER);
if (index != NO_ROBOT)
{
rsd->setMessage(index, "Penalty Shot Us Formation");
rsd->getDestination(index)->setPos(rsd->getSystemParams().field.HALF_LINE + DEFENDER_X,
rsd->getSystemParams().field.SPLIT_LINE + DEFENDER_Y);
rsd->getDestination(index)->setRotation(angleBetween
(getLocation(index, *vision, rsd->getSystemParams()),
Pair(BALL_X,
BALL_Y)));
rsd->getDestination(index)->setControl(OMNI_NORMAL_ENTERBOX);
rsd->getDestination(index)->setSpeed(FORMATION_SPEED);
}
//==============================================
//AGGRESSOR
index = rsd->getRobotByPosition(AGGRESSOR);
if (index != NO_ROBOT)
{
rsd->setMessage(index, "Penalty Shot Us Formation");
rsd->getDestination(index)->setPos(rsd->getSystemParams().field.THEIR_GOAL_LINE -
rsd->getSystemParams().field.PENALTY_KICK_X_OFFSET +
AGGRESSOR_X,
rsd->getSystemParams().field.SPLIT_LINE + AGGRESSOR_Y);
rsd->getDestination(index)->setRotation(AGGRESSOR_ROTATION);
rsd->getDestination(index)->setControl(OMNI_NORMAL_ENTERBOX);
rsd->getDestination(index)->setDribble(SLOW_DRIBBLE);
rsd->getDestination(index)->setVerticalDribble(SLOW_V_DRIBBLE);
rsd->getDestination(index)->setSpeed(FORMATION_SPEED);
}
//==============================================
//SPECIAL_OP
index = rsd->getRobotByPosition(SPECIAL_OP_DEFENDER);
if (index == NO_ROBOT)
index = rsd->getRobotByPosition(SPECIAL_OP_AGGRESSOR);
if (index == NO_ROBOT)
index = rsd->getRobotByPosition(SPECIAL_OP_CREATOR);
if (index != NO_ROBOT)
{
rsd->setMessage(index, "Penalty Shot Us Formation");
rsd->getDestination(index)->setPos(BALL_X + SPECIAL_OP_X - rsd->getSystemParams().general.PLAYER_RADIUS,
rsd->getSystemParams().field.SPLIT_LINE + SPECIAL_OP_Y);
rsd->getDestination(index)->setRotation(angleBetween
(getLocation(index, *vision, rsd->getSystemParams()),
Pair(BALL_X,
BALL_Y)));
rsd->getDestination(index)->setControl(OMNI_NORMAL_ENTERBOX);
rsd->getDestination(index)->setSpeed(FORMATION_SPEED);
}
//==============================================
//CREATOR
index = rsd->getRobotByPosition(CREATOR);
if (index != NO_ROBOT)
{
rsd->setMessage(index, "Penalty Shot Us Formation");
rsd->getDestination(index)->setPos(BALL_X +
CREATOR_X -
rsd->getSystemParams().general.PLAYER_RADIUS,
rsd->getSystemParams().field.SPLIT_LINE + CREATOR_Y);
rsd->getDestination(index)->setRotation(angleBetween
(getLocation(index, *vision, rsd->getSystemParams()),
Pair(BALL_X,
BALL_Y)));
rsd->getDestination(index)->setControl(OMNI_NORMAL_ENTERBOX);
rsd->getDestination(index)->setSpeed(FORMATION_SPEED);
}
//==============================================
}
void loop() {
api.getMyZRState(me);
if(api.getTime() % 60 == 0 && state != 3) {
state = 0;
for (int i = 0; i < 3; i++) oldPOIIDs[i] = false;
}
rebootIfStorm();
DEBUG(("I am in state %d.\n", state));
DEBUG(("I can still take %d photos.\n", game.getMemorySize() - game.getMemoryFilled()));
if (state == 0) {// POI Selection
game.getPOILoc(POI,0);
game.getPOILoc(otherPOI1,1);
game.getPOILoc(otherPOI2,2);
if (!oldPOIIDs[0] || !oldPOIIDs[1] || !oldPOIIDs[2]) {
//if you haven't already been to all the POIs
if (oldPOIIDs[0]) for (int i = 0; i < 3; i++) POI[i] = 1000;
if (oldPOIIDs[1]) for (int i = 0; i < 3; i++) otherPOI1[i] = 1000;
if (oldPOIIDs[2]) for (int i = 0; i < 3; i++) otherPOI2[i] = 1000;
//make sure you don't go to a previously-visited POI
}
if (distance(me,otherPOI1) <= distance(me,POI)) {
if (distance(me,otherPOI2) <= distance(me,otherPOI1)){
POIID = 2;
for (int i = 0; i < 3; i++) POI[i] = otherPOI2[i];
}
else {
POIID = 1;
for (int i = 0; i < 3; i++) POI[i] = otherPOI1[i];
}
}
else if (distance(me,otherPOI2) <= distance(me,POI)) {
POIID = 2;
for (int i = 0; i < 3; i++) POI[i] = otherPOI2[i];
}
else POIID = 0;
DEBUG(("POI Coors = %f,%f,%f\n",POI[0],POI[1],POI[2]));
state = 1;
}
if (state == 1) {// Take pic in outer zone
mathVecSubtract(facing,POI,me,3);
if (mathVecMagnitude(me,3) > 0.42 && mathVecMagnitude(me,3) < 0.53 && angleBetween(me, facing) > 2.94159265359) {
// in the outer zone and angle between me and POI is less than 0.2
picNum = game.getMemoryFilled();
game.takePic(POIID);
if (game.getMemoryFilled() - picNum == 1) {
DEBUG(("I just took a picture in the outer zone.\n"));
if (solarFlareBegin - api.getTime() > 13) state = 2;
else state = 3;
}
}
else {
for (int i = 0; i < 3; i++) {
destination[i] = POI[i] * 0.43 / mathVecMagnitude(POI, 3);
}
if (mathVecMagnitude(facing,3) > 0.5) setPositionTarget(destination, 2);
else setPositionTarget(destination);
}
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
}
if (state == 2) {// Take pic in inner zone
mathVecSubtract(facing,POI,me,3);
if (mathVecMagnitude(me,3) < 0.42 && angleBetween(me, facing) > 2.74159265359) {
//in the inner zone and angle between me and POI is less than 0.4
picNum = game.getMemoryFilled();
game.takePic(POIID);
if (game.getMemoryFilled() - picNum == 1) {
DEBUG(("I just took a picture in the inner zone.\n"));
state = 3;
}
}
else {
for (int i = 0; i < 3; i++) {
destination[i] = POI[i] * 0.33 / mathVecMagnitude(POI, 3);
}
if (mathVecMagnitude(facing,3) > 0.5) setPositionTarget(destination, 2);
else setPositionTarget(destination);
}
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
}
if (state == 3) {// Upload the picture
if (mathVecMagnitude(me,3) > 0.53) {
picNum = game.getMemoryFilled();
game.uploadPic();
DEBUG(("I just uploaded %d picture(s).\n", (picNum - game.getMemoryFilled())));
state = 0;
oldPOIIDs[POIID] = true;
}
else {
for (int i = 0; i < 3; i++) {
destination[i] = me[i] * 0.61 / mathVecMagnitude(me,3);
}
if (solarFlareBegin - api.getTime() < 8) setPositionTarget(destination, 4);
else if (mathVecMagnitude(me,3) < 0.5) setPositionTarget(destination, 2);
else setPositionTarget(destination);
}
}
}
TEST(VecTest, angleBetween) {
ASSERT_FLOAT_EQ(angleBetween(Vec3f::PosX, Vec3f::PosX, Vec3f::PosZ), 0.0f);
ASSERT_FLOAT_EQ(angleBetween(Vec3f::PosY, Vec3f::PosX, Vec3f::PosZ), Math::Cf::piOverTwo());
ASSERT_FLOAT_EQ(angleBetween(Vec3f::NegX, Vec3f::PosX, Vec3f::PosZ), Math::Cf::pi());
ASSERT_FLOAT_EQ(angleBetween(Vec3f::NegY, Vec3f::PosX, Vec3f::PosZ), 3.0f * Math::Cf::piOverTwo());
}
void monster_autocannon_turn(edict_t *self)
{
vec3_t old_angles;
VectorCopy(self->s.angles, old_angles);
if (!self->enemy)
{
if (self->monsterinfo.linkcount > 0)
{
int ideal_yaw = self->monsterinfo.attack_state;
int max_yaw = anglemod(ideal_yaw + self->monsterinfo.linkcount);
int min_yaw = anglemod(ideal_yaw - self->monsterinfo.linkcount);
while (max_yaw < min_yaw)
max_yaw += 360.0;
self->s.angles[YAW] += (self->monsterinfo.lefty ? -AC_TURN_SPEED : AC_TURN_SPEED);
// back and forth
if (self->s.angles[YAW] > max_yaw)
{
self->monsterinfo.lefty = 1;
self->s.angles[YAW] = max_yaw;
}
else if (self->s.angles[YAW] < min_yaw)
{
self->monsterinfo.lefty = 0;
self->s.angles[YAW] = min_yaw;
}
}
else
{
self->s.angles[YAW] = anglemod(self->s.angles[YAW] + AC_TURN_SPEED);
}
// angle pitch towards 5 to 10...
if (!self->onFloor)
{
if (self->s.angles[PITCH] > 10)
self->s.angles[PITCH] -= 4;
else if (self->s.angles[PITCH] < 5)
self->s.angles[PITCH] += 4;
}
else
{
if (self->s.angles[PITCH] < -10)
self->s.angles[PITCH] += 4;
else if (self->s.angles[PITCH] > -5)
self->s.angles[PITCH] -= 4;
}
}
else
{
// look toward enemy mid point
if (visible(self, self->enemy))
{
vec3_t offset, dest;
VectorCopy(self->enemy->mins, offset);
VectorAdd(offset, self->enemy->maxs, offset);
VectorScale(offset, 0.65, offset);
VectorAdd(self->enemy->s.origin, offset, dest);
angleToward(self, dest, AC_TURN_SPEED);
VectorCopy(dest, self->monsterinfo.last_sighting);
self->timeout = level.time + AC_TIMEOUT;
// restrict our range of movement if need be
if (self->monsterinfo.linkcount > 0)
{
float amax = anglemod(self->monsterinfo.attack_state + self->monsterinfo.linkcount);
float amin = anglemod(self->monsterinfo.attack_state - self->monsterinfo.linkcount);
self->s.angles[YAW] = anglemod(self->s.angles[YAW]);
if (!angleBetween(&self->s.angles[YAW], &amin, &amax))
{
// which is closer?
if (self->s.angles[YAW] - amax < amin - self->s.angles[YAW])
self->s.angles[YAW] = amin;
else
self->s.angles[YAW] = amax;
}
}
}
else // not visible now, so head toward last known spot
angleToward(self, self->monsterinfo.last_sighting, AC_TURN_SPEED);
}
// get our angles between 180 and -180
while(self->s.angles[PITCH] > 180)
self->s.angles[PITCH] -= 360.0;
while(self->s.angles[PITCH] < -180)
self->s.angles[PITCH] += 360;
// outside of the pitch extents?
if (self->s.angles[PITCH] > acPitchExtents[self->onFloor][1])
self->s.angles[PITCH] = acPitchExtents[self->onFloor][1];
else if (self->s.angles[PITCH] < acPitchExtents[self->onFloor][0])
self->s.angles[PITCH] = acPitchExtents[self->onFloor][0];
// make sure the turret's angles match the gun's
self->chain->s.angles[YAW] = self->s.angles[YAW];
self->chain->s.angles[PITCH] = 0;
// setup the sound
if (VectorCompare(self->s.angles, old_angles))
self->chain->s.sound = 0;
else
self->chain->s.sound = gi.soundindex("objects/acannon/ac_idle.wav");
}
//===============================================================================
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void JamAndShootSkill::execute()
{
///If not active, dont do anything!
if(!initialized)
{
return;
}
//aggresive mode
if(sp->strategy2002.JAM_AND_SHOOT_MODE == 1)
{
skillSet->getSkill(AggressiveJamAndShootSkill::skillNum)->run();
return;
}
//stupid mode
else if(sp->strategy2002.JAM_AND_SHOOT_MODE == 2)
{
skillSet->getSkill(StupidJamAndShootSkill::skillNum)->run();
return;
}
//else normal mode
//decrease the shoot threshold linearly as the skill runs.
shoot_threshold = sp->strategy2002.SHOOT_LANE_THRESH;// *
//(WAIT_TIME - (float)timer->currentTime()) / WAIT_TIME;
Pair currentPos(getLocation(robotID,*currentVisionData,*sp));
float currentRot = getRotation(robotID,*currentVisionData,*sp);
float good_shoot_thresh=shoot_threshold;
if(kicked) good_shoot_thresh*=1.5f;
else if(aimed) good_shoot_thresh=.05f;
//finished waiting. Shoot!
//note, we only shoot if we're facing in the direction of the goal.
kickFrames--;
if(
!gaveup &&
towardGoal(currentPos,
currentRot,sp->field.THEIR_GOAL_LINE,
sp->field.LEFT_GOAL_POST,
sp->field.RIGHT_GOAL_POST) &&
(timer->currentTime() > WAIT_TIME ||
goodImmediateShot(sp->general.TEAM,robotID,sp->strategy2002.KICK_VELOCITY,
*currentVisionData,*sp,good_shoot_thresh)
//goodCurrentShot(currentPos,currentRot,*currentVisionData,*sp,good_shoot_thresh)
)
)
{
gaveup=true;
if(driftdir != 0){
kickFrames = PAUSE_FRAMES + 2*rand()*PAUSE_RANDOM/RAND_MAX - PAUSE_RANDOM;
char msg[80];
sprintf(msg,"Jam and Shoot pausing for %d frames",kickFrames);
GUI_Record.debuggingInfo.addDebugMessage(msg);
}
kicked=false;
timer->resetTimer();
}
if (gaveup && kickFrames <= 0){
//we're out of time or have a good current shot, so just kick.
SimpleKickSkill * kickSkill=(SimpleKickSkill *) skillSet->getSkill(SimpleKickSkill::skillNum);
if(!kickSkill->isInitialized()){
kickSkill->initialize(KICK_SHOT);
}
kickSkill->run();
strategy->getCurrentFrame()->setMessage(robotID,"No Turn and Kick. Just Shooting.");
}else if(skillSet->getSkill(PullBallOffWallSkill::skillNum)->isValid()){
//deal with the ball in the corner or on the wall
if(!skillSet->getSkill(PullBallOffWallSkill::skillNum)->isInitialized()){
skillSet->getSkill(PullBallOffWallSkill::skillNum)->initialize();
}
skillSet->getSkill(PullBallOffWallSkill::skillNum)->run();
}else if(kicked) {
//We have a shot, let's aim and kick.
TurnAndKickSkill *kicker=(TurnAndKickSkill *)skillSet->getSkill(TurnAndKickSkill::skillNum);
kicker->run();
//strategy->getCurrentFrame()->setMessage(robotID,"Have Shot - taking it");
}else{
bool isShot;
//find best shot (if we're already drifting, use a larger threshold)
if(aimed)isShot=calcShot(robotID,sp->field.THEIR_GOAL_LINE,sp->strategy2002.SHOOT_LANE_THRESH+2*sp->general.PLAYER_RADIUS,
sp->field.RIGHT_GOAL_POST,sp->field.LEFT_GOAL_POST,NO_ROBOT,*currentVisionData,*sp,&target);
else isShot=calcShot(robotID,sp->field.THEIR_GOAL_LINE,sp->strategy2002.SHOOT_LANE_THRESH,
sp->field.RIGHT_GOAL_POST,sp->field.LEFT_GOAL_POST,NO_ROBOT,*currentVisionData,*sp,&target);
if(isShot && driftdir==0)
{
//take the shot
TurnAndKickSkill *kicker=(TurnAndKickSkill *)skillSet->getSkill(TurnAndKickSkill::skillNum);
kicker->initialize(target,KICK_SHOT,true);
kicker->run();
kicked=true;
strategy->getCurrentFrame()->setMessage(robotID,"Have a shot!");
kickFrames=0;
}else{
target.set(sp->field.THEIR_GOAL_LINE,sp->field.SPLIT_LINE);
//if no shot, start to wait, dodge if possible
GUI_Record.debuggingInfo.setDebugPoint(robotID,4,upperbound,testline);
GUI_Record.debuggingInfo.setDebugPoint(robotID,5,lowerbound,testline);
if (driftdir==0){
//Calculate which direction to drift
//first, see if we can shoot by shifting to the near post.
//if not, drift across the field.
//Default to drift across.
float goalPost;
if(currentPos.getY()>sp->field.SPLIT_LINE){
driftdir = 1;
goalPost=sp->field.LEFT_GOAL_POST;
}else{
driftdir = -1;
goalPost=sp->field.RIGHT_GOAL_POST;
}
upperbound=sp->field.THEIR_GOAL_LINE;
lowerbound=sp->field.KILL_ZONE_LINE;
testline=sp->field.SPLIT_LINE + driftdir*(SIDE_DIST);
Pair loc;
//check near-side shot.
if(currentPos.getY() * driftdir >= goalPost*driftdir &&
calcYShot(Pair(sp->field.THEIR_GOAL_LINE,sp->field.SPLIT_LINE),
testline,
sp->strategy2002.SHOOT_LANE_THRESH*SIDE_LANE_FACTOR,
upperbound,
lowerbound,NO_ROBOT,
*currentVisionData,
*sp,
&loc)){
driftdir=-driftdir;
}
}
// printf("%f\n",ABS(angleDifference(angleBetween(currentPos,target),currentRot)));
if(ABS(angleDifference(angleBetween(currentPos,target),currentRot)) < AIM_ANGLE || aimed){
aimed=true;
//if(target.getX() != sp->field.THEIR_GOAL_LINE) target.setX(sp->field.THEIR_GOAL_LINE);
//drift to the side to see if we have a shot.
//calculate the direction to drift
Pair dest;
float changeAngle;
//switch directions if we've gone too far.
if(angleBetween(currentPos,target)*driftdir > BOUNCE_ANGLE ){
driftdir=-driftdir;
}
target.setY(target.getY() - AIM_DISTANCE*driftdir);
changeAngle= DRIFT_ANGLE*driftdir;
char msg[80];
sprintf(msg, "No Shot - drifting %s",( driftdir > 0 ? "right" : "left"));
strategy->getCurrentFrame()->setMessage(robotID,msg);
//drift around goal
//See if we can push forward:
rotateAboutPoint(frontOfRobot(robotID,*currentVisionData,*sp), target,changeAngle, dest);
//DUE TO J&S BACKING UP TOO MUCH, I'M GETTING RID OF THIS NEXT PART
if(!isLane(currentPos,dest,sp->general.PLAYER_RADIUS,*currentVisionData,*sp,true)){
//we can't. :( drift sideways.
rotateAboutPoint(currentPos, target,changeAngle, dest);
}
//see if one of their robots is in the way, and back up if they are.
if(!isLane(currentPos,dest,sp->general.PLAYER_RADIUS,*currentVisionData,*sp,true)){
//move destination back
extendPoint(target,dest,sp->general.PLAYER_RADIUS,dest);
}
command->setPos(dest);
//Adjust angle to fast the direction we're drifting a bit.
command->setRotation(angleBetween(currentPos,target) - driftdir*DRIFT_ADJUST_ANGLE);
command->setDribble(DRIBBLE_DEFAULT);
command->setVerticalDribble(V_DRIBBLE_DEFAULT);
command->setSpeed(BALL_POSSESSION_SPEED);
}else{
strategy->getCurrentFrame()->setMessage(robotID,"No Shot - turning to goal");
//turn to face goal
SpinAroundBallSkill *spin=(SpinAroundBallSkill *)strategy->getSkillSet(robotID)->getSkill(SpinAroundBallSkill::skillNum);
spin->initialize(target);
spin->run();
}
}
}
RobotIndex goalie;
if(getLocation(robotID,*currentVisionData,*sp).getX() > sp->field.HALF_LINE){
if(getTheirGoalie(*currentVisionData,*sp,&goalie)){
if(getLocation(sp->general.OTHER_TEAM,goalie,*currentVisionData).distanceTo(getLocation(robotID,*currentVisionData,*sp)) >=
ENTERBOX_CAUTION_DIST+sp->general.PLAYER_RADIUS + sp->general.OPPONENT_RADIUS){
command->setControl(OMNI_FAST_ENTERBOX);
}
}else{
//if no goalie, enter the box
command->setControl(OMNI_FAST_ENTERBOX);
}
}
GUI_Record.debuggingInfo.setDebugPoint(robotID,0,target);
//command->setControl(OMNI_NORMAL_ENTERBOX);
command->setRotation(angleBetween(currentPos,target));
}
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void ThreeManSideSkill::execute()
{
///If not active, dont do anything!
if(!initialized) return;
//find goalie's shadow
// grab the ball location
if(!presetBall){
ball = strategy->getCurrentRoboCupFrame()->getDefensiveBallLocation();
}
//find blocker's shadow
float y1, y2;
RobotIndex BlockerID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(BLOCKER);
if(BlockerID == NO_ROBOT)
{
//blocker does not exist. we're in trouble
y1 = sp->field.LEFT_GOAL_POST;
y2 = sp->field.LEFT_GOAL_POST;
}
else
{
Pair blocker = getLocation(BlockerID,*currentVisionData,*sp);
findShadow(ball,blocker, *sp, y1, y2);
}
int side=1;
//Figure out which side we should be on.
if(ball.getY() < sp->field.HALF_LINE) side=-side;
if(strategy->getCurrentRoboCupFrame()->getPositionOfRobot(robotID) == DEFENDER){
side=-side;
chase=true;
}
if(side == -1)
{
//y2 = y1;
y1 = sp->field.RIGHT_GOAL_POST - sp->general.BALL_RADIUS;
y2 = sp->field.SPLIT_LINE - sp->general.BALL_RADIUS;
// if(y2<y1)y2=y1;
}
else
{
// y1=y2;
// if(y2<y1)y1=y2;
y1 = sp->field.SPLIT_LINE + sp->general.BALL_RADIUS;
y2 = sp->field.LEFT_GOAL_POST + sp->general.BALL_RADIUS;
}
if(presetBall){
((CutGoalSkill *)skillSet->getSkill(CutGoalSkill::skillNum))->initialize(y1,y2,UPPER_X,LOWER_X,ball);
}else{
((CutGoalSkill *)skillSet->getSkill(CutGoalSkill::skillNum))->initialize(y1,y2,UPPER_X,LOWER_X);
}
skillSet->getSkill(CutGoalSkill::skillNum)->run();
Pair robotLoc = getLocation(robotID, *currentVisionData, *sp);
//make him look at the ball, but never rotate too far back
float destAngle = angleBetween(robotLoc, getBallLocation(*currentVisionData));
float maxAngle = 3.0f*PI/8.0f;
if(destAngle > maxAngle)
destAngle = maxAngle;
if(destAngle < -maxAngle)
destAngle = -maxAngle;
command->setRotation(destAngle);
/*
//if robot moving past other side, make him run to the close side of the other robot, forcing
//position switching with obstacle avoidance getting in the way
int friendID;
if(strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER) == robotID)
friendID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(SPECIAL_OP_DEFENDER);
else
friendID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(DEFENDER);
if(friendID != NO_ROBOT)
{
Pair friendLoc = getLocation((RobotIndex)friendID, *currentVisionData, *sp);
Pair robotLoc = getLocation(robotID, *currentVisionData, *sp);
//other guy closer, go to his side
if(friendLoc.squareDistanceTo(command->getPos()) < robotLoc.squareDistanceTo(command->getPos()))
{
command->setXPos(friendLoc.getX());
if(friendLoc.getY() < robotLoc.getY())
{
command->setYPos(friendLoc.getY() + 2.0f*sp->general.PLAYER_RADIUS);
}
else
{
command->setYPos(friendLoc.getY() - 2.0f*sp->general.PLAYER_RADIUS);
}
}
}
*/
//see if we should run out and kick the ball
if(
ball.distanceTo(command->getPos()) <= sp->general.PLAYER_RADIUS + KICK_DIST &&
kickSafe(robotID, *currentVisionData, *sp, *strategy->getCurrentRoboCupFrame())
)
{
strategy->getCurrentFrame()->setMessage(robotID,"3Man: Kicking ball away");
command->setPos(robotPositionGivenFront(ball,0.0f,*sp));
}
if(
ABS(getRotation(robotID,*currentVisionData,*sp)) <= KICK_ANGLE &&
kickSafe(robotID, *currentVisionData, *sp, *strategy->getCurrentRoboCupFrame())
)
{
command->setKick(KICK_SHOT);
}
//debugging only:
findShadow(ball,getLocation(robotID,*currentVisionData,*sp),*sp,y1,y2);
GUI_Record.debuggingInfo.setDebugPoint(robotID,1,sp->field.OUR_GOAL_LINE,y1);
GUI_Record.debuggingInfo.setDebugPoint(robotID,2,sp->field.OUR_GOAL_LINE,y2);
}
