bool OgreMesh::intersectTri(const Ogre::Ray &ray, IntersectResult &rtn, Triangle*itr, bool isplane) {
std::pair<bool, Real> hit = isplane
? Ogre::Math::intersects(ray, Ogre::Plane(itr->v1.coord, itr->v2.coord,itr->v3.coord))
: Ogre::Math::intersects(ray, itr->v1.coord, itr->v2.coord,itr->v3.coord, true, false);
rtn.u = 0;
rtn.v = 0;
if (hit.first && hit.second < rtn.distance) {
rtn.intersected = hit.first;
rtn.distance = hit.second;
Ogre::Vector3 nml=(itr->v1.coord-itr->v2.coord).
crossProduct(itr->v3.coord-itr->v2.coord);
rtn.normal.x=nml.x;
rtn.normal.y=nml.y;
rtn.normal.z=nml.z;
rtn.tri = *itr;
Ogre::Vector3 intersect = ray.getPoint(hit.second) - rtn.tri.v2.coord;
Ogre::Vector3 aVec = (rtn.tri.v1.coord - rtn.tri.v2.coord);
Ogre::Vector3 bVec = (rtn.tri.v3.coord - rtn.tri.v2.coord);
if (aVec.length() > 1.0e-10 && bVec.length() > 1.0e-10) {
rtn.u = rtn.tri.v2.u + (rtn.tri.v1.u - rtn.tri.v2.u)*cos(aVec.angleBetween(intersect).valueRadians())*intersect.length()/aVec.length();
rtn.v = rtn.tri.v2.v + (rtn.tri.v3.v - rtn.tri.v2.v)*cos(bVec.angleBetween(intersect).valueRadians())*intersect.length()/bVec.length();
}
}
return rtn.intersected;
}
void EnemyController::approach(void)
{
EnemyPlane* enemy = static_cast<EnemyPlane*>(IDManager::getPointer(_enemyName, ACTOR));
Ogre::Vector3 temp = FCKnowledge::getSingleton().getPlayerPosition() - enemy->getPosition();
Ogre::Vector3 direction = temp * enemy->getAxis();
//std::cout<<direction.x<<" "<<direction.y<<" "<<direction.z<<std::endl;
if(direction.angleBetween(Ogre::Vector3::NEGATIVE_UNIT_Z) >= Ogre::Radian(Ogre::Degree(1)))
{
Ogre::Quaternion test = direction.getRotationTo(Ogre::Vector3::NEGATIVE_UNIT_Z);
Ogre::Degree angle = enemy->getRotateLimit();
double yawNum = test.getYaw().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
yawNum = Ogre::Math::Clamp(yawNum, -1.0, 1.0);
enemy->yaw(yawNum);
double pitchNum = test.getPitch().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
pitchNum = Ogre::Math::Clamp(pitchNum, -1.0, 1.0);
enemy->pitch(pitchNum);
double rollNum = test.getRoll().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
rollNum = Ogre::Math::Clamp(rollNum, -1.0, 1.0);
enemy->roll(rollNum);
}
else
{
enemy->yaw(0);
enemy->pitch(0);
enemy->roll(0);
}
}
//-----------------------------------------------------------------------
bool Light::isInLightRange(const Ogre::Sphere& container) const
{
bool isIntersect = true;
//directional light always intersects (check only spotlight and point)
if (mLightType != LT_DIRECTIONAL)
{
//Check that the sphere is within the sphere of the light
isIntersect = container.intersects(Sphere(mDerivedPosition, mRange));
//If this is a spotlight, check that the sphere is within the cone of the spot light
if ((isIntersect) && (mLightType == LT_SPOTLIGHT))
{
//check first check of the sphere surrounds the position of the light
//(this covers the case where the center of the sphere is behind the position of the light
// something which is not covered in the next test).
isIntersect = container.intersects(mDerivedPosition);
//if not test cones
if (!isIntersect)
{
//Calculate the cone that exists between the sphere and the center position of the light
Ogre::Vector3 lightSphereConeDirection = container.getCenter() - mDerivedPosition;
Ogre::Radian halfLightSphereConeAngle = Math::ASin(container.getRadius() / lightSphereConeDirection.length());
//Check that the light cone and the light-position-to-sphere cone intersect)
Radian angleBetweenConeDirections = lightSphereConeDirection.angleBetween(mDerivedDirection);
isIntersect = angleBetweenConeDirections <= halfLightSphereConeAngle + mSpotOuter * 0.5;
}
}
}
return isIntersect;
}
void PlayerCharacter::updateCharDirLooking(Ogre::Vector3* aimPoint, Ogre::Vector3* moveDir, unsigned long timeSinceLastFrame)
{
// Get directions
Ogre::Vector3 nodeDirection = this->node->getOrientation() * Vector3::UNIT_X;
Ogre::Vector3 aimDirection = *aimPoint - this->node->getPosition();
Ogre::Vector3 aimDirectionY = *aimPoint - this->animation->getRightShoulderPosition();
nodeDirection.y = 0;
aimDirection.y = 0;
nodeDirection.normalise();
aimDirection.normalise();
aimDirectionY.normalise();
// Horizontal angle
Ogre::Radian angleHorizontal = nodeDirection.angleBetween(aimDirection);
// Is the target on the left or the right side of the direction vector?
// see http://www.c-plusplus.de/forum/viewtopic-var-t-is-266934.html and http://www.mikrocontroller.net/topic/105993 (German)
Vector3 A = node->getPosition(); // Stützvektor
Vector3 B = nodeDirection + node->getPosition(); // Zielpunkt Richtungsvektor (Node Richtung)
Vector3 C = (*aimPoint); // Gefragter Punkt
Vector3 R = B-A; // Richtungsvektor
if((R.z * (C.x-A.x) - R.x * (C.z - A.z)) > 0)
angleHorizontal = -angleHorizontal; // Make angle negative if on left side
angleHorizontal += Ogre::Radian(Ogre::Degree(180)); // From -180° to 180° becomes 0° to 360° (where 180° is forwards)
// Vertical angle
Ogre::Radian angleVertical = aimDirection.angleBetween(aimDirectionY);
if(aimDirectionY.y > 0.f)
angleVertical = -angleVertical; // Angle negative when looking up
angleVertical += Ogre::Radian(Ogre::Degree(90)); // From -90° to 90° becomes 0° to 180° (where 0° is up and 180° is down)
// Use theses angles
this->animation->updateAimRotation(angleHorizontal, angleVertical, timeSinceLastFrame); // For Animation
this->animation->setDirectionVector("nodeDirection", nodeDirection); // For AnimationGUI...
this->animation->setDirectionVector("aimDirHorizontal", aimDirection);
this->animation->setDirectionVector("aimDirVertical", aimDirectionY);
}
void LaserScanner::slotDoScan()
{
// Theoretically, there is no reason to emit complete scans instead of "streaming"
// single world coordinates. I suspect that fewer callbacks, fewer and bigger
// network-packets will be more performant, though. It might actually be even
// smarter to emit every n CoordinateGps, when n marshalled CoordinateGps reach
// the interface's MTU. But thats for later, we're still simulating right now...
if(mScannerOrientationPrevious == mScannerOrientation && mScannerPositionPrevious == mScannerPosition)
{
// No need to scan, we'll get the same results as previously. IF THE REST OF
// THE WORLD IS STATIC, that is. Sleep for one scan, then try again.
// qDebug() << "LaserScanner::slotDoScan(): vehicle hasn't moved, sleeping scan";
usleep(1000000 / (mSpeed / 60) * (1.0 / mTimeFactor));
return;
}
// emit bottomBeamLength four times a second. mSpeed is rounds per minute
mBottomBeamClockDivisor++;
mBottomBeamClockDivisor %= mSpeed < 120.0f ? 1 : ((quint16)mSpeed)/240;
const long long realTimeBetweenRaysUS = (1000000.0 / 6.0 / mSpeed * mAngleStep) * (1.0 / mTimeFactor);
struct timeval timeStart;
gettimeofday(&timeStart, NULL);
struct timeval timeNow;
int numberOfRays = 0;
std::normal_distribution<> noiseDistYaw(0, 1.0);
Ogre::Quaternion quatNoiseYaw(Ogre::Degree(noiseDistYaw(*mRandomMersenneTwister)), Ogre::Vector3::UNIT_Y);
std::normal_distribution<> noiseDistPitch(0, 0.5);
Ogre::Quaternion quatNoisePitch(Ogre::Degree(noiseDistPitch(*mRandomMersenneTwister)), Ogre::Vector3::UNIT_X);
std::normal_distribution<> noiseDistRoll(0, 0.5);
Ogre::Quaternion quatNoiseRoll(Ogre::Degree(noiseDistRoll(*mRandomMersenneTwister)), Ogre::Vector3::UNIT_Z);
while(mCurrentScanAngle <= mAngleStop)
{
numberOfRays++;
mNumberOfRaySceneQueries++;
// Build a quaternion that represents the laserbeam's current rotation
Ogre::Quaternion quatBeamRotation(Ogre::Degree(mCurrentScanAngle), Ogre::Vector3::UNIT_Y);
QMutexLocker locker(&mMutex);
mLaserBeam.setOrigin(mScannerPosition);
mLaserBeam.setDirection(mScannerOrientation * (quatNoiseYaw * quatNoisePitch * quatNoiseRoll) * quatBeamRotation * Ogre::Vector3::NEGATIVE_UNIT_Z);
locker.unlock();
float closestDistanceToEntity = -1.0f;
// We once used a rayscenequery from the scenemanager for querying meshes, but that was slow and not thread-safe.
// Instead, each laserscanner now keeps a copy of all meshes' information. Meshes are read once in the c'tor from
// OgreWidget::mEntities and then a pointer is kept. When scanning, a corresponding MeshInformation-Pointer is
// filled with information for that mesh once and used subsequently. No more RaySceneQuery needed. HAH!
QMapIterator<Ogre::Entity*, OgreWidget::MeshInformation*> i(mOgreWidget->mEntities);
while(i.hasNext())
{
i.next();
OgreWidget::MeshInformation* currentMeshInformation = i.value();
std::pair<bool,Ogre::Real> result = mLaserBeam.intersects(currentMeshInformation->aabb);
if(result.first)
{
// stop checking if we have found a raycast hit that is closer than all remaining entities
if(closestDistanceToEntity >= 0.0f && closestDistanceToEntity < result.second) break;
// also stop checking if the distance to this object's bbox is out of this lidar's range
if(mRange < result.second) break;
// test for hitting individual triangles on the mesh
for(size_t i = 0; i < currentMeshInformation->index_count; i += 3)
{
// check for a hit against this triangle
const std::pair<bool, Ogre::Real> hit = Ogre::Math::intersects(
mLaserBeam,
currentMeshInformation->vertices[currentMeshInformation->indices[i]],
currentMeshInformation->vertices[currentMeshInformation->indices[i+1]],
currentMeshInformation->vertices[currentMeshInformation->indices[i+2]],
true, // positiveSide
false // negativeSide
);
// if it was a hit, check if its the closest
if(hit.first)
{
if((closestDistanceToEntity < 0.0f) || (hit.second < closestDistanceToEntity))
{
// this is the closest so far, save it off
closestDistanceToEntity = hit.second;
}
}
}
}
}
// Do a RSQ against the terrain.
// http://www.ogre3d.org/docs/api/html/classOgre_1_1TerrainGroup.html says about rayIntersects:
// This can be called from any thread as long as no parallel write to the terrain data occurs.
Ogre::TerrainGroup::RayResult rayResultTerrain = mOgreWidget->mTerrainGroup->rayIntersects(mLaserBeam);
float distanceValidEntity = 99999.9;
float distanceValidTerrain = 99999.9;
if(closestDistanceToEntity > 0)
distanceValidEntity = closestDistanceToEntity;
if(rayResultTerrain.hit)
distanceValidTerrain = mLaserBeam.getOrigin().distance(rayResultTerrain.position);
float distanceFinal = std::min(distanceValidEntity, distanceValidTerrain);
// Create gaussian noise around 0, standard deviation increases with distance
std::normal_distribution<> d(0, 0.005 * qBound(1.0f, distanceFinal, 10.0f));
distanceFinal += d(*mRandomMersenneTwister);
if(distanceFinal < mRange)
{
const Ogre::Vector3 point = mLaserBeam.getPoint(distanceFinal);
if(mBottomBeamClockDivisor == 0)
{
static Ogre::Vector3 vectorDownWorld = Ogre::Vector3::NEGATIVE_UNIT_Y;
if(fabs(vectorDownWorld.angleBetween(mLaserBeam.getDirection()).valueDegrees()) < 0.2f)
{
// qDebug() << t() << "LaserScanner::slotDoScan(): currentScanAngle" << mCurrentScanAngle << "emitting heightOverGround, angle is" << vectorDownWorld.angleBetween(mLaserBeam.getDirection()).valueDegrees();
emit heightOverGround(distanceFinal);
}
}
mRegisteredPoints << QVector4D(point.x, point.y, point.z, pow(distanceFinal, 2.0));
}
// Increase mCurrentScanAngle by mAngleStep for the next laserBeam
mCurrentScanAngle += mAngleStep;
gettimeofday(&timeNow, NULL);
const long long scanTimeElapsed = (timeNow.tv_sec - timeStart.tv_sec) * 1000000 + (timeNow.tv_usec - timeStart.tv_usec);
const long long scanTimeAtNextRay = realTimeBetweenRaysUS * (numberOfRays+1);
// if(mNumberOfRaySceneQueries%10000 == 0) qDebug() << "Number of RSQs:" << mNumberOfRaySceneQueries;
usleep(std::max(0, (int)(scanTimeAtNextRay - scanTimeElapsed)));
}
gettimeofday(&timeNow, NULL);
// const long long timeDiff = (timeNow.tv_sec - timeStart.tv_sec) * 1000000 + (timeNow.tv_usec - timeStart.tv_usec);
// qDebug() << "LaserScanner::slotDoScan(): took" << timeDiff << "us, should have been" << (long long)(realTimeBetweenRaysUS * ((mAngleStop - mAngleStart)/mAngleStep));
if(mRegisteredPoints.size())
{
emit scanFinished(mSimulator->getSimulationTime());
// qDebug() << "LaserScanner::doScan(): emitting" << scanContainer.size() << "points.";
mScannerPositionQt = QVector3D(mScannerPosition.x, mScannerPosition.y, mScannerPosition.z);
emit newLidarPoints(mRegisteredPoints, mScannerPositionQt);
}
// Set mCurrentScanAngle for the next scan to mAngleStart
mCurrentScanAngle = mAngleStart;
mScannerOrientationPrevious = mScannerOrientation;
mScannerPositionPrevious = mScannerPosition;
mRegisteredPoints.clear();
// Sleep for degreesToNextScan = 360.0 - mAngleStop + mAngleStart
gettimeofday(&timeNow, NULL);
const long long scanTimeElapsed = (timeNow.tv_sec - timeStart.tv_sec) * 1000000 + (timeNow.tv_usec - timeStart.tv_usec);
const long long timeRest = (1000000/(mSpeed/60)) * (1.0 / mTimeFactor) - scanTimeElapsed;
// qDebug() << "LaserScanner::slotDoScan(): emitted results, resting" << timeRest << "us after scan.";
usleep(std::max(0, (int)timeRest));
}
void Turret::Update()
{
bool PlayerInRoom = false;
DynamicObject::Update();
hkpWorldRayCastOutput OutPut;
hkpWorldRayCastInput Ray;
Player* theplayer = 0;
Ogre::Vector3 RayDirection = (mPlayerPos - mPosition).normalisedCopy();
Ray.m_from = hkVector4(mPosition.x + (RayDirection.x * 25)
,mPosition.y + (RayDirection.y * 25)
,mPosition.z + (RayDirection.z * 25));
Ray.m_to = hkVector4(mPlayerPos.x, mPlayerPos.y, mPlayerPos.z);
mPhysicsManager->GetPhysicsWorld()->castRay(Ray,OutPut);
if(OutPut.hasHit())
{
const hkpCollidable* col = OutPut.m_rootCollidable;
hkpRigidBody* body = hkpGetRigidBody(col);
theplayer = dynamic_cast<Player*> ((BaseObject *)body->getUserData());
if(theplayer != 0)
{
PlayerInRoom = true;
}
else
{
mPlayerInSight = false;
mKillTimer = 0;
}
}
Ogre::Vector3 NewDir = Ogre::Vector3(RayDirection.x,0,RayDirection.z);
NewDir.normalise();
Ogre::Radian angle = NewDir.angleBetween(ObjectNode->getOrientation() * Ogre::Vector3::UNIT_X);
if(PlayerInRoom || !mPlayerInSight)
{
if(angle.valueDegrees() < 20 || angle.valueDegrees() > -20)
{
mPlayerInSight = true;
}
}
if(mPlayerInSight)
{
mRotateValue += angle.valueRadians();
Body->setRotation(hkQuaternion(hkVector4(0,1,0),mRotateValue));
mKillTimer++;
if(mKillTimer > 800)
{
mShutdown = true;
//theplayer->OnDeath();
}
}
else
{
if(mRotateValue < -2)
{
mChangeInRotation = 0.001;
}
else if (mRotateValue > 2)
{
mChangeInRotation = -0.001;
}
mRotateValue += mChangeInRotation;
Body->setRotation(hkQuaternion(hkVector4(0,1,0),mRotateValue));
}
}
NavigationPath* NavigationMesh::straightenPath( NavigationPath* path, Ogre::Radian maxTurnAngle, Ogre::Real pathWidth )
{
NavigationPath* straightenedPath = new NavigationPath;
NavigationPath::iterator startPoint;
NavigationPath::iterator endPoint;
Ogre::Vector3 rightAngleOffset;
Ogre::Vector3 originalDirection;
Ogre::Vector3 currentDirection;
Ogre::Real slope;
startPoint = path->begin();
straightenedPath->push_back( *startPoint );
while( startPoint != path->end() )
{
endPoint = startPoint;
endPoint++;
originalDirection = *endPoint - *startPoint;
originalDirection.normalise();
currentDirection = originalDirection;
while( endPoint != path->end() )
{
slope = currentDirection.y;
currentDirection = *endPoint - *startPoint;
currentDirection.normalise();
rightAngleOffset.x = currentDirection.z;
rightAngleOffset.y = 0;
rightAngleOffset.z = -currentDirection.x;
rightAngleOffset *= pathWidth;
// Test this line: startPoint -> endPoint
// Do we turn to much in one spot?
if( originalDirection.angleBetween( currentDirection ) > maxTurnAngle )
break;
// Does the path slope change?
if( Ogre::Math::Abs(slope - currentDirection.y) > 0.01 )
break;
NavigationCell* prevCell;
NavigationCell* testCell;
NavigationCell* nextCell;
Ogre::Vector3 sideStartPoint;
Ogre::Vector3 sideEndPoint;
NavigationCell::LINE_CLASSIFICATION ret;
sideStartPoint = *startPoint + rightAngleOffset;
sideEndPoint = *endPoint + rightAngleOffset;
prevCell = getExactCellContainingPoint( sideStartPoint );
if( prevCell )
{
ret = prevCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, testCell );
while( ret == NavigationCell::LINE_EXITS && testCell != 0 )
{
ret = testCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, nextCell );
prevCell = testCell;
testCell = nextCell;
}
if( testCell == 0 ) // Line leaves the navigation mesh.
break;
}
sideStartPoint = *startPoint - rightAngleOffset;
sideEndPoint = *endPoint - rightAngleOffset;
prevCell = getExactCellContainingPoint( sideStartPoint );
if( prevCell )
{
ret = prevCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, testCell );
while( ret == NavigationCell::LINE_EXITS && testCell != 0 )
{
ret = testCell->classifyLine2D( sideStartPoint, sideEndPoint, prevCell, nextCell );
prevCell = testCell;
testCell = nextCell;
}
if( testCell == 0 ) // Line leaves the navigation mesh.
break;
}
// We survived all the tests. Move to next the point.
endPoint++;
}
// Either we have got to the end of the points, or a point failed a test.
if( startPoint >= (endPoint - 1))
{
// Point directly after startPoint failed a test... all we can do is just move along.
startPoint++;
}
else
{
// We can skip some points.
startPoint = endPoint - 1;
}
if( startPoint != path->end() )
straightenedPath->push_back( *startPoint );
}
return straightenedPath;
}
tdt::real PhysicsHelper::get_angle(Ogre::Vector3 v1, Ogre::Vector3 v2)
{
return v1.angleBetween(v2).valueRadians();
}
