/// Test if bbox intersect with frustum defined by the given matrix
/// (interpreted as an OpenGL projection matrix)
bool BBox::inFrustum(const ftl::Mat4x4f& frustum, bool exact) const
{
if (isEmpty()) return false;
if (exact)
{ // axis-aligned tests
Mat3x3f m0;
m0(0,0)=frustum(0,0); m0(0,1)=frustum(0,1); m0(0,2)=frustum(0,2);
m0(1,0)=frustum(1,0); m0(1,1)=frustum(1,1); m0(1,2)=frustum(1,2);
m0(3,0)=frustum(3,0); m0(3,1)=frustum(3,1); m0(3,2)=frustum(3,2);
Mat3x3f minv; minv.invert(m0);
// m0.p0 + frustum.col(3) = 0;
// p0 = minv.(-frustum.col(3));
Vec3f p0;
p0 = minv*Vec3f(-frustum(0,3),-frustum(1,3),-frustum(2,3));
// p1 = inv.( 1, 1,1)
// p2 = inv.(-1, 1,1)
// p3 = inv.( 1,-1,1)
// p4 = inv.(-1,-1,1)
// if p*[C] >= 0 then bb.b[C] >= p0[C]
// if p*[C] <= 0 then bb.a[C] <= p0[C]
const Mat4x3f pmat(Vec3f(1,1,1),Vec3f(-1,1,1),Vec3f(1,-1,1),Vec3f(-1,-1,1));
for (int c=0;c<3;c++)
{
Vec4f p = pmat*minv[c];
if (p[0]>=0 && p[1]>=0 && p[2]>=0 && p[3]>=0)
{
if (b[c]<p0[c]) return false;
}
else if (p[0]<=0 && p[1]<=0 && p[2]<=0 && p[3]<=0)
{
if (a[c]>p0[c]) return false;
}
}
}
// halfplanes from frustum
return inHalfspace(frustum[3]) // W>=0
&& inHalfspace(frustum[3]-frustum[0]) // X<=W : W-X>=0
&& inHalfspace(frustum[3]-frustum[1]) // Y<=W : W-Y>=0
&& inHalfspace(frustum[3]+frustum[0]) // X>=-W : W+X>=0
&& inHalfspace(frustum[3]+frustum[1]) // Y>=-W : W+Y>=0
;
}
void SurfaceObj::computeRotationMatrix(Vec3f axis, float angle, Mat3x3f& rot)
{
float x,y,z;
x=axis[0];
y=axis[1];
z=axis[2];
rot(0,0)=x*x+(y*y+z*z)*cos(angle);
rot(1,1)=y*y+(x*x+z*z)*cos(angle);
rot(2,2)=z*z+(x*x+y*y)*cos(angle);
rot(0,1)=(1-cos(angle))*x*y+z*sin(angle);
rot(1,0)=(1-cos(angle))*x*y-z*sin(angle);
rot(0,2)=(1-cos(angle))*x*z-y*sin(angle);
rot(2,0)=(1-cos(angle))*z*x+y*sin(angle);
rot(1,2)=(1-cos(angle))*y*z+x*sin(angle);
rot(2,1)=(1-cos(angle))*z*y-x*sin(angle);
rot.transpose();
};
system::error_code AttitudeControlSimple::update(asio::yield_context yctx)
{
Mat3x3f R = m_ISAttitudeEstimated.m_Value.toRotationMatrix();
#if 1
Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
#else
#if 0
Mat3x3f R_sp = Quaternionf(AngleAxisf(M_PI_4, Vec3f::UnitX())).toRotationMatrix();
#else
static once_flag initSetpoint;
call_once(initSetpoint, [this]() { m_ISAttitudeSetpoint.m_Value = m_ISAttitudeEstimated.m_Value; } );
Mat3x3f R_sp = m_ISAttitudeSetpoint.m_Value.toRotationMatrix();
#endif
#endif
float dT = std::min(std::max(m_ISDT.m_Value, 0.002f), 0.02f);
Vec3f R_z(R(0, 2), R(1, 2), R(2, 2));
Vec3f R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
Vec3f e_R = R.transpose() * (R_z.cross(R_sp_z));
float e_R_z_sin = e_R.norm();
float e_R_z_cos = R_z.dot(R_sp_z);
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
Mat3x3f R_rp;
if(e_R_z_sin > 0.0f)
{
float e_R_z_angle = std::atan2(e_R_z_sin, e_R_z_cos);
Vec3f e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_angle * e_R_z_axis;
Mat3x3f e_R_cp = Mat3x3f::Zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
R_rp = R * (Mat3x3f::Identity() + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
}
else
{
R_rp = R;
}
Vec3f R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
Vec3f R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = std::atan2(R_rp_x.cross(R_sp_x).dot(R_sp_z), R_rp_x.dot(R_sp_x)) * yaw_w;
if(e_R_z_cos < 0.0f)
{
Quaternionf q(R.transpose() * R_sp);
Vec3f e_R_d = q.vec();
e_R_d.normalize();
e_R_d *= 2.0f * std::atan2(e_R_d.norm(), q.w());
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
Vec3f const rotationRateSetpoint = m_AttitudeP.cwiseProduct(e_R);
Vec3f e_RR = rotationRateSetpoint - m_ISRotationRateMeasured.m_Value;
Vec3f rotationRateControl =
m_RotationRateP.cwiseProduct(e_RR) +
m_RotationRateD.cwiseProduct(m_RotationRateMeasuredPrev - m_ISRotationRateMeasured.m_Value) / dT +
m_RotationRateIError;
m_RotationRateMeasuredPrev = m_ISRotationRateMeasured.m_Value;
m_RotationRateSetpointPrev = rotationRateSetpoint;
m_OSRotationRateSetpoint.m_Value = rotationRateControl;
return base::makeErrorCode(base::kENoError);
}
int main(int argc, char** argv)
{
flowvr::InputPort pRotX("rotX");
flowvr::InputPort pRotY("rotY");
flowvr::InputPort pTransX("transX");
flowvr::InputPort pTransY("transY");
flowvr::InputPort pZoom("zoom");
SceneOutputPort pOut("scene");
flowvr::OutputPort pOutMat("matrix");
flowvr::OutputPort pTerrain("terrain");
flowvr::OutputPort pOutFluid("fluidpos");
flowvr::StampInfo terrainN("N",flowvr::TypeArray::create(2,flowvr::TypeInt::create()));
flowvr::StampInfo terrainP("P",flowvr::TypeArray::create(2,flowvr::TypeFloat::create()));
flowvr::StampInfo terrainS("S",flowvr::TypeArray::create(2,flowvr::TypeFloat::create()));
flowvr::StampInfo terrainH("H",flowvr::TypeArray::create(2,flowvr::TypeFloat::create()));
pTerrain.stamps->add(&terrainN);
pTerrain.stamps->add(&terrainP);
pTerrain.stamps->add(&terrainS);
pTerrain.stamps->add(&terrainH);
std::vector<flowvr::Port*> ports;
ports.push_back(&pRotX);
ports.push_back(&pRotY);
ports.push_back(&pTransX);
ports.push_back(&pTransY);
ports.push_back(&pZoom);
ports.push_back(&pOut);
ports.push_back(&pOutMat);
ports.push_back(&pOutFluid);
ports.push_back(&pTerrain);
flowvr::ModuleAPI* module = flowvr::initModule(ports);
if (module == NULL)
{
return 1;
}
flowvr::render::ChunkRenderWriter scene;
ID idP = 0x300; //module->generateID();
ID idT = 0x301; //module->generateID();
ID idVB = 0x302; //module->generateID();
ID idIB = 0x303; //module->generateID();
ID idVS = 0x304; //module->generateID();
ID idPS = 0x305; //module->generateID();
std::cout << "idVS="<<idVS<<std::endl;
std::cout << "idPS="<<idPS<<std::endl;
std::string ftexture = "images/valley2.jpg";
std::string fheight = "valley2.ter";
if (argc>=2) ftexture = argv[1];
if (argc>=3) fheight = argv[2];
int nx = 257; int ny = 257;
TerragenTerrain terrain;
if (!terrain.load(fheight))
{
module->close();
return 2;
}
Vec3f position0;
Vec3f position(-25,0,0);
Vec3f rotation;
float zoom = 1;
if (cx >= terrain.nx) cx = terrain.nx/2;
if (cy >= terrain.ny) cy = terrain.ny/2;
if (cx-nx/2<0) nx = cx*2;
if (cy-ny/2<0) ny = cy*2;
if (cx-nx/2+nx>terrain.nx) nx = (terrain.nx-cx)*2-1;
if (cy-ny/2+ny>terrain.ny) ny = (terrain.ny-cy)*2-1;
terrain.scale = Vec3f(1.0f,1.0f,2.0f);
//terrain.hbase -= 18.033f;
terrain.hbase = -terrain.hscale*terrain.height[(cy)*terrain.nx+(cx-25)];
std::cout << "Using data centered @ "<<cx<<"x"<<cy<<" size "<<nx<<"x"<<ny<<" scale "<<terrain.scale.x()<<"x"<<terrain.scale.y()<<"x"<<terrain.scale.z()<<" z="<<terrain.hbase<<"+h*"<<terrain.hscale<<std::endl;
if (!scene.loadTexture(idT,ftexture))
scene.addDefaultTexture(idT);
int dataType[1] = { Type::Vec3s };
short hmin = 0xffff;
short hmax = 0;
int xmin=0,xmax=1,ymin=0,ymax=1; // fluid interval
{
short* height = terrain.height+(cy-ny/2)*terrain.nx+(cx-nx/2);
for (int y=0;y<ny;y++)
{
for (int x=0;x<nx;x++)
{
if (height[x] < hmin)
{
hmin = height[x];
xmin=xmax=x;
ymin=ymax=y;
}
else if (height[x] == hmin)
{
if (x<xmin) xmin=x; else if (x>xmax) xmax=x;
if (y<ymin) ymin=y; else if (y>ymax) ymax=y;
}
if (height[x] > hmax) hmax = height[x];
}
height+=terrain.nx;
}
}
std::cout << "height min="<<hmin<<" max="<<hmax<<std::endl;
ChunkVertexBuffer* vb = scene.addVertexBuffer(idVB, nx*ny, 1, dataType, BBox(Vec3f(0,0,hmin),Vec3f(nx-1,ny-1,hmax)));
{
Vec<3,short>* vertex = (Vec<3,short>*)vb->data();
short* height = terrain.height+(cy-ny/2)*terrain.nx+(cx-nx/2);
for (int y=0;y<ny;y++)
{
for (int x=0;x<nx;x++)
{
vertex->x() = x;
vertex->y() = y;
vertex->z() = height[x];
++vertex;
}
height+=terrain.nx;
}
}
/*
std::cout << "Primitive mode: QUAD\n";
ChunkIndexBuffer* ib = scene.addIndexBuffer(idIB, 4*(nx-1)*(ny-1), Type::Int, ChunkIndexBuffer::Quad);
{
unsigned int* ind = (unsigned int*)ib->data();
for (int y=0;y<ny-1;y++)
{
for (int x=0;x<nx-1;x++)
{
ind[0] = (y )*nx+(x );
ind[1] = (y )*nx+(x+1);
ind[2] = (y+1)*nx+(x+1);
ind[3] = (y+1)*nx+(x );
ind+=4;
}
}
}
*/
/*
std::cout << "Primitive mode: TRIANGLE STRIP\n";
int restart = -1;
ChunkIndexBuffer* ib = scene.addIndexBuffer(idIB, 2*(nx)*(ny-1)+(ny-2), Type::Int, ChunkIndexBuffer::TriangleStrip);
ib->restart = restart;
{
unsigned int* ind = (unsigned int*)ib->data();
for (int y=0;y<ny-1;y++)
{
if (y)
*(ind++) = (unsigned int)restart; // start a new strip
for (int x=0;x<nx;x++)
{
ind[0] = (y )*nx+(x );
ind[1] = (y+1)*nx+(x );
ind+=2;
}
}
}
*/
std::cout << "Primitive mode: TRIANGLE FAN\n";
int restart = -1;
int nindex =
((nx-1)/2)*((ny-1)/2)*(10+1) // full circles
+((nx&1)?0:((ny-1)/2)*(6+1)) // half circles if nx is even
+((ny&1)?0:((nx-1)/2)*(6+1)) // half circles if ny is even
+((nx&1 || ny&1)?0: (4+1)) // final quad if both nx and ny are even
-1; // last fan does not need a restart index
ChunkIndexBuffer* ib = scene.addIndexBuffer(idIB, nindex, Type::Int, ChunkIndexBuffer::TriangleFan);
ib->restart = restart;
{
unsigned int* ind = (unsigned int*)ib->data();
unsigned int* start=ind;
for (int y=1;y<ny;y+=2)
{
for (int x=1;x<nx;x+=2)
{
if (ind!=start)
*(ind++) = (unsigned int)restart; // start a new fan
*(ind++) = (y )*nx+(x );
if (y<ny-1)
*(ind++) = (y+1)*nx+(x );
if (y<ny-1)
*(ind++) = (y+1)*nx+(x-1);
*(ind++) = (y )*nx+(x-1);
*(ind++) = (y-1)*nx+(x-1);
*(ind++) = (y-1)*nx+(x );
if (x<nx-1)
*(ind++) = (y-1)*nx+(x+1);
if (x<nx-1)
*(ind++) = (y )*nx+(x+1);
if (x<nx-1 && y<ny-1)
*(ind++) = (y+1)*nx+(x+1);
if (x<nx-1 && y<ny-1)
*(ind++) = (y+1)*nx+(x );
}
}
}
scene.loadVertexShader(idVS, "shaders/terrain_v.cg");
scene.loadPixelShader(idPS, "shaders/terrain_p.cg");
scene.addPrimitive(idP,"Terrain");
//scene.addParam(idP, ChunkPrimParam::ORDER,"",0);
scene.addParamID(idP, ChunkPrimParam::VSHADER,"",idVS);
scene.addParamID(idP, ChunkPrimParam::PSHADER,"",idPS);
scene.addParamID(idP, ChunkPrimParam::VBUFFER_ID,"position",idVB);
scene.addParamID(idP, ChunkPrimParam::IBUFFER_ID,"",idIB);
scene.addParamEnum(idP, ChunkPrimParam::PARAMVSHADER, "ModelViewProj", ChunkPrimParam::ModelViewProjection);
scene.addParam(idP, ChunkPrimParam::PARAMVSHADER, "TextureScale", Vec2f(1.0f/nx,-1.0f/ny));
scene.addParamID(idP, ChunkPrimParam::TEXTURE, "texture", idT);
scene.addParam(idP,ChunkPrimParam::TRANSFORM_SCALE,"",Vec3f(terrain.scale.x(),terrain.scale.y(),terrain.hscale*terrain.scale.z()));
scene.addParam(idP,ChunkPrimParam::TRANSFORM_POSITION,"",Vec3f(-nx/2*terrain.scale.x(),-ny/2*terrain.scale.y(),terrain.hbase*terrain.scale.z()));
scene.put(&pOut);
flowvr::MessageWrite mt;
mt.data = module->alloc(nx*ny*sizeof(short));
mt.stamps.write(terrainN[0],nx);
mt.stamps.write(terrainN[1],ny);
mt.stamps.write(terrainP[0],-nx/2*terrain.scale.x());
mt.stamps.write(terrainP[1],-ny/2*terrain.scale.y());
mt.stamps.write(terrainS[0],terrain.scale.x());
mt.stamps.write(terrainS[1],terrain.scale.y());
mt.stamps.write(terrainH[0],terrain.scale.z()*terrain.hbase);
mt.stamps.write(terrainH[1],terrain.scale.z()*terrain.hscale);
{
short* height = terrain.height+(cy-ny/2)*terrain.nx+(cx-nx/2);
for (int y=0;y<ny;y++)
{
memcpy(mt.data.getWrite<short>(y*nx*sizeof(short)), height, nx*sizeof(short));
height+=terrain.nx;
}
}
module->put(&pTerrain, mt);
// Fluid position
{
short fluidh = hmin+(short)(0.3f/terrain.hscale);
short* height = terrain.height+(cy-ny/2)*terrain.nx+(cx-nx/2);
for (int y=0;y<ny;y++)
{
for (int x=0;x<nx;x++)
{
if (height[x] <= fluidh)
{
if (x<xmin) xmin=x; else if (x>xmax) xmax=x;
if (y<ymin) ymin=y; else if (y>ymax) ymax=y;
}
}
height+=terrain.nx;
}
std::cout << "Fluid pos: <"<<xmin<<','<<ymin<<">-<"<<xmax<<','<<ymax<<") h="<<fluidh<<std::endl;
flowvr::MessageWrite m;
m.data = module->alloc(sizeof(Mat4x4f));
Mat4x4f& fluid = *m.data.getWrite<Mat4x4f>();
fluid.identity();
// Scale
fluid[0][0] = (xmax-xmin+1)*terrain.scale.x();
fluid[1][1] = (ymax-ymin+1)*terrain.scale.y();
fluid[2][2] = terrain.scale.z();
// Position
fluid[0][3] = (xmin-nx/2-0.5f)*terrain.scale.x();
fluid[1][3] = (ymin-ny/2-0.5f)*terrain.scale.y();
fluid[2][3] = (terrain.hbase+fluidh*terrain.hscale)*terrain.scale.z();
module->put(&pOutFluid,m);
}
module->wait();
flowvr::BufferPool pool;
if (pRotX.isConnected())
{
float rotSpeed = 0.01f;
float speed = 0.04f;
do
{
Vec3f depl;
depl.x() = getSum(pTransX,position0.x(),speed);
depl.y() = -getSum(pTransY,position0.y(),speed);
getSum(pRotX,rotation.x(),rotSpeed);
getSum(pRotY,rotation.y(),rotSpeed);
get(pZoom,zoom);
Mat3x3f mrot;
Quat qx,qy,qz;
qx.fromAngAxis(M_PI/2*rotation.x(),Vec3f(0,0,1));
qy.fromAngAxis(M_PI/2*rotation.y(),Vec3f(1,0,0));
qz.fromAngAxis(M_PI/2*rotation.z(),Vec3f(0,1,0));
Quat q = qz*qy*qx;
q.toMatrix(&mrot);
Vec3f newPos = position+mrot*depl;
newPos.z() = terrain.getHeight(newPos.x(), newPos.y()); //+(zoom+1);
position = newPos;
Mat3x3f xrot;
qx.toMatrix(&xrot);
Vec3f pcam = position+xrot*(Vec3f(0,-2,0)*(1+zoom))+Vec3f(0,0,1.5);
float zTer = terrain.getHeight(pcam.x(), pcam.y())+1;
if (zTer > pcam.z()) pcam.z()=zTer;
Mat4x4f m;
m.identity();
m = mrot;
m(0,3) = pcam[0];
m(1,3) = pcam[1];
m(2,3) = pcam[2];
Mat4x4f mcam; mcam.identity();
Mat3x3f rotcam;
Quat qcam; qcam.fromAngAxis(-M_PI/2,Vec3f(1,0,0));
qcam.toMatrix(&rotcam);
mcam = rotcam;
m = m*mcam;
scene.addParam(ID_CAMERA,ChunkPrimParam::TRANSFORM,"",m);
mrot.identity();
m = mrot;
m(0,3) = position[0];
m(1,3) = position[1];
m(2,3) = position[2];
flowvr::MessageWrite msg;
msg.data = pool.alloc(module,sizeof(Mat4x4f));
*msg.data.getWrite<Mat4x4f>() = m;
module->put(&pOutMat, msg);
scene.put(&pOut);
}
while (module->wait());
}
module->close();
return 0;
}
void Quat::fromMatrix(const Mat3x3f &m)
{
float tr, s;
tr = m.x().x() + m.y().y() + m.z().z();
// check the diagonal
if (tr > 0)
{
s = (float)sqrt (tr + 1);
w = s * 0.5f; // w OK
s = 0.5f / s;
x = (m.y().z() - m.z().y()) * s; // x OK
y = (m.z().x() - m.x().z()) * s; // y OK
z = (m.x().y() - m.y().x()) * s; // z OK
}
else
{
if (m.y().y() > m.x().x() && m.z().z() <= m.y().y())
{
s = (float)sqrt ((m.y().y() - (m.z().z() + m.x().x())) + 1.0f);
y = s * 0.5f; // y OK
if (s != 0.0f)
s = 0.5f / s;
z = (m.z().y() + m.y().z()) * s; // z OK
x = (m.y().x() + m.x().y()) * s; // x OK
w = (m.z().x() - m.x().z()) * s; // w OK
}
else if ((m.y().y() <= m.x().x() && m.z().z() > m.x().x()) || (m.z().z() > m.y().y()))
{
s = (float)sqrt ((m.z().z() - (m.x().x() + m.y().y())) + 1.0f);
z = s * 0.5f; // z OK
if (s != 0.0f)
s = 0.5f / s;
x = (m.x().z() + m.z().x()) * s; // x OK
y = (m.z().y() + m.y().z()) * s; // y OK
w = (m.x().y() - m.y().x()) * s; // w OK
}
else
{
s = (float)sqrt ((m.x().x() - (m.y().y() + m.z().z())) + 1.0f);
x = s * 0.5f; // x OK
if (s != 0.0f)
s = 0.5f / s;
y = (m.y().x() + m.x().y()) * s; // y OK
z = (m.x().z() + m.z().x()) * s; // z OK
w = (m.y().z() - m.z().y()) * s; // w OK
}
}
}
