glm::mat4x4 Planet::getSatelliteModelMatrix()
{
glm::vec3 axisX(1.0f, 0.0f, 0.0f),
axisY(0.0f, 1.0f, 0.0f),
axisZ(0.0f, 0.0f, 1.0f);
glm::mat4x4 Model = glm::mat4x4(1.0f);
Model = glm::rotate(Model, glm::degrees(orbit.ascNodeLongitude - 3 * PI / 2), axisY);
Model = glm::rotate(Model, glm::degrees(orbit.inclination), axisZ);
Model = glm::rotate(Model, glm::degrees(orbit.periapsisArg - PI / 2), axisY);
float meanAnomaly = startMeanAnomaly + time * (2 * PI) / orbitalPeriod;
float e = orbit.eccentricity;
float trueAnomaly = meanAnomaly + (2 * e - 0.25 * pow(e, 3) + 5/96 * pow(e, 5)) * sin(meanAnomaly)
+ (5 / 4 * pow(e, 2) - 11/24 * pow(e, 4)) * sin(2 * meanAnomaly)
+ (13 / 12 * pow(e, 3) - 43/64 * pow(e, 5)) * sin(3 * meanAnomaly)
+ 103/96 * pow(e, 4) * sin(4 * meanAnomaly)
+ 1097/960 * pow(e, 5) * sin(5 * meanAnomaly);
/*float trueAnomaly = meanAnomaly + (2 * e - pow(e, 3) / 4) * sin(meanAnomaly) +
5 / 4 * pow(e, 2) * sin(2 * meanAnomaly) +
13 / 12 * pow(e, 3) * sin(3 * meanAnomaly);*/
// Approximate solution of Kepler's equasion
float currX = orbit.semiMajorAxis * distanceScale * cos(trueAnomaly);
float currY = orbit.semiMinorAxis * distanceScale * sin(trueAnomaly);
glm::vec3 shiftVec = glm::vec3(currX, 0.0f, -currY) +
glm::vec3(-orbit.eccentricity * orbit.semiMajorAxis * distanceScale, 0.0f, 0.0f);
Model = glm::translate(Model, shiftVec);
return Model;
}
void vtRoute::_StringWires(long ll, vtHeightField3d *pHeightField)
{
// pick pole numbers i and i-1 and string a wire between them
long numiterations = NUM_WIRE_SEGMENTS;
FPoint3 fp0, fp1;
vtUtilNode *n0 = m_Nodes[ll-1];
vtUtilNode *n1 = m_Nodes[ll];
vtUtilStruct *st0 = n0->m_struct;
vtUtilStruct *st1 = n1->m_struct;
// safety check
if (!st0 || !st1)
return;
DPoint2 p0 = n0->m_Point;
DPoint2 p1 = n1->m_Point;
pHeightField->ConvertEarthToSurfacePoint(p0, fp0);
pHeightField->ConvertEarthToSurfacePoint(p1, fp1);
FMatrix4 rot;
rot.Identity();
FPoint3 axisY(0, 1, 0);
FPoint3 offset, wire0, wire1;
vtMesh *pWireMesh;
for (int j = 0; j < st1->m_iNumWires; j++)
{
pWireMesh = new vtMesh(osg::PrimitiveSet::LINE_STRIP, 0, numiterations+1);
offset = st0->m_fpWireAtt1[j];
rot.AxisAngle(axisY, n0->dRadAzimuth);
rot.Transform(offset, wire0);
FPoint3 wire_start = fp0 + wire0;
pWireMesh->AddVertex(wire_start);
offset = st1->m_fpWireAtt2[j];
rot.AxisAngle(axisY, n1->dRadAzimuth);
rot.Transform(offset, wire1);
FPoint3 wire_end = fp1 + wire1;
_DrawCat(wire_start, wire_end, vtGetTS()->m_fCatenaryFactor, numiterations, pWireMesh);
pWireMesh->AddVertex(wire_end);
pWireMesh->AddStrip2(numiterations+1, 0);
m_pWireGeom->AddMesh(pWireMesh, m_mi_wire);
}
}
int CGPData::Deg2Q( D3DXVECTOR3 rot, CQuaternion* dstq )
{
CQuaternion qx, qy, qz;
D3DXVECTOR3 axisX( 1.0f, 0.0f, 0.0f );
D3DXVECTOR3 axisY( 0.0f, 1.0f, 0.0f );
D3DXVECTOR3 axisZ( 0.0f, 0.0f, 1.0f );
qx.SetAxisAndRot( axisX, rot.x * (float)DEG2PAI );
qy.SetAxisAndRot( axisY, rot.y * (float)DEG2PAI );
qz.SetAxisAndRot( axisZ, rot.z * (float)DEG2PAI );
*dstq = qy * qx * qz;
return 0;
}
void TerrainView::paintEvent(QPaintEvent *e)
{
Q_D(TerrainView);
auto size = this->size();
SceneDefinition def;
def.cameraDir = QVector3D(std::cos(d->yaw_) * std::cos(d->pitch_),
std::sin(d->yaw_) * std::cos(d->pitch_),
std::sin(d->pitch_));
def.eye = d->centerPos_ - def.cameraDir;
def.viewWidth = size.width() / d->scale_;
def.viewHeight = size.height() / d->scale_;
auto img = d->render(size, def);
QPainter painter(this);
painter.drawImage(0, 0, img);
if (d->viewOptions.axises) {
QPointF axisPos(40, size.height() - 50);
QPointF axisX(d->rightVector.x(), d->upVector.x());
QPointF axisY(d->rightVector.y(), d->upVector.y());
QPointF axisZ(d->rightVector.z(), d->upVector.z());
float axisLen = 18.f;
auto drawText = [&](QString text, QPointF origin, QPointF dir) {
dir *= 1.f / std::sqrt(dir.x() * dir.x() + dir.y() * dir.y());
origin += dir * 12.f;
QRectF r(origin.x() - 100.f, origin.y() - 100.f, 200.f, 200.f);
QTextOption opt;
opt.setAlignment(Qt::AlignCenter);
painter.drawText(r, text, opt);
};
painter.setPen(qRgb(0, 0, 255));
painter.drawLine(axisPos, axisPos + axisZ * axisLen);
drawText("Z", axisPos + axisZ * axisLen, axisZ);
painter.setPen(qRgb(255, 0, 0));
painter.drawLine(axisPos, axisPos + axisX * axisLen);
drawText("X", axisPos + axisX * axisLen, axisX);
painter.setPen(qRgb(0, 255, 0));
painter.drawLine(axisPos, axisPos + axisY * axisLen);
drawText("Y", axisPos + axisY * axisLen, axisY);
}
emit clientPaint(e);
}
glm::mat4x4 Planet::getOrbitModelMatrix()
{
glm::vec3 axisX(1.0f, 0.0f, 0.0f),
axisY(0.0f, 1.0f, 0.0f),
axisZ(0.0f, 0.0f, 1.0f);
glm::mat4x4 Model = glm::mat4x4(1.0f);
Model = glm::rotate(Model, glm::degrees(orbit.ascNodeLongitude - 3 * PI / 2), axisY);
Model = glm::rotate(Model, glm::degrees(orbit.inclination), axisZ);
Model = glm::rotate(Model, glm::degrees(orbit.periapsisArg - PI / 2), axisY);
glm::vec3 shiftVec = glm::vec3(-orbit.eccentricity * orbit.semiMajorAxis * distanceScale, 0.0f, 0.0f);
Model = glm::translate(Model, shiftVec);
return Model;
}
// TEST 6 - WORLD-TO-EXTRINSICS AND EXTRINSICS-TO-WORLD TRANSFORMATIONS
bool test6(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
{
vcg::Matrix44d WtoE1 = shot1.GetWorldToExtrinsicsMatrix();
vcg::Matrix44d WtoE2 = shot2.GetWorldToExtrinsicsMatrix();
vcg::Matrix44d EtoW1 = shot1.GetExtrinsicsToWorldMatrix();
vcg::Matrix44d EtoW2 = shot2.GetExtrinsicsToWorldMatrix();
vcg::Matrix44d I1 = WtoE1 * EtoW1;
vcg::Matrix44d I2 = WtoE2 * EtoW2;
vcg::Matrix44d I3 = EtoW1 * WtoE1;
vcg::Matrix44d I4 = EtoW2 * WtoE2;
if (checkIdentity(I1) > precision)
return false;
if (checkIdentity(I2) > precision)
return false;
if (checkIdentity(I3) > precision)
return false;
if (checkIdentity(I4) > precision)
return false;
vcg::Point3d axisX(1.0, 0.0, 0.0);
vcg::Point3d axisY(0.0, 1.0, 0.0);
vcg::Point3d axisZ(0.0, 0.0, 1.0);
vcg::Point3d vx = EtoW1 * axisX;
vcg::Point3d vy = EtoW1 * axisY;
vcg::Point3d vz = EtoW1 * axisZ;
if (dist3(vx, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(0)) > precision)
return false;
if (dist3(vy, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(1)) > precision)
return false;
if (dist3(vz, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(2)) > precision)
return false;
return true;
}
/**
* Graph::draw, render whole scene.
*/
void Graph::draw()
{
static int cnt = 0;
cnt++;
glViewport((int)mViewPos.x, (int)mViewPos.y, mWidth, mHeight);
checkGLError("glViewport");
mDeltaTime.tick();
float tick = static_cast<float>(mTime.update()) * 0.001f;
mScene.setElapsedTime(tick);
// Clear the color buffer
glClearColor(mBGColor.r,mBGColor.g,mBGColor.b, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
checkGLError("glClear");
mTouch.update();
mScene.updateCamera(mTouch.getScale());
// working with angular speed
const glm::vec3 rotSpeed = mTouch.getAngularSpeed();
glm::vec3 axisY(0.0f,1.0f,0.0f);
glm::mat4 mY = glm::rotate(rotSpeed.x*30.0f,axisY);
glm::vec3 axisX(1.0f,0.0f,0.0f);
glm::mat4 mX = glm::rotate(rotSpeed.y*30.0f,axisX);
glm::vec3 axisZ(0.0f,0.0f,1.0f);
glm::mat4 mZ = glm::rotate(rotSpeed.z*30.0f,axisZ);
glm::mat4 mW = mScene.getWorldMat();
glm::mat4 m = mZ*mY*mX*mW;
mScene.setWorldMat( m );
mScene.updateMatrix(); // need to update the PVW Matrix, Projection * View * World.
glEnable(GL_DEPTH_TEST);
drawBars(tick);
drawAxis(tick);
drawText(tick);
}
int CGPAnim::CalcFrameData( CShdHandler* lpsh, CMotHandler* lpmh, D3DXMATRIX* matWorld )
{
int ret;
int frameno;
if( !m_firstkey ) {
for( frameno = 0; frameno <= m_maxframe; frameno++ ) {
( m_framedata + frameno )->m_frameno = frameno;
( m_framedata + frameno )->m_gpe = *m_defgpeptr;
( m_framedata + frameno )->m_interp = *m_definterptr;
( m_framedata + frameno )->m_keyflag = 0;
}
return 0;//!!!!!!!
}
CGPKey* gpkptr = m_firstkey;
#ifdef INEASY3D
while( gpkptr ) {
CQuaternion orgq;
CQuaternion qx, qy, qz;
D3DXVECTOR3 axisX( 1.0f, 0.0f, 0.0f );
D3DXVECTOR3 axisY( 0.0f, 1.0f, 0.0f );
D3DXVECTOR3 axisZ( 0.0f, 0.0f, 1.0f );
qx.SetAxisAndRot( axisX, gpkptr->m_gpe.rot.x * (float)DEG2PAI );
qy.SetAxisAndRot( axisY, gpkptr->m_gpe.rot.y * (float)DEG2PAI );
qz.SetAxisAndRot( axisZ, gpkptr->m_gpe.rot.z * (float)DEG2PAI );
orgq = qy * qx * qz;
D3DXMATRIX orgmat;
orgmat = orgq.MakeRotMatX();
orgmat._41 = gpkptr->m_gpe.pos.x;
orgmat._42 = gpkptr->m_gpe.pos.y;
orgmat._43 = gpkptr->m_gpe.pos.z;
/////////
D3DXMATRIX multmat;
multmat = orgmat * *m_offmatptr;
//multmat = *m_offmatptr * orgmat;
gpkptr->m_gpe.e3dpos.x = multmat._41;
gpkptr->m_gpe.e3dpos.y = multmat._42;
gpkptr->m_gpe.e3dpos.z = multmat._43;
//////////
D3DXMATRIX rotmat;
rotmat = multmat;
rotmat._41 = 0.0f;
rotmat._42 = 0.0f;
rotmat._43 = 0.0f;
D3DXQUATERNION mqx, invmqx;
D3DXQuaternionRotationMatrix( &mqx, &rotmat );
D3DXQuaternionInverse( &invmqx, &mqx );
CQuaternion mq;
mq.x = mqx.x;
mq.y = mqx.y;
mq.z = mqx.z;
mq.w = mqx.w;
D3DXVECTOR3 neweul;
ret = qToEuler( 0, &mq, &neweul );
ret = modifyEuler( &neweul, &gpkptr->m_gpe.e3drot );
gpkptr->m_gpe.e3drot = neweul;
gpkptr = gpkptr->next;
}
#endif
//キーの接地計算
//CGPKey* gpkptr = m_firstkey;
gpkptr = m_firstkey;
if( lpsh ) {
while( gpkptr ) {
if( gpkptr->m_gpe.ongmode != GROUND_NONE ) {
//D3DXMATRIX inimat;
//D3DXMatrixIdentity( &inimat );
D3DXVECTOR3 befpos, newpos;
#ifdef INEASY3D
befpos = gpkptr->m_gpe.e3dpos;
befpos.y = gpkptr->m_gpe.rayy + m_offmatptr->_42;
newpos = gpkptr->m_gpe.e3dpos;
newpos.y = gpkptr->m_gpe.rayy - gpkptr->m_gpe.rayleng;
int result = 0;
D3DXVECTOR3 adjustv, nv;
ret = lpsh->ChkConfGround( 0, 0, matWorld, befpos, newpos, lpmh, 0, 200.0f, newpos.y - 100.0f, &result, &adjustv, &nv );
if( ret ) {
DbgOut( "GPAnim : CalcFrameData : sh ChkConfGround error !!!\n" );
_ASSERT( 0 );
return 1;
}
if( result ) {
gpkptr->m_gpe.e3dpos = adjustv;
gpkptr->m_gpe.e3dpos.y += gpkptr->m_gpe.offsety;
}
#else
befpos = gpkptr->m_gpe.pos;
befpos.y = gpkptr->m_gpe.rayy;
newpos = gpkptr->m_gpe.pos;
newpos.y = gpkptr->m_gpe.rayy - gpkptr->m_gpe.rayleng;
int result = 0;
D3DXVECTOR3 adjustv, nv;
ret = lpsh->ChkConfGround( 0, 0, matWorld, befpos, newpos, lpmh, 0, 200.0f, newpos.y - 100.0f, &result, &adjustv, &nv );
if( ret ) {
DbgOut( "GPAnim : CalcFrameData : sh ChkConfGround error !!!\n" );
_ASSERT( 0 );
return 1;
}
if( result ) {
gpkptr->m_gpe.pos = adjustv;
gpkptr->m_gpe.pos.y += gpkptr->m_gpe.offsety;
}
#endif
}
gpkptr = gpkptr->next;
}
}
CGPKey* prevkey = 0;
CGPKey* nextkey = 0;
for( frameno = 0; frameno <= m_maxframe; frameno++ ) {
CGPKey* dstkey = m_framedata + frameno;
dstkey->m_frameno = frameno;
int findkey = 0;
CGPKey* tmpkey = 0;
ExistGPKey( frameno, &tmpkey );
if( tmpkey ) {
findkey = 1;
prevkey = tmpkey;
}
if( prevkey ) {
nextkey = prevkey->next;
}
dstkey->m_keyflag = findkey;
if( findkey ) {
dstkey->m_gpe = prevkey->m_gpe;
dstkey->m_interp = prevkey->m_interp;
} else if( !prevkey ) {
dstkey->m_gpe = *m_defgpeptr;
dstkey->m_interp = *m_definterptr;
} else if( !nextkey ) {
dstkey->m_gpe = prevkey->m_gpe;
dstkey->m_interp = prevkey->m_interp;
} else {
dstkey->m_interp = prevkey->m_interp;
int framenum;
framenum = nextkey->m_frameno - prevkey->m_frameno;
int framecnt;
framecnt = frameno - prevkey->m_frameno;
//DbgOut( "check!!! : GPAnim : CalcFrameData : frameno %d, maxframe %d, framenum %d, framecnt %d\r\n",
// frameno, m_maxframe, framenum, framecnt );
ret = FillUpGPElem( prevkey, nextkey, &dstkey->m_gpe, framenum, framecnt, prevkey->m_interp, lpsh, lpmh, matWorld );
if( ret ) {
DbgOut( "GPAnim : CalcFrameData : FillUpGPElem error !!!\n" );
_ASSERT( 0 );
return 1;
}
}
}
return 0;
}
/// Set Y axis
inline void axisY(Real x,Real y,Real z)
{
axisY(vec3(x,y,z));
}
/// Project direction into local coordinate system
inline vec3 projectDir(const vec3 &a)
{
return vec3(vecProjectLen(a,axisX()),
vecProjectLen(a,axisY()),
vecProjectLen(a,axisZ()));
}
/// Project point into local coordinate system
inline vec3 project(const vec3 &a)const
{
return vec3(vecProjectLen(a-origin(),axisX()),
vecProjectLen(a-origin(),axisY()),
vecProjectLen(a-origin(),axisZ()));
}
Vector3 toObstacle( const Vector3& startPosition, const Vector3& direction,
bool snapToBoundingBox, float distance, bool *snapped)
{
Vector3 extent = startPosition + ( direction * distance );
ObstacleLockCollisionCallback* pCallback = &ObstacleLockCollisionCallback::s_default;
pCallback->dist_ = 1e23f;
ChunkSpacePtr space = ChunkManager::instance().cameraSpace();
distance = space->collide( startPosition,
extent,
*pCallback );
if ( pCallback->dist_ > 0.f && pCallback->dist_ < 1e23f )
{
if (snapped != NULL)
*snapped = true;
Vector3 pos = startPosition + ( direction * pCallback->dist_ );
// modify the position and normal if want to select on bounding box
if (snapToBoundingBox && pCallback->pItem_->pOwnSect())
{
// use bounding box
BoundingBox bb;
pCallback->pItem_->edBounds(bb);
// find apporpriate side of box
// and determine the new normal
Vector3 axisX(1.f, 0.f, 0.f);
Vector3 axisY(0.f, 1.f, 0.f);
Vector3 axisZ(0.f, 0.f, 1.f);
Vector3 triangleNormal = pCallback->triangleNormal_;
float dotX = triangleNormal.dotProduct(axisX);
float dotY = triangleNormal.dotProduct(axisY);
float dotZ = triangleNormal.dotProduct(axisZ);
Vector3 boxNormal;
float temp = 0.f;
if (fabs(dotX) > fabs(dotY))
{
temp = dotX;
boxNormal = axisX;
if (dotX < 0.f)
boxNormal *= -1.f;
}
else
{
temp = dotY;
boxNormal = axisY;
if (dotY < 0.f)
boxNormal *= -1.f;
}
if (fabs(dotZ) > fabs(temp))
{
boxNormal = axisZ;
if (dotZ < 0.f)
boxNormal *= -1.f;
}
// shift the selected point to the surface of the bounding box
Matrix trans = pCallback->pItem_->edTransform();
trans.postMultiply(pCallback->pItem_->chunk()->transform());
Matrix invTrans = trans;
invTrans.invert();
Vector3 localPos = invTrans.applyPoint(pos);
float dist = boxNormal.dotProduct(bb.minBounds() - localPos);
if (dist < 0.f)
{
dist = boxNormal.dotProduct(bb.maxBounds() - localPos);
}
pCallback->normal_ = trans.applyVector( boxNormal );;
pos += dist * pCallback->normal_;
}
return pos;
}
else
{
if (snapped != NULL)
*snapped = false;
}
return startPosition;
}