void PartialDisque::BuildAndDisplay()
{
// Génération du cylindre partiel exterieur
_cylExtPartiel->FastDisplay();
// Génération du cylindre interieur
if(_diametreInterieur != 0){
GLUquadric* paramsCylInt = gluNewQuadric();
gluCylinder(paramsCylInt, _diametreInterieur/2, _diametreInterieur/2,_height,_slices,1);
}
// Géréation des 2 disques
GLUquadric* paramsDiscH = gluNewQuadric();
gluPartialDisk(paramsDiscH,_diametreInterieur/2,_diametreExterieur/2,_slices,1, 0, _angle);
gluDisk(paramsDiscH,0,_diametreInterieur/2,_slices,1);
glPushMatrix();
glTranslatef(0.0, 0.0, _height);
GLUquadric* paramsDiscB = gluNewQuadric();
gluPartialDisk(paramsDiscH,_diametreInterieur/2,_diametreExterieur/2,_slices,1, 0, _angle);
gluDisk(paramsDiscH,0,_diametreInterieur/2,_slices,1);
glPopMatrix();
float angle = DEGREES_TO_RADIANS(15);
// Pattern
Point3D patternFlanc[2], flanc[4];
patternFlanc[0] = Point3D(0, -_diametreExterieur/2, 0);
patternFlanc[1] = Point3D(0, -_diametreExterieur/2, _height);
for(int i=0; i<4; i++)
flanc[i] = Point3D(patternFlanc[i%2]._x * cosf((i>1)?-angle:angle) - patternFlanc[i%2]._y * sinf((i>1)?-angle:angle), patternFlanc[i%2]._x * sinf((i>1)?-angle:angle) + patternFlanc[i%2]._y * cosf((i>1)?-angle:angle), patternFlanc[i%2]._z);
}
static INLINE VGboolean
try_to_fix_radii(struct arc *arc)
{
double COS, SIN, rot, x0p, y0p, x1p, y1p;
double dx, dy, dsq, scale;
/* Convert rotation angle from degrees to radians */
rot = DEGREES_TO_RADIANS(arc->theta);
/* Pre-compute rotation matrix entries */
COS = cos(rot); SIN = sin(rot);
/* Transform (x0, y0) and (x1, y1) into unit space */
/* using (inverse) rotate, followed by (inverse) scale */
x0p = (arc->x1*COS + arc->y1*SIN)/arc->a;
y0p = (-arc->x1*SIN + arc->y1*COS)/arc->b;
x1p = (arc->x2*COS + arc->y2*SIN)/arc->a;
y1p = (-arc->x2*SIN + arc->y2*COS)/arc->b;
/* Compute differences and averages */
dx = x0p - x1p;
dy = y0p - y1p;
dsq = dx*dx + dy*dy;
#if 0
if (dsq <= 0.001) {
debug_printf("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAaaaaa\n");
}
#endif
scale = 1/(2/sqrt(dsq));
arc->a *= scale;
arc->b *= scale;
return VG_TRUE;
}
/*virtual*/ void WBCompEldSensorVision::InitializeFromDefinition( const SimpleString& DefinitionName )
{
Super::InitializeFromDefinition( DefinitionName );
MAKEHASH( DefinitionName );
STATICHASH( EyeOffsetZ );
m_EyeOffsetZ = ConfigManager::GetInheritedFloat( sEyeOffsetZ, 0.0f, sDefinitionName );
STATICHASH( Radius );
m_RadiusSq = Square( ConfigManager::GetInheritedFloat( sRadius, 0.0f, sDefinitionName ) );
STATICHASH( ConeAngle );
m_ConeCos = Cos( DEGREES_TO_RADIANS( ConfigManager::GetInheritedFloat( sConeAngle, 0.0f, sDefinitionName ) ) );
STATICHASH( ConeScaleZ );
const float ConeScaleZ = ConfigManager::GetInheritedFloat( sConeScaleZ, 0.0f, sDefinitionName );
m_ConeInvZScale = ( ConeScaleZ > 0.0f ) ? ( 1.0f / ConeScaleZ ) : ConeScaleZ;
STATICHASH( CertaintyFalloffRadius );
m_CertaintyFalloffRadius = ConfigManager::GetInheritedFloat( sCertaintyFalloffRadius, 0.0f, sDefinitionName );
STATICHASH( DistanceCertaintyFactor );
m_DistanceCertaintyFactor = ConfigManager::GetInheritedFloat( sDistanceCertaintyFactor, 0.0f, sDefinitionName );
STATICHASH( CertaintyVelocity );
m_CertaintyVelocity = ConfigManager::GetInheritedFloat( sCertaintyVelocity, 0.0f, sDefinitionName );
STATICHASH( CertaintyDecay );
m_CertaintyDecay = ConfigManager::GetInheritedFloat( sCertaintyDecay, 0.0f, sDefinitionName );
}
SimpleCamera::SimpleCamera(const VECTOR3& startPosition, const float_t startHeadingRadians)
: Camera(),
_startHeadingRadians(startHeadingRadians),
_maxPitchRate(DEGREES_TO_RADIANS(2.0f)),
_maxHeadingRate(DEGREES_TO_RADIANS(16.0f)),
_maxForwardVelocity(100.0f)
{
_hasChangedParameters = true;
_up = Vec3(0.0f, 1.0f, 0.0f);
_startPosition.x = startPosition.x;
_startPosition.y = startPosition.y;
_startPosition.z = startPosition.z;
reset();
}
DrawCircle::DrawCircle(double radius){
this->radius = radius;
vertexCircle vertexArray[360];
double posX, posY;
for(int cpt = 0; cpt<360; cpt++){
posX = this->radius * (cos(DEGREES_TO_RADIANS(cpt)));
posY = this->radius * (sin(DEGREES_TO_RADIANS(cpt)));
setVertex(vertexArray[cpt], posX, posY);
}
glGenBuffers(1,&VCbuffer);
glBindBuffer(GL_ARRAY_BUFFER,VCbuffer);
glBufferData(GL_ARRAY_BUFFER,sizeof(vertexArray),vertexArray,GL_STATIC_DRAW);
}
PointCloudRenderer::PointCloudRenderer(int imageWidth, int imageHeight, int pixelRadius, real theta) :
m_image(new Image(imageWidth, imageHeight, 3)),
m_transform(),
m_pixelRadius(pixelRadius),
m_depthBuffer(imageWidth * imageHeight, FLT_MAX)
{
float fovY = DEGREES_TO_RADIANS(45.0f);
float aspectRatio = imageWidth / static_cast<float>(imageHeight);
float yScale = static_cast<float>(1.0 / tan(fovY * 0.5));
float xScale = yScale / aspectRatio;
float zNear = 1;
float zFar = 1000;
// Set the background color
memset(m_image->getPixels(), 0xaa, m_image->getPixelBufferSize());
Eigen::Matrix4f projection;
projection <<
xScale, 0, 0, 0,
0, yScale, 0, 0,
0, 0, (zNear + zFar)/(zNear - zFar), -1,
0, 0, (-2 * zNear * zFar)/(zNear - zFar), 0;
Setup * setup = Setup::get();
Eigen::Affine3f translation(Eigen::Translation3f(setup->cameraLocation.x, -1.0 * setup->cameraLocation.y, -1.0 * setup->cameraLocation.z));
Eigen::Affine3f rotation(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitY()));
Eigen::Matrix4f modelView = translation.matrix() * rotation.matrix();
m_transform = projection * modelView;
}
mat4* mat4_from_rotation(float x, float y, float z, float angle){
float c = cos(DEGREES_TO_RADIANS(angle));
//printf("c = %f from angle = %f\n", c, angle);
float s = sin(DEGREES_TO_RADIANS(angle));
float t = 1.0 - c;
mat4* rot_mat = mat4_create_id();
mat4_set_member(0, 'x', t * pow(x, 2) + c, rot_mat);
mat4_set_member(0, 'y', (t * x * y) + (z * s), rot_mat);
mat4_set_member(0, 'z', (t * x * z) - (y * s), rot_mat);
mat4_set_member(1, 'x', (t * x * y) - (z * s), rot_mat);
mat4_set_member(1, 'y', t * pow(y, 2) + c, rot_mat);
mat4_set_member(1, 'z', (t * y * z) + (x * s), rot_mat);
mat4_set_member(2, 'x', (t * x * z) + (y * s), rot_mat);
mat4_set_member(2, 'y', (t * y * z) - (x * s), rot_mat);
mat4_set_member(2, 'z', t * pow(z, 2) + c, rot_mat);
return rot_mat;
}
bool TransformComponent::Init(TiXmlElement* pData)
{
CB_ASSERT(pData);
Vec3 yawPitchRoll = m_Transform.GetYawPitchRoll();
yawPitchRoll.x = RADIANS_TO_DEGREES(yawPitchRoll.x);
yawPitchRoll.y = RADIANS_TO_DEGREES(yawPitchRoll.y);
yawPitchRoll.z = RADIANS_TO_DEGREES(yawPitchRoll.z);
Vec3 position = m_Transform.GetPosition();
TiXmlElement* pPositionElement = pData->FirstChildElement("Position");
if (pPositionElement)
{
double x = 0;
double y = 0;
double z = 0;
pPositionElement->Attribute("x", &x);
pPositionElement->Attribute("y", &y);
pPositionElement->Attribute("z", &z);
position = Vec3(x, y, z);
}
TiXmlElement* pOrientationElement = pData->FirstChildElement("YawPitchRoll");
if (pOrientationElement)
{
double yaw = 0;
double pitch = 0;
double roll = 0;
pOrientationElement->Attribute("x", &yaw);
pOrientationElement->Attribute("y", &pitch);
pOrientationElement->Attribute("z", &roll);
yawPitchRoll = Vec3(yaw, pitch, roll);
}
Mat4x4 translation;
translation.BuildTranslation(position);
Mat4x4 rotation;
rotation.BuildYawPitchRoll((float)DEGREES_TO_RADIANS(yawPitchRoll.x), (float)DEGREES_TO_RADIANS(yawPitchRoll.y), (float)DEGREES_TO_RADIANS(yawPitchRoll.z));
m_Transform = rotation * translation;
return true;
}
void find4thPoint(vector<float>& p4,
vector<float>& p1, vector<float>& p2, vector<float>& p3,
float dist, float ang, float dihed) {
vector<float> n1(3), n2(3), a(3), b(3);
point_sub(a, p1,p2); point_sub(b, p3,p2);
cross_product(n1, a,b); normalize_point(n1,n1);
cross_product(n2, b,n1); normalize_point(n2,n2);
double Sang = sin( DEGREES_TO_RADIANS(ang) );
double Cang = cos( DEGREES_TO_RADIANS(ang) );
double Sdihed = sin( DEGREES_TO_RADIANS(dihed) );
double Cdihed = cos( DEGREES_TO_RADIANS(dihed) );
normalize_point(b,b);
//cout << point_string(b) << point_string(n1) << point_string(n2) << endl;
linear_combination(p4, Sang*Cdihed, n2, -1*Cang, b);
linear_combination(p4, -1*Sang*Sdihed, n1, 1, p4);
//cout << point_string(p4) << endl;
linear_combination(p4, dist, p4, 1, p3);
}
INTERNAL v2 *createAsteroidVertexList(MemoryArena_ *arena, i32 iterations,
i32 asteroidRadius)
{
f32 iterationAngle = 360.0f / iterations;
iterationAngle = DEGREES_TO_RADIANS(iterationAngle);
v2 *result = memory_pushBytes(arena, iterations * sizeof(v2));
for (i32 i = 0; i < iterations; i++)
{
i32 randValue = rand();
// NOTE(doyle): Sin/cos generate values from +-1, we want to create
// vertices that start from 0, 0 (i.e. strictly positive)
result[i] = V2(((math_cosf(iterationAngle * i) + 1) * asteroidRadius),
((math_sinf(iterationAngle * i) + 1) * asteroidRadius));
ASSERT(result[i].x >= 0 && result[i].y >= 0);
#if 1
f32 displacementDist = 0.50f * asteroidRadius;
i32 vertexDisplacement =
randValue % (i32)displacementDist + (i32)(displacementDist * 0.25f);
i32 quadrantSize = iterations / 4;
i32 firstQuadrant = quadrantSize;
i32 secondQuadrant = quadrantSize * 2;
i32 thirdQuadrant = quadrantSize * 3;
i32 fourthQuadrant = quadrantSize * 4;
if (i < firstQuadrant)
{
result[i].x += vertexDisplacement;
result[i].y += vertexDisplacement;
}
else if (i < secondQuadrant)
{
result[i].x -= vertexDisplacement;
result[i].y += vertexDisplacement;
}
else if (i < thirdQuadrant)
{
result[i].x -= vertexDisplacement;
result[i].y -= vertexDisplacement;
}
else
{
result[i].x += vertexDisplacement;
result[i].y -= vertexDisplacement;
}
#endif
}
return result;
}
void PhysicsComponent::BuildRigidBodyTransform(TiXmlElement* pTransformElement)
{
// FUTURE WORK Mrmike - this should be exactly the same as the TransformComponent - maybe factor into a helper method?
GCC_ASSERT(pTransformElement);
TiXmlElement* pPositionElement = pTransformElement->FirstChildElement("Position");
if (pPositionElement)
{
double x = 0;
double y = 0;
double z = 0;
pPositionElement->Attribute("x", &x);
pPositionElement->Attribute("y", &y);
pPositionElement->Attribute("z", &z);
m_RigidBodyLocation = Vec3(x, y, z);
}
TiXmlElement* pOrientationElement = pTransformElement->FirstChildElement("Orientation");
if (pOrientationElement)
{
double yaw = 0;
double pitch = 0;
double roll = 0;
pPositionElement->Attribute("yaw", &yaw);
pPositionElement->Attribute("pitch", &pitch);
pPositionElement->Attribute("roll", &roll);
m_RigidBodyOrientation = Vec3((float)DEGREES_TO_RADIANS(yaw), (float)DEGREES_TO_RADIANS(pitch), (float)DEGREES_TO_RADIANS(roll));
}
TiXmlElement* pScaleElement = pTransformElement->FirstChildElement("Scale");
if (pScaleElement)
{
double x = 0;
double y = 0;
double z = 0;
pScaleElement->Attribute("x", &x);
pScaleElement->Attribute("y", &y);
pScaleElement->Attribute("z", &z);
m_RigidBodyScale = Vec3((float)x, (float)y, (float)z);
}
}
void PhysicsComponent::BuildRigidBodyTransform(tinyxml2::XMLElement* pTransformElement)
{
// FUTURE WORK Mrmike - this should be exactly the same as the TransformComponent - maybe factor into a helper method?
LOG_ASSERT(pTransformElement);
tinyxml2::XMLElement* pPositionElement = pTransformElement->FirstChildElement("Position");
if (pPositionElement)
{
double x = 0;
double y = 0;
double z = 0;
x = std::stod(pPositionElement->Attribute("x"));
y = std::stod(pPositionElement->Attribute("y"));
z = std::stod(pPositionElement->Attribute("z"));
m_RigidBodyLocation = glm::vec3(x, y, z);
}
tinyxml2::XMLElement* pOrientationElement = pTransformElement->FirstChildElement("Orientation");
if (pOrientationElement)
{
double yaw = 0;
double pitch = 0;
double roll = 0;
yaw = std::stod(pOrientationElement->Attribute("yaw"));
pitch = std::stod(pOrientationElement->Attribute("pitch"));
roll = std::stod(pOrientationElement->Attribute("roll"));
m_RigidBodyOrientation = glm::vec3((float)DEGREES_TO_RADIANS(yaw), (float)DEGREES_TO_RADIANS(pitch), (float)DEGREES_TO_RADIANS(roll));
}
tinyxml2::XMLElement* pScaleElement = pTransformElement->FirstChildElement("Scale");
if (pScaleElement)
{
double x = 0;
double y = 0;
double z = 0;
x = std::stod(pScaleElement->Attribute("x"));
y = std::stod(pScaleElement->Attribute("y"));
z = std::stod(pScaleElement->Attribute("z"));
m_RigidBodyScale = glm::vec3((float)x, (float)y, (float)z);
}
}
void SimpleCamera::changePitch(const float_t degrees)
{
float_t changeValue = DEGREES_TO_RADIANS(degrees);
if(fabs(changeValue) < fabs(_maxPitchRate))
{
// Our pitch is less than the max pitch rate that we
// defined so lets increment it.
_pitchRadians += changeValue;
}
else
{
// Our pitch is greater than the max pitch rate that
// we defined so we can only increment our pitch by the
// maximum allowed value.
if(changeValue < 0)
{
// We are pitching down so decrement
_pitchRadians -= _maxPitchRate;
}
else
{
// We are pitching up so increment
_pitchRadians += _maxPitchRate;
}
}
// We don't want our pitch to run away from us. Although it
// really doesn't matter I prefer to have my pitch degrees
// within the range of -360.0f to 360.0f
if(_pitchRadians > DEGREES_TO_RADIANS(360.0f))
{
_pitchRadians -= DEGREES_TO_RADIANS(360.0f);
}
else if(_pitchRadians < DEGREES_TO_RADIANS(-360.0f))
{
_pitchRadians += DEGREES_TO_RADIANS(360.0f);
}
_hasChangedParameters = true;
}
void my_set_empty_circle(t_bunny_pixelarray *pix,
t_bunny_position *origin,
t_bunny_position *dist,
unsigned int color)
{
double radius;
double angle;
t_bunny_position pos;
angle = 0;
dist->x = dist->x - origin->x;
dist->y = dist->y - origin->y;
radius = sqrt(pow(dist->x, 2) + pow(dist->y, 2));
while (angle < 360)
{
pos.x = cos(DEGREES_TO_RADIANS(angle)) * radius + origin->x;
pos.y = sin(DEGREES_TO_RADIANS(angle)) * radius + origin->y;
if (pos.x > 0 && pos.x < pix->clipable.clip_width &&
pos.y > 0 && pos.y < pix->clipable.clip_height)
set_pixel(pix, color, pos.x, pos.y);
angle += 0.1;
}
}
void FireTower::fireAtCreep(Creep *creep)
{
if (m_fTimeSinceLastShot > m_fCooldownTime)
{
float radians = DEGREES_TO_RADIANS(m_fAngle);
float cos = cosf(radians);
float sin = sinf(radians);
World::getInstance()->getFireBolts().push_back(std::unique_ptr<FireBolt>(new FireBolt(m_position->getX() + (cos * 0.275f), m_position->getY() + (sin * 0.275f), m_fProjectileWidth, m_fProjectileHeight, m_fProjectileSpeed, m_fAngle, 4.0f + getStage(), m_iProjectileDamage)));
m_fTimeSinceLastShot = 0;
GameListener::getInstance()->playSound(SOUND_FIRE_BOLT);
}
}
void ElectricTower::fireAtCreep(Creep *creep)
{
if (m_fTimeSinceLastShot > m_fCooldownTime)
{
GameListener::getInstance()->playSound(SOUND_ELECTRIC_BOLT);
float radians = DEGREES_TO_RADIANS(m_fAngle);
float cos = cosf(radians);
float sin = sinf(radians);
if (getStage() == 0 || getStage() == 2)
{
World::getInstance()->getElectricBolts().push_back(std::unique_ptr<ElectricBolt>(new ElectricBolt(m_position->getX() + (cos * 0.40f), m_position->getY() + (sin * 0.40f), m_fProjectileWidth, m_fProjectileHeight, m_fProjectileSpeed, m_fAngle, m_iProjectileDamage, m_iFadeTimeSeconds)));
}
if (getStage() >= 1)
{
float alteredAngle = m_fAngle + 90;
if (alteredAngle >= 360)
{
alteredAngle -= 360;
}
float alteredRadians = DEGREES_TO_RADIANS(alteredAngle);
float alteredCos = cosf(alteredRadians);
float alteredSin = sinf(alteredRadians);
float xOffset = alteredCos * 0.225f;
float yOffset = alteredSin * 0.225f;
World::getInstance()->getElectricBolts().push_back(std::unique_ptr<ElectricBolt>(new ElectricBolt(m_position->getX() + (cos * 0.52f) + xOffset, m_position->getY() + (sin * 0.52f) + yOffset, m_fProjectileWidth, m_fProjectileHeight, m_fProjectileSpeed, m_fAngle, m_iProjectileDamage, m_iFadeTimeSeconds)));
World::getInstance()->getElectricBolts().push_back(std::unique_ptr<ElectricBolt>(new ElectricBolt(m_position->getX() + (cos * 0.52f) - xOffset, m_position->getY() + (sin * 0.52f) - yOffset, m_fProjectileWidth, m_fProjectileHeight, m_fProjectileSpeed, m_fAngle, m_iProjectileDamage, m_iFadeTimeSeconds)));
}
m_fTimeSinceLastShot = 0;
}
}
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{
float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X;
float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y;
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
&& (distanceToActualTarget <= (posControl.navConfig->fw_loiter_radius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f);
// Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) {
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f);
float loiterTargetX = posControl.desiredState.pos.V.X + posControl.navConfig->fw_loiter_radius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.V.Y + posControl.navConfig->fw_loiter_radius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - posControl.actualState.pos.V.X;
posErrorY = loiterTargetY - posControl.actualState.pos.V.Y;
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
}
// Calculate virtual waypoint
virtualDesiredPosition.V.X = posControl.actualState.pos.V.X + posErrorX * (trackingDistance / distanceToActualTarget);
virtualDesiredPosition.V.Y = posControl.actualState.pos.V.Y + posErrorY * (trackingDistance / distanceToActualTarget);
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
if (posControl.flags.isAdjustingPosition) {
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband);
if (rcRollAdjustment) {
float rcShiftY = rcRollAdjustment * posControl.navConfig->max_manual_speed / 500.0f * trackingPeriod;
// Rotate this target shift from body frame to to earth frame and apply to position target
virtualDesiredPosition.V.X += -rcShiftY * posControl.actualState.sinYaw;
virtualDesiredPosition.V.Y += rcShiftY * posControl.actualState.cosYaw;
posControl.flags.isAdjustingPosition = true;
}
}
}
bool OverlapTester::doRectanglesOverlap(Rectangle &r1, Rectangle &r2)
{
if (r1.getAngle() > 0)
{
float halfWidth = r1.getWidth() / 2;
float halfHeight = r1.getHeight() / 2;
float rad = DEGREES_TO_RADIANS(r1.getAngle());
float cos = cosf(rad);
float sin = sinf(rad);
float x1 = -halfWidth * cos - (-halfHeight) * sin;
float y1 = -halfWidth * sin + (-halfHeight) * cos;
float x2 = halfWidth * cos - (-halfHeight) * sin;
float y2 = halfWidth * sin + (-halfHeight) * cos;
float x3 = halfWidth * cos - halfHeight * sin;
float y3 = halfWidth * sin + halfHeight * cos;
float x4 = -halfWidth * cos - halfHeight * sin;
float y4 = -halfWidth * sin + halfHeight * cos;
float x = r1.getLowerLeft().getX() + r1.getWidth() / 2;
float y = r1.getLowerLeft().getY() + r1.getHeight() / 2;
x1 += x;
y1 += y;
x2 += x;
y2 += y;
x3 += x;
y3 += y;
x4 += x;
y4 += y;
return isPointInRectangle(Vector2D(x1, y1), r2) || isPointInRectangle(Vector2D(x2, y2), r2) || isPointInRectangle(Vector2D(x3, y3), r2) || isPointInRectangle(Vector2D(x4, y4), r2);
}
else
{
return (r1.getLowerLeft().getX() < r2.getLowerLeft().getX() + r2.getWidth() && r1.getLowerLeft().getX() + r1.getWidth() > r2.getLowerLeft().getX() && r1.getLowerLeft().getY() < r2.getLowerLeft().getY() + r2.getHeight() && r1.getLowerLeft().getY() + r1.getHeight() > r2.getLowerLeft().getY());
}
}
void Direct3DCircleBatcher::renderCircle(Circle &circle, Color &c, GpuProgramWrapper &gpuProgramWrapper)
{
m_iNumPoints = 0;
D3DManager->m_colorVertices.clear();
for (int i = 0; i <= 360; i += DEGREE_SPACING)
{
float rad = DEGREES_TO_RADIANS(i);
float cos = cosf(rad);
float sin = sinf(rad);
addVertexCoordinate(cos * circle.m_fRadius + circle.getCenter().getX(), sin * circle.m_fRadius + circle.getCenter().getY(), 0, c.red, c.green, c.blue, c.alpha, 0, 0);
addVertexCoordinate(circle.getCenter().getX(), circle.getCenter().getY(), 0, c.red, c.green, c.blue, c.alpha, 0, 0);
}
endBatch(gpuProgramWrapper);
}
static
POINT
FASTCALL
app_boundary_point(Rect r, int angle)
{
int cx, cy;
double tangent;
cx = r.width;
cx /= 2;
cx += r.x;
cy = r.height;
cy /= 2;
cy += r.y;
if (angle == 0)
return pt(r.x+r.width, cy);
else if (angle == 45)
return pt(r.x+r.width, r.y);
else if (angle == 90)
return pt(cx, r.y);
else if (angle == 135)
return pt(r.x, r.y);
else if (angle == 180)
return pt(r.x, cy);
else if (angle == 225)
return pt(r.x, r.y+r.height);
else if (angle == 270)
return pt(cx, r.y+r.height);
else if (angle == 315)
return pt(r.x+r.width, r.y+r.height);
tangent = tan(DEGREES_TO_RADIANS(angle));
if ((angle > 45) && (angle < 135))
return pt((int)(cx+r.height/tangent/2), r.y);
else if ((angle > 225) && (angle < 315))
return pt((int)(cx-r.height/tangent/2), r.y+r.height);
else if ((angle > 135) && (angle < 225))
return pt(r.x, (int)(cy+r.width*tangent/2));
else
return pt(r.x+r.width, (int)(cy-r.width*tangent/2));
}
void SimpleCamera::setFrustum(uint16_t width, uint16_t height, float_t fovInDegree, float_t nearPlane, float_t farPlane)
{
_fovInRad = DEGREES_TO_RADIANS(fovInDegree);
_zNear = nearPlane;
_zFar = farPlane;
_screenWidth = width;
_screenHeight = height;
_aspectRatio = float_t(_screenWidth) / float_t(_screenHeight);
MatrixPerspectiveFovRH(
_projectionMatrix,
_fovInRad,
_aspectRatio,
_zNear,
_zFar,
0);
_hasChangedParameters = true;
}
void EldritchFramework::CreateMinimapView() {
PRINTF("EldritchFramework::CreateMinimapView\n");
SafeDelete(m_MinimapView);
STATICHASH(EldMinimap);
STATICHASH(MinimapRTWidth);
const float fMinimapRTWidth =
ConfigManager::GetFloat(sMinimapRTWidth, 0.0f, sEldMinimap);
STATICHASH(MinimapRTHeight);
const float fMinimapRTHeight =
ConfigManager::GetFloat(sMinimapRTHeight, 0.0f, sEldMinimap);
STATICHASH(MinimapViewDistance);
const float ViewDistance =
ConfigManager::GetFloat(sMinimapViewDistance, 0.0f, sEldMinimap);
STATICHASH(MinimapViewPitch);
const float ViewPitch = DEGREES_TO_RADIANS(
ConfigManager::GetFloat(sMinimapViewPitch, 0.0f, sEldMinimap));
STATICHASH(MinimapViewFOV);
const float FOV = ConfigManager::GetFloat(sMinimapViewFOV, 0.0f, sEldMinimap);
STATICHASH(MinimapViewNearClip);
const float NearClip =
ConfigManager::GetFloat(sMinimapViewNearClip, 0.0f, sEldMinimap);
STATICHASH(MinimapViewFarClip);
const float FarClip =
ConfigManager::GetFloat(sMinimapViewFarClip, 0.0f, sEldMinimap);
const Angles EyeOrientation = Angles(ViewPitch, 0.0f, 0.0f);
const Vector EyeDirection = EyeOrientation.ToVector();
const Vector EyePosition = -EyeDirection * ViewDistance;
const float AspectRatio = fMinimapRTWidth / fMinimapRTHeight;
m_MinimapView = new View(EyePosition, EyeOrientation, FOV, AspectRatio,
NearClip, FarClip);
}
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
{
const float swingVelocity = (ABS(imuMeasuredRotationBF.A[Z]) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.A[Y] / imuMeasuredRotationBF.A[Z]) : 0;
const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.A[X] > navConfig()->fw.launch_accel_thresh);
const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.A[X] > 0);
if (isBungeeLaunched || isSwingLaunched) {
launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviosUpdate);
launchState.launchDetectorPreviosUpdate = currentTimeUs;
if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) {
launchState.launchDetected = true;
}
}
else {
launchState.launchDetectorPreviosUpdate = currentTimeUs;
launchState.launchDetectionTimeAccum = 0;
}
}
static void createEllipsePath(CGContextRef context, CGPoint center,
CGSize ellipseSize)
{
CGContextSaveGState(context);
// Translate the coordinate origin to the center point.
CGContextTranslateCTM(context, center.x, center.y);
// Scale the coordinate system to half the width and height
// of the ellipse.
CGContextScaleCTM(context,
ellipseSize.width/2, ellipseSize.height/2);
CGContextBeginPath(context);
// Add a circular arc to the path, centered at the origin and
// with a radius of 1.0. This radius, together with the
// scaling above for the width and height, produces an ellipse
// of the correct size.
CGContextAddArc(context, 0.0, 0.0, 1.0, 0.0, DEGREES_TO_RADIANS(360.0), 0.0);
// Close the path so that this path is suitable for stroking,
// should that be desired.
CGContextClosePath(context);
CGContextRestoreGState(context);
}
void PlaneDynamicGameObject::ascend()
{
if(!m_isHit && !m_isDead)
{
m_velocity->set(0, 24.0f);
m_fTimeSinceFlap = 0;
m_fAngle = 384;
float radians = DEGREES_TO_RADIANS(m_fAngle);
float cos = cosf(radians);
float sin = sinf(radians);
float height = getHeight() / 3 * 2;
m_puffClouds.push_back(std::unique_ptr<PuffCloud>(new PuffCloud(getPosition().getX() - (cos * 2.4f), getPosition().getY() - (sin * 2.4f), height * 1.33f, height, m_fAngle)));
Assets::getInstance()->setCurrentSoundId(ASCEND_SOUND);
}
}
// as mentioned in AxisRotation.pdf in the same directory
void findRotnOperator(vector<vector<float> > & op, vector<float> & axis, float angle) {
op.resize(3); op[0].resize(3); op[1].resize(3); op[2].resize(3);
vector<float> n(3);
normalize_point(n, axis);
angle = DEGREES_TO_RADIANS(angle);
float ct = cos(angle), st = sin(angle);
op[0][0] = n[0]*n[0] + ct * (n[1]*n[1] + n[2]*n[2]);
op[1][1] = n[1]*n[1] + ct * (n[2]*n[2] + n[0]*n[0]);
op[2][2] = n[2]*n[2] + ct * (n[0]*n[0] + n[1]*n[1]);
op[0][1] = n[0]*n[1]*(1-ct) - n[2]*st;
op[1][0] = n[0]*n[1]*(1-ct) + n[2]*st;
op[0][2] = n[0]*n[2]*(1-ct) + n[1]*st;
op[2][0] = n[0]*n[2]*(1-ct) - n[1]*st;
op[1][2] = n[1]*n[2]*(1-ct) - n[0]*st;
op[2][1] = n[1]*n[2]*(1-ct) + n[0]*st;
}
int PacketHandler::pickup_spawn(User *user)
{
//uint32_t curpos = 4; //warning: unused variable ‘curpos’
spawnedItem item;
item.health = 0;
int8_t yaw, pitch, roll;
user->buffer >> (int32_t&)item.EID;
user->buffer >> (int16_t&)item.item >> (int8_t&)item.count >> (int16_t&)item.health;
user->buffer >> (int32_t&)item.pos.x() >> (int32_t&)item.pos.y() >> (int32_t&)item.pos.z();
user->buffer >> yaw >> pitch >> roll;
if(!user->buffer)
{
return PACKET_NEED_MORE_DATA;
}
user->buffer.removePacket();
item.EID = generateEID();
item.spawnedBy = user->UID;
item.spawnedAt = time(NULL);
// Modify the position of the dropped item so that it appears in front of user instead of under user
int distanceToThrow = 64;
float angle = DEGREES_TO_RADIANS(user->pos.yaw + 45); // For some reason, yaw seems to be off to the left by 45 degrees from where you're actually looking?
int x = int(cos(angle) * distanceToThrow - sin(angle) * distanceToThrow);
int z = int(sin(angle) * distanceToThrow + cos(angle) * distanceToThrow);
item.pos += vec(x, 0, z);
Mineserver::get()->map(user->pos.map)->sendPickupSpawn(item);
return PACKET_OK;
}
bool TransformComponent::VInit(tinyxml2::XMLElement* pData)
{
LOG_ASSERT(pData);
// [mrmike] - this was changed post-press - because changes to the TransformComponents can come in partial definitions,
// such as from the editor, its better to grab the current values rather than clear them out.
/*glm::vec3 yawPitchRoll = GetYawPitchRoll(m_transform);
yawPitchRoll.x = RADIANS_TO_DEGREES(yawPitchRoll.x);
yawPitchRoll.y = RADIANS_TO_DEGREES(yawPitchRoll.y);
yawPitchRoll.z = RADIANS_TO_DEGREES(yawPitchRoll.z);*/
glm::vec3 position = ::GetPosition(m_transform);
glm::mat4 rotation = ::GetRotMat(m_transform);
tinyxml2::XMLElement* pPositionElement = pData->FirstChildElement("Position");
if (pPositionElement)
{
double x = 0;
double y = 0;
double z = 0;
x = std::stod(pPositionElement->Attribute("x"));
y = std::stod(pPositionElement->Attribute("y"));
z = std::stod(pPositionElement->Attribute("z"));
position = glm::vec3(x, y, z);
}
tinyxml2::XMLElement* pOrientationElement = pData->FirstChildElement("YawPitchRoll");
if (pOrientationElement)
{
double yaw = 0;
double pitch = 0;
double roll = 0;
yaw = std::stod(pOrientationElement->Attribute("x"));
pitch = std::stod(pOrientationElement->Attribute("y"));
roll = std::stod(pOrientationElement->Attribute("z"));
glm::vec3 eulers = glm::vec3(yaw, pitch, roll);
glm::quat tmpQuat = glm::quat(glm::vec3((float)DEGREES_TO_RADIANS(eulers.x), (float)DEGREES_TO_RADIANS(eulers.y), (float)DEGREES_TO_RADIANS(eulers.z)));
rotation = glm::toMat4(tmpQuat);
}
/**
// This is not supported yet.
tinyxml2::XMLElement* pLookAtElement = pData->FirstChildElement("LookAt");
if (pLookAtElement)
{
double x = 0;
double y = 0;
double z = 0;
pLookAtElement->Attribute("x", &x);
pLookAtElement->Attribute("y", &y);
pLookAtElement->Attribute("z", &z);
glm::vec3 lookAt((float)x, (float)y, (float)z);
rotation.BuildRotationLookAt(translation.GetPosition(), lookAt, g_Up);
}
tinyxml2::XMLElement* pScaleElement = pData->FirstChildElement("Scale");
if (pScaleElement)
{
double x = 0;
double y = 0;
double z = 0;
pScaleElement->Attribute("x", &x);
pScaleElement->Attribute("y", &y);
pScaleElement->Attribute("z", &z);
m_scale = glm::vec3((float)x, (float)y, (float)z);
}
**/
m_transform = glm::translate(glm::mat4(), position) * rotation;
return true;
}
int
main (int argc, char **argv)
{
WeatherInfo info;
GOptionContext* context;
GError* error = NULL;
gdouble latitude, longitude;
WeatherLocation location;
gchar* gtime = NULL;
GDate gdate;
struct tm tm;
gboolean bsun, bmoon;
time_t phases[4];
const GOptionEntry entries[] = {
{ "latitude", 0, 0, G_OPTION_ARG_DOUBLE, &latitude,
"observer's latitude in degrees north", NULL },
{ "longitude", 0, 0, G_OPTION_ARG_DOUBLE, &longitude,
"observer's longitude in degrees east", NULL },
{ "time", 0, 0, G_OPTION_ARG_STRING, >ime,
"time in seconds from Unix epoch", NULL },
{ NULL }
};
memset(&location, 0, sizeof(WeatherLocation));
memset(&info, 0, sizeof(WeatherInfo));
context = g_option_context_new ("- test libmateweather sun/moon calculations");
g_option_context_add_main_entries (context, entries, NULL);
g_option_context_parse (context, &argc, &argv, &error);
if (error) {
perror (error->message);
return error->code;
}
else if (latitude < -90. || latitude > 90.) {
perror ("invalid latitude: should be [-90 .. 90]");
return -1;
} else if (longitude < -180. || longitude > 180.) {
perror ("invalid longitude: should be [-180 .. 180]");
return -1;
}
location.latitude = DEGREES_TO_RADIANS(latitude);
location.longitude = DEGREES_TO_RADIANS(longitude);
location.latlon_valid = TRUE;
info.location = &location;
info.valid = TRUE;
if (gtime != NULL) {
// printf(" gtime=%s\n", gtime);
g_date_set_parse(&gdate, gtime);
g_date_to_struct_tm(&gdate, &tm);
info.update = mktime(&tm);
} else {
info.update = time(NULL);
}
bsun = calc_sun_time(&info, info.update);
bmoon = calc_moon(&info);
printf (" Latitude %7.3f %c Longitude %7.3f %c for %s All times UTC\n",
fabs(latitude), (latitude >= 0. ? 'N' : 'S'),
fabs(longitude), (longitude >= 0. ? 'E' : 'W'),
asctime(gmtime(&info.update)));
printf("sunrise: %s",
(info.sunriseValid ? ctime(&info.sunrise) : "(invalid)\n"));
printf("sunset: %s",
(info.sunsetValid ? ctime(&info.sunset) : "(invalid)\n"));
if (bmoon) {
printf("moonphase: %g\n", info.moonphase);
printf("moonlat: %g\n", info.moonlatitude);
if (calc_moon_phases(&info, phases)) {
printf(" New: %s", asctime(gmtime(&phases[0])));
printf(" 1stQ: %s", asctime(gmtime(&phases[1])));
printf(" Full: %s", asctime(gmtime(&phases[2])));
printf(" 3rdQ: %s", asctime(gmtime(&phases[3])));
}
}
return 0;
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useYaw, float yawError)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float hx, hy, bx;
float ex = 0, ey = 0, ez = 0;
float qa, qb, qc;
// Calculate general spin rate (rad/s)
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
ez += sin_approx(yawError / 2.0f);
}
// Use measured magnetic field vector
recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
ey += rMat[2][1] * ez_ef;
ez += rMat[2][2] * ez_ef;
}
// Use measured acceleration vector
recipNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipNorm > 0.01f) {
// Normalise accelerometer measurement
recipNorm = invSqrt(recipNorm);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Error is sum of cross product between estimated direction and measured direction of gravity
ex += (ay * rMat[2][2] - az * rMat[2][1]);
ey += (az * rMat[2][0] - ax * rMat[2][2]);
ez += (ax * rMat[2][1] - ay * rMat[2][0]);
}
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
float dcmKiGain = imuRuntimeConfig->dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
}
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
gy += dcmKpGain * ey + integralFBy;
gz += dcmKpGain * ez + integralFBz;
// Integrate rate of change of quaternion
gx *= (0.5f * dt);
gy *= (0.5f * dt);
gz *= (0.5f * dt);
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
}