LatLonHead Spherical::get() const {
LatLonHead llh = get_rad();
llh.Latitude = radiansToDegrees(llh.Latitude);
llh.Longatude = radiansToDegrees(llh.Longatude);
llh.Heading = radiansToDegrees(llh.Heading);
return llh;
}
EulerAnglesStruct Quaternion::getEulerAngles(void) const {
EulerAnglesStruct eulerAnglesStruct;
eulerAnglesStruct.roll = radiansToDegrees(atan2(2.0f * (q[2] * q[3] - q[0] * q[1]), 2.0f * q[0] * q[0] - 1.0f + 2.0f * q[3] * q[3]));
eulerAnglesStruct.pitch = radiansToDegrees(-atan((2.0f * (q[1] * q[3] + q[0] * q[2])) / sqrt(1.0f - pow((2.0f * q[1] * q[3] + 2.0f * q[0] * q[2]), 2.0f))));
eulerAnglesStruct.yaw = radiansToDegrees(atan2(2.0f * (q[1] * q[2] - q[0] * q[3]), 2.0f * q[0] * q[0] - 1.0f + 2.0f * q[1] * q[1]));
return eulerAnglesStruct;
}
void Camera::lookAt(glm::vec3 position) {
assert(position != _position);
glm::vec3 direction = glm::normalize(position - _position);
_verticalAngle = radiansToDegrees(asinf(-direction.y));
_horizontalAngle = -radiansToDegrees(atan2f(-direction.x, -direction.z));
normalizeAngles();
}
DEGREES dirOf(Point pt, Point from){
//This function returns which direction the robot would have
//to travel in to get to [pt] if it started at [from].
//Note: this function's comments assume that from is mainRobot.loc,
//because in most cases it is
if(!isPtInField(pt) || !isPtInField(from)) return NULL;
INCHES triWidth = pt.x - from.x; //triangle's width (length of horizontal side)
INCHES triHeight = pt.y - from.y; //triangle's height (length of vertical side)
//stores the measure of the angle that is across from the triangle's "width" (horizontal) leg
DEGREES angleAcrossWidth = radiansToDegrees(atan((float)triWidth / (float)triHeight));
//stores the measure of the angle that is across from the triangle's "height" (vertical) leg
DEGREES angleAcrossHeight = radiansToDegrees(atan((float)triHeight / (float)triWidth));
//It seems that this series of if statements is necessary,
//even without the abs() function around the 2nd and 3rd lines
//of code in this function. Please feel free to experiment with
//and correct any code.
//Return the direction of from from pt.
if(from.y < pt.y){ //if robot above point
if(from.x < pt.x) //if robot left
return 180 - angleAcrossWidth; //or angleAcrossHeight + 90
else if(from.x > pt.x) //if robot right
return 180 + angleAcrossWidth; //or -angleAcrossHeight - 90
else //if robot has the same x value (if they are on the same vertical line)
return 180;
}
else if(from.y > pt.y){ //if robot below point
if(from.x < pt.x) //if robot left
return angleAcrossWidth; //or 90 - angleAcrossHeight
else if(from.x > pt.x) //if robot right
return 270 + angleAcrossHeight; //or 270 + angleAcrossHeight
else //if robot has the same x value (if they are on the same vertical line)
return 0;
}
else{ //if robot has the same y value (if they are on the same horizontal line)
if(from.x < pt.x) //if robot left
return 90;
else if(from.x > pt.x) //if robot right
return 270;
else //if the point and the robot coincide
return COINCIDENCE;
}
}
// Descr: test degrees to radians and radians to degrees
// Implementation details: See gtest/samples for GTest syntax and usage
TEST(GeometryTest, D2RandR2D)
{
//test converting Degrees to Radians
// I know this 'should' be EXPECT_DOUBLE_EQ, but even though the values were good to the 5th or 6th decimal place, it was failing.
// So, there you go.
EXPECT_FLOAT_EQ(degreesToRadians(1),0.0174532925 );
EXPECT_FLOAT_EQ(degreesToRadians(254),4.4331363 );
EXPECT_FLOAT_EQ(degreesToRadians(360),6.283185307 );
EXPECT_FLOAT_EQ(degreesToRadians(15),0.261799388 );
EXPECT_FLOAT_EQ(radiansToDegrees(3.14),179.908747671 );
EXPECT_FLOAT_EQ(radiansToDegrees(0.174532925),10 );
EXPECT_FLOAT_EQ(radiansToDegrees(1.745329252),100 );
EXPECT_FLOAT_EQ(radiansToDegrees(.1234567),7.073547863 );
EXPECT_FLOAT_EQ(radiansToDegrees(6.195918845),355 );
}
//a direct clone from the old getCamera by changing it to glm
glm::mat4 FlyCamera::getCamera(){
glm::mat4 m(1.0f);
glm::mat4 m2(1.0f);
glm::mat4 m3(1.0f);
m = glm::translate(m, glm::vec3(-posX, -posY, -posZ));
m2 = glm::rotate(glm::mat4(1.0f),radiansToDegrees(-direction), glm::vec3(0, 1, 0));
glm::mat4 c3 = m2;
m3 = m2 * m;
c3 = m3;
m2 = glm::rotate(glm::mat4(1.0f), radiansToDegrees(pitch), glm::vec3(1, 0, 0));
glm::mat4 c4 = m2;
m = m2 * m3;
return m;
}
CGFloat angleBetweenPoints(CGPoint first, CGPoint second) {
CGFloat height = second.y - first.y;
CGFloat width = first.x - second.x;
CGFloat rads = atan(height/width);
return radiansToDegrees(rads);
//degs = degrees(atan((top - bottom)/(right - left)))
}
void ParticlesBombfire::update(const float time){
sf::Vector2f tempPosition;
b2Vec2 positionVector;
for (int i = 0; i < particles.size(); i++){
if (particles.at(i) != NULL){
// Set particle position at bomb burning hole.
positionVector = directionPoint(0,0,direction, 0.25f);
positionVector.x *= visual->getBlockSize();
positionVector.y *= visual->getBlockSize();
tempPosition.x = emitter.x + positionVector.x;
tempPosition.y = emitter.y + positionVector.y;
particles.at(i)->sprite.setPosition(tempPosition.x, tempPosition.y);
particles.at(i)->direction += particles.at(i)->rotationSpeed*time*0.01;
particles.at(i)->sprite.setRotation( radiansToDegrees(particles.at(i)->direction) );
// Since it's an array of pointers the speed penalty of this quickfix is neglectable.
if(particles.at(i)->timer > particles.at(i)->lifetime){
delete particles.at(i);
particles.at(i) = NULL;
}
}
}
timer += time;
}
//----------------------------------------------------------------------------------------------------------------------
void CTRNNNeuronViz::onMouseMove(const Vec4f& mPos)
{
int N = m_ctrnn->getSize();
m_selected = -1;
// Update mouse info: transform mouse position into local space angle and radius
Vec4f posLocal = mPos - m_pTM->getTranslate();
float angle = radiansToDegrees(atan2(posLocal.x, posLocal.y));
if(angle < 0)
{
angle += 360.0f;
}
const float networkRadius = m_width / 2.5;
const float neuronRadius = networkRadius / N + 6;
int neuron = (int)angle / (360 / (int) N);
float neuronAngle = -((float)neuron + 0.5f) * TWO_PI / N + PI_OVER_TWO;
Vec4f neuronPos (networkRadius * cosf(neuronAngle), networkRadius * sinf(neuronAngle), 0.0f, 1.0);
float dist = neuronPos.distance(posLocal);
if(dist < neuronRadius)
{
m_selected = neuron;
}
}
/**
* Detects rotation at one edge of the area specified by left, top, right, bottom.
* Which of the four edges to take depends on whether shiftX or shiftY is non-zero,
* and what sign this shifting value has.
*/
static double detectEdgeRotation(float deskewScanRange, float deskewScanStep, int deskewScanSize, float deskewScanDepth, int shiftX, int shiftY, int left, int top, int right, int bottom, struct IMAGE* image) {
// either shiftX or shiftY is 0, the other value is -i|+i
// depending on shiftX/shiftY the start edge for shifting is determined
double rangeRad;
double stepRad;
double rotation;
int peak;
int maxPeak;
double detectedRotation;
double m;
rangeRad = degreesToRadians((double)deskewScanRange);
stepRad = degreesToRadians((double)deskewScanStep);
detectedRotation = 0.0;
maxPeak = 0;
// iteratively increase test angle, alterating between +/- sign while increasing absolute value
for (rotation = 0.0; rotation <= rangeRad; rotation = (rotation>=0.0) ? -(rotation + stepRad) : -rotation ) {
m = tan(rotation);
peak = detectEdgeRotationPeak(m, deskewScanSize, deskewScanDepth, shiftX, shiftY, left, top, right, bottom, image);
if (peak > maxPeak) {
detectedRotation = rotation;
maxPeak = peak;
}
}
return radiansToDegrees(detectedRotation);
}
void RCDPad::processTouch(cocos2d::CCPoint point)
{
CCSpriteFrameCache *cache = CCSpriteFrameCache::sharedSpriteFrameCache();
setColor(ccc3(255,200,200));
m_pressed = true;
CCPoint center = ccp( rect().origin.x+rect().size.width/2, rect().origin.y+rect().size.height/2 );
if(distanceBetweenPoints(point, center) < rect().size.width/10){
setDisplayFrame(cache->spriteFrameByName("d_pad_normal.png"));
setRotation(0);
m_pressedVector = ccp(0,0);
m_direction = kDPadNoDirection;
return;
}
float radians = vectorToRadians( ccp(point.x-center.x, point.y-center.y) );
float degrees = radiansToDegrees(radians) + 90;
float sin45 = 0.7071067812f;
if(degrees >= 337.5 || degrees < 22.5){
setDisplayFrame(cache->spriteFrameByName("d_pad_horizontal.png"));
setRotation(180);
m_pressedVector = ccp(-1,0);
m_direction = kDPadLeft;
}else if(degrees >= 22.5 && degrees < 67.5){
setDisplayFrame(cache->spriteFrameByName("d_pad_diagonal.png"));
setRotation(-90);
m_pressedVector = ccp(-sin45,sin45);
m_direction = kDPadUpLeft;
}else if(degrees >= 67.5 && degrees < 112.5){
setDisplayFrame(cache->spriteFrameByName("d_pad_horizontal.png"));
setRotation(-90);
m_pressedVector = ccp(0,1);
m_direction = kDPadUp;
}else if(degrees >= 112.5 && degrees < 157.5){
setDisplayFrame(cache->spriteFrameByName("d_pad_diagonal.png"));
setRotation(0);
m_pressedVector = ccp(sin45,sin45);
m_direction = kkDPadUpRight;
}else if(degrees >= 157.5 && degrees < 202.5){
setDisplayFrame(cache->spriteFrameByName("d_pad_horizontal.png"));
setRotation(0);
m_pressedVector = ccp(1,0);
m_direction = kkDPadRight;
}else if(degrees >= 202.5 && degrees < 247.5){
setDisplayFrame(cache->spriteFrameByName("d_pad_diagonal.png"));
setRotation(90);
m_pressedVector = ccp(sin45,-sin45);
m_direction = kDPadDownRight;
}else if(degrees >= 247.5 && degrees < 292.5){
setDisplayFrame(cache->spriteFrameByName("d_pad_horizontal.png"));
setRotation(90);
m_pressedVector = ccp(0,-1);
m_direction = kDPadDown;
}else{
setDisplayFrame(cache->spriteFrameByName("d_pad_diagonal.png"));
setRotation(180);
m_pressedVector = ccp(-sin45,-sin45);
m_direction = kDPadDownLeft;
}
}
//----------------------------------------------------------------------------------------------------------------------
void SMCAgent::toXml(ci::XmlTree& xml)
{
if(m_distanceSensor != NULL)
m_distanceSensor->toXml(xml);
xml.push_back(ci::XmlTree("MaxSpeed", toString(m_maxSpeed)));
float maxAngSpeed = radiansToDegrees(m_maxAngularSpeed);
xml.push_back(ci::XmlTree("MaxAngularSpeed", toString(maxAngSpeed)));
xml.push_back(ci::XmlTree("MaxPosition", toString(m_maxPosition)));
xml.push_back(ci::XmlTree("PositionWraps", toString(m_positionWraps)));
float maxAngle = radiansToDegrees(m_maxAngle);
xml.push_back(ci::XmlTree("MaxAngle", toString(maxAngle)));
xml.push_back(ci::XmlTree("AngleWraps", toString(m_angleWraps)));
// ctrnn
m_ctrnn->toXml(xml);
}
// See diagrams: angle = arctan(z/x),
// where gyro sensor faces up, with white part
// is furthest from locus of movement
float angleFromAccel(tSensors sensor)
{
int x, y, z;
float angle; // in radians
HTACreadAllAxes(sensor, x, y, z);
angle = atan((float) z / (float) x);
return radiansToDegrees(angle);
}
void testGetAngleBetweenPlanes()
{
cout << "Testing GetAngleBetweenPlanes" <<endl;
srand(time(NULL));
int numberOfTests = 100;
for (int i = 0; i < numberOfTests; i++)
{
Plane a, b;
a.atom1.x = ((double) rand() / RAND_MAX) * 10;
a.atom1.y = ((double) rand() / RAND_MAX) * 10;
a.atom1.z = ((double) rand() / RAND_MAX) * 10;
a.atom2.x = ((double) rand() / RAND_MAX) * 10;
a.atom2.y = ((double) rand() / RAND_MAX) * 10;
a.atom2.z = ((double) rand() / RAND_MAX) * 10;
a.atom3.x = ((double) rand() / RAND_MAX) * 10;
a.atom3.y = ((double) rand() / RAND_MAX) * 10;
a.atom3.z = ((double) rand() / RAND_MAX) * 10;
b.atom1.x = ((double) rand() / RAND_MAX) * 10;
b.atom1.y = ((double) rand() / RAND_MAX) * 10;
b.atom1.z = ((double) rand() / RAND_MAX) * 10;
b.atom2.x = ((double) rand() / RAND_MAX) * 10;
b.atom2.y = ((double) rand() / RAND_MAX) * 10;
b.atom2.z = ((double) rand() / RAND_MAX) * 10;
b.atom3.x = ((double) rand() / RAND_MAX) * 10;
b.atom3.y = ((double) rand() / RAND_MAX) * 10;
b.atom3.z = ((double) rand() / RAND_MAX) * 10;
Point aNormal = getNormal(a);
Point bNormal = getNormal(b);
double numerator = aNormal.x * bNormal.x + aNormal.y * bNormal.y + aNormal.z * bNormal.z;
double aMag = sqrt(aNormal.x * aNormal.x + aNormal.y * aNormal.y + aNormal.z * aNormal.z);
double bMag = sqrt(bNormal.x * bNormal.x + bNormal.y * bNormal.y + bNormal.z * bNormal.z);
double denominator = aMag * bMag;
double thetaR = acos(numerator / denominator);
double expectedTheta = radiansToDegrees(thetaR);
double testTheta = getAngle(a, b);
if (!((testTheta - expectedTheta) / expectedTheta < PRECISION))
{
printf("Test GetAngleBetweenPlanes #%d failed. testTheta = %f | expectedTheta = %f.\n", i, testTheta, expectedTheta);
}
assert((testTheta - expectedTheta) / expectedTheta < PRECISION);
}
cout << "Testing GetAngleBetweenPlanes Completed\n" <<endl;
}
task main()
{
while (true)
{
getJoystickSettings(joystick);
int a = radiansToDegrees(atan2(joystick.joy1_x1,joystick.joy1_y1));
nxtDisplayString(0, "%i", a);
}
}
void testD2RandR2D()
{
cout << "Testing degrees2radins and radian2degrees" << endl;
//test converting Degrees to Radians
assert(percentDifference(degreesToRadians(1),0.0174532925) );
assert(percentDifference(degreesToRadians(45),0.785398163) );
assert(percentDifference(degreesToRadians(254),4.4331363) );
assert(percentDifference(degreesToRadians(360),6.283185307) );
assert(percentDifference(degreesToRadians(15),0.261799388) );
//test converting Radians to Degrees
assert(percentDifference( radiansToDegrees(3.14),179.908747671) );
assert(percentDifference( radiansToDegrees(.1234567),7.073547863) );
assert(percentDifference( radiansToDegrees(0.174532925),10) );
assert(percentDifference( radiansToDegrees(1.745329252),100) );
assert(percentDifference( radiansToDegrees(6.195918845),355) );
cout << "Testing degrees2radins and radian2degrees Complete\n" << endl;
}
/* @brief Get angle Z,X calculated data
*
* @param Angle - calculated angle
*
* @retval void
*/
inline MPU6050_errorstatus MPU6050_Get_AccAngleXZ_Data( float* Angle ){
MPU6050_errorstatus errorstatus = MPU6050_NO_ERROR;
uint8_t xlow,xhigh, zlow, zhigh;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_L, &xlow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_H, &xhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_L, &zlow, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_H, &zhigh, 1);
#ifndef _NoError
if(errorstatus != 0){
return errorstatus;
}
#endif
int16_t X = (xhigh << 8 | xlow);
int16_t Z = (zhigh << 8 | zlow);
float tempAngle = 0.0f;
tempAngle = radiansToDegrees( atanf( ( (float)Z )/ ( (float)X ) ) );
if( ( Z < 0 && tempAngle < 0 )
|| ( Z > 0 && tempAngle > 0 )
)
{
*Angle = tempAngle;
}
else if( Z > 0 && tempAngle < 0 )
{
*Angle = 180 + tempAngle;
}
else if( Z < 0 && tempAngle > 0 )
{
*Angle = -180 + tempAngle;
}
*Angle += ANGLE_OFFSET;
return errorstatus;
}
float Quaternion::Angle(const Quaternion &q1, const Quaternion &q2)
{
float dotProduct = Dot(q1, q2);
if(dotProduct > 1)
dotProduct = 1;
else if(dotProduct < -1)
dotProduct = -1;
return radiansToDegrees(acosf(dotProduct));
}
// Descr: evident
TEST(GeometryTest, GetAngleBetweenPlanes)
{
srand(time(NULL));
int numberOfTests = 100;
for (int i = 0; i < numberOfTests; i++)
{
Plane a, b;
a.atom1.x = ((double) rand() / RAND_MAX) * 10;
a.atom1.y = ((double) rand() / RAND_MAX) * 10;
a.atom1.z = ((double) rand() / RAND_MAX) * 10;
a.atom2.x = ((double) rand() / RAND_MAX) * 10;
a.atom2.y = ((double) rand() / RAND_MAX) * 10;
a.atom2.z = ((double) rand() / RAND_MAX) * 10;
a.atom3.x = ((double) rand() / RAND_MAX) * 10;
a.atom3.y = ((double) rand() / RAND_MAX) * 10;
a.atom3.z = ((double) rand() / RAND_MAX) * 10;
b.atom1.x = ((double) rand() / RAND_MAX) * 10;
b.atom1.y = ((double) rand() / RAND_MAX) * 10;
b.atom1.z = ((double) rand() / RAND_MAX) * 10;
b.atom2.x = ((double) rand() / RAND_MAX) * 10;
b.atom2.y = ((double) rand() / RAND_MAX) * 10;
b.atom2.z = ((double) rand() / RAND_MAX) * 10;
b.atom3.x = ((double) rand() / RAND_MAX) * 10;
b.atom3.y = ((double) rand() / RAND_MAX) * 10;
b.atom3.z = ((double) rand() / RAND_MAX) * 10;
Point aNormal = getNormal(a);
Point bNormal = getNormal(b);
double numerator = aNormal.x * bNormal.x + aNormal.y * bNormal.y + aNormal.z * bNormal.z;
double aMag = sqrt(aNormal.x * aNormal.x + aNormal.y * aNormal.y + aNormal.z * aNormal.z);
double bMag = sqrt(bNormal.x * bNormal.x + bNormal.y * bNormal.y + bNormal.z * bNormal.z);
double denominator = aMag * bMag;
double thetaR = acos(numerator / denominator);
double expectedTheta = radiansToDegrees(thetaR);
double testTheta = getAngle(a, b);
ASSERT_LT( (testTheta - expectedTheta) / expectedTheta, PRECISION);
}
}
//This method extracts the ZYX Euler angles from the rotation matrix.
void Matrix3::extractEulerZYX(float &xDegrees, float &yDegrees, float &zDegrees)
{
//Diagram of the Euler ZYX Matrix:
//
// -------------------------------------------------------------
//| cY * cZ, | -cY * sZ, | sY |
//|------------------------|-------------------------|----------|
//| cZ * sX * sY + cX * sZ | cX * cZ - sX * sY * sZ | -cY * sX |
//|------------------------|-------------------------|----------|
//| sX * sZ - cX * cZ * sY | cZ * sX + cX * sY * sZ | cX * cY |
// -------------------------------------------------------------
//Case 1: -pi/2 < arcsin(sY) < pi/2 (or, -1 < sY < 1). This solution is
//unique.
if(-1 + 0.000001f <= m_Matrix[2][0] && m_Matrix[2][0] <= 1 - 0.000001f)
{
xDegrees = radiansToDegrees(atan2f(-m_Matrix[2][1], m_Matrix[2][2]));
yDegrees = radiansToDegrees(asinf(m_Matrix[2][0]));
zDegrees = radiansToDegrees(atan2f(-m_Matrix[1][0], m_Matrix[0][0]));
}
//Case 2: arcsin(sY) = pi/2 (or, sY = 1). This solution is not unique, so
//the x-angle is set to 0.
else if(1 - 0.000001f < m_Matrix[2][0] && m_Matrix[2][0] < 1 + 0.000001f)
{
yDegrees = 90;
xDegrees = 0;
zDegrees = radiansToDegrees(atan2f(m_Matrix[0][1], m_Matrix[1][1]));
}
//Case 3: arcsin(sY) = -pi/2 (or, sY = -1). This solution is not unique,
//so the x-angle is set to 0.
else
{
yDegrees = -90;
xDegrees = 0;
zDegrees = radiansToDegrees(atan2f(m_Matrix[0][1], m_Matrix[1][1]));
}
}
//This method extracts the ZXY Euler angles from the rotation matrix.
void Matrix3::extractEulerZXY(float &xDegrees, float &yDegrees, float &zDegrees)
{
//Diagram of the Euler ZXY Matrix:
//
// ------------------------------------------------------------
//| cY * cZ + sX * sY * sZ | cZ * sX * sY - cY * sZ | cX * sY |
//|------------------------|------------------------|----------|
//| cX * sZ | cX * cZ | -sX |
//|------------------------|------------------------|----------|
//| cY * sX * sZ - cZ * sY | cY * cZ * sX + sY * sZ | cX * cY |
// ------------------------------------------------------------
//Case 1: -pi/2 < arcsin(sX) < pi/2 (or, -1 < sX < 1). This solution is
//unique.
if(-1 + 0.000001f <= m_Matrix[2][1] && m_Matrix[2][1] <= 1 - 0.000001f)
{
xDegrees = radiansToDegrees(asinf(-m_Matrix[2][1]));
yDegrees = radiansToDegrees(atan2f(m_Matrix[2][0], m_Matrix[2][2]));
zDegrees = radiansToDegrees(atan2f(m_Matrix[0][1], m_Matrix[1][1]));
}
//Case 2: arcsin(sX) = pi/2 (or, sX = 1). This solution is not unique, so
//the y-angle is set to 0.
else if(1 - 0.000001f < -m_Matrix[2][1] && -m_Matrix[2][1] < 1 + 0.000001f)
{
xDegrees = 90;
yDegrees = 0;
zDegrees = radiansToDegrees(atan2f(-m_Matrix[1][0], m_Matrix[0][0]));
}
//Case 3: arcsin(sX) = -pi/2 (or, sX = -1). This solution is not unique,
//so the y-angle is set to 0.
else
{
xDegrees = -90;
yDegrees = 0;
zDegrees = radiansToDegrees(atan2f(-m_Matrix[1][0], m_Matrix[0][0]));
}
}
void GlobeGame::globePointToLatLong(const GlobeGame::Point3D &pt, int16 latOrigin, int16 longOrigin,
int16 &latitude, int16 &longitude) {
Point3D scratch = pt;
// Translate globe center to origin.
scratch.x -= kGlobeCenter.x;
scratch.y -= kGlobeCenter.y;
scratch.z -= kGlobeCenter.z;
// Rotate around z axis latOrigin degrees to bring equator parallel with XZ plane
float theta = degreesToRadians(latOrigin);
float s = sin(theta);
float c = cos(theta);
float x = scratch.x * c - scratch.y * s;
float y = scratch.y * c + scratch.x * s;
scratch.x = x;
scratch.y = y;
// Calculate latitude
latitude = (int16)radiansToDegrees(asin(scratch.y / kGlobeRadius));
// Rotate around y axis longOrigin degrees to bring longitude 0 to positive X axis
theta = degreesToRadians(longOrigin);
s = sin(theta);
c = cos(theta);
x = scratch.x * c - scratch.z * s;
float z = scratch.z * c + scratch.x * s;
scratch.x = x;
scratch.z = z;
// Calculate longitude
longitude = (int16)radiansToDegrees(acos(scratch.x / sqrt(scratch.x * scratch.x + scratch.z * scratch.z)));
if (scratch.z < 0)
longitude = -longitude;
}
void TextStruct::transform(const CTMatrix& transformationMatrix)
{
CBasesVector basesVector(m_pnt.x,m_pnt.y,1.,radiansToDegrees(m_angleRadians),m_mirror);
basesVector.transform(transformationMatrix);
m_pnt.x = (DbUnit)basesVector.getOrigin().x;
m_pnt.y = (DbUnit)basesVector.getOrigin().y;
double scale = basesVector.getScale();
m_height = (DbUnit)(m_height * scale);
m_width = (DbUnit)(m_width * scale);
m_angleRadians = (DbUnit)degreesToRadians(basesVector.getRotation());
m_mirror = basesVector.getMirror();
}
//----------------------------------------------------------------------------------------------------------------------
void CTRNNWheelViz::onMouseMove(const Vec4f& mPos)
{
m_selected = -1;
const float r3 = m_width / 2;
const float r2 = r3 - 5;
const float r1 = r2 - 10;
// update mouse info: transform mouse position into local space angle and radius
Vec4f posLocal = mPos - m_pTM->getTranslate();
float radius = posLocal.length();
float angle = radiansToDegrees(atan2(posLocal.x, posLocal.y));
if(angle < 0) angle += 360.0f;
if(radius > r1 && radius < r3)
{
m_selected = (int)angle / (360 / (int) m_ctrnn->getSize());
}
}
//
// GLfloat bearingSensor(cellID)
// Last modified: 08Nov2009
//
// Returns the relative bearing (in degrees) from the robot
// with the parameterized ID and this robot.
//
// Returns: the relative bearing from robot[ID] and this robot
// Parameters:
// cellID in/out the ID of the robot
GLfloat Robot::bearingSensor(GLint &cellID)
{
// get the vector between the auctioneer and this robot
Vector e = (*(Vector *)this) - (*(Vector *)env->getCell(cellID));
// get the distance between the auctioneer and this robot
GLfloat dist = e.magnitude();
// get the bearing between the auctioneer and this robot
GLfloat bearing = radiansToDegrees(atan2(e.y, e.x));
// subtract the bearing between the auction and this robot
// from the auctioneer's heading
bearing = env->getCell(cellID)->getHeading() - bearing;
return bearing;
} // bearingSensor(GLint &)
KDvoid AnalogStick::resetDirection ( KDvoid )
{
if ( m_tPressedVector.x == 0 && m_tPressedVector.y == 0 )
{
m_nDirection = AS_NO_DIRECTION;
return;
}
KDfloat fRadians = vectorToRadians ( m_tPressedVector );
KDfloat fDegrees = radiansToDegrees ( fRadians ) + 90;
if ( fDegrees >= 337.5f || fDegrees < 22.5f )
{
m_nDirection = AS_LEFT;
}
else if ( fDegrees >= 22.5f && fDegrees < 67.5f )
{
m_nDirection = AS_UP_LEFT;
}
else if ( fDegrees >= 67.5f && fDegrees < 112.5f )
{
m_nDirection = AS_UP;
}
else if ( fDegrees >= 112.5f && fDegrees < 157.5f )
{
m_nDirection = AS_UP_RIGHT;
}
else if ( fDegrees >= 157.5f && fDegrees < 202.5f )
{
m_nDirection = AS_RIGHT;
}
else if ( fDegrees >= 202.5f && fDegrees < 247.5f )
{
m_nDirection = AS_DOWN_RIGHT;
}
else if ( fDegrees >= 247.5f && fDegrees < 292.5f )
{
m_nDirection = AS_DOWN;
}
else
{
m_nDirection = AS_DOWN_LEFT;
}
}
/**
* The main task
*/
task main(){
nxtDisplayCenteredTextLine(0, "Dexter Ind.");
nxtDisplayCenteredBigTextLine(1, "IMU");
nxtDisplayCenteredTextLine(3, "Test 3");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
// If configuration fails, the program ends.
if (!DIMUconfigAccel(DIMU, DIMU_ACC_RANGE_2G))
{
PlaySound(soundException);
while(bSoundActive){}
StopAllTasks();
}
while(nNxtButtonPressed == kNoButton)
{
z_val = DIMUreadAccelZAxis10Bit(DIMU);
x_val = DIMUreadAccelXAxis10Bit(DIMU);
// Since the axes are constantly 90 degrees apart, we can use the sum of forces law,
// which looks like the pythagorean theorem, to discover the total force along both axes.
Gforce = sqrt(pow(z_val, 2) + pow(x_val, 2));
// Then we divide both values received by the total force to get numbers on the interval [-1,1].
// This way we can input them into the arcsine and arccosine functions.
z_val = z_val/Gforce;
x_val = x_val/Gforce;
pXrads[0] = asin(x_val);
pXrads[1] = PI-pXrads[0]; //other possible X tilt value.
pZrads[0] = acos(z_val);
pZrads[1] = -1*pZrads[0]; //other possible Z tilt value.
normalize();
displayArrow(radiansToDegrees(match()));
// This stops the screen from flashing.
wait1Msec(100);
}
}
task main ()
{
// This is the struct that holds all the info on all buttons and joypads/sticks
tPSP controller;
int angle;
int speed;
int rotation;
while (true)
{
// Read the state of the buttons
PSPV4readButtons(PSPNXV4, controller);
// Two controls are used:
// The left joystick controls speed and direction
// The right joystick controls rotational speed
// Calculate the angle we need to travel at using simple geometry
angle = radiansToDegrees(atan2(-controller.joystickLeft_x, -controller.joystickLeft_y));
// This is the length of the vector, which is based on the amount we've moved the
// joystick in the X and Y directions
speed = sqrt((controller.joystickLeft_x * controller.joystickLeft_x) +
(controller.joystickLeft_y*controller.joystickLeft_y));
// Rotation speed is controlled by the right joystick
rotation = -controller.joystickRight_x;
nxtDisplayCenteredTextLine(1, "%d:%d", controller.joystickLeft_x, controller.joystickLeft_y);
nxtDisplayCenteredTextLine(3, "%d:%d", controller.joystickRight_x, controller.joystickRight_y);
nxtDisplayCenteredTextLine(5, "%d", angle);
nxtDisplayCenteredTextLine(6, "%d", speed);
nxtDisplayCenteredTextLine(7, "%d", rotation);
MoveRobot(angle, speed, rotation);
wait1Msec(100);
}
PlaySound(soundBeepBeep);
MoveRobot(0, 0, 0);
while (bSoundActive) EndTimeSlice();
wait1Msec(100);
}
//----Determine rotation of robot from encoder values----//
int getRotation()
{
float motorBCount = nMotorEncoder[motorB];
float motorCCount = nMotorEncoder[motorC];
motorBCount /= 190; //distance between wheels in encoder clicks
motorCCount /= 190;
int thetaOne;
int thetaTwo;
int thetaThree;
if(motorBCount<0)
{
motorBCount *= -1;
thetaOne = radiansToDegrees(atan(motorBCount));
thetaTwo = radiansToDegrees(atan(motorCCount));
return -(thetaOne + thetaTwo);
}
else if(motorCCount<0)
{
motorCCount *= -1;
thetaOne = radiansToDegrees(atan(motorBCount));
thetaTwo = radiansToDegrees(atan(motorCCount));
return (thetaOne + thetaTwo);
}
else if(motorCCount < 0 && motorBCount < 0)
{
motorBCount *= -1;
motorCCount *= -1;
thetaOne = radiansToDegrees(atan(motorBCount));
thetaTwo = radiansToDegrees(atan(motorCCount));
return (thetaOne + thetaTwo);
}
else
{
thetaOne = radiansToDegrees(atan(motorBCount));
thetaTwo = radiansToDegrees(atan(motorCCount));
return (thetaOne - thetaTwo);
}
return thetaThree;
}
//----------------------------------------------------------------------------------------------------------------------
void CTRNNViz::drawNeuron(int i, const CTRNN* ctrnn, float inner, float outer)
{
// white background
glColor3f(1,1,1);
drawDisk(outer, 0.0, 32, 1);
// disk size indicating output value
ci::Color col = ctrnn->getState(i) > 0 ? ci::Color(0,0,0) : ci::Color(235.0/255.0, 0.0/255.0, 103.0/255.0);
if (ctrnn->getOutput(i) < 0) col = ci::Color(235.0/255.0, 0.0/255.0, 103.0/255.0);
ci::gl::color(col);
drawDisk(inner * clamp(getOutputNorm(i, ctrnn), 0.0, 1.0), 0.0, 32, 1);
// ring size indicating external input value
//glColor3f(181.0/255.0, 206.0/255.0, 26.0/255.0);
col = ctrnn->getExternalInput(i) > 0 ? ci::Color(185/255.0, 185/255.0, 185/255.0) : ci::Color(235.0/255.0, 205.0/255.0, 221.0/255.0);
ci::gl::color(col);
float width = radiansToDegrees(ctrnn->getExternalInput(i) * TWO_PI);
dmx::drawPartialDisk(inner, outer, 32, 1, 0, width, GLU_FILL);
glColor3f(0,0,0);
ci::gl::drawStrokedCircle(ci::Vec2f(0,0), outer, 32);
ci::gl::drawStrokedCircle(ci::Vec2f(0,0), inner, 32);
}
