SensorFusion/lib/Quat2RotMat.m

   1 function R = Quat2RotMat(q)\r
   2 % Quat2RotMat\r
   3 %\r
   4 % Copyright (c) 2014 Samsung Electronics Co., Ltd.\r
   5 %\r
   6 % Licensed under the Apache License, Version 2.0 (the "License");\r
   7 % you may not use this file except in compliance with the License.\r
   8 % You may obtain a copy of the License at\r
   9 %\r
  10 % http://www.apache.org/licenses/LICENSE-2.0\r
  11 %\r
  12 % Unless required by applicable law or agreed to in writing, software\r
  13 % distributed under the License is distributed on an "AS IS" BASIS,\r
  14 % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\r
  15 % See the License for the specific language governing permissions and\r
  16 % limitations under the License.\r
  17 \r
  18 % Converts a quaternion orientation to a rotation matrix.\r
  19 \r
  20     R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;\r
  21     R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4));\r
  22     R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3));\r
  23     R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));\r
  24     R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2;\r
  25     R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2));\r
  26     R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));\r
  27     R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));\r
  28     R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;\r
  29 end\r
  30 \r