void kalman_2D_update(kalman_filter_2D_t *kalman, vector_2_t measurement) { vector_2_t innovation = vsub2( measurement, mvmul2(kalman->observation_model, kalman->state)); matrix_2x2_t innovation_covariance = madd2( mmul2( mmul2(kalman->observation_model, kalman->covariance), trans2(kalman->observation_model)), kalman->noise_measurement); matrix_2x2_t kalman_gain = mmul2( mmul2(kalman->covariance, trans2(kalman->observation_model)), inv2(innovation_covariance)); kalman->state = vadd2(kalman->state, mvmul2(kalman_gain, innovation)); kalman->covariance = mmul2( msub2(ident_2x2, mmul2(kalman_gain, kalman->observation_model)), kalman->covariance); }
void kalman_2D_prediction(kalman_filter_2D_t *kalman, vector_2_t control) { kalman->state = vadd2( mvmul2(kalman->system_model, kalman->state), mvmul2(kalman->control_model, control)); kalman->covariance= madd2( mmul2( mmul2( kalman->system_model, kalman->covariance), trans2(kalman->system_model)), kalman->noise_prediction); }
//------------------------------------------------------------------------------- bool Crate::isPlaceFree(const Ogre::Vector3 &dir, bool ignorePlayer) { //The cast result callback. struct CrateCheckResult : public btDynamicsWorld::ConvexResultCallback { btCollisionObject *mIgnore; int mDimension; bool mHit, mIgnorePlayer; CrateCheckResult(btCollisionObject *ignore, int dimension, bool ignorePlayer) : mIgnore(ignore), mDimension(dimension), mHit(false), mIgnorePlayer(ignorePlayer) { } btScalar addSingleResult(btDynamicsWorld::LocalConvexResult &convexResult, bool) { mHit = true; return convexResult.m_hitFraction; } bool needsCollision(btBroadphaseProxy* proxy0) const { //If it's us, or isn't in our dimension, we don't care. return ((btCollisionObject*) proxy0->m_clientObject != mIgnore) && !(proxy0->m_collisionFilterGroup & DimensionManager::NO_CRATE_CHECK) && !(mIgnorePlayer && proxy0->m_collisionFilterGroup & DimensionManager::PLAYER) && (proxy0->m_collisionFilterGroup & mDimension); //&& !(((btCollisionObject*) proxy0->m_clientObject)->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE); } }; //Where to cast from, where to cast to, etc. btTransform myTrans; mBody->getMotionState()->getWorldTransform(myTrans); btVector3 pos1 = myTrans.getOrigin(); btVector3 pos2 = myTrans.getOrigin() + BtOgre::Convert::toBullet(dir); btQuaternion rot = myTrans.getRotation(); btTransform trans1(rot, pos1); btTransform trans2(rot, pos2); //Do the cast. CrateCheckResult res(mBody, mDimensions, ignorePlayer); GlbVar.phyWorld->convexSweepTest(mCastShape, trans1, trans2, res); //If not hit, return true. return !(res.mHit); }
TEST( utility, Atelier_TransformXTransform) { Eigen::Matrix4f m1; m1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 8, 5, 1, 0, 0, 0, 1; //std::cout << "Here is the Matrix4f m: \n" << m4x4 << std::endl; ATELIER::UTILITY::Transform trans1(m1); ATELIER::UTILITY::Transform trans2(Eigen::Matrix4f::Identity()); Timer t; t.start(); for(unsigned int i = 0; i < COUNT; ++i) { trans1 = trans1 * trans2; } double cost = t.getElapsedTimeInMilliSec(); std::cout << "Atelier_TransformXTransform: " << cost <<"ms"<< std::endl; std::cout << "Result: \n" << trans1.getMatrix() << std::endl; EXPECT_EQ( true, true); }
TEST( utility, Eigen_TransformXTransform) { Eigen::Matrix4f m1; m1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 8, 5, 1, 0, 0, 0, 1; Eigen::Affine3f trans1(m1); Eigen::Affine3f trans2(Eigen::Matrix4f::Identity()); Timer t; t.start(); for(unsigned int i = 0; i < COUNT; ++i) { trans1 = trans1 * trans2; } double cost = t.getElapsedTimeInMilliSec(); std::cout << "Eigen_TransformXTransform: " << cost <<"ms"<< std::endl; Eigen::Matrix4f result = trans1.matrix(); std::cout << "Result: \n" << result << std::endl; EXPECT_EQ( true, true); }
* Test functions for small matrix */ #include "test_small_matrix.h" #include "small_matrix.h" #include "linear_algebra.h" #include "print_util.h" bool run_matrix_tests(void) { matrix_2x2_t m2={.v={{1, 2},{3,2}}}; print_util_print_matrix(print_util_get_debug_stream(), m2.v[0], 2, 2, 2); print_util_dbg_print("\n"); matrix_2x2_t m2t=trans2(m2); print_util_print_matrix(print_util_get_debug_stream(), m2t.v[0], 2, 2, 2); print_util_dbg_print("\n"); matrix_2x2_t m2i=inv2(m2); print_util_print_matrix(print_util_get_debug_stream(), m2i.v[0], 2, 2, 2); print_util_dbg_print("\n"); print_util_print_matrix(print_util_get_debug_stream(), madd2(m2, m2t).v[0], 2, 2, 2); print_util_dbg_print("\n"); matrix_2x2_t m2_=mmul2(m2, m2i); print_util_print_matrix(print_util_get_debug_stream(), m2_.v[0], 2, 2, 2); print_util_dbg_print("\n"); matrix_3x3_t m3={.v={{1, 2, 4},{2,4,2},{4,2,1}}}; print_util_print_matrix(print_util_get_debug_stream(), m3.v[0], 3, 3, 2); print_util_dbg_print("\n"); matrix_3x3_t m3d=diag_3x3(row3(m3, 0));
//------------------------------------------------------------------------------- void SlidingBrush::unpausedTick(const Ogre::FrameEvent &evt) { GraLL2GameObject::unpausedTick(evt); if (mEnabled) { //Get old place (current place actually). btTransform oldTrans; mBody->getMotionState()->getWorldTransform(oldTrans); btVector3 prevPos = oldTrans.getOrigin(); //Find next point position, and velocity. int currPlace = mForward ? mCurrentPlace + 0.5 : mCurrentPlace -0.5; Ogre::Vector3 currPoint = mPoints[currPlace]; Ogre::Vector3 velocity = (currPoint - BtOgre::Convert::toOgre(prevPos)).normalisedCopy() * mSpeed; //Check if we're near next point. Ogre::Real sqSpeed = mSpeed * mSpeed * evt.timeSinceLastFrame * evt.timeSinceLastFrame; Ogre::Real sqDist = (currPoint - BtOgre::Convert::toOgre(prevPos)).squaredLength(); if (sqDist < sqSpeed) { //If near, jump to it. mBody->getMotionState()->setWorldTransform(btTransform(oldTrans.getRotation(), BtOgre::Convert::toBullet(currPoint))); mNode->setPosition(currPoint); //If next 'place' exists, get there.. unsigned int nextPlace = currPlace + (mForward ? 1 : -1); if (nextPlace >= 0 && nextPlace < mPoints.size()) mCurrentPlace += mForward ? 1 : -1; //Call the Python 'point' event. Cut out repeated events using a 'last place' idea. if (mLastPlace != currPlace) NGF_PY_CALL_EVENT(point, currPlace); mLastPlace = currPlace; } else { //Otherwise, usual velocity movement. bool canMove = true; btVector3 currVel = BtOgre::Convert::toBullet(velocity) * evt.timeSinceLastFrame; btVector3 newPos = prevPos + currVel; //If we're not ignoring collisions, check for 'em. if (!mIgnoreCollisions) { //The cast result callback. struct SlidingBrushCheckResult : public btDynamicsWorld::ConvexResultCallback { btCollisionObject *mIgnore; int mDimension; bool mHit; Ogre::Real mHitFraction; bool mYCast; SlidingBrushCheckResult(btCollisionObject *ignore, int dimension, bool yCast) : mIgnore(ignore), mDimension(dimension), mHit(false), mHitFraction(1), mYCast(yCast) { } btScalar addSingleResult(btDynamicsWorld::LocalConvexResult &convexResult, bool) { mHit = true; mHitFraction = convexResult.m_hitFraction < mHitFraction ? convexResult.m_hitFraction : mHitFraction; return convexResult.m_hitFraction; } bool needsCollision(btBroadphaseProxy* proxy0) const { return ((btCollisionObject*) proxy0->m_clientObject != mIgnore) //Shouldn't be us. && (proxy0->m_collisionFilterGroup & mDimension) //Should be in our dimension. && (!(proxy0->m_collisionFilterGroup & DimensionManager::STATIC)) //No need to check with static. && !(mYCast && (proxy0->m_collisionFilterGroup & DimensionManager::LIFTABLE)) //If moving up, forget the liftables. && !(((btCollisionObject*) proxy0->m_clientObject)->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE); //If no contact response, ignore. } }; //Where to cast from, where to cast to, etc. btVector3 normVel = currVel.normalized(); btVector3 pos1 = prevPos; btVector3 pos2 = prevPos + currVel + (normVel * CAST_SHAPE_SHRINK); btQuaternion rot = mBody->getWorldTransform().getRotation(); btTransform trans1(rot, pos1); btTransform trans2(rot, pos2); //Do the cast. SlidingBrushCheckResult res(mBody, mDimensions, GlbVar.gravMgr->isUp() ? pos1.y() < pos2.y() : pos1.y() > pos2.y()); GlbVar.phyWorld->convexSweepTest(mCastShape, trans1, trans2, res); //If hit, don't move. if (res.mHit) { goto skip; } } if (canMove) { oldTrans.setOrigin(newPos); mBody->getMotionState()->setWorldTransform(oldTrans); mNode->setPosition(BtOgre::Convert::toOgre(newPos)); } } } skip: //Python utick event. NGF_PY_CALL_EVENT(utick, evt.timeSinceLastFrame); }