/// sets the servo angles for MAVLink, note angles are in degrees
void AP_Mount::set_control_angles(float roll, float tilt, float pan)
{
_control_angles = Vector3f(roll, tilt, pan);
}
void Sphere::tesselate( int nTheta, int nPhi,
std::vector< Vector3f >& positions,
std::vector< Vector3f >& normals )
{
positions.clear();
normals.clear();
positions.reserve( 6 * nTheta * nPhi );
normals.reserve( 6 * nTheta * nPhi );
float dt = libcgt::core::math::TWO_PI / nTheta;
float dp = libcgt::core::math::PI / nPhi;
Vector3f c( center );
for( int t = 0; t < nTheta; ++t )
{
float t0 = t * dt;
float t1 = t0 + dt;
for( int p = 0; p < nPhi; ++p )
{
float p0 = p * dp;
float p1 = p0 + dp;
float x00 = cosf( t0 ) * sinf( p0 );
float y00 = sinf( t0 ) * sinf( p0 );
float z00 = cosf( p0 );
float x10 = cosf( t1 ) * sinf( p0 );
float y10 = sinf( t1 ) * sinf( p0 );
float z10 = cosf( p0 );
float x01 = cosf( t0 ) * sinf( p1 );
float y01 = sinf( t0 ) * sinf( p1 );
float z01 = cosf( p1 );
float x11 = cosf( t1 ) * sinf( p1 );
float y11 = sinf( t1 ) * sinf( p1 );
float z11 = cosf( p1 );
Vector3f v00 = c + Vector3f( radius * x00, radius * y00, radius * z00 );
Vector3f n00( x00, y00, z00 );
Vector3f v10 = c + Vector3f( radius * x10, radius * y10, radius * z10 );
Vector3f n10( x10, y10, z10 );
Vector3f v01 = c + Vector3f( radius * x01, radius * y01, radius * z01 );
Vector3f n01( x01, y01, z01 );
Vector3f v11 = c + Vector3f( radius * x11, radius * y11, radius * z11 );
Vector3f n11( x11, y11, z11 );
positions.push_back( v00 );
normals.push_back( n00 );
positions.push_back( v10 );
normals.push_back( n10 );
positions.push_back( v01 );
normals.push_back( n01 );
positions.push_back( v01 );
normals.push_back( n01 );
positions.push_back( v10 );
normals.push_back( n10 );
positions.push_back( v11 );
normals.push_back( n11 );
}
}
}
void pbrtRotate(Float angle, Float dx, Float dy, Float dz) {
VERIFY_INITIALIZED("Rotate");
FOR_ACTIVE_TRANSFORMS(curTransform[i] =
curTransform[i] *
Rotate(angle, Vector3f(dx, dy, dz));)
}
Mesh MeshLoader::getPortalBox(const Entity &wall) {
// Generate vertex array object
GLuint vao = 0;
glGenVertexArrays(1, &vao);
glBindVertexArray(vao);
/* == Static part: vertices, normals and tangents == */
constexpr unsigned int coordsSize = sizeof(float) * 3,
texcSize = sizeof(float) * 2,
normalsSize = sizeof(int8_t) * 3,
tangentsSize = sizeof(int8_t) * 3,
vtxSize = coordsSize + texcSize + normalsSize + tangentsSize,
vboSize = vtxSize * 3 /*verts*/ * 2 /*tris*/ * 6; /*faces*/
TightDataPacker data(vboSize);
static constexpr float vertices[8][3] = {
{-0.5f, -0.5f, -0.5f}, {-0.5f, -0.5f, 0.5f}, {-0.5f, 0.5f, -0.5f},
{-0.5f, 0.5f, 0.5f}, {0.5f, -0.5f, -0.5f}, {0.5f, -0.5f, 0.5f},
{0.5f, 0.5f, -0.5f}, {0.5f, 0.5f, 0.5f}};
static constexpr uint8_t vi[36] = {
3, 1, 5, 3, 5, 7, // Front
7, 5, 4, 7, 4, 6, // Left
6, 4, 0, 6, 0, 2, // Back
2, 0, 1, 2, 1, 3, // Right
2, 3, 7, 2, 7, 6, // Top
1, 0, 4, 1, 4, 5 // Bottom
};
const Vector3f &scale = wall.getScale();
const float texCoords[8][2] = {
{0, 0}, {scale.x, 0}, {scale.z, 0}, {0, scale.y},
{0, scale.z}, {scale.x, scale.y}, {scale.x, scale.z}, {scale.z, scale.y}};
static constexpr uint8_t ti[36] = {0, 3, 5, 0, 5, 1, 0, 3, 7, 0, 7, 2,
0, 3, 5, 0, 5, 1, 0, 3, 7, 0, 7, 2,
0, 4, 6, 0, 6, 1, 0, 4, 6, 0, 6, 1};
static constexpr int8_t normals[6][3] = {{0, 0, 1}, {1, 0, 0}, {0, 0, -1},
{-1, 0, 0}, {0, 1, 0}, {0, -1, 0}};
static constexpr uint8_t ni[36] = {0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1,
2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3,
4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5};
static const int8_t tangents[6][3] = {{0, 1, 0}, {0, 0, 1}, {0, -1, 0},
{0, 0, -1}, {1, 0, 0}, {-1, 0, 0}};
static constexpr uint8_t tai[36] = {0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1,
2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3,
4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5};
constexpr unsigned int texCoordsOffset = coordsSize,
normalsOffset = texCoordsOffset + texcSize,
tangentsOffset = normalsOffset + normalsSize;
for (int i = 0; i < 36; ++i) {
const float *v = vertices[vi[i]];
data << v[0] << v[1] << v[2];
const float *t = texCoords[ti[i]];
data << t[0] << t[1];
const int8_t *n = normals[ni[i]];
data << n[0] << n[1] << n[2];
const int8_t *ta = tangents[tai[i]];
data << ta[0] << ta[1] << ta[2];
}
Mesh mesh;
mesh.vertices.resize(8);
for (unsigned int i = 0; i < 8; ++i) {
const float *v = vertices[i];
mesh.vertices[i] = Vector3f(v[0], v[1], v[2]);
}
assert(data.getSize() == vboSize);
createVBO(GL_ARRAY_BUFFER, vboSize, data.getDataPtr(), vtxSize,
{{0, 3, GL_FLOAT, nullptr}, // Vertices
{1, 2, GL_FLOAT, reinterpret_cast<GLvoid *>(texCoordsOffset)}, // Tex coords
{2, 3, GL_BYTE, reinterpret_cast<GLvoid *>(normalsOffset)}, // Normals
{3, 3, GL_BYTE, reinterpret_cast<GLvoid *>(tangentsOffset)}}); // Tangents
// unbind Vertex Array Object (VAO)
glBindVertexArray(0);
// Store relevant data in the new mesh
mesh.handle = static_cast<int>(vao);
mesh.numFaces = 12;
return mesh;
}
Terrain::Terrain(float longueur, float largeur, float amplitude):
longueur(longueur), largeur(largeur)
{
box = Box(Vector3f(0,0,0),Vector3f(largeur,longueur,amplitude));
heatMapGradient.createDefaultHeatMapGradient();
}
Vector3f
PinguAction::get_center_pos() const
{
return pingu->get_pos() + Vector3f(0, -16);
}
Quatf Player::GetOrientation(bool baseOnly)
{
Quatf baseQ = Quatf(Vector3f(0,1,0), BodyYaw.Get());
return baseOnly ? baseQ : baseQ * HeadPose.Rotation;
}
Vector3f Rectangle::WorldCentroid() const {
return TransformPoint(local_to_world, Vector3f(0.f, 0.f, 0.f));
}
Vector3f normalize( Vector3f a)
{
float len = length(a);
return Vector3f( a.x/len, a.y/len, a.z/len);
}
/**
operator *
Transforms a point by this matrix.
*/
Vector3f Matrix4x4::operator * (Vector3f& p)
{
return Vector3f(_mat[0] * p[0] + _mat[4] * p[1] + _mat[8] * p[2] + _mat[12],
_mat[1] * p[0] + _mat[5] * p[1] + _mat[9] * p[2] + _mat[13],
_mat[2] * p[0] + _mat[6] * p[1] + _mat[10] * p[2] + _mat[14]);
}
BBOX Rectangle::LocalBBOX() const {
return BBOX(Vector3f(-x_size / 2.f, EPS, -z_size / 2.f),
Vector3f(x_size / 2.f, -EPS, z_size / 2.f));
}
Vector2i Obj::load_heightmap(string file_name, float scale)
{
int width = 0, height = 0;
string line;
ifstream myfile (file_name.c_str());
if (myfile.is_open()) {
while (! myfile.eof() ) {
getline (myfile,line);
int this_width = 0;
for (string::iterator it=line.begin() ; it < line.end(); it++ )
if (*it == ',') this_width++;
if (width == 0) {
width = this_width;
} else if (this_width == 0) {
break;
} else if (this_width != width) {
cerr << "all lines in the heightmap must be the same length" << endl;
}
height++;
}
myfile.close();
} else {
cerr << "could not open file " << file_name << endl;
}
int size = width*height;
vertices = new Vertex[size];
v_poss = new Vector3f[size];
v_norms = new Vector3f[size];
v_texts = new Vector2f[size];
face_count = (width-1)*(height-1);
faces = new Face[(width-1)*(height-1)];
//myfile.open(file_name.c_str());
ifstream myfile2 (file_name.c_str());
for (int i = 0 ; i < height ; i++) {
getline (myfile2,line);
string tmp = "";
int j = 0;
for (string::iterator it=line.begin() ; it < line.end(); it++ ) {
if (*it == ',') {
int index = i*width+j;
v_poss[index].init((i/*-((float)width)/2.0f*/)*scale,atof(tmp.c_str())*scale,(j/*-((float)height)/2.0f*/)*scale);
v_texts[index].init(i,j);
v_norms[index].init(0,1,0);
vertices[index].pos = &v_poss[index];
vertices[index].text = &v_texts[index];
vertices[index].normal = &v_norms[index];
tmp = "";
j++;
} else {
tmp += *it;
}
}
}
myfile2.close();
for (int i = 0; i < height-1 ; i++) {
for (int j = 0; j < width-1 ; j++) {
if (i > 0 && j > 0) {
int index = i*width+j;
//cout << "setting normal at:" << v_poss[index] << endl;
Vector3f norm_i = v_poss[(i-1)*width+j].dir_between(v_poss[(i+1)*width+j]);
//cout << "i dir " << norm_i << endl;
norm_i.rotate(Vector3f(0,0,90));
//cout << "i dir " << norm_i << endl;
norm_i.point_up();
//cout << "i dir " << norm_i << endl;
Vector3f norm_j = v_poss[i*width+(j-1)].dir_between(v_poss[i*width+(j+1)]);
//cout << "j dir " << norm_j << endl;
norm_j.rotate(Vector3f(90,0,0));
//cout << "j dir " << norm_j << endl;
norm_j.point_up();
//cout << "j dir " << norm_j << endl;
v_norms[index] = (norm_i + norm_j);
//cout << "normal " << v_norms[index] << endl;
v_norms[index].normalise();
//cout << "normal " << v_norms[index] << endl;
vertices[index].normal = &v_norms[index];
//cout << "set normal at " << v_poss[index] << " to " << v_norms[index] << endl;
}
int f_index = i*(width-1)+j;
faces[f_index].vertex_count = 4;
faces[f_index].vertices = new Vertex[4];
faces[f_index].vertices[0] = vertices[i*width+j];
faces[f_index].vertices[1] = vertices[i*width+(j+1)];
faces[f_index].vertices[2] = vertices[(i+1)*width+(j+1)];
faces[f_index].vertices[3] = vertices[(i+1)*width+j];
}
}
Vector2i map_size(width,height);
return map_size;
}
bool Paraboloid::Intersect(const Ray &r, Float *tHit,
SurfaceInteraction *isect) const {
Float phi;
Point3f pHit;
// Transform _Ray_ to object space
Vector3f oErr, dErr;
Ray ray = (*WorldToObject)(r, &oErr, &dErr);
// Compute quadratic paraboloid coefficients
// Initialize _EFloat_ ray coordinate values
EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
EFloat k = EFloat(zMax) / (EFloat(radius) * EFloat(radius));
EFloat a = k * (dx * dx + dy * dy);
EFloat b = 2.f * k * (dx * ox + dy * oy) - dz;
EFloat c = k * (ox * ox + oy * oy) - oz;
// Solve quadratic equation for _t_ values
EFloat t0, t1;
if (!Quadratic(a, b, c, &t0, &t1)) return false;
// Check quadric shape _t0_ and _t1_ for nearest intersection
if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
EFloat tShapeHit = t0;
if (t0.LowerBound() <= 0) {
tShapeHit = t1;
if (tShapeHit.UpperBound() > ray.tMax) return false;
}
// Compute paraboloid inverse mapping
pHit = ray((Float)tShapeHit);
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0.) phi += 2 * Pi;
// Test paraboloid intersection against clipping parameters
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) {
if (tShapeHit == t1) return false;
tShapeHit = t1;
if (t1.UpperBound() > ray.tMax) return false;
// Compute paraboloid inverse mapping
pHit = ray((Float)tShapeHit);
phi = std::atan2(pHit.y, pHit.x);
if (phi < 0.) phi += 2 * Pi;
if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false;
}
// Find parametric representation of paraboloid hit
Float u = phi / phiMax;
Float v = (pHit.z - zMin) / (zMax - zMin);
// Compute paraboloid $\dpdu$ and $\dpdv$
Vector3f dpdu(-phiMax * pHit.y, phiMax * pHit.x, 0.);
Vector3f dpdv = (zMax - zMin) *
Vector3f(pHit.x / (2 * pHit.z), pHit.y / (2 * pHit.z), 1.);
// Compute paraboloid $\dndu$ and $\dndv$
Vector3f d2Pduu = -phiMax * phiMax * Vector3f(pHit.x, pHit.y, 0);
Vector3f d2Pduv =
(zMax - zMin) * phiMax *
Vector3f(-pHit.y / (2 * pHit.z), pHit.x / (2 * pHit.z), 0);
Vector3f d2Pdvv = -(zMax - zMin) * (zMax - zMin) *
Vector3f(pHit.x / (4 * pHit.z * pHit.z),
pHit.y / (4 * pHit.z * pHit.z), 0.);
// Compute coefficients for fundamental forms
Float E = Dot(dpdu, dpdu);
Float F = Dot(dpdu, dpdv);
Float G = Dot(dpdv, dpdv);
Vector3f N = Normalize(Cross(dpdu, dpdv));
Float e = Dot(N, d2Pduu);
Float f = Dot(N, d2Pduv);
Float g = Dot(N, d2Pdvv);
// Compute $\dndu$ and $\dndv$ from fundamental form coefficients
Float invEGF2 = 1 / (E * G - F * F);
Normal3f dndu = Normal3f((f * F - e * G) * invEGF2 * dpdu +
(e * F - f * E) * invEGF2 * dpdv);
Normal3f dndv = Normal3f((g * F - f * G) * invEGF2 * dpdu +
(f * F - g * E) * invEGF2 * dpdv);
// Compute error bounds for paraboloid intersection
// Compute error bounds for intersection computed with ray equation
EFloat px = ox + tShapeHit * dx;
EFloat py = oy + tShapeHit * dy;
EFloat pz = oz + tShapeHit * dz;
Vector3f pError = Vector3f(px.GetAbsoluteError(), py.GetAbsoluteError(),
pz.GetAbsoluteError());
// Initialize _SurfaceInteraction_ from parametric information
*isect = (*ObjectToWorld)(SurfaceInteraction(pHit, pError, Point2f(u, v),
-ray.d, dpdu, dpdv, dndu, dndv,
ray.time, this));
*tHit = (Float)tShapeHit;
return true;
}
// Recursively traverses the assimp node hierarchy, accumulating
// modeling transformations, and creating and transforming any meshes
// found. Meshes comming from assimp can have associated surface
// properties, so each mesh *copies* the current BRDF as a starting
// point and modifies it from the assimp data structure.
void recurseModelNodes(Scene* scene,
const aiScene* aiscene,
const aiNode* node,
const aiMatrix4x4& parentTr,
const int level)
{
// Print line with indentation to show structure of the model node hierarchy.
for (int i=0; i<level; i++) printf("| ");
printf("%s ", node->mName.data);
// Accumulating transformations while traversing down the hierarchy.
aiMatrix4x4 childTr = parentTr*node->mTransformation;
aiMatrix3x3 normalTr = aiMatrix3x3(childTr); // Really should be inverse-transpose for full generality
// Loop through this node's meshes
for (unsigned int i=0; i<node->mNumMeshes; ++i) {
aiMesh* aimesh = aiscene->mMeshes[node->mMeshes[i]];
printf("%d:%d ", aimesh->mNumVertices, aimesh->mNumFaces);
// Extract this node's surface material.
aiString texPath;
aiMaterial* mtl = aiscene->mMaterials[aimesh->mMaterialIndex];
// Assimp can read material colors from the input files, but
// it seems to invent colors of its own unpredictably so I
// ignore them. Textures and texture coordinates seem to work
// well.
//aiColor3D diff (0.f,0.f,0.f);
//aiColor3D spec (0.f,0.f,0.f);
//float s;
// if (AI_SUCCESS == mtl->Get(AI_MATKEY_COLOR_DIFFUSE, diff))
// scene->setKd(Vector3f(diff.r, diff.g, diff.b));
// if (AI_SUCCESS == mtl->Get(AI_MATKEY_COLOR_SPECULAR, spec))
// scene->setKs(Vector3f(spec.r, spec.g, spec.b));
// if (AI_SUCCESS == mtl->Get(AI_MATKEY_SHININESS, &s, NULL))
// scene->setAlpha(s);
Material *material = new Material(*scene->currentMat);
if (AI_SUCCESS == mtl->GetTexture(aiTextureType_DIFFUSE, 0, &texPath))
material->setTexture(texPath.C_Str());
// Arrays to hold all vertex and triangle data.
MeshData* meshdata = new MeshData;
// Loop through all vertices and record the
// vertex/normal/texture/tangent data with the node's model
// transformation applied.
for (unsigned int t=0; t<aimesh->mNumVertices; ++t) {
aiVector3D aipnt = childTr*aimesh->mVertices[t];
aiVector3D ainrm = aimesh->HasNormals() ? normalTr*aimesh->mNormals[t] : aiVector3D(0,0,1);
aiVector3D aitex = aimesh->HasTextureCoords(0) ? aimesh->mTextureCoords[0][t] : aiVector3D(0,0,0);
aiVector3D aitan = aimesh->HasTangentsAndBitangents() ? normalTr*aimesh->mTangents[t] : aiVector3D(1,0,0);
meshdata->vertices.push_back(VertexData(Vector3f(aipnt.x, aipnt.y, aipnt.z),
Vector3f(ainrm.x, ainrm.y, ainrm.z),
Vector2f(aitex.x, aitex.y),
Vector3f(aitan.x, aitan.y, aitan.z))); }
// Loop through all faces, recording indices
for (unsigned int t=0; t<aimesh->mNumFaces; ++t) {
aiFace* aiface = &aimesh->mFaces[t];
meshdata->triangles.push_back(TriData(aiface->mIndices[0],
aiface->mIndices[1],
aiface->mIndices[2])); }
meshdata->mat = material;
scene->triangleMesh(meshdata);
delete meshdata;
}
printf("\n");
// Recurse onto this node's children
for (unsigned int i=0; i<node->mNumChildren; ++i)
recurseModelNodes(scene, aiscene, node->mChildren[i], childTr, level+1);
}
Enclosure::Enclosure(engine::IClump* clump, float delay)
{
_clump = clump;
_clump->getFrame()->translate( Vector3f( 0,0,0 ) );
_clump->getFrame()->getLTM();
_delay = delay;
_ray = Gameplay::iEngine->createRayIntersection();
_sphere = Gameplay::iEngine->createSphereIntersection();
callback::Locator locator;
locator.targetName = _clump->getName();
locator.target = NULL;
_clump->forAllAtomics( callback::locateAtomic, &locator );
_collisionAtomic = reinterpret_cast<engine::IAtomic*>( locator.target );
assert( _collisionAtomic );
engine::IGeometry* geometry = _collisionAtomic->getGeometry();
engine::IShader* shader;
unsigned int numFaces = geometry->getNumFaces();
Vector3f vertex[3];
Vector3f normal;
bool flag;
unsigned int i,j;
// enumerate normals of wall faces
for( i=0; i<numFaces; i++ )
{
geometry->getFace( i, vertex[0], vertex[1], vertex[2], &shader );
if( strcmp( shader->getName(), "EnclosureFloor" ) != 0 )
{
normal.cross( vertex[1]-vertex[0], vertex[2]-vertex[0] );
normal.normalize();
// check this normal was enumerated
flag = false;
for( j=0; j<_wallNormals.size(); j++ )
{
if( Vector3f::dot( _wallNormals[j], normal ) > 0.999f )
{
flag = true;
break;
}
}
if( !flag ) _wallNormals.push_back( normal );
}
}
// enumerate position markers (all atomics under enclosure care)
callback::AtomicL atomicL;
_clump->forAllAtomics( callback::enumerateAtomics, &atomicL );
for( callback::AtomicI atomicI=atomicL.begin(); atomicI!=atomicL.end(); atomicI++ )
{
if( (*atomicI) != _collisionAtomic )
{
_markers.push_back( (*atomicI)->getFrame() );
}
}
/*
// check number of path points (limit of 2)
unsigned int numPathPoints = 0;
for( unsigned int i=0; i<getNumMarkers(); i++ )
{
if( getMarkerFlags( i ) & mtPath ) numPathPoints++;
}
if( numPathPoints > 2 )
{
throw Exception( "Too many pathpoints in enclosure %s", clump->getName() );
}
*/
}
#include "RotateAroundAxisNode.h"
#include "../../../Math/Lower Math/Quaternion.h"
ADD_NODE_REFLECTION_DATA_CPP(RotateAroundAxisNode, Vector3f(1.0f, 0.0f, 0.0f), Vector3f(0.0f, 0.0f, 1.0f), 0.0f)
RotateAroundAxisNode::RotateAroundAxisNode(const DataLine & toRot, const DataLine & axis, const DataLine & angle, std::string name)
: DataNode(MakeVector(toRot, axis, angle), name)
{
}
void RotateAroundAxisNode::GetMyFunctionDeclarations(std::vector<std::string> & outDecls) const
{
std::string rotFunc = "vec3 " + GetName() + "_rotFunc(vec3 toRot, vec3 rotAxis, float rotAmount) \n\
{ \n\
float halfAng = rotAmount * 0.5f; \n\
vec3 quatXYZ = sin(halfAng) * rotAxis; \n\
return toRot + (2.0f * cross(quatXYZ, cross(quatXYZ, toRot) + (toRot * cos(halfAng)))); \n\
}\n";
outDecls.insert(outDecls.end(), rotFunc);
}
void RotateAroundAxisNode::WriteMyOutputs(std::string & outCode) const
{
outCode += "\tvec3 " + GetOutputName(0) + " = " +
GetName() + "_rotFunc(" + GetToRotateInput().GetValue() + ", " +
GetRotateAxisInput().GetValue() + ", " +
GetRotateAmountInput().GetValue() + ");\n";
}
void
PinguAction::move_with_forces ()
{
// Apply gravity
pingu->set_velocity(pingu->get_velocity() + Vector3f(0.0f, 1.0f));
#if 0 // New Code
Vector3f pos = pingu->get_pos();
Vector3f target_pos = pos + pingu->get_velocity();
Vector3f dir = target_pos - pingu->get_pos();
Vector3f velocity = pingu->get_velocity();
float length = dir.length();
dir.normalize();
for(float i = 0; i < length; ++i)
{
pingu->set_pos(pos + (dir * i));
// If there is something below the Pingu
if (rel_getpixel(0, -1) != Groundtype::GP_NOTHING)
{
// FIXME: this shouldn't be really here, but its a
// FIXME: quick&dirty way to kill falling pingus
if (velocity.y > Actions::Faller::deadly_velocity+1)
{
// log_debug("Velocity: " << velocity);
pingu->set_action(Actions::Splashed);
return;
}
else
{
// Make it so that the Pingu won't go down any further.
pingu->set_velocity(Vector3f(0, 0));
return;
}
}
else if (head_collision_on_walk(0, 1))
{
return;
}
else if (collision_on_walk(1, 0))
{
// Make the Pingu bounce off the wall
velocity.x = -velocity.x / 3.0f;
pingu->set_velocity(velocity);
pingu->direction.change();
return;
}
}
#else // Old Code
// FIXME: What does this variable do?
Vector3f resultant_force = pingu->get_velocity();
// FIXME: and what is this all about?! Can't we just use floats?
// Strictly speaking x_numerator should be initialised with
// (resultant_force.y / 2) and y_numerator with (resultant_force.x / 2).
// This would make the algorithm essentially match the Mid-Point Line
// Algorithm. However, zero should do and is more comprehendable.
int x_numerator = 0;
int y_numerator = 0;
int denominator = 0;
int x_inc = 0;
int y_inc = 0;
if (Math::abs(resultant_force.x) > Math::abs(resultant_force.y))
{
// Initialise so that we move in whole pixels in x direction and
// 'fractions' of a pixel in y direction.
denominator = static_cast<int>(Math::abs(resultant_force.x));
x_inc = denominator;
y_inc = static_cast<int>(Math::abs(resultant_force.y));
}
else
{
// Initialise so that we move in whole pixels in y direction and
// 'fractions' of a pixel in x direction.
denominator = static_cast<int>(Math::abs(resultant_force.y));
x_inc = static_cast<int>(Math::abs(resultant_force.x));
y_inc = denominator;
}
Vector3f force_counter = resultant_force;
// Keep moving the Pingu until there is only a fraction left
while ( force_counter.x <= -1
|| force_counter.x >= 1
|| force_counter.y <= -1
|| force_counter.y >= 1)
{
x_numerator += x_inc;
// Is it now not a fraction?
if (x_numerator >= denominator)
{
// Revert back to being a fraction
x_numerator -= denominator;
// If there is something to the left of the Pingu
if (collision_on_walk(1, 0))
{
// Make the Pingu reflect off the wall
force_counter.x = -(force_counter.x);
resultant_force.x = -(resultant_force.x/3);
pingu->set_velocity(resultant_force);
pingu->direction.change();
}
else
{
// Move the Pingu left
pingu->set_x(pingu->get_x() + static_cast<float>(pingu->direction));
force_counter.x -= static_cast<float>(pingu->direction);
}
}
y_numerator += y_inc;
// Is it now not a fraction?
if (y_numerator >= denominator)
{
// Revert back to being a fraction
y_numerator -= denominator;
// Move the Pingu depending on what the direction of the force is
if (force_counter.y >= 1)
{
// If there is something below the Pingu
if (rel_getpixel(0, -1) != Groundtype::GP_NOTHING)
{
// FIXME: this shouldn't be really here, but its a
// FIXME: quick&dirty way to kill falling pingus
if (resultant_force.y >= deadly_velocity)
{
pingu->set_action(ActionName::SPLASHED);
return;
}
// Make it so that the Pingu won't go down any further.
pingu->set_velocity(Vector3f(0, 0));
return;
}
else
{
// Move the Pingu down
pingu->set_y(pingu->get_y() + 1);
force_counter.y--;
}
}
else if (force_counter.y <= -1)
{
// If there is something in the way above the Pingu
if (head_collision_on_walk(0, 1))
{
// Make it so that the Pingu won't go up any further.
force_counter.y = 0;
resultant_force.y = 0;
pingu->set_velocity(resultant_force);
}
else
{
// Move the Pingu up
pingu->set_y(pingu->get_y() - 1);
force_counter.y++;
}
}
}
}
#endif
}
Vector3f get_pos() const { return Vector3f(); }
Vector3f Player::GetPosition()
{
return BodyPos + Quatf(Vector3f(0,1,0), BodyYaw.Get()).Rotate(HeadPose.Translation);
}
//----------------------------------------------------------------------------//
void BasicImage::render(GeometryBuffer& buffer, const Rectf& dest_area,
const Rectf* clip_area, const ColourRect& colours) const
{
const QuadSplitMode quad_split_mode(TopLeftToBottomRight);
Rectf dest(dest_area);
// apply rendering offset to the destination Rect
dest.offset(d_scaledOffset);
// get the rect area that we will actually draw to (i.e. perform clipping)
Rectf final_rect(clip_area ? dest.getIntersection(*clip_area) : dest );
// check if rect was totally clipped
if ((final_rect.getWidth() == 0) || (final_rect.getHeight() == 0))
return;
// Obtain correct scale values from the texture
const Vector2f& scale = d_texture->getTexelScaling();
const Vector2f tex_per_pix(d_area.getWidth() / dest.getWidth(), d_area.getHeight() / dest.getHeight());
// calculate final, clipped, texture co-ordinates
const Rectf tex_rect((d_area.d_min + ((final_rect.d_min - dest.d_min) * tex_per_pix)) * scale,
(d_area.d_max + ((final_rect.d_max - dest.d_max) * tex_per_pix)) * scale);
// URGENT FIXME: Shouldn't this be in the hands of the user?
final_rect.d_min.d_x = CoordConverter::alignToPixels(final_rect.d_min.d_x);
final_rect.d_min.d_y = CoordConverter::alignToPixels(final_rect.d_min.d_y);
final_rect.d_max.d_x = CoordConverter::alignToPixels(final_rect.d_max.d_x);
final_rect.d_max.d_y = CoordConverter::alignToPixels(final_rect.d_max.d_y);
Vertex vbuffer[6];
// vertex 0
vbuffer[0].position = Vector3f(final_rect.left(), final_rect.top(), 0.0f);
vbuffer[0].colour_val = colours.d_top_left;
vbuffer[0].tex_coords = Vector2f(tex_rect.left(), tex_rect.top());
// vertex 1
vbuffer[1].position = Vector3f(final_rect.left(), final_rect.bottom(), 0.0f);
vbuffer[1].colour_val = colours.d_bottom_left;
vbuffer[1].tex_coords = Vector2f(tex_rect.left(), tex_rect.bottom());
// vertex 2
vbuffer[2].position.d_x = final_rect.right();
vbuffer[2].position.d_z = 0.0f;
vbuffer[2].colour_val = colours.d_bottom_right;
vbuffer[2].tex_coords.d_x = tex_rect.right();
// top-left to bottom-right diagonal
if (quad_split_mode == TopLeftToBottomRight)
{
vbuffer[2].position.d_y = final_rect.bottom();
vbuffer[2].tex_coords.d_y = tex_rect.bottom();
}
// bottom-left to top-right diagonal
else
{
vbuffer[2].position.d_y = final_rect.top();
vbuffer[2].tex_coords.d_y = tex_rect.top();
}
// vertex 3
vbuffer[3].position = Vector3f(final_rect.right(), final_rect.top(), 0.0f);
vbuffer[3].colour_val = colours.d_top_right;
vbuffer[3].tex_coords = Vector2f(tex_rect.right(), tex_rect.top());
// vertex 4
vbuffer[4].position.d_x = final_rect.left();
vbuffer[4].position.d_z = 0.0f;
vbuffer[4].colour_val = colours.d_top_left;
vbuffer[4].tex_coords.d_x = tex_rect.left();
// top-left to bottom-right diagonal
if (quad_split_mode == TopLeftToBottomRight)
{
vbuffer[4].position.d_y = final_rect.top();
vbuffer[4].tex_coords.d_y = tex_rect.top();
}
// bottom-left to top-right diagonal
else
{
vbuffer[4].position.d_y = final_rect.bottom();
vbuffer[4].tex_coords.d_y = tex_rect.bottom();
}
// vertex 5
vbuffer[5].position = Vector3f(final_rect.right(), final_rect.bottom(), 0.0f);
vbuffer[5].colour_val= colours.d_bottom_right;
vbuffer[5].tex_coords = Vector2f(tex_rect.right(), tex_rect.bottom());
buffer.setActiveTexture(d_texture);
buffer.appendGeometry(vbuffer, 6);
}
void Player::HandleMovement(double dt, Array<Ptr<CollisionModel> >* collisionModels,
Array<Ptr<CollisionModel> >* groundCollisionModels, bool shiftDown)
{
// Handle keyboard movement.
// This translates BasePos based on the orientation and keys pressed.
// Note that Pitch and Roll do not affect movement (they only affect view).
Vector3f controllerMove;
if(MoveForward || MoveBack || MoveLeft || MoveRight)
{
if (MoveForward)
{
controllerMove += ForwardVector;
}
else if (MoveBack)
{
controllerMove -= ForwardVector;
}
if (MoveRight)
{
controllerMove += RightVector;
}
else if (MoveLeft)
{
controllerMove -= RightVector;
}
}
else if (GamepadMove.LengthSq() > 0)
{
controllerMove = GamepadMove;
}
controllerMove = GetOrientation(bMotionRelativeToBody).Rotate(controllerMove);
controllerMove.y = 0; // Project to the horizontal plane
if (controllerMove.LengthSq() > 0)
{
// Normalize vector so we don't move faster diagonally.
controllerMove.Normalize();
controllerMove *= OVR::Alg::Min<float>(MoveSpeed * (float)dt * (shiftDown ? 3.0f : 1.0f), 1.0f);
}
// Compute total move direction vector and move length
Vector3f orientationVector = controllerMove;
float moveLength = orientationVector.Length();
if (moveLength > 0)
orientationVector.Normalize();
float checkLengthForward = moveLength;
Planef collisionPlaneForward;
bool gotCollision = false;
for(unsigned int i = 0; i < collisionModels->GetSize(); ++i)
{
// Checks for collisions at model base level, which should prevent us from
// slipping under walls
if (collisionModels->At(i)->TestRay(BodyPos, orientationVector, checkLengthForward,
&collisionPlaneForward))
{
gotCollision = true;
break;
}
}
if (gotCollision)
{
// Project orientationVector onto the plane
Vector3f slideVector = orientationVector - collisionPlaneForward.N
* (orientationVector.Dot(collisionPlaneForward.N));
// Make sure we aren't in a corner
for(unsigned int j = 0; j < collisionModels->GetSize(); ++j)
{
if (collisionModels->At(j)->TestPoint(BodyPos - Vector3f(0.0f, RailHeight, 0.0f) +
(slideVector * (moveLength))) )
{
moveLength = 0;
break;
}
}
if (moveLength != 0)
{
orientationVector = slideVector;
}
}
// Checks for collisions at foot level, which allows us to follow terrain
orientationVector *= moveLength;
BodyPos += orientationVector;
Planef collisionPlaneDown;
float finalDistanceDown = GetScaledEyeHeight() + 10.0f;
// Only apply down if there is collision model (otherwise we get jitter).
if (groundCollisionModels->GetSize())
{
for(unsigned int i = 0; i < groundCollisionModels->GetSize(); ++i)
{
float checkLengthDown = GetScaledEyeHeight() + 10;
if (groundCollisionModels->At(i)->TestRay(BodyPos, Vector3f(0.0f, -1.0f, 0.0f),
checkLengthDown, &collisionPlaneDown))
{
finalDistanceDown = Alg::Min(finalDistanceDown, checkLengthDown);
}
}
// Maintain the minimum camera height
if (GetScaledEyeHeight() - finalDistanceDown < 1.0f)
{
BodyPos.y += GetScaledEyeHeight() - finalDistanceDown;
}
}
}
bool DeferredPhongLightingPass::Process(const SceneData& sceneData, unsigned int firstWorkTexture, unsigned secondWorkTexture) const
{
NazaraAssert(sceneData.viewer, "Invalid viewer");
NazaraUnused(secondWorkTexture);
m_workRTT->SetColorTarget(firstWorkTexture);
Renderer::SetTarget(m_workRTT);
Renderer::SetViewport(Recti(0, 0, m_dimensions.x, m_dimensions.y));
Renderer::SetTexture(0, m_GBuffer[0]);
Renderer::SetTextureSampler(0, m_pointSampler);
Renderer::SetTexture(1, m_GBuffer[1]);
Renderer::SetTextureSampler(1, m_pointSampler);
Renderer::SetTexture(2, m_GBuffer[2]);
Renderer::SetTextureSampler(2, m_pointSampler);
Renderer::SetTexture(3, m_depthStencilTexture);
Renderer::SetTextureSampler(3, m_pointSampler);
Renderer::SetClearColor(Color::Black);
Renderer::Clear(RendererBuffer_Color);
RenderStates lightStates;
lightStates.dstBlend = BlendFunc_One;
lightStates.srcBlend = BlendFunc_One;
lightStates.blending = true;
lightStates.depthBuffer = false;
lightStates.depthWrite = false;
// Directional lights
if (!m_renderQueue->directionalLights.empty())
{
Renderer::SetRenderStates(lightStates);
Renderer::SetShader(m_directionalLightShader);
m_directionalLightShader->SendColor(m_directionalLightShaderSceneAmbientLocation, sceneData.ambientColor);
m_directionalLightShader->SendVector(m_directionalLightShaderEyePositionLocation, sceneData.viewer->GetEyePosition());
for (auto& light : m_renderQueue->directionalLights)
{
m_directionalLightShader->SendColor(m_directionalLightUniforms.locations.color, light.color);
m_directionalLightShader->SendVector(m_directionalLightUniforms.locations.factors, Vector2f(light.ambientFactor, light.diffuseFactor));
m_directionalLightShader->SendVector(m_directionalLightUniforms.locations.parameters1, Vector4f(light.direction));
Renderer::DrawFullscreenQuad();
}
}
// Point lights/Spot lights
if (!m_renderQueue->pointLights.empty() || !m_renderQueue->spotLights.empty())
{
// http://www.altdevblogaday.com/2011/08/08/stencil-buffer-optimisation-for-deferred-lights/
lightStates.cullingSide = FaceSide_Front;
lightStates.stencilTest = true;
lightStates.stencilDepthFail.back = StencilOperation_Invert;
lightStates.stencilDepthFail.front = StencilOperation_Invert;
lightStates.stencilFail.back = StencilOperation_Keep;
lightStates.stencilFail.front = StencilOperation_Keep;
lightStates.stencilPass.back = StencilOperation_Keep;
lightStates.stencilPass.front = StencilOperation_Keep;
lightStates.stencilReference.back = 0;
lightStates.stencilReference.front = 0;
lightStates.stencilWriteMask.back = 0xFF;
lightStates.stencilWriteMask.front = 0xFF;
Renderer::SetRenderStates(lightStates);
Renderer::SetShader(m_pointSpotLightShader);
m_pointSpotLightShader->SendColor(m_pointSpotLightShaderSceneAmbientLocation, sceneData.ambientColor);
m_pointSpotLightShader->SendVector(m_pointSpotLightShaderEyePositionLocation, sceneData.viewer->GetEyePosition());
Matrix4f lightMatrix;
lightMatrix.MakeIdentity();
if (!m_renderQueue->pointLights.empty())
{
const IndexBuffer* indexBuffer = m_sphereMesh->GetIndexBuffer();
Renderer::SetIndexBuffer(indexBuffer);
Renderer::SetVertexBuffer(m_sphereMesh->GetVertexBuffer());
m_pointSpotLightShader->SendInteger(m_pointSpotLightUniforms.locations.type, LightType_Point);
for (const auto& light : m_renderQueue->pointLights)
{
m_pointSpotLightShader->SendColor(m_pointSpotLightUniforms.locations.color, light.color);
m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.factors, Vector2f(light.ambientFactor, light.diffuseFactor));
m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters1, Vector4f(light.position, light.attenuation));
m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters2, Vector4f(0.f, 0.f, 0.f, light.invRadius));
lightMatrix.SetScale(Vector3f(light.radius * 1.1f)); // To correct imperfections due to the sphere
lightMatrix.SetTranslation(light.position);
Renderer::SetMatrix(MatrixType_World, lightMatrix);
// Sphere rendering in the stencil buffer
Renderer::Enable(RendererParameter_ColorWrite, false);
Renderer::Enable(RendererParameter_DepthBuffer, true);
Renderer::Enable(RendererParameter_FaceCulling, false);
Renderer::SetStencilCompareFunction(RendererComparison_Always);
m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, true);
Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount());
// Sphere rendering as effect zone
Renderer::Enable(RendererParameter_ColorWrite, true);
Renderer::Enable(RendererParameter_DepthBuffer, false);
Renderer::Enable(RendererParameter_FaceCulling, true);
Renderer::SetStencilCompareFunction(RendererComparison_NotEqual, FaceSide_Back);
Renderer::SetStencilPassOperation(StencilOperation_Zero, FaceSide_Back);
m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, false);
Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount());
}
if (m_lightMeshesDrawing)
{
Renderer::Enable(RendererParameter_DepthBuffer, true);
Renderer::Enable(RendererParameter_DepthWrite, true);
Renderer::Enable(RendererParameter_FaceCulling, false);
Renderer::Enable(RendererParameter_StencilTest, false);
Renderer::SetFaceFilling(FaceFilling_Line);
const Shader* shader = ShaderLibrary::Get("DebugSimple");
static int colorLocation = shader->GetUniformLocation("Color");
Renderer::SetShader(shader);
for (const auto& light : m_renderQueue->pointLights)
{
lightMatrix.SetScale(Vector3f(light.radius * 1.1f)); // To correct imperfections due to the sphere
lightMatrix.SetTranslation(light.position);
Renderer::SetMatrix(MatrixType_World, lightMatrix);
shader->SendColor(colorLocation, light.color);
Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount());
}
Renderer::Enable(RendererParameter_DepthBuffer, false);
Renderer::Enable(RendererParameter_DepthWrite, false);
Renderer::Enable(RendererParameter_FaceCulling, true);
Renderer::Enable(RendererParameter_StencilTest, true);
Renderer::SetFaceFilling(FaceFilling_Fill);
}
}
if (!m_renderQueue->spotLights.empty())
{
const IndexBuffer* indexBuffer = m_coneMesh->GetIndexBuffer();
Renderer::SetIndexBuffer(indexBuffer);
Renderer::SetVertexBuffer(m_coneMesh->GetVertexBuffer());
m_pointSpotLightShader->SendInteger(m_pointSpotLightUniforms.locations.type, LightType_Spot);
for (const auto& light : m_renderQueue->spotLights)
{
m_pointSpotLightShader->SendColor(m_pointSpotLightUniforms.locations.color, light.color);
m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.factors, Vector2f(light.ambientFactor, light.diffuseFactor));
m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters1, Vector4f(light.position, light.attenuation));
m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters2, Vector4f(light.direction, light.invRadius));
m_pointSpotLightShader->SendVector(m_pointSpotLightUniforms.locations.parameters3, Vector2f(light.innerAngleCosine, light.outerAngleCosine));
float baseRadius = light.radius * light.outerAngleTangent * 1.1f;
lightMatrix.MakeTransform(light.position, Quaternionf::RotationBetween(Vector3f::Forward(), light.direction), Vector3f(baseRadius, baseRadius, light.radius));
Renderer::SetMatrix(MatrixType_World, lightMatrix);
// Sphere rendering in the stencil buffer
Renderer::Enable(RendererParameter_ColorWrite, false);
Renderer::Enable(RendererParameter_DepthBuffer, true);
Renderer::Enable(RendererParameter_FaceCulling, false);
Renderer::SetStencilCompareFunction(RendererComparison_Always);
m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, true);
Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount());
// Sphere rendering as effect zone
Renderer::Enable(RendererParameter_ColorWrite, true);
Renderer::Enable(RendererParameter_DepthBuffer, false);
Renderer::Enable(RendererParameter_FaceCulling, true);
Renderer::SetFaceCulling(FaceSide_Front);
Renderer::SetStencilCompareFunction(RendererComparison_NotEqual, FaceSide_Back);
Renderer::SetStencilPassOperation(StencilOperation_Zero, FaceSide_Back);
m_pointSpotLightShader->SendBoolean(m_pointSpotLightShaderDiscardLocation, false);
Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount());
}
if (m_lightMeshesDrawing)
{
Renderer::Enable(RendererParameter_DepthBuffer, true);
Renderer::Enable(RendererParameter_DepthWrite, true);
Renderer::Enable(RendererParameter_FaceCulling, false);
Renderer::Enable(RendererParameter_StencilTest, false);
Renderer::SetFaceFilling(FaceFilling_Line);
const Shader* shader = ShaderLibrary::Get("DebugSimple");
static int colorLocation = shader->GetUniformLocation("Color");
Renderer::SetShader(shader);
for (const auto& light : m_renderQueue->spotLights)
{
float baseRadius = light.radius * light.outerAngleTangent * 1.1f;
lightMatrix.MakeTransform(light.position, Quaternionf::RotationBetween(Vector3f::Forward(), light.direction), Vector3f(baseRadius, baseRadius, light.radius));
Renderer::SetMatrix(MatrixType_World, lightMatrix);
shader->SendColor(colorLocation, light.color);
Renderer::DrawIndexedPrimitives(PrimitiveMode_TriangleList, 0, indexBuffer->GetIndexCount());
}
Renderer::Enable(RendererParameter_DepthBuffer, false);
Renderer::Enable(RendererParameter_DepthWrite, false);
Renderer::Enable(RendererParameter_FaceCulling, true);
Renderer::Enable(RendererParameter_StencilTest, true);
Renderer::SetFaceFilling(FaceFilling_Fill);
}
}
Renderer::Enable(RendererParameter_StencilTest, false);
}
return true;
}
Mesh MeshLoader::uploadMesh(const aiMesh *mesh) {
Mesh model;
// Generate vertex array object
GLuint vao;
glGenVertexArrays(1, &vao);
glBindVertexArray(vao);
// Create vertex indices VBO
{
// Store face indices in an array
std::unique_ptr<unsigned int[]> faceArray(new unsigned int[mesh->mNumFaces * 3]);
for (unsigned int f = 0; f < mesh->mNumFaces; ++f) {
const aiFace &face = mesh->mFaces[f];
std::memcpy(&faceArray[f * 3], face.mIndices, 3 * sizeof(face.mIndices[0]));
}
// Prepare vertex indices VBO
createVBO(GL_ELEMENT_ARRAY_BUFFER,
sizeof(unsigned int) * mesh->mNumFaces * 3, faceArray.get());
}
// Cache vertex attributes existence to avoid dereferencing and calling every
// time
bool hasPositions = mesh->HasPositions(),
hasTexCoords = mesh->HasTextureCoords(0),
hasNormals = mesh->HasNormals(),
hasTangents = mesh->HasTangentsAndBitangents();
// Use them to determine the vertex definition size
unsigned int vtxSize = +(hasPositions ? sizeof(float) * 3 : 0) +
(hasTexCoords ? sizeof(float) * 2 : 0) +
(hasNormals ? sizeof(float) * 3 : 0) +
(hasTangents ? sizeof(float) * 3 : 0);
// Create a buffer to store vertex data, with determined size since we now
// know it
TightDataPacker data(mesh->mNumVertices * vtxSize);
if (hasPositions) {
// Since we keep the vertices in the model, better determine the storage
// size once and for all
model.vertices.resize(mesh->mNumVertices);
}
// Process each vertex and store its data
for (unsigned int i = 0; i < mesh->mNumVertices; ++i) {
if (hasPositions) {
const aiVector3D &v = mesh->mVertices[i];
model.vertices[i] = Vector3f(v.x, v.y, v.z);
data << v.x << v.y << v.z;
}
if (hasTexCoords) {
const aiVector3D &t = mesh->mTextureCoords[0][i];
data << t.x
<< 1.f - t.y; // Y must be flipped due to OpenGL's coordinate system
}
if (hasNormals) {
const aiVector3D &n = mesh->mNormals[i];
data << n.x << n.y << n.z;
}
if (hasTangents) {
const aiVector3D &t = mesh->mTangents[i];
data << t.x << t.y << t.z;
}
}
// Describe the vertex format we have
{
intptr_t offset = 0;
specVector vertexAttrib;
if (hasPositions) {
vertexAttrib.push_back({0, 3, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)});
offset += sizeof(float) * 3;
}
if (hasTexCoords) {
vertexAttrib.push_back({1, 2, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)});
offset += sizeof(float) * 2;
}
if (hasNormals) {
vertexAttrib.push_back({2, 3, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)});
offset += sizeof(float) * 3;
}
if (hasTangents) {
vertexAttrib.push_back({3, 3, GL_FLOAT, reinterpret_cast<GLvoid*>(offset)});
}
createVBO(GL_ARRAY_BUFFER, data.getSize(), data.getDataPtr(),
static_cast<int>(vtxSize), vertexAttrib);
}
glBindVertexArray(0);
// Store relevant data in the new mesh
model.handle = static_cast<int>(vao);
model.numFaces = static_cast<int>(mesh->mNumFaces);
return model;
}
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, true);
break;
}
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
switch(packet.command) {
case MAV_CMD_START_RX_PAIR:
// initiate bind procedure
if (!hal.rcin->rc_bind(packet.param1)) {
result = MAV_RESULT_FAILED;
} else {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
rover.set_mode(RTL);
result = MAV_RESULT_ACCEPTED;
break;
#if MOUNT == ENABLED
// Sets the region of interest (ROI) for the camera
case MAV_CMD_DO_SET_ROI:
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
// switch off the camera tracking if enabled
if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
rover.camera_mount.set_mode_to_default();
}
} else {
// send the command to the camera mount
rover.camera_mount.set_roi_target(roi_loc);
}
result = MAV_RESULT_ACCEPTED;
break;
#endif
#if CAMERA == ENABLED
case MAV_CMD_DO_DIGICAM_CONFIGURE:
rover.camera.configure(packet.param1,
packet.param2,
packet.param3,
packet.param4,
packet.param5,
packet.param6,
packet.param7);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_DIGICAM_CONTROL:
if (rover.camera.control(packet.param1,
packet.param2,
packet.param3,
packet.param4,
packet.param5,
packet.param6)) {
rover.log_picture();
}
result = MAV_RESULT_ACCEPTED;
break;
#endif // CAMERA == ENABLED
case MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLED
rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
result = MAV_RESULT_ACCEPTED;
#endif
break;
case MAV_CMD_MISSION_START:
rover.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
if(hal.util->get_soft_armed()) {
result = MAV_RESULT_FAILED;
break;
}
if (is_equal(packet.param1,1.0f)) {
rover.ins.init_gyro();
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param3,1.0f)) {
rover.init_barometer(false);
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param4,1.0f)) {
rover.trim_radio();
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param5,1.0f)) {
result = MAV_RESULT_ACCEPTED;
// start with gyro calibration
rover.ins.init_gyro();
// reset ahrs gyro bias
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
} else {
result = MAV_RESULT_FAILED;
}
rover.ins.acal_init();
rover.ins.get_acal()->start(this);
} else if (is_equal(packet.param5,2.0f)) {
// start with gyro calibration
rover.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if(rover.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
else {
send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
}
break;
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
{
uint8_t compassNumber = -1;
if (is_equal(packet.param1, 2.0f)) {
compassNumber = 0;
} else if (is_equal(packet.param1, 5.0f)) {
compassNumber = 1;
} else if (is_equal(packet.param1, 6.0f)) {
compassNumber = 2;
}
if (compassNumber != (uint8_t) -1) {
rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
rover.set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
rover.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_STABILIZE_DISARMED:
case MAV_MODE_STABILIZE_ARMED:
rover.set_mode(LEARNING);
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_SERVO:
if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_SERVO:
if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_SET_RELAY:
if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_RELAY:
if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) {
// run pre_arm_checks and arm_checks and display failures
if (rover.arm_motors(AP_Arming::MAVLINK)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_zero(packet.param1)) {
if (rover.disarm_motors()) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_GET_HOME_POSITION:
if (rover.home_is_set != HOME_UNSET) {
send_home(rover.ahrs.get_home());
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (is_equal(packet.param1,1.0f)) {
send_autopilot_version(FIRMWARE_VERSION);
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_DO_SET_HOME:
{
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
// param6 : longitude
// param7 : altitude
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1,1.0f)) {
rover.init_home();
} else {
if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) {
// don't allow the 0,0 position
break;
}
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location new_home_loc {};
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
rover.ahrs.set_home(new_home_loc);
rover.home_is_set = HOME_SET_NOT_LOCKED;
rover.Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(new_home_loc);
result = MAV_RESULT_ACCEPTED;
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
(double)(new_home_loc.lat*1.0e-7f),
(double)(new_home_loc.lng*1.0e-7f),
(uint32_t)(new_home_loc.alt*0.01f));
}
break;
}
case MAV_CMD_DO_START_MAG_CAL:
case MAV_CMD_DO_ACCEPT_MAG_CAL:
case MAV_CMD_DO_CANCEL_MAG_CAL:
result = rover.compass.handle_mag_cal_command(packet);
break;
default:
break;
}
mavlink_msg_command_ack_send_buf(
msg,
chan,
packet.command,
result);
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t));
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
{
handle_mission_request_list(rover.mission, msg);
break;
}
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
case MAVLINK_MSG_ID_MISSION_REQUEST:
{
handle_mission_request(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_ACK:
{
// not used
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
// mark the firmware version in the tlog
send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING);
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
#endif
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
handle_param_request_read(msg);
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
{
handle_mission_clear_all(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
{
handle_mission_set_current(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT:
{
handle_mission_count(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
handle_mission_write_partial_list(rover.mission, msg);
break;
}
// GCS has sent us a mission item, store to EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM:
{
if (handle_mission_item(msg, rover.mission)) {
rover.DataFlash.Log_Write_EntireMission(rover.mission);
}
break;
}
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
{
if (handle_mission_item(msg, rover.mission)) {
rover.DataFlash.Log_Write_EntireMission(rover.mission);
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
handle_param_set(msg, &rover.DataFlash);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if(msg->sysid != rover.g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_rc_channels_override_t packet;
int16_t v[8];
mavlink_msg_rc_channels_override_decode(msg, &packet);
v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw;
v[2] = packet.chan3_raw;
v[3] = packet.chan4_raw;
v[4] = packet.chan5_raw;
v[5] = packet.chan6_raw;
v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw;
hal.rcin->set_overrides(v, 8);
rover.failsafe.rc_override_timer = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_RC, false);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if(msg->sysid != rover.g.sysid_my_gcs) break;
rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis();
rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
break;
}
case MAVLINK_MSG_ID_GPS_INPUT:
{
rover.gps.handle_msg(msg);
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
// sanity check location
if (!check_latlng(packet.lat, packet.lon)) {
break;
}
// set gps hil sensor
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000,
loc, vel, 10, 0);
// rad/sec
Vector3f gyros;
gyros.x = packet.rollspeed;
gyros.y = packet.pitchspeed;
gyros.z = packet.yawspeed;
// m/s/s
Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros);
ins.set_accel(0, accels);
compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
break;
}
#endif // HIL_MODE
#if CAMERA == ENABLED
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
{
break;
}
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
{
rover.camera.control_msg(msg);
rover.log_picture();
break;
}
#endif // CAMERA == ENABLED
#if MOUNT == ENABLED
//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
{
rover.camera_mount.configure_msg(msg);
break;
}
//deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
case MAVLINK_MSG_ID_MOUNT_CONTROL:
{
rover.camera_mount.control_msg(msg);
break;
}
#endif // MOUNT == ENABLED
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS:
{
handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM));
break;
}
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
case MAVLINK_MSG_ID_LOG_ERASE:
rover.in_log_download = true;
/* no break */
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
if (!rover.in_mavlink_delay) {
handle_log_message(msg, rover.DataFlash);
}
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
rover.in_log_download = false;
if (!rover.in_mavlink_delay) {
handle_log_message(msg, rover.DataFlash);
}
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, rover.gps);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg, rover.gps);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
rover.sonar.handle_msg(msg);
break;
case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
rover.DataFlash.remote_log_block_status_msg(chan, msg);
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
send_autopilot_version(FIRMWARE_VERSION);
break;
case MAVLINK_MSG_ID_SETUP_SIGNING:
handle_setup_signing(msg);
break;
case MAVLINK_MSG_ID_LED_CONTROL:
// send message to Notify
AP_Notify::handle_led_control(msg);
break;
case MAVLINK_MSG_ID_PLAY_TUNE:
// send message to Notify
AP_Notify::handle_play_tune(msg);
break;
} // end switch
} // end handle mavlink
/*
this offset nulling algorithm is inspired by this paper from Bill Premerlani
http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf
The base algorithm works well, but is quite sensitive to
noise. After long discussions with Bill, the following changes were
made:
1) we keep a history buffer that effectively divides the mag
vectors into a set of N streams. The algorithm is run on the
streams separately
2) within each stream we only calculate a change when the mag
vector has changed by a significant amount.
This gives us the property that we learn quickly if there is no
noise, but still learn correctly (and slowly) in the face of lots of
noise.
*/
void
Compass::null_offsets(void)
{
if (_null_enable == false || _learn == 0) {
// auto-calibration is disabled
return;
}
// this gain is set so we converge on the offsets in about 5
// minutes with a 10Hz compass
const float gain = 0.01;
const float max_change = 10.0;
const float min_diff = 50.0;
Vector3f ofs;
ofs = _offset.get();
if (!_null_init_done) {
// first time through
_null_init_done = true;
for (uint8_t i=0; i<_mag_history_size; i++) {
// fill the history buffer with the current mag vector,
// with the offset removed
_mag_history[i] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
}
_mag_history_index = 0;
return;
}
Vector3f b1, b2, diff;
float length;
// get a past element
b1 = Vector3f(_mag_history[_mag_history_index].x,
_mag_history[_mag_history_index].y,
_mag_history[_mag_history_index].z);
// the history buffer doesn't have the offsets
b1 += ofs;
// get the current vector
b2 = Vector3f(mag_x, mag_y, mag_z);
// calculate the delta for this sample
diff = b2 - b1;
length = diff.length();
if (length < min_diff) {
// the mag vector hasn't changed enough - we don't get
// enough information from this vector to use it.
// Note that we don't put the current vector into the mag
// history here. We want to wait for a larger rotation to
// build up before calculating an offset change, as accuracy
// of the offset change is highly dependent on the size of the
// rotation.
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
return;
}
// put the vector in the history
_mag_history[_mag_history_index] = Vector3i((mag_x+0.5) - ofs.x, (mag_y+0.5) - ofs.y, (mag_z+0.5) - ofs.z);
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
// equation 6 of Bills paper
diff = diff * (gain * (b2.length() - b1.length()) / length);
// limit the change from any one reading. This is to prevent
// single crazy readings from throwing off the offsets for a long
// time
length = diff.length();
if (length > max_change) {
diff *= max_change / length;
}
// set the new offsets
_offset.set(_offset.get() - diff);
}
bool LogReader::update(uint8_t &type)
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format &f = formats[num_formats];
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
num_formats++;
type = f.type;
return true;
}
uint8_t i;
for (i=0; i<num_formats; i++) {
if (formats[i].type == hdr[2]) break;
}
if (i == num_formats) {
return false;
}
const struct log_Format &f = formats[i];
uint8_t data[f.length];
memcpy(data, hdr, 3);
if (::read(fd, &data[3], f.length-3) != f.length-3) {
return false;
}
switch (f.type) {
case LOG_MESSAGE_MSG: {
struct log_Message msg;
if(sizeof(msg) != f.length) {
printf("Bad MESSAGE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VEHICLE_PLANE;
::printf("Detected Plane\n");
} else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
vehicle = VEHICLE_COPTER;
::printf("Detected Copter\n");
} else if (strncmp(msg.msg, "APMRover2", strlen("APMRover2")) == 0) {
vehicle = VEHICLE_ROVER;
::printf("Detected Rover\n");
}
break;
}
case LOG_IMU_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 1) {
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 1) {
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
case LOG_IMU2_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 2) {
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 2) {
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
case LOG_GPS_MSG: {
struct log_GPS msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
gps->setHIL(msg.status==3?GPS::FIX_3D:GPS::FIX_NONE,
msg.apm_time,
msg.latitude*1.0e-7f,
msg.longitude*1.0e-7f,
msg.altitude*1.0e-2f,
msg.ground_speed*1.0e-2f,
msg.ground_course*1.0e-2f,
0,
msg.num_sats);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
rel_altitude = msg.rel_altitude*0.01f;
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad SIMSTATE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
case LOG_BARO_MSG: {
struct log_BARO msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_BARO length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
baro.setHIL(msg.pressure, msg.temperature*0.01f);
break;
}
case LOG_PARAMETER_MSG: {
struct log_Parameter msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_PARAMETER length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
set_parameter(msg.name, msg.value);
break;
}
default:
if (vehicle == VEHICLE_PLANE) {
process_plane(f.type, data, f.length);
} else if (vehicle == VEHICLE_COPTER) {
process_copter(f.type, data, f.length);
}
break;
}
type = f.type;
return true;
}
void pbrtTranslate(Float dx, Float dy, Float dz) {
VERIFY_INITIALIZED("Translate");
FOR_ACTIVE_TRANSFORMS(curTransform[i] = curTransform[i] *
Translate(Vector3f(dx, dy, dz));)
}
Vector3f Enclosure::move(const Vector3f& fromPos, const Vector3f& direction, float width, float height)
{
// first, obtain motion distance
float distance = direction.length();
// we should determine maximum distance that we can move safely, without probability of
// leave enclosure boundary
float safeDistance = 0.5f * ( width < height ? width : height );
// now prepare to move
Vector3f pos = fromPos;
Vector3f dir = direction;
dir.normalize();
Vector3f penetration;
float stepDistance;
unsigned int i;
// move until there is a distance left
do
{
// determine step distance
stepDistance = safeDistance < distance ? safeDistance : distance;
// move along normalized direction
pos += dir * stepDistance;
// detect "foot" collision
_numTriangles = 0;
_ray->setRay( pos, Vector3f( 0,-1,0 ) * height );
_ray->intersect( _collisionAtomic, onRayCollision, this );
if( _numTriangles )
{
// determine penetration vector
penetration = _nearestTriangle.normal * height * ( 1.0f - _nearestTriangle.distance );
// disable penetration
pos += penetration;
}
// detect "body" collision
for( i=0; i<_wallNormals.size(); i++ )
{
_numTriangles = 0;
_ray->setRay( pos, _wallNormals[i] * -width );
_ray->intersect( _collisionAtomic, onRayCollision, this );
if( _numTriangles )
{
// determine penetration vector
penetration = _nearestTriangle.normal * width * ( 1.0f - _nearestTriangle.distance );
// disable penetration
pos += penetration;
}
}
// decrease distance left
distance -= stepDistance;
}
while( distance > 0 );
return pos;
}
void pbrtLookAt(Float ex, Float ey, Float ez, Float lx, Float ly, Float lz,
Float ux, Float uy, Float uz) {
VERIFY_INITIALIZED("LookAt");
Transform lookAt =
LookAt(Point3f(ex, ey, ez), Point3f(lx, ly, lz), Vector3f(ux, uy, uz));
FOR_ACTIVE_TRANSFORMS(curTransform[i] = curTransform[i] * lookAt;);
/// sets the servo angles for retraction, note angles are in degrees
void AP_Mount::set_retract_angles(float roll, float tilt, float pan)
{
_retract_angles = Vector3f(roll, tilt, pan);
}
