void DemoApplication::updateCamera() {
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
btScalar rele = m_ele * btScalar(0.01745329251994329547); // rads per deg
btScalar razi = m_azi * btScalar(0.01745329251994329547); // rads per deg
btQuaternion rot(m_cameraUp, razi);
btVector3 eyePos(0, 0, 0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0], eyePos[1], eyePos[2]);
if (forward.length2() < SIMD_EPSILON) {
forward.setValue(1.f, 0.f, 0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right, -rele);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0) return;
btScalar aspect;
btVector3 extents;
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f, 0);
if (m_ortho) {
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
// gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(), -1000,
1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// glTranslatef(100,210,0);
} else {
// glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
glFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear,
-m_frustumZNear, m_frustumZNear, m_frustumZNear,
m_frustumZFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1],
m_cameraTargetPosition[2], m_cameraUp.getX(),
m_cameraUp.getY(), m_cameraUp.getZ());
}
}
void SteamVR::UpdateCamFromTracker(CCamera* cam, int eye)
{
vr::HmdMatrix34_t pose;
vr::HmdTrackingResult eResult;
mVRHmd->GetTrackerFromHeadPose(0, &pose, &eResult);
Matrix3 rot(pose.m[0][0], pose.m[0][1], pose.m[0][2],
pose.m[1][0], pose.m[1][1], pose.m[1][2],
pose.m[2][0], pose.m[2][1], pose.m[2][2]);
Vector3 eyePos(pose.m[0][3], pose.m[1][3], pose.m[2][3]);
eyePos.z *= -1;
// Need to negate z - LHS
Quaternion orientation = ToQuaternion(rot);
orientation.Invert();
orientation.z *= -1;
Vector3 upVec = orientation.RotatedVector(Vector3::cYAxis);
Vector3 forwardVec = orientation.RotatedVector(Vector3::cZAxis);
CTransform* camTransform = cam->gameObject->transform;
mEyePos[eye] = camTransform->GetGlobalPosition() + eyePos;
mView[eye] = Math::LookAtLH(mEyePos[eye], mEyePos[eye] + forwardVec, upVec);
camTransform->SetPosition(mEyePos[eye]);
camTransform->SetRotationFromQuaternion(orientation);
// Calculates new frustum and assigns oculus view and proj matrix
cam->ComputeOculusFrustum(mView[eye], mProj[eye], mOrthoProj[eye]);
}
void clientMotionFunc(int x,int y)
{
if (gPickingConstraintId && physicsEnvironmentPtr)
{
//move the constraint pivot
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p)
{
//keep it at the same picking distance
SimdVector3 newRayTo = GetRayTo(x,y);
SimdVector3 eyePos(eye[0],eye[1],eye[2]);
SimdVector3 dir = newRayTo-eyePos;
dir.normalize();
dir *= gOldPickingDist;
SimdVector3 newPos = eyePos + dir;
p2p->SetPivotB(newPos);
}
}
}
//---------------------------------------------------------------------
void Light::affectRenderable(const RenderablePtr & renderable)
{
/// Set light color.
renderable->setEffectSemanticValue("LightColor", static_cast<void *>(&mColor));
/// Set eye position.
XMVECTOR eyePos(EngineManager::getSingleton().getCamera()->getPosition());
renderable->setEffectSemanticValue("EyePosition", static_cast<void *>(&eyePos));
affectRenderable_impl(renderable);
}
void BasicGLWidget::mousePressEvent(QMouseEvent *e)
{
if (e->buttons() & Qt::LeftButton) {
_camera->mousePress(e->pos(), e->modifiers());
} else if (e->button() == Qt::RightButton) {
QVector3D clickPos(_camera->mapPoint(e->pos())),
eyePos(_camera->world().inverted().column(3)),
direction = (clickPos - eyePos).normalized();
Mesh* selected = getNearestMesh(eyePos, direction);
renderer->setCurrentMesh(selected);
if (selected)
emit meshSelected(selected);
}
}
void EngineRunningBase::DrawLensflare()
{
// Lensflare: draw if the sparkling sun is on screen.
if (!m_pLevel->IsCurrentRoomIndoors() &&
GetEngine()->GetDayNightSky()->IsSparkleVisible())
{
// Make a bounding sphere around the sun coords.
Vec3f sunPos = GetEngine()->GetDayNightSky()->GetSunPosition();
BoundingSphere sunBs(sunPos, 50.0f); // sun's "radius"
// Find out if the sun intersects the view frustum
if (Frustum::Instance()->Contains(sunBs))
{
PCamera pCam = GetCamera();
Assert(pCam.GetPtr());
Vec3f eyePos(pCam->GetOrientation()->GetX(),
pCam->GetOrientation()->GetY(),
pCam->GetOrientation()->GetZ());
// We have to draw the lens flare, unless the sun is obscured by part of
// the scene.
// Test the line from camera to sun for obstructions.
//Mgc::Segment3 seg;
//seg.Origin() = Mgc::Vector3(eyePos.x, eyePos.y, eyePos.z);
//seg.Direction() = Mgc::Vector3(
// sunPos.x - eyePos.x,
// sunPos.y - eyePos.y,
// sunPos.z - eyePos.z);
// Do intersection test on the scenery for the current room.
if (m_pLevel->GetScene()->LineIntersects(eyePos, sunPos, 1.0f /* some radius */ ))
{
return; // Sun is obscured.
}
// We should draw the lens flare. Get the Camera eye position and
// "look at" position (some point along the line we are pointing at).
GetEngine()->PushColour(1.0f, 1.0f, 1.0f, 1.0f);
//GetEngine()->GetLensflare()->Draw(
// GetEngine()->GetDayNightSky()->GetSunPosition(),
// eyePos,
// pCam->GetLookAtPos() );
GetEngine()->PopColour();
}
}
}
void ofDrawCircle(const ofVec3f& center, float radius)
{
ofVec3f eye = PCXYZ_OFVEC3F(eyePos());
ofVec3f dir = eye - center;
ofVec3f yAxis (0,1,0);
ofVec3f xAxis (1,0,0);
ofVec3f rotationAxis = dir.crossed(yAxis);
glPushMatrix();
glTranslatef(center.x,center.y,center.z);
glRotatef(dir.angle(yAxis),rotationAxis.x,rotationAxis.y,rotationAxis.z);
rotationAxis = dir.crossed(xAxis);
glRotatef(dir.angle(xAxis),rotationAxis.x,rotationAxis.y,rotationAxis.z);
ofCircle(0,0,0,radius);
glPopMatrix();
}
void UnitLaser::render( SDL_Surface *const target )
{
#ifdef _DEBUG
debugString += Utility::numToStr( angle ) + "\n" + Utility::numToStr( angleVel );
#endif
UnitBase::render( target );
Vector2d<float> eyePos( *x + cos( angle ) * LASER_EYE_DISTANCE, *y + sin( angle ) * LASER_EYE_DISTANCE );
if ( hasCharged )
{
float factor = (float)charge.getTime() / (float)LASER_CHARGE_TIME;
spEllipse( eyePos.x, eyePos.y, -1, 8, 8, spGetRGB( 255, 255 * factor, 255 * factor ) );
spEllipse( eyePos.x, eyePos.y, -1, 4, 4, spGetRGB( 255.0f * (1.0f - factor), 0, 255.0f * factor ) );
}
else
{
spEllipse( eyePos.x, eyePos.y, -1, 8, 8, -1 );
spEllipse( eyePos.x, eyePos.y, -1, 4, 4, spGetFastRGB( 0, 0, 255 ) );
}
}
// 描画.
IZ_BOOL CStateBevelShader::Render(izanagi::graph::CGraphicsDevice* device)
{
izanagi::math::SMatrix44 mtxL2W;
izanagi::math::SMatrix44::SetUnit(mtxL2W);
m_Shader->Begin(device, 0, IZ_FALSE);
{
if (m_Shader->BeginPass(0)) {
// パラメータ設定
SetShaderParam(
m_Shader,
"g_mL2W",
(void*)&mtxL2W,
sizeof(mtxL2W));
SetShaderParam(
m_Shader,
"g_mW2C",
(void*)&m_Camera.mtxW2C,
sizeof(m_Camera.mtxW2C));
izanagi::SParallelLightParam parallelLight;
// ライトパラメータ
{
// Ambient Light Color
izanagi::SAmbientLightParam ambient;
ambient.color.Set(0.0f, 0.0f, 0.0f);
// Parallel Light Color
parallelLight.color.Set(1.0f, 1.0f, 1.0f);
// Parallel Light Direction
parallelLight.vDir.Set(-1.0f, -1.0f, -1.0f);
izanagi::math::SVector4::Normalize(parallelLight.vDir, parallelLight.vDir);
// マテリアル
izanagi::SMaterialParam mtrl;
{
mtrl.vDiffuse.Set(1.0f, 1.0f, 1.0f, 1.0f);
mtrl.vAmbient.Set(1.0f, 1.0f, 1.0f, 1.0f);
mtrl.vSpecular.Set(1.0f, 1.0f, 1.0f, 20.0f);
}
SetShaderParam(m_Shader, "g_vLitParallelColor", ¶llelLight.color, sizeof(parallelLight.color));
SetShaderParam(m_Shader, "g_vLitAmbientColor", &ambient.color, sizeof(ambient.color));
SetShaderParam(m_Shader, "g_vMtrlDiffuse", &mtrl.vDiffuse, sizeof(mtrl.vDiffuse));
SetShaderParam(m_Shader, "g_vMtrlAmbient", &mtrl.vAmbient, sizeof(mtrl.vAmbient));
SetShaderParam(m_Shader, "g_vMtrlSpecular", &mtrl.vSpecular, sizeof(mtrl.vSpecular));
}
{
// ライトの方向をローカル座標に変換する
// ライトの方向はワールド座標なので World -> Localマトリクスを計算する
izanagi::math::SMatrix44 mtxW2L;
izanagi::math::SMatrix44::Inverse(mtxW2L, mtxL2W);
// World -> Local
izanagi::math::SVector4 parallelLightLocalDir;
izanagi::math::SMatrix44::ApplyXYZ(
parallelLightLocalDir,
parallelLight.vDir,
mtxW2L);
SetShaderParam(
m_Shader,
"g_vLitParallelDir",
(void*)¶llelLightLocalDir,
sizeof(parallelLightLocalDir));
// L2V = L2W * W2V の逆行列を計算する
izanagi::math::SMatrix44 mtxV2L;
izanagi::math::SMatrix44::Mul(mtxV2L, mtxL2W, m_Camera.mtxW2V);
izanagi::math::SMatrix44::Inverse(mtxV2L, mtxV2L);
// ビュー座標系における視点は常に原点
izanagi::math::CVector4 eyePos(0.0f, 0.0f, 0.0f, 1.0f);
// 視点のローカル座標を計算する
izanagi::math::SMatrix44::Apply(eyePos, eyePos, mtxV2L);
SetShaderParam(
m_Shader,
"g_vEye",
(void*)&eyePos,
sizeof(eyePos));
}
m_Shader->CommitChanges(device);
m_Mesh->Draw(device);
}
}
m_Shader->End(device);
RenderName(device, "BevelShader");
return IZ_TRUE;
}
void updateCamera() {
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
btScalar rele = 0;
btScalar razi = 0;
btVector3 m_cameraUp(0,1,0);
btScalar m_cameraDistance = 10.f;
btVector3 m_cameraPosition;
btVector3 m_cameraTargetPosition = (tr[0].getOrigin()+tr[1].getOrigin())*0.5;
btQuaternion rot(m_cameraUp,razi);
int m_forwardAxis = 2;
btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0)
return;
btScalar aspect;
btVector3 extents;
if (m_glutScreenWidth > m_glutScreenHeight)
{
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
aspect = m_glutScreenHeight / (btScalar)m_glutScreenWidth;
extents.setValue(1.0f, aspect*1.f,0);
}
if (m_ortho)
{
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//glTranslatef(100,210,0);
} else
{
if (m_glutScreenWidth > m_glutScreenHeight)
{
// glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
} else
{
// glFrustum (-1.0, 1.0, -aspect, aspect, 1.0, 10000.0);
glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
}
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
}
}
//-----------------------------------------------------------------------
void PUBillboardChain::updateVertexBuffer(const Mat4 &camMat)
{
setupBuffers();
// The contents of the vertex buffer are correct if they are not dirty
// and the camera used to build the vertex buffer is still the current
// camera.
if (!_vertexContentDirty)
return;
VertexInfo vi = {Vec3(0.0f, 0.0f, 0.0f), Vec2(0.0f, 0.0f), Vec4::ONE};
_vertices.assign(_vertices.size(), vi);
//HardwareVertexBufferSharedPtr pBuffer =
// _vertexData->vertexBufferBinding->getBuffer(0);
//void* pBufferStart = pBuffer->lock(HardwareBuffer::HBL_DISCARD);
//const Vector3& camPos = cam->getDerivedPosition();
//Vector3 eyePos = mParentNode->_getDerivedOrientation().Inverse() *
// (camPos - mParentNode->_getDerivedPosition()) / mParentNode->_getDerivedScale();
Vec3 eyePos(camMat.m[12], camMat.m[13], camMat.m[14]);
Vec3 chainTangent;
for (ChainSegmentList::iterator segi = _chainSegmentList.begin();
segi != _chainSegmentList.end(); ++segi)
{
ChainSegment& seg = *segi;
// Skip 0 or 1 element segment counts
if (seg.head != SEGMENT_EMPTY && seg.head != seg.tail)
{
size_t laste = seg.head;
for (size_t e = seg.head; ; ++e) // until break
{
// Wrap forwards
if (e == _maxElementsPerChain)
e = 0;
Element& elem = _chainElementList[e + seg.start];
CCASSERT (((e + seg.start) * 2) < 65536, "Too many elements!");
unsigned short vertexIndex = static_cast<unsigned short>((e + seg.start) * 2);
//// Determine base pointer to vertex #1
//void* pBase = static_cast<void*>(
// static_cast<char*>(pBufferStart) +
// pBuffer->getVertexSize() * baseIdx);
// Get index of next item
size_t nexte = e + 1;
if (nexte == _maxElementsPerChain)
nexte = 0;
if (e == seg.head)
{
// No laste, use next item
chainTangent = _chainElementList[nexte + seg.start].position - elem.position;
}
else if (e == seg.tail)
{
// No nexte, use only last item
chainTangent = elem.position - _chainElementList[laste + seg.start].position;
}
else
{
// A mid position, use tangent across both prev and next
chainTangent = _chainElementList[nexte + seg.start].position - _chainElementList[laste + seg.start].position;
}
Vec3 vP1ToEye;
//if( _faceCamera )
vP1ToEye = eyePos - elem.position;
//else
// vP1ToEye = elem.orientation * _normalBase;
Vec3 vPerpendicular;
Vec3::cross(chainTangent, vP1ToEye, &vPerpendicular);
vPerpendicular.normalize();
vPerpendicular *= (elem.width * 0.5f);
Vec3 pos0 = elem.position - vPerpendicular;
Vec3 pos1 = elem.position + vPerpendicular;
//float* pFloat = static_cast<float*>(pBase);
//// pos1
//*pFloat++ = pos0.x;
//*pFloat++ = pos0.y;
//*pFloat++ = pos0.z;
_vertices[vertexIndex + 0].position = pos0;
//pBase = static_cast<void*>(pFloat);
if (_useVertexColour)
{
//RGBA* pCol = static_cast<RGBA*>(pBase);
//Root::getSingleton().convertColourValue(elem.colour, pCol);
//pCol++;
//pBase = static_cast<void*>(pCol);
_vertices[vertexIndex + 0].color = elem.color;
}
if (_useTexCoords)
{
//pFloat = static_cast<float*>(pBase);
if (_texCoordDir == TCD_U)
{
//*pFloat++ = elem.texCoord;
//*pFloat++ = _otherTexCoordRange[0];
_vertices[vertexIndex + 0].uv.x = elem.texCoord;
_vertices[vertexIndex + 0].uv.y = _otherTexCoordRange[0];
}
else
{
//*pFloat++ = _otherTexCoordRange[0];
//*pFloat++ = elem.texCoord;
_vertices[vertexIndex + 0].uv.x = _otherTexCoordRange[0];
_vertices[vertexIndex + 0].uv.y = elem.texCoord;
}
//pBase = static_cast<void*>(pFloat);
}
// pos2
//pFloat = static_cast<float*>(pBase);
//*pFloat++ = pos1.x;
//*pFloat++ = pos1.y;
//*pFloat++ = pos1.z;
//pBase = static_cast<void*>(pFloat);
_vertices[vertexIndex + 1].position = pos1;
if (_useVertexColour)
{
//RGBA* pCol = static_cast<RGBA*>(pBase);
//Root::getSingleton().convertColourValue(elem.colour, pCol);
//pCol++;
//pBase = static_cast<void*>(pCol);
_vertices[vertexIndex + 1].color = elem.color;
}
if (_useTexCoords)
{
//pFloat = static_cast<float*>(pBase);
if (_texCoordDir == TCD_U)
{
//*pFloat++ = elem.texCoord;
//*pFloat++ = _otherTexCoordRange[1];
_vertices[vertexIndex + 1].uv.x = elem.texCoord;
_vertices[vertexIndex + 1].uv.y = _otherTexCoordRange[1];
}
else
{
//*pFloat++ = _otherTexCoordRange[1];
//*pFloat++ = elem.texCoord;
_vertices[vertexIndex + 1].uv.x = _otherTexCoordRange[1];
_vertices[vertexIndex + 1].uv.y = elem.texCoord;
}
}
if (e == seg.tail)
break; // last one
laste = e;
//vertexIndex += 2;
} // element
} // segment valid?
} // each segment
_vertexBuffer->updateVertices(&_vertices[0], (int)_vertices.size(), 0);;
//pBuffer->unlock();
//_vertexCameraUsed = cam;
_vertexContentDirty = false;
}
void GLInstancingRenderer::updateCamera()
{
GLint err = glGetError();
assert(err==GL_NO_ERROR);
int m_forwardAxis(2);
float m_frustumZNear=1;
float m_frustumZFar=10000.f;
// m_azi=m_azi+0.01;
b3Scalar rele = m_data->m_ele * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar razi = m_data->m_azi * b3Scalar(0.01745329251994329547);// rads per deg
b3Quaternion rot(m_data->m_cameraUp,razi);
b3Vector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_data->m_cameraDistance;
b3Vector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < B3_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
b3Vector3 right = m_data->m_cameraUp.cross(forward);
b3Quaternion roll(right,-rele);
eyePos = b3Matrix3x3(rot) * b3Matrix3x3(roll) * eyePos;
m_data->m_cameraPosition[0] = eyePos.getX();
m_data->m_cameraPosition[1] = eyePos.getY();
m_data->m_cameraPosition[2] = eyePos.getZ();
m_data->m_cameraPosition += m_data->m_cameraTargetPosition;
if (m_screenWidth == 0 && m_screenHeight == 0)
return;
b3Scalar aspect;
b3Vector3 extents;
aspect = m_screenWidth / (b3Scalar)m_screenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
if (m_ortho)
{
// reset matrix
extents *= m_data->m_cameraDistance;
//b3Vector3 lower = m_data->m_cameraTargetPosition - extents;
//b3Vector3 upper = m_data->m_cameraTargetPosition + extents;
//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
//glTranslatef(100,210,0);
} else
{
// glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
}
if (m_screenWidth > m_screenHeight)
{
b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,projectionMatrix);
} else
{
b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,projectionMatrix);
}
b3CreateLookAt(m_data->m_cameraPosition,m_data->m_cameraTargetPosition,m_data->m_cameraUp,modelviewMatrix);
}
void BasicDemo::updateCamera() {
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
float rele = m_ele * 0.01745329251994329547;// rads per deg
float razi = m_azi * 0.01745329251994329547;// rads per deg
btQuaternion rot(m_cameraUp,razi);
btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0)
return;
btScalar aspect;
btVector3 extents;
if (m_glutScreenWidth > m_glutScreenHeight)
{
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
} else
{
aspect = m_glutScreenHeight / (btScalar)m_glutScreenWidth;
extents.setValue(1.0f, aspect*1.f,0);
}
if (m_ortho)
{
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//glTranslatef(100,210,0);
} else
{
if (m_glutScreenWidth > m_glutScreenHeight)
{
glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
} else
{
glFrustum (-1.0, 1.0, -aspect, aspect, 1.0, 10000.0);
}
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
btTransform invCam = m_blendReader->m_cameraTrans.inverse();
float m[16];
invCam.getOpenGLMatrix(m);
glMultMatrixf(m);
//gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
// m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
// m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
}
}
void clientMouseFunc(int button, int state, int x, int y)
{
//printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y);
//button 0, state 0 means left mouse down
SimdVector3 rayTo = GetRayTo(x,y);
switch (button)
{
case 2:
{
if (state==0)
{
shootBox(rayTo);
}
break;
};
case 1:
{
if (state==0)
{
//apply an impulse
if (physicsEnvironmentPtr)
{
float hit[3];
float normal[3];
PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
if (hitObj)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
if (body)
{
body->SetActivationState(ACTIVE_TAG);
SimdVector3 impulse = rayTo;
impulse.normalize();
float impulseStrength = 10.f;
impulse *= impulseStrength;
SimdVector3 relPos(
hit[0] - body->getCenterOfMassPosition().getX(),
hit[1] - body->getCenterOfMassPosition().getY(),
hit[2] - body->getCenterOfMassPosition().getZ());
body->applyImpulse(impulse,relPos);
}
}
}
} else
{
}
break;
}
case 0:
{
if (state==0)
{
//add a point to point constraint for picking
if (physicsEnvironmentPtr)
{
float hit[3];
float normal[3];
PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
if (hitObj)
{
CcdPhysicsController* physCtrl = static_cast<CcdPhysicsController*>(hitObj);
RigidBody* body = physCtrl->GetRigidBody();
if (body)
{
pickedBody = body;
pickedBody->SetActivationState(DISABLE_DEACTIVATION);
SimdVector3 pickPos(hit[0],hit[1],hit[2]);
SimdVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
gPickingConstraintId = physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT,
localPivot.getX(),
localPivot.getY(),
localPivot.getZ(),
0,0,0);
//printf("created constraint %i",gPickingConstraintId);
//save mouse position for dragging
gOldPickingPos = rayTo;
SimdVector3 eyePos(eye[0],eye[1],eye[2]);
gOldPickingDist = (pickPos-eyePos).length();
Point2PointConstraint* p2p = static_cast<Point2PointConstraint*>(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId));
if (p2p)
{
//very weak constraint for picking
p2p->m_setting.m_tau = 0.1f;
}
}
}
}
} else
{
if (gPickingConstraintId && physicsEnvironmentPtr)
{
physicsEnvironmentPtr->removeConstraint(gPickingConstraintId);
//printf("removed constraint %i",gPickingConstraintId);
gPickingConstraintId = 0;
pickedBody->ForceActivationState(ACTIVE_TAG);
pickedBody->m_deactivationTime = 0.f;
pickedBody = 0;
}
}
break;
}
default:
{
}
}
}
//--------------------------------------------------------------
map<int,TrackedCloudPtr> ObjectsThread::computeOcclusions(const list<TrackedCloudPtr>& potentialOcclusions)
{
map<int,TrackedCloudPtr> occlusions;
ofVec3f origin = PCXYZ_OFVEC3F(eyePos());
inCloudMutex.lock();
PCPtr cloud = PCPtr(new PC(*inRawCloud));
inRawCloud.reset();
inCloudMutex.unlock();
saveCloud("rawInternal.pcd",*cloud);
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree(Constants::CLOUD_VOXEL_SIZE*2);
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::AlignedPointTVector voxelList;
if(cloud->size() > 0)
{
octree.setInputCloud(cloud);
octree.defineBoundingBox();
octree.addPointsFromInputCloud();
gModel->objectsMutex.lock();
for (list<TrackedCloudPtr>::const_iterator iter = potentialOcclusions.begin(); iter != potentialOcclusions.end(); iter++)
{
if((*iter)->hasObject())
{
bool occludedPol = true;
PCPolyhedron* polyhedron = dynamic_cast<PCPolyhedron*>((*iter)->getTrackedObject().get());
polyhedron->resetOccludedFaces();
vector<IPolygonPtr> pols = polyhedron->getVisiblePolygons();
int occludedFaces = 0;
for(int i = 0; i < pols.size(); i++)
{
vector<ofVec3f> vexs = pols.at(i)->getMathModel().getVertexs();
int occludedVertexs = 0;
for(int o = 0; o < vexs.size(); o++)
{
bool occludedVertex = false;
ofVec3f end = vexs.at(o);
Eigen::Vector3f endPoint(end.x,end.y,end.z);
Eigen::Vector3f originPoint = PCXYZ_EIGEN3F(eyePos());
voxelList.clear();
int voxs = octree.getApproxIntersectedVoxelCentersBySegment(originPoint,endPoint,voxelList,Constants::CLOUD_VOXEL_SIZE*2);
for(int i = 0; i < voxelList.size(); i ++)
{
if(octree.isVoxelOccupiedAtPoint(voxelList.at(i)))
{
ofVec3f intersect (voxelList.at(i).x,voxelList.at(i).y,voxelList.at(i).z);
if(((intersect - end).length() > Constants::CLOUD_VOXEL_SIZE*5) &&
(intersect - origin).length() < (end - origin).length())
occludedVertexs++;
}
}
}
if(occludedVertexs >= 2)
{
polyhedron->setOccludedFace(pols.at(i)->getName());
occludedFaces++;
}
}
if(occludedFaces > 1)
{
occlusions[(*iter)->getTrackedObject()->getId()] = (*iter);
//cout << " occluded pol " << endl;
}
}
}
gModel->objectsMutex.unlock();
}
return occlusions;
}
void csVProcStandardProgram::SetupState (const csRenderMesh* mesh,
csRenderMeshModes& modes,
const csShaderVariableStack& stack)
{
bool skin_updated = false;// @@@ FIXME - time related detection if vertices are not already updated
if (doVertexSkinning || doNormalSkinning ||
doTangentSkinning || doBiTangentSkinning)
{
skin_updated = UpdateSkinnedVertices (modes, stack);
}
if (numLights > 0)
{
int lightsActive = 0;
CS::ShaderVarStringID id;
id = shaderPlugin->lsvCache.GetDefaultSVId (
csLightShaderVarCache::varLightCount);
csShaderVariable* sv;
if ((stack.GetSize () > id) && ((sv = stack[id]) != 0))
sv->GetValue (lightsActive);
iRenderBuffer *vbuf = doVertexSkinning && skin_updated ?
modes.buffers->GetRenderBuffer (skinnedPositionOutputBuffer) :
GetBuffer (positionBuffer, modes, stack);
iRenderBuffer *nbuf = doNormalSkinning && skin_updated ?
modes.buffers->GetRenderBuffer (skinnedNormalOutputBuffer) :
GetBuffer (normalBuffer, modes, stack);
iRenderBuffer *cbuf = GetBuffer (colorBuffer, modes, stack);
if (vbuf == 0 || nbuf == 0) return;
csReversibleTransform camtrans;
if ((stack.GetSize () > shaderPlugin->string_world2camera)
&& ((sv = stack[shaderPlugin->string_world2camera]) != 0))
sv->GetValue (camtrans);
csVector3 eyePos (camtrans.GetT2OTranslation ());
csVector3 eyePosObject (mesh->object2world.Other2This (eyePos));
bool hasAlpha = cbuf && cbuf->GetComponentCount() >= 4;
//copy output
size_t elementCount = vbuf->GetElementCount ();
csRef<iRenderBuffer> clbuf;
if (doDiffuse)
{
clbuf = csRenderBuffer::CreateRenderBuffer (elementCount, CS_BUF_STREAM,
CS_BUFCOMP_FLOAT, hasAlpha ? 4 : 3);
// @@@ FIXME: should probably get rid of the multiple locking/unlocking...
csRenderBufferLock<float> tmpColor (clbuf);
memset (tmpColor, 0, sizeof(float) * (hasAlpha?4:3) * elementCount);
}
csRef<iRenderBuffer> specBuf;
if (doSpecular)
{
specBuf = csRenderBuffer::CreateRenderBuffer (elementCount,
CS_BUF_STREAM, CS_BUFCOMP_FLOAT, 3);
csRenderBufferLock<float> tmpColor (specBuf);
memset (tmpColor, 0, sizeof(csColor) * elementCount);
}
float shininess = GetParamFloatVal (stack, shininessParam, 0.0f);
if (lightsActive > 0)
{
if (lightMixMode == LIGHTMIXMODE_NONE)
{
//only calculate last, other have no effect
const size_t lightNum = csMin((size_t)lightsActive, numLights)-1;
if ((disableMask.GetSize() <= lightNum)
|| !disableMask.IsBitSet (lightNum))
{
csLightProperties light (lightNum, shaderPlugin->lsvCache, stack,
mesh->object2world);
iVertexLightCalculator *calc =
shaderPlugin->GetLightCalculator (light, useAttenuation);
calc->CalculateLighting (light, eyePosObject, shininess, elementCount,
vbuf, nbuf, clbuf, specBuf);
}
}
else
{
LightMixmode useMixMode = LIGHTMIXMODE_ADD;
for (size_t i = 0; i < (csMin((size_t)lightsActive, numLights)); i++)
{
if ((disableMask.GetSize() > i) && disableMask.IsBitSet (i))
{
useMixMode = lightMixMode;
continue;
}
csLightProperties light (i, shaderPlugin->lsvCache, stack,
mesh->object2world);
iVertexLightCalculator *calc =
shaderPlugin->GetLightCalculator (light, useAttenuation);
switch (useMixMode)
{
case LIGHTMIXMODE_ADD:
{
calc->CalculateLightingAdd (light, eyePosObject, shininess, elementCount,
vbuf, nbuf, clbuf, specBuf);
break;
}
case LIGHTMIXMODE_MUL:
{
calc->CalculateLightingMul (light, eyePosObject, shininess, elementCount,
vbuf, nbuf, clbuf, specBuf);
break;
}
case LIGHTMIXMODE_NONE:
break;
}
useMixMode = lightMixMode;
}
}
}
if (doDiffuse && cbuf)
{
switch (colorMixMode)
{
case LIGHTMIXMODE_NONE:
if (!hasAlpha) break;
{
csVertexListWalker<float> cbufWalker (cbuf, 4);
csRenderBufferLock<csVector4, iRenderBuffer*> tmpColor (clbuf);
for (size_t i = 0; i < elementCount; i++)
{
const float* c = cbufWalker;
tmpColor[i].w = c[3];
++cbufWalker;
}
}
break;
case LIGHTMIXMODE_ADD:
{
csVertexListWalker<float> cbufWalker (cbuf, 4);
csRenderBufferLock<csVector4, iRenderBuffer*> tmpColor (clbuf);
for (size_t i = 0; i < elementCount; i++)
{
csVector4& t = tmpColor[i];
const float* c = cbufWalker;
for (int j = 0; j < 3; j++)
t[j] += c[j];
if (hasAlpha) t[3] = c[3];
++cbufWalker;
}
}
break;
case LIGHTMIXMODE_MUL:
{
csVertexListWalker<float> cbufWalker (cbuf, 4);
csRenderBufferLock<csVector4, iRenderBuffer*> tmpColor (clbuf);
for (size_t i = 0; i < elementCount; i++)
{
csVector4& t = tmpColor[i];
const float* c = cbufWalker;
for (int j = 0; j < 3; j++)
t[j] *= c[j];
if (hasAlpha) t[3] = c[3];
++cbufWalker;
}
}
break;
default:
CS_ASSERT (false);
}
if (doSpecular && specularOnDiffuse)
{
csRenderBufferLock<csColor> tmpColor (clbuf);
csRenderBufferLock<csColor> tmpColor2 (specBuf);
for (size_t i = 0; i < elementCount; i++)
{
tmpColor[i] += tmpColor2[i];
}
}
}
float finalLightFactorReal = GetParamFloatVal (stack, finalLightFactor,
1.0f);
if (doDiffuse)
{
{
csRenderBufferLock<csColor> tmpColor (clbuf);
for (size_t i = 0; i < elementCount; i++)
{
tmpColor[i] *= finalLightFactorReal;
}
}
modes.buffers->SetAccessor (modes.buffers->GetAccessor(),
modes.buffers->GetAccessorMask() & ~CS_BUFFER_COLOR_MASK);
modes.buffers->SetRenderBuffer (CS_BUFFER_COLOR, clbuf);
}
if (doSpecular && !specularOnDiffuse)
{
csRenderBufferLock<csColor> tmpColor (specBuf);
for (size_t i = 0; i < elementCount; i++)
{
tmpColor[i] *= finalLightFactorReal;
}
modes.buffers->SetAccessor (modes.buffers->GetAccessor(),
modes.buffers->GetAccessorMask() & ~(1 << specularOutputBuffer));
modes.buffers->SetRenderBuffer (specularOutputBuffer, specBuf);
}
}
}
void Lux::Graphics::RenderingSystem::RenderPass()
{
if (!m_LightEntry || !m_MainCamera || !m_MainCameraTransform)
return;
// Resize Window if needed
if (m_RenderWindow->IsWindowResized())
{
float aspect = m_RenderWindow->GetWidth() / (float)m_RenderWindow->GetHeight();
m_MainCamera->GetRawPtr()->ChangeAspect(aspect);
}
m_MainCameraTransform->GetRawPtr()->ApplyTransform();
EntityMap::iterator it;
for (it = m_EntityMap.begin(); it != m_EntityMap.end(); ++it)
{
if (!it->second.m_Transform)
continue;
if (!it->second.m_MeshRenderer)
continue;
Core::Mesh* mesh = it->second.m_MeshRenderer->GetRawPtr()->GetMesh().get();
if (!mesh)
continue;
// Make sure we multiply the transforms properly, taking into account parents
mat4x4 finalTransform;
Core::Transform* transformPtr = it->second.m_Transform->GetRawPtr();
bool hasParent = true;
do
{
hasParent = transformPtr->GetParentTransform().IsValid();
transformPtr->ApplyTransform();
finalTransform *= transformPtr->GetMatrix();
transformPtr = transformPtr->GetParentTransform().GetRawPtr();
} while (hasParent);
// Set the default uniform buffer
Core::ShaderVariable worldMatVal(Core::VALUE_MAT4X4, glm::value_ptr(finalTransform), sizeof(mat4));
Core::ShaderVariable viewMatVal(Core::VALUE_MAT4X4, glm::value_ptr(m_MainCameraTransform->GetRawPtr()->GetInverseTranslationMatrix()), sizeof(mat4));
Core::ShaderVariable projMatVal(Core::VALUE_MAT4X4, glm::value_ptr(m_MainCamera->GetRawPtr()->GetProjectionMatrix()), sizeof(mat4));
m_ObjUniformBuffer.SetVariable(0, viewMatVal);
m_ObjUniformBuffer.SetVariable(1, projMatVal);
m_ObjUniformBuffer.SetVariable(2, worldMatVal);
// Light Buffer
LightShaderResource lightShaderRes;
lightShaderRes.m_Position = m_LightEntry->m_Transform->GetRawPtr()->GetPosition();
lightShaderRes.m_Color = m_LightEntry->m_Light->GetRawPtr()->GetColor();
lightShaderRes.m_Direction = m_LightEntry->m_Light->GetRawPtr()->GetDirection();
lightShaderRes.m_Intensity = m_LightEntry->m_Light->GetRawPtr()->GetIntensity();
//lightShaderRes.m_Type = (int)m_LightEntry->m_Light->GetRawPtr()->GetType();
Core::ShaderVariable lightShaderVar(Core::VALUE_STRUCT, &lightShaderRes, sizeof(LightShaderResource));
Core::ShaderVariable eyePos(Core::VALUE_VEC3, glm::value_ptr(m_MainCameraTransform->GetRawPtr()->GetPosition()), sizeof(vec3));
m_LightUniformBuffer.SetVariable(0, lightShaderVar);
m_LightUniformBuffer.SetVariable(1, eyePos);
Core::Shader* shader = mesh->GetShader().get();
shader->Activate();
// Material Buffer
Core::Material* matRes = mesh->GetMaterialProperties().get();
MaterialShaderResource matShaderRes(*matRes);
Core::ShaderVariable matShader(Core::VALUE_STRUCT, &matShaderRes, sizeof(MaterialShaderResource));
m_MatUniformBuffer.SetVariable(0, matShader);
if (!it->second.m_Init)
{
shader->InitializeUniformBuffer("ObjectBuffer", m_ObjUniformBuffer, VERTEX_PROGRAM);
shader->InitializeUniformBuffer("LightBuffer", m_LightUniformBuffer, FRAGMENT_PROGRAM);
shader->InitializeUniformBuffer("MaterialBuffer", m_MatUniformBuffer, FRAGMENT_PROGRAM);
it->second.m_Init = true;
}
// Bind Samplers and Textures
Core::Texture2D* diffuseTex = mesh->GetTexture(Core::DIFFUSE_MAP_IDX).get();
LuxAssert(diffuseTex);
Core::TextureSampler* texSampler = diffuseTex->GetSampler().get();
LuxAssert(texSampler);
texSampler->Activate(Core::DIFFUSE_MAP_IDX, FRAGMENT_PROGRAM);
diffuseTex->Bind(Core::DIFFUSE_MAP_IDX, "DiffuseTexture", shader, FRAGMENT_PROGRAM);
shader->Update();
mesh->PreRender();
m_RenderWindow->Render(mesh);
mesh->PostRender();
diffuseTex->Unbind();
texSampler->Deactivate();
shader->Deactivate();
}
}
//-----------------------------------------------------------------------------
void CHomingMissile::UpdateControlledMissile(float frameTime)
{
bool isServer = gEnv->bServer;
bool isClient = gEnv->bClient;
CActor *pClientActor=0;
if (gEnv->bClient)
pClientActor=static_cast<CActor *>(g_pGame->GetIGameFramework()->GetClientActor());
bool isOwner = ((!m_ownerId && isServer) || (isClient && pClientActor && (pClientActor->GetEntityId() == m_ownerId) && pClientActor->IsPlayer()));
IRenderer* pRenderer = gEnv->pRenderer;
IRenderAuxGeom* pGeom = pRenderer->GetIRenderAuxGeom();
float color[4] = {1,1,1,1};
const static float step = 15.f;
float y = 20.f;
bool bDebug = g_pGameCVars->i_debug_projectiles > 0;
if (isOwner || isServer)
{
//If there's a target, follow the target
if(isServer)
{
if (m_targetId)
{
if (m_lockedTimer>0.0f)
m_lockedTimer=m_lockedTimer-frameTime;
else
{
// If we are here, there's a target
IEntity* pTarget = gEnv->pEntitySystem->GetEntity(m_targetId);
if (pTarget)
{
AABB box;
pTarget->GetWorldBounds(box);
Vec3 finalDes = box.GetCenter();
SetDestination(finalDes);
//SetDestination( box.GetCenter() );
if (bDebug)
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "Target Entity: %s", pTarget->GetName());
}
m_lockedTimer+=0.05f;
}
}
else if(m_autoControlled)
return;
}
if (m_controlled && !m_autoControlled && isOwner && !m_targetId)
{
//Check if the weapon is still selected
CWeapon *pWeapon = GetWeapon();
if(!pWeapon || !pWeapon->IsSelected())
return;
if (m_controlledTimer>0.0f)
m_controlledTimer=m_controlledTimer-frameTime;
else if (pClientActor && pClientActor->IsPlayer()) //Follow the crosshair
{
if (IMovementController *pMC=pClientActor->GetMovementController())
{
Vec3 eyePos(ZERO);
Vec3 eyeDir(ZERO);
IVehicle* pVehicle = pClientActor->GetLinkedVehicle();
if(!pVehicle)
{
SMovementState state;
pMC->GetMovementState(state);
eyePos = state.eyePosition;
eyeDir = state.eyeDirection;
}
else
{
SViewParams viewParams;
pVehicle->UpdateView(viewParams, pClientActor->GetEntityId());
eyePos = viewParams.position;
eyeDir = viewParams.rotation * Vec3(0,1,0);
//eyeDir = (viewParams.targetPos - viewParams.position).GetNormalizedSafe();
}
int pierceability=7;
if (IPhysicalEntity *pPE=GetEntity()->GetPhysics())
{
if (pPE->GetType()==PE_PARTICLE)
{
pe_params_particle pp;
if (pPE->GetParams(&pp))
pierceability=pp.iPierceability;
}
}
static const int objTypes = ent_all;
static const int flags = (geom_colltype_ray << rwi_colltype_bit) | rwi_colltype_any | (pierceability & rwi_pierceability_mask) | (geom_colltype14 << rwi_colltype_bit);
IPhysicalWorld* pWorld = gEnv->pPhysicalWorld;
static IPhysicalEntity* pSkipEnts[10];
int numSkip = CSingle::GetSkipEntities(pWeapon, pSkipEnts, 10);
ray_hit hit;
int hits = 0;
float range=m_maxTargetDistance;
hits = pWorld->RayWorldIntersection(eyePos + 1.5f*eyeDir, eyeDir*range, objTypes, flags, &hit, 1, pSkipEnts, numSkip);
while (hits)
{
if (gEnv->p3DEngine->RefineRayHit(&hit, eyeDir*range))
break;
eyePos = hit.pt+eyeDir*0.003f;
range -= hit.dist+0.003f;
hits = pWorld->RayWorldIntersection(eyePos, eyeDir*range, objTypes, flags, &hit, 1, pSkipEnts, numSkip);
}
DestinationParams params;
if(hits)
params.pt=hit.pt;
else
params.pt=(eyePos+m_maxTargetDistance*eyeDir); //Some point in the sky...
GetGameObject()->InvokeRMI(SvRequestDestination(), params, eRMI_ToServer);
if (bDebug)
{
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "PlayerView eye direction: %.3f %.3f %.3f", eyeDir.x, eyeDir.y, eyeDir.z);
pRenderer->Draw2dLabel(5.0f, y+=step, 1.5f, color, false, "PlayerView Target: %.3f %.3f %.3f", hit.pt.x, hit.pt.y, hit.pt.z);
pRenderer->GetIRenderAuxGeom()->DrawCone(m_destination, Vec3(0,0,-1), 2.5f, 7.f, ColorB(255,0,0,255));
}
}
m_controlledTimer+=0.0f;
}
}
}
//This code is shared by both modes above (auto and controlled)
if(!m_destination.IsZero())
{
pe_status_dynamics status;
if (!GetEntity()->GetPhysics()->GetStatus(&status))
{
CryLogAlways("couldn't get physics status!");
return;
}
pe_status_pos pos;
if (!GetEntity()->GetPhysics()->GetStatus(&pos))
{
CryLogAlways("couldn't get physics pos!");
return;
}
float currentSpeed = status.v.len();
if (currentSpeed>0.001f)
{
Vec3 currentVel = status.v;
Vec3 currentPos = pos.pos;
Vec3 goalDir(ZERO);
assert(!_isnan(currentSpeed));
assert(!_isnan(currentVel.x) && !_isnan(currentVel.y) && !_isnan(currentVel.z));
//Just a security check
if((currentPos-m_destination).len2()<(m_detonationRadius*m_detonationRadius))
{
Explode(true, true, m_destination, -currentVel.normalized(), currentVel, m_targetId);
return;
}
goalDir = m_destination - currentPos;
goalDir.Normalize();
//Turn more slowly...
currentVel.Normalize();
if(bDebug)
{
pRenderer->Draw2dLabel(50,55,2.0f,color,false, " Destination: %.3f, %.3f, %.3f",m_destination.x,m_destination.y,m_destination.z);
pRenderer->Draw2dLabel(50,80,2.0f,color,false, " Current Dir: %.3f, %.3f, %.3f",currentVel.x,currentVel.y,currentVel.z);
pRenderer->Draw2dLabel(50,105,2.0f,color,false," Goal Dir: %.3f, %.3f, %.3f",goalDir.x,goalDir.y,goalDir.z);
}
float cosine = currentVel.Dot(goalDir);
cosine = CLAMP(cosine,-1.0f,1.0f);
float totalAngle = RAD2DEG(cry_acosf(cosine));
assert(totalAngle>=0);
if (cosine<0.99)
{
float maxAngle = m_turnSpeed*frameTime;
if (maxAngle>totalAngle)
maxAngle=totalAngle;
float t=(maxAngle/totalAngle)*m_lazyness;
assert(t>=0.0 && t<=1.0);
goalDir = Vec3::CreateSlerp(currentVel, goalDir, t);
goalDir.Normalize();
}
if(bDebug)
pRenderer->Draw2dLabel(50,180,2.0f,color,false,"Corrected Dir: %.3f, %.3f, %.3f",goalDir.x,goalDir.y,goalDir.z);
pe_action_set_velocity action;
action.v = goalDir * currentSpeed;
GetEntity()->GetPhysics()->Action(&action);
}
}
}
Solver::Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl,
const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), eyesRefGen(_eyesRefGen), loc(_loc),
ctrl(_ctrl), period(_period), Ts(_period/1000.0)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");
// remove constraints on the links: logging purpose
imu->setAllConstraints(false);
// block neck dofs
eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
// joints bounds alignment
alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// store neck pitch/roll/yaw bounds
neckPitchMin=(*chainNeck)[3].getMin();
neckPitchMax=(*chainNeck)[3].getMax();
neckRollMin =(*chainNeck)[4].getMin();
neckRollMax =(*chainNeck)[4].getMax();
neckYawMin =(*chainNeck)[5].getMin();
neckYawMax =(*chainNeck)[5].getMax();
neckPos.resize(3);
gazePos.resize(3);
updateAngles();
updateTorsoBlockedJoints(chainNeck,fbTorso);
chainNeck->setAng(3,fbHead[0]);
chainNeck->setAng(4,fbHead[1]);
chainNeck->setAng(5,fbHead[2]);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
updateNeckBlockedJoints(chainEyeL,fbHead);
updateNeckBlockedJoints(chainEyeR,fbHead);
Vector eyePos(2);
eyePos[0]=gazePos[0];
eyePos[1]=gazePos[1]+gazePos[2]/2.0;
chainEyeL->setAng(eyePos);
eyePos[1]=gazePos[1]-gazePos[2]/2.0;
chainEyeR->setAng(eyePos);
fbTorsoOld=fbTorso;
neckAngleUserTolerance=0.0;
}
void OpenGL2Renderer::updateCamera()
{
GLint err = glGetError();
btAssert(err==GL_NO_ERROR);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg
btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg
btQuaternion rot(m_cameraUp,razi);
btVector3 eyePos(0,0,0);
eyePos[m_forwardAxis] = -m_cameraDistance;
btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
if (forward.length2() < SIMD_EPSILON)
{
forward.setValue(1.f,0.f,0.f);
}
btVector3 right = m_cameraUp.cross(forward);
btQuaternion roll(right,-rele);
err = glGetError();
btAssert(err==GL_NO_ERROR);
eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_openglViewportWidth == 0 && m_openglViewportHeight == 0)
return;
btScalar aspect;
btVector3 extents;
aspect = m_openglViewportWidth / (btScalar)m_openglViewportHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
if (m_ortho)
{
// reset matrix
glLoadIdentity();
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
} else
{
glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2],
m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2],
m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
}
err = glGetError();
btAssert(err==GL_NO_ERROR);
}
