void CFloorPowerFunctions::SetPovCamera()
{
m_Renderer = dynamic_cast<CQTOpenGLRender*>(&GetSimulator().GetVisualization());
m_Camera = &m_Renderer->GetMainWindow().GetOpenGLWidget().GetCamera();
m_CameraSettings = &m_Camera->GetActiveSettings();
m_SelectedEntity = dynamic_cast<CEFootBotEntity*>(m_Renderer->GetMainWindow().GetOpenGLWidget().GetSelectedEntity());
if(m_SelectedEntity != NULL){
// get robot position and orientation
CVector3 pos_vec = m_SelectedEntity->GetEmbodiedEntity().GetOriginAnchor().Position;
CQuaternion orientation = m_SelectedEntity->GetEmbodiedEntity().GetOriginAnchor().Orientation;
// get robot angle
CRadians cZAngle, cYAngle, cXAngle;
orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
// calculate camera direction vector
double x = pos_vec.GetX() + cos(cZAngle.GetValue());
double y = pos_vec.GetY() + sin(cZAngle.GetValue());
// fixate X so you don't get tilt
m_CameraSettings->Up.SetX(0);
// set position and viewpoint target
m_CameraSettings->Position.Set((double)(pos_vec.GetX()), (double)(pos_vec.GetY()), POV_HEIGHT);
m_CameraSettings->Target.Set(x, y, POV_HEIGHT);
}
}
CMultibodyLinkEntity::CMultibodyLinkEntity(CMultibodyEntity* parent, const Link &linkDef)
: CComposableEntity(parent, parent->GetId() + "_" + linkDef.name), defaultState(linkDef), currentState(linkDef)
{
// Get our orientation relative to our parent
CRadians roll{linkDef.roll};
CRadians pitch{linkDef.pitch};
CRadians yaw{linkDef.yaw};
// And wrap it in a quaternion
CQuaternion orientation;
orientation.FromEulerAngles(yaw, pitch, roll);
// Get our position relative to our parent
CVector3 pos{linkDef.originX, linkDef.originY, linkDef.originZ};
// Create our embodied entity
embodiedEntity = new CEmbodiedEntity{this, GetId(), pos, orientation};
embodiedEntity->SetEnabled(true);
embodiedEntity->SetMovable(true);
// And add the embodied component
AddComponent(*embodiedEntity);
Reset();
}
void SimRender::ConstructGimbal(const CVector3D& center, float radius, SOverlayLine& out, size_t numSteps)
{
ENSURE(numSteps > 0 && numSteps % 4 == 0); // must be a positive multiple of 4
out.m_Coords.clear();
size_t fullCircleSteps = numSteps;
const float angleIncrement = 2.f*M_PI/fullCircleSteps;
const CVector3D X_UNIT(1, 0, 0);
const CVector3D Y_UNIT(0, 1, 0);
const CVector3D Z_UNIT(0, 0, 1);
CVector3D rotationVector(0, 0, radius); // directional vector based in the center that we will be rotating to get the gimbal points
// first draw a quarter of XZ gimbal; then complete the XY gimbal; then continue the XZ gimbal and finally add the YZ gimbal
// (that way we can keep a single continuous line)
// -- XZ GIMBAL (PART 1/2) -----------------------------------------------
CQuaternion xzRotation;
xzRotation.FromAxisAngle(Y_UNIT, angleIncrement);
for (size_t i = 0; i < fullCircleSteps/4; ++i) // complete only a quarter of the way
{
out.PushCoords(center + rotationVector);
rotationVector = xzRotation.Rotate(rotationVector);
}
// -- XY GIMBAL ----------------------------------------------------------
// now complete the XY gimbal while the XZ gimbal is interrupted
CQuaternion xyRotation;
xyRotation.FromAxisAngle(Z_UNIT, angleIncrement);
for (size_t i = 0; i < fullCircleSteps; ++i) // note the <; the last point of the XY gimbal isn't added, because the XZ gimbal will add it
{
out.PushCoords(center + rotationVector);
rotationVector = xyRotation.Rotate(rotationVector);
}
// -- XZ GIMBAL (PART 2/2) -----------------------------------------------
// resume the XZ gimbal to completion
for (size_t i = fullCircleSteps/4; i < fullCircleSteps; ++i) // exclude the last point of the circle so the YZ gimbal can add it
{
out.PushCoords(center + rotationVector);
rotationVector = xzRotation.Rotate(rotationVector);
}
// -- YZ GIMBAL ----------------------------------------------------------
CQuaternion yzRotation;
yzRotation.FromAxisAngle(X_UNIT, angleIncrement);
for (size_t i = 0; i <= fullCircleSteps; ++i)
{
out.PushCoords(center + rotationVector);
rotationVector = yzRotation.Rotate(rotationVector);
}
}
void
Moose::Spatial::COrientable::RotateLocal( float fRightAngle, float fUpAngle, float fForwardAngle )
{
/* Get the Rotations from LOCAL axis */
CQuaternion qOne; qOne.CreateFromAxisAngle( m_vRight[0],
m_vRight[1],
m_vRight[2],
fRightAngle );
CQuaternion qTwo; qTwo.CreateFromAxisAngle( m_vUpward[0],
m_vUpward[1],
m_vUpward[2],
fUpAngle );
qOne = qTwo * qOne;
qTwo.CreateFromAxisAngle( m_vForward[0],
m_vForward[1],
m_vForward[2],
fForwardAngle );
qOne = qTwo * qOne;
/* Apply the combined rotation to each vector */
RotateAllDirections(qOne);
m_qRotation = qOne * m_qRotation;
SetRotationChanged(1);
}
/**
* Reset this link back to its original state
*/
void CMultibodyLinkEntity::Reset()
{
// Call through to our super reset method
CComposableEntity::Reset();
// And reset
embodiedEntity->Reset();
// Restore our state
currentState = defaultState;
// Get our reset position (relative to our parent)
CVector3 pos{currentState.originX, currentState.originY, currentState.originZ};
// Get our orientation (relative to our parent)
CRadians roll{currentState.roll};
CRadians pitch{currentState.pitch};
CRadians yaw{currentState.yaw};
// And wrap it in a quaternion
CQuaternion orientation;
orientation.FromEulerAngles(yaw, pitch, roll);
// Set our position
embodiedEntity->GetOriginAnchor().Position = pos;
embodiedEntity->GetOriginAnchor().Orientation = orientation;
UpdateComponents();
}
bool CDynamics3DEntity::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_check_only) {
/* Move the body to the new position */
dBodySetPosition(m_tBody, c_position.GetX(), c_position.GetY(), c_position.GetZ());
/* Rotate the body to the new orientation */
dQuaternion tQuat = { c_orientation.GetW(),
c_orientation.GetX(),
c_orientation.GetY(),
c_orientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
/* Check for collisions */
bool bCollisions = m_cEngine.IsEntityColliding(m_tEntitySpace);
if(bCollisions || b_check_only) {
/*
* Undo the changes if there is a collision or
* if the move was just a check
*/
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ());
const CQuaternion& cOrientation = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat2 = { cOrientation.GetW(),
cOrientation.GetX(),
cOrientation.GetY(),
cOrientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat2);
return !bCollisions;
}
else {
/* Set the new position and orientation */
GetEmbodiedEntity().SetPosition(c_position);
GetEmbodiedEntity().SetOrientation(c_orientation);
return true;
}
}
void CMatrix::rotate(float angle, float x, float y, float z)
{
CQuaternion q;
vec3f axis(x,y,z);
q.fromAngleAxis(angle, axis);
q.toMatrix(*this);
}
CVector3 CQuaternion::operator* (const CVector3& v) const {
#if 0
CMatrix4 m;
toRotationMatrix(m);
return m * v;
#else
// nVidia SDK implementation
CVector3 uv, uuv;
CVector3 qvec(x, y, z);
uv = qvec ^ v;
uuv = qvec ^ uv;
uv *= (2.0f * w);
uuv *= 2.0f;
return v + uv + uuv;
#endif
#if 0
CQuaternion thiss = *this;
thiss.invert();
thiss = thiss * CQuaternion(v.x, v.y, v.z, 0.0);
thiss = thiss * *this;
return CVector3(thiss.x, thiss.y, thiss.z);
#endif
}
CQuaternion Bisect(CQuaternion const& a, CQuaternion const& b)
{
CQuaternion result;
result = a+b;
return result.Normalize();
}
//============ Interpolation
CQuaternion CQuaternion::lerp(const CQuaternion &q1, const CQuaternion &q2,
float t)
{
CQuaternion res = (q1 * (1 - t) + q2 * t);
res.Normalize();
return res;
}
void CParticle::Update()
{
float deltaTime = 1.0f / 60.0f;
CVector3 addGrafity = gravity;
addGrafity.Scale(deltaTime);
velocity.Add(addGrafity);
CVector3 force = applyForce;
force.x += (((float)random->GetRandDouble() - 0.5f) * 2.0f) * addVelocityRandomMargih.x;
force.y += (((float)random->GetRandDouble() - 0.5f) * 2.0f) * addVelocityRandomMargih.y;
force.z += (((float)random->GetRandDouble() - 0.5f) * 2.0f) * addVelocityRandomMargih.z;
force.Scale(deltaTime);
velocity.Add(force);
CVector3 addPos = velocity;
addPos.Scale(deltaTime);
applyForce = CVector3::Zero;
position.Add(addPos);
CMatrix mTrans;
mTrans.MakeTranslation(position);
if (isBillboard) {
//ビルボード処理を行う。
const CMatrix& mCameraRot = camera->GetCameraRotation();
CQuaternion qRot;
qRot.SetRotation(CVector3(mCameraRot.m[2][0], mCameraRot.m[2][1], mCameraRot.m[2][2]), rotateZ);
CMatrix rot;
rot.MakeRotationFromQuaternion(qRot);
mWorld.Mul(mCameraRot, rot);
mWorld.Mul(mWorld, mTrans);
}
else {
mWorld = mTrans;
}
timer += deltaTime;
switch (state) {
case eStateRun:
if (timer >= life) {
if (isFade) {
state = eStateFadeOut;
timer = 0.0f;
}
else {
state = eStateDead;
}
}
break;
case eStateFadeOut: {
float t = timer / fadeTIme;
timer += deltaTime;
alpha = initAlpha + (-initAlpha)*t;
if (alpha <= 0.0f) {
alpha = 0.0f;
state = eStateDead; //死亡。
}
}break;
case eStateDead:
GameObjectManager().DeleteGameObject(this);
break;
}
}
// --[ Method ]---------------------------------------------------------------
//
// - Class : CMatrix
//
// - prototype : CMatrix Quaternion()
//
// - Purpose : Returns the equivalent quaternion.
//
// -----------------------------------------------------------------------------
CQuaternion CMatrix::Quaternion() const
{
CQuaternion result;
float fTrace, fS;
fTrace = 1.0f + m_fM[0][0] + m_fM[1][1] + m_fM[2][2];
// Check trace value
if(fTrace > 0.000001f)
{
fS = 0.5f / sqrtf(fTrace);
result.SetW(0.25f / fS);
CVector3 v;
v.SetX((m_fM[2][1] - m_fM[1][2]) * fS);
v.SetY((m_fM[0][2] - m_fM[2][0]) * fS);
v.SetZ((m_fM[1][0] - m_fM[0][1]) * fS);
result.SetV(v);
return result;
}
else
{
float qx, qy, qz, qw;
if(m_fM[0][0] > m_fM[1][1] && m_fM[0][0] > m_fM[2][2])
{
fS = sqrtf(1.0f + m_fM[0][0] - m_fM[1][1] - m_fM[2][2]) * 2.0f;
qx = 0.25f * fS;
qy = (m_fM[0][1] + m_fM[1][0] ) / fS;
qz = (m_fM[0][2] + m_fM[2][0] ) / fS;
qw = (m_fM[1][2] - m_fM[2][1] ) / fS;
}
else if(m_fM[1][1] > m_fM[2][2])
{
fS = sqrt( 1.0 + m_fM[1][1] - m_fM[0][0] - m_fM[2][2] ) * 2;
qx = (m_fM[0][1] + m_fM[1][0] ) / fS;
qy = 0.25f * fS;
qz = (m_fM[1][2] + m_fM[2][1] ) / fS;
qw = (m_fM[0][2] - m_fM[2][0] ) / fS;
}
else
{
fS = sqrt( 1.0 + m_fM[2][2] - m_fM[0][0] - m_fM[1][1] ) * 2;
qx = (m_fM[0][2] + m_fM[2][0] ) / fS;
qy = (m_fM[1][2] + m_fM[2][1] ) / fS;
qz = 0.25f * fS;
qw = (m_fM[0][1] - m_fM[1][0] ) / fS;
}
result.SetW(qw);
result.SetV(CVector3(qx, qy, qz));
}
return result;
}
CQuaternion<T> operator+(const CQuaternion<T> &q)
{
CQuaternion quart = CQuaternion(w+q.w, i+q.i, j+q.j, k+q.k);
if (quart.getLength() != 1) quart.normalize();
return quart;
}
CQuaternion CQuaternion::operator *(const CQuaternion& oQuat) const {
CLog("math","CQuaternion::operator*");
CQuaternion oResult;
oResult.m_dScalar = m_dScalar * oQuat.m_dScalar - m_oVector.Dot(oQuat.m_oVector);
oResult.m_oVector = oQuat.m_oVector.Cross(m_oVector) + (oQuat.m_oVector * m_dScalar) + (m_oVector * oQuat.m_dScalar);
oResult.Normalise();
return oResult;
} //operator *(const CQuaternion& quat) const
inline void onTouchMove(int ix, int iy) {
if (m_spinning) {
vec3 start = MapToSphere(m_fingerStart);
vec3 end = MapToSphere(vec2(ix,iy));
CQuaternion delta = CQuaternion::CreateFromVectors(start, end);
m_orientation = delta.Rotated(m_prevOrientation);
}
}
void
Moose::Spatial::COrientable::RotateAroundZ( float degrees )
{
CQuaternion q; q.CreateFromAxisAngle( 0.0f, 0.0f, 1.0f, degrees );
m_qRotation = q * m_qRotation;
RotateAllDirections( q );
SetRotationChanged(1);
}
CVector3& CVector3::Rotate(const CQuaternion& c_quaternion) {
CQuaternion cResult;
cResult = c_quaternion;
cResult *= CQuaternion(0.0f, m_fX, m_fY, m_fZ);
cResult *= c_quaternion.Inverse();
m_fX = cResult.GetX();
m_fY = cResult.GetY();
m_fZ = cResult.GetZ();
return *this;
}
CMatrix4 CRenderableQueueMember::getInverseTransformMatrix() const {
CMatrix4 m;
CQuaternion q = mOrientation;
q.invert();
q.toRotationMatrix(m);
m.setTransform(-mPosition);
return m;
}
// Return the quaternion result of multiplying two quaternions - non-member function
CQuaternion operator*
(
const CQuaternion& q1,
const CQuaternion& q2
)
{
const CVector3& v1 = q1.Vector();
const CVector3& v2 = q2.Vector();
return CQuaternion( q1.w*q2.w - Dot( v1, v2 ), q1.w*v2 + q2.w*v1 + Cross( v2, v1 ) );
}
/*****
* Return the angle the robot is facing relative to the arena's origin.
*****/
CRadians iAnt_controller::GetHeading() {
/* in ARGoS, the robot's orientation is represented by a quaternion */
const CCI_PositioningSensor::SReading& sReading = compass->GetReading();
CQuaternion orientation = sReading.Orientation;
/* convert the quaternion to euler angles */
CRadians z_angle, y_angle, x_angle;
orientation.ToEulerAngles(z_angle, y_angle, x_angle);
/* the angle to the z-axis represents the compass heading */
return z_angle;
}
int CTraQ::CalcTorso( CTraQ* traqptr, RPSELEM* rpsptr, int frameno, int skipflag )
{
D3DXVECTOR3 befpos, aftpos;
if( (skipflag & 1) == 0 ){
befpos = ( rpsptr + 0 * SKEL_MAX + SKEL_TORSO )->pos;
aftpos = ( rpsptr + frameno * SKEL_MAX + SKEL_TORSO )->pos;
D3DXVECTOR3 befhip, afthip;
befhip = ( rpsptr + 0 * SKEL_MAX + SKEL_LEFT_HIP )->pos - ( rpsptr + 0 * SKEL_MAX + SKEL_RIGHT_HIP )->pos;
afthip = ( rpsptr + frameno * SKEL_MAX + SKEL_LEFT_HIP )->pos - ( rpsptr + frameno * SKEL_MAX + SKEL_RIGHT_HIP )->pos;
befhip.y = 0.0f;
afthip.y = 0.0f;
D3DXVECTOR3 nbefhip, nafthip;
DVec3Normalize( &nbefhip, befhip );
DVec3Normalize( &nafthip, afthip );
double radxz;
D3DXVECTOR3 vDir0, vDir;
D3DXVECTOR3 dirY( 0.0f, 1.0f, 0.0f );
DVec3Cross( &nafthip, &dirY, &vDir0 );
DVec3Normalize( &vDir, vDir0 );
if( vDir.x == 0.0f ){
if( vDir.z >= 0.0f )
radxz = 0.0;
else
radxz = PAI;
}else if( vDir.x > 0.0f ){
radxz = -atanf( vDir.z / vDir.x ) + PAI / 2;
}else{
radxz = -atanf( vDir.z / vDir.x ) - PAI / 2;
}
D3DXVECTOR3 axis( 0.0f, 1.0f, 0.0f );
CQuaternion qxz;
qxz.SetAxisAndRot( axis, radxz );
( traqptr + frameno * SKEL_MAX + SKEL_TORSO )->m_q = qxz;
( traqptr + frameno * SKEL_MAX + SKEL_TORSO )->m_totalq = qxz;
( traqptr + frameno * SKEL_MAX + SKEL_TORSO )->m_cureul = D3DXVECTOR3( 0.0f, (float)( radxz * PAI2DEG ), 0.0f );
}
/////////
if( skipflag != 3 ){
D3DXVECTOR3 difftorso;
difftorso = aftpos - befpos;
( traqptr + frameno * SKEL_MAX + SKEL_TOPOFJOINT )->m_tra = difftorso;
}
return 0;
}
int CMotionPoint::MakeWorldMat( D3DXMATRIX* wmat )
{
m_worldmat = m_totalmat * *wmat;
D3DXQUATERNION tmpxq;
D3DXQuaternionRotationMatrix( &tmpxq, wmat );
CQuaternion wq;
wq.SetParams( tmpxq );
m_worldq = wq * m_totalq;
return 0;
}
void applyAngularRotation(const CVector<3,T> &angular_rotation)
{
CQuaternion n;
T length = angular_rotation.getLength();
if (length <= 0)
{
n.reset();
return;
}
this->rotatePost(angular_rotation * (1.0/length), length);
}
CAxisRotation::CAxisRotation(const CQuaternion & oQuat) {
CLog oLog("math","CAxisRotation::Constructor (CQuaternion)",LL_OBJECT);
double dHalfAngle = acos(oQuat.Scalar());
double dSinHalfAngle = sin(dHalfAngle);
m_dAngle = 2.0F * dHalfAngle;
if (fabs(dSinHalfAngle) == 0) {
CVector3D temp(0,1,0);
m_oAxis = temp;
}
else {
m_oAxis = oQuat.Vector() / dSinHalfAngle;
}
} //CAxisRotation(const CQuaternion & quat)
void CShapeModule::InitParticleForCircle(CVec3& localPos, CVec3& direction, float fParticleScale) const
{
direction = CVec3(1, 0, 0);
CQuaternion quat;
quat.FromPitchYawRoll(0, 0, PARTICLE_RAND_RANGE(0, DegreesToRadians(m_fArcForCircle)));
direction *= quat;
float fRadius = m_fRadius * fParticleScale;
localPos = direction * (m_bEmitFromShell ? fRadius : PARTICLE_RAND_RANGE(0, fRadius));
if (m_bRandomDirection)
{
direction = GetRandomDirection();
}
}
void
Moose::Spatial::COrientable::RotateAroundForward( float fDegrees )
{
/* get rotations using local m_vForward */
CQuaternion q; q.CreateFromAxisAngle( m_vForward[0], m_vForward[1],
m_vForward[2], fDegrees );
m_qRotation = q * m_qRotation;
RotateVector(q,m_vRight);
RotateVector(q,m_vUpward);
SetRotationChanged(1);
m_vRight.Normalize();
m_vUpward.Normalize();
}
/**
* apply the rotation from the "right side".
*
* thus first the point is rotated with the rotation specified by the
* parameters, then the point is rotated with the quaternion stored
* in this class before this method was called.
*/
CQuaternion<T> &rotatePost(
const CVector<3,float> &p_axis,
T p_angle
)
{
/*
* rotation is computed by converting the angular rotation into a quaternion.
* then the rotation is applied by multiplication of both quaternions.
*/
CQuaternion q;
q.setRotation(p_axis, p_angle);
*this *= q;
return *this;
}
CQuaternion CVector3f::toQuat(const int type)
{
//INERTIATOOBJECT
CQuaternion rs;
FLOAT p = x*H3DMath::M_DEG2RAD,h = y*H3DMath::M_DEG2RAD,b = z*H3DMath::M_DEG2RAD;
rs.w = (FLOAT)( cos(h/2)*cos(p/2)*cos(b/2) + sin(h/2)*sin(p/2)*sin(b/2));
rs.x = (FLOAT)(-cos(h/2)*sin(p/2)*cos(b/2) - sin(h/2)*cos(p/2)*sin(b/2));
rs.y = (FLOAT)( cos(h/2)*sin(p/2)*sin(b/2) - sin(h/2)*cos(p/2)*cos(b/2));
rs.z = (FLOAT)( sin(h/2)*sin(p/2)*cos(b/2) - cos(h/2)*cos(p/2)*sin(b/2));
if(type == OBJECTTOINERTIA)
{
rs = rs.conjugated();
}
return rs;
}
void CDynamics2DMultiBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Set target position and orientation */
cpVect tBodyPos = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpFloat tBodyOrient = cZAngle.GetValue();
/* For each body: */
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
/* Set body orientation at anchor */
cpBodySetAngle(m_vecBodies[i].Body,
tBodyOrient + m_vecBodies[i].OffsetOrient);
/* Set body position at anchor */
cpBodySetPos(m_vecBodies[i].Body,
cpvadd(tBodyPos,
cpvrotate(m_vecBodies[i].OffsetPos,
m_vecBodies[i].Body->rot)));
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(),
m_vecBodies[i].Body);
}
/* Update ARGoS entity state */
UpdateEntityStatus();
}
void
Moose::Spatial::COrientable::Rotate( float fXAngle, float fYAngle, float fZAngle )
{
// get rotations
CQuaternion qOne; qOne.CreateFromAxisAngle( 1.0, 0.0, 0.0, fXAngle );
CQuaternion qTwo; qTwo.CreateFromAxisAngle( 0.0, 1.0, 0.0, fYAngle );
qOne = qTwo * qOne;
qTwo.CreateFromAxisAngle( 0.0, 0.0, 1.0, fZAngle );
qOne = qTwo * qOne;
RotateAllDirections(qOne);
m_qRotation = qOne * m_qRotation;
SetRotationChanged(1);
}
