void FlyingLayer::Update(TimeDelta real_time_delta) {
static const float PERIOD_TRANS = 0.0045f;
static const float PERIOD_ROT = 1.0f;
static const float FILTER = 0.2f;
if (m_Palms.size() > 0) {
Vector3f positionSum = Vector3f::Zero();
Vector3f rotationAASum = Vector3f::Zero();
for (int i = 0; i < m_Palms.size(); i++) {
positionSum += m_GridOrientation.block<3, 3>(0, 0).transpose()*(m_Palms[i] - m_EyePos - m_EyeView.transpose()*Vector3f(0, -0.15, -0.05));
//rotationAASum += RotationMatrixToVector(m_EyeView.transpose()*m_PalmOrientations[i]*m_EyeView.transpose());
Matrix3x3f rot;
RotationMatrix_VectorToVector(-Vector3f::UnitZ(), m_EyeView*(m_Palms[i] - m_EyePos) - Vector3f(0, -0.15, -0.05), rot);
//std::cout << __LINE__ << ":\t rot = " << (rot) << std::endl;
rotationAASum += RotationMatrixToVector(rot);
}
if (m_Palms.size() == 2) {
const Vector3f dir0 = m_EyeView*(m_Palms[0] - m_EyePos) - Vector3f(0, -0.15, -0.05);
const Vector3f dir1 = m_EyeView*(m_Palms[1] - m_EyePos) - Vector3f(0, -0.15, -0.05);
Matrix3x3f rot;
RotationMatrix_VectorToVector((dir0.x() < dir1.x() ? 1.0f : -1.0f) * Vector3f::UnitX(), dir1 - dir0, rot);
//std::cout << __LINE__ << ":\t positionSum = " << (positionSum) << std::endl;
rotationAASum += 2.0f*RotationMatrixToVector(rot);
}
m_Velocity = (1 - FILTER)*m_Velocity + FILTER*positionSum/m_Palms.size();
m_RotationAA = (1 - FILTER)*m_RotationAA + FILTER*rotationAASum/m_Palms.size();
} else {
m_Velocity = (1 - 0.3f*FILTER)*m_Velocity;
m_RotationAA = (1 - 0.3f*FILTER)*m_RotationAA;
}
m_GridCenter -= m_Velocity*m_Velocity.squaredNorm()*(real_time_delta/PERIOD_TRANS);
const Matrix3x3f rot = RotationVectorToMatrix((real_time_delta/PERIOD_ROT)*m_RotationAA*m_RotationAA.squaredNorm());
//std::cout << __LINE__ << ":\t rot = " << (rot) << std::endl;
//Matrix3x3f foo = ;
m_GridOrientation.block<3, 3>(0, 0) = m_EyeView.transpose()*rot.transpose()*m_EyeView*m_GridOrientation.block<3, 3>(0, 0);
}
void FlyingLayer::RotationMatrixSuppress(Matrix3x3f& A0, float t) const {
assert(t >= 0.0 && t <= 1.0);
const Vector3f dA = RotationMatrixToVector(A0);
A0 = RotationVectorToMatrix(dA * t);
}
Matrix3x3f FlyingLayer::RotationMatrixLinearInterpolation(const Matrix3x3f& A0, const Matrix3x3f& A1, float t) const {
Vector3f dA = RotationMatrixToVector(A0.transpose()*A1);
float angle = std::fmod(t*dA.norm(), M_PI);
Matrix3x3f At = A0*RotationVectorToMatrix(angle*dA.normalized());
return At;
}
void MathUtility::RotationMatrixSuppress(EigenTypes::Matrix3x3f& A0, float t) {
const EigenTypes::Vector3f dA = RotationMatrixToVector(A0);
A0 = RotationVectorToMatrix(dA * t);
}
EigenTypes::Matrix3x3f MathUtility::RotationMatrixLinearInterpolation(const EigenTypes::Matrix3x3f& A0, const EigenTypes::Matrix3x3f& A1, float t) {
EigenTypes::Vector3f dA = RotationMatrixToVector(A0.transpose()*A1);
float angle = std::fmod(t*dA.norm(), (float)M_PI);
EigenTypes::Matrix3x3f At = A0*RotationVectorToMatrix(angle*dA.normalized());
return At;
}
