//----------------------------------------------------------------------------
void DirectionalLight::ComputeSpecular (const Matrix3f& rkWorldRotate,
const Vector3f&, float, const Vector3f* akVertex,
const Vector3f* akNormal, int iQuantity, const bool* abVisible,
const Vector3f& rkCameraModelLocation, ColorRGB* akSpecular)
{
// transform light direction to model space of old mesh
Vector3f kModelDir = m_kDirection*rkWorldRotate;
// adjust specular color by light intensity
ColorRGB kAdjSpecular = m_fIntensity*m_kSpecular;
for (int i = 0; i < iQuantity; i++)
{
if ( abVisible[i] )
{
float fDot = kModelDir.Dot(akNormal[i]);
Vector3f kReflect = kModelDir - (2.0f*fDot)*akNormal[i];
Vector3f kViewDir = rkCameraModelLocation - akVertex[i];
fDot = kViewDir.Dot(kReflect);
if ( fDot > 0.0f )
{
float fCoeff = fDot*fDot/kViewDir.Dot(kViewDir);
akSpecular[i] += fCoeff*kAdjSpecular;
}
}
}
}
bool Camera3d::PointInFrustum(const Vector3f &point)
{
float pcz,pcx,pcy,h;
// compute vector from camera position to p
Vector3f v = point - _eye;
// compute and test the Z coordinate
pcz = v.Dot(-_frustumZ);
if (pcz > _far || pcz < _near)
return false;
// compute and test the Y coordinate
pcy = v.Dot(_frustumY);
h = pcz * _frustumTanFOV;
if (pcy > h || pcy < -h)
return false;
// compute and test the X coordinate
pcx = v.Dot(_frustumX);
h = h * _ratioAspect;
if (pcx > h || pcx < -h)
return false;
return true;
}
// intersect3D_SegmentPlane(): intersect a segment and a plane
// Input: a_Ray = a segment, and a_Plane = a plane = {Point V0; Vector n;}
// Output: *I0 = the intersect point (when it exists)
// Return: 0 = disjoint (no intersection)
// 1 = intersection in the unique point *I0
// 2 = the segment lies in the plane
int cTracer::intersect3D_SegmentPlane( const Vector3f & a_Origin, const Vector3f & a_End, const Vector3f & a_PlanePos, const Vector3f & a_PlaneNormal)
{
Vector3f u = a_End - a_Origin; // a_Ray.P1 - S.P0;
Vector3f w = a_Origin - a_PlanePos; // S.P0 - Pn.V0;
float D = a_PlaneNormal.Dot( u); // dot(Pn.n, u);
float N = -(a_PlaneNormal.Dot( w)); // -dot(a_Plane.n, w);
const float EPSILON = 0.0001f;
if (fabs(D) < EPSILON)
{
// segment is parallel to plane
if (N == 0)
{
// segment lies in plane
return 2;
}
return 0; // no intersection
}
// they are not parallel
// compute intersect param
float sI = N / D;
if (sI < 0 || sI > 1)
{
return 0; // no intersection
}
// Vector3f I ( a_Ray->GetOrigin() + sI * u);// S.P0 + sI * u; // compute segment intersect point
RealHit = a_Origin + u * sI;
return 1;
}
bool Intersect::RaySphere (const Vector3f& origin, const Vector3f& dir, const Vector3f& center, float radius)
{
Vector3f diff (origin - center);
float a = diff.Dot(dir);
float b = (a * a) + (radius * radius) - diff.Dot();
return Float::IsPositive(b);
// Distance if 'b' is positive: (-a - Float::Sqrt(b));
}
//----------------------------------------------------------------------------
// Clip C+t*V with [t0,t1] against the plane Dot(N,X-P) = 0, discarding that
// portion of the interval on the side of the plane to which N is directed.
// The return value is 'true' when a nonempty interval exists after clipping.
//----------------------------------------------------------------------------
static bool ClipAgainstPlane (const Vector3f& C, const Vector3f& V,
const Vector3f& N, const Vector3f& P, float& t0, float& t1)
{
// Define f(t) = Dot(N,C+t*V-P)
// = Dot(N,C-P) + t*Dot(N,V)
// = a0 + t*a1
// Evaluate at the endpoints of the time interval.
float a0 = N.Dot(C - P);
float a1 = N.Dot(V);
float f0 = a0 + t0*a1;
float f1 = a0 + t1*a1;
// Clip [t0,t1] against the plane. There are nine cases to consider,
// depending on the signs of f0 and f1.
if (f0 > 0.0f)
{
if (f1 > 0.0f)
{
// The segment is strictly outside the plane.
return false;
}
else if (f1 < 0.0f)
{
// The segment intersects the plane at an edge-interior point.
// T = -a0/a1 is the time of intersection, so discard [t0,T].
t0 = -a0/a1;
}
else // f1 == 0.0f
{
// The segment is outside the plane but touches at the
// t1-endpoint, so discard [t0,t1] (degenerate to a point).
t0 = t1;
}
}
else if (f0 < 0.0f)
{
if (f1 > 0.0f)
{
// The segment intersects the plane at an edge-interior point.
// T = -a0/a1 is the time of intersection, so discard [T,t1].
t1 = -a0/a1;
}
}
else // f0 == 0.0f
{
if (f1 > 0.0f)
{
// The segment is outside the plane but touches at the
// t0-endpoint, so discard [t0,t1] (degenerate to a point).
t1 = t0;
}
}
return true;
}
Matrix4f Matrix4f::LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up)
{
Vector3f z = (at - eye).Normalized(); // Forward
Vector3f x = up.Cross(z).Normalized(); // Right
Vector3f y = z.Cross(x);
Matrix4f m(x.x, x.y, x.z, -(x.Dot(eye)),
y.x, y.y, y.z, -(y.Dot(eye)),
z.x, z.y, z.z, -(z.Dot(eye)),
0, 0, 0, 1 );
return m;
}
void CRigidBody::HandleSphereToSphereCollision(CCollisionMessage* ColMsg)
{
CVehicle* Car = (CVehicle*)ColMsg->GetEntity();
Vector3f CenterToCenter = *ColMsg->GetCenterToCenter();
// set translate
m_vReflection = CenterToCenter*0.05f;
m_vPosition = m_translate;
m_vDirectionWhenDisturbed = m_vReflection;
// set rotation
if (CenterToCenter.Length() == 0.0f) CenterToCenter = Vector3f(1.0f, 0.0f, 0.0f);
float theta1 = (CenterToCenter.Dot(Car->GetVehicleHeadingWC()))/(CenterToCenter.Length()*Car->GetVehicleHeadingWC().Length());
Vector3f CP = CenterToCenter.Cross(Vector3f(0.0f, 1.0f, 0.0f));
float theta2 = (CP.Dot(Car->GetVehicleHeadingWC()))/(CP.Length()*Car->GetVehicleHeadingWC().Length());
// CLog::GetLog().Write(LOG_DEBUGOVERLAY, 120, "theta1 = %f", theta1);
// CLog::GetLog().Write(LOG_DEBUGOVERLAY, 121, "theta2 = %f", theta2);
float spin;
if (0.0f < theta1 && theta1 < 1.0f) {
// LOWER-LEFT QUADRANT
if (0.0f < theta2 && theta2 < 1.0f) {
spin = theta2;
}
// LOWER-RIGHT QUADRANT
else {
spin = theta2;
}
}
else {
// UPPER-LEFT QUADRANT
if (0.0f < theta2 && theta2 < 1.0f) {
spin = -theta2;
}
// UPPER-RIGHT QUADRANT
else {
spin = -theta2;
}
}
spin *= RIGIDBODY_SPIN_FACTOR;
m_vRotation = Vector3f(0.0f, spin, 0.0f);
// actually set it!
Car->SetVehicleVelocityLC(0.0f);
Car->SetVehiclePositionLC(Vector3f(m_vPosition.X(), m_vPosition.Z(), m_vPosition.Y()));
disturbed = true;
}
bool RtIntersect::RayTriangle( const Vector3f & rayStart, const Vector3f & rayDir,
const Vector3f & v0, const Vector3f & v1, const Vector3f & v2,
float & t0, float & u, float & v )
{
assert( rayDir.IsNormalized() );
const Vector3f edge1 = v1 - v0;
const Vector3f edge2 = v2 - v0;
const Vector3f tv = rayStart - v0;
const Vector3f pv = rayDir.Cross( edge2 );
const Vector3f qv = tv.Cross( edge1 );
const float det = edge1.Dot( pv );
// If the determinant is negative then the triangle is backfacing.
if ( det <= 0.0f )
{
return false;
}
// This code has been modified to only perform a floating-point
// division if the ray actually hits the triangle. If back facing
// triangles are not culled then the sign of 's' and 't' need to
// be flipped. This can be accomplished by multiplying the values
// with the determinant instead of the reciprocal determinant.
const float s = tv.Dot( pv );
const float t = rayDir.Dot( qv );
if ( s >= 0.0f && s <= det )
{
if ( t >= 0.0f && s + t <= det )
{
// If the determinant is almost zero then the ray lies in the triangle plane.
// This comparison is done last because it is usually rare for
// the ray to lay in the triangle plane.
if ( fabsf( det ) > Math<float>::SmallestNonDenormal )
{
const float rcpDet = 1.0f / det;
t0 = edge2.Dot( qv ) * rcpDet;
u = s * rcpDet;
v = t * rcpDet;
return true;
}
}
}
return false;
}
//--------------------------------------------------------------------------------
Plane3f::Plane3f(const Vector3f& normal, const Vector3f& v)
{
auto d = -normal.Dot(v);
m_fComponents[0] = normal.x;
m_fComponents[1] = normal.y;
m_fComponents[2] = normal.z;
m_fComponents[3] = d;
}
//-----------------------------------------------------------
float LOCAL_DistToPlane(Vector3f * p, Vector3f * p0, Vector3f * p1, Vector3f * p2)
{
Vector3f q0 = *p1 - *p0;
Vector3f q1 = *p2 - *p0;
Vector3f c = (q0.Cross(q1)).Normalized();
Vector3f q = *p - *p0;
return(c.Dot(q));
}
void ModelViewer::OnDraw()
{
// Animate the model if requested
if (mAnimate)
{
ulong delta = Time::GetDeltaMS();
if (delta > 0)
{
const Vector3f axis (0.0f, 0.0f, 1.0f);
Quaternion rot (axis, -Float::Sin(0.0005f * delta));
mStage->SetRelativeRotation( rot * mStage->GetRelativeRotation() );
}
}
// Fade out the status bar
if (mSbHighlight != 0 && mTimestamp != 0.0f)
{
float current = Time::GetTime();
float factor = (current - mTimestamp) / 2.0f;
factor = Float::Clamp(factor, 0.0f, 1.0f);
mSbHighlight->SetAlpha( (1.0f - factor) * 0.85f );
if (factor == 1.0f) mTimestamp = 0.0f;
}
// If there was a request to reset the viewpoint, now should be a good time as we know what's visible
if (mResetCamera > 0 && mResetCamera++ > 1)
{
mResetCamera = 0;
const Bounds& bounds = mInst->GetAbsoluteBounds();
if (bounds.IsValid())
{
const Vector3f& center (bounds.GetCenter());
Vector3f total = bounds.GetMax() - bounds.GetMin();
total.Normalize();
// Default positions should always be at a downward angle
Vector3f down (0.175f, -0.5f, -1.0f );
Vector3f straight ( 0.35f, -1.0f, -0.35f);
down.Normalize();
straight.Normalize();
// How much the camera will be angled downward depends on the dot product
float dot = total.Dot( Vector3f(0.0f, 0.0f, 1.0f) );
Vector3f dir ( Interpolation::Linear(down, straight, dot) );
// Distance should be far enough away to view the entire model
float distance = Float::Clamp(bounds.GetRadius() * 1.2f, 1.0f, mCam->GetDolly().z);
// Animate the camera to the calculated position
mCam->Stop();
mCam->AnimateTo(center, dir, distance, 0.5f);
}
}
}
void Projectile::Update(float elapsedTime)
{
// Test si les conditions de fin du projectile sont vraies
if (m_timeToLive <= 0 || !m_shot || elapsedTime == 0)
return;
// Test si atteint la cible
if( abs(m_destination.x - m_pos.x) < m_collisionRadius.x
&& abs(m_destination.y - m_pos.y) < m_collisionRadius.y
&& abs(m_destination.z - m_pos.z) < m_collisionRadius.z) {
Hit();
return;
}
Vector3f speed = m_rot * m_speed;
speed = speed * m_speed.Lenght();
// Met a jour la valeur de la vitesse en
// fonction de l'acceleration et du temps
m_speed += m_acceleration * elapsedTime;
m_timeToLive -= elapsedTime;
// distance entre le projectile et sa destination
// chemin le plus court
Vector3f distance;
distance.x = m_pos.x - m_destination.x;
distance.y = m_pos.y - m_destination.y;
distance.z = m_pos.z - m_destination.z;
// calculer l'angle entre les 2 vecteurs
// fix imprecision float
float n = distance.Dot(speed) / (distance.Lenght() * speed.Lenght());
if (n > 1)
n = 1;
else if (n < -1)
n = -1;
float angleA = acos(n);
std::cout << angleA << std::endl;
Vector3f axis = distance.Cross(speed);
axis.Normalize();
// rotation autour de laxe
float rotation;
if (abs(angleA) >= m_maxRot && abs(angleA) < PII - m_maxRot) {
rotation = (angleA > 0) ? -m_maxRot : m_maxRot;
rotation *= elapsedTime;
}
else
rotation = angleA - PII;
Quaternion q;
q.FromAxis(rotation, axis);
q.Normalise();
m_rot = q * m_rot;
m_rot.Normalise();
// calcul la nouvelle position
m_pos += speed * elapsedTime;
}
// -1 to 1 range on panelMatrix, returns -2,-2 if looking away from the panel
Vector2f GazeCoordinatesOnPanel( const Matrix4f & viewMatrix, const Matrix4f panelMatrix, const bool floatingPanel )
{
// project along -Z in the viewMatrix onto the Z = 0 plane of activityMatrix
const Vector3f viewForward = MatrixForward( viewMatrix ).Normalized();
Vector3f panelForward;
float approach;
if ( floatingPanel )
{
Matrix4f mat = panelMatrix;
mat.SetTranslation( Vector3f( 0.0f ) );
panelForward = mat.Transform( Vector3f( 0.0f, 0.0f, 1.0f ) ).Normalized();
approach = viewForward.Dot( panelForward );
if ( approach >= -0.1 )
{ // looking away
return Vector2f( -2.0f, -2.0f );
}
}
else
{
panelForward = -MatrixForward( panelMatrix ).Normalized();
approach = viewForward.Dot( panelForward );
if ( approach <= 0.1 )
{ // looking away
return Vector2f( -2.0f, -2.0f );
}
}
const Matrix4f panelInvert = panelMatrix.Inverted();
const Matrix4f viewInvert = viewMatrix.Inverted();
const Vector3f viewOrigin = viewInvert.Transform( Vector3f( 0.0f ) );
const Vector3f panelOrigin = MatrixOrigin( panelMatrix );
// Should we disallow using panels from behind?
const float d = panelOrigin.Dot( panelForward );
const float t = -( viewOrigin.Dot( panelForward ) + d ) / approach;
const Vector3f impact = viewOrigin + viewForward * t;
const Vector3f localCoordinate = panelInvert.Transform( impact );
return Vector2f( localCoordinate.x, localCoordinate.y );
}
//----------------------------------------------------------------------------
void BouncingTetrahedra::ComputeImpulseMagnitude (float* preRelVelocities,
float* impulseMagnitudes)
{
// The coefficient of restitution.
float restitution = 0.8f;
float temp = 20.0f*NUM_TETRA;
if (mTotalKE < temp)
{
restitution *= 0.5f*mTotalKE/temp;
}
float coeff = -(1.0f + restitution);
for (int i = 0; i < mNumContacts; ++i)
{
if (preRelVelocities[i] < 0.0f)
{
const Contact& contact = mContacts[i];
const RigidBodyf& bodyA = *contact.A;
const RigidBodyf& bodyB = *contact.B;
Vector3f velDiff = bodyA.GetLinearVelocity() -
bodyB.GetLinearVelocity();
Vector3f relA = contact.PA - bodyA.GetPosition();
Vector3f relB = contact.PB - bodyB.GetPosition();
Vector3f AxN = relA.Cross(contact.N);
Vector3f BxN = relB.Cross(contact.N);
Vector3f JInvAxN = bodyA.GetWorldInverseInertia()*AxN;
Vector3f JInvBxN = bodyB.GetWorldInverseInertia()*BxN;
float numer = coeff*(contact.N.Dot(velDiff)
+ bodyA.GetAngularVelocity().Dot(AxN)
- bodyB.GetAngularVelocity().Dot(BxN));
float denom = bodyA.GetInverseMass() + bodyB.GetInverseMass()
+ AxN.Dot(JInvAxN) + BxN.Dot(JInvBxN);
impulseMagnitudes[i] = numer/denom;
}
else
{
impulseMagnitudes[i] = 0.0f;
}
}
}
Vector3f SlideMove(
const Vector3f & footPos,
const float eyeHeight,
const Vector3f & moveDirection,
const float moveDistance,
const CollisionModel & collisionModel,
const CollisionModel & groundCollisionModel
)
{
// Check for collisions at eye level to prevent slipping under walls.
Vector3f eyePos = footPos + UpVector * eyeHeight;
// Pop out of any collision models.
collisionModel.PopOut( eyePos );
{
Planef fowardCollisionPlane;
float forwardDistance = moveDistance;
if ( !collisionModel.TestRay( eyePos, moveDirection, forwardDistance, &fowardCollisionPlane ) )
{
// No collision, move the full distance.
eyePos += moveDirection * moveDistance;
}
else
{
// Move up to the point of collision.
eyePos += moveDirection * forwardDistance;
// Project the remaining movement onto the collision plane.
const float COLLISION_BOUNCE = 0.001f; // don't creep into the plane due to floating-point rounding
const float intoPlane = moveDirection.Dot( fowardCollisionPlane.N ) - COLLISION_BOUNCE;
const Vector3f slideDirection = ( moveDirection - fowardCollisionPlane.N * intoPlane );
// Try to finish the move by sliding along the collision plane.
float slideDistance = moveDistance;
collisionModel.TestRay( eyePos - UpVector * RailHeight, slideDirection, slideDistance, NULL );
eyePos += slideDirection * slideDistance;
}
}
if ( groundCollisionModel.Polytopes.GetSizeI() != 0 )
{
// Check for collisions at foot level, which allows following terrain.
float downDistance = 10.0f;
groundCollisionModel.TestRay( eyePos, - UpVector, downDistance, NULL );
// Maintain the minimum camera height.
if ( eyeHeight - downDistance < 1.0f )
{
eyePos += UpVector * ( eyeHeight - downDistance );
}
}
return eyePos - UpVector * eyeHeight;
}
// Compute a rotation required to transform "estimated" into "measured"
// Returns an approximation of the goal rotation in the Simultaneous Orthogonal Rotations Angle representation
// (vector direction is the axis of rotation, norm is the angle)
Vector3f SensorFusion_ComputeCorrection(Vector3f measured, Vector3f estimated)
{
measured.Normalize();
estimated.Normalize();
Vector3f correction = measured.Cross(estimated);
float cosError = measured.Dot(estimated);
// from the def. of cross product, correction.Length() = sin(error)
// therefore sin(error) * sqrt(2 / (1 + cos(error))) = 2 * sin(error / 2) ~= error in [-pi, pi]
// Mathf::Tolerance is used to avoid div by 0 if cos(error) = -1
return correction * sqrt(2 / (1 + cosError + Mathf::Tolerance));
}
// -1 to 1 range on screenMatrix, returns -2,-2 if looking away from the screen
Vector2f MoviePlayerView::GazeCoordinatesOnScreen( const Matrix4f & viewMatrix, const Matrix4f screenMatrix ) const
{
// project along -Z in the viewMatrix onto the Z = 0 plane of screenMatrix
const Vector3f viewForward = MatrixForward( viewMatrix ).Normalized();
Vector3f screenForward;
if ( Cinema.SceneMgr.FreeScreenActive )
{
// FIXME: free screen matrix is inverted compared to bounds screen matrix.
screenForward = -Vector3f( screenMatrix.M[0][2], screenMatrix.M[1][2], screenMatrix.M[2][2] ).Normalized();
}
else
{
screenForward = -MatrixForward( screenMatrix ).Normalized();
}
const float approach = viewForward.Dot( screenForward );
if ( approach <= 0.1f )
{
// looking away
return Vector2f( -2.0f, -2.0f );
}
const Matrix4f panelInvert = screenMatrix.Inverted();
const Matrix4f viewInvert = viewMatrix.Inverted();
const Vector3f viewOrigin = viewInvert.Transform( Vector3f( 0.0f ) );
const Vector3f panelOrigin = MatrixOrigin( screenMatrix );
// Should we disallow using panels from behind?
const float d = panelOrigin.Dot( screenForward );
const float t = -( viewOrigin.Dot( screenForward ) + d ) / approach;
const Vector3f impact = viewOrigin + viewForward * t;
const Vector3f localCoordinate = panelInvert.Transform( impact );
return Vector2f( localCoordinate.x, localCoordinate.y );
}
void BoundingSphere::Encapsulate(const Vector3f &v)
{
// TODO : check if this is correct
Vector3f diff = v - center;
float dist = diff.Dot(diff);
if (dist > radiusSq)
{
Vector3f diff2 = diff.Normalized() * radius;
Vector3f delta = 0.5f * (diff - diff2);
center += delta;
radius += delta.Length();
radiusSq = radius*radius;
}
}
//----------------------------------------------------------------------------------------------------
Matrix4f Matrix4f::LookAtLHMatrix( Vector3f& eye, Vector3f& at, Vector3f& up )
{
// This method is based on the method of the same name from the D3DX library.
Matrix4f ret;
Vector3f zaxis = at - eye;
zaxis.Normalize();
Vector3f xaxis = up.Cross( zaxis );
xaxis.Normalize();
Vector3f yaxis = zaxis.Cross( xaxis );
ret.m_afEntry[ 0] = xaxis.x;
ret.m_afEntry[ 1] = yaxis.x;
ret.m_afEntry[ 2] = zaxis.x;
ret.m_afEntry[ 3] = 0.0f;
ret.m_afEntry[ 4] = xaxis.y;
ret.m_afEntry[ 5] = yaxis.y;
ret.m_afEntry[ 6] = zaxis.y;
ret.m_afEntry[ 7] = 0.0f;
ret.m_afEntry[ 8] = xaxis.z;
ret.m_afEntry[ 9] = yaxis.z;
ret.m_afEntry[10] = zaxis.z;
ret.m_afEntry[11] = 0.0f;
ret.m_afEntry[12] = -(xaxis.Dot(eye));
ret.m_afEntry[13] = -(yaxis.Dot(eye));
ret.m_afEntry[14] = -(zaxis.Dot(eye));
ret.m_afEntry[15] = 1.0f;
return( ret );
}
void ExtendedWmlCamera::DollyZoom( float fDistance )
{
Vector3f in(GetDirection());
in.Normalize();
Vector3f translate( in * fDistance );
// if new camera point is past target point, ignore this dolly request
Vector3f newEye = GetLocation() + translate;
Vector3f normDir = GetDirection();
normDir.Normalize();
if ( newEye.Dot(normDir) < m_ptTarget.Dot(normDir) ) {
if (!m_bUsePerspective)
OrthoDollyZoom( fDistance );
SetTargetFrame( GetLocation() + translate, GetLeft(), GetUp(), m_ptTarget );
}
}
//----------------------------------------------------------------------------
void DirectionalLight::ComputeDiffuse (const Matrix3f& rkWorldRotate,
const Vector3f&, float, const Vector3f*, const Vector3f* akNormal,
int iQuantity, const bool* abVisible, ColorRGB* akDiffuse)
{
// transform light direction to model space of old mesh
Vector3f kModelDir = m_kDirection*rkWorldRotate;
// adjust diffuse color by light intensity
ColorRGB kAdjDiffuse = m_fIntensity*m_kDiffuse;
for (int i = 0; i < iQuantity; i++)
{
if ( abVisible[i] )
{
float fDot = kModelDir.Dot(akNormal[i]);
if ( fDot < 0.0f )
akDiffuse[i] -= fDot*kAdjDiffuse;
}
}
}
/* Calculates the GLOBAL rotation of the world, not relative to the world's current rotation
* * pFrom, the first point,
* * pDest, the next point. The two points create a line and the line's angle from the x axis is measured */
double SplineTraveler::calcRotation(Vector3f axis, Vector3f pVertex, Vector3f pDest)
{
//Vector3f firstBranch = (pFrom - pVertex).Normalize();
Vector3f secondBranch = (pDest - pVertex).Normalize();
//TODO handle 180 degree case
float railAngle;
//dot product angle. magtides of the vectors are already equal to one
railAngle = acos(axis.Dot(secondBranch));
if(secondBranch.z > 0)
{
railAngle = 2.0*3.14159265 - railAngle;
}
rotationAngle = railAngle;
rotateAxis = secondBranch.Cross(axis); //axis of this rotation.
//rotateAxis = Vector3f(0,1,0);
/*Vector3f xaxis = Vector3f(1,0,0);
Vector3f yaxis = Vector3f(0,1,0);
Vector3f zaxis = Vector3f(0,0,1);
Vector3f point = pDest - pFrom;
point.y = 0;
bool tr = false;
//if(!(point.x == 0 && point.y == 0))
{
tr = true;
double x = acos((point.Dot(xaxis))/(point.Magnitude()));
if(point.z > 0)
{
x = 2.0*3.14159 - x;
}
//double y = acos((point.Dot(yaxis))/(point.Magnitude()));
double y = 0;
double z = 0;//acos((point.Dot(zaxis))/(point.Magnitude()));
grotation = Vector3f(x,y,z);
}*/
return railAngle;
}
string SBDelanteroL::Accion()
{
Vector3f b = soccerPerceptor.GetDriveVec(VO_BALL);
Vector3f myPos = soccerPerceptor.getMyPos();
int unum=soccerPerceptor.getUnum();
Vector3f g1 = soccerPerceptor.GetDriveVec(G1R);
Vector3f g2 = soccerPerceptor.GetDriveVec(G2R);
Vector3f Dir = (g1 + g2) / 2;
if (myPos.x() <-10 && b.Length() > 5) {
return Ir(Dir);
}
//Buscar centrar el Balon si esta cerca de las esquinas del contrario
if (gAbs(myPos.y()) + myPos.x() > 28) {
Dir += (soccerPerceptor.GetDriveVec(F2L) + soccerPerceptor.GetDriveVec(F1L))*.01;
}
Dir -= b;
Dir = Dir.Normalize();
float Pesc = Dir.Dot(b) / b.Length(), fact = 0;
// cout << " Pesc " << Pesc <<" b.Length() " << b.Length() << endl;
if (Pesc < 0.3 && b.Length() < 2) {
if (unum % 2 == 0) {
Dir = soccerPerceptor.GetDriveVec(F1L);
} else {
Dir = soccerPerceptor.GetDriveVec(F2L);
}
Dir = Dir.Normalize() - b.Normalize();
} else if (Pesc > 0.75 && b.Length() < 5) {
Dir = Dir + b;
} else {
fact = -b.Length() *.2 - 1;
Dir = Dir * fact + b;
}
return Ir(Dir);
}
//----------------------------------------------------------------------------
Vector3f BouncingTetrahedra::ClosestEdge (const Vector3f* vertices,
const Vector3f& closest, Vector3f& otherVertex)
{
// Find the edge of the tetrahedra nearest to the contact point. If
// otherVertexB is ZERO, then ClosestEdge skips the calculation of an
// unneeded other-vertex for the tetrahedron B.
Vector3f closestEdge;
float minDist = Mathf::MAX_REAL;
for (int i = 0; i < 3; ++i)
{
for (int j = i + 1; j < 4; ++j)
{
Vector3f edge = vertices[j] - vertices[i];
Vector3f diff = closest - vertices[i];
float DdE = diff.Dot(edge);
float edgeLength = edge.Length();
float diffLength = diff.Length();
float dist = Mathf::FAbs(DdE/(edgeLength*diffLength) - 1.0f);
if (dist < minDist)
{
minDist = dist;
closestEdge = edge;
for (int k = 0; otherVertex != Vector3f::ZERO && k < 3; ++k)
{
if (k != i && k != j)
{
otherVertex = vertices[k];
continue;
}
}
}
}
}
return closestEdge;
}
//----------------------------------------------------------------------------
void BouncingTetrahedra::Reposition (int t0, int t1, Contact& contact)
{
RigidTetra& tetra0 = *mTetras[t0];
RigidTetra& tetra1 = *mTetras[t1];
// Compute the centroids of the tetrahedra.
Vector3f vertices0[4], vertices1[4];
tetra0.GetVertices(vertices0);
tetra1.GetVertices(vertices1);
Vector3f centroid0 = Vector3f::ZERO;
Vector3f centroid1 = Vector3f::ZERO;
int i;
for (i = 0; i < 4; ++i)
{
centroid0 += vertices0[i];
centroid1 += vertices1[i];
}
centroid0 *= 0.25f;
centroid1 *= 0.25f;
// Randomly perturb the tetrahedra vertices by a small amount. This is
// done to help prevent the LCP solver from getting into cycles and
// degenerate cases.
const float reduction = 0.95f;
float reduceI = reduction*Mathf::IntervalRandom(0.9999f, 1.0001f);
float reduceJ = reduction*Mathf::IntervalRandom(0.9999f, 1.0001f);
for (i = 0; i < 4; ++i)
{
vertices0[i] = centroid0 + (vertices0[i] - centroid0)*reduceI;
vertices1[i] = centroid1 + (vertices1[i] - centroid1)*reduceJ;
}
// Compute the distance between the tetrahedra.
float dist = 1.0f;
int statusCode = 0;
Vector3f closest[2];
LCPPolyDist3(4, vertices0, 4, mFaces, 4, vertices1, 4, mFaces,
statusCode, dist, closest);
++mLCPCount;
// In theory, LCPPolyDist<3> should always find a valid distance, but just
// in case numerical round-off errors cause problems, let us trap it.
assertion(dist >= 0.0f, "LCP polyhedron distance calculator failed.\n");
// Reposition the tetrahedra to the theoretical points of contact.
closest[0] = centroid0 + (closest[0] - centroid0)/reduceI;
closest[1] = centroid1 + (closest[1] - centroid1)/reduceJ;
for (i = 0; i < 4; ++i)
{
vertices0[i] = centroid0 + (vertices0[i] - centroid0)/reduceI;
vertices1[i] = centroid1 + (vertices1[i] - centroid1)/reduceJ;
}
// Numerical round-off errors can cause interpenetration. Move the
// tetrahedra to back out of this situation. The length of diff
// estimates the depth of penetration when dist > 0 was reported.
Vector3f diff = closest[0] - closest[1];
// Apply the separation distance along the line containing the centroids
// of the tetrahedra.
Vector3f diff2 = centroid1 - centroid0;
diff = diff2/diff2.Length()*diff.Length();
// Move each tetrahedron by half of kDiff when the distance was large,
// but move each by twice kDiff when the distance is really small.
float mult = (dist >= mTolerance ? 0.5f : 1.0f);
Vector3f delta = mult*diff;
// Undo the interpenetration.
if (tetra0.Moved && !tetra1.Moved)
{
// Tetra t0 has moved but tetra t1 has not moved.
tetra1.SetPosition(tetra1.GetPosition() + 2.0f*delta);
tetra1.Moved = true;
}
else if (!tetra0.Moved && tetra1.Moved)
{
// Tetra t1 has moved but tetra t0 has not moved.
tetra0.SetPosition(tetra0.GetPosition() - 2.0f*delta);
tetra0.Moved = true;
}
else
{
// Both tetras moved or both tetras did not move.
tetra0.SetPosition(tetra0.GetPosition() - delta);
tetra0.Moved = true;
tetra1.SetPosition(tetra1.GetPosition() + delta);
tetra1.Moved = true;
}
// Test whether the two tetrahedra intersect in a vertex-face
// configuration.
contact.IsVFContact = IsVertex(vertices0, closest[0]);
if (contact.IsVFContact)
{
contact.A = mTetras[t1];
contact.B = mTetras[t0];
CalculateNormal(vertices1, closest[1], contact);
}
else
{
contact.IsVFContact = IsVertex(vertices1, closest[1]);
if (contact.IsVFContact)
{
contact.A = mTetras[t0];
contact.B = mTetras[t1];
CalculateNormal(vertices0, closest[0], contact);
}
}
// Test whether the two tetrahedra intersect in an edge-edge
// configuration.
if (!contact.IsVFContact)
{
contact.A = mTetras[t0];
contact.B = mTetras[t1];
Vector3f otherVertexA = Vector3f::UNIT_X;
Vector3f otherVertexB = Vector3f::ZERO;
contact.EA = ClosestEdge(vertices0, closest[0], otherVertexA);
contact.EB = ClosestEdge(vertices1, closest[1], otherVertexB);
Vector3f normal = contact.EA.UnitCross(contact.EB);
if (normal.Dot(otherVertexA - closest[0]) < 0.0f)
{
contact.N = normal;
}
else
{
contact.N = -normal;
}
}
// Reposition results to correspond to relocaton of tetra.
contact.PA = closest[0] - delta;
contact.PB = closest[1] + delta;
}
//----------------------------------------------------------------------------
// Compute the intersection of the segment C+t*V, [0,tMax], with a finite
// cylinder. The cylinder has center P, radius r, height h, and axis
// direction U2. The set {U0,U1,U2} is orthonormal and right-handed. In the
// coordinate system of the cylinder, a point A = P + x*U0 + y*U1 + z*U2. To
// be inside the cylinder, x*x + y*y <= r*r and |z| <= h/2. The function
// returns 'true' iff [t0,t1] is a nonempty interval.
//----------------------------------------------------------------------------
static bool IntersectLineCylinder (const SphereStruct& sph,
const Vector3f& V, const Vector3f& P, const Vector3f& U0,
const Vector3f& U1, const Vector3f& U2, float halfHeight,
const float tMax, float& t0, float& t1)
{
t0 = 0.0f;
t1 = tMax;
// Clip against the plane caps.
if (!ClipAgainstPlane(sph.C, V, U2, P + halfHeight*U2, t0, t1)
|| !ClipAgainstPlane(sph.C, V, -U2, P - halfHeight*U2, t0, t1))
{
return false;
}
// In cylinder coordinates, C+t*V = P + x(t)*U0 + y(t)*U1 + z(t)*U2,
// x(t) = Dot(U0,C+t*V-P) = a0 + t*b0, y(t) = Dot(U1,C+t*V-P) = a1 + t*b1
Vector3f CmP = sph.C - P;
float a0 = U0.Dot(CmP), b0 = U0.Dot(V);
float a1 = U1.Dot(CmP), b1 = U1.Dot(V);
// Clip the segment [t0,t1] against the cylinder wall.
float x0 = a0 + t0*b0, y0 = a1 + t0*b1, r0Sqr = x0*x0 + y0*y0;
float x1 = a0 + t1*b0, y1 = a1 + t1*b1, r1Sqr = x1*x1 + y1*y1;
float rSqr = sph.RSqr;
// Some case require computing intersections of the segment with the
// circle of radius r. This amounts to finding roots for the quadratic
// Q(t) = x(t)*x(t) + y(t)*y(t) - r*r = q2*t^2 + 2*q1*t + q0, where
// q0 = a0*a0+b0*b0-r*r, q1 = a0*a1+b0*b1, and q2 = a1*a1+b1*b1. Compute
// the coefficients only when needed.
float q0, q1, q2, T;
if (r0Sqr > rSqr)
{
if (r1Sqr > rSqr)
{
q2 = b0*b0 + b1*b1;
if (q2 > 0.0f)
{
q0 = a0*a0 + a1*a1 - rSqr;
q1 = a0*b0 + a1*b1;
float discr = q1*q1 - q0*q2;
if (discr < 0.0f)
{
// The quadratic has no real-valued roots, so the
// segment is outside the cylinder.
return false;
}
float rootDiscr = sqrtf(discr);
float invQ2 = 1.0f/q2;
float root0 = (-q1 - rootDiscr)*invQ2;
float root1 = (-q1 + rootDiscr)*invQ2;
// We know that (x0,y0) and (x1,y1) are outside the
// cylinder, so Q(t0) > 0 and Q(t1) > 0. This reduces
// the number of cases to analyze for intersection of
// [t0,t1] and [root0,root1].
if (t1 < root0 || t0 > root1)
{
// The segment is strictly outside the cylinder.
return false;
}
else
{
// [t0,t1] strictly contains [root0,root1]
t0 = root0;
t1 = root1;
}
}
else // q2 == 0.0f and q1 = 0.0f; that is, Q(t) = q0
{
// The segment is degenerate, a point that is outside the
// cylinder.
return false;
}
}
else if (r1Sqr < rSqr)
{
// Solve nondegenerate quadratic and clip. There must be a single
// root T in [t0,t1]. Discard [t0,T].
q0 = a0*a0 + a1*a1 - rSqr;
q1 = a0*b0 + a1*b1;
q2 = b0*b0 + b1*b1;
t0 = (-q1 - sqrtf(fabsf(q1*q1 - q0*q2)))/q2;
}
else // r1Sqr == rSqr
{
// The segment intersects the circle at t1. The other root is
// necessarily T = -t1-2*q1/q2. Use it only when T <= t1, in
// which case discard [t0,T].
q1 = a0*b0 +a1*b1;
q2 = b0*b0 + b1*b1;
T = -t1 - 2.0f*q1/q2;
t0 = (T < t1 ? T : t1);
}
}
else if (r0Sqr < rSqr)
{
if (r1Sqr > rSqr)
{
// Solve nondegenerate quadratic and clip. There must be a single
// root T in [t0,t1]. Discard [T,t1].
q0 = a0*a0 + a1*a1 - rSqr;
q1 = a0*b0 + a1*b1;
q2 = b0*b0 + b1*b1;
t1 = (-q1 + sqrtf(fabsf(q1*q1 - q0*q2)))/q2;
}
// else: The segment is inside the cylinder.
}
else // r0Sqr == rSqr
{
if (r1Sqr > rSqr)
{
// The segment intersects the circle at t0. The other root is
// necessarily T = -t0-2*q1/q2. Use it only when T >= t0, in
// which case discard [T,t1].
q1 = a0*b0 + a1*b1;
q2 = b0*b0 + b1*b1;
T = -t0 - 2.0f*q1/q2;
t1 = (T > t0 ? T : t0);
}
// else: The segment is inside the cylinder.
}
return true;
}
//----------------------------------------------------------------------------
void OpenGLRenderer::Draw (const PlanarReflection& rkPReflection)
{
TriMeshPtr spkPlane = rkPReflection.GetPlane();
NodePtr spkCaster = rkPReflection.GetCaster();
if ( !m_bCapPlanarReflection )
{
// The effect is not supported. Draw normally without the mirror.
// The OnDraw calls are necessary to handle culling and camera plane
// state.
spkPlane->OnDraw(*this);
spkCaster->OnDraw(*this);
return;
}
if ( m_bDrawingReflected )
{
// Some other object is currently doing a planar reflection. Do not
// allow the recursion and just draw normally.
Renderer::Draw(spkCaster);
SetState(spkPlane->GetRenderStateArray());
Draw(*spkPlane);
return;
}
// TO DO: Support for multiple mirrors could be added here by iterating
// over the section delimited by START PER-MIRROR and END PER-MIRROR.
// None of the OpenGL code needs to change, just the mirror-plane data.
// START PER-MIRROR
// enable depth buffering
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
glDepthMask(GL_TRUE);
// Step 1 setup and render.
// Render the mirror into the stencil plane (but no color). All visible
// mirror pixels will have the stencil value of the mirror.
// Make sure that no pixels are written to the depth buffer or color
// buffer, but use depth buffer testing so that the stencil will not
// be written where the plane is behind something already in the
// depth buffer.
glEnable(GL_STENCIL_TEST);
glStencilFunc(GL_ALWAYS,rkPReflection.GetStencilValue(),~0);
glStencilOp(GL_KEEP,GL_KEEP,GL_REPLACE);
glStencilMask(~0);
glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
glDepthMask(GL_FALSE);
Draw(*spkPlane);
// Step 2 setup and render.
// Render the mirror plane again by only processing pixels where
// the stencil buffer contains the reference value. This time
// there is no changes to the stencil buffer and the depth buffer value
// is reset to the far view clipping plane (this is done by setting the
// range of depth values in the viewport volume to be [1,1]. Since the
// mirror plane cannot also be semi-transparent, then there we do not
// care what is behind the mirror plane in the depth buffer. We need
// to move the depth buffer values back where the mirror plane will
// be rendered so that when the reflected caster is rendered
// it can be depth buffered correctly (note that the rendering
// of the reflected caster will cause depth value to be written
// which will appear to be behind the mirror plane). Enable writes
// to the color buffer. Later when we want to render the reflecting
// plane and have it blend with the background (which should contain
// the reflected caster), we want to use the same blending function
// so that the pixels where the reflected caster was not rendered
// will contain the reflecting plane and in that case, the blending
// result will have the reflecting plane appear to be opaque when
// in reality it was blended with blending coefficients adding to one.
SetState(spkPlane->GetRenderStateArray());
glDepthRange(1.0,1.0);
glDepthFunc(GL_ALWAYS);
glStencilFunc(GL_EQUAL,rkPReflection.GetStencilValue(),~0);
glStencilOp(GL_KEEP,GL_KEEP,GL_KEEP);
glStencilMask(~0);
glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
glDepthMask(GL_TRUE);
Draw(*spkPlane);
// Step 2 cleanup.
// Restore the depth range and depth testing function.
glDepthFunc(GL_LESS);
glDepthRange(0.0,1.0);
// Step 3 setup.
// We are about to render the reflected caster. For that, we
// will need to compute the reflection viewing matrix.
Vector3f kCurrNormal = spkPlane->WorldRotate()*
rkPReflection.GetPlaneNormal();
Vector3f kCurrPoint = spkPlane->WorldTranslate()+spkPlane->WorldScale()*
(spkPlane->WorldRotate()*rkPReflection.GetPointOnPlane());
GLdouble adPlaneEq[4] = { -kCurrNormal.X(), -kCurrNormal.Y(),
-kCurrNormal.Z(), kCurrNormal.Dot(kCurrPoint) };
adPlaneEq[0] = -adPlaneEq[0];
adPlaneEq[1] = -adPlaneEq[1];
adPlaneEq[2] = -adPlaneEq[2];
adPlaneEq[3] = -adPlaneEq[3];
GLfloat aafReflectionMatrix[4][4];
ComputeReflectionMatrix(aafReflectionMatrix,adPlaneEq);
// Save the modelview transform before replacing it with
// the viewing transform which will handle the reflection.
glPushMatrix();
glMultMatrixf(&aafReflectionMatrix[0][0]);
// Setup a clip plane so that only objects above the mirror plane
// get reflected.
glClipPlane(GL_CLIP_PLANE0,adPlaneEq);
glEnable(GL_CLIP_PLANE0);
// Reverse the cull direction. Allow for models that are not necessarily
// set up with front or back face culling.
m_bReverseCullState = true;
// We do not support mirrors reflecting mirrors. They just appear as the
// base color in a reflection.
m_bDrawingReflected = true;
// Step 3 render.
// Render the reflected caster. Only render where the stencil buffer
// contains the reference value. Enable depth testing. This time
// allow writes to the color buffer.
glStencilFunc(GL_EQUAL,rkPReflection.GetStencilValue(),~0);
glStencilOp(GL_KEEP,GL_KEEP,GL_KEEP);
glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
Renderer::Draw(spkCaster);
// Step 3 cleanup.
// Restore state.
m_bDrawingReflected = false;
m_bReverseCullState = false;
glDisable(GL_CLIP_PLANE0);
glPopMatrix();
// Step 4 setup.
// We are about to render the reflecting plane again. Reset to
// the render state for the reflecting plane. We want to blend
// the reflecting plane with what is already in the color buffer
// where the reflecting plane will be rendered, particularly
// either the image of the reflected caster or the reflecting
// plane. All we want to change about the rendering of the
// reflecting plane at this stage is to force the alpha channel
// to always be the reflectance value for the reflecting plane.
// Render the reflecting plane wherever the stencil buffer is set
// to the reference value. This time clear the stencil buffer
// reference value where it is set. Perform the normal depth
// buffer testing and writes. Allow the color buffer to be
// written to, but this time blend the relecting plane with
// the values in the color buffer based on the reflectance value.
// Note that where the stencil buffer is set, the color buffer
// has either color values from the reflecting plane or the
// reflected caster. Blending will use src=1-alpha (reflecting plane)
// and dest=alpha background (reflecting plane or reflected caster).
SetState(spkPlane->GetRenderStateArray());
glEnable(GL_BLEND);
glBlendColorEXT(0.0f,0.0f,0.0f,rkPReflection.GetReflectance());
glBlendFunc(GL_ONE_MINUS_CONSTANT_ALPHA_EXT,GL_CONSTANT_ALPHA_EXT);
glStencilFunc(GL_EQUAL,rkPReflection.GetStencilValue(),~0);
glStencilOp(GL_KEEP,GL_KEEP,GL_INVERT);
glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
Draw(*spkPlane);
// Step 4 cleanup.
glDisable(GL_BLEND);
glDisable(GL_STENCIL_TEST);
// END PER-MIRROR
// render the objects as usual
Renderer::Draw(spkCaster);
}
bool Plane::IsFacing(const Vector3f& point)
{
Vector3f v = point - Point;
return v.Dot(Normal) > 0.0f;
}
void SensorFusion::handleMessage(const MessageBodyFrame& msg)
{
if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled())
return;
// Put the sensor readings into convenient local variables
Vector3f gyro = msg.RotationRate;
Vector3f accel = msg.Acceleration;
Vector3f mag = msg.MagneticField;
// Insert current sensor data into filter history
FRawMag.AddElement(mag);
FAngV.AddElement(gyro);
// Apply the calibration parameters to raw mag
Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean();
// Set variables accessible through the class API
DeltaT = msg.TimeDelta;
AngV = gyro;
A = accel;
RawMag = mag;
CalMag = calMag;
// Keep track of time
Stage++;
RunningTime += DeltaT;
// Small preprocessing
Quatf Qinv = Q.Inverted();
Vector3f up = Qinv.Rotate(Vector3f(0, 1, 0));
Vector3f gyroCorrected = gyro;
// Apply integral term
// All the corrections are stored in the Simultaneous Orthogonal Rotations Angle representation,
// which allows to combine and scale them by just addition and multiplication
if (EnableGravity || EnableYawCorrection)
gyroCorrected -= GyroOffset;
if (EnableGravity)
{
const float spikeThreshold = 0.01f;
const float gravityThreshold = 0.1f;
float proportionalGain = 5 * Gain; // Gain parameter should be removed in a future release
float integralGain = 0.0125f;
Vector3f tiltCorrection = SensorFusion_ComputeCorrection(accel, up);
if (Stage > 5)
{
// Spike detection
float tiltAngle = up.Angle(accel);
TiltAngleFilter.AddElement(tiltAngle);
if (tiltAngle > TiltAngleFilter.Mean() + spikeThreshold)
proportionalGain = integralGain = 0;
// Acceleration detection
const float gravity = 9.8f;
if (fabs(accel.Length() / gravity - 1) > gravityThreshold)
integralGain = 0;
}
else // Apply full correction at the startup
{
proportionalGain = 1 / DeltaT;
integralGain = 0;
}
gyroCorrected += (tiltCorrection * proportionalGain);
GyroOffset -= (tiltCorrection * integralGain * DeltaT);
}
if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f)
{
const float maxMagRefDist = 0.1f;
const float maxTiltError = 0.05f;
float proportionalGain = 0.01f;
float integralGain = 0.0005f;
// Update the reference point if needed
if (MagRefIdx < 0 || calMag.Distance(MagRefsInBodyFrame[MagRefIdx]) > maxMagRefDist)
{
// Delete a bad point
if (MagRefIdx >= 0 && MagRefScore < 0)
{
MagNumReferences--;
MagRefsInBodyFrame[MagRefIdx] = MagRefsInBodyFrame[MagNumReferences];
MagRefsInWorldFrame[MagRefIdx] = MagRefsInWorldFrame[MagNumReferences];
}
// Find a new one
MagRefIdx = -1;
MagRefScore = 1000;
float bestDist = maxMagRefDist;
for (int i = 0; i < MagNumReferences; i++)
{
float dist = calMag.Distance(MagRefsInBodyFrame[i]);
if (bestDist > dist)
{
bestDist = dist;
MagRefIdx = i;
}
}
// Create one if needed
if (MagRefIdx < 0 && MagNumReferences < MagMaxReferences)
{
MagRefIdx = MagNumReferences;
MagRefsInBodyFrame[MagRefIdx] = calMag;
MagRefsInWorldFrame[MagRefIdx] = Q.Rotate(calMag).Normalized();
MagNumReferences++;
}
}
if (MagRefIdx >= 0)
{
Vector3f magEstimated = Qinv.Rotate(MagRefsInWorldFrame[MagRefIdx]);
Vector3f magMeasured = calMag.Normalized();
// Correction is computed in the horizontal plane (in the world frame)
Vector3f yawCorrection = SensorFusion_ComputeCorrection(magMeasured.ProjectToPlane(up),
magEstimated.ProjectToPlane(up));
if (fabs(up.Dot(magEstimated - magMeasured)) < maxTiltError)
{
MagRefScore += 2;
}
else // If the vertical angle is wrong, decrease the score
{
MagRefScore -= 1;
proportionalGain = integralGain = 0;
}
gyroCorrected += (yawCorrection * proportionalGain);
GyroOffset -= (yawCorrection * integralGain * DeltaT);
}
}
// Update the orientation quaternion based on the corrected angular velocity vector
Q = Q * Quatf(gyroCorrected, gyroCorrected.Length() * DeltaT);
// The quaternion magnitude may slowly drift due to numerical error,
// so it is periodically normalized.
if (Stage % 500 == 0)
Q.Normalize();
}
void Player::HandleMovement(double dt, std::vector<Ptr<CollisionModel> >* collisionModels,
std::vector<Ptr<CollisionModel> >* groundCollisionModels, bool shiftDown)
{
// Handle keyboard movement.
// This translates BasePos based on the orientation and keys pressed.
// Note that Pitch and Roll do not affect movement (they only affect view).
Vector3f controllerMove;
if(MoveForward || MoveBack || MoveLeft || MoveRight)
{
if (MoveForward)
{
controllerMove += ForwardVector;
}
else if (MoveBack)
{
controllerMove -= ForwardVector;
}
if (MoveRight)
{
controllerMove += RightVector;
}
else if (MoveLeft)
{
controllerMove -= RightVector;
}
}
else if (GamepadMove.LengthSq() > 0)
{
controllerMove = GamepadMove;
}
controllerMove = GetOrientation(bMotionRelativeToBody).Rotate(controllerMove);
controllerMove.y = 0; // Project to the horizontal plane
if (controllerMove.LengthSq() > 0)
{
// Normalize vector so we don't move faster diagonally.
controllerMove.Normalize();
controllerMove *= OVR::Alg::Min<float>(MoveSpeed * (float)dt * (shiftDown ? 3.0f : 1.0f), 1.0f);
}
// Compute total move direction vector and move length
Vector3f orientationVector = controllerMove;
float moveLength = orientationVector.Length();
if (moveLength > 0)
orientationVector.Normalize();
float checkLengthForward = moveLength;
Planef collisionPlaneForward;
bool gotCollision = false;
for(size_t i = 0; i < collisionModels->size(); ++i)
{
// Checks for collisions at model base level, which should prevent us from
// slipping under walls
if (collisionModels->at(i)->TestRay(BodyPos, orientationVector, checkLengthForward,
&collisionPlaneForward))
{
gotCollision = true;
break;
}
}
if (gotCollision)
{
// Project orientationVector onto the plane
Vector3f slideVector = orientationVector - collisionPlaneForward.N
* (orientationVector.Dot(collisionPlaneForward.N));
// Make sure we aren't in a corner
for(size_t j = 0; j < collisionModels->size(); ++j)
{
if (collisionModels->at(j)->TestPoint(BodyPos - Vector3f(0.0f, RailHeight, 0.0f) +
(slideVector * (moveLength))) )
{
moveLength = 0;
break;
}
}
if (moveLength != 0)
{
orientationVector = slideVector;
}
}
// Checks for collisions at foot level, which allows us to follow terrain
orientationVector *= moveLength;
BodyPos += orientationVector;
Planef collisionPlaneDown;
float adjustedUserEyeHeight = GetFloorDistanceFromTrackingOrigin(ovrTrackingOrigin_EyeLevel);
float finalDistanceDown = adjustedUserEyeHeight + 10.0f;
// Only apply down if there is collision model (otherwise we get jitter).
if (groundCollisionModels->size())
{
for(size_t i = 0; i < groundCollisionModels->size(); ++i)
{
float checkLengthDown = adjustedUserEyeHeight + 10;
if (groundCollisionModels->at(i)->TestRay(BodyPos, Vector3f(0.0f, -1.0f, 0.0f),
checkLengthDown, &collisionPlaneDown))
{
finalDistanceDown = Alg::Min(finalDistanceDown, checkLengthDown);
}
}
// Maintain the minimum camera height
if (adjustedUserEyeHeight - finalDistanceDown < 1.0f)
{
BodyPos.y += adjustedUserEyeHeight - finalDistanceDown;
}
}
SetBodyPos(BodyPos, false);
}
