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; }