void testMatrix::testMatrices()
{
Vector4 testTransV = Vector4(4,5,6,1);
Matrix4x4 testTrans = Matrix4x4(1,0,0,-4,0,1,0,-5,0,0,1,-6,0,0,0,1);
Matrix4x4 testRotZ90 = Matrix4x4(0.f,-1.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,1.f);
Matrix4x4 testRotY90 = Matrix4x4(0.f,0.f,1.f,0.f,0.f,1.f,0.f,0.f,-1.f,0.f,0.f,0.f,0.f,0.f,0.f,1.f);
Matrix4x4 testRotX90 = Matrix4x4(1.f,0.f,0.f,0.f,0.f,0.f,-1.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,0.f,1.f);
//y
Matrix4x4 testRot1 = getRotMat(Vector4(0,0,0,1),Vector4(0,1,0,1),90);
//x
Matrix4x4 testRot2 = getRotMat(Vector4(0,0,0,1),Vector4(1,0,0,1), 90);
//z
Matrix4x4 testRot3 = getRotMat(Vector4(0,0,0,1),Vector4(0,0,1,1), 90);
Matrix4x4 trz90 = getRotZMat(M_PI/2);
Matrix4x4 try90 = getRotYMat(M_PI/2);
Matrix4x4 trx90 = getRotXMat(M_PI/2);
Matrix4x4 trans = getTransMat(testTransV);
//Test rotation on z axis
compareMatrices(&testRotZ90,&trz90);
//Z axis as arbitrary rotation
compareMatrices(&testRotZ90,&testRot3);
//inverses are equivalent:
compareMatrices(&getInvRotZMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(0,0,1,1),90));
//Test rotation on y axis
compareMatrices(&testRotY90,&try90);
//Y axis as arbitrary rotation
compareMatrices(&testRotY90,&testRot1);
//inverses are equivalent:
compareMatrices(&getInvRotYMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(0,1,0,1),90));
//Test rotation on x axis
compareMatrices(&testRotX90,&trx90);
//X axis as arbitrary rotation
compareMatrices(&testRotX90,&testRot2);
//inverses are equivalent:
compareMatrices(&getInvRotXMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(1,0,0,1),90));
//test translation
compareMatrices(&testTrans,&trans);
}
// Get normal, clockwise
vec2 OBB::GetNormal(uint32_t idx) const {
switch (idx % 4) {
case 0:
return getRotMat() * vec2(1, 0);
case 1:
return getRotMat() * vec2(0, -1);
case 2:
return getRotMat() * vec2(-1, 0);
case 3:
default:
return getRotMat() * vec2(0, 1);
}
}
// Get verts, clockwise
vec2 OBB::GetVert(uint32_t idx) const {
vec2 ret(0);
switch (idx % 4) {
case 0:
return C + getRotMat() * R;
case 1:
return C + getRotMat() * vec2(R.x, -R.y);
case 2:
return C - getRotMat() * R;
case 3:
default:
return C + getRotMat() * vec2(-R.x, R.y);
}
}
Matrix4x4 DataProcessor::calcTransMat(std::vector<CS123SceneTransformation*> tranVect){
Matrix4x4 totalMat = Matrix4x4::identity();
//how to handle one transformation
std::vector<CS123SceneTransformation*>::const_iterator i;
for(i = tranVect.begin(); i!=tranVect.end(); i++){
Matrix4x4 tmat;
CS123SceneTransformation cTran = **i;
switch(cTran.type){
case TRANSFORMATION_TRANSLATE:
tmat = getTransMat(cTran.translate);
break;
case TRANSFORMATION_SCALE:
tmat = getScaleMat(cTran.scale);
break;
case TRANSFORMATION_ROTATE:
tmat = getRotMat(Vector4(0,0,0,0),cTran.rotate,cTran.angle);
break;
case TRANSFORMATION_MATRIX:
tmat = cTran.matrix;
break;
}
totalMat = totalMat*tmat;
}
return totalMat;
}
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// const Vector4 &v2=Vector4(-v.x,-v.y,-v.z,1);
return getRotMat(p,v,-a);
// @TODO: (CAMTRANS) Fill this in...
}
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// @TODO: [CAMTRANS] Fill this in...
//return Matrix4x4::identity();
return getRotMat(p,v,-1*a);
}
void CamtransCamera::rotateU(float degrees)
{
// @TODO: [CAMTRANS] Fill this in...
REAL radians = degrees/180*M_PI;
Matrix4x4 rot = Matrix4x4::identity();
if( !m_useQuaternion )
rot = getRotMat( m_eyePosition,m_u,-radians );
else
rot = getRotMatFromQuaternion( m_eyePosition,m_u,-radians );
m_v = rot*m_v;
m_n = rot*m_n;
m_lookAt = rot*m_lookAt;
m_up = rot*m_up;
updateModelViewMatrix();
}
TEST(GeoLib, SurfaceIsPointInSurface)
{
std::vector<std::function<double(double, double)>> surface_functions;
surface_functions.push_back(constant);
surface_functions.push_back(coscos);
for (auto f : surface_functions) {
std::random_device rd;
std::string name("Surface");
// generate ll and ur in random way
std::mt19937 random_engine_mt19937(rd());
std::normal_distribution<> normal_dist_ll(-10, 2);
std::normal_distribution<> normal_dist_ur(10, 2);
MathLib::Point3d ll(std::array<double,3>({{
normal_dist_ll(random_engine_mt19937),
normal_dist_ll(random_engine_mt19937),
0.0
}
}));
MathLib::Point3d ur(std::array<double,3>({{
normal_dist_ur(random_engine_mt19937),
normal_dist_ur(random_engine_mt19937),
0.0
}
}));
for (std::size_t k(0); k<3; ++k)
if (ll[k] > ur[k])
std::swap(ll[k], ur[k]);
// random discretization of the domain
std::default_random_engine re(rd());
std::uniform_int_distribution<std::size_t> uniform_dist(2, 25);
std::array<std::size_t,2> n_steps = {{uniform_dist(re),uniform_dist(re)}};
std::unique_ptr<MeshLib::Mesh> sfc_mesh(
MeshLib::MeshGenerator::createSurfaceMesh(
name, ll, ur, n_steps, f
)
);
// random rotation angles
std::normal_distribution<> normal_dist_angles(
0, boost::math::double_constants::two_pi);
std::array<double,3> euler_angles = {{
normal_dist_angles(random_engine_mt19937),
normal_dist_angles(random_engine_mt19937),
normal_dist_angles(random_engine_mt19937)
}
};
MathLib::DenseMatrix<double, std::size_t> rot_mat(getRotMat(
euler_angles[0], euler_angles[1], euler_angles[2]));
std::vector<MeshLib::Node*> const& nodes(sfc_mesh->getNodes());
GeoLib::rotatePoints<MeshLib::Node>(rot_mat, nodes);
MathLib::Vector3 const normal(0,0,1.0);
MathLib::Vector3 const surface_normal(rot_mat * normal);
double const eps(1e-6);
MathLib::Vector3 const displacement(eps * surface_normal);
GeoLib::GEOObjects geometries;
MeshLib::convertMeshToGeo(*sfc_mesh, geometries);
std::vector<GeoLib::Surface*> const& sfcs(*geometries.getSurfaceVec(name));
GeoLib::Surface const*const sfc(sfcs.front());
std::vector<GeoLib::Point*> const& pnts(*geometries.getPointVec(name));
// test triangle edge point of the surface triangles
for (auto const p : pnts) {
EXPECT_TRUE(sfc->isPntInSfc(*p));
MathLib::Point3d q(*p);
for (std::size_t k(0); k<3; ++k)
q[k] += displacement[k];
EXPECT_FALSE(sfc->isPntInSfc(q));
}
// test edge middle points of the triangles
for (std::size_t k(0); k<sfc->getNTriangles(); ++k) {
MathLib::Point3d p, q, r;
std::tie(p,q,r) = getEdgeMiddlePoints(*(*sfc)[k]);
EXPECT_TRUE(sfc->isPntInSfc(p));
EXPECT_TRUE(sfc->isPntInSfc(q));
EXPECT_TRUE(sfc->isPntInSfc(r));
}
}
}
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// [PASS]
return getRotMat(p, v, -a);
}
//------------------------------------------------------------------------------
// weaponDynamics -- default missile dynamics; using Robot Aircraft (RAC) dynamics
//------------------------------------------------------------------------------
void Missile::weaponDynamics(const LCreal dt)
{
static const LCreal g = ETHG; // Acceleration of Gravity
// ---
// Max turning G (Missiles: Use Gmax)
// ---
LCreal gmax = maxG;
// ---
// Computer max turn rate, max/min pitch rates
// ---
// Turn rate base on vp and g,s
LCreal ra_max = gmax * g / getTotalVelocity();
// Set max (pull up) pitch rate same as turn rate
LCreal qa_max = ra_max;
// Set min (push down) pitch rate
LCreal qa_min = -qa_max;
// ---
// Get old angular values
// ---
const osg::Vec3 oldRates = getAngularVelocities();
//LCreal pa1 = oldRates[IROLL];
LCreal qa1 = oldRates[IPITCH];
LCreal ra1 = oldRates[IYAW];
// ---
// Find pitch rate and update pitch
// ---
LCreal qa = lcAepcRad(cmdPitch - (LCreal) getPitchR());
if(qa > qa_max) qa = qa_max;
if(qa < qa_min) qa = qa_min;
// Using Pitch rate, integrate pitch
LCreal newTheta = (LCreal) (getPitch() + (qa + qa1) * dt / 2.0);
// Find turn rate
LCreal ra = lcAepcRad(cmdHeading - (LCreal) getHeadingR());
if(ra > ra_max) ra = ra_max;
if(ra < -ra_max) ra = -ra_max;
// Use turn rate integrate heading
LCreal newPsi = (LCreal) (getHeading() + (ra + ra1) * dt / 2.0);
if(newPsi > 2.0f*PI) newPsi -= (LCreal)(2.0*PI);
if(newPsi < 0.0f) newPsi += (LCreal)(2.0*PI);
// Roll angle proportional to max turn rate - filtered
LCreal pa = 0.0;
LCreal newPhi = (LCreal) ( 0.98 * getRollR() + 0.02 * ((ra / ra_max) * (Basic::Angle::D2RCC * 60.0)) );
// Sent angular values
setEulerAngles(newPhi, newTheta, newPsi);
setAngularVelocities(pa, qa, ra);
// Find Acceleration
LCreal vpdot = (cmdVelocity - getTotalVelocity());
if(vpdot > maxAccel) vpdot = maxAccel;
if(vpdot < -maxAccel) vpdot = -maxAccel;
// Set acceleration vector
osg::Vec3 aa(vpdot, 0.0, 0.0);
osg::Vec3 ae = aa * getRotMat();
setAcceleration(ae);
// Comute new velocity
LCreal newVP = getTotalVelocity() + vpdot * dt;
// Set acceleration vector
osg::Vec3 ve0 = getVelocity();
osg::Vec3 va(newVP, 0.0, 0.0);
osg::Vec3 ve1 = va * getRotMat();
setVelocity(ve1);
setVelocityBody(newVP, 0.0, 0.0);
}
// Definitely not the cheapest way of doing this
vec2 OBB::ws_clamp(vec2 p) const {
vec2 p1 = glm::inverse(getRotMat()) * (p - C);
p1 = glm::clamp(p1, -R, R);
return C + getRotMat() * p1;
}
void Viewport::Move(glm::vec4 offset) {
position -= offset*getRotMat();
}
void LappedUtils::assignSeedUV(PatchTri* seed, vec2<float> &v0st, vec2<float> &v1st, vec2<float> &v2st)
{
float scale = .25;
Vector4 A,B,C;
A = seed->v0->pos;
B = seed->v1->pos;
C = seed->v2->pos;
A.w = 1;
B.w = 1;
C.w = 1;
Vector4 ctr = (A+B+C)/3.0;
ctr.w=0;
Vector4 Ap, Bp, Cp, Tp;
Matrix4x4 transMat = getTransMat(-ctr);
Ap = transMat*A;
Bp = transMat*B;
Cp = transMat*C;
Ap.w=0;
Bp.w=0;
Cp.w=0;
Vector4 norm = ((Bp-Ap).cross(Cp-Ap)).getNormalized();
printVector4(norm);
double angle = acos(norm.dot(Vector4(0,0,1.0,0)));
//cout << "angle between normal and plane z=1: " << angle << endl;
if (norm.cross(Vector4(0,0,1,0)).getMagnitude()>0.00001)
{
//cout << (norm.cross(Vector4(0,0,1,0))).getMagnitude() << endl;
//cout << norm.cross(Vector4(0,0,1,0)) << endl;
Matrix4x4 rotMat = getRotMat(Vector4(0,0,0,0),norm.cross(Vector4(0,0,1.0,0)).getNormalized(),angle);
Ap.w = 1;
Bp.w = 1;
Cp.w = 1;
Ap = rotMat*Ap;
Bp = rotMat*Bp;
Cp = rotMat*Cp;
Tp = rotMat*seed->tangent;
}
else Tp=seed->tangent;
Ap.w = 0;
Bp.w = 0;
Cp.w = 0;
double avgLen = max(max((Ap-Cp).getMagnitude(),(Ap-Bp).getMagnitude()),(Cp-Bp).getMagnitude());
Ap = Ap*scale/avgLen;
Bp = Bp*scale/avgLen;
Cp = Cp*scale/avgLen;
Ap.w = 1;
Bp.w = 1;
Cp.w = 1;
Tp.w = 0;
seed->tangent = Tp;
Tp.normalize();
double tanAngle = acos(Vector4(0,1,0,0).dot(Tp));
Matrix4x4 tanMat = getRotMat(Vector4(0,0,0,0),Vector4(0,1,0,0).cross(Tp),tanAngle);
Ap = tanMat*Ap;
Bp = tanMat*Bp;
Cp = tanMat*Cp;
Ap.w = 1;
Bp.w = 1;
Cp.w = 1;
transMat = getTransMat(Vector4(.5,.5,0,0));
Ap = transMat*Ap;
Bp = transMat*Bp;
Cp = transMat*Cp;
v0st.x = Ap.x;
v0st.y = Ap.y;
v1st.x = Bp.x;
v1st.y = Bp.y;
v2st.x = Cp.x;
v2st.y = Cp.y;
}
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// [CAMTRANS] Fill this in...
return getRotMat(p, v, -a);
}
//------------------------------------------------------------------------------
// onRfEmissionEventAntenna() -- process events for RF Emission not sent by us.
//
// 1) Build a list of emission packets from the queue and compute the
// Line-Of-Sight (LOS) vectors back to the transmitter.
//
// 2) Transform LOS vectors to antenna coordinates
//
// 3) Compute antenna gains in the direction of the transmitter
//
// 4) Compute Antenna Effective Gains
//
// 5) Compute Antenna Effective Area and Polarization Gains
//
// 6) Compute total receiving antenaa gain and send the emission to our sensor
//------------------------------------------------------------------------------
bool Antenna::onRfEmissionEvent(Emission* const em)
{
// Is this emission from a player of interest?
if (fromPlayerOfInterest(em)) {
Player* ownship = getOwnship();
RfSystem* sys1 = getSystem();
if (ownship != 0 && sys1 != 0) {
sys1->ref();
// Line-Of-Sight (LOS) vectors back to the transmitter.
osg::Vec3d xlos = em->getTgtLosVec();
osg::Vec4d los0( xlos.x(), xlos.y(), xlos.z(), 0.0);
// 2) Transform local NED LOS vectors to antenna coordinates
osg::Matrixd mm = getRotMat();
mm *= ownship->getRotMat();
osg::Vec4d losA = mm * los0;
// ---
// Compute antenna gains in the direction of the transmitter
// ---
double rGainDb = 0.0f;
if (gainPattern != 0) {
Basic::Func1* gainFunc1 = dynamic_cast<Basic::Func1*>(gainPattern);
Basic::Func2* gainFunc2 = dynamic_cast<Basic::Func2*>(gainPattern);
if (gainFunc2 != 0) {
// ---
// 3-a) Antenna pattern: 2D table (az & el off antenna boresight)
// ---
// Get component arrays and ground range squared
double xa = losA.x();
double ya = losA.y();
double za = -losA.z();
double ra2 = xa*xa + ya*ya;
// Compute range along antenna x-y plane
double ra = sqrt(ra2);
// Compute azimuth off boresight
double aazr = atan2(ya,xa);
// Compute elevation off boresight
double aelr = atan2(za,ra);
// Lookup gain in 2D table and convert from dB
if (gainPatternDeg)
rGainDb = gainFunc2->f( aazr * Basic::Angle::R2DCC, aelr * Basic::Angle::R2DCC );
else
rGainDb = gainFunc2->f( aazr, aelr );
}
else if (gainFunc1 != 0) {
// ---
// 3-b) Antenna Pattern: 1D table (off antenna boresight only
// ---
// Compute angle off antenna boresight
double aar = acos(losA.x());
// Lookup gain in 1D table and convert from dB
if (gainPatternDeg)
rGainDb = gainFunc1->f( aar * Basic::Angle::R2DCC );
else
rGainDb = gainFunc1->f(aar);
}
}
// Compute off-boresight gain
double rGain = pow(10.0,rGainDb/10.0);
// Compute Antenna Effective Gain
double aeGain = rGain * getGain();
double lambda = em->getWavelength();
double aea = getEffectiveArea(aeGain, lambda);
double pGain = getPolarizationGain(em->getPolarization());
double raGain = aea * pGain;
sys1->rfReceivedEmission(em, this, LCreal(raGain));
sys1->unref();
}
}
return BaseClass::onRfEmissionEvent(em);
}
