//----------------------------------------------------------------------------
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 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;
}
}
void Viewer3D::Rotate(int winX, int winY)
{
to.x = winX * view.fw - 1.0f;
to.y = winY * view.fh - 1.0f;
Vector3f delta = from - to;
delta.y = -delta.y;
float length = delta.Length();
if (length < 1e-5) return;
delta /= length;
float rotDist = 0.5f * max(view.w, view.h) * view.s;
float angle = length / rotDist * 180.0f;
Vector3f axis = Vector3f(delta.y, -delta.x, 0.0f);
qRotation = Quaternion(axis, angle) * qRotation;
from = to;
changed = true;
}
void TrackballCamera::Rotate(int winX, int winY)
{
to.x = winX * view.fw - 1.0f;
to.y = winY * view.fh - 1.0f;
Vector3f delta = to - from;
float length = delta.Length();
if (length < 1e-5) return;
delta /= length;
float rotDist = isConstSpeed ? constSpeedValue :
0.5f * max(view.w, view.h) * view.s;
float angle = length / rotDist * 180.0f;
Vector3f axis = Vector3f(delta.y, delta.x, 0.0f);
qRotation = Quaternion(axis, angle) * qRotation;
from = to;
changed = true;
}
// This is a simple predictive filter based only on extrapolating the smoothed, current angular velocity.
// Note that both QP (the predicted future orientation) and Q (the current orientation) are both maintained.
Quatf SensorFusion::GetPredictedOrientation()
{
Lock::Locker lockScope(Handler.GetHandlerLock());
Quatf qP = QUncorrected;
if (EnablePrediction) {
#if 1
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
float angVelFL = angVelF.Length();
if (angVelFL > 0.001f)
{
Vector3f rotAxisP = angVelF / angVelFL;
float halfRotAngleP = angVelFL * PredictionDT * 0.5f;
float sinaHRAP = sin(halfRotAngleP);
Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
qP = QUncorrected * deltaQP;
}
#else
Quatd qpd = Quatd(Q.x,Q.y,Q.z,Q.w);
int predictionStages = (int)(PredictionDT / DeltaT);
Vector3f aa = FAngV.SavitzkyGolayDerivative12();
Vector3d aad = Vector3d(aa.x,aa.y,aa.z);
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
Vector3d avkd = Vector3d(angVelF.x,angVelF.y,angVelF.z);
for (int i = 0; i < predictionStages; i++)
{
double angVelLengthd = avkd.Length();
Vector3d rotAxisd = avkd / angVelLengthd;
double halfRotAngled = angVelLengthd * DeltaT * 0.5;
double sinHRAd = sin(halfRotAngled);
Quatd deltaQd = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd,
cos(halfRotAngled));
qpd = qpd * deltaQd;
// Update vel
avkd += aad;
}
qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w);
#endif
}
return qP;
}
Spectrum SpecularReflection::Sample_f( const Vector3f& wo , const Vector3f& n , Vector3f* wi , const Point2f& samplePoint , float* pdf , bool& bNoOccur ) const
{
*wi = Normalize( 2 * Dot( wo , n ) / n.Length() * Normalize( n ) - wo );
*pdf = 1.0;
Spectrum F = fresnel->Evalute( wo * n );
// 经验设定
// 参考该问题的答案：http://www.opengpu.org/forum.php?mod=viewthread&tid=18099&extra=page%3D1
float P = 0.25f + 0.5f * F[0];
if( RNG::Get().GetFloat() > /*F[0]*/P )
{
// 无反射
bNoOccur = true;
return Spectrum( 0 );
}
return F / P * R / AbsDot( *wi , n );
}
void FPSController::UpdateStatic(float deltaTime)
{
if (mBody.get() == 0)
{
return;
}
Matrix matrix;
Matrix fwd;
Vector3f vec;
PrepareUpdate(matrix, fwd, vec);
mBody->SetRotation(matrix);
Vector3f bodyVel = mBody->GetVelocity() + (deltaTime * vec * STATIC_ACCELERATION);
bodyVel *= STATIC_VELOCITY_DECAY;
Vector3f velocity =
vec.y() * fwd.Up() +
vec.x() * fwd.Right() +
vec.z() * Vector3f(0,0,1);
matrix.Pos() = mBody->GetPosition() + velocity * bodyVel.Length();
mBody->SetPosition(matrix.Pos());
mBody->SetVelocity(velocity);
shared_ptr<BaseNode> bodyParent = shared_static_cast<BaseNode>
(mBody->GetParent().lock());
if (bodyParent.get() != 0)
{
mBody->SynchronizeParent();
bodyParent->UpdateHierarchy();
}
mHAngleDelta = 0.0;
mVAngleDelta = 0.0;
}
void Explosion::Update(float dt) {
this->elapsedLifetime -= dt;
if (this->elapsedLifetime < 0.0f) {
this->exists = false;
std::cout << "КОНЕЦ ВЗРЫВА!\n";
}
// Обработка взрыва
//float currRadius = ((totalLifetime - elapsedLifetime) / totalLifetime) * this->maxRadius;
for (int i = 0; i < owner->GetParticleSystem()->GetParticlesCount(); i++) {
ParticleHandle<ParticleInfo> blockParticle = owner->GetParticleSystem()->GetParticle(size_t(i));
Vector3f distance = blockParticle.GetPos() - this->pos;
//if (distance.Length() <= currRadius) {
//this->force = desc.force;
//float force = 1.0f;
float a = distance.Length();
float offset = force / (a * a * a);
Vector3f resultAcceleration = blockParticle.GetAcceleration() + offset * distance.GetNorm();
blockParticle.SetAcceleration(resultAcceleration);
//}
}
}
// A predictive filter based on extrapolating the smoothed, current angular velocity
Quatf SensorFusion::GetPredictedOrientation(float pdt)
{
Lock::Locker lockScope(Handler.GetHandlerLock());
Quatf qP = Q;
if (EnablePrediction)
{
// This method assumes a constant angular velocity
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
float angVelFL = angVelF.Length();
// Force back to raw measurement
angVelF = AngV;
angVelFL = AngV.Length();
// Dynamic prediction interval: Based on angular velocity to reduce vibration
const float minPdt = 0.001f;
const float slopePdt = 0.1f;
float newpdt = pdt;
float tpdt = minPdt + slopePdt * angVelFL;
if (tpdt < pdt)
newpdt = tpdt;
//LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f));
if (angVelFL > 0.001f)
{
Vector3f rotAxisP = angVelF / angVelFL;
float halfRotAngleP = angVelFL * newpdt * 0.5f;
float sinaHRAP = sin(halfRotAngleP);
Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
qP = Q * deltaQP;
}
}
return qP;
}
void DebugRenderer::AddLine(const Vector3f& a, const Vector3f& b, const Color& color, Lifespan lifespan) {
Vector3f dir = b - a;
AddLine(a, dir, dir.Length(), color, lifespan);
}
Vector3f lzmath::Normalize(const Vector3f &v)
{
return (v / v.Length());
}
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 GelatinCube::CreateSprings ()
{
// The inner 4-by-4-by-4 particles are used as the control points of a
// B-spline volume. The outer layer of particles are immovable to
// prevent the cuboid from collapsing into itself.
int iSlices = 6;
int iRows = 6;
int iCols = 6;
// Viscous forces applied. If you set viscosity to zero, the cuboid
// wiggles indefinitely since there is no dissipation of energy. If
// the viscosity is set to a positive value, the oscillations eventually
// stop. The length of time to steady state is inversely proportional
// to the viscosity.
#ifdef _DEBUG
float fStep = 0.1f;
#else
float fStep = 0.01f; // simulation needs to run slower in release mode
#endif
float fViscosity = 0.01f;
m_pkModule = new PhysicsModule(iSlices,iRows,iCols,fStep,fViscosity);
// The initial cuboid is axis-aligned. The outer shell is immovable.
// All other masses are constant.
float fSFactor = 1.0f/(float)(iSlices-1);
float fRFactor = 1.0f/(float)(iRows-1);
float fCFactor = 1.0f/(float)(iCols-1);
int iSlice, iRow, iCol;
for (iSlice = 0; iSlice < iSlices; iSlice++)
{
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->Position(iSlice,iRow,iCol) =
Vector3f(iCol*fCFactor,iRow*fRFactor,iSlice*fSFactor);
if ( 1 <= iSlice && iSlice < iSlices-1
&& 1 <= iRow && iRow < iRows-1
&& 1 <= iCol && iCol < iCols-1 )
{
m_pkModule->SetMass(iSlice,iRow,iCol,1.0f);
m_pkModule->Velocity(iSlice,iRow,iCol) =
0.1f*Vector3f(Mathf::SymmetricRandom(),
Mathf::SymmetricRandom(),Mathf::SymmetricRandom());
}
else
{
m_pkModule->SetMass(iSlice,iRow,iCol,Mathf::MAX_REAL);
m_pkModule->Velocity(iSlice,iRow,iCol) = Vector3f::ZERO;
}
}
}
}
// springs are at rest in the initial configuration
const float fConstant = 10.0f;
Vector3f kDiff;
for (iSlice = 0; iSlice < iSlices-1; iSlice++)
{
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->ConstantS(iSlice,iRow,iCol) = fConstant;
kDiff = m_pkModule->Position(iSlice+1,iRow,iCol) -
m_pkModule->Position(iSlice,iRow,iCol);
m_pkModule->LengthS(iSlice,iRow,iCol) = kDiff.Length();
}
}
}
for (iSlice = 0; iSlice < iSlices; iSlice++)
{
for (iRow = 0; iRow < iRows-1; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->ConstantR(iSlice,iRow,iCol) = fConstant;
kDiff = m_pkModule->Position(iSlice,iRow+1,iCol) -
m_pkModule->Position(iSlice,iRow,iCol);
m_pkModule->LengthR(iSlice,iRow,iCol) = kDiff.Length();
}
}
}
for (iSlice = 0; iSlice < iSlices; iSlice++)
{
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols-1; iCol++)
{
m_pkModule->ConstantC(iSlice,iRow,iCol) = fConstant;
kDiff = m_pkModule->Position(iSlice,iRow,iCol+1) -
m_pkModule->Position(iSlice,iRow,iCol);
m_pkModule->LengthC(iSlice,iRow,iCol) = kDiff.Length();
}
}
}
}
//----------------------------------------------------------------------------
void Cloth::CreateSprings ()
{
// set up the mass-spring system
int iRows = 8;
int iCols = 16;
float fStep = 0.01f;
Vector3f kGravity(0.0f,0.0f,-1.0f);
Vector3f kWind(0.5f,0.0f,0.0f);
float fViscosity = 10.0f;
float fMaxAmplitude = 2.0f;
m_pkModule = new PhysicsModule(iRows,iCols,fStep,kGravity,kWind,
fViscosity,fMaxAmplitude);
// The top row of the mesh is immovable (infinite mass). All other
// masses are constant.
int iRow, iCol;
for (iCol = 0; iCol < iCols; iCol++)
m_pkModule->SetMass(iRows-1,iCol,Mathf::MAX_REAL);
for (iRow = 0; iRow < iRows-1; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
m_pkModule->SetMass(iRow,iCol,1.0f);
}
// initial position on a vertical axis-aligned rectangle, zero velocity
float fRFactor = 1.0f/(float)(iRows-1);
float fCFactor = 1.0f/(float)(iCols-1);
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->Position(iRow,iCol) =
Vector3f(iCol*fCFactor,0.0f,iRow*fRFactor);
m_pkModule->Velocity(iRow,iCol) = Vector3f::ZERO;
}
}
// springs are at rest in the initial configuration
const float fRConstant = 1000.0f;
const float fBConstant = 100.0f;
Vector3f kDiff;
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols-1; iCol++)
{
m_pkModule->ConstantC(iRow,iCol) = fRConstant;
kDiff = m_pkModule->Position(iRow,iCol+1) -
m_pkModule->Position(iRow,iCol);
m_pkModule->LengthC(iRow,iCol) = kDiff.Length();
}
}
for (iRow = 0; iRow < iRows-1; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->ConstantR(iRow,iCol) = fBConstant;
kDiff = m_pkModule->Position(iRow,iCol) -
m_pkModule->Position(iRow+1,iCol);
m_pkModule->LengthR(iRow,iCol) = kDiff.Length();
}
}
}
void SensorFusion::handleMessage(const MessageBodyFrame& msg)
{
if (msg.Type != Message_BodyFrame)
return;
// Put the sensor readings into convenient local variables
Vector3f angVel = msg.RotationRate;
Vector3f rawAccel = msg.Acceleration;
Vector3f mag = msg.MagneticField;
// Set variables accessible through the class API
DeltaT = msg.TimeDelta;
AngV = msg.RotationRate;
AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity
A = rawAccel;
// Allow external access to uncalibrated magnetometer values
RawMag = mag;
// Apply the calibration parameters to raw mag
if (HasMagCalibration())
{
mag.x += MagCalibrationMatrix.M[0][3];
mag.y += MagCalibrationMatrix.M[1][3];
mag.z += MagCalibrationMatrix.M[2][3];
}
// Provide external access to calibrated mag values
// (if the mag is not calibrated, then the raw value is returned)
CalMag = mag;
float angVelLength = angVel.Length();
float accLength = rawAccel.Length();
// Acceleration in the world frame (Q is current HMD orientation)
Vector3f accWorld = Q.Rotate(rawAccel);
// Keep track of time
Stage++;
float currentTime = Stage * DeltaT; // Assumes uniform time spacing
// Insert current sensor data into filter history
FRawMag.AddElement(RawMag);
FAccW.AddElement(accWorld);
FAngV.AddElement(angVel);
// Update orientation Q based on gyro outputs. This technique is
// based on direct properties of the angular velocity vector:
// Its direction is the current rotation axis, and its magnitude
// is the rotation rate (rad/sec) about that axis. Our sensor
// sampling rate is so fast that we need not worry about integral
// approximation error (not yet, anyway).
if (angVelLength > 0.0f)
{
Vector3f rotAxis = angVel / angVelLength;
float halfRotAngle = angVelLength * DeltaT * 0.5f;
float sinHRA = sin(halfRotAngle);
Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle));
Q = Q * deltaQ;
}
// The quaternion magnitude may slowly drift due to numerical error,
// so it is periodically normalized.
if (Stage % 5000 == 0)
Q.Normalize();
// Maintain the uncorrected orientation for later use by predictive filtering
QUncorrected = Q;
// Perform tilt correction using the accelerometer data. This enables
// drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected
// because the rotation axis is parallel to the gravity vector.
if (EnableGravity)
{
// Correcting for tilt error by using accelerometer data
const float gravityEpsilon = 0.4f;
const float angVelEpsilon = 0.1f; // Relatively slow rotation
const int tiltPeriod = 50; // Req'd time steps of stability
const float maxTiltError = 0.05f;
const float minTiltError = 0.01f;
// This condition estimates whether the only measured acceleration is due to gravity
// (the Rift is not linearly accelerating). It is often wrong, but tends to average
// out well over time.
if ((fabs(accLength - 9.81f) < gravityEpsilon) &&
(angVelLength < angVelEpsilon))
TiltCondCount++;
else
TiltCondCount = 0;
// After stable measurements have been taken over a sufficiently long period,
// estimate the amount of tilt error and calculate the tilt axis for later correction.
if (TiltCondCount >= tiltPeriod)
{ // Update TiltErrorEstimate
TiltCondCount = 0;
// Use an average value to reduce noice (could alternatively use an LPF)
Vector3f accWMean = FAccW.Mean();
// Project the acceleration vector into the XZ plane
Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z);
// The unit normal of xzAcc will be the rotation axis for tilt correction
Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized();
Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f);
// This is the amount of rotation
float tiltAngle = yUp.Angle(accWMean);
// Record values if the tilt error is intolerable
if (tiltAngle > maxTiltError)
{
TiltErrorAngle = tiltAngle;
TiltErrorAxis = tiltAxis;
}
}
// This part performs the actual tilt correction as needed
if (TiltErrorAngle > minTiltError)
{
if ((TiltErrorAngle > 0.4f)&&(Stage < 8000))
{ // Tilt completely to correct orientation
Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q;
TiltErrorAngle = 0.0f;
}
else
{
//LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n",
// TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z);
//float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f;
// This uses agressive correction steps while your head is moving fast
float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f);
// Incrementally "untilt" by a small step size
Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q;
TiltErrorAngle += deltaTiltAngle;
}
}
}
// Yaw drift correction based on magnetometer data. This corrects the part of the drift
// that the accelerometer cannot handle.
// This will only work if the magnetometer has been enabled, calibrated, and a reference
// point has been set.
const float maxAngVelLength = 3.0f;
const int magWindow = 5;
const float yawErrorMax = 0.1f;
const float yawErrorMin = 0.01f;
const int yawErrorCountLimit = 50;
const float yawRotationStep = 0.00002f;
if (angVelLength < maxAngVelLength)
MagCondCount++;
else
MagCondCount = 0;
YawCorrectionInProgress = false;
if (EnableYawCorrection && MagReady && (currentTime > 2.0f) && (MagCondCount >= magWindow) &&
(Q.Distance(MagRefQ) < MagRefDistance))
{
// Use rotational invariance to bring reference mag value into global frame
Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM));
// Bring current (averaged) mag reading into global frame
Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean()));
// Calculate the reference yaw in the global frame
float gryaw = atan2(grefmag.x,grefmag.z);
// Calculate the current yaw in the global frame
float gyaw = atan2(gmag.x,gmag.z);
//LogText("Yaw error estimate: %f\n",YawErrorAngle);
// The difference between reference and current yaws is the perceived error
YawErrorAngle = AngleDifference(gyaw,gryaw);
// If the perceived error is large, keep count
if ((fabs(YawErrorAngle) > yawErrorMax) && (!YawCorrectionActivated))
YawErrorCount++;
// After enough iterations of high perceived error, start the correction process
if (YawErrorCount > yawErrorCountLimit)
YawCorrectionActivated = true;
// If the perceived error becomes small, turn off the yaw correction
if ((fabs(YawErrorAngle) < yawErrorMin) && YawCorrectionActivated)
{
YawCorrectionActivated = false;
YawErrorCount = 0;
}
// Perform the actual yaw correction, due to previously detected, large yaw error
if (YawCorrectionActivated)
{
YawCorrectionInProgress = true;
int sign = (YawErrorAngle > 0.0f) ? 1 : -1;
// Incrementally "unyaw" by a small step size
Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * sign) * Q;
}
}
}
//----------------------------------------------------------------------------
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;
}
void Particle::Render(Camera* camera, unsigned int i, bool drawAxes)
{
switch( this->_type )
{
case BillBoard:
break;
case VectorAligned:
break;
case ScreenFacing:
default:
{
Vector3f particleToCamera = camera->GetPosition()-this->_position;
this->_depth = particleToCamera.Length();
Vector3f cameraYAxis = camera->GetEulerRotation()*Vector3f(sin(this->_rotation), cos(this->_rotation), 0.0f);
Vector3f localX = cameraYAxis.Cross(particleToCamera);
Vector3f localY = particleToCamera.Cross(localX);
localX.Normalize();
localY.Normalize();
// Set blend mode
switch( this->_blendMode )
{
case Subtractive:
glBlendFunc(GL_ONE_MINUS_SRC_ALPHA, GL_ONE);
break;
case Additive:
glBlendFunc(GL_SRC_ALPHA, GL_ONE);
break;
case Normal:
default:
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
break;
}
// Evaluate animators
float t = 1.0f-this->_life/this->_maxLife;
Vector2f size = this->_size->GetValue(t);
// Bind texture
if( this->_texture )
{
glBindTexture(GL_TEXTURE_2D, this->_texture->GetTextureName());
}
// Render
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glBegin(GL_QUADS);
glColor4f(this->_color.r, this->_color.g, this->_color.b, this->_color.a);
// TODO (Viet Nguyen): Get rid of texture coordinate hacks!!!!
glTexCoord2f(0.0f, 0.0f);
glVertex3fv((float*)
(this->_position+(-localX)*size.width+(-localY)*size.height));
glTexCoord2f(1.0f/4.0f/*!!!!*/, 0.0f);
glVertex3fv((float*)
(this->_position+localX*size.width+(-localY)*size.height));
glTexCoord2f(1.0f/4.0f/*!!!!*/, 1.0f);
glVertex3fv((float*)
(this->_position+localX*size.width+localY*size.height));
glTexCoord2f(0.0f, 1.0f);
glVertex3fv((float*)
(this->_position+(-localX)*size.width+localY*size.height));
glEnd();
// Unbind texture
if( this->_texture)
{
glBindTexture(GL_TEXTURE_2D, 0);
}
if( drawAxes )
{
glBegin(GL_LINES);
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
glVertex3fv((float*)(this->_position));
glVertex3fv((float*)(this->_position+localX*size.width));
glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
glVertex3fv((float*)(this->_position));
glVertex3fv((float*)(this->_position+localY*size.height));
glEnd();
}
}
}
}
void
KickEffector::PrePhysicsUpdateInternal(float /*deltaTime*/)
{
// this should also include the case when there is no ball
// (because then there will be no body, neither).
if (mAction.get() == 0 || mBallBody.get() == 0)
{
return;
}
if (mAgent.get() == 0)
{
GetLog()->Error()
<< "ERROR: (KickEffector) parent node is not derived from BaseNode\n";
return;
}
shared_ptr<KickAction> kickAction =
shared_dynamic_cast<KickAction>(mAction);
mAction.reset();
if (kickAction.get() == 0)
{
GetLog()->Error()
<< "ERROR: (KickEffector) cannot realize an unknown ActionObject\n";
return;
}
// if the agent doesn't have a body, we're done (this should never happen)
if (mBall.get() == 0) return;
Vector3f force =
mBallBody->GetWorldTransform().Pos() -
mAgent->GetWorldTransform().Pos();
// the ball can be kicked if the distance is
// less then Ball-Radius + Player-Radius + KickMargin AND
// the player is close to the ground
if (mAgent->GetWorldTransform().Pos().z() > mPlayerRadius + 0.01 ||
force.Length() > mPlayerRadius + mBallRadius + mKickMargin)
{
// ball is out of reach, or player is in the air:
// kick has no effect
return;
}
// get the kick angle in the horizontal plane
double theta = salt::gArcTan2(force[1], force[0]);
if (mThetaErrorRNG.get() != 0)
{
theta += (*(mThetaErrorRNG.get()))();
}
float phi = salt::gMin(salt::gMax(kickAction->GetAngle(), mMinAngle), mMaxAngle);
if (mSigmaPhiEnd > 0.0 || mSigmaPhiMid > 0.0)
{
// f will be close to 0.0 if the angle is near the minimum or the maximum.
// f will be close to 1.0 if the angle is somewhere in the middle of the range.
float f = 1.0 - 2.0 * salt::gAbs((phi - mMinAngle) / (mMaxAngle - mMinAngle) - 0.5);
// f is set to a number between mSigmaPhiEnd and mSigmaPhiMid
f = salt::gMax(mSigmaPhiEnd + f * (mSigmaPhiMid-mSigmaPhiEnd), 0.0);
phi = salt::NormalRNG<>(phi,f)();
}
phi = salt::gDegToRad(90.0-phi);
// x = r * cos(theta) * sin(90 - phi), with r = 1.0
force[0] = salt::gCos(theta) * salt::gSin(phi);
// y = r * sin(theta) * sin(90 - phi), with r = 1.0
force[1] = salt::gSin(theta) * salt::gSin(phi);
// z = r * cos(90 - phi), with r = 1.0
force[2] = salt::gCos(phi);
float kick_power = salt::gMin(salt::gMax(kickAction->GetPower(), 1.0f), mMaxPower);
if (mForceErrorRNG.get() != 0)
{
kick_power += (*(mForceErrorRNG.get()))();
}
force *= (mForceFactor * kick_power);
const Vector3f torque(-mTorqueFactor*force[1]/salt::g2PI,
mTorqueFactor*force[0]/salt::g2PI,
0.0);
mBall->SetAcceleration(mSteps,force,torque,mAgent);
mBallStateAspect->UpdateLastKickingAgent(mAgent);
}
void CRigidBody::DeliverCollisionMessage(CCollisionMessage* ColMsg)
{
if (!ColMsg) return;
if (ColMsg->GetCollisionType() == SPHERE_TO_SPHERE) {
HandleSphereToSphereCollision(ColMsg);
return;
}
/****** Deploy to HandlePushCollision() if this is the case ******/
// Push collision not working. Commenting this out for now.
/* if (ColMsg->GetCollisionType() == PUSHED) {
HandlePushCollision(ColMsg);
return;
}
*/
CVehicle* Car = (CVehicle*)ColMsg->GetEntity();
/************** set reflection motion ************/
m_vPosition = m_translate + (*ColMsg->GetReverse())*RIGIDBODY_SPACING_FACTOR;
Vector3f velocityWC;
if (disturbed) {
velocityWC = m_vDirectionWhenDisturbed*80.0f;
}
else {
velocityWC = Car->GetVehicleVelocityWC();
// if reversing, velocityWC will point in opposite direction as actual velocity
if (Car->GetVehicleVelocityLC().X() < 0.0f)
velocityWC *= -1.0f;
}
m_vReflection = velocityWC - 2.0f*(velocityWC.Dot(*ColMsg->GetNormal()))*(*ColMsg->GetNormal());
m_vReflection *= RIGIDBODY_REFLECTION_FACTOR;
m_vDirectionWhenDisturbed = m_vReflection;
Car->SetVehicleVelocityLC(0.0f);
Car->SetVehiclePositionLC(Vector3f(m_vPosition.X(), m_vPosition.Z(), m_vPosition.Y()));
/*************** set angular velocity ******************/
// Compute angle between vehicle heading and plane
Vector3f PlaneEdge = ColMsg->GetPlane()->Edge0();
Vector3f CarHeading = Car->GetVehicleHeadingWC();
float angle = acos(CarHeading.Dot(PlaneEdge)/(PlaneEdge.Length()*CarHeading.Length()));
// spin CW or CCW?
float spin; // magnitude of spin about the y-axis
if (angle > Math<float>::PI/2.0f) { // CCW
spin = Math<float>::PI - angle;
}
else { // CW
spin = -angle;
}
spin *= RIGIDBODY_SPIN_FACTOR;
m_vRotation = Vector3f(0.0f, spin, 0.0f);
/*************** set vectors that are actually used in renderer ************/
/*********************** to the values computed above **********************/
m_translate = m_vPosition;
m_box.Center() = m_vPosition;
m_sphere.Center() = m_vPosition;
Car->GetRotationLC().Z() += spin;
disturbed = true;
/*
CLog::GetLog().Write(LOG_MISC, "\n\n\nreflection = (%f, %f, %f)",
m_vReflection.X(), m_vReflection.Y(), m_vReflection.Z());
CLog::GetLog().Write(LOG_MISC, "direction = (%f, %f, %f)\n\n",
m_vDirectionWhenDisturbed.X(), m_vDirectionWhenDisturbed.Y(), m_vDirectionWhenDisturbed.Z());
*/
}
F32 Vector3f::Length(const Vector3f& vec) {
return vec.Length();
}
//----------------------------------------------------------------------------
void BouncingSpheres::DoCollisionDetection ()
{
mBoundaryContacts.clear();
// Collisions with boundaries.
Contact contact;
int i;
for (i = 0; i < NUM_BALLS; ++i)
{
Vector3f position = mBalls[i]->GetPosition();
float radius = mBalls[i]->GetRadius();
mBalls[i]->Moved = false;
mBlocked[i].clear();
// These checks are done in pairs under the assumption that the ball
// radii are smaller than the separation of opposite boundaries, hence
// only one of each opposite pair of boundaries may be touched at any
// time.
// rear[0] and front[5] boundaries
if (position.X() < mBoundaryLocations[0].X() + radius)
{
SetBoundaryContact(i, 0, position, radius, contact);
}
else if (position.X() > mBoundaryLocations[5].X() - radius)
{
SetBoundaryContact(i, 5, position, radius, contact);
}
// left[1] and right[3] boundaries
if (position.Y() < mBoundaryLocations[1].Y() + radius)
{
SetBoundaryContact(i, 1, position, radius, contact);
}
else if (position.Y() > mBoundaryLocations[3].Y() - radius)
{
SetBoundaryContact(i, 3, position, radius, contact);
}
// bottom[2] and top[4] boundaries
if (position.Z() < mBoundaryLocations[2].Z() + radius)
{
SetBoundaryContact(i, 2, position, radius, contact);
}
else if (position.Z() > mBoundaryLocations[4].Z() - radius)
{
SetBoundaryContact(i, 4, position, radius, contact);
}
}
// Collisions between balls.
for (i = 0; i < NUM_BALLS-1; ++i)
{
for (int j = i + 1; j < NUM_BALLS; ++j)
{
Vector3f diff =
mBalls[j]->GetPosition() - mBalls[i]->GetPosition();
float diffLen = diff.Length();
float radiusI = mBalls[i]->GetRadius();
float radiusJ = mBalls[j]->GetRadius();
float magnitude = diffLen - radiusI - radiusJ;
if (magnitude < 0.0f)
{
contact.A = mBalls[i];
contact.B = mBalls[j];
contact.N = diff/diffLen;
Vector3f deltaPos = magnitude*contact.N;
if (mBalls[i]->Moved && !mBalls[j]->Moved)
{
// Ball i moved but ball j did not.
mBalls[j]->Position() -= deltaPos;
}
else if (!mBalls[i]->Moved && mBalls[j]->Moved)
{
// Ball j moved but ball i did not.
mBalls[i]->Position() += deltaPos;
}
else
{
// Neither ball moved or both balls moved already.
deltaPos *= 0.5f;
mBalls[j]->Position() -= deltaPos;
mBalls[i]->Position() += deltaPos;
}
contact.P = mBalls[i]->Position() + radiusI*contact.N;
mBoundaryContacts.push_back(contact);
}
}
}
mNumContacts = (int)mBoundaryContacts.size();
}
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);
}
//----------------------------------------------------------------------------
void GelatinBlob::CreateSprings ()
{
// The icosahedron has 12 vertices and 30 edges. Each vertex is a
// particle in the system. Each edge represents a spring. To keep the
// icosahedron from collapsing, 12 immovable particles are added, each
// outside the icosahedron in the normal direction above a vertex. The
// immovable particles are connected to their corresponding vertices with
// springs.
int iNumParticles = 24, iNumSprings = 42;
// Viscous forces applied. If you set viscosity to zero, the cuboid
// wiggles indefinitely since there is no dissipation of energy. If
// the viscosity is set to a positive value, the oscillations eventually
// stop. The length of time to steady state is inversely proportional
// to the viscosity.
#ifdef _DEBUG
float fStep = 0.1f;
#else
float fStep = 0.01f; // simulation needs to run slower in release mode
#endif
float fViscosity = 0.01f;
m_pkModule = new PhysicsModule(iNumParticles,iNumSprings,fStep,
fViscosity);
// Set positions and velocities. The first 12 positions are the vertices
// of the icosahedron. The last 12 are the extra particles added to
// stabilize the system.
const Vector3f* akVertex = m_spkSphere->Vertices();
int i;
for (i = 0; i < 12; i++)
{
m_pkModule->SetMass(i,1.0f);
m_pkModule->Position(i) = akVertex[i];
m_pkModule->Velocity(i) = 0.1f*Vector3f(Mathf::SymmetricRandom(),
Mathf::SymmetricRandom(),Mathf::SymmetricRandom());
}
for (i = 12; i < 24; i++)
{
m_pkModule->SetMass(i,Mathf::MAX_REAL);
m_pkModule->Position(i) = 2.0f*akVertex[i-12];
m_pkModule->Velocity(i) = Vector3f::ZERO;
}
// get unique set of edges for icosahedron
set<EdgeKey> kEdge;
int iTQuantity = m_spkSphere->GetTriangleQuantity();
const int* piConnect = m_spkSphere->Connectivity();
for (i = 0; i < iTQuantity; i++)
{
int iV0 = *piConnect++;
int iV1 = *piConnect++;
int iV2 = *piConnect++;
kEdge.insert(EdgeKey(iV0,iV1));
kEdge.insert(EdgeKey(iV1,iV2));
kEdge.insert(EdgeKey(iV2,iV0));
}
// springs are at rest in the initial configuration
const float fConstant = 10.0f;
Vector3f kDiff;
int iSpring = 0;
set<EdgeKey>::iterator pkIter;
for (pkIter = kEdge.begin(); pkIter != kEdge.end(); pkIter++)
{
const EdgeKey& rkKey = *pkIter;
int iV0 = rkKey.V[0], iV1 = rkKey.V[1];
kDiff = m_pkModule->Position(iV1) - m_pkModule->Position(iV0);
m_pkModule->SetSpring(iSpring,iV0,iV1,fConstant,kDiff.Length());
iSpring++;
}
for (i = 0; i < 12; i++)
{
kDiff = m_pkModule->Position(i+12) - m_pkModule->Position(i);
m_pkModule->SetSpring(iSpring,i,i+12,fConstant,kDiff.Length());
iSpring++;
}
}
//----------------------------------------------------------------------------
void GelatinCube::CreateSprings ()
{
// The inner 4-by-4-by-4 particles are used as the control points of a
// B-spline volume. The outer layer of particles are immovable to
// prevent the cuboid from collapsing into itself.
int numSlices = 6;
int numRows = 6;
int numCols = 6;
// Viscous forces applied. If you set viscosity to zero, the cuboid
// wiggles indefinitely since there is no dissipation of energy. If
// the viscosity is set to a positive value, the oscillations eventually
// stop. The length of time to steady state is inversely proportional
// to the viscosity.
#ifdef _DEBUG
float step = 0.1f;
#else
float step = 0.01f; // simulation needs to run slower in release mode
#endif
float viscosity = 0.01f;
mModule = new0 PhysicsModule(numSlices, numRows, numCols, step,
viscosity);
// The initial cuboid is axis-aligned. The outer shell is immovable.
// All other masses are constant.
float sFactor = 1.0f/(float)(numSlices - 1);
float rFactor = 1.0f/(float)(numRows - 1);
float cFactor = 1.0f/(float)(numCols - 1);
int s, r, c;
for (s = 0; s < numSlices; ++s)
{
for (r = 0; r < numRows; ++r)
{
for (c = 0; c < numCols; ++c)
{
mModule->Position(s, r, c) =
Vector3f(c*cFactor, r*rFactor, s*sFactor);
if (1 <= s && s < numSlices - 1
&& 1 <= r && r < numRows - 1
&& 1 <= c && c < numCols - 1)
{
mModule->SetMass(s, r, c, 1.0f);
mModule->Velocity(s, r, c) = 0.1f*Vector3f(
Mathf::SymmetricRandom(), Mathf::SymmetricRandom(),
Mathf::SymmetricRandom());
}
else
{
mModule->SetMass(s, r, c, Mathf::MAX_REAL);
mModule->Velocity(s, r, c) = Vector3f::ZERO;
}
}
}
}
// Springs are at rest in the initial configuration.
const float constant = 10.0f;
Vector3f diff;
for (s = 0; s < numSlices - 1; ++s)
{
for (r = 0; r < numRows; ++r)
{
for (c = 0; c < numCols; ++c)
{
mModule->ConstantS(s, r, c) = constant;
diff = mModule->Position(s + 1, r, c) -
mModule->Position(s, r, c);
mModule->LengthS(s, r, c) = diff.Length();
}
}
}
for (s = 0; s < numSlices; ++s)
{
for (r = 0; r < numRows - 1; ++r)
{
for (c = 0; c < numCols; ++c)
{
mModule->ConstantR(s, r, c) = constant;
diff = mModule->Position(s, r + 1, c) -
mModule->Position(s, r, c);
mModule->LengthR(s, r, c) = diff.Length();
}
}
}
for (s = 0; s < numSlices; ++s)
{
for (r = 0; r < numRows; ++r)
{
for (c = 0; c < numCols - 1; ++c)
{
mModule->ConstantC(s, r, c) = constant;
diff = mModule->Position(s, r, c + 1) -
mModule->Position(s, r, c);
mModule->LengthC(s, r, c) = diff.Length();
}
}
}
}
/// Returns false if the colliding entities are no longer in contact after resolution.
bool FirstPersonCR::ResolveCollision(Collision & c)
{
Entity * dynamic;
Entity * other;
if (c.one->physics->type == PhysicsType::DYNAMIC)
{
dynamic = c.one;
other = c.two;
}
else
{
dynamic = c.two;
other = c.one;
}
if (c.one->physics->noCollisionResolutions || c.two->physics->noCollisionResolutions)
return false;
// Retardation?
// if (dynamic->physics->lastCollisionMs + dynamic->physics->minCollisionIntervalMs > physicsNowMs)
// return false;
dynamic->physics->lastCollisionMs = physicsNowMs;
Entity * dynamic2 = (other->physics->type == PhysicsType::DYNAMIC) ? other : NULL;
Entity * staticEntity;
Entity * kinematicEntity;
if (dynamic == c.one)
other = c.two;
else
other = c.one;
if (!dynamic2)
{
staticEntity = other->physics->type == PhysicsType::STATIC? other : NULL;
kinematicEntity = other->physics->type == PhysicsType::KINEMATIC? other : NULL;
}
// std::cout<<"\nCollision: "<<c.one->name<<" "<<c.two->name;
// Collision..!
// Static-Dynamic collision.
if (!dynamic2)
{
PhysicsProperty * pp = dynamic->physics;
// Skip? No.
// if (kinematicEntity)
// return true;
/// Flip normal if dynamic is two.
if (dynamic == c.two)
c.collisionNormal *= -1;
if (c.collisionNormal.y > 0.8f)
pp->lastGroundCollisionMs = physicsNowMs;
// Default plane? reflect velocity upward?
// Reflect based on the normal.
float velDotNormal = pp->velocity.DotProduct(c.collisionNormal);
/// This will be used to reflect it.
Vector3f velInNormalDir = c.collisionNormal * velDotNormal;
Vector3f velInTangent = pp->velocity - velInNormalDir;
Vector3f newVel = velInTangent * (1 - pp->friction) + velInNormalDir * (-pp->restitution);
/// Apply resitution and stuffs.
dynamic->physics->velocity = newVel;
assert(dynamic->parent == 0);
/// Adjusting local position may not help if child entity.
// Old code
Vector3f moveVec = AbsoluteValue(c.distanceIntoEachOther) * c.collisionNormal;
// if (moveVec.x || moveVec.z)
// std::cout<<"\nMoveVec: "<<moveVec;
dynamic->localPosition += moveVec;
/// For double-surface collision resolution (not bouncing through walls..)
/// For the previously backwards-collisions.
// if (c.distanceIntoEachOther < 0)
// dynamic->localPosition += c.distanceIntoEachOther * c.collisionNormal;
// else // Old code
// dynamic->localPosition += AbsoluteValue(c.distanceIntoEachOther) * c.collisionNormal;
/// If below threshold, sleep it.
if (dynamic->physics->velocity.Length() < inRestThreshold && c.collisionNormal.y > 0.8f)
{
// Sleep eeet.
dynamic->physics->state |= CollisionState::IN_REST;
// Nullify velocity.
dynamic->physics->velocity = Vector3f();
}
if (debug == 7)
{
std::cout<<"\nCollision resolution: "<<(c.one->name+" "+c.two->name+" ")<<c.collisionNormal<<" onePos"<<c.one->worldPosition<<" twoPos"<<c.two->worldPosition;
}
}
// Dynamic-dynamic collision.
else
{
// std::cout<<"\nProblem?" ;
/// Push both apart?
PhysicsProperty * pp = dynamic->physics, * pp2 = dynamic2->physics;
/// Flip normal if dynamic is two.
// See if general velocities align with one or the other? or...
// if (dynamic == c.two)
;// c.collisionNormal *= -1;
// std::cout<<"\nNormal: "<<c.collisionNormal;
// Default plane? reflect velocity upward?
/// This will be used to reflect it.
Vector3f velInNormalDir = c.collisionNormal * pp->velocity.DotProduct(c.collisionNormal);
Vector3f velInNormalDir2 = c.collisionNormal * pp2->velocity.DotProduct(c.collisionNormal);
Vector3f velInTangent = pp->velocity - velInNormalDir,
velInTangent2 = pp2->velocity - velInNormalDir2;
Vector3f velInTangentTot = velInTangent + velInTangent2,
velInNormalDirTot = velInNormalDir + velInNormalDir2;
/// Use lower value restitution of the two?
float restitution = 0.5f; // MinimumFloat(pp->restitution, pp2->restitution);
Vector3f normalDirPart = velInNormalDirTot * restitution * 0.5f;
Vector3f to2 = (dynamic2->worldPosition - dynamic->worldPosition).NormalizedCopy();
float partTo2 = normalDirPart.DotProduct(to2);
// std::cout<<" normalDirPart: "<<normalDirPart;
float normalDirPartVal = normalDirPart.Length();
Vector3f fromCollisionToDyn1 = (dynamic->worldPosition - c.collissionPoint).NormalizedCopy();
Vector3f fromCollisionToDyn2 = (dynamic2->worldPosition - c.collissionPoint).NormalizedCopy();
pp->velocity = velInTangent * (1 - pp->friction) + fromCollisionToDyn1 * normalDirPartVal;
pp2->velocity = velInTangent2 * (1 - pp->friction) + fromCollisionToDyn2 * normalDirPartVal;
/// Apply resitution and stuffs.
assert(dynamic->parent == 0);
/// Adjusting local position may not help if child entity.
// Old code
float distanceIntoAbsH = AbsoluteValue(c.distanceIntoEachOther * 0.5f);
dynamic->localPosition += distanceIntoAbsH * fromCollisionToDyn1;
dynamic2->localPosition += distanceIntoAbsH * fromCollisionToDyn2;
/// For double-surface collision resolution (not bouncing through walls..)
/// If below threshold, sleep it.
if (dynamic->physics->velocity.Length() < inRestThreshold && c.collisionNormal.y > 0.8f)
{
pp->Sleep();
}
else
pp->Activate();
if (pp2->velocity.Length() < inRestThreshold && c.collisionNormal.y > 0.8f)
{
pp2->Sleep();
}
else
pp2->Activate();
if (debug == 7)
{
std::cout<<"\nCollision resolution: "<<(c.one->name+" "+c.two->name+" ")<<c.collisionNormal<<" onePos"<<c.one->worldPosition<<" twoPos"<<c.two->worldPosition;
}
}
// Check collision normal.
return true;
}
/// Query if it has reached next waypoint. Just compares distance with the proximity threshold.
bool PathableProperty::HasReachedNextWaypoint()
{
Vector3f vecDist = nextWaypoint->position - owner->worldPosition;
float distance = vecDist.Length();
return distance < proximityThreshold;
}