void GLCamera::Move(float dx, float dy, float dz){
//if(m_hold)
// m_ref = m_holdRef;
Vector3 shift;
Vector3 absShift;
Vector3 relPos;
Vector3 up(0,0,1);
switch(m_Mode){
case CAMMODE_FreeMove:
shift.Set(0,0,dz);
shift *= 1/50.0;
m_orient.Mult(shift,absShift);
relPos = m_lookAtPoint - m_position;
if((relPos+absShift).Dot(m_orient.GetColumn(2))<-1.0){
m_lookAtPoint += absShift;
}else{
m_lookAtPoint = m_position - Vector3(relPos.Norm()*m_orient(0,2), relPos.Norm()*m_orient(1,2), relPos.Norm()*m_orient(2,2));
}
shift.Set(dx,dy,0);
shift *= 1/50.0;
m_orient.Mult(shift,absShift);
m_lookAtPoint += absShift;
relPos = m_position-m_lookAtPoint;
m_orient.SetColumn(relPos,2);
m_orient.SetColumn(up,1);
m_orient.Normalize();
break;
case CAMMODE_Centered:
relPos = m_position-m_lookAtPoint;
float norm = relPos.Norm();
float origNorm = norm;
shift.Set(dx,dy,0);
shift *= norm /100.0;
m_orient.Mult(shift,absShift);
relPos += absShift;
if(dz!=0.0){
norm += 0.05*dz/norm;
norm = MAX(m_closeupDistance,fabs(norm));
relPos *= norm/origNorm;
}else{
relPos *= origNorm/relPos.Norm();
}
//m_position *= norm/origNorm;
m_position = m_lookAtPoint + relPos;
m_orient.SetColumn(relPos,2);
m_orient.SetColumn(up,1);
m_orient.Normalize();
break;
}
}
bool CDSExecution::syncWorldWithModel()
{
// Compute the mTargetFrame with respect to the Attractor frame and Object Frame
mObjectFrame.Mult(mAttractorFrame, mTargetFrame);
// Compute Robot EE frame w.r.t target reference frame
// vRobotEEPosInTargetFrame = mTargetFrame.InverseTransformation().Transform(vRobotEEAbsolutePos); // ==>> Does not work like this due to bug in MathLib
vRobotEEPosInTargetFrame =
mTargetFrame.GetOrientation().Transpose()*(vRobotEEAbsolutePos - mTargetFrame.GetTranslation());
if(vRobotEEPosInTargetFrame.Norm() > reachingThr)
{
//Set GMR states
cdsCtrl->SetMasterRelativeState(vRobotEEPosInTargetFrame.Array());
// Computes the orientation of the EE in the target frame
Matrix3 mTmpMat3;
mTargetFrame.GetOrientation().Transpose().Mult(mRobotEEAbsoluteOrient, mTmpMat3);
vRobotEEOrientInTargetFrame = mTmpMat3.GetExactRotationAxis();
//vTmpOrient = mRobotEEAbsoluteOrient.GetExactRotationAxis();
//mTargetFrame.GetOrientation().Transpose().Mult(mRobotEEAbsoluteOrient, mTmpMat3);
Vector3 vTmpOrient = vRobotEEOrientInTargetFrame;
float tmpTheta;
tmpTheta = vTmpOrient.Norm();
if (tmpTheta > 0.001)
vTmpOrient = vTmpOrient/tmpTheta;
Vector vTmpSlaveState(4);
vTmpSlaveState(0) = vTmpOrient(0);
vTmpSlaveState(1) = vTmpOrient(1);
vTmpSlaveState(2) = vTmpOrient(2);
vTmpSlaveState(3) = tmpTheta;
cdsCtrl->SetSlaveRelativeState(vTmpSlaveState.Array());
vTargetOrientAA = mAttractorFrame.GetOrientation().GetExactRotationAxis();
vTmpOrient = vTargetOrientAA;
tmpTheta = vTmpOrient.Norm();
if(fabs(tmpTheta) < 1e-5) {
vTmpSlaveState.Zero();
} else {
vTmpOrient = vTmpOrient/tmpTheta;
vTmpSlaveState(0) = vTmpOrient(0);
vTmpSlaveState(1) = vTmpOrient(1);
vTmpSlaveState(2) = vTmpOrient(2);
vTmpSlaveState(3) = tmpTheta;
}
cdsCtrl->SetSlaveTarget(vTmpSlaveState);
}
return true;
}
bool InternalPhysics::TestSphereSphereCollision(Sphere * aA,Sphere * aB)
{
// Getting the distance between the centers
Vector3 distance = aB->GetNewPosition() - aA->GetNewPosition();
float norme = distance.Norm();
// If the distance is less that the sum of the 2 radius so there is intersection
return ( norme <= aA->GetRadius() + aB->GetRadius() );
// Else , there isn't
}
/*
* http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
Remember to use consistent orientation
*/
std::pair<float, bool> ImplicitMesh::DistanceSquared(const Vector3<float> & p,
const Vector3<float> &v1,
const Vector3<float> &v2,
const Vector3<float> &v3){
Vector3<float> kDiff = v1 - p;
Vector3<float> kEdge0 = v2 - v1;
Vector3<float> kEdge1 = v3 - v1;
float fA00 = kEdge0.Norm();
float fA01 = kEdge0*kEdge1;
float fA11 = kEdge1.Norm();
float fB0 = kDiff*kEdge0;
float fB1 = kDiff*kEdge1;
float fC = kDiff.Norm();
float fDet = std::abs(fA00*fA11-fA01*fA01);
float fS = fA01*fB1-fA11*fB0;
float fT = fA01*fB0-fA00*fB1;
float fSqrDistance;
if (fS + fT <= fDet)
{
if (fS < 0.0)
{
if (fT < 0.0) // region 4
{
if (fB0 < 0.0)
{
fT = 0.0;
if (-fB0 >= fA00)
{
fS = 1.0;
fSqrDistance = fA00+(2.0)*fB0+fC;
}
else
{
fS = -fB0/fA00;
fSqrDistance = fB0*fS+fC;
}
}
else
{
fS = 0.0;
if (fB1 >= 0.0)
{
fT = 0.0;
fSqrDistance = fC;
}
else if (-fB1 >= fA11)
{
fT = 1.0;
fSqrDistance = fA11+(2.0)*fB1+fC;
}
else
{
fT = -fB1/fA11;
fSqrDistance = fB1*fT+fC;
}
}
}
else // region 3
{
fS = 0.0;
if (fB1 >= 0.0)
{
fT = 0.0;
fSqrDistance = fC;
}
else if (-fB1 >= fA11)
{
fT = 1.0;
fSqrDistance = fA11+(2.0)*fB1+fC;
}
else
{
fT = -fB1/fA11;
fSqrDistance = fB1*fT+fC;
}
}
}
else if (fT < 0.0) // region 5
{
fT = 0.0;
if (fB0 >= 0.0)
{
fS = 0.0;
fSqrDistance = fC;
}
else if (-fB0 >= fA00)
{
fS = 1.0;
fSqrDistance = fA00+(2.0)*fB0+fC;
}
else
{
fS = -fB0/fA00;
fSqrDistance = fB0*fS+fC;
}
}
else // region 0
{
// minimum at interior point
float fInvDet = (1.0)/fDet;
fS *= fInvDet;
fT *= fInvDet;
fSqrDistance = fS*(fA00*fS+fA01*fT+(2.0)*fB0) +
fT*(fA01*fS+fA11*fT+(2.0)*fB1)+fC;
}
}
else
{
float fTmp0, fTmp1, fNumer, fDenom;
if (fS < 0.0) // region 2
{
fTmp0 = fA01 + fB0;
fTmp1 = fA11 + fB1;
if (fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if (fNumer >= fDenom)
{
fS = 1.0;
fT = 0.0;
fSqrDistance = fA00+(2.0)*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0 - fS;
fSqrDistance = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
fT*(fA01*fS+fA11*fT+(2.0)*fB1)+fC;
}
}
else
{
fS = 0.0;
if (fTmp1 <= 0.0)
{
fT = 1.0;
fSqrDistance = fA11+(2.0)*fB1+fC;
}
else if (fB1 >= 0.0)
{
fT = 0.0;
fSqrDistance = fC;
}
else
{
fT = -fB1/fA11;
fSqrDistance = fB1*fT+fC;
}
}
}
else if (fT < 0.0) // region 6
{
fTmp0 = fA01 + fB1;
fTmp1 = fA00 + fB0;
if (fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-(2.0)*fA01+fA11;
if (fNumer >= fDenom)
{
fT = 1.0;
fS = 0.0;
fSqrDistance = fA11+(2.0)*fB1+fC;
}
else
{
fT = fNumer/fDenom;
fS = 1.0 - fT;
fSqrDistance = fS*(fA00*fS+fA01*fT+(2.0)*fB0) +
fT*(fA01*fS+fA11*fT+(2.0)*fB1)+fC;
}
}
else
{
fT = 0.0;
if (fTmp1 <= 0.0)
{
fS = 1.0;
fSqrDistance = fA00+(2.0)*fB0+fC;
}
else if (fB0 >= 0.0)
{
fS = 0.0;
fSqrDistance = fC;
}
else
{
fS = -fB0/fA00;
fSqrDistance = fB0*fS+fC;
}
}
}
else // region 1
{
fNumer = fA11 + fB1 - fA01 - fB0;
if (fNumer <= 0.0)
{
fS = 0.0;
fT = 1.0;
fSqrDistance = fA11+(2.0)*fB1+fC;
}
else
{
fDenom = fA00-2.0f*fA01+fA11;
if (fNumer >= fDenom)
{
fS = 1.0;
fT = 0.0;
fSqrDistance = fA00+(2.0)*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0 - fS;
fSqrDistance = fS*(fA00*fS+fA01*fT+(2.0)*fB0) +
fT*(fA01*fS+fA11*fT+(2.0)*fB1)+fC;
}
}
}
}
// account for numerical round-off error
if (fSqrDistance < 0.0)
{
fSqrDistance = 0.0;
}
// Could be used for fun stuff...
// Vector3<float> closest = v1 + fS*kEdge0 + fT*kEdge1;
// float u = fs,v fT, w = std::max<float>(0.0, 1-u-v);
bool outside = true;
if (kDiff*(Cross(kEdge0,kEdge1)) < 0)
outside = false;
return std::make_pair(fSqrDistance, outside);
}
void InternalPhysics::ManageSphereSphereCollision(Sphere* A, Sphere* B)
{
// Local variable initialisation
Vector3 VelocityA,VelocityB;
// Getting masses
double AMass = A->GetMass();
double BMass = B->GetMass();
// Getting the distance between the two spheres
Vector3 centerDistanceVector = B->GetNewPosition() - A->GetNewPosition();
float norme = centerDistanceVector.Norm();
// Getting the collision unit vector
centerDistanceVector.Normalize();
// We are testing if this collision has already been managed
// For A
if(A->IsInCollision() == true)
{
VelocityA=A->GetCollisionVelocity();
}
else
{
VelocityA=A->GetNewVelocity();
}
//For B
if(B->IsInCollision() == true)
{
VelocityB=B->GetCollisionVelocity();
}
else
{
VelocityB=B->GetNewVelocity();
}
//Speed Collision managing
// For A , pre-collision speed
Vector3 vAAfterx = (centerDistanceVector * VelocityA) * centerDistanceVector;
Vector3 vAAftery = VelocityA - vAAfterx;
// For B , pre-collision speed
Vector3 vBAfterx = (-centerDistanceVector * VelocityB) * (-centerDistanceVector);
Vector3 vBAftery = VelocityB - vBAfterx;
//Applying collision thorem
Vector3 VAx = ( (vAAfterx*AMass) + (vBAfterx*BMass) - (vAAfterx-vBAfterx)*BMass )/(AMass+BMass);
Vector3 VAy = vAAftery;
Vector3 VBx = ( (vAAfterx*AMass) + (vBAfterx*BMass) - (vBAfterx-vAAfterx)*AMass )/(AMass+BMass);
Vector3 VBy = vBAftery;
Vector3 vAAfter = VAx+VAy;
Vector3 vBAfter = VBx+VBy;
// Reducing the speed by a factor 0.9, it simulates energy loss
vAAfter= 0.9*vAAfter;
vBAfter=0.9*vBAfter;
// Adding a reaction force when the balls touches the ground
// For A
Vector3 verticalForce( A->GetConstantForces() );
Vector3 reactionForce = fabs(verticalForce.GetZ()/centerDistanceVector.GetZ())*centerDistanceVector;
Vector3 applicationPoint(A->GetPosition());
Force reaction(reactionForce,reactionForce);
A->AddNewVariableForce( reaction );
// For B
verticalForce= B->GetConstantForces() ;
reactionForce = fabs(verticalForce.GetZ()/centerDistanceVector.GetZ())*centerDistanceVector;
reaction.SetForce(reactionForce);
B->AddNewVariableForce( reaction );
// Changing brutally the speed, the accelerations and the positions and cancelling the collision
//For A
Vector3 NIL(0.0f,0.0f,0.0f);
A->SetNewAcceleration( NIL );
A->SetNewVelocity(vAAfter);
A->SetNewPosition( A->GetPosition() );
A->ResetIsInCollision();
//For B
B->SetNewAcceleration( NIL );
B->SetNewVelocity(vBAfter);
B->SetNewPosition( B->GetPosition() );
B->ResetIsInCollision();
}