bool Spatial::rotateToHeading(Vec2d pos) { Vec2d toTarget = Vec2d(pos-mPos); toTarget.normalize(); double angle = acos(mHeading.dot(pos)); if (angle < 0.000001) return true; if (angle > mMaxTurnRate) angle = mMaxTurnRate; Mat3d rotationMat; rotationMat.rotate(angle*mHeading.sign(toTarget)); rotationMat.transformVec2d(mHeading); rotationMat.transformVec2d(mVelocity); mSide = mHeading.perp(); return false; }