//-------------------------------------------------------------------------------- bool EditLine::checkBallCollide(Ball *ball){ V3 lineLocalVec(x2-x1, y2-y1, 0); //get the angle between the ball and one end of the wall float angleCurrent1 = checkAngle(ball->x , ball->y , x1,y1, lineLocalVec); float angleCurrent2 = checkAngle(ball->x , ball->y , x2,y2, lineLocalVec); //lets get the angle between the ball and one end of the wall float angleFuture1 = checkAngle(ball->x+ball->force.x, ball->y+ball->force.y ,x1,y1, lineLocalVec); float angleFuture2 = checkAngle(ball->x+ball->force.x, ball->y+ball->force.y ,x2,y2, lineLocalVec); if(diffSign(angleCurrent1,angleFuture1) && diffSign(angleCurrent2,angleFuture2)){ float d1x = ball->x - x1; float d1y = ball->y - y1; float d2x = ball->x - x2; float d2y = ball->y - y2; float wallLength = lineLocalVec.getLength(); if( (sqrt(d1x*d1x + d1y*d1y) < wallLength) && (sqrt(d2x*d2x + d2y*d2y) < wallLength)){ return true; } else return false; } else return false; }
bool OperatorTrigonometric::executeAngle(double &value) { value = angleAdjust(value); if (!checkAngle(value)) { setErrMsg("Invalid trigonometric angle"); return false; } value = angle2radian(value); return executeRadian(value); }
bool OperatorTrigonometricArc::executeAngle(double &value) { if (!checkValue(value)) { setErrMsg("Invalid arc trigonometric value"); return false; } if (!executeRadian(value)) { return false; } value = radian2angle(value); if (!checkAngle(value)) { setErrMsg("Invalid trigonometric angle"); return false; } return true; }
//returns true if p is inside the polygon and false otherwise //pathological cases where p is on a vertex or edge are not //explicitly handled and result in undefined behavior bool isInside(const Point& p) { if(m_pts.size() < 2) return false; double angle = 0; Point p1, p2; for(size_t i = 0; i < m_pts.size(); ++i) { p1 = m_pts.at(i); p1 -= p; p2 = m_pts.at((i+1)%m_pts.size()); p2 -= p; angle += checkAngle(p1.x(), p1.y(), p2.x(), p2.y()); } if(fabs(angle) < PI) return false; else return true; }
void AIStateSearching::setPeriodAngles() { m_vSearchAngles.push_back(m_fEnterAngle); int maxTrigger = 0; for (int i = 1; i < m_vSearchPeriods.size(); i++) { float tempangle = static_cast <float> (rand()) / static_cast <float> (RAND_MAX / 360); while (checkAngle(tempangle)) { if (++maxTrigger > 1000) break; tempangle = static_cast <float> (rand()) / static_cast <float> (RAND_MAX / 360); } m_vSearchAngles.push_back(tempangle); } }
int main() { double *time; double **angles; int n; double slope; double intercept; CMobot mobot; angles = (double**)malloc(sizeof(double*)*4); mobot.connect(); if(!mobot.isConnected()) { ERRMSG("Connect failed"); return -1; } mobot.setJointSpeeds(45, 45, 45, 45); mobot.resetToZero(); mobot.moveContinuousNB(ROBOT_FORWARD, ROBOT_HOLD, ROBOT_HOLD, ROBOT_BACKWARD); mobot.recordAnglesBegin(time, angles[0], angles[1], angles[2], angles[3], 0.1); #ifndef _WIN32 sleep(12); #else Sleep(12000); #endif printf("Sleep done.\n"); mobot.recordAnglesEnd(n); printf("Ended Recording.\n"); mobot.stopAllJoints(); printf("Joints Stopped.\n"); /* Check end angle. */ double angle; mobot.getJointAngle(ROBOT_JOINT1, angle); if(ABS(angle - 45*12) > 20) { ERRMSG("Mobot movement speed incorrect"); return -1; } /* Perform least squares regression */ double sum_y = 0; double sum_y_2 = 0; // sum(y^2) double sum_x = 0; double sum_x_2 = 0; double sum_xy = 0; double angle_mean = 0; int i; for(i = 0; i < n; i++) { sum_y += angles[0][i]; sum_y_2 += angles[0][i]*angles[0][i]; sum_x += time[i]; sum_x_2 += time[i] * time[i]; sum_xy += angles[0][i] * time[i]; angle_mean += angles[0][i]/n; } intercept = ((sum_y * sum_x_2) - (sum_x * sum_xy)) / (((double)n)*sum_x_2 - sum_x*sum_x); slope = ((double)n * sum_xy - sum_x * sum_y)/ (((double)n)*sum_x_2 - sum_x*sum_x); printf("Slope is %lf\n", slope); if(checkAngle(slope, 45)) { ERRMSG("Slope incorrect"); return -1; } /* Check the residual */ double SS_tot = 0; double SS_err = 0; double R_2 = 0; double diff; for(i = 0; i < n; i++) { SS_tot += pow(angles[0][i] - angle_mean, 2); SS_err += pow(angles[0][i] - (slope*time[i] + intercept), 2); } R_2 = 1 - SS_err/SS_tot; printf("R squared: %lf\n", R_2); if(R_2 < 0.97) { ERRMSG("Recorded data too noisy"); } /* Now do it again but faster */ mobot.setJointSpeeds(60, 60, 60, 60); mobot.resetToZero(); mobot.moveContinuousNB(ROBOT_FORWARD, ROBOT_HOLD, ROBOT_HOLD, ROBOT_BACKWARD); mobot.recordAnglesBegin(time, angles[0], angles[1], angles[2], angles[3], 0.1); #ifndef _WIN32 sleep(12); #else Sleep(12); #endif printf("Sleep done.\n"); mobot.recordAnglesEnd(n); printf("Ended Recording.\n"); mobot.stopAllJoints(); printf("Joints Stopped.\n"); /* Check end angle. */ mobot.getJointAngle(ROBOT_JOINT1, angle); if(ABS(angle - 60*12) > 30) { ERRMSG("Mobot movement speed incorrect"); return -1; } /* Perform least squares regression */ sum_y = 0; sum_y_2 = 0; // sum(y^2) sum_x = 0; sum_x_2 = 0; sum_xy = 0; angle_mean = 0; for(i = 0; i < n; i++) { sum_y += angles[0][i]; sum_y_2 += angles[0][i]*angles[0][i]; sum_x += time[i]; sum_x_2 += time[i] * time[i]; sum_xy += angles[0][i] * time[i]; angle_mean += angles[0][i]/n; } intercept = ((sum_y * sum_x_2) - (sum_x * sum_xy)) / (((double)n)*sum_x_2 - sum_x*sum_x); slope = ((double)n * sum_xy - sum_x * sum_y)/ (((double)n)*sum_x_2 - sum_x*sum_x); printf("Slope is %lf\n", slope); if(checkAngle(slope, 60)) { ERRMSG("Slope incorrect"); return -1; } /* Check the residual */ SS_tot = 0; SS_err = 0; R_2 = 0; for(i = 0; i < n; i++) { SS_tot += pow(angles[0][i] - angle_mean, 2); SS_err += pow(angles[0][i] - (slope*time[i] + intercept), 2); } R_2 = 1 - SS_err/SS_tot; printf("R squared: %lf\n", R_2); if(R_2 < 0.97) { ERRMSG("Recorded data too noisy"); } return 0; }
int main() { double angles[4]; CMobot mobot; mobot.connect(); if(!mobot.isConnected()) { ERRMSG("Connect failed"); return -1; } mobot.disconnect(); mobot.connect(); if(!mobot.isConnected()) { ERRMSG("Reconnect failed"); return -1; } mobot.setJointSpeeds(45, 45, 45, 45); mobot.resetToZero(); /* Begin testing. First, rotate joint 2 */ mobot.moveJointToNB(ROBOT_JOINT2, -85); /* This should take about 2 seconds */ #ifndef _WIN32 usleep(1000000); #else Sleep(1000); #endif if (!mobot.isMoving()) { ERRMSG("Error: Robot should still be moving."); return -1; } #ifndef _WIN32 usleep(1000000); #else Sleep(1000); #endif if (mobot.isMoving()) { ERRMSG("Error: Robot did not reach joint angle in time."); return -1; } mobot.getJointAngle(ROBOT_JOINT2, angles[0]); if(checkAngle(angles[0], -85)) { ERRMSG("Angle mismatch"); return -1; } /*Now rotate joint 3 */ mobot.moveJointTo(ROBOT_JOINT3, 70); mobot.moveJointTo(ROBOT_JOINT1, 45); mobot.moveJointTo(ROBOT_JOINT2, 20); /* Check the joint angles */ mobot.getJointAngles(angles[0], angles[1], angles[2], angles[3]); if(checkAngle(angles[0], 45)) { ERRMSG("Angle Mismatch"); return -1; } if(checkAngle(angles[1], 20)) { ERRMSG("Angle Mismatch"); return -1; } if(checkAngle(angles[2], 70)) { ERRMSG("Angle Mismatch"); return -1; } if(checkAngle(angles[3], 0)) { ERRMSG("Angle Mismatch"); return -1; } mobot.disconnect(); return 0; }