void Box2DBody::setAngularVelocity(float velocity)
{
if (angularVelocity() == velocity)
return;
mBodyDef.angularVelocity = toRadians(velocity);
if (mBody)
mBody->SetAngularVelocity(mBodyDef.angularVelocity);
emit angularVelocityChanged();
}
// Function to calculate which direction we need to move the camera and by what amount
void Camera::CalculateCameraMovement(const glm::vec3& direction) {
// Break up our movement into components along the X, Y and Z axis
float camMovementXComponent = 0.0f;
float camMovementYComponent = 0.0f;
float camMovementZComponent = 0.0f;
// Forward/backward movement
if (direction.z != 0.0f) {
// Control X-Axis movement
float pitchFactor = cos(_rotation.x);
camMovementXComponent += (movementSpeedFactor * float(sin((_rotation.y))) * direction.z) * pitchFactor;
// Control Y-Axis movement
camMovementYComponent += movementSpeedFactor * float(sin((_rotation.x)) * -direction.z);
// Control Z-Axis movement
float yawFactor = (cos((_rotation.x)));
camMovementZComponent += (movementSpeedFactor * float(cos((_rotation.y))) * direction.z) * yawFactor;
}
// Strafing
if ( direction.x != 0.0f ) {
// Calculate our Y-Axis _rotation in radians once here because we use it twice
float yRotRad = (_rotation.y);
camMovementXComponent += movementSpeedFactor * float(cos(yRotRad)) * direction.x;
camMovementZComponent += movementSpeedFactor * float(sin(yRotRad)) * -direction.x;
}
// Vertical movement
if ( direction.y != 0.0f ) {
float xRotRad = toRadians(_rotation.x);
float yRotRad = toRadians(_rotation.y);
camMovementYComponent += movementSpeedFactor * float(cos(xRotRad)) * -direction.y;
camMovementXComponent += movementSpeedFactor * float(sin(yRotRad)) * direction.y;
camMovementZComponent += movementSpeedFactor * float(cos(yRotRad)) * direction.y;
}
// After combining our movements for any & all keys pressed, assign them to our camera speed along the given axis
speed.x = camMovementXComponent;
speed.y = camMovementYComponent;
speed.z = camMovementZComponent;
// Cap the speeds to our movementSpeedFactor (otherwise moving diagonally will be faster than along an axis!)
speed.x = clamp(speed.x, -movementSpeedFactor, movementSpeedFactor);
speed.y = clamp(speed.y, -movementSpeedFactor, movementSpeedFactor);
speed.z = clamp(speed.z, -movementSpeedFactor, movementSpeedFactor);
}
void Planet::generateHeightImage(const int surfaceDensity) {
m_heightMapSurface.setPointCount(surfaceDensity);
float angleInc = (float)(360 / surfaceDensity);
int index = 0;
for (float i = 0; i <= 360 && index < surfaceDensity; i += angleInc) {
float x = cos(toRadians(i)) * m_circumference / 4 + m_circumference / 4;
float y = sin(toRadians(i)) * m_circumference / 4 + m_circumference / 4;
float height;
if (heightMap.GetValue((int)x, (int)y) >= m_waterLevel) {
height = m_radius + heightMap.GetValue((int)x, (int)y) * m_heightVariance;
} else {
height = m_radius + m_waterLevel * m_heightVariance;
}
sf::Vector2f position(makeVector(i, height));
m_heightMapSurface.setPoint(index, position);
m_test_image.setPixel((int)x, (int)y, sf::Color(0, 255, 0, 255));
index++;
}
}
/** Calculates the points in a circle.
*
* @param radius Distance from center to a point
* @param number Number of points in the circle
* @return List of points in circle.
*/
list<Vec4> Math::computeCircle(float radius, int number) {
double delta, theta, x, y;
list<Vec4> points;
// Initialize
delta = 360.0 / number;
// Calculate points
points.push_back(Vec4(radius,0,0,1));
theta = delta;
while (theta < 360) {
x = radius * cos(toRadians(theta));
y = radius * sin(toRadians(theta));
points.push_back(Vec4(x,y,0,1));
theta += delta;
}
// Finish
return points;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr toPointCloud(tf::TransformListener &tf_listener, std::vector<std::vector<cv::Point>> contours, int height, int width) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
tf::StampedTransform transform;
tf_listener.lookupTransform("/base_footprint", "/camera_left", ros::Time(0), transform);
double roll, pitch, yaw;
tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
double origin_z = transform.getOrigin().getZ();
double origin_y = transform.getOrigin().getY();
double HFOV = toRadians(66.0);
double VFOV = toRadians(47.6);
pitch = -roll;
for (std::vector<cv::Point> contour : contours) {
for (cv::Point p : contour) {
cloud->points.push_back(PointFromPixelNoCam(p, height, width, HFOV, VFOV, origin_z, origin_y, pitch));
}
}
cloud->header.frame_id = "base_footprint";
return cloud;
}
void Box2DMotorJoint::setAngularOffset(float angularOffset)
{
m_defaultAngularOffset = false;
if (m_angularOffset == angularOffset)
return;
m_angularOffset = angularOffset;
if (motorJoint())
motorJoint()->SetAngularOffset(toRadians(angularOffset));
emit angularOffsetChanged();
}
void Camera::onKeyPressed(int key, int mouseX, int mouseY, bool special){
if ( useAnimation && !special && key != 'u' ) return;
if (!special){
float sinX = float(sin(toRadians(rotation.x))) / 2;
float sinY = float(sin(toRadians(rotation.y))) / 2;
// float cosX = float(cos(toRadians(rotation.x))) / 2;
float cosY = float(cos(toRadians(rotation.y))) / 2;
switch( key ) {
case 'w':
position.x += sinY;
position.y -= sinX;
position.z -= cosY;
break;
case 'a':
position.x -= cosY;
position.z -= sinY;
break;
case 's':
position.x -= sinY;
position.y += sinX;
position.z += cosY;
break;
case 'd':
position.x += cosY;
position.z += sinY;
break;
case 'u':
useAnimation = !useAnimation;
break;
}
}else{
if (key == GLUT_KEY_PAGE_UP) {
position.y += 1;
} else if (key == GLUT_KEY_PAGE_DOWN){
position.y -= 1;
}
}
}
point rotate( float point_x, float point_y, float orig_x, float orig_y, float angle )
{
// DANGER: Math zone
// convert degrees to radians
float rad_angle = toRadians( -angle );
// the trig identities used for rotation
float x_prime = orig_x + ( ( ( point_x - orig_x ) * cos( rad_angle ) ) - ( ( point_y - orig_y ) * sin( rad_angle ) ) );
float y_prime = orig_y + ( ( ( point_x - orig_x ) * sin( rad_angle ) ) + ( ( point_y - orig_y ) * cos( rad_angle ) ) );
// End of DANGER
return point( x_prime, y_prime );
}
void Camera_update(Camera* const camera, Input* const input) {
Camera_offset_orientation(
camera,
toRadians((-input->mouse_dx * input->mouse_sensitivity)),
toRadians((-input->mouse_dy * input->mouse_sensitivity))
);
// TODO: Delta time
vec3 dir = {0.0f, 0.0f, 0.0f};
float vel = 0.2f;
vec3 forward, backward, left, right;
Camera_relative_directions(camera, forward, backward, left, right);
if (input->forward_key) {
vec3 f = {forward[0], 0.0f, forward[2]};
vec3_norm(f, f);
vec3_add(dir, dir, f);
}
if (input->back_key) {
vec3 b = {backward[0], 0.0f, backward[2]};
vec3_norm(b, b);
vec3_add(dir, dir, b);
}
if (input->left_key) {
vec3_add(dir, dir, left);
}
if (input->right_key) {
vec3_add(dir, dir, right);
}
if (vec3_len(dir) > 0.0f) {
vec3_norm(dir, dir);
}
vec3 movement;
vec3_scale(movement, dir, vel);
vec3_add(camera->transform.position, camera->transform.position, movement);
}
//Uses law of sines to solve a triangle, with the user providing angle-side-side
double solveTriangle(double _a, double _A, double _B){
double c;
double b;
double a = _a;
double C;
double B = _B;
double A = _A;
double ae = sin(toRadians(a))/A;
double be;
double ce;
b = toDegrees(asin(ae*B));
be = sin(toRadians(b))/B;
c = 180-a-b;
C = sin(toRadians(c))/be;
//C*C is the area of the rectangle for whichever triangle was being solved
return C*C;
}
void Ship::input(float dt)
{
if (Input::isKeyDown(AO_KEY_LEFT))
{
rotation += TURN_RATE * dt;
if (rotation >= 360)
rotation -= 360;
}
if (Input::isKeyDown(AO_KEY_RIGHT))
{
rotation -= TURN_RATE * dt;
if (rotation < 0)
rotation += 360;
}
if (Input::isKeyDown(AO_KEY_UP))
{
if (Timer::getTime() >= thrusterSoundTime)
SoundManager::getSound("thruster")->play();
if (thrusterSoundTime < Timer::getTime())
thrusterSoundTime = Timer::getTime() + 418;
float rad = toRadians(rotation);
velocity.x += cosf(rad) * ACCELERATION * dt;
velocity.y += sinf(rad) * ACCELERATION * dt;
acceleratingTimer += dt;
if (acceleratingTimer > 0.1f)
acceleratingTimer = 0;
if (velocity.length() > MAX_VELOCITY)
velocity = velocity.normalized() * MAX_VELOCITY;
updateFlameShape();
isAccelerating = true;
}
else
{
acceleratingTimer = 0;
isAccelerating = false;
}
if (Input::isKeyDown(AO_KEY_SPACE))
{
spaceKeyHeld = true;
if (shootTime < Timer::getTime())
{
shootTime = Timer::getTime() + SHOOT_COOLDOWN;
fire();
}
}
if (!Input::isKeyDown(AO_KEY_SPACE) && spaceKeyHeld)
{
spaceKeyHeld = false;
shootTime = Timer::getTime();
}
}
/*
per-frame non-drawing code for animations, logic, etc.
*/
void Game::step()
{
float time = elapsed();
shipModel.identity();
shipModel.Translate(shipX, shipY, 0.f);
shipModel.Translate(shipVariance * random(), shipVariance*random(), 0.f);
shipModel.Scale(shipScale, shipScale, 1.f);
shipModel.Rotate(shipRot);
bigX += bigSpeed * time;
bigRot += bigSpin * time;
if (bigX < -3.f)
{
bigX += 6.f;
bigY = (random() * 2.f) - 1.f;
}
if (bigRot > toRadians(360.f))
bigRot -= toRadians(360.f);
bigModel.identity();
bigModel.Translate(bigX, bigY, 0.f);
bigModel.Scale(bigScale, bigScale, 1.f);
bigModel.Rotate(bigRot);
medX += medSpeed * time;
medRot += medSpin * time;
if (medX < -3.f)
{
medX += 6.f;
medY = (random() * 2.f) - 1.f;
}
if (medRot < toRadians(-360.f))
medRot += toRadians(360.f);
medModel.identity();
medModel.Translate(medX, medY, 0.f);
medModel.Scale(medScale, medScale, 1.f);
medModel.Rotate(medRot);
}
void MainCamera::perspView() {
float f = float(1. / std::tan(toRadians(fovy_) * 0.5));
float g = -((far_ + near_) / (far_ - near_));
float h = -((2 * far_ * near_) / (far_ - near_));
float i = f / aspect_;
Eigen::Matrix4f m;
m << i, 0.0f, 0.0f, 0.0f,
0.0f, f, 0.0f, 0.0f,
0.0f, 0.0f, g, h,
0.0f, 0.0f, -1, 0.0f;
glMultMatrixf(m.data());
}
double Waypoint::bearingRhumbTo(Waypoint wp, int scale)
{
double latDiff = toRadians(wp.lat - lat); // Δφ
double lonDiff = toRadians(wp.lon - lon); // Δλ
double lat1R = toRadians(lat); // Original latitude
double lat2R = toRadians(wp.lat); // Desitination latitude
double projectedLatDiff =
log(
tan(pi / 4 + lat2R / 2) / tan(pi / 4 + lat1R / 2)
); // Δψ
double q =
abs(projectedLatDiff) > 10 * exp(-12) ?
latDiff / projectedLatDiff : cos(lat1R);
// if dLon over 180° take shorter rhumb across anti-meridian:
if (abs(lonDiff) > pi)
lonDiff = lonDiff > 0 ? -(2 * pi - lonDiff) : (2 * pi + lonDiff);
double rhumbBearing = toDegrees(atan2(lonDiff, projectedLatDiff));
while ((rhumbBearing < 0) || (rhumbBearing > 360))
{
if (rhumbBearing < 0)
{
rhumbBearing = 360 + rhumbBearing;
}
else if (rhumbBearing > 360)
{
rhumbBearing = 360 - rhumbBearing;
}
}
return rhumbBearing;
}
mat4 mat4::perspective(float fov, float aspectRatio, float near, float far)
{
mat4 result(1.0f);
float q = (float) (1.0f / tan(toRadians(0.5f * fov)));
float a = q / aspectRatio;
float b = (near + far) / (near - far);
float c = (2.0f * near * far) / (near - far);
result.elements[0 + 0 * 4] = a;
result.elements[1 + 1 * 4] = q;
result.elements[2 + 2 * 4] = b;
result.elements[3 + 2 * 4] = -1.0f;
result.elements[2 + 3 * 4] = c;
return result;
}
/**
* Function calculates distance between two spherical coordinates using haversine formula.
* @param first - first coordinates in tCoords structure
* @param second - second coordinates in tCoords structure
* @return distance in km between two provided coordinates (double)
*/
double getDistance(tCoords first, tCoords second)
{
double deltaLat = toRadians(first.lat) - toRadians(second.lat);
double deltaLon = toRadians(first.lon) - toRadians(second.lon);
double aHarv = pow(sin(deltaLat / 2.0), 2.0) + cos(toRadians(first.lat)) * cos(toRadians(second.lat)) * pow(sin(deltaLon / 2), 2);
double cHarv = 2 * atan2(sqrt(aHarv), sqrt(1.0-aHarv));
return EARTH_RADIUS * cHarv;
}
/*
* The window resized callback
*
* This is made to properly handle the rendering of a scene based off
* of the QGLWidget's size parameters. (Called whenever dimensions change)
*/
void GLWidget::resizeGL( int width, int height )
{
int side = qMin( width, height );
// Setting up the viewport
glViewport( (width - side) / 2, (height - side) / 2, side, side );
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
/*
* For perspective projection, since we aren't using GLUT, we need
* to do a conversion.
*
* This site helped:
http://jleland.blogspot.com/2008/12/gluperspective-to-glfrustrumf.html
*/
GLfloat top, bottom, left, right;
GLfloat fov = 40, near = 0.1, far = 100.0;
if (perspectiveMode) {
// GLfloat aspect = (GLfloat) width / (GLfloat) height;
// Always make aspect ratio 0 to avoid nasty looking resizes
GLfloat aspect = 1.0;
top = tan( toRadians( fov ) ) * near;
bottom = -top;
left = aspect * bottom;
right = aspect * top;
// See if we're running OpenGL ES and use the proper function
#ifdef QT_OPENGL_ES_1
glFrustumf( left, right, bottom, top, near, far );
#else
glFrustum( left, right, bottom, top, near, far );
#endif
} else {
// See if we're running OpenGL ES and use the proper function
#ifdef QT_OPENGL_ES_1
glOrthof( ortho_left, ortho_right, ortho_top, ortho_bottom, near, far );
#else
glOrtho( ortho_left, ortho_right, ortho_top, ortho_bottom, near, far );
#endif
}
glMatrixMode( GL_MODELVIEW );
}
void PretzelColorPicker::draw()
{
gl::pushMatrices(); {
gl::translate(mOffset + vec2(0,2));
// TOP ------------------------------------------------------------
gl::color(1,1,1,1);
if( mArrowTex ){
gl::pushMatrices();{
gl::translate( vec2(14, 8) );
gl::rotate( toRadians( mArrowRotation ) );
gl::translate( floor(mArrowTex->getWidth() * -0.5), floor(mArrowTex->getHeight() * -0.5) );
gl::draw( mArrowTex );
}gl::popMatrices();
}
// checkered bg
gl::draw(mCheckerPat, mColorPickRect.getUpperLeft() );
// color
if( bUseAlpha ){ gl::color( *mColorA ); }
else { gl::color(*mColor); }
pretzel()->drawSolidRect(mColorPickRect);
// outline
gl::color( mGlobal->P_OUTLINE_COLOR );
// pretzel()->drawStrokedRect(mColorPickRect);
mGlobal->renderText(mLabel, vec2(25, 1));
// BOTTOM ------------------------------------------------------------
if(bExpanded){
gl::color( ColorA(1, 1, 1, 1 ) );
gl::draw( mHueStrip, mHueStripRect );
gl::drawLine( vec2(mHueStripRect.x1 - 3, mHueStripRect.y1 + mHueStripRect.getHeight() * mHueNorm),
vec2(mHueStripRect.x2 + 3, mHueStripRect.y1 + mHueStripRect.getHeight() * mHueNorm) );
gl::draw(mBoxFbo->getColorTexture(), mColorSwatchRect);
gl::draw(mCrosshairTex,
mColorSwatchRect.getUpperLeft() + (mCrosshairPos * mColorSwatchRect.getSize()) -
( vec2(mCrosshairTex->getSize()) * 0.5f ) ); //* 0.5f
}
}gl::popMatrices();
}
void TestRegistry::test<1>()
{
set_test_name("RotateAboutX (3x3 matrix)");
a3f[0] = 2.0f; a3f[3] = 5.0f; a3f[6] = 8.0f;
a3f[1] = 3.0f; a3f[4] = 6.0f; a3f[7] = 9.0f;
a3f[2] = 4.0f; a3f[5] = 7.0f; a3f[8] = 10.0f;
degrees = 123.4f;
radians = toRadians(degrees);
c = std::cos(radians);
s = std::sin(radians);
jship::Hazmat::RotateAboutX(a3f, radians, b3f);
DOUBLES_EQUAL(a3f[ 0], 2.0, 1e-5);
DOUBLES_EQUAL(a3f[ 1], 3.0, 1e-5);
DOUBLES_EQUAL(a3f[ 2], 4.0, 1e-5);
DOUBLES_EQUAL(a3f[ 3], 5.0, 1e-5);
DOUBLES_EQUAL(a3f[ 4], 6.0, 1e-5);
DOUBLES_EQUAL(a3f[ 5], 7.0, 1e-5);
DOUBLES_EQUAL(a3f[ 6], 8.0, 1e-5);
DOUBLES_EQUAL(a3f[ 7], 9.0, 1e-5);
DOUBLES_EQUAL(a3f[ 8], 10.0, 1e-5);
DOUBLES_EQUAL(b3f[ 0], 2.0f, 1e-5);
DOUBLES_EQUAL(b3f[ 1], 3.0f * c - 4.0f * s, 1e-5);
DOUBLES_EQUAL(b3f[ 2], 4.0f * c + 3.0f * s, 1e-5);
DOUBLES_EQUAL(b3f[ 3], 5.0f, 1e-5);
DOUBLES_EQUAL(b3f[ 4], 6.0f * c - 7.0f * s, 1e-5);
DOUBLES_EQUAL(b3f[ 5], 7.0f * c + 6.0f * s, 1e-5);
DOUBLES_EQUAL(b3f[ 6], 8.0f, 1e-5);
DOUBLES_EQUAL(b3f[ 7], 9.0f * c - 10.0f * s, 1e-5);
DOUBLES_EQUAL(b3f[ 8], 10.0f * c + 9.0f * s, 1e-5);
jship::Hazmat::RotateAboutX(a3f, radians, a3f);
DOUBLES_EQUAL(a3f[ 0], 2.0f, 1e-5);
DOUBLES_EQUAL(a3f[ 1], 3.0f * c - 4.0f * s, 1e-5);
DOUBLES_EQUAL(a3f[ 2], 4.0f * c + 3.0f * s, 1e-5);
DOUBLES_EQUAL(a3f[ 3], 5.0f, 1e-5);
DOUBLES_EQUAL(a3f[ 4], 6.0f * c - 7.0f * s, 1e-5);
DOUBLES_EQUAL(a3f[ 5], 7.0f * c + 6.0f * s, 1e-5);
DOUBLES_EQUAL(a3f[ 6], 8.0f, 1e-5);
DOUBLES_EQUAL(a3f[ 7], 9.0f * c - 10.0f * s, 1e-5);
DOUBLES_EQUAL(a3f[ 8], 10.0f * c + 9.0f * s, 1e-5);
}
mat4 mat4::perspective(int fov, float aspectRatio, float near, float far)
{
mat4 result(1.0f);
auto q = 1.0f / tan(toRadians(0.5f * fov));
auto a = q / aspectRatio;
auto b = (near + far) / (near - far);
auto c = 2.0f * near * far / (near - far);
result.elements[0 + 0 * 4] = a;
result.elements[1 + 1 * 4] = q;
result.elements[2 + 2 * 4] = b;
result.elements[2 + 3 * 4] = c;
result.elements[3 + 2 * 4] = -1.0f;
return result;
}
/*
The perspective matrix is used to show stuff in a perspective to were the
camera is. It should essentially filter out stuff we cannot see, and make
stuff in the distance smaller etc etc.
The matrix looks like:
1/(ar*tan(fov/2) 0 0 0
0 1/(tan(fov/2) 0 0
0 0 -(F+N)/(F-N) -(2*F*N)/(F - N)
0 0 -1 0
*/
mat4 mat4::perspective(float fov, float aspectRatio, float near, float far)
{
mat4 result;
float index5 = 1.0f / maths_tan(toRadians(0.5f *fov));
float index0 = index5 / aspectRatio;
float index10 = (near + far) / (near - far);
float index14 = (2.0f * near * far) / (near - far);
result.elements[4 * 0 + 0] = index0;
result.elements[4 * 1 + 1] = index5;
result.elements[4 * 2 + 2] = index10;
result.elements[4 * 2 + 3] = -1.0f;
result.elements[4 * 3 + 2] = index14;
return result;
}
void F16FlightModel::calculateForceAndMoment(Vector3 &force, Vector3 &moment) {
m_ScaleLEF = 1.0 - m_LeadingEdgeFlap * toRadians(1.0/25.0);
Vector3 drag_direction(sin(m_Beta), -cos(m_Beta)*cos(m_Alpha), cos(m_Beta)*sin(m_Alpha));
Vector3 lift_direction = (drag_direction ^ Vector3::ZAXIS) ^ drag_direction;
/**
* rotate from nasa to internal coordinate system
*/
Vector3 force_c(calculateForceCoefficientY(), calculateForceCoefficientX(), -calculateForceCoefficientZ());
/**
* scale nasa low-speed drag by hffm drag normalized to 1.0 at low speed.
*/
double CD = force_c * drag_direction;
double mach_drag_factor = m_CD_m_a[m_Mach][m_Alpha] - 1.0;
force_c += drag_direction * (CD * mach_drag_factor);
/**
* blend from nasa lift to hffm lift at high speed (M > 0.5), neglecting beta and de
* at high speed. use a cubic function to create a smooth 0-1 transition from M 0.2
* to M 0.5.
*/
double CL = force_c * lift_direction;
double mach_lift_value = m_CL_m_a[m_Mach][m_Alpha];
double blend = 0.0;
if (m_Mach > 0.5) {
blend = 1.0;
} else if (m_Mach > 0.2) {
double x = m_Mach - 0.2;
blend = (33.333 - 74.074 * x) * x *x;
}
force_c += lift_direction * ((mach_lift_value - CL) * blend);
/**
* add in ground effect if necessary. we operate only on the low speed lift, but
* the difference should not matter under normal flight conditions!
*/
if (m_GE > 1.0) {
force_c += lift_direction * (CL * (m_GE - 1.0));
}
force = force_c * m_qBarS;
moment.set(calculatePitchMoment(), calculateRollMoment(), calculateYawMoment());
}
Camera* Camera_init(
vec3 const position,
float fov,
float aspect,
float zNear,
float zFar
) {
Camera* camera = malloc(sizeof(Camera));
camera->transform = Transform_init_default();
camera->transform.position[0] = position[0];
camera->transform.position[1] = position[1];
camera->transform.position[2] = position[2];
mat4x4_perspective(camera->projection, toRadians(fov), aspect, zNear, zFar);
return camera;
}
mat4 mat4::perspective(float fov, float aspect, float near, float far)
{
mat4 mat;
float q = 1.0f / tan(toRadians(0.5f * fov));
float a = q / aspect;
float b = (near + far) / (near - far);
float c = (2.0f * near * far) / (near - far);
mat.elements[0 + 0 * 4] = a;
mat.elements[1 + 1 * 4] = q;
mat.elements[2 + 2 * 4] = b;
mat.elements[3 + 2 * 4] = -1.0f;
mat.elements[2 + 3 * 4] = c;
mat.elements[15] = 0.0f;
return mat;
}
mat4 mat4::perspective(float fov, float aspectRatio, float near, float far)
{
mat4 result(1.0f);
/*float q = 1.0f / tan(toRadians(0.5f*fov));
float a = q / aspectRatio;
float b = (near + far) / (near - far);
float c = (2.0f*near*far) / (near - far);
result.elements[0 + 0 * 4] = a;
result.elements[1 + 1 * 4] = q;1
result.elements[2 + 2 * 4] = b;
result.elements[3 + 2 * 4] = -1.0f;
result.elements[2 + 3 * 4] = c;
*/
float top = near*tan(toRadians(fov / 2));
float bottom = -top;
float right = top*aspectRatio;
float left = -right;
/* result.elements[0 + 0 * 4] = (2 * near) / (right-left);
result.elements[1 + 1 * 4] = (2 * near) / (top-bottom);
result.elements[2 + 2 * 4] = -((far + near) / (far-near));
result.elements[2 + 3 * 4] = -2*(far*near)/(far-near);
result.elements[3 + 2 * 4] = 1;
result.elements[3 + 3 * 4] = 0;
result.elements[0 + 2 * 4] = (right + left) / (right - left);
result.elements[1 + 2 * 4] = (top + bottom) / (top - bottom);
*/
/*result.elements[0 + 0 * 4] = (2 * near) / (right - left);
result.elements[1 + 1 * 4] = (2 * near) / (top - bottom);
result.elements[2 + 2 * 4] = ((far + near) / (far - near));
result.elements[3 + 2 * 4] = -1;
result.elements[3 + 3 * 4] = 0;
result.elements[2 + 3 * 4] = -2*(far*near)/(far-near);*/
result.elements[0 + 0 * 4] = 1/((tan(fov/2)*aspectRatio));
result.elements[1 + 1 * 4] = 1 / (tan(fov / 2));
result.elements[2 + 2 * 4] = (far+near)/(far-near);
result.elements[3 + 2 * 4] = -1;
result.elements[2 + 3 * 4] = -(2*near*far) / (far - near);
result.elements[3 + 3 * 4] = 0;
return result;
}
void Demo::onInit() {
md2Model = MD2Model::fromFile("d:/games/data/quake2/players/pknight/tris.md2");
GMaterial material;
material.texture.append(MD2Model::textureFromFile("d:/games/data/quake2/players/pknight/ctf_b.pcx"));
CoordinateFrame obj = CoordinateFrame(Matrix3::fromAxisAngle(Vector3::unitY(), toRadians(30)), Vector3(2, 0, 0));
app->debugCamera.setPosition(Vector3(5,5,5));
app->debugCamera.lookAt(Vector3::zero());
// World control
// manipulator->setFrame(obj);
// manipulator->setControlFrame(CoordinateFrame());
models.append(md2Model->pose(CoordinateFrame(), MD2Model::Pose(), material));
// Local control
manipulator->setFrame(obj);
manipulator->setControlFrame(obj);
}
/*
The rotation matrix is used to rotate stuff around an axis by the
specified amount of degrees
*/
mat4 mat4::rotation(float angle, const vec3& axis)
{
mat4 result(1.0f);
float rad = toRadians(angle);
float c = maths_cos(rad);
float s = maths_sin(rad);
float omc = 1.0f - c;
result.elements[4 * 0 + 0] = axis.x * omc + c;
result.elements[4 * 1 + 0] = axis.y * axis.x * omc + axis.z * s;
result.elements[4 * 2 + 0] = axis.x * axis.z * omc - axis.y * s;
result.elements[4 * 0 + 1] = axis.x * axis.y * omc - axis.z * s;
result.elements[4 * 1 + 1] = axis.y * omc + c;
result.elements[4 * 2 + 1] = axis.y * axis.z * omc + axis.x * s;
result.elements[4 * 0 + 2] = axis.x * axis.z * omc + axis.y * s;
result.elements[4 * 1 + 2] = axis.y * axis.z * omc - axis.x * s;
result.elements[4 * 2 + 2] = axis.z * omc + c;
return result;
}
// initialize screen coordinates
// parm1=Number of Strings/Arches
// parm2=Pixels Per String/Arch
void ModelClass::SetLineCoord()
{
int x,y;
int idx=0;
size_t NodeCount=GetNodeCount();
int numlights=parm1*parm2;
int half=numlights/2;
SetRenderSize(numlights*2,numlights);
double radians=toRadians(PreviewRotation);
for(size_t n=0; n<NodeCount; n++)
{
size_t CoordCount=GetCoordCount(n);
for(size_t c=0; c < CoordCount; c++)
{
x=cos(radians)*idx;
x=IsLtoR ? x - half : half - x;
y=sin(radians)*idx;
Nodes[n]->Coords[c].screenX=x;
Nodes[n]->Coords[c].screenY=y + numlights;
idx++;
}
}
}
void ArchesModel::SetArchCoord() {
double xoffset,x,y;
size_t NodeCount=GetNodeCount();
double midpt=parm2*parm3;
midpt -= 1.0;
midpt /= 2.0;
double total = toRadians(arc);
double start = (M_PI - total) / 2.0;
double angle=-M_PI/2.0 + start;
x=midpt*sin(angle)*2.0+parm2*parm3;
double width = parm2*parm3*2 - x;
double minY = 999999;
for(size_t n=0; n<NodeCount; n++) {
xoffset=Nodes[n]->StringNum*width;
size_t CoordCount=GetCoordCount(n);
for(size_t c=0; c < CoordCount; c++) {
double angle=-M_PI/2.0 + start + total * ((double)(Nodes[n]->Coords[c].bufX * parm3 + c))/midpt/2.0;
x=xoffset + midpt*sin(angle)*2.0+parm2*parm3;
y=(parm2*parm3)*cos(angle);
Nodes[n]->Coords[c].screenX=x;
Nodes[n]->Coords[c].screenY=y;
minY = std::min(minY, y);
}
}
float renderHt = parm2*parm3;
if (minY > 1) {
renderHt -= minY;
for (auto it = Nodes.begin(); it != Nodes.end(); it++) {
for (auto coord = (*it)->Coords.begin(); coord != (*it)->Coords.end(); coord++) {
coord->screenY -= minY;
}
}
}
screenLocation.SetRenderSize(width*parm1, renderHt);
}
void Rect::calcVecCoords() {
Vector3 tl(-w / 2.0f, -h / 2.0f, 0.0f);
Vector3 tr( w / 2.0f, -h / 2.0f, 0.0f);
Vector3 bl(-w / 2.0f, h / 2.0f, 0.0f);
Vector3 br( w / 2.0f, h / 2.0f, 0.0f);
float radians = toRadians(degrees);
vecCoords[0].x = center.x + tl.x * cos(radians) - tl.y * sin(radians);
vecCoords[0].y = center.y + tl.x * sin(radians) + tl.y * cos(radians);
vecCoords[0].z = center.z;
vecCoords[1].x = center.x + tr.x * cos(radians) - tr.y * sin(radians);
vecCoords[1].y = center.y + tr.x * sin(radians) + tr.y * cos(radians);
vecCoords[1].z = center.z;
vecCoords[2].x = center.x + bl.x * cos(radians) - bl.y * sin(radians);
vecCoords[2].y = center.y + bl.x * sin(radians) + bl.y * cos(radians);
vecCoords[2].z = center.z;
vecCoords[3].x = center.x + br.x * cos(radians) - br.y * sin(radians);
vecCoords[3].y = center.y + br.x * sin(radians) + br.y * cos(radians);
vecCoords[3].z = center.z;
}
